高翔《视觉SLAM十四讲》从理论到实践

目录

第1讲 前言：本书讲什么；如何使用本书；

第2讲 初始SLAM：引子-小萝卜的例子；经典视觉SLAM框架；SLAM问题的数学表述；实践-编程基础；

第3讲 三维空间刚体运动 旋转矩阵；实践-Eigen；旋转向量和欧拉角；四元数；相似、仿射、射影变换；实践-Eigen几何模块；可视化演示；

第4讲 李群与李代数 李群李代数基础；指数与对数映射；李代数求导与扰动模型；实践-Sophus；相似变换群与李代数；小结；

第5讲 相机与图像 相机模型；图像；实践-图像的存取与访问；实践-拼接点云；

第6讲 非线性优化 状态估计问题；非线性最小二乘；实践-Ceres；实践-g2o；小结；

第7讲 视觉里程计1 特征点法；特征提取和匹配；2D-2D：对极几何；实践-对极约束求解相机运动；三角测量；实践-三角测量；3D-2D：Pnp；实践-求解PnP；3D-3D：ICP；实践-求解ICP；小结；

第8讲 视觉里程计2 直接法的引出；光流（Optical Flow）；实践-LK光流；直接法（Direct Methods）；实践-RGBD的直接法；

第9讲 实践章：设计前端 搭建VO前端；基本的VO-特征提取和匹配；改进-优化PnP的结果；改进-局部地图；小结

第10讲 后端1 概述；BA与图优化；实践-g2o；实践-Ceres；小结

第11讲 后端2 位姿图（Pose Graph）；实践-位姿图优化；因子图优化初步；实践-gtsam；

第12讲 回环检测 回环检测概述；词袋模型；字典；相似度计算；实验分析与评述；

第13讲 建图 概述；单目稠密重建；实践-单目稠密重建；实验分析与讨论；RGBD稠密建图；TSDF地图和Fusion系列；小结；

第14讲 SLAM：现在与未来 当前的开源方案；未来的SLAM话题；

附录A 高斯分布的性质

附录B ROS入门

第1讲 前言

1.1 本书讲什么

讲的是SLAM（Simultaneous Localization And Mapping，同步定位与成图），即利用传感器来进行机器人的自身定位以及对周围环境的成图。根据传感器来划分主要分为雷达SLAM和视觉SLAM。这里当然主要讲的是视觉SLAM。

从定义上可以看出SLAM主要解决的是“自身定位”和“周围环境的成图”。

这里主要把SLAM系统分成几个模块：视觉里程计、后端优化、建图以及回环检测。

这是一个很复杂的过程，共分十四讲来讲述，每一讲都有一个主题，然后介绍该主题的算法和实践，即从理论到实践。

 ok，这讲就到这里了。

第2讲 初始SLAM

这讲主要是讲SLAM的基本框架，以及开发前期准备工作。

如上图所述，SLAM基本框架就是这样。一个机器人，靠一个摄像头来感知周围的世界并评估自身的位置，它需要利用Visual Odometry视觉里程计来通过相邻两张相片来计算姿态参数估算距离角度这些用来计算自身位置xyz以及恢复图像上各点的位置pxpypz，由于有上一步有累积误差需要Optimization后端优化，对于周围环境的感知需要Mapping地图构建并配准，最后需要Loop Closure回环检测计算闭合误差并修正。

前期准备工作有：

准备一台电脑和一个Turtlebot机器人（没有也不要紧，只要有一台Kinect相机就可以了，你可以用手拿着它模拟机器人的走动，它会还原你的位置信息）。这台电脑和Turtlebot机器人都是安装的Linux操作系统，没错，机器人身上有一台电脑作为控制，电脑就是一个高级点的单片机，这是它的大脑控制着它发射信息行走以及计算汇总，而另一台电脑作为我们的服务器它接收机器人发送的信息如图片等以及给机器人发送指令。希望安装的是Ubuntu这款Linux操作系统，因为我们以后将在该操作系统下下载安装软件。ROS软件就不用说了，这是机器人操作系统，是必备的，可以利用它与机器人完成交互传递信息发送指令等。另外我们还需要配置开发环境，因为我们需要开发我们自己的程序，调节参数。这是ROS所不具备的。

配置开发环境: Kdevelop。

编写实例程序: HelloSLAM。

第3讲 三维空间刚体运动

这讲主要是讲三维几何空间中刚体的运动方程。

a'=Ra+t

其中R为旋转矩阵，t为平移矩阵

将R和t融合到一个矩阵中，将上式变为线性方程

那么编程如何实现上式呢？用Eigen开源库。

Eigen使用方法：添加头文件，编写程序。

旋转向量与旋转矩阵之间的变换：，其中n为旋转向量，R为旋转矩阵

旋转向量只需要三个变量，比旋转矩阵三维矩阵的9个变量要简洁，节省运算成本。

由旋转向量到四元数：由于旋转矩阵的冗余，加上旋转向量和角度的奇异性，引入四元数，即用复数来表示旋转从而避免了奇异性。

用四元数来表示旋转：设空间点p=[x,y,z]，以及一个由旋转轴和角度指定的旋转，那么旋转后的点p'用四元数怎么表示呢？我们知道使用矩阵描述的话p'=Rp。而用四元数表达：

1. 把三维空间点用一个虚四元数描述：

p=[0, x, y, z]=[0, v]

这相当于我们把四元数的三个虚部与空间中的三个轴相对应。用四元数q表示这个旋转：

q=[cos(θ/2), ncos(θ/2)]

那么旋转后的点p'即可表示为这样的乘积：

p'=qpq^(-1)

2. 四元数到旋转矩阵的转换

那么用Eigen演示如何进行各种旋转变换。在Eigen中使用四元数、欧拉角和旋转矩阵，演示它们三者之间的变换关系。

Eigen中各个数据类型总结如下：

• 旋转矩阵（3×3）：Eigen::Matrix3d。
• 旋转向量（3×1）：Eigen::AngleAxisd。
• 欧拉角（3×1）：Eigen::Vector3d。
• 四元数（4×1）：Eigen::Quaterniond。
• 欧氏变换矩阵（4×4）：Eigen::Isometry3d。
• 仿射变换（4×4）：Eigen::Affine3d。
• 射影变换（4×4）：Eigen::Perspective3d。

本讲结束。

第4讲 李群与李代数

三维旋转矩阵构成了特殊正交群SO(3)，而变换矩阵构成了特殊欧氏群SE(3)

 

 但无论SO(3)，还是SE(3)，它们都不符合加法封闭性，即加之后不再符合旋转矩阵的定义，但是乘法却满足，将这样的矩阵称为群。即只有一种运算的集合叫做群。

 群记作G=(A, .)，其中A为集合，.表示运算。群要求运算满足以下几个条件：

（1）封闭性。

（2）结合律。

（3）幺元。一种集合里特殊的数集。

