ref: 618a7d68f8a5b925d92c24f88cb1b4292ec9a348
parent: d9f0106d9dd8b8adbfa716f32c787d81327de790
author: Dan Zhu <[email protected]>
date: Fri Jun 7 10:54:30 EDT 2019
3D reconstruction tool build by Processing (a java based language for data visualization) add MotionField module reformat the code by using newest clang-format version add necessary comments add new functions move basic settings to setup Change-Id: I64a6b2daec06037daa9e54c6b8d1eebe58aa6de0
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Camera.pde
@@ -1,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);
+ }
+}
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde
@@ -1,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);
+ }
+ }
+}
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/PointCloud.pde
@@ -1,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);
+ }
+ }
+}
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Transform.pde
@@ -1,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]);
+}
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Util.pde
@@ -1,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);
+}
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/sketch_3D_reconstruction.pde
@@ -1,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();
+}