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 流程分为以下几步:
-
传感器信息读取。在视觉 SLAM 中主要为相机图像信息的读取和预处理。如果在机 器人中,还可能有码盘、惯性传感器等信息的读取和同步。
-
视觉里程计 (Visual Odometry, VO)。视觉里程计任务是估算相邻图像间相机的运动, 以及局部地图的样子。VO 又称为前端(Front End)。
-
后端优化(Optimization)。后端接受不同时刻视觉里程计测量的相机位姿,以及回 环检测的信息,对它们进行优化,得到全局一致的轨迹和地图。由于接在 VO 之后, 又称为后端(Back End)。
-
回环检测(Loop Closing)。回环检测判断机器人是否曾经到达过先前的位置。如果 检测到回环,它会把信息提供给后端进行处理。
-
建图(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:
-
git clone https://github.com/strasdat/Sophus.git
-
cd Sophus
-
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 两种优 化库求解同一个曲线拟合问题,发现它们给出了相似的结果。