（4）逆。

可以证明，旋转矩阵集合和矩阵乘法构成群，而变换矩阵和矩阵乘法也构成群。

介绍了群的概念之后，那么，什么叫李群呢？

李群就是连续（光滑）的群。一个刚体的运动是连续的，所以它是李群。

每个李群都有对应的李代数。那么什么叫李代数呢？

李代数就是李群对应的代数关系式。

李群和李代数之间的代数关系如下：

可见两者之间是指数与对数关系。

 那么exp(φ^)是如何计算的呢？它是一个矩阵的指数，在李群和李代数中，它称为指数映射。任意矩阵的指数映射可以写成一个泰勒展开式，但是只有在收敛的情况下才会有结果，它的结果仍然是一个矩阵。

 同样对任意一元素φ，我们亦可按此方式定义它的指数映射：

 由于φ是三维向量，我们可以定义它的模长θ和方向向量a满足使φ=θa。那么，对于a^，可以推导出以下两个公式：

 设a=(cosα, cosβ, cosγ),可知(cosα)^2+(cosβ)^2+(cosγ)^2=1

 （1）a^a^=aa^T-I

 （2）a^a^a^=-a^

 上面两个公式说明了a^的二次方和a^的三次方的对应变换，从而可得：

exp(φ^)=exp(θa^)=∑(1/n!(θa^)ⁿ)=...=a^a^+I+sinθa^-cosθa^a^=(1-cosθ)a^a^+I+sinθa^=cosθI+(1-cosθ)aa^T+sinθa^.

回忆前一讲内容，它和罗德里格斯公式如出一辙。这表明，so(3)实际上就是由旋转向量组成的空间，而指数映射即罗德里格斯公式。通过它们我们把so(3)中任意一个向量对应到了一个位于SO(3)中的旋转矩阵。反之，如果定义对数映射，我们也能把SO(3)中的元素对应到so(3)中：

但通常我们会通过迹的性质分别求解转角和转轴，那种方式会更加省事一些。

 OK，讲了李群和李代数的对应转换关系之后，有什么用呢？

主要是通过李代数来对李群进行优化。比如说，对李群中的两个数进行运算，对应的他们的李代数会有什么变化？

首先是，两个李群中的数进行乘积时，对应的李代数是怎么样的变化，是不是指数变化呢？但是注意，李群里的数是矩阵，不是常数，所以不满足ln(exp(A+B))=A+B，因为A,B是矩阵，不是常数，那么是怎么的对应关系呢？

是

第5讲 相机与图像

之前介绍了机器人的运动方程，那么现在来讲相机的原理。相机最早是根据小孔成像原理产生的最早相机。后面又有了加凸透镜的相机。

相机模型：

这个公式表示了世界坐标Pw到像素坐标Puv的转换。表示了从世界坐标(Pw)到相机坐标(通过外参R,t)再到像素坐标(通过K内方位元素)的转换。

对于凸透镜的镜头畸变则采用：

进行径向畸变纠正和切向畸变纠正。其中k1,k2,k3为径向畸变参数，而p1,p2为切向畸变参数。

而对于相机标定，即求解内方位参数K，可以采用OpenCV, Matlab, ROS三种方法求解。参照：链接。

接下来是使用OpenCV处理图像的示例。OpenCV是处理图像的类库，是非常重要的。

那么接下来呢，当然是将上一步求的相机内参数应用到实际的图像点云数据处理上。

有了相机内参数和相片，就可以恢复像点的像点X,Y,Z坐标吗？不能，由可知，要求像点X,Y,Z除了内参数矩阵外，还需要像点的(x,y,w)，而从像片上只能得到x,y，那么w是距离，怎么获得呢？通过RGD-D相机可以获得深度数据，即w。（通过Kinect获取彩色和深度图像，保存 显示）

假设你已经通过Kinect获得彩色和深度图像了，那么就可以通过上式恢复像素的相机坐标点了(即通过彩色图像(x,y)和深度图像(w)就可以得到点云了)。然后就可以(通过IMU得到的位姿参数)当前点的R,t这些旋转矩阵和平移矩阵(外参)恢复到它的世界坐标Pw(Xw, Yw, Zw)。

以上两个文件夹分别为彩色图像和深度图像，一一对应。

接下来来写一个通过两张彩色图像和对应的两张深度图像得到点云数据，并且将两个点云数据融合生成地图的程序：

程序略。

拼接完成的数据保存到map.pcd文件中。可以通过PCL的点云显示程序来显示保存的map.pcd。10万多个点加载了进来，但是颜色在windows程序下怎么没有显示出来。在Linux下试一下看看。

pcd_viewer.exe本身有问题，pcl_viewer.exe在Windows下找不到。pcl_visualization可以显示彩色图像，但是只是一个链接库文件。

原来是使用pcd_viewer打开pcd文件后，按5键就可以渲染RGB色彩了，而按4键则可以渲染深度图像。众多使用说明请见：PCL Visualization overview。

第6讲 非线性优化

通过传感器的运动参数来估计运动方程(位姿x)，通过相机的照片来估计物体的位置(地图y)，都是有噪声的。因为运动参数和照片都有噪声，所以需要进行优化。而过去卡尔曼滤波只关心当前的状态估计，而非线性优化则对所有时刻采集的数据进行状态估计，被认为优于卡尔曼滤波。由于要估计所有的采集数据，所以待估计变量就变成：

x={x1,…,xN,y1,….,yM}

所以对机器人状态的估计，就是求已知输入数据u(传感器参数)和观测数据z(图像像素)的条件下，计算状态x的条件概率分布（也就是根据u和z的数据事件好坏来估计x的优劣事件概率情况，这其中包含着关联，就好像已知一箱子里面有u和z个劣质的商品，求取出x个全是好商品的概率，同样的样本点，但是从不同角度分析可以得出不同的事件，不同的事件概率之间可以通过某些已知数据得出另些事件的概率，通过一系列函数式计算得出）：

P(x|z, u)

在不考虑运动方程，只考虑观测照片z的情况下求x（这个过程也称SfM运动恢复），那么就变成P(x|z)。这种根据已知求未知的情况可以通过贝叶斯公式求解，P(x|z)=P(x)P(z|x)/P(z)。

又因为只求x所以忽略P(z)，所以P(x|z)=P(x)P(z|x)

那么根据贝叶斯公式的含义，P(x)为先验概率，P(z|x)为似然概率。

而P(x)是不知道，所以主要求P(z|x)也就是最大似然概率。

最大似然的概率用自然语言描述就是在什么状态下最有可能产生当前的照片结果！

如何求呢？

我们知道z与x之间存在一个函数式：zk,j = h(yj, xk)+vk,j,即观测方程

现在要求x导致z出现的概率，要求z的最大概率，由于v是误差，符合高斯分布vk~N(0, Qk,j)，所以z也符合高斯分布，即P(zi,k|xk, yj)=N(h(yj, xk), Qk,j)。

