上手一个RGBD深度相机：从原理到实践--ROS noetic+Astra S（上）：解读深度测距原理和内外参推导

前言

最近在做项目的时候，项目组丢给了我一个深度相机，今天我们来尝试上手一个实体深度相机。
本教程设计基础相机的原理，使用，标定，和读取。(注：本教程默认大家有ROS1基础，故不对程序进行详细解释)
本教程使用的环境
- ubuntu 20.04 LTS ROS1 noetic
- Astra S

产品基础参数

根据摄像机背后所贴的标签，顺藤摸瓜的爬到Orbbec公司的官网，以下是Astra系列3D摄像头的介绍，大致是一系列的介绍(这里我拿到手的是Astra S(short-range)小范围的深度相机)
往下翻可以看到相机的具体参数
分为两个部分：摄像头规格（Camera Specifications）和物理参数（Physical Parameters）。以下是具体内容：
- 摄像头规格（Camera Specifications）
  1. 深度技术 ：该摄像头采用结构光（Structured Light）技术来获取深度信息。
  2. 波长：摄像头使用850nm的波长进行深度感应。
  3. 深度范围：摄像头能够感应到的深度范围是0.6米到8米，对于S型号，这个范围是0.4米到2米。(short-range)
  4. 深度分辨率/帧率：摄像头能够提供的最高深度分辨率为640x480，帧率为30fps。
  5. 深度视场角：摄像头的深度视场角为水平58.4度，垂直45.5度。
  6. RGB分辨率/帧率：Pro Plus版摄像头的RGB分辨率为1920x1080，帧率为30fps，而其他版本的分辨率为640x480，帧率为30fps。
  7. RGB视场角：Pro Plus版摄像头的RGB视场角为水平66.1度，垂直40.2度，其他版本为水平63.1度，垂直49.4度。
  8. 处理芯片：摄像头使用的是Orbec ASIC处理芯片。
  9. IMU：该摄像头没有配备IMU（惯性测量单元）。
- 物理参数（Physical Parameters）
  1. 数据连接：摄像头通过USB 2.0 Type-A接口进行数据连接。
  2. 电源输入：摄像头的电源输入也是通过USB 2.0 Type-A接口。
  3. 触发器：该摄像头没有配备触发器功能。
  4. 功耗：摄像头的平均功耗小于2.4瓦。
  5. 工作环境：摄像头适用于10°C到40°C的室内环境，湿度范围为10%到85%。
  6. SDK支持：摄像头支持Orbbec OpenNI SDK。
  7. 数据输出：摄像头能够输出点云、深度图、红外或RGB数据。
  8. 尺寸（宽_高_深）：摄像头的尺寸为165毫米 x 48毫米 x 40毫米。
  9. 重量：摄像头的重量为310克。
  10. 安装方式：摄像头的底部有M6螺丝孔，用于安装。

原理

上述产品参数中，有一条比较有意思，深度技术：该摄像头采用结构光（Structured Light）技术来获取深度信息。那啥是结构光呢，这个相机如何利用结构光来获取深度呢，这小节我们来看看。

RGBD：RGB+D

我们先来理解RGBD深度相机，RGBD相机，也称为彩色深度相机或三维相机，是一种结合了传统RGB成像和深度信息捕捉功能的设备。它能够同时输出两个数据流：一个是普通的彩色图像（RGB图像），另一个是深度图（Depth Map），其中包含了场景中每个点的深度信息。
RGBD相机的核心组件通常包括：
1. 彩色摄像头：用于捕捉场景的RGB图像，提供颜色的信息。
2. 深度传感器：用于捕捉场景的深度信息。这可以通过多种技术实现，如结构光、飞行时间（ToF）、立体视觉或激光雷达等。
3. 如下图我收到的这个RGBD相机为例子
深度相机通常使用以下几种技术来测量深度：
1. 结构光技术：通过向场景投射特定的光图案（如条纹或点阵），然后分析这些图案在场景表面的变形来计算深度。后面我们重点分析。
2. 飞行时间（Time-of-Flight, ToF）技术 ：通过测量光从相机发出，照射到场景中的物体并反射回相机的飞行时间来计算深度。(说人话就是计算光飞了多久，换算成距离深度)
3. 立体视觉（Stereo Vision） ：使用两个或多个摄像头从不同的角度拍摄同一场景，通过比较不同视角下的图像来确定深度。(包括对两张图像的角点提取，特征点匹配)
4. 激光雷达（Lidar）：使用激光束来测量从发射到反射回来的时间，或者通过分析激光束的相位变化来计算深度。（这个不用多说了吧）

结构光Structured Light

