From 618a7d68f8a5b925d92c24f88cb1b4292ec9a348 Mon Sep 17 00:00:00 2001 From: Dan Zhu Date: Fri, 7 Jun 2019 14:54:30 -0700 Subject: [PATCH] 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 --- .../sketch_3D_reconstruction/Camera.pde | 119 +++++++++++++++++++++ .../sketch_3D_reconstruction/MotionField.pde | 110 +++++++++++++++++++ .../sketch_3D_reconstruction/PointCloud.pde | 42 ++++++++ .../sketch_3D_reconstruction/Transform.pde | 82 ++++++++++++++ .../sketch_3D_reconstruction/Util.pde | 22 ++++ .../sketch_3D_reconstruction.pde | 69 ++++++++++++ 6 files changed, 444 insertions(+) create mode 100644 tools/3D-Reconstruction/sketch_3D_reconstruction/Camera.pde create mode 100644 tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde create mode 100644 tools/3D-Reconstruction/sketch_3D_reconstruction/PointCloud.pde create mode 100644 tools/3D-Reconstruction/sketch_3D_reconstruction/Transform.pde create mode 100644 tools/3D-Reconstruction/sketch_3D_reconstruction/Util.pde create mode 100644 tools/3D-Reconstruction/sketch_3D_reconstruction/sketch_3D_reconstruction.pde 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 0000000..e8dae77 --- /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 0000000..48a2c56 --- /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 last_positions; + ArrayList current_positions; + ArrayList 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(); + this.current_positions = new ArrayList(); + this.motion_field = new ArrayList(); + 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(); + 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 getMotionField() { + ArrayList depth = new ArrayList(); + 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(); + 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 0000000..8885d32 --- /dev/null +++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/PointCloud.pde @@ -0,0 +1,42 @@ +class PointCloud { + ArrayList points; // array to save points + IntList point_colors; // array to save points color + PVector cloud_mass; + PointCloud() { + // initialize + points = new ArrayList(); + 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 0000000..e4bd5f5 --- /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 0000000..c84e5df --- /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 0000000..e7a8e82 --- /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(); +} -- 2.7.4