所以求多维的概率最大值即为

所以对于所有的运动和观测，有：

从而得到了一个总体意义下的最小二乘问题。它的最优解等于状态的最大似然估计。

它的意义是说P(z,u|x)表明我们把估计的轨迹和地图(xk,yj)代入SLAM的运动、观测方程中时，它们并不会完美的成立。这时候怎么办呢？我们把状态的估计值进行微调，使得整体的误差下降一些。它一般会到极小值。这就是一个典型的非线性优化过程。

因为它非线性，所以df/dx有时候不好求，那么可以采用迭代法（有极值的话，那么它收敛，一步步逼近）：

1.给定某个初始值x0。

2.对于第k次迭代，寻找一个增量△xk，使得||f(xk+△xk)||2²达到极小值。

3. 若△xk足够小，则停止。

4. 否则，令xk+1=xk+ △xk，返回2。

这样求导问题就变成了递归逼近问题，那么增量△xk如何确定？

这里介绍三种方法：

（1）一阶和二阶梯度法

将目标函数在x附近进行泰勒展开：

其中J为||f(x)||2关于x的导数（雅克比矩阵），而H则是二阶导数（海塞(Hessian)矩阵）。我们可以选择一阶或二阶，增量不同。但都是为了求最小值，使函数值最小。

但最速下降法下降太快容易走出锯齿路线，反而增加了迭代次数。而牛顿法则需要计算目标函数的H矩阵，这在问题规模较大时非常困难，我们通常为了避免求解H。所以，接下来的两种最常用的方法：高斯牛顿法和列文伯格-马夸尔特方法。

（2）高斯牛顿法

将f(x)一阶展开：

这里J(x)为f(x)关于x的导数，实际上是一个m×n的矩阵，也是一个雅克比矩阵。现在要求△x，使得||f(x+△x)|| 达到最小。为求△ x，我们需要解一个线性的最小二乘问题：

这里注意变量是△ x，而非之前的x了。所以是线性函数，好求。为此，先展开目标函数的平方项：

求导，令其等于零从而得到：

我们称为高斯牛顿方程。我们把左边的系数定义为H，右边定义为g，那么上式变为：

H △x=g

跟第（1）种方法对比可以发现，我们用J(x)^TJ(x)代替H的求法，从而节省了计算量。所以高斯牛顿法跟牛顿法相比的优点就在于此，步骤列于下：

1.给定初始值x0。

2.对于第k次迭代，求出当前的雅克比矩阵J(xk)和误差f(xk)。

3.求解增量方程：H△xk=g。

4.若△xk足够小，则停止。否则，令xk+1=xk+△xk，返回2。

但是，还是有缺陷，就是它要求我们所用的近似H矩阵是可逆的（而且是正定的），但实际数据中计算得到的J^TJ却只有半正定性。也就是说，在使用Gauss Newton方法时，可能出现J^TJ为奇异矩阵或者病态（ill-condition）的情况，此时增量的稳定性较差，导致算法不收敛。更严重的是，就算我们假设H非奇异也非病态，如果我们求出来的步长△x太大，也会导致我们采用的局部近似（6.19）不够准确，这样一来我们甚至都无法保证它的迭代收敛，哪怕是让目标函数变得更大都是可能的。

所以，接下来的Levenberg-Marquadt方法加入了α，在△x确定了之后又找到了α，从而||f(x+α△x)||²达到最小，而不是直接令α=1。

（3）列文伯格-马夸尔特方法（Levenberg-Marquadt法）

由于Gauss-Newton方法中采用的近似二阶泰勒展开只能在展开点附近有较好的近似效果，所以我们很自然地想到应该给△x添加一个信赖区域（Trust Region），不能让它太大而使得近似不准确。非线性优化中有一系列这类方法，这类方法也被称之为信赖区域方法（Trust Region Method）。在信赖区域里边，我们认为近似是有效的；出了这个区域，近似可能会出问题。

那么如何确定这个信赖区域的范围呢？一个比较好的方法是根据我们的近似模型跟实际函数之间的差异来确定这个范围：如果差异小，我们就让范围尽可能大；如果差异大，我们就缩小这个近似范围。因此，考虑使用：

来判断泰勒近似是否够好。ρ的分子是实际函数下降的值，分母是近似模型下降的值。如果ρ接近于1，则近似是好的。如果ρ太小，说明实际减小的值远少于近似减小的值，则认为近似比较差，需要缩小近似范围。反之，如果ρ比较大，则说明实际下降的比预计的更大，我们可以放大近似范围。

于是，我们构建一个改良版的非线性优化框架，该框架会比Gauss Newton有更好的效果。

1. 给定初始值x0，以及初始化半径μ。

2. 对于第k次迭代，求解：

 

 这里面的限制条件μ是信赖区域的半径，D将在后文说明。

3. 计算ρ。

4. 若ρ>3/4，则μ=2μ；

5. 若ρ<1/4，则μ=0.5μ；

6. 如果ρ大于某阈值，认为近似可行。令xk+1=xk+△xk。

7. 判断算法是否收敛。如不收敛则返回2，否则结束。

最后化简得到：

当λ比较小时，接近于牛顿高斯法，当λ比较大时，接近于最速下降法。L-M的求解方式，可在一定程度上避免线性方程组的系数矩阵的非奇异和病态问题，提供更稳定更准确的增量△x。

下面是实践：

1. Ceres：最小二乘法类库。

编译glog、gflags和ceres-solver，然后在项目工程中设置：

include头文件：

E:ceresceres-solver-1.3.0ceres-solver-masterconfig;
E:cereseigenEigen 3.3.3Eigen;
E:Softwareceres-solver for windowsglog-0.3.3glog-0.3.3srcwindows;
E:Softwareceres-solver for windowsgflags-mastergflags-master-buildinclude;
E:ceresceres-solver-1.3.0ceres-solver-masterinclude;

库文件：

E:Softwareceres-solver for windowsgflags-mastergflags-master-buildlibDebug;
E:Softwareceres-solver for windowsglog-0.3.3glog-0.3.3Debug;
E:ceresceres-bin3libDebug;

Executable目录：

E:Softwareceres-solver for windowsgflags-mastergflags-master-buildDebug;
E:Softwareceres-solver for windowsglog-0.3.3glog-0.3.3Debug;
E:ceresceres-bin3in;

附加依赖：

