📚 课程目录第4章深度相机（RGB-D）建模

4、深度相机（RGB-D）建模：RGB与Depth坐标系关系、点云数据流定义、IR投影仪建模

深度相机，说白了就是给机器人装上一双能「看」到距离的眼睛。我最早接触RGB-D传感器是在2015年做服务机器人项目时，那时候Kinect v2刚出来，大家都觉得这东西太神奇了——既能拍彩色照片，又能告诉你每个像素离你多远。

但在URDF里给深度相机建模，很多人容易栽跟头。为什么？因为RGB相机和Depth相机其实是两个独立的传感器，它们有各自的坐标系、各自的光学中心。你想想看，如果这两个坐标系没对齐，点云投影出来就是歪的。

4.1 RGB与Depth坐标系关系

先搞清楚一个核心问题：RGB-D相机内部，彩色摄像头和深度传感器是分开的。它们之间有一个固定的刚体变换，这个变换在URDF里必须明确表达。

核心概念：RGB-D相机本质上是两个相机（RGB + Depth）刚性连接在一起，它们之间的变换矩阵 T_rgb_depth 是出厂标定好的。

我个人习惯把RGB坐标系作为主坐标系，Depth坐标系作为子坐标系。这样在URDF里，Depth link就挂在RGB link下面。举个例子，Intel RealSense D435的典型参数：

参数	RGB相机	Depth相机
分辨率	1920×1080	1280×720
视场角(H×V)	69°×42°	87°×58°
基线距离	—	约55mm（左右IR相机间距）
RGB与Depth偏移	参考系原点	X: +20mm, Y: 0, Z: +10mm

这个偏移量怎么来的？我在项目里踩过坑——直接拿厂家给的CAD模型量出来的。但要注意，不同批次的传感器可能有微小差异，最好用官方标定文件里的数值。

URDF里怎么写？看这段代码：

<link name="camera_rgb_frame">
  <visual>
    <geometry>
      <mesh filename="package://my_robot/meshes/d435_rgb.stl"/>
    </geometry>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </visual>
</link>

<link name="camera_depth_frame">
  <visual>
    <geometry>
      <mesh filename="package://my_robot/meshes/d435_depth.stl"/>
    </geometry>
    <origin xyz="0.02 0 0.01" rpy="0 0 0"/>
  </visual>
</link>

<joint name="rgb_to_depth" type="fixed">
  <parent link="camera_rgb_frame"/>
  <child link="camera_depth_frame"/>
  <origin xyz="0.02 0 0.01" rpy="0 0 0"/>
</joint>

注意：这里的xyz偏移是RGB坐标系到Depth坐标系的平移。我曾经见过有人把方向搞反了，结果点云和RGB图像怎么都对不上。建议用rviz里的TF树检查一下，确保变换方向正确。

4.2 点云数据流定义

深度相机生成点云，本质上是一个数据流管道。从原始深度图到最终的点云，中间要经过好几步。我习惯把这个流程画成一张图，方便团队理解。

这个流程里，我最想强调的是「坐标系变换」这一步。很多新手以为点云生成后直接就能用，其实不然。深度图生成的点云是在depth_optical_frame坐标系下的，而机器人其他部件用的是camera_depth_frame。这两个坐标系之间可能有一个旋转偏移。

举个例子，RealSense的depth_optical_frame是Z轴朝前、X轴朝右、Y轴朝下。但URDF里通常用Z轴朝上。所以你需要加一个固定的旋转：

<joint name="depth_optical_joint" type="fixed">
  <parent link="camera_depth_frame"/>
  <child link="camera_depth_optical_frame"/>
  <origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
</joint>

这个rpy值怎么来的？其实就是把光学坐标系的Z轴转到URDF的Z轴。我刚开始做的时候，对着这个旋转矩阵算了半天，后来发现直接查厂家文档里的坐标系定义最省事。

4.3 IR投影仪建模

IR投影仪是深度相机的「隐形搭档」。它发射红外光斑，两个IR相机捕捉反射光，通过三角测量算出深度。在URDF里，IR投影仪虽然不直接参与点云生成，但它的位置会影响深度图的精度。

