Add Scene module to manage other objects

and calculation

Add interpolation in the Scene

Delete Color interpolation

Build triangle mesh

Reconstruct the code of depth interpolation

Add new data structure Node for back linking

Change-Id: Ibb1e896a2e3623d4549d628539d81d79827ba684

6 files changed, 260 insertions, 125 deletions

diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/Camera.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/Camera.pde
index e8dae7709..176245b8f 100644
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/Camera.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Camera.pde
@@ -19,6 +19,14 @@ class Camera {
     init_axis = axis.copy();
   }
 
+  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);
+    proj.y = (float)height / 2.0 * proj.y / proj.z / tan(fov / 2.0f);
+    proj.z = proj.z;
+    return proj;
+  }
+
   float[] getCameraMat() {
     float[] mat = new float[9];
     PVector dir = PVector.sub(center, pos);
@@ -112,8 +120,14 @@ class Camera {
           }
       }
     }
-    perspective(fov, float(width) / float(height), 1e-6, 1e5);
+  }
+  void open() {
+    perspective(fov, float(width) / height, 1e-6, 1e5);
     camera(pos.x, pos.y, pos.z, center.x, center.y, center.z, axis.x, axis.y,
            axis.z);
   }
+  void close() {
+    ortho(-width, 0, -height, 0);
+    camera(0, 0, 0, 0, 0, 1, 0, 1, 0);
+  }
 }
diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde
index 48a2c56a0..e6d412d28 100644
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde
@@ -1,102 +1,41 @@
 class MotionField {
-  Camera camera;
   int block_size;
-  PointCloud point_cloud;
-  ArrayList<PVector> last_positions;
-  ArrayList<PVector> current_positions;
   ArrayList<PVector> motion_field;
-
-  MotionField(Camera camera, PointCloud point_cloud, int block_size) {
-    this.camera = camera;
-    this.point_cloud = point_cloud;
+  MotionField(int block_size) {
     this.block_size = block_size;
-    this.last_positions = new ArrayList<PVector>();
-    this.current_positions = new ArrayList<PVector>();
-    this.motion_field = new ArrayList<PVector>();
-    for (int i = 0; i < point_cloud.points.size(); i++) {
-      PVector pos = point_cloud.points.get(i);
-      PVector proj_pos = getProjectedPos(pos);
-      last_positions.add(proj_pos);
-      current_positions.add(proj_pos);
-    }
-  }
-
-  PVector getProjectedPos(PVector pos) {
-    float[] cam_mat = camera.getCameraMat();
-    PVector trans_pos =
-        PVector.sub(pos, camera.pos);  // translate based on camera's position
-    PVector rot_pos =
-        MatxVec3(cam_mat, trans_pos);  // rotate based on camera angle
-    PVector proj_pos = new PVector(0, 0, 0);
-    proj_pos.x =
-        height / 2.0f * rot_pos.x / (rot_pos.z) / tan(camera.fov / 2.0f);
-    proj_pos.y =
-        height / 2.0f * rot_pos.y / (rot_pos.z) / tan(camera.fov / 2.0f);
-    proj_pos.z = trans_pos.z;
-
-    return proj_pos;
-  }
-
-  void run() {
-    last_positions = current_positions;
-    // take care
-    current_positions = new ArrayList<PVector>();
-    for (int i = 0; i < point_cloud.points.size(); i++) {
-      PVector pos = point_cloud.points.get(i);
-      PVector proj_pos = getProjectedPos(pos);
-      current_positions.add(proj_pos);
-    }
+    motion_field = new ArrayList<PVector>();
   }
 
-  ArrayList<PVector> getMotionField() {
-    ArrayList<PVector> depth = new ArrayList<PVector>();
-    IntList pixel_idx = new IntList();
-    for (int i = 0; i < width * height; i++)
-      depth.add(new PVector(Float.POSITIVE_INFINITY, -1));
-    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) {
-        PVector depth_info = depth.get(idx);
-        if (depth_info.y == -1) {
-          depth.set(idx, new PVector(pos.z, pixel_idx.size()));
-          pixel_idx.append(i);
-        } else if (pos.z < depth_info.x) {
-          depth.set(idx, new PVector(pos.z, depth_info.y));
-          pixel_idx.set(int(depth_info.y), i);
-        }
-      }
-    }
+  void update(ArrayList<PVector> last_positions,
+              ArrayList<PVector> current_positions, int[] render_list) {
+    // build 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));
-    for (int i = 0; i < pixel_idx.size(); i++) {
-      PVector cur_pos = current_positions.get(pixel_idx.get(i));
-      PVector last_pos = last_positions.get(pixel_idx.get(i));
-      int row = int(cur_pos.y + height / 2);
-      int col = int(cur_pos.x + width / 2);
-      int idx = row / block_size * c_num + col / 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));
-    }
+    // 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));
+      }
     for (int i = 0; i < r_num * c_num; i++) {
       PVector mv = motion_field.get(i);
       if (mv.z > 0) {
         motion_field.set(i, new PVector(mv.x / mv.z, mv.y / mv.z, 0));
       }
     }
