Hi im trying to recreate the project_to_rgbd tensor pointcloud function in python but for a legacy pointcloud. Basically going from a pointcloud to an rgbd image. I have pretty much translated everything, I get no errors but the projection is shifted. To test my projection I create a random cube pointcloud, launch the visualizer and get the camera data from that. When I see it through the visualizer I see the whole cube in the middle but the result I get is this.
Here is the code so you can test:
import open3d as o3d
import numpy as np
class Projector:
def __init__(self, cloud) -> None:
self.cloud = cloud
self.points = np.asarray(cloud.points)
self.colors = np.asarray(cloud.colors)
self.n = len(self.points)
#intri 3x3, extr 4x4
def project_to_rgbd(self,
width,
height,
intrinsic,
extrinsic,
depth_scale,
depth_max
):
depth = np.zeros((height, width, 1), dtype=np.uint8)
color = np.zeros((height, width, 3), dtype=np.uint8)
for i in range(0,self.n):
point4d = np.append(self.points[i],1)
new_point4d = np.matmul(extrinsic, point4d)
point3d = new_point4d[:-1]
new_point3d = np.matmul(intrinsic, point3d)
u = int(round(new_point3d[0]))
v = int(round(new_point3d[1]))
zc = new_point3d[2]
if (u < 0 or u > height-1 or v < 0 or v > width-1 or zc <= 0 or zc > depth_max):
continue
d = zc * depth_scale
depth[u,v] = d
color[u,v,:] = self.colors[i] * 255
im_color = o3d.geometry.Image(color)
im_depth = o3d.geometry.Image(depth)
rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
im_color, im_depth,depth_trunc=100.0, convert_rgb_to_intensity=False)
return rgbd
points = np.random.rand(100000, 3)
colors = np.random.rand(100000, 3)
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)
pcd.colors = o3d.utility.Vector3dVector(colors)
scene = o3d.visualization.Visualizer()
scene.create_window()
scene.add_geometry(pcd)
scene.update_renderer()
scene.poll_events()
view_control = scene.get_view_control()
cam = view_control.convert_to_pinhole_camera_parameters()
p = Projector(pcd)
p.project_to_rgbd(cam.intrinsic.width,
cam.intrinsic.height,
cam.intrinsic.intrinsic_matrix,
cam.extrinsic,
1000,
10)
Also im using open3d 16.1 because newer versions have pointer issues in the visualizer. Does anyone see the issue with what I wrote?
There are a few bugs which is causing this issue.
depth[v, u] = d
, not depth[u, v] = d
.new_point3d = new_point3d/new_point3d[2]
uint16
.Tip - Since you have added code to view pointcloud, then project pointcloud to RGBD. You should have added rgbd to pointcloud code which would have allowed you to visualize if you get back the original pointcloud or not. I have added in below code.
import open3d as o3d
import numpy as np
class Projector:
def __init__(self, cloud) -> None:
self.cloud = cloud
self.points = np.asarray(cloud.points)
self.colors = np.asarray(cloud.colors)
self.n = len(self.points)
# intri 3x3, extr 4x4
def project_to_rgbd(self,
width,
height,
intrinsic,
extrinsic,
depth_scale,
depth_max
):
depth = np.zeros((height, width, 1), dtype=np.uint16)
color = np.zeros((height, width, 3), dtype=np.uint8)
# The commented code here is vectorized but is missing the filtering at the end where projected points are
# outside image bounds and depth bounds.
# world_points = np.asarray(self.points).transpose()
# world_points = np.append(world_points, np.ones((1, world_points.shape[1])), axis=0)
# points_in_ccs = np.matmul(extrinsic, world_points)[:3]
# points_in_ccs = points_in_ccs / points_in_ccs[2, :]
# projected_points = np.matmul(intrinsic, points_in_ccs)
# projected_points = projected_points.astype(np.int16)
for i in range(0, self.n):
point4d = np.append(self.points[i], 1)
new_point4d = np.matmul(extrinsic, point4d)
point3d = new_point4d[:-1]
zc = point3d[2]
new_point3d = np.matmul(intrinsic, point3d)
new_point3d = new_point3d/new_point3d[2]
u = int(round(new_point3d[0]))
v = int(round(new_point3d[1]))
# Fixed u, v checks. u should be checked for width
if (u < 0 or u > width - 1 or v < 0 or v > height - 1 or zc <= 0 or zc > depth_max):
continue
d = zc * depth_scale
depth[v, u ] = d
color[v, u, :] = self.colors[i] * 255
im_color = o3d.geometry.Image(color)
im_depth = o3d.geometry.Image(depth)
rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
im_color, im_depth, depth_scale=1000.0, depth_trunc=depth_max, convert_rgb_to_intensity=False)
return rgbd
# Use unequal length, width and height.
points = np.random.rand(30000, 3) * [1,2,3]
colors = np.random.rand(30000, 3)
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)
pcd.colors = o3d.utility.Vector3dVector(colors)
scene = o3d.visualization.Visualizer()
scene.create_window()
scene.add_geometry(pcd)
scene.update_renderer()
scene.poll_events()
scene.run()
view_control = scene.get_view_control()
cam = view_control.convert_to_pinhole_camera_parameters()
scene.destroy_window()
p = Projector(pcd)
rgbd = p.project_to_rgbd(cam.intrinsic.width,
cam.intrinsic.height,
cam.intrinsic.intrinsic_matrix,
cam.extrinsic,
1000,
10)
pcd1 = o3d.geometry.PointCloud.create_from_rgbd_image(rgbd, intrinsic=cam.intrinsic, extrinsic=cam.extrinsic, project_valid_depth_only=True)
scene = o3d.visualization.Visualizer()
scene.create_window()
scene.add_geometry(pcd1)
scene.run()