4. 深度相机点云获取

任务目标

本案例使用Intel RealSense深度相机（以D435i为例）完成以下闭环：获取彩色图像与对齐后的深度图像，并基于相机内参将深度图转换为彩色点云（XYZRGB），最终以PLY格式保存，供后续机器人抓取位姿估计使用。

环境依赖

• Python 3.8+
• pyrealsense2（RealSense SDK）
• OpenCV（opencv-python）
• NumPy
• Open3D（用于点云保存与可视化验证）

安装命令：pip install pyrealsense2 opencv-python numpy open3d

关键完整代码

import pyrealsense2 as rs
import numpy as np
import cv2
import open3d as o3d

# 1. 配置相机流：彩色1280x720@30fps，深度1280x720@30fps
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.color, 1280, 720, rs.format.bgr8, 30)
config.enable_stream(rs.stream.depth, 1280, 720, rs.format.z16, 30)

# 2. 启动相机并获取对齐后的深度帧
profile = pipeline.start(config)
align = rs.align(rs.stream.color)  # 将深度图对齐到彩色图坐标系

try:
    # 等待稳定帧（跳过前5帧）
    for _ in range(5):
        pipeline.wait_for_frames()

    frames = pipeline.wait_for_frames()
    aligned_frames = align.process(frames)

    color_frame = aligned_frames.get_color_frame()
    depth_frame = aligned_frames.get_depth_frame()
    if not color_frame or not depth_frame:
        raise RuntimeError("无法获取对齐后的彩色/深度帧")

    # 3. 转换为numpy数组
    color_image = np.asanyarray(color_frame.get_data())      # BGR
    depth_image = np.asanyarray(depth_frame.get_data())      # 单位：毫米

    # 4. 获取相机内参（基于彩色流）
    intr = profile.get_stream(rs.stream.color).as_video_stream_profile().get_intrinsics()
    fx, fy = intr.fx, intr.fy
    cx, cy = intr.ppx, intr.ppy

    # 5. 生成点云（仅保留深度有效点）
    h, w = depth_image.shape
    K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])

    # 构建像素坐标网格
    u, v = np.meshgrid(np.arange(w), np.arange(h))
    z = depth_image.astype(np.float64) / 1000.0  # 毫米转米

    # 反投影：p_cam = K^{-1} * [u, v, 1]^T * z
    ones = np.ones_like(u)
    pixels = np.stack([u, v, ones], axis=-1).reshape(-1, 3)  # (N,3)
    K_inv = np.linalg.inv(K)
    cam_coords = (K_inv @ pixels.T).T * z.reshape(-1, 1)     # (N,3)

    # 提取颜色（BGR转RGB）
    colors = color_image.reshape(-1, 3)[:, ::-1]  # 转为RGB

    # 过滤深度为0的无效点
    valid = z.reshape(-1) > 0
    points = cam_coords[valid]
    point_colors = colors[valid]

    # 6. 构建Open3D点云并保存为PLY
    pcd = o3d.geometry.PointCloud()
    pcd.points = o3d.utility.Vector3dVector(points)
    pcd.colors = o3d.utility.Vector3dVector(point_colors.astype(np.float64) / 255.0)
    o3d.io.write_point_cloud("scene_pointcloud.ply", pcd)

    print(f"点云生成完成：{len(points)} 个点，已保存为 scene_pointcloud.ply")

    # 7. 可视化验证（可选）
    o3d.visualization.draw_geometries([pcd])

finally:
    pipeline.stop()

运行与验证

1. 运行：将代码保存为capture_pointcloud.py，连接RealSense相机后执行python capture_pointcloud.py。
2. 预期输出：终端打印点云点数，当前目录生成scene_pointcloud.ply文件，并弹出Open3D可视化窗口显示彩色3D点云。
3. 通过条件：
   - 点云文件存在且非空（文件大小通常大于1MB）；
   - 可视化窗口中场景物体轮廓清晰、颜色与真实场景一致；
   - 点云中物体边缘无明显错位（验证深度对齐正确）。

常见问题

• 无相机连接：检查USB 3.0接口及驱动，运行rs-enumerate-devices确认设备可见。
• 深度图全零：相机启动初期或光照过强导致，增加预热帧数或调整相机位置。
• 点云噪声大：可对深度图应用中值滤波（cv2.medianBlur）后再反投影。