curve_fitting.lib
ellipse_approximation.lib
sampled_function.lib
robust_curve_fitting.lib
helloworld.lib
helloworld_analytic_diff.lib
helloworld_numeric_diff.lib
rosenbrock.lib
simple_bundle_adjuster.lib
ceres-debug.lib
libglog.lib
libglog_static.lib
gflags_nothreads_static.lib
gflags_static.lib
opencv_calib3d310d.lib
opencv_core310d.lib
opencv_features2d310d.lib
opencv_flann310d.lib
opencv_highgui310d.lib
opencv_imgcodecs310d.lib
opencv_imgproc310d.lib
opencv_ml310d.lib
opencv_objdetect310d.lib
opencv_photo310d.lib
opencv_shape310d.lib
opencv_stitching310d.lib
opencv_superres310d.lib
opencv_ts310d.lib
opencv_video310d.lib
opencv_videoio310d.lib
opencv_videostab310d.lib

 

注意Release/Debug要匹配，VS编译器要匹配，以及在工程中添加glog和gflags的目录。

2. g2o：图优化。

 在视觉slam里更常用的非线性优化是图优化，它是将非线性优化和图论结合在一起的理论。它不是仅仅将几种类型的误差加在一起用最小二乘法求解，而是增加了位姿最小二乘和观测最小二乘之间的关系（比如位姿方程xk=f(fk-1,uk)+wk和观测方程zk,j=h(yj,xk)+vk,j都有一个变量xk联系了位姿方程和观测方程）。所以就用到了图优化。

蓝线表示了位姿最小二乘优化结果（运动误差），而红线表示观测最小二乘结果（观测误差），两者之间用位置变量xk关联，或者说约束。顶点表示优化变量（xk处的u导致的位姿参数误差和pk,j处的图像噪声导致的观测值误差），

所以在g2o图优化中，就可以将问题抽象为要优化的点和误差边，然后求解。

第7讲 视觉里程计1

根据相邻图像的信息估算相机的运动称为视觉里程计（VO）。一般需要先提取两幅图像的特征点，然后进行匹配，根据匹配的特征点估计相机运动。从而给后端提供较为合理的初始值。

1. 特征点：特征检测算子SIFT，SURF，ORB等等。

SIFT算子比较“奢侈”，考虑的比较多，对于SLAM算法来说有点太“奢侈”。不太常用目前。

ORB算子是在FAST算子基础上发展起来，在特征点数量上精简了，而且加上了方向和旋转特性(通过求质心)，并改进了尺度不变性（通过构建图像金字塔）。从而实现通过FAST提取特征点并计算主方向，通过BRIEF计算描述子的ORB算法。

2. 特征匹配：特征匹配精度将影响到后续的位姿估计、优化等。

实践：特征提取和匹配

略。

根据提取的匹配点对，估计相机的运动。由于相机的不同，情况不同：

1.当相机为单目时，我们只知道2D的像素坐标，因而问题是根据两组2D点估计运动。该问题用对极几何来解决。

2.当相机为双目、RGB-D时，或者我们通过某种方法得到了距离信息，那问题就是根据两组3D点估计运动。该问题通常用ICP来解决。

3.如果我们有3D点和它们在相机的投影位置，也能估计相机的运动。该问题通过PnP求解。

分别来介绍。

1. 对极几何

假设我们从两张图像中，得到了若干对这样的匹配点，就可以通过这些二维图像点的对应关系，恢复出在两帧之间摄像机的运动。

根据小孔成像原理（s₁p₁=KP, s₂p₂=K(RP+t)）可以得到x₂^Tt^Rx₁=0

其中x2,x1为两个像素点的归一化平面上的坐标。

代入x₂=K^-1p₂,x₁=K^-1p₁,得到：

p₂^TK^-Tt^RK^-1p₁=0

上面两式都称为对极约束。

它代表了O1,O2,P三点共面。它同时包含了旋转R和平移t。把中间部分分别记为基础矩阵F和本质矩阵E，可以进一步化简对极约束公式：

E=t^R, F=K^-TEK^-1, x₂^TEx₁=p₂^TFp₁=0

它给出了两个匹配点的空间位置关系，于是，相机位姿估计问题可以变为两步：

1. 根据配对点的像素位置，求出E或者F；

2. 根据E或者F，求出R, t。

由于K一般已知，所以一般求E。而E=t^R可知共有6个自由度，但是由于尺度不变性，所以共有5个自由度。可以利用八点法采用线性代数求解。

那么得到本质矩阵E之后，如何求出R,t呢？这个过程是由奇异值分解（SVD）得到的。设E的SVD分解为：

E=UΣV^T

其中U,V为正交阵，Σ为奇异值矩阵。根据E的内在性质，我们知道Σ=diag(δ, δ, 0)。对于任意一个E，存在两个可能的t,R与它对应：

其中Rz(π/2)表示绕Z轴旋转90度得到的旋转矩阵。同时对E来说，E与-E对零等式无影响。所以可以得到4种组合。

但根据点的深度值可以确定正确的那一组组合。

除了本质矩阵，我们还要求单应矩阵，单应矩阵是指当特征点位于同一平面时可以构建单应矩阵。它的作用是在相机不满足八个参数时，比如只旋转没有移动，那么就可以通过单应矩阵来估计。求法略。

实践：对极约束求解相机运动

略。

求得了相机运动参数之后，那么需要利用相机的运动参数估计特征点的空间位置。由s₁x₁=s₂Rx₂+t可得s₁和s₂的值（通过s₁x₁^x₁=0=s₂x₁^Rx₂+x₁^t）。由R,t以及深度信息可以求得特征像素点的空间点坐标。

实践：三角测量

略。

下面介绍3D-2D方法，即利用RGB-D获取的深度数据和彩色图像进行的计算。

可以直接采用直接线性变换，即不需要求解内外方位元素的变换。直接线性变换即DLT。

而P3P是另一种解PnP的方法。P3P即只利用三对匹配点，它仅使用三对匹配点，对数据要求较少。推导过程：

通过三角形相似原理，求得OA,OB,OC的长度，从而求得R,t。

它存在2个缺点：（1）P3P只利用三个点的信息。当给定的配对点多于3组时，难以利用更多的信息。（2）如果3D点或2D点受噪声影响，或者存在误匹配，则算法失效。

那么，后续人们还提出了许多别的方法，如EPnP、UPnP等。它们利用更多的信息，而且用迭代的方式对相机位姿进行优化，以尽可能地消除噪声的影响。

在SLAM中通常做法是先使用P3P/EPnP等方法估计相机位姿(R,t)，然后构建最小二乘优化问题对估计值(R,t)进行调整（Bundle Adjustment）。下面，介绍一下Bundle Adjustment。

Bundle Adjustment是一种非线性的方式，它是将相机位姿和空间点位置都看成优化变量，放在一起优化。可以用它对PnP或ICP给出的结果进行优化。在PnP中，这个Bundle Adjustment问题，是一个最小化重投影误差的问题。本节给出此问题在两个视图下的基本形式，然后在第十讲讨论较大规模的BA问题。

考虑n个三维空间点P和它们的投影p，我们希望计算相机的位姿R,t，它的李代数表示为ζ。假设某空间点坐标为P_i=[X_i, Y_i, Z_i]^T。其投影的像素坐标为u_i=[u_i,v_i]。根据第五章的内容，像素位置与空间点位置的关系如下：

 除了用ζ为李代数表示的相机姿态之外，别的都和前面的定义保持一致。写成矩阵形式就是：

