《视觉SLAM十四讲》学习笔记：第5讲相机与图像

前言：本学习笔记将记录《视觉SLAM十四将》中一些重要的知识点，并对书中一些比较难的知识点添加上一些笔者个人的理解，以供笔者本人复习并与各位同学一起交流学习。本笔记结构将与原书结构一致，如果某一目录下面没有任何笔记，则代表笔者认为该小节内容相对来说没有过多重点知识。

本讲主要解决问题
本讲只要讨论机器人如何观测外部世界，也就是观测方程部分。
理论：
1.理解针孔相机的模型、内参与径向畸变参数。
2.理解一个空间点是如何投影到相机成像平面的。
实践：
3.掌握OpenCV的图像存储与表达方式。
4.学会基本的摄像头标定方法。

5.1 相机模型

5.1.1 针孔相机模型
针孔相机模型，也就是简单的小孔成像模型，如下图所示：

现在来对这个简单的针孔模型进行几何建模。设O−x−y−zO-x-y-zO−x−y−z为相机坐标系，习惯上我们让zzz轴指向相机前方，xxx向右，yyy向下。OOO为摄像机的广信，也是针孔模型中的针孔。现实世界的空间点PPP，经过小孔OOO投影之后，落在物理成像平面O′−x′−y′O'-x'-y'O′−x′−y′上，成像点为P′P'P′。设P的坐标为[X,Y,Z]T[X,Y,Z]^T[X,Y,Z]T，P′P'P′为[X′,Y′,Z′]T[X',Y',Z']^T[X′,Y′,Z′]T，并且设物理成像平面到小孔的距离为fff（焦距）。那么，根据三角形相似关系，有：

其中负号表示成的像是倒立的。为了简化模型，我们把可以成像平面对称到相机前面，和三维空间点一起放在摄像机坐标系的同一侧（注意，后面的对称平面和归一化平面其实都是虚拟的平面），这样可以把公式中的负号去掉：

真实的成像平面，对称的成像平面和归一化成像平面如下图所示：

整理得：

虽然式(5.3)描述了点PPP和它的像之间的空间关系。不过，在相机中，我们最终获得的是一个个的像素，这需要在成像平面上对像进行采样和量化。为了描述传感器将感受到的光线转换成图像像素的过程，我们设在物理成像平面上固定着一个像素平面o−u−vo-u-vo−u−v。我们在像素平面得到了P′P'P′的像素坐标：[u,v]T[u,v]^T[u,v]T。
像素坐标系通常的定义方式是：原点o′o'o′位于图像的左上角，uuu轴向右与xxx轴平行，vvv轴向下与yyy轴平行。像素坐标系与成像平面之间，相差了一个缩放和一个原点的平移。我们设像素坐标在uuu轴上缩放了α\alphaα倍，在vvv上缩放了β\betaβ倍。同时，原点平移了[cx,cy]T[c_x,c_y]^T[cx,cy]T。那么，P′P'P′的坐标与像素坐标[u,v]T[u,v]^T[u,v]T的关系为：

代入式（5.3）并把αf\alpha fαf合并成fxf_xfx，把βf\beta fβf合并成fyf_yfy,得：

关注单位：fff的单位为米，α,β\alpha ,\betaα,β的单位为像素每米，所以fx,fyf_x,f_yfx,fy的单位为像素。把该式写成矩阵形式，会更加简洁，不过左侧需要用到齐次坐标：

按照习惯，将ZZZ挪到左侧：

该式中，我们把中间的量组成的矩阵称为相机的内参数矩阵KKK。通常认为，相机的内参在出厂之后是固定的，不会在使用过程中发生变换。有的相机生产厂商会告诉你相机的内参，而有时需要自己确定相机的内参，也就是标定。
除了内参之外，相机还有相对的外参。在式（5.6）中，我们使用的是PPP在相机坐标系下的。由于相机在运动，所以PPP的相机坐标应该是它的世界坐标(记为PwP_wPw)，根据相机的当前位姿，变换到相机坐标系下的结果。（即PPP的坐标是相机坐标系下）。相机的位姿由它的旋转矩阵RRR和平移向量ttt来描述，那么有：

