From 20103ec897f1b8cc3d44f491fc5c040b4223ea72 Mon Sep 17 00:00:00 2001 From: Dan Zhu Date: Fri, 14 Jun 2019 11:42:01 -0700 Subject: [PATCH] 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 --- .../sketch_3D_reconstruction/Camera.pde | 16 ++- .../sketch_3D_reconstruction/MotionField.pde | 101 +++----------- .../sketch_3D_reconstruction/PointCloud.pde | 131 +++++++++++++++--- .../sketch_3D_reconstruction/Scene.pde | 87 ++++++++++++ .../sketch_3D_reconstruction/Util.pde | 14 +- .../sketch_3D_reconstruction.pde | 36 +++-- 6 files changed, 260 insertions(+), 125 deletions(-) create mode 100644 tools/3D-Reconstruction/sketch_3D_reconstruction/Scene.pde 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 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; + MotionField(int block_size) { 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); - } + motion_field = new ArrayList(); } - 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); - } - } - } + void update(ArrayList last_positions, + ArrayList current_positions, int[] render_list) { + // build motion field 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)); - } + // 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 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(); 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 queue = new ArrayList(); + 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 last_positions; + ArrayList 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 project2Camera() { + ArrayList projs = new ArrayList(); + 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 } -- 2.40.0