s_iu_i=Kexp(ζ^{^})Pi

但由于噪声以及相机位姿的原因，该等式并不总成立。所以，需要用最小二乘法

这也叫重投影误差。由于需要考虑很多个点，所以最后每个点的误差通常都不会为零。最小二乘优化问题已经在第六讲介绍过了。使用李代数，可以构建无约束的优化问题，很方便地通过G-N，L-M等优化算法进行求解。不过，在使用G-N和L-M之前，我们需要知道每个误差项关于优化变量的导数，也就是线性化：

e(x+Δx)≈e(x)+JΔx

其中Δx即像素变量坐标误差。e(x+Δx)即偏移后的值。

当e为像素坐标误差(2维)，x为相机位姿(6维,旋转+平移)，J将是一个2×6的矩阵。推导一下J的形式。

回忆李代数的内容，我们介绍了如何使用扰动模型来求李代数的导数。首先，记变换到相机坐标系下的空间点坐标为P’，并且把它前三维取出来：

P'=(exp(ζ^{^})P)_1:3=[X', Y', Z']^T

那么，相机投影模型相对于P'则为：

                 su=KP'

展开之：

 

利用第3行消去s，实际上就是P'的距离，得：

这与之前讲的相机模型是一致的。当我们求误差时，可以把这里的u,v与实际的测量值比较，求差。在定义了中间变量后，我们对ζ^左乘扰动量δζ，然后考虑e的变化关于扰动量的导数。利用链式法则，可以列写如下(这里ζ指的是旋转量和平移量6个参数)：

        --->     

除了扰动量，还有希望优化特征点的空间位置。

      --->     

于是，推导出了观测相机方程关于相机位姿与特征点的两个导数矩阵。它们特别重要，能够在优化过程中提供重要的梯度方向，指导优化的迭代。

实践：（1）先使用EPnP程序求解位姿，然后（2）对位姿和点坐标进行BA非线性优化。

那么对于两组3D点怎么求解参数呢，下面介绍3D-3D的方法：ICP。ICP是Iterative Closet Point的简称，即迭代最近点算法。它也有两种求解方法：SVD和非线性两种方法。

ICP:SVD法：已有两个RGB-D图像，通过特征匹配获取两组3D点，最后用ICP计算它们的位姿变换。先定义第i对点的误差项：e_i=p_i-(Rp_i'+t)。然后构建最小二乘问题，求使误差平方和达到极小的R,t。先求R,再求t。为求R，得到-tr(R∑q_i'q_i^T)，要使R最小从而定义矩阵W=U∑V^T，其中∑为奇异值组成的对角矩阵，对角线元素从大到小排列，而U和V为正交矩阵。当W满秩时，R为：

R=UV^T.

得到R后，即可求解t。

ICP:非线性方法。跟第四讲的内容一样。直接使用。

实践：求解ICP，略。

第8讲 视觉里程计2

本节先介绍光流法跟踪特征点的原理，然后介绍另一种估计相机位姿的方法——直接法，现在直接法已经一定程度上可以和特征点法估计相机位姿平分秋色。

光流即图像上的某个灰度值在不同时刻的图像上的流动。用图像表示就是，不同图像的灰度像素之间的关联

 按照追踪的像素的多少可以分为稀疏光流和稠密光流，这里主要讲稀疏光流中的Lucas-Kanade法，称为LK光流。

它假设一个像素在不同的图像中是固定不变的，我们知道这并不总是成立的，但我们只是先这样假设，不然什么都没法做。

所以对于t时刻图像(x,y)处的像素I(x, y, t)，经过dt时刻后在t+dt它来到了(x+dx, y+dy)处，这是一个运动过程，具体怎么运动方向(实际运动是三维反映在图像上是二维)怎么样不去管它，那么由假设可知I(x+dx, y+dy, t+dt)=I(x, y, t)。我们知道一个像素沿着x,y轴各移动一定的距离，距离上的速度分别为u,v。那么u,v各是多少呢？怎么求呢？求它们有什么用呢？看下图，假设前一秒和后一秒相机运动如下

左上方(1,1)灰色方格超右下方瞬间移动到了(4.2)处灰色方格，如何用方程来描述这个过程呢？

可以通过相邻像素的颜色梯度和自身像素的颜色变化来描述。也就是通过相邻像素梯度和运行速度得到自身该位置的颜色值变化，列出一个方程，以此来描述图像的平移。

当这个运动足够小时可以对左边的式子进行泰勒展开，因为I(x,y,t)部分是不变的所以得到：

--->

 两边除以dt，得：

 记梯度分别为I_x,I_y，速度分别为u,v，而右侧的灰度对时间的变化量I_t。

 可以得到：

根据最小二乘法可以得到： 通过特征块求解u,v。

得到了像素在图像间的运行速度u,v，就可以估计特征点在不同图像中的位置了。当t取离散的时刻而不是连续时间时，我们可以估计某块像素在若干个图像中出现的位置。由于像素梯度仅在局部有效，所以如果一次迭代不够好的话，我们会多迭代几次这个方程。

下面通过实践来体会一下LK光流在跟踪角点上的应用。

例：使用LK光流法对第一张图像提取FAST角点，然后用LK光流跟踪它们，画在图中。

slambook/ch8/useLK/useLK.cpp

 

可以看到特征点在逐渐减少，需要不断添加新的特征点进去。光流法的优点是可以避免描述子的计算，这可以避免误匹配，但是如果运动太快时就容易追踪错误还是描述子好些。

然后就可以根据光流法跟踪的特征点通过PnP、ICP或对极几何来估计相机运动，这之前已讲过，不再赘述。

而直接法是直接连特征点都不提取了，直接用类似光流法的梯度迭代完成。计算得到位姿估计。它的思想是光流法+迭代近似。从而在光流法的基础上继续减少特征提取的时间。

要求的是位姿。根据光流法假设的像素差最小原则，得：

   e=I₁(p₁)-I₂(p₂)

利用最小二乘法

当有多个点时，，ei为第i像素的像素差

我们要求的是ζ的优化值，假设ei有偏差，那么J(ζ)有如何的变动呢？我们需要求两者之间的关系

我们假设ζ出现了一点偏差，考虑ei的变化，道理是一样的，求两者的关系，得到：

最后经过一系列变换得到一个e与ζ的关系式：

其中，而[X, Y, Z]为三维点坐标。

对于RGBD相机来说像素对应的XYZ比较好确定(我们不知道第二个像素是什么我们不获取第二个像素对应的XYZ，假设第二个像素是由第一个像素和第一个像素对应的XYZ经过姿态影射得到的)，但对于没有深度的纯单目相机来说要麻烦，因为深度不好确定，那只能假设其有一个深度，经过姿态后投影到第二个像素上，详细深度估计放到第13讲。

