Open3D Python中点云投影至RGBD图像偏移问题排查

问题背景 #

尝试复刻project_to_rgbd函数实现点云转RGBD图像，基于Open3D 0.16.1版本（新版本可视化器存在指针问题）。代码无语法报错，但投影结果存在偏移：测试用随机立方体点云在可视化窗口中位于画面中央，但生成的RGBD图像中点云投影位置偏移。

代码问题分析 #

原代码存在4个核心错误导致投影偏移：

• 类方法定义错误：project_to_rgbd函数未作为Projector类的成员方法定义（缩进错误），虽未报错但不符合类封装逻辑，且易引发调用问题。
• 透视投影未做归一化：相机透视投影需将归一化平面坐标除以Z分量（相机坐标系下的深度），原代码仅执行内参矩阵与点的乘法，未完成除以Z的步骤，导致坐标缩放错误。
• 图像坐标索引混淆：Open3D图像的维度为(height, width)，对应行（v）和列（u）。原代码将投影后的u（列索引）作为图像的行索引，v（行索引）作为列索引，完全颠倒了坐标映射关系。
• 深度图像数据类型溢出：原代码用uint8存储深度值，当depth_scale=1000、depth_max=10时，最大深度值为10000，远超uint8的0-255范围，导致深度值溢出失真。

修正后的代码 #

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
                        ):
        # 改用uint16存储深度，避免溢出
        depth = np.zeros((height, width, 1), dtype=np.uint16)
        color = np.zeros((height, width, 3), dtype=np.uint8)
        
        for i in range(self.n):
            point4d = np.append(self.points[i], 1)
            # 点云坐标转换到相机坐标系
            cam_point4d = np.matmul(extrinsic, point4d)
            cam_point3d = cam_point4d[:-1]
            zc = cam_point3d[2]
            
            # 过滤相机坐标系下Z<=0（在相机后方）或超出深度范围的点
            if zc <= 0 or zc > depth_max:
                continue
            
            # 执行透视投影：内参乘相机坐标后除以Z分量
            proj_point = np.matmul(intrinsic, cam_point3d) / zc
            u = int(round(proj_point[0]))
            v = int(round(proj_point[1]))
            
            # 过滤超出图像尺寸的坐标
            if u < 0 or u >= width or v < 0 or v >= height:
                continue
            
            # 写入深度和颜色，注意索引顺序是[v, u]（行，列）
            depth[v, u] = zc * depth_scale
            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_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)
rgbd_image = p.project_to_rgbd(cam.intrinsic.width,
                              cam.intrinsic.height,
                              cam.intrinsic.intrinsic_matrix,
                              cam.extrinsic,
                              1000,
                              10)

# 可选：可视化投影后的RGBD图像
o3d.visualization.draw_geometries([o3d.geometry.RGBDImage.create_point_cloud_from_rgbd_image(
    rgbd_image, cam.intrinsic, cam.extrinsic)])

关键修正说明 #

1. 调整project_to_rgbd的缩进，使其成为Projector类的成员方法。
2. 增加透视投影的归一化步骤：proj_point = np.matmul(intrinsic, cam_point3d) / zc，确保坐标映射符合相机透视模型。
3. 修正图像索引顺序：将depth[u,v]改为depth[v,u]，color[u,v,:]改为color[v,u,:]，匹配Open3D图像的(height, width)维度定义。
4. 将深度图像的数据类型从uint8改为uint16，避免深度值溢出。

内容的提问来源于stack exchange，提问作者wakkko