3. 从RGB-D图像生成彩色点云

任务目标

给定同一视角下的RGB彩色图像与对应的深度图像（16位单通道PNG，单位mm），生成包含XYZ坐标和RGB颜色的彩色点云，并保存为PCD格式文件。本案例使用合成数据演示完整流程，不依赖真实传感器。

环境依赖

需要安装：numpy、opencv-python、open3d（仅用于PCD保存与可视化验证）。相机内参在本案例中直接给定，无需标定。

核心实现

import numpy as np
import cv2
import open3d as o3d

# 相机内参（合成数据设定）
FX, FY = 525.0, 525.0
CX, CY = 319.5, 239.5

def generate_synthetic_data(h=480, w=640):
    """生成一张简单的合成RGB图与深度图（单位mm）"""
    # 深度：平面z=1000mm，中心叠加一个半径100px的球面凸起
    y, x = np.mgrid[0:h, 0:w].astype(np.float32)
    depth = np.full((h, w), 1000.0, dtype=np.float32)
    dist = np.sqrt((x - CX)**2 + (y - CY)**2)
    sphere_mask = dist < 100
    depth[sphere_mask] = 1000.0 - 50.0 * np.sqrt(1 - (dist[sphere_mask]/100)**2)
    
    # RGB：背景为灰色，球体区域为蓝色
    rgb = np.full((h, w, 3), 128, dtype=np.uint8)
    rgb[sphere_mask] = [200, 100, 50]  # BGR格式
    return rgb, depth

def rgbd_to_colored_pointcloud(rgb_img, depth_img):
    """将RGB-D图像转换为彩色点云（单位：米）"""
    h, w = depth_img.shape
    # 生成像素坐标网格
    v, u = np.mgrid[0:h, 0:w].astype(np.float32)
    
    # 反投影：z = depth/1000（mm转m），x=(u-cx)*z/fx, y=(v-cy)*z/fy
    z = depth_img / 1000.0
    x = (u - CX) * z / FX
    y = (v - CY) * z / FY
    
    # 堆叠为Nx3点云，并过滤无效深度（z=0或无穷）
    points = np.stack([x, y, z], axis=-1).reshape(-1, 3)
    colors = rgb_img.reshape(-, 3).astype(np.float32) / 255.0  # 归一化到[0,1]
    
    valid = np.isfinite(points).all(axis=1) & (points[:, 2] > 0)
    return points[valid], colors[valid]

# 生成合成数据
rgb_img, depth_img = generate_synthetic_data()

# 生成彩色点云
points, colors = rgbd_to_colored_pointcloud(rgb_img, depth_img)

# 构建Open3D点云对象并保存为PCD
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(points)
pcd.colors = o3d.utility.Vector3dVector(colors)
o3d.io.write_point_cloud("colored_pointcloud.pcd", pcd)

print(f"点云点数: {len(points)}")
print("已保存: colored_pointcloud.pcd")

运行与验证

1. 将上述代码保存为 rgbd_to_pcd.py，在终端运行 python rgbd_to_pcd.py。
2. 程序执行后，当前目录下应生成 colored_pointcloud.pcd 文件。
3. 使用以下命令可视化验证点云内容：

python -c "import open3d as o3d; pcd=o3d.io.read_point_cloud('colored_pointcloud.pcd'); o3d.visualization.draw_geometries([pcd])"

通过条件：可视化窗口中应看到约30万个点，整体呈灰色平面，中心区域有蓝色球面凸起，且点云颜色与原始RGB图像一致。终端输出的点云点数应接近640×480=307200（去除无效点后略少）。

关键说明

• 深度图单位必须为毫米，代码中除以1000转换为米，与Open3D默认单位一致。
• RGB图像需为BGR顺序（OpenCV默认），转换颜色时直接reshape即可，无需通道转换。
• 若使用真实RGB-D相机（如Kinect），需将深度图无效像素（值为0或超过量程）过滤，本案例合成数据已保证深度有效。