初识SLAM

主要目标：

理解一个SLAM框架由哪几个模块组成，各模块的任务是什么。
搭建编程环境，为开发和实验做准备。
理解如何在Linux下编译并运行一个程序，如果有Bug，该如何调试。
掌握cmake的基本使用方法。

2.1引子

轮式移动机器人+相机。

问题：定位和建图。

传感器可以分为两类：

携带于机器人本体的，如轮式编码器、相机、激光传感器。
安装于环境中的，导轨、二维码标志等。

轮式编码器可以测得轮子转动的角度；IMU测量运动的角速度和加速度；相机和激光传感器则读取外部环境的某种观测数据。这些传感器让几人通过间接手段推断自己的位置而没有对环境提出任何要求(对比于导轨，二维码，Gps—有些环境没法用Gps)。

视觉slam主要用相机解决定位和建图问题。

相机有很多种，按照工作方式分为单目相机(Monocular)、双目相机(Stereo)和深度相机(RGB-D)三大类。

我们先详细讨论下他们的结构和工作原理吧。

相机模型

这里提前可能先跳到第五讲了，主要是想先来个对三大类相机的理解。相机工作原理会解释“机器人如何观测外部世界”。

小目标：

理解针孔相机的模型(针孔是棱镜模型的精简版)、内参与径向畸变参数。
理解一个点是如何投影到相机成像平面的。

单目相机(Monocular):

我们在现实生活中能看到大量的照片。在计算机中，一张照片由很多个像素组成，每个像素记录了色彩或亮度信息。三维世界中的一个物体反射或者发出光线，穿过相机光心后，投影在相机的成像平面上。相机的的感光器件接受到光线后，产生测量值，就得到了像素，形成我们见到的照片。这个过程如何用数学语言来描述呢？如何用数学语言描述投影关系。

相机将三维世界中的座标点(单位为米)映射到二维图像平面(单位为像素)的过程能够用一个几何模型进行描述。最简单的是针孔模型，它很常用很有效，它描述了一束光线通过针孔后，在针孔背面投影成像的关系。

我们将由简单的针孔成型模型来对这种映射关系建模。同时由于相机镜头上的透镜存在，使得光线投影到成像平面的过程中会产生畸变。因此，要分为针孔和畸变两个模型来讨论整个投影过程。

两个模型能够把外部的三维点投影到相机内部成像平面，构成相机的内参数(Intrinsics)。

针孔相机模型

