pythonopen3d

Projection issue in open3D cpp

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.enter image description here

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?

Solution

  • There are a few bugs which is causing this issue.

    1. u,v corresponds to image width and height but your if check is w.r.t height and width respectively. Accordingly, it should be depth[v, u] = d, not depth[u, v] = d.
    2. You forgot to normalize XYZ coordinates in camera or pixel coordinate system, i.e. - new_point3d = new_point3d/new_point3d[2]
    3. Depth image cannot store values from 1mm to 10000mm (10 m with depth scale 1000) if the type is uint8. Use 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()