ref: 20103ec897f1b8cc3d44f491fc5c040b4223ea72
parent: 99be4b1476e140ebd94e168913cff0def3aa92dc
author: Dan Zhu <zxdan@google.com>
date: Fri Jun 14 07:42:01 EDT 2019
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
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/Camera.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Camera.pde
@@ -19,6 +19,14 @@
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 @@
}
}
}
- 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);
}
}
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/MotionField.pde
@@ -1,89 +1,30 @@
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);
- }
+ motion_field = new ArrayList<PVector>();
}
- 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);
- }
- }
- }
+ 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) {@@ -90,13 +31,11 @@
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++) {--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/PointCloud.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/PointCloud.pde
@@ -2,41 +2,132 @@
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;
}
}
--- /dev/null
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Scene.pde
@@ -1,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();
+ }
+ }
+}
--- a/tools/3D-Reconstruction/sketch_3D_reconstruction/Util.pde
+++ b/tools/3D-Reconstruction/sketch_3D_reconstruction/Util.pde
@@ -13,10 +13,16 @@
// 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);
}
--- 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 @@
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 @@
//- 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
}