ref: a709b694435c23d63689f2146b2764a5bc736dd0
parent: 20103ec897f1b8cc3d44f491fc5c040b4223ea72
author: Dan Zhu <zxdan@google.com>
date: Wed Jun 19 06:54:23 EDT 2019
Add Ray Tracing Add braces Change-Id: I5355ccd8f745dfbd4fe3923a81aa3c9f8fda07b3
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/BVH.pde
@@ -1,0 +1,163 @@
+/*
+ *AABB bounding box
+ *Bouding Volume Hierarchy
+ */
+class BoundingBox {+ float min_x, min_y, min_z, max_x, max_y, max_z;
+ PVector center;
+ BoundingBox() {+ min_x = Float.POSITIVE_INFINITY;
+ min_y = Float.POSITIVE_INFINITY;
+ min_z = Float.POSITIVE_INFINITY;
+ max_x = Float.NEGATIVE_INFINITY;
+ max_y = Float.NEGATIVE_INFINITY;
+ max_z = Float.NEGATIVE_INFINITY;
+ center = new PVector();
+ }
+ // build a bounding box for a triangle
+ void create(Triangle t) {+ min_x = min(t.p1.x, min(t.p2.x, t.p3.x));
+ max_x = max(t.p1.x, max(t.p2.x, t.p3.x));
+
+ min_y = min(t.p1.y, min(t.p2.y, t.p3.y));
+ max_y = max(t.p1.y, max(t.p2.y, t.p3.y));
+
+ min_z = min(t.p1.z, min(t.p2.z, t.p3.z));
+ max_z = max(t.p1.z, max(t.p2.z, t.p3.z));
+ center.x = (max_x + min_x) / 2;
+ center.y = (max_y + min_y) / 2;
+ center.z = (max_z + min_z) / 2;
+ }
+ // merge two bounding boxes
+ void add(BoundingBox bbx) {+ min_x = min(min_x, bbx.min_x);
+ min_y = min(min_y, bbx.min_y);
+ min_z = min(min_z, bbx.min_z);
+
+ max_x = max(max_x, bbx.max_x);
+ max_y = max(max_y, bbx.max_y);
+ max_z = max(max_z, bbx.max_z);
+ center.x = (max_x + min_x) / 2;
+ center.y = (max_y + min_y) / 2;
+ center.z = (max_z + min_z) / 2;
+ }
+ // get bounding box center axis value
+ float getCenterAxisValue(int axis) {+ if (axis == 1) {+ return center.x;
+ } else if (axis == 2) {+ return center.y;
+ }
+ // when axis == 3
+ return center.z;
+ }
+ // check if a ray is intersected with the bounding box
+ boolean intersect(Ray r) {+ float tmin, tmax;
+ if (r.dir.x >= 0) {+ tmin = (min_x - r.ori.x) * (1.0f / r.dir.x);
+ tmax = (max_x - r.ori.x) * (1.0f / r.dir.x);
+ } else {+ tmin = (max_x - r.ori.x) * (1.0f / r.dir.x);
+ tmax = (min_x - r.ori.x) * (1.0f / r.dir.x);
+ }
+
+ float tymin, tymax;
+ if (r.dir.y >= 0) {+ tymin = (min_y - r.ori.y) * (1.0f / r.dir.y);
+ tymax = (max_y - r.ori.y) * (1.0f / r.dir.y);
+ } else {+ tymin = (max_y - r.ori.y) * (1.0f / r.dir.y);
+ tymax = (min_y - r.ori.y) * (1.0f / r.dir.y);
+ }
+
+ if (tmax < tymin || tymax < tmin) {+ return false;
+ }
+
+ tmin = tmin < tymin ? tymin : tmin;
+ tmax = tmax > tymax ? tymax : tmax;
+
+ float tzmin, tzmax;
+ if (r.dir.z >= 0) {+ tzmin = (min_z - r.ori.z) * (1.0f / r.dir.z);
+ tzmax = (max_z - r.ori.z) * (1.0f / r.dir.z);
+ } else {+ tzmin = (max_z - r.ori.z) * (1.0f / r.dir.z);
+ tzmax = (min_z - r.ori.z) * (1.0f / r.dir.z);
+ }
+ if (tmax < tzmin || tmin > tzmax) {+ return false;
+ }
+ return true;
+ }
+}
+// Bounding Volume Hierarchy
+class BVH {+ // Binary Tree
+ BVH left, right;
+ BoundingBox overall_bbx;
+ ArrayList<Triangle> mesh;
+ BVH(ArrayList<Triangle> mesh) {+ this.mesh = mesh;
+ overall_bbx = new BoundingBox();
+ left = null;
+ right = null;
+ int mesh_size = this.mesh.size();
+ if (mesh_size <= 1) {+ return;
+ }
+ // random select an axis
+ int axis = int(random(100)) % 3 + 1;
+ // build bounding box and save the selected center component
+ float[] axis_values = new float[mesh_size];
+ for (int i = 0; i < mesh_size; i++) {+ Triangle t = this.mesh.get(i);
+ overall_bbx.add(t.bbx);
+ axis_values[i] = t.bbx.getCenterAxisValue(axis);
+ }
+ // find the median value of selected center component as pivot
+ axis_values = sort(axis_values);
+ float pivot;
+ if (mesh_size % 2 == 1) {+ pivot = axis_values[mesh_size / 2];
+ } else {+ pivot =
+ 0.5f * (axis_values[mesh_size / 2 - 1] + axis_values[mesh_size / 2]);
+ }
+ // Build left node and right node by partitioning the mesh based on triangle
+ // bounding box center component value
+ ArrayList<Triangle> left_mesh = new ArrayList<Triangle>();
+ ArrayList<Triangle> right_mesh = new ArrayList<Triangle>();
+ for (int i = 0; i < mesh_size; i++) {+ Triangle t = this.mesh.get(i);
+ if (t.bbx.getCenterAxisValue(axis) < pivot) {+ left_mesh.add(t);
+ } else if (t.bbx.getCenterAxisValue(axis) > pivot) {+ right_mesh.add(t);
+ } else if (left_mesh.size() < right_mesh.size()) {+ left_mesh.add(t);
+ } else {+ right_mesh.add(t);
+ }
+ }
+ left = new BVH(left_mesh);
+ right = new BVH(right_mesh);
+ }
+ // check if a ray intersect with current volume
+ boolean intersect(Ray r, float[] param) {+ if (mesh.size() == 0) {+ return false;
+ }
+ if (mesh.size() == 1) {+ Triangle t = mesh.get(0);
+ return t.intersect(r, param);
+ }
+ if (!overall_bbx.intersect(r)) {+ return false;
+ }
+ boolean left_res = left.intersect(r, param);
+ boolean right_res = right.intersect(r, param);
+ return left_res || right_res;
+ }
+}
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/Camera.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Camera.pde
@@ -19,6 +19,11 @@
init_axis = axis.copy();
}
+ Camera copy() {+ Camera cam = new Camera(fov, pos.copy(), center.copy(), axis.copy());
+ return cam;
+ }
+
PVector project(PVector pos) {PVector proj = MatxVec3(getCameraMat(), PVector.sub(pos, this.pos));
proj.x = (float)height / 2.0 * proj.x / proj.z / tan(fov / 2.0f);
@@ -31,12 +36,12 @@
float[] mat = new float[9];
PVector dir = PVector.sub(center, pos);
dir.normalize();
- PVector left = axis.cross(dir);
+ PVector left = dir.cross(axis);
left.normalize();
-
- mat[0] = left.x;
- mat[1] = left.y;
- mat[2] = left.z;
+ // processing camera system does not follow right hand rule
+ mat[0] = -left.x;
+ mat[1] = -left.y;
+ mat[2] = -left.z;
mat[3] = axis.x;
mat[4] = axis.y;
mat[5] = axis.z;
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde
@@ -6,25 +6,57 @@
motion_field = new ArrayList<PVector>();
}
- void update(ArrayList<PVector> last_positions,
- ArrayList<PVector> current_positions, int[] render_list) {- // build motion field
+ void update(Camera last_cam, Camera current_cam, PointCloud point_cloud,
+ BVH bvh) {+ // clear motion field
motion_field = new ArrayList<PVector>();
int r_num = height / block_size, c_num = width / block_size;
for (int i = 0; i < r_num * c_num; i++)
motion_field.add(new PVector(0, 0, 0));
- // accumate motion vector of pixel in each block
- for (int i = 0; i < height; i++)
- for (int j = 0; j < width; j++) {- if (render_list[i * width + j] == -1) continue;
- PVector cur_pos = current_positions.get(render_list[i * width + j]);
- PVector last_pos = last_positions.get(render_list[i * width + j]);
- int idx = i / block_size * c_num + j / block_size;
- PVector mv = PVector.sub(last_pos, cur_pos);
- PVector acc_mv = motion_field.get(idx);
- motion_field.set(
- idx, new PVector(acc_mv.x + mv.x, acc_mv.y + mv.y, acc_mv.z + 1));
+ // estimate motion vector of each point in point cloud
+ for (int i = 0; i < point_cloud.size(); i++) {+ PVector p = point_cloud.getPosition(i);
+ PVector p0 = current_cam.project(p);
+ PVector p1 = last_cam.project(p);
+ int row = int((p0.y + height / 2.0f) / block_size);
+ int col = int((p0.x + width / 2.0f) / block_size);
+ if (row >= 0 && row < r_num && col >= 0 && col < c_num) {+ PVector accu = motion_field.get(row * c_num + col);
+ accu.x += p1.x - p0.x;
+ accu.y += p1.y - p0.y;
+ accu.z += 1;
}
+ }
+ // if some blocks do not have point, then use ray tracing to see if they are
+ // in triangles
+ for (int i = 0; i < r_num; i++)
+ for (int j = 0; j < c_num; j++) {+ PVector accu = motion_field.get(i * c_num + j);
+ if (accu.z > 0) {+ continue;
+ }
+ // use the center of the block to generate view ray
+ float cx = j * block_size + block_size / 2.0f - width / 2.0f;
+ float cy = i * block_size + block_size / 2.0f - height / 2.0f;
+ float cz = 0.5f * height / tan(current_cam.fov / 2.0f);
+ PVector dir = new PVector(cx, cy, cz);
+ float[] camMat = current_cam.getCameraMat();
+ dir = MatxVec3(transpose3x3(camMat), dir);
+ dir.normalize();
+ Ray r = new Ray(current_cam.pos, dir);
+ // ray tracing
+ float[] param = new float[4];
+ param[0] = Float.POSITIVE_INFINITY;
+ if (bvh.intersect(r, param)) {+ PVector p = new PVector(param[1], param[2], param[3]);
+ PVector p0 = current_cam.project(p);
+ PVector p1 = last_cam.project(p);
+ accu.x += p1.x - p0.x;
+ accu.y += p1.y - p0.y;
+ accu.z += 1;
+ }
+ }
+ // estimate the motion vector of each block
for (int i = 0; i < r_num * c_num; i++) {PVector mv = motion_field.get(i);
if (mv.z > 0) {@@ -34,8 +66,6 @@
}
void render() {- // ortho(-width,0,-height,0);
- // camera(0,0,0,0,0,1,0,1,0);
int r_num = height / block_size, c_num = width / block_size;
for (int i = 0; i < r_num; i++)
for (int j = 0; j < c_num; j++) {--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/PointCloud.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/PointCloud.pde
@@ -29,7 +29,9 @@
// get depth value (red channel)
color depth_px = depth_img.get(u, v);
depth[v * width + u] = depth_px & 0x0000FFFF;
- if (int(depth[v * width + u]) != 0) real[v * width + u] = true;
+ if (int(depth[v * width + u]) != 0) {+ real[v * width + u] = true;
+ }
point_colors.append(rgb_img.get(u, v));
}
for (int v = 0; v < height; v++)
@@ -58,8 +60,9 @@
// interpolate
void interpolateDepth(int row, int col) {if (row < 0 || row >= height || col < 0 || col >= width ||
- int(depth[row * width + col]) != 0)
+ int(depth[row * width + col]) != 0) {return;
+ }
ArrayList<Node> queue = new ArrayList<Node>();
queue.add(new Node(row, col, null));
boolean[] visited = new boolean[width * height];
@@ -109,7 +112,9 @@
}
// get cloud center
PVector getCloudCenter() {- if (points.size() > 0) return PVector.div(cloud_mass, points.size());
+ if (points.size() > 0) {+ return PVector.div(cloud_mass, points.size());
+ }
return new PVector(0, 0, 0);
}
// merge two clouds
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Ray_Tracing.pde
@@ -1,0 +1,61 @@
+// Triangle
+class Triangle {+ // position
+ PVector p1, p2, p3;
+ // color
+ color c1, c2, c3;
+ BoundingBox bbx;
+ Triangle(PVector p1, PVector p2, PVector p3, color c1, color c2, color c3) {+ this.p1 = p1;
+ this.p2 = p2;
+ this.p3 = p3;
+ this.c1 = c1;
+ this.c2 = c2;
+ this.c3 = c3;
+ bbx = new BoundingBox();
+ bbx.create(this);
+ }
+ // check to see if a ray intersects with the triangle
+ boolean intersect(Ray r, float[] param) {+ PVector p21 = PVector.sub(p2, p1);
+ PVector p31 = PVector.sub(p3, p1);
+ PVector po1 = PVector.sub(r.ori, p1);
+
+ PVector dxp31 = r.dir.cross(p31);
+ PVector po1xp21 = po1.cross(p21);
+ float denom = p21.dot(dxp31);
+ float t = p31.dot(po1xp21) / denom;
+ float alpha = po1.dot(dxp31) / denom;
+ float beta = r.dir.dot(po1xp21) / denom;
+
+ boolean res = t > 0 && alpha > 0 && alpha < 1 && beta > 0 && beta < 1 &&
+ alpha + beta < 1;
+ // depth test
+ if (res && t < param[0]) {+ param[0] = t;
+ param[1] = alpha * p1.x + beta * p2.x + (1 - alpha - beta) * p3.x;
+ param[2] = alpha * p1.y + beta * p2.y + (1 - alpha - beta) * p3.y;
+ param[3] = alpha * p1.z + beta * p2.z + (1 - alpha - beta) * p3.z;
+ }
+ return res;
+ }
+ void render() {+ beginShape(TRIANGLES);
+ fill(c1);
+ vertex(p1.x, p1.y, p1.z);
+ fill(c2);
+ vertex(p2.x, p2.y, p2.z);
+ fill(c3);
+ vertex(p3.x, p3.y, p3.z);
+ endShape();
+ }
+}
+// Ray
+class Ray {+ // origin and direction
+ PVector ori, dir;
+ Ray(PVector ori, PVector dir) {+ this.ori = ori;
+ this.dir = dir;
+ }
+}
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/Scene.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Scene.pde
@@ -1,86 +1,49 @@
class Scene {- Camera camera;
PointCloud point_cloud;
+ ArrayList<Triangle> mesh;
+ BVH bvh;
MotionField motion_field;
- ArrayList<PVector> last_positions;
- ArrayList<PVector> current_positions;
- int[] render_list;
+ Camera last_cam;
+ Camera current_cam;
Scene(Camera camera, PointCloud point_cloud, MotionField motion_field) {- this.camera = camera;
this.point_cloud = point_cloud;
this.motion_field = motion_field;
- last_positions = project2Camera();
- current_positions = project2Camera();
- render_list = new int[width * height];
- updateRenderList();
- }
-
- ArrayList<PVector> project2Camera() {- ArrayList<PVector> projs = new ArrayList<PVector>();
- for (int i = 0; i < point_cloud.size(); i++) {- PVector proj = camera.project(point_cloud.getPosition(i));
- projs.add(proj);
- }
- return projs;
- }
- // update render list by using depth test
- void updateRenderList() {- // clear render list
- for (int i = 0; i < width * height; i++) render_list[i] = -1;
- // depth test and get render list
- float[] depth = new float[width * height];
- for (int i = 0; i < width * height; i++) depth[i] = Float.POSITIVE_INFINITY;
- for (int i = 0; i < current_positions.size(); i++) {- PVector pos = current_positions.get(i);
- int row = int(pos.y + height / 2);
- int col = int(pos.x + width / 2);
- int idx = row * width + col;
- if (row >= 0 && row < height && col >= 0 && col < width) {- if (render_list[idx] == -1 || pos.z < depth[idx]) {- depth[idx] = pos.z;
- render_list[idx] = i;
- }
+ mesh = new ArrayList<Triangle>();
+ for (int v = 0; v < height - 1; v++)
+ for (int u = 0; u < width - 1; u++) {+ PVector p1 = point_cloud.getPosition(v * width + u);
+ PVector p2 = point_cloud.getPosition(v * width + u + 1);
+ PVector p3 = point_cloud.getPosition((v + 1) * width + u + 1);
+ PVector p4 = point_cloud.getPosition((v + 1) * width + u);
+ color c1 = point_cloud.getColor(v * width + u);
+ color c2 = point_cloud.getColor(v * width + u + 1);
+ color c3 = point_cloud.getColor((v + 1) * width + u + 1);
+ color c4 = point_cloud.getColor((v + 1) * width + u);
+ mesh.add(new Triangle(p1, p2, p3, c1, c2, c3));
+ mesh.add(new Triangle(p3, p4, p1, c3, c4, c1));
}
- }
+ bvh = new BVH(mesh);
+ last_cam = camera.copy();
+ current_cam = camera;
}
void run() {- camera.run();
- last_positions = current_positions;
- current_positions = project2Camera();
- updateRenderList();
- motion_field.update(last_positions, current_positions, render_list);
+ last_cam = current_cam.copy();
+ current_cam.run();
+ motion_field.update(last_cam, current_cam, point_cloud, bvh);
}
void render(boolean show_motion_field) {// build mesh
- camera.open();
+ current_cam.open();
noStroke();
- beginShape(TRIANGLES);
- for (int i = 0; i < height - 1; i++)
- for (int j = 0; j < width - 1; j++) {- PVector pos0 = point_cloud.getPosition(i * width + j);
- PVector pos1 = point_cloud.getPosition(i * width + j + 1);
- PVector pos2 = point_cloud.getPosition((i + 1) * width + j + 1);
- PVector pos3 = point_cloud.getPosition((i + 1) * width + j);
- fill(point_cloud.getColor(i * width + j));
- vertex(pos0.x, pos0.y, pos0.z);
- fill(point_cloud.getColor(i * width + j + 1));
- vertex(pos1.x, pos1.y, pos1.z);
- fill(point_cloud.getColor((i + 1) * width + j + 1));
- vertex(pos2.x, pos2.y, pos2.z);
-
- fill(point_cloud.getColor((i + 1) * width + j + 1));
- vertex(pos2.x, pos2.y, pos2.z);
- fill(point_cloud.getColor((i + 1) * width + j + 1));
- vertex(pos3.x, pos3.y, pos3.z);
- fill(point_cloud.getColor(i * width + j));
- vertex(pos0.x, pos0.y, pos0.z);
- }
- endShape();
+ for (int i = 0; i < mesh.size(); i++) {+ Triangle t = mesh.get(i);
+ t.render();
+ }
if (show_motion_field) {- camera.close();
+ current_cam.close();
motion_field.render();
}
}
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/Transform.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Transform.pde
@@ -5,13 +5,13 @@
int w, h; // the width and height of the frame
float normalier; // nomalization factor of depth
Transform(float tx, float ty, float tz, float qx, float qy, float qz,
- float qw, float focal, int w, int h, float normalier) {+ float qw, float fov, int w, int h, float normalier) {// currently, we did not use the info of real camera's position and
// quaternion maybe we will use it in the future when combine all frames
float[] rot = quaternion2Mat3x3(qx, qy, qz, qw);
inv_rot = transpose3x3(rot);
inv_mov = new PVector(-tx, -ty, -tz);
- this.focal = focal;
+ this.focal = 0.5f * h / tan(fov / 2.0);
this.w = w;
this.h = h;
this.normalier = normalier;
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/sketch_3D_reconstruction.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/sketch_3D_reconstruction.pde
@@ -8,10 +8,9 @@
void setup() {size(640, 480, P3D);
// default settings
- float focal = 525.0f; // focal distance of camera
- int frame_no = 118; // frame number
+ int frame_no = 0; // frame number
float fov = PI / 3; // field of view
- int block_size = 8; // block size
+ int block_size = 32; // block size
float normalizer = 5000.0f; // normalizer
// initialize
PointCloud point_cloud = new PointCloud();
@@ -29,8 +28,8 @@
qw = float(info[13]); // quaternion
// build transformer
- Transform trans = new Transform(tx, ty, tz, qx, qy, qz, qw, focal, width,
- height, normalizer);
+ Transform trans =
+ new Transform(tx, ty, tz, qx, qy, qz, qw, fov, width, height, normalizer);
PImage rgb = loadImage(rgb_path);
PImage depth = loadImage(depth_path);
// generate point cloud
@@ -64,4 +63,5 @@
}
scene.render(
true); // true: turn on motion field; false: turn off motion field
+ showGrids(scene.motion_field.block_size);
}