Merge "3D reconstruction tool build by Processing"

6 files changed, 444 insertions, 0 deletions

diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/Camera.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/Camera.pde
new file mode 100644
index 000000000..e8dae7709
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Camera.pde
@@ -0,0 +1,119 @@
+class Camera {
+  // camera's field of view
+  float fov;
+  // camera's position, look at point and axis
+  PVector pos, center, axis;
+  PVector init_pos, init_center, init_axis;
+  float move_speed;
+  float rot_speed;
+  Camera(float fov, PVector pos, PVector center, PVector axis) {
+    this.fov = fov;
+    this.pos = pos;
+    this.center = center;
+    this.axis = axis;
+    this.axis.normalize();
+    move_speed = 0.001;
+    rot_speed = 0.01 * PI;
+    init_pos = pos.copy();
+    init_center = center.copy();
+    init_axis = axis.copy();
+  }
+
+  float[] getCameraMat() {
+    float[] mat = new float[9];
+    PVector dir = PVector.sub(center, pos);
+    dir.normalize();
+    PVector left = axis.cross(dir);
+    left.normalize();
+
+    mat[0] = left.x;
+    mat[1] = left.y;
+    mat[2] = left.z;
+    mat[3] = axis.x;
+    mat[4] = axis.y;
+    mat[5] = axis.z;
+    mat[6] = dir.x;
+    mat[7] = dir.y;
+    mat[8] = dir.z;
+
+    return mat;
+  }
+
+  void run() {
+    PVector dir, left;
+    if (mousePressed) {
+      float angleX = (float)mouseX / width * PI - PI / 2;
+      float angleY = (float)mouseY / height * PI - PI;
+      PVector diff = PVector.sub(center, pos);
+      float radius = diff.mag();
+      pos.x = radius * sin(angleY) * sin(angleX) + center.x;
+      pos.y = radius * cos(angleY) + center.y;
+      pos.z = radius * sin(angleY) * cos(angleX) + center.z;
+      dir = PVector.sub(center, pos);
+      dir.normalize();
+      PVector up = new PVector(0, 1, 0);
+      left = up.cross(dir);
+      left.normalize();
+      axis = dir.cross(left);
+      axis.normalize();
+    }
+
+    if (keyPressed) {
+      switch (key) {
+        case 'w':
+          dir = PVector.sub(center, pos);
+          dir.normalize();
+          pos = PVector.add(pos, PVector.mult(dir, move_speed));
+          center = PVector.add(center, PVector.mult(dir, move_speed));
+          break;
+        case 's':
+          dir = PVector.sub(center, pos);
+          dir.normalize();
+          pos = PVector.sub(pos, PVector.mult(dir, move_speed));
+          center = PVector.sub(center, PVector.mult(dir, move_speed));
+          break;
+        case 'a':
+          dir = PVector.sub(center, pos);
+          dir.normalize();
+          left = axis.cross(dir);
+          left.normalize();
+          pos = PVector.add(pos, PVector.mult(left, move_speed));
+          center = PVector.add(center, PVector.mult(left, move_speed));
+          break;
+        case 'd':
+          dir = PVector.sub(center, pos);
+          dir.normalize();
+          left = axis.cross(dir);
+          left.normalize();
+          pos = PVector.sub(pos, PVector.mult(left, move_speed));
+          center = PVector.sub(center, PVector.mult(left, move_speed));
+          break;
+        case 'r':
+          dir = PVector.sub(center, pos);
+          dir.normalize();
+          float[] mat = getRotationMat3x3(rot_speed, dir.x, dir.y, dir.z);
+          axis = MatxVec3(mat, axis);
+          axis.normalize();
+          break;
+        case 'b':
+          pos = init_pos.copy();
+          center = init_center.copy();
+          axis = init_axis.copy();
+          break;
+        case '+': move_speed *= 2.0f; break;
+        case '-': move_speed /= 2.0; break;
+        case CODED:
+          if (keyCode == UP) {
+            pos = PVector.add(pos, PVector.mult(axis, move_speed));
+            center = PVector.add(center, PVector.mult(axis, move_speed));
+          } else if (keyCode == DOWN) {
+            pos = PVector.sub(pos, PVector.mult(axis, move_speed));
+            center = PVector.sub(center, PVector.mult(axis, move_speed));
+          }
+      }
+    }
+    perspective(fov, float(width) / float(height), 1e-6, 1e5);
+    camera(pos.x, pos.y, pos.z, center.x, center.y, center.z, axis.x, axis.y,
+           axis.z);
+  }
+}
diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde
new file mode 100644
index 000000000..48a2c56a0
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde
@@ -0,0 +1,110 @@
+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;
+    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);
+    }
+  }
+
+  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);
+        }
+      }
+    }
+    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));
+    }
+    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();
+    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++) {
+        PVector mv = motion_field.get(i * c_num + j);
+        float ox = j * block_size + 0.5f * block_size;
+        float oy = i * block_size + 0.5f * block_size;
+        stroke(255, 0, 0);
+        line(ox, oy, ox + mv.x, oy + mv.y);
+      }
+  }
+}
diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/PointCloud.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/PointCloud.pde
new file mode 100644
index 000000000..8885d32b7
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/PointCloud.pde
@@ -0,0 +1,42 @@
+class PointCloud {
+  ArrayList<PVector> points;  // array to save points
+  IntList point_colors;       // array to save points color
+  PVector cloud_mass;
+  PointCloud() {
+    // initialize
+    points = new ArrayList<PVector>();
+    point_colors = new IntList();
+    cloud_mass = new PVector(0, 0, 0);
+  }
+
+  void generate(PImage rgb, PImage depth, Transform trans) {
+    for (int v = 0; v < depth.height; v++)
+      for (int u = 0; u < depth.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);
+        }
+      }
+  }
+
+  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);
+    }
+  }
+}
diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/Transform.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/Transform.pde
new file mode 100644
index 000000000..e4bd5f541
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Transform.pde
@@ -0,0 +1,82 @@
+class Transform {
+  float[] inv_rot;  // inverse of rotation matrix
+  PVector inv_mov;  // inverse of movement vector
+  float focal;      // the focal distacne of real camera
+  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) {
+    // 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.w = w;
+    this.h = h;
+    this.normalier = normalier;
+  }
+
+  PVector transform(int i, int j, float d) {
+    // transfer from camera view to world view
+    float z = d / normalier;
+    float x = (i - w / 2.0f) * z / focal;
+    float y = (j - h / 2.0f) * z / focal;
+    return new PVector(x, y, z);
+  }
+}
+
+// get rotation matrix by using rotation axis and angle
+float[] getRotationMat3x3(float angle, float ax, float ay, float az) {
+  float[] mat = new float[9];
+  float c = cos(angle);
+  float s = sin(angle);
+  mat[0] = c + ax * ax * (1 - c);
+  mat[1] = ax * ay * (1 - c) - az * s;
+  mat[2] = ax * az * (1 - c) + ay * s;
+  mat[3] = ay * ax * (1 - c) + az * s;
+  mat[4] = c + ay * ay * (1 - c);
+  mat[5] = ay * az * (1 - c) - ax * s;
+  mat[6] = az * ax * (1 - c) - ay * s;
+  mat[7] = az * ay * (1 - c) + ax * s;
+  mat[8] = c + az * az * (1 - c);
+  return mat;
+}
+
+// get rotation matrix by using quaternion
+float[] quaternion2Mat3x3(float qx, float qy, float qz, float qw) {
+  float[] mat = new float[9];
+  mat[0] = 1 - 2 * qy * qy - 2 * qz * qz;
+  mat[1] = 2 * qx * qy - 2 * qz * qw;
+  mat[2] = 2 * qx * qz + 2 * qy * qw;
+  mat[3] = 2 * qx * qy + 2 * qz * qw;
+  mat[4] = 1 - 2 * qx * qx - 2 * qz * qz;
+  mat[5] = 2 * qy * qz - 2 * qx * qw;
+  mat[6] = 2 * qx * qz - 2 * qy * qw;
+  mat[7] = 2 * qy * qz + 2 * qx * qw;
+  mat[8] = 1 - 2 * qx * qx - 2 * qy * qy;
+  return mat;
+}
+
+// tranpose a 3x3 matrix
+float[] transpose3x3(float[] mat) {
+  float[] Tmat = new float[9];
+  for (int i = 0; i < 3; i++)
+    for (int j = 0; j < 3; j++) {
+      Tmat[i * 3 + j] = mat[j * 3 + i];
+    }
+  return Tmat;
+}
+
+// multiply a matrix with vector
+PVector MatxVec3(float[] mat, PVector v) {
+  float[] vec = v.array();
+  float[] res = new float[3];
+  for (int i = 0; i < 3; i++) {
+    res[i] = 0.0f;
+    for (int j = 0; j < 3; j++) {
+      res[i] += mat[i * 3 + j] * vec[j];
+    }
+  }
+  return new PVector(res[0], res[1], res[2]);
+}
diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/Util.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/Util.pde
new file mode 100644
index 000000000..c84e5df84
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Util.pde
@@ -0,0 +1,22 @@
+// show grids
+void showGrids(int block_size) {
+  ortho(-width, 0, -height, 0);
+  camera(0, 0, 0, 0, 0, 1, 0, 1, 0);
+  stroke(0, 0, 255);
+  for (int i = 0; i < height; i += block_size) {
+    line(0, i, width, i);
+  }
+  for (int i = 0; i < width; i += block_size) {
+    line(i, 0, i, height);
+  }
+}
+
+// save the point clould information
+void savePointCloud(PointCloud point_cloud, String file_name) {
+  String[] results = 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);
+  }
+  saveStrings(file_name, results);
+}
diff --git a/tools/3D-Reconstruction/sketch_3D_reconstruction/sketch_3D_reconstruction.pde b/tools/3D-Reconstruction/sketch_3D_reconstruction/sketch_3D_reconstruction.pde
new file mode 100644
index 000000000..e7a8e8202
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/sketch_3D_reconstruction.pde
@@ -0,0 +1,69 @@
+/*The dataset is from
+ *Computer Vision Group
+ *TUM Department of Informatics Technical
+ *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
+void setup() {
+  size(640, 480, P3D);
+  // basic settings
+  float focal = 525.0f;        // focal distance of camera
+  int frame_no = 0;            // 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();
+  // 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], ' ');
+  String rgb_path = head + info[1];
+  String depth_path = head + info[3];
+  float tx = float(info[7]), ty = float(info[8]),
+        tz = float(info[9]);  // real camera position
+  float qx = float(info[10]), qy = float(info[11]), qz = float(info[12]),
+        qw = float(info[13]);  // quaternion
+
+  // build transformer
+  Transform trans = new Transform(tx, ty, tz, qx, qy, qz, qw, focal, width,
+                                  height, normalizer);
+  PImage rgb = loadImage(rgb_path);
+  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));
+  // initialize motion field
+  mf = new MotionField(cam, point_cloud, block_size);
+}
+void draw() {
+  background(0);
+  // run camera dragged mouse to rotate camera
+  // w: go forward
+  // s: go backward
+  // a: go left
+  // d: go right
+  // up arrow: go up
+  // down arrow: go down
+  //+ increase move speed
+  //- 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();
+}