下面进行直接法的实践。[这里Eigen要使用Eigen3以上的包，因为要使用Eigen::Map函数。]

（1）稀疏直接法。基于特征点的深度恢复上一讲介绍过了，基于块匹配的深度恢复将在后面章节中介绍，所以本节我们来考虑RGB-D上的稀疏直接法VO。求解直接法最后等价于求解一个优化问题，因此可以使用g2o或Ceres这些优化库来求解。稀疏怎么个稀疏吗？求特征点。稀疏直接法跟光流法的区别在于光流法是通过平面图像的运动来估计第二个图像上的关键点像素，而稀疏直接法是通过位姿估计来实现。

（2）半稠密直接法。把特征点改为凡是像素梯度较大的点就是半稠密直接法。

第9讲 实践章：设计前端

前面两节我们学习了视觉里程计（VO）的原理，现在设计一个VO。VO也就是前端，与后端优化相对应。这节我们利用前面所学的知识实际设计一个前端框架。你将会发现VO前端会遇到很多突发状况，毕竟我们前面所学的只是完美假设情况下的理论，但实际情况是总会有意外的情况如相机运动过快、图像模糊、误匹配等。可以看到这节主要是前面的理论的实践，你将实际动手制作一个视觉里程计程序。你会管理局部的机器人轨迹与路标点(参考论文)，并体验一下一个软件框架是如何组成的。

我们知道光知道原理跟建造起伟大的建筑作品之间是有很大的区别的，可以说这里面是还有长长的路要走的。就像你知道三维建模的原理，知道各个部件的建造原理知道怎么搭建，这跟真正建造起美轮美奂的建筑物之间是有区别的，这需要发挥你的创造力和毅力来建造起复杂的建筑模型，光知道各个部分原理是不够的，你既需要统筹全局又需要优化部分的能力，这是需要广与小都具备才行。这跟SLAM相似，SLAM是个很伟大的建筑物，而各个柱子和屋顶之间如何搭建需要考虑之间的关联，而各个柱子、各个瓦片又需要再细细优化打磨，所以说，从局部到整体都需要考虑到，整体需要局部以及局部之间的关系。

这是一个从简到繁的过程，我们会从简单的数据结构开始，慢慢从简单到复杂建立起整个SLAM。在project文件夹下，从0.1到0.4你可以感受到版本的变化，从简单到复杂的整个过程。

现在我们要写一个SLAM库文件，目标是帮读者将本书用到的各种算法融会贯通，书写自己的SLAM程序。

程序框架：新建文件夹，把源代码、头文件、文档、测试数据、配置文件、日志等等分类存放

基本的数据结构：

数据是基础，数据和算法构成程序。那么，有哪些基本的数据结构呢？

1. 图像帧。一帧是相机采集到的图像单位。它主要包含一个图像。此外还有特征点、位姿、内参等信息。而且图像一般要选择关键帧，不会都存储那样成本太高。

2. 路标。路标点即图像中的特征点。

3. 配置文件。将各种配置参数单独存储到一个文件中，方便读取。

4. 坐标变换类。你需要经常进行坐标系间的坐标变换。

所以我们建立五个类：Frame为帧，Camera为相机模型，MapPoint为特征点/路标点，Map管理特征点，Config提供配置参数。

其中的包含关系。一帧图像有一个Camera模型和多个特征点。而一个地图有多个特征点组成。

 1. Camera类

#ifndef CAMERA_H
#define CAMERA_H
#include "common_include.h"

namespace myslam
{
    //Pinhole RGB-D camera model
    class Camera
    {
    public:
        typedef std::shared_ptr<Camera> Ptr; //公共camera指针成员变量
        float fx_, fy_, cx_, cy_, depth_scale_; //Camera intrinsics

        Camera();
        Camera(float fx, float fy, float cx, float cy, float depth_scale=0):
        fx_(fx), fy_(fy), cx_(cx), cy_(cy), depth_scale_(depth_scale)
        {}

        //coordinate transform:world, camera, pixel
        Vector3d world2camera(const Vector3d& p_w, const SE3& T_c_w);
        Vector3d camera2world(const Vector3d& p_c, const SE3& T_c_w);
        Vector2d camera2pixel(const Vector3d& p_c);
        Vector3d pixel2camera(const Vector3d& p_p, double depth=1);
        Vector3d pixel2world(const Vector2d& p_p, const SE3& T_c_w, double depth=1);
        Vector2d world2pixel(const Vector3d& p_w, const SE3& T_c_w);
    };
}


#endif //CAMERA_H

其中#ifndef是为了避免两次重复引用导致出错，在头文件中必须要加上(这点很重要切记，不然A->B,A->C,那么B,C->D时，D类中就引用了两次A)，而这样还不够，有时候我们总喜欢用同样的类名，为了尽量避免这种情况发生，我们也必须加上自己的namespace命名空间，一般是以自己的个性化来命名区分。而且为了避免发生内存泄露，我们应该为我们的类加上智能指针shared_ptr，以后在传递参数时只需要使用Camera::Ptr类型即可。

2. Frame类

#ifndef FRAME_H
#define FRAME_H

#include "common_include.h"
#include "camera.h"

namespace myslam 
{
    
// forward declare 
class MapPoint;
class Frame
{
public:
    typedef std::shared_ptr<Frame> Ptr;
    unsigned long                  id_;         // id of this frame
    double                         time_stamp_; // when it is recorded
    SE3                            T_c_w_;      // transform from world to camera
    Camera::Ptr                    camera_;     // Pinhole RGBD Camera model 
    Mat                            color_, depth_; // color and depth image 
    
public: // data members 
    Frame();
    Frame( long id, double time_stamp=0, SE3 T_c_w=SE3(), Camera::Ptr camera=nullptr, Mat color=Mat(), Mat depth=Mat() );
    ~Frame();
    
    // factory function
    static Frame::Ptr createFrame(); 
    
    // find the depth in depth map
    double findDepth( const cv::KeyPoint& kp );
    
    // Get Camera Center
    Vector3d getCamCenter() const;
    
    // check if a point is in this frame 
    bool isInFrame( const Vector3d& pt_world );
};

}

#endif // FRAME_H

3. MapPoint类

#ifndef MAPPOINT_H
#define MAPPOINT_H

namespace myslam
{
    
class Frame;
class MapPoint
{
public:
    typedef shared_ptr<MapPoint> Ptr;
    unsigned long      id_; // ID
    Vector3d    pos_;       // Position in world
    Vector3d    norm_;      // Normal of viewing direction 
    Mat         descriptor_; // Descriptor for matching 
    int         observed_times_;    // being observed by feature matching algo.
    int         correct_times_;     // being an inliner in pose estimation
    
    MapPoint();
    MapPoint( long id, Vector3d position, Vector3d norm );
    
