1class PointCloud { 2 ArrayList<PVector> points; // array to save points 3 IntList point_colors; // array to save points color 4 PVector cloud_mass; 5 float[] depth; 6 boolean[] real; 7 PointCloud() { 8 // initialize 9 points = new ArrayList<PVector>(); 10 point_colors = new IntList(); 11 cloud_mass = new PVector(0, 0, 0); 12 depth = new float[width * height]; 13 real = new boolean[width * height]; 14 } 15 16 void generate(PImage rgb_img, PImage depth_img, Transform trans) { 17 if (depth_img.width != width || depth_img.height != height || 18 rgb_img.width != width || rgb_img.height != height) { 19 println("rgb and depth file dimension should be same with window size"); 20 exit(); 21 } 22 // clear depth and real 23 for (int i = 0; i < width * height; i++) { 24 depth[i] = 0; 25 real[i] = false; 26 } 27 for (int v = 0; v < height; v++) 28 for (int u = 0; u < width; u++) { 29 // get depth value (red channel) 30 color depth_px = depth_img.get(u, v); 31 depth[v * width + u] = depth_px & 0x0000FFFF; 32 if (int(depth[v * width + u]) != 0) { 33 real[v * width + u] = true; 34 } 35 point_colors.append(rgb_img.get(u, v)); 36 } 37 for (int v = 0; v < height; v++) 38 for (int u = 0; u < width; u++) { 39 if (int(depth[v * width + u]) == 0) { 40 interpolateDepth(v, u); 41 } 42 // add transformed pixel as well as pixel color to the list 43 PVector pos = trans.transform(u, v, int(depth[v * width + u])); 44 points.add(pos); 45 // accumulate z value 46 cloud_mass = PVector.add(cloud_mass, pos); 47 } 48 } 49 void fillInDepthAlongPath(float d, Node node) { 50 node = node.parent; 51 while (node != null) { 52 int i = node.row; 53 int j = node.col; 54 if (depth[i * width + j] == 0) { 55 depth[i * width + j] = d; 56 } 57 node = node.parent; 58 } 59 } 60 // interpolate 61 void interpolateDepth(int row, int col) { 62 if (row < 0 || row >= height || col < 0 || col >= width || 63 int(depth[row * width + col]) != 0) { 64 return; 65 } 66 ArrayList<Node> queue = new ArrayList<Node>(); 67 queue.add(new Node(row, col, null)); 68 boolean[] visited = new boolean[width * height]; 69 for (int i = 0; i < width * height; i++) visited[i] = false; 70 visited[row * width + col] = true; 71 // Using BFS to Find the Nearest Neighbor 72 while (queue.size() > 0) { 73 // pop 74 Node node = queue.get(0); 75 queue.remove(0); 76 int i = node.row; 77 int j = node.col; 78 // if current position have a real depth 79 if (depth[i * width + j] != 0 && real[i * width + j]) { 80 fillInDepthAlongPath(depth[i * width + j], node); 81 break; 82 } else { 83 // search unvisited 8 neighbors 84 for (int r = max(0, i - 1); r < min(height, i + 2); r++) { 85 for (int c = max(0, j - 1); c < min(width, j + 2); c++) { 86 if (!visited[r * width + c]) { 87 visited[r * width + c] = true; 88 queue.add(new Node(r, c, node)); 89 } 90 } 91 } 92 } 93 } 94 } 95 // get point cloud size 96 int size() { return points.size(); } 97 // get ith position 98 PVector getPosition(int i) { 99 if (i >= points.size()) { 100 println("point position: index " + str(i) + " exceeds"); 101 exit(); 102 } 103 return points.get(i); 104 } 105 // get ith color 106 color getColor(int i) { 107 if (i >= point_colors.size()) { 108 println("point color: index " + str(i) + " exceeds"); 109 exit(); 110 } 111 return point_colors.get(i); 112 } 113 // get cloud center 114 PVector getCloudCenter() { 115 if (points.size() > 0) { 116 return PVector.div(cloud_mass, points.size()); 117 } 118 return new PVector(0, 0, 0); 119 } 120 // merge two clouds 121 void merge(PointCloud point_cloud) { 122 for (int i = 0; i < point_cloud.size(); i++) { 123 points.add(point_cloud.getPosition(i)); 124 point_colors.append(point_cloud.getColor(i)); 125 } 126 cloud_mass = PVector.add(cloud_mass, point_cloud.cloud_mass); 127 } 128} 129 130class Node { 131 int row, col; 132 Node parent; 133 Node(int row, int col, Node parent) { 134 this.row = row; 135 this.col = col; 136 this.parent = parent; 137 } 138} 139