这里我们详细看一下这个摄像头用到的技术，我们来看维基百科对他的定义
翻译：结构光是一种深度感测技术，它将已知图案（通常是网格或水平条）投影到场景上。当这些图案撞击表面时，它们发生变形的方式使得视觉系统能够计算场景中物体的深度和表面信息，这被用于结构光3D扫描仪中。来张图你就懂了
结构光类型分为线结构光和面结构光:
1. 线结构光通过向物体表面投射一条或多条线形光束，然后捕捉这些光线在物体表面的变形。(通过机械结构改变扫描角度，可以理解成行扫描)
2. 面结构光通过向物体表面投射一个或多个面状光图案（如网格或点阵），然后捕捉这些图案在物体表面的变形。
面结构光又分为正弦曲线光栅条纹组(Sinusoidal Grating Stripes)和格雷码形式(Gray Code Pattern)：
- 正弦曲线光栅条纹是一种周期性变化的条纹图案，其亮度和位置随着正弦函数变化。通过捕捉这些条纹在物体表面的变形，可以计算出物体的三维形状。（高中死去的知识）
- 也就是上面那个图，通过根据相位差和周期可以计算出深度信息
- 格雷码是一种特殊的二进制编码方式，其中连续的码字之间只有一个比特的差异。在三维扫描中，格雷码光图案通常由多个不同的图案组成，每个图案代表格雷码的一个码字。通过比较不同图案的变形，可以计算出物体的三维形状。

ros_astra_camera的使用

其他SDK(不使用)

在官网所提供的购买链接右下方，我们可以看到官方提供的SDK
值得一提的是本人试过其他官方提供的几个ROS的SDK，OrbbecSDK_ROS2和OrbbecSDK_ROS1，但是都出现了多多少少的问题：
- OrbbecSDK_ROS2并不支持Astra S这个相机
- OrbbecSDK_ROS1我试图配置了但是无法激活相机(这个存疑)

ros_astra_camera

ros_astra_camera 是一个为 Orbbec 3D 相机创建的 OpenNI2 ROS 包装器。这个软件包允许在 ROS Kinetic、Melodic 和 Noetic 发行版中使用 Orbbec 3D 相机。通过这个包装器，用户可以在 ROS 环境中方便地使用 Orbbec 相机的三维扫描功能，获取深度信息和彩色图像。链接：ros_astra_camera

配置以及依赖安装

根据这个功能包的README.md，我们来配置使用

安装必要依赖

bash 复制代码

sudo apt install libgflags-dev  ros-$ROS_DISTRO-image-geometry ros-$ROS_DISTRO-camera-info-manager\
ros-$ROS_DISTRO-image-transport ros-$ROS_DISTRO-image-publisher  libusb-1.0-0-dev libeigen3-dev
ros-$ROS_DISTRO-backward-ros libdw-dev

安装libuvc：libuvc 是一个开源的 USB 视频类 (UVC) 设备的跨平台库，它提供了一个用户空间接口来访问 UVC 设备。UVC 是 USB 设备类定义的一部分，它允许计算机通过 USB 接口与视频捕捉设备（如网络摄像头、USB 摄像头等）进行通信。

bash 复制代码

git clone https://github.com/libuvc/libuvc.git
cd libuvc
mkdir build && cd build
cmake .. && make -j4
sudo make install
sudo ldconfig

新建一个工作空间并进入

bash 复制代码

mkdir -p ~/ros_ws/src
cd ~/ros_ws/src

拉取仓库（没安装git的朋友sudo apt install git就行了）

bash 复制代码

git clone https://github.com/orbbec/ros_astra_camera.git

安装udev规则:udev 是一个设备管理器，用于在 Linux 系统中管理设备文件。udev 规则（udev rules）是用于定义如何创建和管理设备节点的配置文件。这些规则通常位于 /etc/udev/rules.d/ 目录下，并以 .rules 为文件扩展名。

bash 复制代码

cd ~/ros_ws
source ./devel/setup.bash
roscd astra_camera
./scripts/create_udev_rules
sudo udevadm control --reload && sudo  udevadm trigger

启动

在正式启动前，我们检查相机已经连接

bash 复制代码

source ./devel/setup.bash 
roslaunch astra_camera list_devices.launch

进入工作空间，打开一个终端,启动相机

bash 复制代码

source ./devel/setup.bash 
roslaunch astra_camera astra.launch

看到终端输出: Start device done就说明摄像头启动成功(这一步如果有报错的朋友根据报错去launch中修正相机参数，我这里使用的Astra S可以直接启动)
我们另外开启一个终端,开启rviz(没安装的朋友记得sudo apt install ros-noetic-rviz)