这里第三个等号左右两边利用了齐次坐标的转换。这个齐次坐标的转换描述里PPP的世界坐标到像素坐标的投影关系。其中，相机的位姿R,tR,tR,t又称为相机的外参数。相比于不变的内参，外参会随着相机运动发生改变，用是也是SLAMSLAMSLAM中待估计的目标，代表着机器人的轨迹。
上式两侧都是齐次坐标。因为齐次坐标乘上非零常数后表达同样的含义，所以可以简单地把ZZZ去掉：

但这样等号意义就变里，成为在齐次坐标下像等的概念，相差里一个非零常数。
这里作者重点提了一下齐次到非齐次的变换。可以看到，右侧的TPWTP_WTPW表示把一个世界坐标系下的齐次坐标，变换到相机坐标系下。为了使它与KKK相乘，需要取它的前三维组成向量——因此TPwTP_wTPw最后一维为1。此时，对于这个三维向量，我们还可以按照齐次坐标的方式，把最后一维进行归一化处理，得到了PPP在相机归一化平面上的投影：

这时 PcP_cPc 可以看成一个二维的齐次坐标，称为归一化坐标。它位于相机前方 z=1z = 1z=1 处的平面上。该平面称为归一化平面。由于 PcP_cPc 经过内参之后就得到了像素坐标，所以我们可以把像素坐标 [u,v]T[u, v]^T[u,v]T，看成对归一化平面上的点进行量化测量的结果。
5.1.2 畸变
畸变是由透镜产生的，由透镜形状引起的畸变称为径向畸变，由透镜安装不与成像面严格平行产生的畸变为切向畸变。其中，径向畸变又分为桶形失真和枕形失真，如下图所示：

桶形畸变是由于图像放大率随着离光轴的距离增加而减小，而枕形畸变却恰好相反。在这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。
利用数学方法来定义两种畸变就是，将平面上一点ppp用极坐标表示[r,θ]T[r,\theta]^T[r,θ]T，径向畸变可看成坐标点沿着长度方向发生里变化δr\delta rδr，也就是其距离原点的长度发生了变化。切向畸变可以看成坐标点沿着切线方向发生里变化，也就是水平夹角发生里变化δθ\delta \thetaδθ。
因此，对于径向畸变，可以用和距中心距离有关的二次及高次多项式函数进行纠正：

其中[x,y]T[x,y]^T[x,y]T是未纠正的点的坐标，[xcorrected,ycorrected]T[x_{corrected},y_{corrected}]^T[xcorrected,ycorrected]T是纠正后的点的坐标，注意它们都是归一化平面上的点，而不是像素平面上的点。在式（5.11）描述的纠正模型中，对于畸变较小的图像中心区域，畸变纠正主要是k1k_1k1起作用。而对于畸变较大的边缘区域主要是k2k_2k2起作用。普通摄像头用这两个系数就能很好的纠正径向畸变。对于畸变较大的摄像头，比如鱼眼镜头，可以加入k3k_3k3畸变项对畸变进行纠正，
而对于切向畸变，可以用另外两个参数p1,p2p1,p2p1,p2来进行纠正：

因此，联合式（5.11）和式（5.12），对于相机坐标系中的一点P(X,Y,Z)P(X,Y,Z)P(X,Y,Z)，我们能够通过五个畸变系数找到这个点在像素平面上的正确位置：
1.将三维空间点投影到归一化图像平面。设它的归一化坐标为[x,y]T[x,y]^T[x,y]T。
2.对归一化平面上的点进行径向畸变和切向畸变纠正。

3.将纠正后的点通过内参数矩阵投影到像素平面，得到该点在图像上的正确位置。

