视觉 SLAM 十四讲-基础数学篇

1、第1讲  前言

SLAM 是 Simultaneous Localization and Mapping 的缩写，中文译作“同时定位与地图构建”。本书介绍以视觉传感器为主体的视觉 SLAM 技 术，分为两部分：第一部分为数学基础篇，第二部分为 SLAM 技术篇。

本书采用C++ 作为编码语言，所有源代码均托管到 github 上:https://github.com/gaoxiang12/slambook

2、第2讲  初识SLAM

（1）引子:小萝卜的例子

如下图所示我们希望机器人小萝卜具有自主运动能力，够在房间里自由的移动，它至少需要知道两件事:1. 我在什么地方?——定位。2. 周围环境是什么样?——建图。

“定位”和“建图”，可以看成感知的“内外之分”。内就是白自身的状态(即位置)，外就是外在的环境(即地图)。

视觉 SLAM 是本书的主题，按SLAM照相机的工作方式，我们把相 机分为单目(Monocular)、双目(Stereo)和深度相机(RGB-D)三个大类，此外还有全景相机和事件相机等特殊或新兴的种类。

（2）经典视觉 SLAM 框架

如下图所示即为经典的视觉 SLAM 框架：

我们把整个视觉 SLAM 流程分为以下几步:

1. 传感器信息读取。在视觉 SLAM 中主要为相机图像信息的读取和预处理。如果在机 器人中，还可能有码盘、惯性传感器等信息的读取和同步。

2. 视觉里程计 (Visual Odometry, VO)。视觉里程计任务是估算相邻图像间相机的运动， 以及局部地图的样子。VO 又称为前端(Front End)。

3. 后端优化(Optimization)。后端接受不同时刻视觉里程计测量的相机位姿，以及回 环检测的信息，对它们进行优化，得到全局一致的轨迹和地图。由于接在 VO 之后， 又称为后端(Back End)。

4. 回环检测(Loop Closing)。回环检测判断机器人是否曾经到达过先前的位置。如果 检测到回环，它会把信息提供给后端进行处理。

5. 建图(Mapping)。它根据估计的轨迹，建立与任务要求对应的地图。

（3）SLAM 问题的数学表述

运动方程：

观测方程：

参数化运动方程：

参数化观测方程：

 

把运动方程和观测方程取成通用的抽象形式，那么 SLAM 过程可总结为两个基本方程:

我们按照运动和观测方程是否为线性，噪声是否服从高斯分布进行分类，分为线性/非线性和高 斯/非高斯系统。

（4）实践：编程基础

这部分主要讲了安装linux系统和使用g++、cmake、IDE等不同编译方式编译HelloSLAM程序，顺带讲了讲如在编程中何使用库。

3、第3讲  三维空间钢体运动

（1）旋转矩阵

1.点和向量、坐标系

2.坐标系间的欧氏变换

3.变换矩阵与齐次坐标

（2）实践：Eigen

这个例程演示了 Eigen 矩阵的基本操作与运算。比如Matrix定义方式：Eigen::Matrix<数据类型, 行数, 列数> Matrix实例名称，矩阵Amatrix_33的转置、求逆、求和、求迹、行列式等方法以及矩阵方程求解等，详见代码。

（3）旋转向量和欧拉角

旋转向量罗德里格斯公式：

以ZY X 为例的欧拉角：

1. 绕物体的 Z 轴旋转，得到偏航角 yaw;
2. 绕旋转之后的 Y 轴旋转，得到俯仰角 pitch;                                                                                                                                                                                              3. 绕旋转之后的 X 轴旋转，得到滚转角 roll。

（4）四元数

（5）相似、仿射、射影变换

（6）实践：Eigen几何模块

（7）可视化演示

4、第4讲  李群与李代数

（1）李群与李代数基础

1）群

群(Group)是一种集合加上一种运算的代数结构。我们把集合记作 A，运算记作 ·，那么群可以记作 G = (A, ·)。群要求这个运算满足以下几个条件:

