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

目录php

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

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

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

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

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

第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^=aaT-I

(2)a^a^a^=-a^

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

exp(φ^)=exp(θa^)=∑(1/n!(θa^)n)=...=a^a^+I+sinθa^-cosθa^a^=(1-cosθ)a^a^+I+sinθa^=cosθI+(1-cosθ)aaT+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)||22达到极小值。

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矩阵是可逆的(并且是正定的)，但实际数据中计算获得的JTJ却只有半正定性。也就是说，在使用Gauss Newton方法时，可能出现JTJ为奇异矩阵或者病态(ill-condition)的状况，此时增量的稳定性较差，致使算法不收敛。更严重的是，就算咱们假设H非奇异也非病态，若是咱们求出来的步长△x太大，也会致使咱们采用的局部近似(6.19)不够准确，这样一来咱们甚至都没法保证它的迭代收敛，哪怕是让目标函数变得更大都是可能的。

因此，接下来的Levenberg-Marquadt方法加入了α，在△x肯定了以后又找到了α，从而||f(x+α△x)||2达到最小，而不是直接令α=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:\ceres\ceres-solver-1.3.0\ceres-solver-master\config;

E:\ceres\eigen\Eigen3.3.3\Eigen;

E:\Software\ceres-solver for windows\glog-0.3.3\glog-0.3.3\src\windows;

E:\Software\ceres-solver for windows\gflags-master\gflags-master-build\include;

E:\ceres\ceres-solver-1.3.0\ceres-solver-master\include;

库文件：

E:\Software\ceres-solver for windows\gflags-master\gflags-master-build\lib\Debug;

E:\Software\ceres-solver for windows\glog-0.3.3\glog-0.3.3\Debug;

E:\ceres\ceres-bin3\lib\Debug;

Executable目录：

E:\Software\ceres-solver for windows\gflags-master\gflags-master-build\Debug;

E:\Software\ceres-solver for windows\glog-0.3.3\glog-0.3.3\Debug;

E:\ceres\ceres-bin3\bin;

附加依赖：

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. 对极几何

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

根据小孔成像原理(s1p1=KP, s2p2=K(RP+t))能够获得x2Tt^Rx1=0

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

代入x2=K-1p2,x1=K-1p1,获得：

p2TK-Tt^RK-1p1=0

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

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

E=t^R, F=K-TEK-1, x2TEx1=p2TFp1=0

它给出了两个匹配点的空间位置关系，因而，相机位姿估计问题能够变为两步：

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

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

因为K通常已知，因此通常求E。而E=t^R可知共有6个自由度，可是因为尺度不变性，因此共有5个自由度。能够利用八点法采用线性代数求解。

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

E=UΣVT

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

其中Rz(π/2)表示绕Z轴旋转90度获得的旋转矩阵。同时对E来讲，E与-E对零等式无影响。因此能够获得4种组合。

但根据点的深度值能够肯定正确的那一组组合。

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

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

略。

