一起自学SLAM算法：4.3 相机

连载文章，长期更新，欢迎关注：

写在前面

第1章-ROS入门必备知识

第2章-C++编程范式

第3章-OpenCV图像处理

第4章-机器人传感器

  4.1 惯性测量单元

        4.2 激光雷达

        4.3 相机

        4.4 带编码器的减速电机

第5章-机器人主机

第6章-机器人底盘

第7章-SLAM中的数学基础

第8章-激光SLAM系统

第9章-视觉SLAM系统

第10章-其他SLAM系统

第11章-自主导航中的数学基础

第12章-典型自主导航系统

第13章-机器人SLAM导航综合实战

相机是机器人进行视觉感知的传感器，相当于机器人的眼睛。在机器人中，主要讨论三种类型的相机，即单目相机、双目相机和RGB-D相机，如图4-46所示。

图4-46 三种类型的相机

4.3.1 单目相机

单目相机其实就是大家通常说的摄像头，由镜头和图像传感器（CMOS或CCD）构成。简单点说，摄像头的原理就是小孔成像，成像信息由图像传感器转换成数字图像输出。利用对应的摄像头驱动程序，就可以读取并显示该数字图像。受摄像头制造偏差等因素的影响，原始的数字图像存在畸变问题，需要对摄像头进行标定。最后图像数据可以被压缩成不同的形式，便于传输和访问。

1.单目相机原理

单目相机的原理可以用小孔成像来解释，如图4-47所示。

图4-47 小孔成像原理

世界环境中的物体点P在相机坐标系下的坐标值为物体点P透过光心（O）在图像传感器上形成点，点在像素坐标系下的坐标值为，借助于相机的焦距（f）可以建立如式（4-109）的几何关系。

通过相似三角形的几何关系直接计算出来的成像点 ${P}'=[(\frac{f}{Z}\cdot X),(\frac{f}{Z}\cdot Y)]$ 的坐标值单位为米，而最终要被量化成像素，在像素坐标系下的像素值单位为像素。通常在图像传感器中，列方向量化尺度 $\alpha$ 和行方向量化尺度 $\beta$ 是不一样的，并且由于制造安装等误差，像素坐标系的原点与相机坐标系的原点并不对齐，也就是说最终的像素值还要考虑像素坐标系原点的偏移量 $(c_{x},c_{y})$ 。令 $f_{x}=\alpha f$ ， $f_{y}=\beta f$ ，这样计算就可以全部在像素单位下进行了，整理一下用矩阵的形式表示就是式（4-110）。

上式（4-110）就是相机的无畸变内参模型，式中的矩阵K就是相机内参数。在小孔成像模型中，物体点P是直接沿直线透过光心形成图像点。但是实际的相机前面都是一个大大的镜头，镜头能让更多的光线进入以加快曝光速度，但是镜头会对光线产生折射，这样成像会产生畸变。这种由镜头折射引起的图像畸变叫径向畸变，如图4-48。

图4-48 径向畸变

除了上面的径向畸变外，还有切向畸变。相机镜头和图像传感器平面由于安装误差导致不平行，就引入了切向畸变，如图4-49所示。

图4-49 切向畸变

径向畸变的程度跟像素点距中心的距离r有关，可以用r的泰勒级数来描述畸变的程度，一般采用三阶泰勒级数就能近似了，级数中的系数为 $k_{1}$ 、 $k_{2}$ 和 $k_{3}$ ，对于径向畸变不明显的镜头，只用 $k_{1}$ 和 $k_{2}$ 两个系数，或者只用 $k_{1}$ 就够了。切向畸变程度跟图像传感器安装偏差大小有关，可以用系数 $p_{1}$ 和 $p_{2}$ 描述。引入参数 $k_{1}$ 、 $k_{2}$ 、 $k_{3}$ 、 $p_{1}$ 和 $p_{2}$ ，就是相机的畸变内参模型，如式（4-111）所示。

我们已经了解了相机的无畸变和畸变内参模型，既然有内参对应的就有外参。在内参模型中，世界环境中的物体点P给的都是相机坐标系下的坐标值 $P^{c}=[X^{c},Y^{c},Z^{c}]$ 。而在很多情况下，世界环境中的物体点P给出的是世界坐标系下的坐标值 $P^{w}=[X^{w},Y^{w},Z^{w}]$ 。那么，就需要根据相机在世界坐标系下的位姿（R,t），将 $P^{w}$ 坐标转化到相机坐标系 $P^{c}$ ，相机的成像模型就可以写成式（4-112）。