1. 封闭性: ∀a1,a2 ∈A, a1 ·a2 ∈A.
2. 结合律: ∀a1,a2,a3 ∈ A, (a1 ·a2)·a3 = a1 ·(a2 ·a3).
3. 幺元: ∃a0 ∈A, s.t. ∀a∈A, a0 ·a=a·a0 =a.                                                                                                                                                                                                  4. 逆: ∀a∈A, ∃a−1 ∈A, s.t. a·a−1 =a0.

矩阵中 常见的群有:一般线性群 GL(n)，特殊正交群 SO(n)，特殊欧氏群 SE(n)， 群结构保证了在群上的运算具有良好的性质，而群论则是研究群的各种结构和性质的理论。李群是指具有连续(光滑)性质的群。 SO(n) 和 SE(n)，它们在实数空间上是连续的，都是李群。

2）李代数的引出

考虑任意旋转矩阵 R，我们知道它满足:

（假设t0=0，同时在 t0 附近，设 φ 保持为常数 φ(t0) = φ0）

3）李代数的定义

每个李群都有与之对应的李代数。李代数描述了李群的局部性质。通用的李代数的定 义如下:

李代数由一个集合 V，一个数域 F 和一个二元运算 [, ] 组成。如果它们满足以下几条 性质，称 (V, F, [, ]) 为一个李代数，记作 g。

1. 封闭性 ∀X,Y ∈V,[X,Y]∈V.
2. 双线性 ∀X,Y,Z ∈V,a,b∈F,有:[aX+bY,Z]=a[X,Z]+b[Y,Z], [Z,aX+bY]=a[Z,X]+b[Z,Y].

3. 自反性x ∀X∈V,[X,X]=0.
4. 雅可比等价 ∀X,Y ,Z ∈ V,[X,[Y ,Z]] + [Z,[Y ,X]] + [Y ,[Z,X]] = 0.

其中二元运算被称为李括号。

4）李代数so(3)

5）李代数se(3)

 

（2）指数与对数映射

1）SO(3)上的指数映射

2）SE(3)上的指数映射

（3）李代数求导与扰动模型

1）BCH公式与近似形式

两个李代数指数映射乘积的完整形式，由 Baker-Campbell-Hausdorff 公式(BCH 公 式)x给出：

特别地，考虑 SO(3) 上的李代数 ln (exp (φ∧1 ) exp (φ∧2 ))∨，当 φ1 或 φ2 为小量时，小量二次以上的项都可以被忽略掉。此时，BCH 拥有线性近似表达:

同样的，对于 SE(3)，亦有类似的BCH 近似公式:

2）SO(3)李代数上的求导

设某个时刻小萝卜的位姿为 T。它观察 到了一个世界坐标位于 p 的点，产生了一个观测数据 z。那么，由坐标变换关系知:z = T p + w.                                                                我们通常会计算理想的观测与实际数据的误差: e = z − T p.

求解此问题，需要计算目标函数 J 关于变换矩阵 T 的导数。我们经常会构建与位姿有关的函数，然后讨论该函数关于 位姿的导数，以调整当前的估计值。使用李代数解决求导 问题的思路分为两种:                                                                                                                                                                                                              1. 用李代数表示姿态，然后对根据李代数加法来对李代数求导。                                                                                                                                                              2. 对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。

3）李代数求导

4）扰动模型（左乘）

5）SE(3)上李代数上的求导

（4）实践：Sophus

一个较好的李代数库是 Strasdat 维护的 Sophus 库。Sophus库有模板类和非模板类的版本。你可以输入以下命令获得非模板类的 Sophus:

1. git clone https://github.com/strasdat/Sophus.git

2. cd Sophus

3. git checkout a621ff

（5）相似变换群与李代数

（6）小结

本讲引入了李群 SO(3) 和 SE(3)，以及它们对应的李代数 so(3) 和 se(3)。我们介绍 了位姿在它们上面的表达和转换，然后通过 BCH 的线性近似，我们可以对位姿进行求导 和扰动了。

