Add Ray Tracing

Add braces

Change-Id: I5355ccd8f745dfbd4fe3923a81aa3c9f8fda07b3

8 files changed, 324 insertions, 97 deletions

diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/BVH.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/BVH.pde
new file mode 100644
index 000000000..7249ee972
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/BVH.pde
@@ -0,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;
+  }
+}
diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/Camera.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/Camera.pde
index 176245b8f..b39dae3a1 100644
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/Camera.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Camera.pde
@@ -19,6 +19,11 @@ class Camera {
     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 @@ class Camera {
     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;
diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde
index e6d412d28..dbba327b7 100644
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde
@@ -6,25 +6,57 @@ class MotionField {
     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 @@ class MotionField {
   }
 
   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++) {
diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/PointCloud.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/PointCloud.pde
index f6f551d0d..714a6f3a0 100644
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/PointCloud.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/PointCloud.pde
@@ -29,7 +29,9 @@ class PointCloud {
         // 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 @@ class PointCloud {
   // 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 @@ class PointCloud {
   }
   // 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
diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/Ray_Tracing.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/Ray_Tracing.pde
new file mode 100644
index 000000000..ef4be691c
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Ray_Tracing.pde
@@ -0,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;
+  }
+}
diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/Scene.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/Scene.pde
index a9c28f631..6189bc0d0 100644
--- 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();
     }
   }
diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/Transform.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/Transform.pde
index e4bd5f541..af2204e8c 100644
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/Transform.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Transform.pde
@@ -5,13 +5,13 @@ class Transform {
   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;
diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/sketch_3D_reconstruction.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/sketch_3D_reconstruction.pde
index 89fa57b60..f9745c8e9 100644
--- 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 @@ Scene scene;
 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 @@ void setup() {
         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 @@ void draw() {
   }
   scene.render(
       true);  // true: turn on motion field; false: turn off motion field
+  showGrids(scene.motion_field.block_size);
 }