bash 复制代码

source ./devel/setup.bash 
rviz

点击Add添加组件，选择By topic ,我们就能看到基本信息了
这里我们选择image组件，分别添加/camera/color/image_raw和/camera/depth/image_raw
你就可以看到相机的画面和深度信息了
同理我们选择点云信息
这里我们选择固定坐标系为camera_link，就可以看到深度信息的点云信息了

astra.launch 分析

我们来看看launch文件

xml 复制代码

<launch>
    <!-- unique camera name-->
    <arg name="camera_name" default="camera"/>
    <!-- Hardware depth registration -->
    <arg name="depth_align" default="false"/>
    <arg name="depth_scale" default="1"/>
    <arg name="serial_number" default=""/>
    <arg name="device_num" default="1"/>
    <arg name="vendor_id" default="0x2bc5"/>
    <arg name="product_id" default=""/>
    <arg name="enable_point_cloud" default="true"/>
    <arg name="enable_point_cloud_xyzrgb" default="false"/>
    <arg name="connection_delay" default="100"/> 
    <arg name="color_width" default="640"/>
    <arg name="color_height" default="480"/>
    <arg name="color_fps" default="30"/>
    <arg name="enable_color" default="true"/>
    <arg name="flip_color" default="false"/>
    <arg name="color_format" default="RGB"/>
    <arg name="depth_width" default="640"/>
    <arg name="depth_height" default="480"/>
    <arg name="depth_fps" default="30"/>
    <arg name="enable_depth" default="true"/>
    <arg name="flip_depth" default="false"/>
    <arg name="depth_format" default="Y11"/>
    <arg name="ir_width" default="640"/>
    <arg name="ir_height" default="480"/>
    <arg name="ir_fps" default="30"/>
    <arg name="enable_ir" default="true"/>
    <arg name="ir_format" default="Y10"/>
    <arg name="flip_ir" default="false"/>
    <arg name="publish_tf" default="true"/>
    <arg name="tf_publish_rate" default="10.0"/>
    <arg name="ir_info_uri" default=""/>
    <arg name="color_info_uri" default=""/>
    <arg name="color_depth_synchronization" default="false"/>
    <arg name="oni_log_level" default="verbose"/>
    <arg name="oni_log_to_console" default="false"/>
    <arg name="oni_log_to_file" default="false"/>
    <arg name="enable_d2c_viewer" default="false"/>
    <arg name="enable_publish_extrinsic" default="false"/>
    <!-- software filter 0-off ,1-on (default,support windows/linux/arm/
    ,not support android),2 - Optimized software filtering algorithm,support windows/linux/arm/android-->
    <arg name="soft_filter" default="2"/>
    <!-- software filter param MaxDiff (default 16)-support when SoftFilter =2 -->
    <arg name="soft_filter_max_diff" default="16"/>
    <!-- software filter param maxSpeckleSize -support when SoftFilter =2-->
    <arg name="soft_filter_max_speckle_size" default="480"/>
    <group ns="$(arg camera_name)">
        <node name="camera" pkg="astra_camera" type="astra_camera_node" output="screen">
            <param name="camera_name" value="$(arg camera_name)"/>
            <param name="depth_align" value="$(arg depth_align)"/>
            <param name="depth_scale" value="$(arg depth_scale)"/>
            <param name="serial_number" type="string" value="$(arg serial_number)"/>
            <param name="device_num" value="$(arg device_num)"/>
            <param name="vendor_id" value="$(arg vendor_id)"/>
            <param name="product_id" value="$(arg product_id)"/>
            <param name="enable_point_cloud" value="$(arg enable_point_cloud)"/>
            <param name="enable_point_cloud_xyzrgb" value="$(arg enable_point_cloud_xyzrgb)"/>
            <param name="connection_delay" value="$(arg connection_delay)"/>
            <param name="color_width" value="$(arg color_width)"/>
            <param name="color_height" value="$(arg color_height)"/>
            <param name="color_fps" value="$(arg color_fps)"/>
            <param name="enable_color" value="$(arg enable_color)"/>
            <param name="color_format" value="$(arg color_format)"/>
            <param name="flip_color" value="$(arg flip_color)"/>
            <param name="depth_width" value="$(arg depth_width)"/>
            <param name="depth_height" value="$(arg depth_height)"/>
            <param name="depth_fps" value="$(arg depth_fps)"/>
            <param name="flip_depth" value="$(arg flip_depth)"/>
            <param name="enable_depth" value="$(arg enable_depth)"/>
            <param name="depth_format" value="$(arg depth_format)"/>
            <param name="ir_width" value="$(arg ir_width)"/>
            <param name="ir_height" value="$(arg ir_height)"/>
            <param name="ir_fps" value="$(arg ir_fps)"/>
            <param name="enable_ir" value="$(arg enable_ir)"/>
            <param name="flip_ir" value="$(arg flip_ir)"/>
            <param name="ir_format" value="$(arg ir_format)"/>
            <param name="publish_tf" value="$(arg publish_tf)"/>
            <param name="tf_publish_rate" value="$(arg tf_publish_rate)"/>
            <param name="ir_info_uri" value="$(arg ir_info_uri)"/>
            <param name="color_info_uri" value="$(arg color_info_uri)"/>
            <param name="color_depth_synchronization" value="$(arg color_depth_synchronization)"/>
            <param name="oni_log_level" value="$(arg oni_log_level)"/>
            <param name="oni_log_to_console" value="$(arg oni_log_to_console)"/>
            <param name="oni_log_to_file" value="$(arg oni_log_to_file)"/>
            <param name="enable_d2c_viewer" value="$(arg enable_d2c_viewer)"/>
            <param name="enable_publish_extrinsic" value="$(arg enable_publish_extrinsic)"/>
            <param name="soft_filter" value="$(arg soft_filter)"/>
            <param name="soft_filter_max_diff" value="$(arg soft_filter_max_diff)"/>
            <param name="soft_filter_max_speckle_size" value="$(arg soft_filter_max_speckle_size)"/>
            <remap from="/$(arg camera_name)/depth/color/points" to="/$(arg camera_name)/depth_registered/points"/>
        </node>
    </group>