上式（4-112）就是相机的外参模型，式中的矩阵T就是相机外参数。外参T就是相机在世界坐标系下的位姿，也是SLAM问题中待求的参数之一。

从相机的内参模型来看，已知物体成像信息，无法唯一确定物体点坐标的坐标值，或者说方程前面的系数 $\frac{1}{Z}$ 是无约束的。如图4-47所示，沿光心（O）方向上出现的所有物体点，在相机中的成像都是一样的。简单点说，距离远的大的物体和距离近的小的物体在相机中的成像尺寸是一样的，其实就是单目相机无法测量物体的深度信息。

我们都知道小孔成像实验中，得到的图像都是倒立的，不过不用担心这个问题，基本上所有的相机硬件系统都已经将倒立问题修正过来了。相机的数字图像在不同的计算机系统和软件中，图像数据的横纵坐标可能有不同的标准，比如OpenCV中的像素坐标系，如图4-50所示。

图4-50 OpenCV中的像素坐标系

注意，在Windows、Linux或Mac系统以及Matlab、OpenCV等不同环境中，图像坐标系的定义可能各有区别，实际使用时需要做适当转换。

2.上位机ROS驱动程序

学习了相机的基本原理之后，就来看看怎么在ROS中驱动相机设备，获取图像数据。按照视频传输协议的不同，相机设备的接口主要分为DVP、LVDS、MIPI、USB等接口类型。USB接口类型的相机不但价格低廉，可选购型号丰富，因此就以这种相机为例子来讲解。将USB相机直接用USB线连接到上位机，上位机运行Ubuntu和ROS系统，我们在上位机上运行对应ROS驱动程序，解析相机传回来的数据并将结果发布到ROS话题//image_raw之中，这样上位机上的其他ROS程序就可以通过订阅该话题来获取图像数据了，如图4-51所示。关于ROS中的话题//image_raw的标准数据格式，请参考ROS官方文档。

图4-51 上位机ROS驱动程序

关于USB相机的ROS驱动程序，下面讨论3种具体实现：usb_cam、gscam和自制驱动包。

（1）ROS驱动功能包usb_cam

在正式安装运行usb_cam包之前，可以检查一下插入Ubuntu上位机中的USB相机设备是否正常。可以用系统自带的工具cheese打开相机设备看看，或者检查一下文件系统是否存在设备/dev/video*，设备名中的*为实际的设备编号0、1、2...之类的。

检查连接正常后，就可以下载usb_cam功能包了，并将下载好的功能包放到自己的ROS工作空间，编译安装。关于ROS包的编译安装就不展开了，已经在第1章有过详细的讲解了。关于usb_cam包源码，可自行下载编译安装。

启动功能包之前，打开功能包中的launch文件，见代码清单4-3。对参数做必要的设置，主要是将video_device参数设置成实际的相机设备号，比如/dev/video0就行了，更多的设置参数可以参考功能包的文档教程。

代码清单4-3 usb_cam.launch启动文件

设置好launch文件后，直接启动该launch文件，就能将相机驱动起来，图像数据被发布到对应的ROS话题。可以用多种方式来查看被发布出来的图像ROS话题，比如可以用rviz和rqt工具查看图像，具体细节就不展开，请参考第1章的内容。

（2）ROS驱动功能包gscam

虽然usb_cam包很方便，但是只能发布相机的image_raw数据，而关于相机自身的CameraInfo信息发布不了。CameraInfo信息中主要是相机的内参校正参数、相机的模式配置、其他硬件信息等，这些信息对很多算法非常重要。gscam功能包，就可以同时发布image_raw信息和CameraInfo信息。关于安装编译和启动的操作，跟usb_cam是一样的，所以不再赘述。关于gscam包源码，可自行下载编译安装。关于ROS中的话题CameraInfo的标准数据格式，请参考ROS官方文档。

（3）自制基于OpenCV的驱动功能包

有时候需要对相机的图像做一些自定义的操作，比如相机的各种参数、读取图像的方式、发布图像的格式做修改，这个时候自己编写的驱动包就更方便了。当然大多数情况，上面的usb_cam与gscam基本都可以满足需要，因此自己编写驱动并不是必须掌握的内容。下面就简单的讲一下编程的思路，如图4-52所示，代码就不介绍了。