在上面的纠正畸变的过程中，我们使用里五个畸变项。实际应用中，可以灵活选择纠正模型，比如只选择k1,p1,p2k_1,p_1,p_2k1,p1,p2这三项等。
（注：获得三项系数的方式可以利用对黑白棋盘进行标定，然后利用matlab库函数求得参数，此处不过多赘述）。
在SLAMSLAMSLAM中，常见的去畸变的做法就是先对整张图像进行去畸变，得到去畸变后的图像，然后讨论此图像上的点的空间位置。因此，当一个图像去畸变后，就可以直接用针孔模型建立投影关系，而不用考虑畸变了。
最终，总结一下单目相机的成像过程：
1.首先，世界坐标系下有一个固定的点PPP,世界坐标为PwP_wPw;
2.由于相机在运动，它的运动由R,tR,tR,t或变换矩阵T∈SE(3)T \in SE(3)T∈SE(3)描述。PPP的相机坐标为：Pc=RPw+tP_c = RP_w +tPc=RPw+t。
3.此时的PcP_cPc仍然有X,Y,ZX,Y,ZX,Y,Z三个量，把它们投影到归一化平面Z=1Z=1Z=1上，得到PPP的归一化相机坐标：Pc=[X/Z,Y/Z,1]TP_c = [X/Z,Y/Z,1]^TPc=[X/Z,Y/Z,1]T。
4.对坐标进行去畸变化处理。
5.最后，PPP的归一化坐标经过内参后，对应到它的像素坐标：Puv=KPcP_{uv}=KP_cPuv=KPc。
注意到ZZZ可能小于1，说明该点位于归一化平面后面，它可能不会在相机平面上成像，实践当中要检查一次。
5.1.3 双目相机模型
针孔相机模型描述里单目相机的成像模型，然而，该模型忽略里深度信息，也就无法确定这个空间点的具体位置的。而只有当PPP的深度确定时，我们才能确切地指导它的空间位置。
测量目标深度的方法有很多，双目相机主要依靠基线去进行测量：

根据上图，同时，根据三角形相似关系，有：

稍作整理，得：

这里d为左右图的横坐标之差，称为视差。根据视差，可以估计一个像素离相机的距离。视差与距离成反比：视差越大，距离越近。同时，由于视差最小为一个像素，于是双目的深度存在一个理论上的最大值，由fbfbfb确定。我们看到，当基线越长时，双目最大能测到的距离就会变远；反之，小型双目器件则只能测量很近的距离。
虽然由视差计算深度的公式很简洁，但视差ddd本身的计算却比较困难。当我们想计算每个像素的深度时，其计算量与精度都将成为问题，而且只有在图像纹理变化丰富的地方才能计算视差。由于计算量的原因，双目深度估计仍需要使用GPU或FPGAGPU或FPGAGPU或FPGA来计算。
5.1.4 RGB-D相机模型
RGB-D相机可以主动测量每个像素的深度，原理主要有两大类：
1.通过红外结构光来测量像素距离的。
2.通过飞行时间法原理测量像素距离的。
无论是结构光还是 ToF， RGB-D 相机都需要向探测目标发射一束光线（通常是红外光）。在结构光原理中，相机根据返回的结构光图案，计算物体离自身的距离。而在 ToF中，相机向目标发射脉冲光，然后根据发送到返回之间的光束飞行时间，确定物体离自身的距离。 ToF 原理和激光传感器十分相似，不过激光是通过逐点扫描来获取距离，而 ToF相机则可以获得整个图像的像素深度，这也正是 RGB-D 相机的特点。所以，如果你把一个 RGB-D 相机拆开，通常会发现除了普通的摄像头之外，至少会有一个发射器和一个接收器。
在测量深度之后，RGB-D相机通常按照生产时的各个相机摆放位置，自己完成深度与彩色图像素之前的配对，输出一一对应的彩色图和深度图。我们可以在同一个图像位置，读取到色彩信息和距离信息，计算像素的3D相机坐标，生成点云。对RGB-D数据，既可以在图像层面进行处理，亦可在点云层面处理。
RGB-D相机能够实时地测量每个像素点的距离。但是，由于这种发射-接受的测量方式，使得它使用方面比较受限。用红外进行深度值测量的 RGB-D 相机，容易受到日光或其他传感器发射的红外光干扰，因此不能在室外使用，同时使用多个时也会相互干扰。对于透射材质的物体，因为接受不到反射光，所以无法测量这些点的位置。此外， RGB-D 相机在成本、功耗方面，都有一些劣势。

5.2 图像

本节的知识较为基础，但是此处不过多描述，但请注意分辨率的宽高与二维数组中的行列的对应关系。

5.3 实践：图像的存取与访问

这里有两点注意：
1.安装ros全家桶的时候就会帮你安装好OpenCV，不需要单独编译，个人安装的ros-melodic版本，cmakelist编辑如下，仅供参考：

2.注意区别赋值和clone的区别。

5.4 实践：拼接点云

拼接点云这边遇到的坑较多，踩坑集锦见博客：https://blog.csdn.net/themarshal/article/details/107467114