为什么要在URDF里建模IR投影仪？我在做机械臂抓取项目时遇到过一个问题——深度相机装在机械臂末端，每次运动时深度图边缘会出现大量噪点。后来发现是IR投影仪的光斑被机械臂结构遮挡了。如果在URDF里把IR投影仪的位置建出来，就能提前做碰撞检测，避免这种问题。

IR投影仪的URDF建模很简单，就是一个link加一个joint：

<link name="ir_projector_frame">
  <visual>
    <geometry>
      <box size="0.02 0.01 0.01"/>
    </geometry>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </visual>
</link>

<joint name="depth_to_ir_projector" type="fixed">
  <parent link="camera_depth_frame"/>
  <child link="ir_projector_frame"/>
  <!-- 以D435为例，IR投影仪在左IR相机右侧约30mm -->
  <origin xyz="-0.03 0 0" rpy="0 0 0"/>
</joint>

小技巧：IR投影仪在可视化里可以设成半透明材质，这样在rviz里能看到它的位置，又不会遮挡其他部件。我一般用<material name="ir_glass"><color rgba="0.5 0.5 0.5 0.3"/></material>。

另外，IR投影仪有一个重要的参数——发散角。它决定了红外光斑的覆盖范围。如果发散角太小，深度图边缘就会出现黑洞。这个参数在URDF里没法直接表达，但可以在传感器的描述文件里加一个属性：

<gazebo reference="ir_projector_frame">
  <sensor type="depth" name="ir_projector">
    <camera>
      <horizontal_fov>1.57</horizontal_fov>
      <image>
        <width>640</width>
        <height>480</height>
      </image>
      <clip>
        <near>0.1</near>
        <far>10.0</far>
      </clip>
    </camera>
  </sensor>
</gazebo>

嗯，这里要注意，Gazebo里的IR投影仪模拟和真实硬件还是有差距的。真实IR投影仪的光斑是散斑图案，不是均匀照明。如果你做仿真，建议用libgazebo_ros_multicamera插件来模拟双目深度，效果更真实。

4.4 完整URDF示例

把上面这些整合起来，一个完整的深度相机URDF结构应该是这样的：

<robot name="rgbd_camera">
  <!-- RGB相机 -->
  <link name="camera_rgb_frame"/>
  <link name="camera_rgb_optical_frame"/>
  <joint name="rgb_optical_joint" type="fixed">
    <parent link="camera_rgb_frame"/>
    <child link="camera_rgb_optical_frame"/>
    <origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
  </joint>

  <!-- Depth相机 -->
  <link name="camera_depth_frame"/>
  <link name="camera_depth_optical_frame"/>
  <joint name="rgb_to_depth" type="fixed">
    <parent link="camera_rgb_frame"/>
    <child link="camera_depth_frame"/>
    <origin xyz="0.02 0 0.01" rpy="0 0 0"/>
  </joint>
  <joint name="depth_optical_joint" type="fixed">
    <parent link="camera_depth_frame"/>
    <child link="camera_depth_optical_frame"/>
    <origin xyz="0 0 0" rpy="-1.5708 0 -1.5708"/>
  </joint>

  <!-- IR投影仪 -->
  <link name="ir_projector_frame"/>
  <joint name="depth_to_ir_projector" type="fixed">
    <parent link="camera_depth_frame"/>
    <child link="ir_projector_frame"/>
    <origin xyz="-0.03 0 0" rpy="0 0 0"/>
  </joint>
</robot>

总结一下：深度相机建模的关键就三点——RGB和Depth的坐标系偏移、点云数据流的变换链路、IR投影仪的位置。把这三点搞清楚了，你的URDF模型就能和真实传感器对上号。

我记得有一次帮客户调试AGV的导航系统，他们用的就是这种URDF结构。一开始点云总是偏，查了两天才发现是depth_optical_joint的rpy写反了。从那以后，我每次写完URDF都会用check_urdf工具验证一下，再用rviz的TF面板手动检查每个坐标系的方向。嗯，这个习惯一直保持到现在。