</launch>

这里分析具体参数
connection_delay: 重新打开设备的延迟时间，以毫秒为单位。
enable_point_cloud: 是否启用点云。
enable_point_cloud_xyzrgb: 是否启用RGB点云。
enable_d2c_viewer: 发布D2C叠加图像（仅用于测试）
device_num: 设备数量。使用多个相机时，需要指定设备数量。
enable_reconfigure: 是否启用ROS动态配置更改，设置为false表示Astra.cfg配置将不起作用。这仅建议用于测试目的。使用时请关闭。
color_width，color_height，color_fps: 彩色流分辨率和帧率。
ir_width，ir_height，ir_fps:IR流分辨率和帧率。(IR 是 "Infrared" 的缩写，中文意思是红外线。)
depth_width，depth_height，depth_fps: 深度流分辨率和帧率。
enable_color: 是否启用RGB相机。当RGB相机使用UVC协议时，此参数无效。
enable_depth: 是否启用深度相机。
enable_ir: 是否启用IR相机。
depth_align: 启用硬件深度到颜色的对齐，启用RGB点云时需要。
depth_scale: 深度图像缩放比例。例如，将其设置为2意味着将大小为320x240的深度图像对齐到大小为640x480的RGB图像。
color_roi_x，color_roi_y，color_roi_width，color_roi_height: 是否裁剪RGB图像。默认为-1，仅在RGB分辨率大于深度分辨率且需要对齐时使用。例如，如果需要将大小为640x400的深度图像与大小为640x480的RGB图像对齐，则需要将color_roi_x设置为0，color_roi_y设置为0，color_roi_width设置为640，color_roi_height设置为400。这将裁剪RGB图像的顶部400像素，与相应的深度ROI对齐。
color_depth_synchronization，启用RGB与深度的同步
use_uvc_camera: 如果RGB相机使用UVC协议，则将此参数设置为true。UVC是当前包括Dabai，Dabai_dcw等的协议。
uvc_product_id:UVC相机的PID。
uvc_camera_format:UVC相机的图像格式。
uvc_retry_count : 有时UVC协议相机在热插拔时无法成功重新连接，需要多次重试。
enable_publish_extrinsic 启用发布相机外参。
oni_log_level: OpenNI的日志级别：详细，信息，警告，错误或无。
oni_log_to_console, 是否将OpenNI日志输出到控制台。
oni_log_to_file: 是否将OpenNI日志输出到文件。默认情况下，它将保存在当前运行程序的路径下的Log文件夹中。针对特殊客户需求：
keep_alive, 是否向固件发送心跳包。默认不启用。
keep_alive_interval, 发送心跳包的时间间隔，以秒为单位。

内外参

内参矩阵原理推导

