使用 Open3D 点云到 RGB 图像转换中的颜色不匹配和深度不一致

我正在使用 Open3D 将点云数据转换为 RGB 图像。但是，我遇到了输出图像中的颜色与输入点云中的颜色不匹配的问题。此外，本应位于背景的点却出现在前景中。这是我的代码的相关部分：

import numpy as np
import cv2

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)

    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)

        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]))

            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
            if depth[v, u] == 0 or depth[v, u] > d:
                depth[v, u] = d
                color[v, u, :] = (self.colors[i] * 255).astype(np.uint8)

        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

pcd_path = "/home/shirono/Poe/data/1944301031BF721300_1721279605_z20.pcd"
pcd = o3d.io.read_point_cloud(pcd_path)

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)

def save_rgb_from_rgbd(rgbd_image, save_path):
    rgb_image = np.asarray(rgbd_image.color)
    rgb_image = (rgb_image * 255).astype(np.uint8)
    rgb_image = cv2.cvtColor(rgb_image, cv2.COLOR_RGB2BGR)
    cv2.imwrite(save_path, rgb_image)

save_path = "output_rgb_image.png"
save_rgb_from_rgbd(rgbd, save_path)



We thought the conversion from RGB to BGR was wrong and tried but could not improve it.

代码中导致颜色不匹配和深度不一致的原因主要有以下几点：

1. 颜色空间转换错误: 虽然代码中尝试使用 cv2.cvtColor(rgb_image, cv2.COLOR_RGB2BGR) 进行颜色空间转换，但转换的时机不对。应该在将颜色数组 color 从 Open3D 图像转换为 OpenCV 图像之前就进行转换，而不是在保存图像之前才转换。
2. 深度值缩放问题: 代码中使用了 depth_scale 参数，但在创建 RGBDImage 时又设置了 depth_scale=1000.0 ，这可能会导致深度值被缩放两次。应该统一使用一个深度值缩放比例。
3. 深度缓冲区精度问题: 使用 uint16 存储深度值可能会导致精度不足，特别是在场景深度范围较大的情况下。可以考虑使用 float32 存储深度值以提高精度。
4. 投影过程中缺少遮挡检测: 当前的代码只是简单地将所有点投影到图像平面，并没有考虑点之间的遮挡关系。这会导致本应位于背景的点出现在前景。

以下是如何修改代码以解决这些问题的建议：

import numpy as np
import cv2
import open3d as o3d

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)

    def project_to_rgbd(self,
                        width,
                        height,
                        intrinsic,
                        extrinsic,
                        depth_scale,
                        depth_max):
        depth = np.zeros((height, width), dtype=np.float32)
        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]
            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]))

            if u < 0 or u >= width or v < 0 or v >= height or zc <= 0 or zc > depth_max:
                continue

            d = zc * depth_scale
            if depth[v, u] == 0 or depth[v, u] > d:
                depth[v, u] = d
                # 在这里进行颜色空间转换
                color[v, u, :] = (self.colors[i] * 255).astype(np.uint8)[::-1]  # RGB to BGR

        im_color = o3d.geometry.Image(color)
        im_depth = o3d.geometry.Image(depth)
        # 统一使用 depth_scale
        rgbd = o3d.geometry.RGBDImage.create_from_color_and_depth(
            im_color, im_depth, depth_scale=depth_scale, depth_trunc=depth_max, convert_rgb_to_intensity=False)
        return rgbd

# ... 其他代码 ...

p = Projector(pcd)
rgbd = p.project_to_rgbd(cam.intrinsic.width,
                         cam.intrinsic.height,
                         cam.intrinsic.intrinsic_matrix,
                         cam.extrinsic,
                         1000.0,  # 使用相同的 depth_scale
                         10)

def save_rgb_from_rgbd(rgbd_image, save_path):
    rgb_image = np.asarray(rgbd_image.color)
    # 不需要再进行颜色空间转换
    cv2.imwrite(save_path, rgb_image)

# ... 其他代码 ...

此外，为了解决遮挡问题，可以使用更复杂的投影方法，例如光线投射。Open3D 提供了一些功能可以帮助实现这一点，例如 o3d.geometry.RaycastingScene 。

希望这些修改可以解决颜色不匹配和深度不一致的问题。