1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
|
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;
}
}
|