    // factory function
    static MapPoint::Ptr createMapPoint();
};
}

#endif

4. Map类

#ifndef MAP_H
#define MAP_H

#include "common_include.h"
#include "frame.h"
#include "mappoint.h"

namespace myslam
{
class Map
{
public:
    typedef shared_ptr<Map> Ptr;
    unordered_map<unsigned long, MapPoint::Ptr >  map_points_;        // all landmarks
    unordered_map<unsigned long, Frame::Ptr >     keyframes_;         // all key-frames

    Map() {}
    
    void insertKeyFrame( Frame::Ptr frame );
    void insertMapPoint( MapPoint::Ptr map_point );
};
}

#endif // MAP_H

5. Config类

#ifndef CONFIG_H
#define CONFIG_H

#include "common_include.h" 

namespace myslam 
{
    class Config
    {
    private:
        static std::shared_ptr<Config> config_;
        cv::FileStorage file_;

        Config(){} //private constructor makes a singleton
    public:
        ~Config(); //close the file when deconstructing

        //set a new config file
        static void setParameterFile(const std::string& filename);

        //access the parameter values
        template<typename T>
        static T get(const std::string& key)
        {
            return T(Config::config_->file_[key]);
        }
    };
}

#endif

数据结构设计完之后就可以设计程序了，先进行最基本的VO：特征提取和匹配

第一种是两两帧的视觉里程计，不进行优化，不计算特征点位置，只进行两两帧的运动估计。

slambook/project/0.2/include/myslam/visual_odometry.h

#ifndef VISUALODOMETRY_H
#define VISUALODOMETRY_H

#include "common_include.h"
#include "map.h"

#include <opencv2/features2d/features2d.hpp>

namespace myslam 
{
    class VisualOdometry
    {
    public:
        typedef shared_ptr<VisualOdometry> Ptr;
        enum VOState{INITIALIZING=-1, OK=0, LOST};

        VOState state_; //current VO status
        Map::Ptr map_; //map with all frames and map points
        Frame::Ptr ref_; //reference frame
        Frame::Ptr curr_; //current frame

        cv::Ptr<cv::ORB> orb_; //orb detector and computer
        vector<cv::Point3f> pts_3d_ref_; //3d points in reference frame
        vector<cv::KeyPoint> keypoints_curr_; //keypoints in current frame
        Mat descriptors_curr_; //descriptor in current frame
        Mat descriptors_ref_; //descriptor in reference frame
        vector<cv::DMatch> feature_matches_;

        SE3 T_c_r_estimated_; //the estimated pose of current frame
        int num_inliers_; //number of inlier features in icp
        int num_lost_; //number of lost times

        //parameters
        int num_of_features_; //number of features
        double scale_factor_; //scale in image pyramid
        int level_pyramid_; //number of pyramid levels
        float match_ratio_; //ratio for selecting good matches
        int max_num_lost_; //max number of continuos lost times
        int min_inliers_; //minimum inliers

        double key_frame_min_rot; //minimal rotation of tow key-frames
        double key_frame_min_trans; //minimal translation of two key-frames
    public: //functions
        VisualOdometry();
        ~VisualOdometry();

        bool addFrame(Frame::Ptr frame); //add a new frame

    protected:
        //inner operation
        void extractKeyPoints();
        void computeDescriptors();
        void featureMatching();
        void poseEstimationPnP();
        void setRef3DPoints();

        void addKeyFrame();
        bool checkEstimatedPose();
        bool checkKeyFrame();
    };
}

#endif

关于这个VisualOdometry类，有几点需要解释：

1. addFrame函数是外部调用的接口。使用VO时，将图像数据装入Frame类后，调用addFrame估计其位姿。该函数根据VO所处的状态实现不同的操作：

bool VisualOdometry::addFrame(Frame::Ptr frame)
{
switch(state_)
{
case INITIALIZING: //初始化，该帧为第一幅
{
state_=OK; //修改状态为OK
curr_=ref_=frame; //将该帧赋给当前帧和参考帧
map_->insertKeyFrame(frame); //将该帧插入地图
//从第一帧中提取特征点
extractKeyPoints(); //提取特征点
computeDescriptors(); //计算描述子
//计算参考帧中的特征点的三维坐标点
setRef3DPoints();
break;
}
case OK:
{
curr_=frame;
extractKeyPoints();
computeDescriptors();
featureMatching();
poseEstimationPnP();
if(checkEstimatedPose()==true) //a good estimation
{
curr_->T_c_w_=T_c_r_estimated_*ref_->T_c_w_; //T_c_w=T_c_r*T_r_w
ref_=curr_;
setRef3DPoints();
num_lost_=0;
if(checkKeyFrame()==true) //is a key-frame
{
addKeyFrame();
}
}
else //bad estimation due to various reasons
{
num_lost_++;
if(num_lost_>max_num_lost_)
{
state_=LOST;
}
return false;
}
break;
}
case LOST:
{
cout<<"vo has lost."<<endl;
break;
}
}
return true;
}

 值得一提的是，由于各种原因，我们设计的上述VO算法，每一步都有可能失败。例如图片中不易提特征、特征点缺少深度值、误匹配、运动估计出错等等。因此，要设计一个鲁棒的VO，必须（最好是显式地）考虑到上述所有可能出错的地方——那自然会使程序变得复杂。我们在checkEstimatedPose中，根据内点（inlier）的数量以及运动的大小做一个简单的检测：认为内点不可太少，而运动不可能过大。当然，读者也可以思考其他检测问题的手段，尝试一下效果。

VisualOdometry的其余部分略，下面进行一下测试Test。

slambook/project/0.2/test/run_vo.cpp

程序略，结果如下图所示。

注意：这里使用的OpenCV3的viz模块显示估计位姿，请确保你安装的是OpenCV3，并且viz模块也编译安装了。准备tum数据集中的其中一个。简单起见，我推荐fr1_xyz那一个。请使用associate.py生成一个配对文件associate.txt。关于tum数据集格式我们已经在8.3节中介绍过了。在config/defualt.yaml中填写你的数据集所在路径，参照我的写法即可。然后用bin/run_vo config/default.yaml执行程序，就可以看到实时的演示了。

改进：优化PnP的结果。沿着之前的内容，尝试一些VO的改进。尝试RANSAC PnP加上迭代优化的方式估计相机位姿。非线性优化之前已经介绍过了，此处略。

改进：局部地图。将VO匹配到的特征点放到地图中，并将当前帧跟地图点进行匹配，计算位姿。地图又分为全局地图和局部地图，全局地图太奢侈了，主要用于回环检测和地图表达。局部地图是指随着相机运动，往地图里添加新的特征点，并将视野之外的特征点删掉，并且统计每个地图点被观测到的次数等等。

第10讲 后端1

