2. 从深度图像生成点云

任务目标

本案例将使用模拟的深度相机内参，把一张640×480的深度图像（每个像素存储毫米单位的深度值）转换为相机坐标系下的三维点云，并以XYZ格式保存，同时输出点云数量与有效点比例用于验证。

环境与依赖

需要 numpy 和 Pillow（用于读取PNG深度图）。若需可视化，可选用 open3d 或 matplotlib，但本案例核心转换不依赖它们。

实现原理

根据针孔相机模型，给定焦距 fx, fy 和光心 cx, cy，深度图中像素 (u, v) 处深度值 d（单位：米）对应的三维坐标为：

x = (u - cx) * d / fx
y = (v - cy) * d / fy
z = d

深度值为0的像素视为无效测量，直接剔除。

关键完整代码

import numpy as np
from PIL import Image

def depth_to_pointcloud(depth_image_path, fx, fy, cx, cy, depth_scale=0.001):
    """
    将深度图像(16bit PNG, 单位mm)转换为三维点云。
    返回: points (N,3) float32, 单位米; valid_ratio (float)
    """
    depth = np.array(Image.open(depth_image_path), dtype=np.float32)
    h, w = depth.shape
    # 生成像素坐标网格
    v, u = np.mgrid[0:h, 0:w].astype(np.float32)
    # 深度单位转换: mm -> m
    d = depth * depth_scale
    # 有效掩码: 深度值 > 0
    mask = d > 0
    # 反投影
    x = (u - cx) * d / fx
    y = (v - cy) * d / fy
    z = d
    # 拼接并过滤
    points = np.stack((x, y, z), axis=-1)[mask]
    valid_ratio = mask.sum() / (h * w)
    return points, valid_ratio

# 模拟深度相机参数 (640x480, 水平视场角约60°)
fx = fy = 525.0
cx, cy = 319.5, 239.5

# 生成一张模拟深度图用于演示 (平面场景: 距离2米处的平面)
h, w = 480, 640
depth_mm = np.full((h, w), 2000, dtype=np.uint16)  # 2000mm = 2m
# 添加一个矩形凸起区域 (距离1.5米)
depth_mm[150:330, 200:440] = 1500
# 添加一个圆形空洞 (无效区域)
yy, xx = np.mgrid[0:h, 0:w]
circle_mask = (xx - 400)**2 + (yy - 100)**2 < 50**2
depth_mm[circle_mask] = 0
Image.fromarray(depth_mm).save('sim_depth.png')

# 执行转换
points, ratio = depth_to_pointcloud('sim_depth.png', fx, fy, cx, cy)
print(f"有效点比例: {ratio:.2%}")
print(f"点云点数: {len(points)}")

# 保存为XYZ格式 (每行: x y z)
np.savetxt('pointcloud.xyz', points, fmt='%.3f')
print("点云已保存至 pointcloud.xyz")

运行与验证

1. 将上述代码保存为 depth2pcd.py，在终端运行 python depth2pcd.py。
2. 程序会生成模拟深度图 sim_depth.png，并输出点云文件 pointcloud.xyz。
3. 验证方法：检查输出的有效点比例。模拟图中总像素为 640×480=307200，其中圆形空洞面积约 π×50²≈7854 像素，因此有效点比例应约为 (307200-7854)/307200 ≈ 97.44%。点云数量应约为 299346。
4. 打开 pointcloud.xyz，前几行应类似 0.000 0.000 2.000（左上角像素），且所有点的z值只应为 1.500 或 2.000（对应两个深度平面）。

可视化（可选）

若已安装 open3d，可运行以下代码快速查看点云形状：

import open3d as o3d
import numpy as np
pcd = o3d.geometry.PointCloud()
pcd.points = o3d.utility.Vector3dVector(np.loadtxt('pointcloud.xyz'))
o3d.visualization.draw_geometries([pcd])

预期可看到两个平行平面（2m处的大平面和1.5m处的矩形凸起），且圆形区域无点。

常见问题

• 深度图单位：本案例假设PNG为16bit且单位是毫米。若深度图单位是米，需将 depth_scale 改为1.0。
• 坐标方向：生成的x轴向右，y轴向下，z轴指向场景前方（与相机光轴一致）。这是OpenCV/ROS中常见的相机坐标系约定。