这里我们来推导是相机内参矩阵，来个问题，什么是相机呢。相机是一种现实3维世界在相机2维屏幕上的一个映射，具体关系可以这样表示
我们假设2D摄像机的点为[X,Y,Z]，其中Z表示这个像素点(X,Y)对应的深度信息，由此我们推断出相机矩阵是一个3✖4的矩阵
摄像机的焦距 𝑓 是指从摄像机镜头的光学中心到成像平面的距离，这个成像平面通常位于摄像机的图像传感器上。
对于相机上的点，我们可以通过三角关系得出下述关系
反解上述矩阵，我们有得到P的表达式，如下
考虑到现实中成像的图像和真实摄像机的终点存在偏移(焦距是摄像头中点计算的)
我们假设相对偏移量为(px,py)，那么我们来修正P矩阵
这里我们先假设相机坐标系和世界坐标系不存在偏移，二者是重合的，我们把P继续进行拆分
其中[I∣0]是一个特定的4×4齐次变换矩阵。这个矩阵由两部分组成：
- 左上角的3×3部分是单位矩阵 I，表示没有旋转。
- 右上角的3×1部分是零向量 0，表示没有沿x、y、z轴的平移。
在齐次坐标下，这个矩阵代表了一个没有旋转和没有平移的变换。它通常用于表示从世界坐标系到摄像机坐标系的变换，其中摄像机和世界坐标系是重合的。
在这种情况下，摄像机内参矩阵 K 直接映射世界坐标系中的点到图像平面上，而不需要考虑任何旋转或平移。
这时候我们就推导出了相机的内参矩阵

进一步考虑

现在我们考虑到摄像机，图像，世界坐标系存在偏差(开始头疼辣)
这里我们来换算摄像机光心C点（即摄像机的光学中心）在世界坐标系中的位置。我们需要分别计算旋转和平移来得出变换式子

我们先来计算偏移量，可以看出现实中的一点相对于光心的偏移量是Xc，而相当于世界坐标系的偏移量是Xw,我们可以得到偏移量是Xw-C
我们来看旋转，我们定义R来表示旋转
如此一来我们得到二者的相对关系

4.这时候我们可以通过这个关系式子，将世界坐标系中的一个点通过旋转和平移操作转换到相机坐标系中。

至此我们得到最终答案

其中 𝑃 是一个 3x4 的投影矩阵。
𝐾 代表相机的内参（intrinsic parameters），通常是一个 3x3 矩阵。
𝑅 代表旋转矩阵（rotation matrix），也是一个 3x3 的矩阵。
[𝐼] 表示单位矩阵（identity matrix）。
𝐶 是相机中心向量，表示为列向量。

内参矩阵通解

合并上述推导，我们得到一般式子

外参

外参（Extrinsic Parameters）在计算机视觉和机器人学中指的是描述摄像机相对于世界坐标系的位置和方向的参数。这些参数通常包括一个旋转矩阵（Rotation Matrix）和一个平移向量（Translation Vector）。外参定义了摄像机坐标系与世界坐标系之间的坐标变换关系。
这下懂了吧（就是上头的R和t），好！懂的明明白白

标定

相机标定（Camera Calibration）是计算机视觉中的一个基本步骤，其目的是确定相机的内参（internal parameters）和可能的外参（external parameters）。内参包括焦距（focal length）、主点（principal point）、畸变系数（distortion coefficients）等，而外参则包括相机的位置和方向。
在ros_astra_camera中，我们可以添加标定的结果到启动文件

xml 复制代码

<launch>
    <!--...-->
    <arg name="ir_info_uri" default="file:///you_ir_camera_calib_path/depth_camera.yaml"/>
    <arg name="color_info_uri" default="file:///you_depth_camera_calib_path/rgb_camera.yaml"/>
    <!--...-->
</launch>

标定的文件大致长这样

yaml 复制代码

image_width: 640
image_height: 480
# The camera name is fixed. The color camera is rgb_camera, the depth/IR camera name is ir_camera
camera_name: rgb_camera
camera_matrix:
  rows: 3
  cols: 3
  data: [517.301, 0, 326.785, 0, 519.291, 244.563, 0, 0, 1]
distortion_model: plumb_bob
distortion_coefficients:
  rows: 1
  cols: 5
  data: [-0.41527, 0.31874, -0.00197, 0.00071, 0]
rectification_matrix:
  rows: 3
  cols: 3
  data: [0.999973, 0.00612598, -0.00406652, -0.00610201, 0.999964, 0.00588094, 0.0041024, -0.00585596, 0.999974 ]
projection_matrix:
  rows: 3
  cols: 4
  data: [517.301, 0, 326.785, -25.3167, 0, 519.291, 244.563, 0.282065, 0, 0, 1, 0.0777703]

小结

本节完完整整的推导了内外参的由来，下一节标定就简单了
如有错误，欢迎指出！！！