ref: d99a0bc7695659a873672686cd7b05441ecbd17e
dir: /tools/3D-Reconstruction/sketch_3D_reconstruction/PointCloud.pde/
class PointCloud { 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_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_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); } // 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; } }