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);
}
}
}
- 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);
+ }
}
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);
- }
- }
-
- 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);
- }
+ motion_field = new ArrayList<PVector>();
}
- 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) {
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++) {
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
+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();
+ }
+ }
+}
// 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);
}
*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], ' ');
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
//- 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
}
projects / libvpx / commitdiff