看下这个图，设O-x-y-z为相机座标系，习惯上我们让z轴指向相机的前方，x向右，y向下。O为相机光心，也是针孔模型的针孔。现实世界的空间点P，经过小孔O投影之后，落在物理成像平面O'-x'-y'上，成像点为P'。设P的座标为[X,Y, Z]T,P‘为[X',Y,' Z']T,并设物理成像平面到小孔的距离为f(焦距)。那么，根据三角形相似，看下上图右边的图。

Z/f = -X/X' = - Y/Y'

其中负号表示成的像是倒立的。不过，实际相机得到的并不是倒像(否则相机的使用会非常不方便)。为了让模型更符合实际，我们可以等价地把成像平面对称地放到相机前方，和三维空间点一起放在相机座标系地同一侧，如下图所示：

这样做可以把公式中地负号去掉，使式子更加简洁：

Z/f = X/X' = Y/Y'

我们把X'，Y'放到等式左侧，整理得:

X' = f*X/Z;
Y' = f*Y/Z;

可能会有疑问，为什么我们可以随意地把成像平面挪到前方呢？这只是我们处理真实世界与相机投影地数学手段，并且，绝大多数图像输出地图像并不是倒像——相机自身的软件会帮我们翻转这张图片，所以我们得到的是正像，也就是对称地成像平面地像。

所以尽管从物理原理来说，小孔成像应该是倒像，但是我们对图像做了预处理。

上面地我们推导出了点P和它的像之间的空间关系，这里的所有单位都可以理解成米。不过，在相机中，我们最终获得的是一个个像素，这还需要在成像平面上对像进行采样和量化。

为了描述传感器将感受到地光线转换成图像像素地过程，我们设再物理成像平面上固定着一个像素平面o-u-v。我们在像素平面得到了P'的像素座标：[u,v]T。

像素座标系通常的定义方式是:原点o'位于图像的左上角，u轴向右与x轴平行，v轴向下与y轴平行。像素座标系与成像平面之间，相差了一个缩放和一个原点的平移。我们设像素座标在u轴上缩放了α倍，在v轴方向缩放了β倍。同时，原点平移了[cx,cy]T。那么，P‘的座标与像素座标[u,v]T的关系为：

注意一下这里的单位问题。α和β的单位？fx和fy的单位？cx和xy的单位？

u = αX'+cx;
v = βY'+cy;

带入上面推导出来的X'和Y’

u = α*f*X/Z + cx；
v = β*f*Y/Z + cy；

我们把α*f合并成fx，β*f合并成fy：

u = fx*X/Z +cx；
v = fy*Y/Z +cy；

其中f单位为米，α，β的单位为像素/米，所以fx，fy和cx，cy的单位为像素。我们把上面的式子写成矩阵形式会更加简洁，不过左侧需要用到其次座标，右侧则是非齐次座标：

上面这个式子要熟记于心。上面的式子中我们把中间的量组成的矩阵成为相机的内参数矩阵K。

相机内参的标定求K。这个有专门的工具箱。

刚才只是一个简单的投影过程。只涉及到了内参，因为P是在相机座标系下的三维点。

由于相机在运动，所以P的相机座标应该是它的世界座标系(记为Pw)根据相机当前的位姿变换到相机座标系下的结果。相机的位姿由它的旋转矩阵R和平移向量t来描述。那么有：

齐次座标到非齐次座标的转换？

上面那个式子描述了P的世界座标到像素座标的投影关系。其中相机的位姿R和t又称为相机的外参数。相比于不变的内参，外参会随着相机运动发生变化，同时也是SLAM中待估计的目标，代表着机器人的轨迹。

投影过程还可以从另一个角度来看。我们可以把一个世界座标点先转换到相机座标系，再除掉它的最后一维的数值(即该点距离相机成像平面的深度)，相当于把最后一维进行了归一化处理，得到点P在相机归一化平面上的投影：

归一化可以看成相机前方z=1处的平面上的一个点，这个z = 1平面也成为归一化平面。归一化座标再左乘内参就得到了像素座标，所以我们可以把像素座标[u,v]T看成对归一化平面上的点进行量化测量的结果。

这个模型中也可以看出，如果对相机座标同时乘以任意非零常数，归一化座标都是一样的，说明点的深度再投影过程中被丢失了，所以单目视觉中没法得到像素点的深度。

理解下上面那句话的含义。

畸变

一般相机为了取得好的成像效果，都会在相机的前方加透镜。但是透镜的加入会对成像过程中光线的传播产生影响：一是透镜自身的形状对光线传播的影响，二世在机械组装过程中，透镜和成像平面不能完全平行，这也会使得光线穿过透镜投影到成像平面时的位置发生变化。

由透镜形状引起的畸变，称为径向畸变。在理想的针孔模型中，一条直线投影到像素平面上还是一条直线。可是在实际拍摄中往往使得真实环境中的一条直线变成了曲线。越靠近图像的边缘，这种现象越明显。由于实际加工的透镜往往是对称的，这使得不规则的畸变通常径向对称。主要分为：桶形畸变和枕形畸变。

桶形畸变是由于图像放大率随着与光轴之间的距离增加而减小，而枕形畸变恰好相反。这两种畸变中，穿过图像中心和光轴有交点的直线还能保持形状不变。

除了透镜的形状会引起径向畸变，在相机的组装过程中由于不能使透镜和成像平面严格平行也会引入切向畸变。

为了更好地理解径向畸变和切向畸变，我们用更严格地数学形式对两者进行描述。考虑归一化平面上地任一点p，它的座标为[x,y]T，也可以写成极座标的形式[r,角度]T，其中r表示p与座标系原点之间地距离，角度表示与水平轴地夹角。径向畸变可以看成座标点沿着长度方向发生了变化，也就是其距离原点的长度发生了变化。切向畸变可以看成座标点沿着切线方向发生了变化，也就是水平夹角发生了变化。

1. 将三维点投影到归一化平面。设它的归一化座标为[x,y]T。

2.对归一化平面上地点计算径向畸变和切向畸变。

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

上面的纠正畸变过程，我们使用了五个畸变项。

总结下小孔模型和畸变模型：

双目相机(Stereo):

这个结合自己写的相关博客1，相关博客2。

针孔相机模型描述了单个相机的成像模型。前面也说了，单目丢失了深度信息(怎么用语言来表达这回事，归一化平面？任意不为零的数？)，所以。。。(官方原因：从相机光心到归一化平面连线上的所有点，都可以投影至该像素。)指由当P的深度确定时(通过双目或者RGB-D)，我们才能确切的知道它的位置。

承上启下的操作？

双目相机原理：通过同步采集左右相机的图像，计算图像间视差，来估计每一个像素的深度。下面简述原理。

双目相机一般由左眼相机和右眼相机两个水平放置的相机组成(上下也可以)。在左右相机中，我们可以把两个相机都视为针孔相机。它们是水平放置的，意味着两个相机的光圈都在x轴上。两者之间的距离称为双目基线(Baseline,计做b)，这是双目相机的重要参数。

其中d定义为左右图的横座标之差，称为视差(Disparity),根据视差，我们可以估计一个像素与相机之间的距离。视差与距离之间成反比：视差越大，距离越近。同时，由于视差最小为一个像素，于是双目的深度存在一个理论上的最大值，由f*b确定。

视差d的计算比较难。计算量与精度的问题。。。

这里讲的Stereo还是比较简单的。

RGB-D相机模型

也贴一些自己写的相关博客链接。

博客1，博客2。

相比双目相机通过视差计算深度的方式，RGB-D相机的做法更为”主动“，它能主动测量每个像素的深度。目前的RGB-D相机按原理可以分为两大类：

红外结构光(Structured Light)来测量像素距离。Kinect1、Project Tango 1、Intel RealSense等。
通过飞行时间法(Time-of-flight,ToF)原理来测量距离。Kinect2 和一些现有的ToF。

无论是哪种类型，RGB-D相机都需要向探测目标发射一束光线(通常是红外光).在结构光原理中，相机根据返回的结构光图案，计算物体与自身之间的距离。而在ToF原理中，相机向目标发射脉冲光，然后根据返回之间的光束飞行时间，确定物体与自身之间的距离。ToF原理和激光传感器十分相似，只不过激光是通过逐点扫描来获取距离，而ToF相机则可以获得整个图像的像素深度，这也是RGB-D相机的特点。

通常在测量完之后，RGB-D相机通常按照生产时的各相机摆放位置，自己完成深度与彩色图像像素之间的配对，输出一一对应的彩色图和深度图。

RGB-D的缺点？这种发射-接收的测量方式范围比较受限。用红外光进行深度值测量的RGB-D相机，容易受到日光或者其他传感器发射的红外线干扰，因此不能在室外使用。同时，在没有调制的情况下，同时使用多个RGB-D相机时也会互相干扰。对于透射材质的物体，因为接收不到反射光，所以无法测量这些点的位置。另外就是成本和功耗方面不占优势。

https://blog.csdn.net/myarrow/article/details/52678020

https://blog.csdn.net/jackkang01/article/details/84134966

相关补充，对双目和RGB-D的原理解释

ToF的原理：