求得了相机运动参数以后，那么须要利用相机的运动参数估计特征点的空间位置。由s1x1=s2Rx2+t可得s1和s2的值(经过s1x1^x1=0=s2x1^Rx2+x1^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，它的李代数表示为ζ。假设某空间点坐标为Pi=[Xi, Yi, Zi]T。其投影的像素坐标为ui=[ui,vi]。根据第五章的内容，像素位置与空间点位置的关系以下：

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

siui=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对点的偏差项：ei=pi-(Rpi'+t)。而后构建最小二乘问题，求使偏差平方和达到极小的R,t。先求R,再求t。为求R，获得-tr(R∑qi'qiT)，要使R最小从而定义矩阵W=U∑VT，其中∑为奇异值组成的对角矩阵，对角线元素从大到小排列，而U和V为正交矩阵。当W满秩时，R为：

R=UVT.

获得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，得：

记梯度分别为Ix,Iy，速度分别为u,v，而右侧的灰度对时间的变化量It。

能够获得：

根据最小二乘法能够获得：

 经过特征块求解u,v。

获得了像素在图像间的运行速度u,v，就能够估计特征点在不一样图像中的位置了。当t取离散的时刻而不是连续时间时，咱们能够估计某块像素在若干个图像中出现的位置。因为像素梯度仅在局部有效，因此若是一次迭代不够好的话，咱们会多迭代几回这个方程。

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

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

slambook/ch8/useLK/useLK.cpp

能够看到特征点在逐渐减小，须要不断添加新的特征点进去。光流法的优势是能够避免描述子的计算，这能够避免误匹配，可是若是运动太快时就容易追踪错误仍是描述子好些。

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

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

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

e=I1(p1)-I2(p2)

利用最小二乘法

当有多个点时，

，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"

namespacemyslam

{//Pinhole RGB-D camera model

classCamera

{public:

typedef std::shared_ptr 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"

namespacemyslam

{//forward declare

classMapPoint;classFrame

{public:

typedef std::shared_ptrPtr;

unsignedlong 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

staticFrame::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

namespacemyslam

{classFrame;classMapPoint

{public:

typedef shared_ptrPtr;

unsignedlong 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(longid, Vector3d position, Vector3d norm );//factory function

staticMapPoint::Ptr createMapPoint();

};

}#endif

4. Map类

#ifndef MAP_H#define MAP_H#include"common_include.h"#include"frame.h"#include"mappoint.h"

namespacemyslam

{classMap

{public:

typedef shared_ptrPtr;

unordered_map map_points_; //all landmarks

unordered_map keyframes_; //all key-frames

Map() {}voidinsertKeyFrame( Frame::Ptr frame );voidinsertMapPoint( MapPoint::Ptr map_point );

};

}#endif //MAP_H

5. Config类

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

namespacemyslam

{classConfig

{private:static std::shared_ptrconfig_;

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

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

namespacemyslam

{classVisualOdometry

{public:

typedef shared_ptrPtr;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<:orb> orb_; //orb detector and computer

vector<:point3f> pts_3d_ref_; //3d points in reference frame

vector<:keypoint> keypoints_curr_; //keypoints in current frame

Mat descriptors_curr_; //descriptor in current frame

Mat descriptors_ref_; //descriptor in reference frame

vector<: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

voidextractKeyPoints();voidcomputeDescriptors();voidfeatureMatching();voidposeEstimationPnP();voidsetRef3DPoints();voidaddKeyFrame();boolcheckEstimatedPose();boolcheckKeyFrame();

};

}#endif

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

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

boolVisualOdometry::addFrame(Frame::Ptr frame)

{switch(state_)

{case INITIALIZING: //初始化，该帧为第一幅

{

state_=OK; //修改状态为OK

curr_=ref_=frame; //将该帧赋给当前帧和参考帧

map_->insertKeyFrame(frame); //将该帧插入地图//从第一帧中提取特征点

extractKeyPoints(); //提取特征点

computeDescriptors(); //计算描述子//计算参考帧中的特征点的三维坐标点

setRef3DPoints();break;

}caseOK:

{

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;

}caseLOST:

{

cout<

}

}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的时间内，有位姿x0到xN，而且有路标y1,…,yM。按照以前的写法，运动和观测方程为：

注意如下几点：

(1)在观测方程中，只有当xk看到了yj时，才会产生观测数据。

(2)咱们可能没有测量运动的装置，因此也可能没有运动方程。在这个状况下，有若干种处理方式：认为确实没有运动方程，或假设相机不动，或假设相机匀速运动。这几种方式都是可行的。在没有运动方程的状况下，整个优化问题就只由许多个观测方程组成。这就很是相似于StM(Structure from Motion)问题，由一组图像恢复运动和结构。与StM中不一样的是，SLAM中的图像有时间上的前后顺序，而StM中容许使用彻底无关的图像。

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

当只有位姿数据时，偏差累积会愈来愈大，不肯定性增高，而若是加上路标则会将不肯定性减少。以下图所示。

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

咱们但愿用过去0到k中的数据来估计如今的状态分布：

P(xk|x0,u1:k,z1:k)

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

由xk和zk的相互依赖关系，能够获得以下近似关系：

P(xk|x0,u1:k,z1:k) ∝P(zk|xk)P(xk|x0,u1:k,z1:k-1)

上式表示由zk推xk，跟由xk推zk结果是同样的几率上来讲。

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

P(xk|x0,u1:k,z1:k-1)= ∫P(xk|xk-1,x0,u1:k, z1:k-1)P(xk-1|x0,u1:k,z1:k-1)dxk-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讲 建图

以前只是提取了特征点，可是稠密的地图须要更多的点。特征点主要用于估计机器人位姿，而稠密的点用于避障和交互等。避障跟导航相似，可是更注重局部的、动态的障碍物的处理。一样，仅有特征点，咱们没法判断某个特征点是否为障碍物，因此须要稠密地图。