图4-52 基于OpenCV的图像ROS发布

首先用OpenCV获取相机设备的数据，可通过cv::VideoCapture类来实现，获取到的图像数据是cv::Mat格式。由于图像在OpenCV与ROS中是不同的格式，需要用cv_bridge对格式进行转换，转换后的sensor_msgs::Image就是ROS中的图像数据格式。这个时候其实可以用ros::Publisher直接发布单一图像消息出去了，不过通常使用image_transport进行转换后发布。image_transport可以将图像打包成多种形式后发布，一方面可以提高数据的传输效率，另一方面满足不同程序对数据格式的要求。关于发布和订阅图像数据的程序示例，请参考ROS官方文档。

3.单目相机标定

利用ROS驱动将我们的相机驱动起来后，就需要对前面说到过的相机内参和畸变进行标定了。并且利用标定得到的内参K和畸变系数 $k_{1}$ 、 $k_{2}$ 、 $k_{3}$ 、 $p_{1}$ 、 $p_{2}$ 对原始图像进行修正，将修正后的图像重新发布。最流行的相机内参标定方法是棋盘格标定法，就是通过多角度拍摄棋盘格图案，并利用其中的几何约束，求出相机模型中的待标参数。相机内参标定算法已经非常成熟了，这里就不展开讲解了，下面主要讲讲ROS中的标定操作实现。

我们采用ROS中的camera_calibration功能包进行标定，这个功能包被包含在image_pipeline功能包集中。由于image_pipeline功能包集中有很多实用的包，将在后面使用到，这里就列举出来给大家看看，见表4-10。

表4-10 功能包集image_pipeline中包含的功能包

功能包集名称	功能包名称
image_pipeline	camera_calibration
	depth_image_proc
	image_proc
	image_publisher
	image_rotate
	image_view
	stereo_image_proc

这里采用usb_cam包驱动我们的USB相机，首先启动usb_cam包，将相机的图像发布出来，命令如下：

roslaunch usb_cam usb_cam.launch

接着启动camera_calibration包，这个包ROS系统默认是自带的，如果没有，可以下载上面的image_pipeline源码安装一下。介绍一下camera_calibration包的launch启动文件，见代码清单4-4。

代码清单4-4 camera_calibration.launch启动文件

第2行，args里面是描述棋盘格的参数，size参数是描述所采用棋盘格的角点数，注意8x6中间的符号x是字母x而不是乘号，square参数是描述棋盘格中每个格子的边长，单位是米。

第3行，指定输入图像的话题，这个要跟实际的相机ROS驱动中发布的话题一致就行了，这里图像话题名/usb_cam/image_raw。

第4行，也是指定话题名称，这里是图像话题的第一级命名空间名，也就是/usb_cam。

设置好launch文件中的参数后，就可以启动camera_calibration功能包了，正常启动后，会弹出图4-53所示的窗口。

图4-53 单目相机标定

这个时候就可以移动我们的棋盘格标定板，让相机采集到棋盘格处在左边、右边、上边、下边、倾斜、旋转、远近等不同视角的图像，各个视角的数据都采集满后，窗口中X、Y、Size和Skew进度条都会变绿，同时CALIBRATE按钮也会从灰色变亮。这个时候点击CALIBRATE按钮就可以开始标定，此过程需要等待几分钟。标定完成后，标定结果会显示在命令行终端，如果觉得满意，点击COMMIT按钮将标定结果保存到默认路径~/.ros/camera_info/head_camera.yaml，就完成标定了。

4.单目图像传输

经过上面的标定，我们得到了相机的标定信息，并保存到了默认目录下。下次再启动相机的ROS驱动，除了发布图像话题image_raw，还会加载这个默认路径下的相机标定信息并发布到话题CameraInfo中。但是，相机驱动并不会利用标定信息对原图像做校正。

在图像的传输过程中，需要将原始图像转换成多种格式，比如RAW图、灰度图、彩色图、利用标定信息校正后的图等。使用image_pipeline功能包集中的image_proc包，就能实现这些格式的转换，非常简单，具体使用方法请参考image_proc包的wiki文档教程。

4.3.2 双目相机

在单目相机原理中提到过，单目相机无法测量物体点的深度信息。那么，用两个单目相机组成的双目相机，就可以测量深度信息，有些地方也把双目相机叫深度相机。

1.双目相机原理