本节我们开始转入SLAM系统的另一个重要模块：后端优化。前面的视觉里程计可以给出一个短时间内的轨迹和地图，但由于不可避免的误差累积，如果时间长了这个地图是不准确的。所以我们希望构建一个尺度、规模更大的优化问题，以考虑长时间内的最优轨迹和地图。实际当中考虑到精度与性能的平衡，有许多不同的做法。

1. 什么叫后端？

 之前提到，视觉里程计只有短暂的记忆，而我们希望整个运动轨迹在较长时间内都能保持最优的状态。我们可能会用最新的知识，更新较久远的状态——站在“久远的状态”的角度上看，仿佛是未来的信息告诉它“你应该在哪里”。所以，在后端优化中，我们通常考虑一段更长时间内（或所有时间内）的状态估计问题，而且不仅使用过去的信息更新自己的状态，也会用未来的信息来更新自己，这种处理方式不妨称为“批量的”（Batch）。否则，如果当前的状态只由过去的时刻决定，甚至只由前一个时刻决定，那不妨称为“渐进的”（Incremental）。

所以这是一个假设检验的过程。先验和后验。先由以前的位姿和观测方程预测下一步，然后利用下一步的内容预测这一步的最大可能。是概率统计中的知识。

2. 状态估计的概率解释

我们已经知道SLAM过程可以由运动方程和观测方程来描述。那么，假设在t=0到t=N的时间内，有位姿x₀到x_N，并且有路标y₁,…,y_M。按照之前的写法，运动和观测方程为：

 注意以下几点：

（1）在观测方程中，只有当x_k看到了y_j时，才会产生观测数据。

（2）我们可能没有测量运动的装置，所以也可能没有运动方程。在这个情况下，有若干种处理方式：认为确实没有运动方程，或假设相机不动，或假设相机匀速运动。这几种方式都是可行的。在没有运动方程的情况下，整个优化问题就只由许多个观测方程组成。这就非常类似于StM(Structure from Motion)问题，由一组图像恢复运动和结构。与StM中不同的是，SLAM中的图像有时间上的先后顺序，而StM中允许使用完全无关的图像。

我们知道每个方程都受噪声影响，所以要把这里的位姿x和路标y看成服从某种概率分布的随机变量，而不是单独的一个数。所以当存在一些运动数据u和观测数据z时，如何去估计状态量的高斯分布？

当只有位姿数据时，误差累积会越来越大，不确定性增高，而如果加上路标则会将不确定性减小。如下图所示。

下面以定量的方式来看待它。

我们希望用过去0到k中的数据来估计现在的状态分布：

P(x_k|x₀,u_1:k,z_1:k)

其中xk为第k时刻的未知量。上式表示根据初始位置、1到k的运动参数、1到k的图像来估计第k时刻的未知量。

由x_k和z_k的相互依赖关系，可以得到如下近似关系：

P(x_k|x₀,u_1:k,z_1:k) ∝P(z_k|x_k)P(x_k|x₀,u_1:k,z_1:k-1)

上式表示由z_k推x_k，跟由x_k推z_k结果是一样的概率上来说。

上式第一项为似然，第二项为先验。似然由观测方程给定，而先验部分我们明白当前状态x_k是基于过去所有的状态估计得来的。至少它会受x_k-1影响，于是按照x_k-1时刻为条件概率展开：

P(x_k|x₀,u_1:k,z_1:k-1)= ∫P(x_k|x_k-1,x₀,u_1:k, z_1:k-1)P(x_k-1|x₀,u_1:k,z_1:k-1)dx_k-1.

如果考虑更久之前的状态，也可以继续对此式进行展开，但现在我们只关心k时刻和k-1时刻的情况。至此，我们给出了贝叶斯估计，虽然上式还没有具体的概率分布形式，所以还没法实际地操作它。对这一步的后续处理，方法上产生了一些分歧。大体上讲，存在若干种选择：其一是假设马尔可夫性，简单的一阶马氏性认为，k时刻状态只与k-1时刻状态有关，而与再之前的无关。如果做出这样的假设，我们就会得到以扩展卡尔曼滤波（EKF）为代表的滤波器方法。时刻注意这种方法与里程计的区别，在于通过概率统计的方法来消除误差累积导致的轨迹偏移。在滤波方法中，我们会从某时刻的状态估计，推导到下一个时刻。另外一种方法是依然考虑k时刻状态与之前所有状态的关系，此时将得到非线性优化为主体的优化框架。这也能消除误差累积的影响。非线性优化的基本知识已在前文介绍过。目前视觉SLAM的主流为非线性优化方法。不过，为了让本书更全面，我们要先介绍一下卡尔曼滤波器和EKF的原理。

卡尔曼滤波器的推导方法有若干种，比如从概率角度出发的最大后验概率估计的形式（贝叶斯），从最小均方差为目的推导的递推等式（链接）。总之，卡尔曼滤波就是推导出先验和后验之间的关系式，也就是增益K。通过预测（先验）和测量反馈（后验）来提高准确度。

但是EKF存在一些缺点，所以现在在SLAM中一般采用BA与图优化，这个之前提到过，这里详细阐述它的原理。

第11讲 后端2

虽然BA有各种优点，但是速度太慢，尤其是随着特征点越来越多它的效率会越来越慢。所以引出了位姿图。即将特征点视为输入值，只考虑位姿的优化。

第12讲 回环检测

回环检测的优点，可以充分利用每一个环来调整地图。

如上图所示第一个图表示真实的轨迹，第二个图红色表示位姿的偏移，而如果有回环检测的话则可以将后面偏离的轨迹拉回到重合处。这就是回环检测的好处。

回环检测如果真的检测到真实的回环则有很大的优化作用，但错误的检测则会造成很大的损失，还不如不检测，所以这里对准确率要求较高，而对召回率要求小一些（召回率即有些真实的回环没有检测到的概率），也就是说对于模棱两可不确定的回环，宁可不回环也不要冒险认定为回环。

那么回环检测用什么模型呢？这里用词袋模型，是特征点法的聚类模型。

聚类问题在无监督机器学习（Unsupervised ML）中特别常见，用于让机器自行寻找数据中的规律，无人工干预。词袋的字典生成亦属于其中之一。先对大量的图像提取了特征点，比如说有N个。现在，我们想找一个有k个单词的字典，每个单词是由特征点组成的，那么特征点该如何组合成有效的单词呢？我们人一般会自动聚合为“书包”、“杯子”、“桌子”这样的单词。每个单词可以看作局部相邻特征点的集合，应该怎么做呢？这可以用经典的K-means（K均值）算法解决。

第13讲 建图

之前只是提取了特征点，但是稠密的地图需要更多的点。特征点主要用于估计机器人位姿，而稠密的点用于避障和交互等。避障跟导航类似，但是更注重局部的、动态的障碍物的处理。同样，仅有特征点，我们无法判断某个特征点是否为障碍物，所以需要稠密地图。