5、第5讲  相机与图像

前面两讲中，我们介绍了“机器人如何表示自身位姿”的问题，部分地解释了 SLAM 经典模型中变量的含义和运动方程部分。本讲，我们要讨论“机器人如何观测外部世界”， 也就是观测方程部分。而在以相机为主的视觉 SLAM 中，观测主要是指相机成像的过程。

（1）相机模型

1）针孔相机模型

针孔相机模型

 

设 P 的坐标为 [X, Y, Z]T ，P ′ 为 [X′, Y ′, Z′]T ，并且设物理成像平面到小孔的距离为 f(焦距)。那么，根据三角形相似关系，有:

 

  

2）畸变

径向畸变的两种类型

切向畸变来源示意图

径向畸变

切向畸变

3）双目相机模型

像素点可能存在的位置

双目相机的成像模型

4）RGB-D相机模型

（2）图像

（3）实践：图像的存取与访问

1）安装 OpenCV

2）操作OpenCV图像

（4）实践：拼接点云

 

6、第6讲  非线性优化

（1）状态估计问题

1）最大后验与最大似然

贝叶斯法则左侧通常称为后验概率。它右侧的 P (z|x) 称为似然，另一部分 P (x) 称 为先验。直接求后验分布是困难的，但是求一个状态最优估计，使得在该状态下，后验概 率最大化(Maximize a Posterior，MAP)，则是可行的:

请注意贝叶斯法则的分母部分与待估计的状态 x 无关，因而可以忽略。贝叶斯法则告 诉我们，求解最大后验概率，相当于最大化似然和先验的乘积。进一步，我们当然也可以 说，对不起，我不知道机器人位姿大概在什么地方，此时就没有了先验。那么，可以求解 x 的最大似然估计(Maximize Likelihood Estimation, MLE):

“在什么样的状态下，最可能产生现在观测到的 数据”。这就是最大似然估计的直观意义。

2）最小二乘的引出

（2）非线性最小二乘

1. 给定某个初始值 x0。

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

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

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

1）一阶和二阶梯度法

最速下降法

一阶梯度法（最速下降法）： 

 

二阶梯度法（牛顿法）：   

2）Gauss-Newton

    

1. 给定初始值 x0。
2. 对于第 k 次迭代，求出当前的雅可比矩阵 J(xk) 和误差 f(xk)。                                                                                                                                                                3. 求解增量方程:H∆xk = g.
4. 若 ∆xk 足够小，则停止。否则，令 xk+1 = xk + ∆xk，返回 2.

3）Levenberg-Marquadt

 

 

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

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

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

3.计算 ρ。

4.若 ρ > 43 ，则 μ = 2μ;

5.若 ρ < 41 ，则 μ = 0.5μ;

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

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

4）小结

这里只罗列了最常见的两 种非线性优化方案，Gauss Newton 和 Levernberg-Marquardt

（3）实践：Ceres

1）Ceres简介

2）安装Ceres

3）使用Ceres拟合曲线

（4）实践：g2o

1）图优化理论简介

图优化，是把优化问题表现成图(Graph)的一种方式。这里的图是图论意义上的图。 一个图由若干个顶点(Vertex)，以及连接着这些节点的边(Edge)组成。进而，用顶点 表示优化变量，用边表示误差项。于是，对任意一个上述形式的非线性最小二乘问题，我们可以构建与之对应的一个图。

2）g2o的编译与安装

3）使用g2o拟合曲线

（5）小结

本节介绍了 SLAM 中经常碰到的一种非线性优化问题:由许多个误差项平方和组成的 最小二乘问题。我们介绍了它的定义和求解，并且讨论了两种主要的梯度下降方式:Gauss- Newton 和 Levenberg-Marquardt。在实践部分中，我们分别使用了 Ceres 和 g2o 两种优 化库求解同一个曲线拟合问题，发现它们给出了相似的结果。