-    return motion_field;
   }
 
-  void showMotionField() {
-    ortho(-width, 0, -height, 0);
-    camera(0, 0, 0, 0, 0, 1, 0, 1, 0);
-    getMotionField();
+  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 8885d32b7..f6f551d0d 100644
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/PointCloud.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/PointCloud.pde
@@ -2,41 +2,132 @@ class PointCloud {
   ArrayList<PVector> points;  // array to save points
   IntList point_colors;       // array to save points color
   PVector cloud_mass;
+  float[] depth;
+  boolean[] real;
   PointCloud() {
     // initialize
     points = new ArrayList<PVector>();
     point_colors = new IntList();
     cloud_mass = new PVector(0, 0, 0);
+    depth = new float[width * height];
+    real = new boolean[width * height];
   }
 
-  void generate(PImage rgb, PImage depth, Transform trans) {
-    for (int v = 0; v < depth.height; v++)
-      for (int u = 0; u < depth.width; u++) {
+  void generate(PImage rgb_img, PImage depth_img, Transform trans) {
+    if (depth_img.width != width || depth_img.height != height ||
+        rgb_img.width != width || rgb_img.height != height) {
+      println("rgb and depth file dimension should be same with window size");
+      exit();
+    }
+    // clear depth and real
+    for (int i = 0; i < width * height; i++) {
+      depth[i] = 0;
+      real[i] = false;
+    }
+    for (int v = 0; v < height; v++)
+      for (int u = 0; u < width; u++) {
         // get depth value (red channel)
-        color depth_px = depth.get(u, v);
-        int d = depth_px & 0x0000FFFF;
-        // only transform the pixel whose depth is not 0
-        if (d > 0) {
-          // add transformed pixel as well as pixel color to the list
-          PVector pos = trans.transform(u, v, d);
-          points.add(pos);
-          point_colors.append(rgb.get(u, v));
-          // accumulate z value
-          cloud_mass = PVector.add(cloud_mass, pos);
+        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;
+        point_colors.append(rgb_img.get(u, v));
+      }
+    for (int v = 0; v < height; v++)
+      for (int u = 0; u < width; u++) {
+        if (int(depth[v * width + u]) == 0) {
+          interpolateDepth(v, u);
         }
+        // add transformed pixel as well as pixel color to the list
+        PVector pos = trans.transform(u, v, int(depth[v * width + u]));
+        points.add(pos);
+        // accumulate z value
+        cloud_mass = PVector.add(cloud_mass, pos);
       }
   }
-
+  void fillInDepthAlongPath(float d, Node node) {
+    node = node.parent;
+    while (node != null) {
+      int i = node.row;
+      int j = node.col;
+      if (depth[i * width + j] == 0) {
+        depth[i * width + j] = d;
+      }
+      node = node.parent;
+    }
+  }
+  // interpolate
+  void interpolateDepth(int row, int col) {
+    if (row < 0 || row >= height || col < 0 || col >= width ||
+        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];
+    for (int i = 0; i < width * height; i++) visited[i] = false;
+    visited[row * width + col] = true;
+    // Using BFS to Find the Nearest Neighbor
+    while (queue.size() > 0) {
+      // pop
+      Node node = queue.get(0);
+      queue.remove(0);
+      int i = node.row;
+      int j = node.col;
+      // if current position have a real depth
+      if (depth[i * width + j] != 0 && real[i * width + j]) {
+        fillInDepthAlongPath(depth[i * width + j], node);
+        break;
+      } else {
+        // search unvisited 8 neighbors
+        for (int r = max(0, i - 1); r < min(height, i + 2); r++) {
+          for (int c = max(0, j - 1); c < min(width, j + 2); c++) {
+            if (!visited[r * width + c]) {
+              visited[r * width + c] = true;
+              queue.add(new Node(r, c, node));
+            }
+          }
+        }
+      }
+    }
+  }
+  // get point cloud size
+  int size() { return points.size(); }
+  // get ith position
+  PVector getPosition(int i) {
+    if (i >= points.size()) {
+      println("point position: index " + str(i) + " exceeds");
+      exit();
+    }
+    return points.get(i);
+  }
+  // get ith color
+  color getColor(int i) {
+    if (i >= point_colors.size()) {
+      println("point color: index " + str(i) + " exceeds");
+      exit();
+    }
+    return point_colors.get(i);
+  }
+  // get cloud center
   PVector getCloudCenter() {
     if (points.size() > 0) return PVector.div(cloud_mass, points.size());
     return new PVector(0, 0, 0);
   }
-
-  void render() {
-    for (int i = 0; i < points.size(); i++) {
-      PVector v = points.get(i);
-      stroke(point_colors.get(i));
-      point(v.x, v.y, v.z);
+  // merge two clouds
+  void merge(PointCloud point_cloud) {
+    for (int i = 0; i < point_cloud.size(); i++) {
+      points.add(point_cloud.getPosition(i));
+      point_colors.append(point_cloud.getColor(i));
     }
+    cloud_mass = PVector.add(cloud_mass, point_cloud.cloud_mass);
+  }
+}
+
+class Node {
+  int row, col;
+  Node parent;
+  Node(int row, int col, Node parent) {
+    this.row = row;
+    this.col = col;
+    this.parent = parent;
   }
 }
diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/Scene.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/Scene.pde
new file mode 100644
index 000000000..a9c28f631
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Scene.pde
@@ -0,0 +1,87 @@
+class Scene {
+  Camera camera;
+  PointCloud point_cloud;
+  MotionField motion_field;
+  ArrayList<PVector> last_positions;
+  ArrayList<PVector> current_positions;
+  int[] render_list;
+
+  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;
+        }
+      }
+    }
+  }
+
+  void run() {
+    camera.run();
+    last_positions = current_positions;
+    current_positions = project2Camera();
+    updateRenderList();
+    motion_field.update(last_positions, current_positions, render_list);
+  }
+
+  void render(boolean show_motion_field) {
+    // build mesh
+    camera.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();
+    if (show_motion_field) {
+      camera.close();
+      motion_field.render();
+    }
+  }
+}
diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/Util.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/Util.pde
index c84e5df84..19d124a0b 100644
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/Util.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Util.pde
@@ -13,10 +13,16 @@ void showGrids(int block_size) {
 
 // save the point clould information
 void savePointCloud(PointCloud point_cloud, String file_name) {
-  String[] results = new String[point_cloud.points.size()];
+  String[] positions = new String[point_cloud.points.size()];
+  String[] colors = new String[point_cloud.points.size()];
   for (int i = 0; i < point_cloud.points.size(); i++) {
-    PVector point = point_cloud.points.get(i);
-    results[i] = str(point.x) + ' ' + str(point.y) + ' ' + str(point.z);
+    PVector point = point_cloud.getPosition(i);
+    color point_color = point_cloud.getColor(i);
+    positions[i] = str(point.x) + ' ' + str(point.y) + ' ' + str(point.z);
+    colors[i] = str(((point_color >> 16) & 0xFF) / 255.0) + ' ' +
+                str(((point_color >> 8) & 0xFF) / 255.0) + ' ' +
+                str((point_color & 0xFF) / 255.0);
   }
-  saveStrings(file_name, results);
+  saveStrings(file_name + "_pos.txt", positions);
+  saveStrings(file_name + "_color.txt", colors);
 }
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 e7a8e8202..89fa57b60 100644
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/sketch_3D_reconstruction.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/sketch_3D_reconstruction.pde
@@ -4,23 +4,20 @@
  *University of Munich
  *https://vision.in.tum.de/data/datasets/rgbd-dataset/download#freiburg1_xyz
  */
-PointCloud point_cloud;  // pointCloud object
-Camera cam;              // build camera object
-MotionField mf;          // motion field
+Scene scene;
 void setup() {
   size(640, 480, P3D);
-  // basic settings
+  // default settings
   float focal = 525.0f;        // focal distance of camera
-  int frame_no = 0;            // frame number
+  int frame_no = 118;          // frame number
   float fov = PI / 3;          // field of view
   int block_size = 8;          // block size
   float normalizer = 5000.0f;  // normalizer
   // initialize
-  point_cloud = new PointCloud();
+  PointCloud point_cloud = new PointCloud();
   // synchronized rgb, depth and ground truth
   String head = "../data/";
   String[] rgb_depth_gt = loadStrings(head + "rgb_depth_groundtruth.txt");
-
   // read in rgb and depth image file paths as well as corresponding camera
   // posiiton and quaternion
   String[] info = split(rgb_depth_gt[frame_no], ' ');
@@ -38,14 +35,15 @@ void setup() {
   PImage depth = loadImage(depth_path);
   // generate point cloud
   point_cloud.generate(rgb, depth, trans);
-  // get the center of cloud
-  PVector cloud_center = point_cloud.getCloudCenter();
   // initialize camera
-  cam =
-      new Camera(fov, new PVector(0, 0, 0), cloud_center, new PVector(0, 1, 0));
+  Camera camera = new Camera(fov, new PVector(0, 0, 0), new PVector(0, 0, 1),
+                             new PVector(0, 1, 0));
   // initialize motion field
-  mf = new MotionField(cam, point_cloud, block_size);
+  MotionField motion_field = new MotionField(block_size);
+  // initialize scene
+  scene = new Scene(camera, point_cloud, motion_field);
 }
+boolean inter = false;
 void draw() {
   background(0);
   // run camera dragged mouse to rotate camera
@@ -59,11 +57,11 @@ void draw() {
   //- decrease move speed
   // r: rotate the camera
   // b: reset to initial position
-  cam.run();
-  // render the point lists
-  point_cloud.render();
-  // update motion field
-  mf.run();
-  // draw motion field
-  mf.showMotionField();
+  scene.run();  // true: make interpolation; false: do not make
+                // interpolation
+  if (keyPressed && key == 'o') {
+    inter = true;
+  }
+  scene.render(
+      true);  // true: turn on motion field; false: turn off motion field
 }