VSLAM学习素材是高翔老师的《视觉SLAM十四讲第二版》。这是我首次接触SLAM，感觉还算有趣。学士学完了，但是还需要系统的梳理一遍知识体系。

内容比较多，放在两个博客里记述，第一部分是基础知识，第二部分是SLAM的前端，第三部分是SLAM的后端，第四部分是SLAM中的回环检测和建图。

好了，废话不多说，开始吧~

第1讲 预备知识

没啥内容，略过

第2将 初始SLAM

2.2 经典视觉SLAM框架

一图以蔽之：

我们把相机传感器放到一个未知空间中，相机边移动边拍摄，我们的目的是通过这些帧图构建未知空间的地图。那么我们首先要先反向计算出相机是怎么移动的，再根据相机的移动轨迹来建图。【建图这里也许还要加上拍摄图片，记不清了，先这么理解，后面再说】

我们通过相机传感器得到了数据，现在需要用堆数据来建图。相机在不停的移动，我们把某个时刻得到的帧图交给前端视觉里程器，视觉里程器可以通过相邻帧的数据（这里要调前面传进来的帧图）来估算相邻相机的运动，进而算出此刻相机的位姿，还要估算这部分数据对应的局部地图。前端会将估计的相机位姿和局部地图提供给后端。

这些数据除了要交给视觉里程计，还要同时给回环检测，回环检测会对比当前帧图和之前的帧图，来判断相机是否之前来过此地，一旦检测到回环，就会将回环信息提供给后端。搞回环主要是为了修正漂移。

后端接收前端和回环给的信息，将这些信息进行优化，得到全局一致的相机轨迹和地图。

建图环节会根据估计的轨迹和任务需求，来建立不同的地图。

2.3 SLAM问题的数学表达

运动方程：k时刻相机位姿<—k-1时刻位姿x + k时刻运动传感器的读数/输入u + k 时刻噪声w。

观测方程：j位置k时刻的观测数据z<—j位置路标y + k时刻相机位姿x + 噪声v，相机对路标点拍摄后得到图像中的像素。

在SLAM里面，通常会已知运动测量的读数u（输入数据）来估计位置x（定位），已知传感器读数z（观测数据）来估计y（建图）。

这俩方程其实挺不规整的，要估计的值一个在右边一个在左边…有毒。

2.4 编程

1. linux 18.04
2. g++编译器
3. cmake编译器
4. IDE：KDevelop

第3讲 三维空间刚体运动

3.1 旋转矩阵

3.1.1 点、向量、坐标系

反对称符号^

3.1.2 坐标系间的欧式变换

将坐标系A中的向量通过计算变换到坐标系B中，可以用欧式变换来实现。

欧式变换包括旋转和平移。其中旋转可以用一个旋转矩阵R来描述，旋转+平移用变换矩阵T来描述。

需要注意的是：

• 旋转矩阵是一个正交矩阵，其逆亦为其转置
• 旋转矩阵的逆表示一个相反的旋转
• R12指坐标系2变换到坐标系1

3.1.3 变换矩阵与齐次坐标

通过引入齐次坐标，两次变换的叠加便可以用两个变换矩阵相乘来表示，算是一个数学上的trick。

需要注意，旋转矩阵不引入齐次坐标。

3.2 编程1

Eigen库：主要用来作矩阵运算

3.3 旋转向量和欧拉角

3.3.1 旋转向量

旋转矩阵缺陷：

• 自由度冗余
• 矩阵自带约束

旋转向量是另一种描述选择的方式，由旋转轴+旋转角组成。

（nθ）

旋转向量和旋转矩阵直接可以相互转换。

3.3.2 欧拉角

首先，这个欧拉角和欧拉变换没啥直接关系，就是撞名了。

由于旋转矩阵和旋转向量都不够直观，这里引出足够直观的另一种描述旋转的方式：欧拉角。

（[r,p,y]^T）

欧拉角将一次旋转分解为3次绕不同轴的旋转：

• 绕z，偏航角yaw
• 绕y，俯仰角pitch
• 绕x，滚转角roll

例如，rpy角的旋转顺序是ZYX

欧拉角的缺陷：万向锁，即欧拉角存在奇异性。

3.4 四元数

3.4.1 四元数的定义

• 旋转矩阵缺点：不够直观，冗余
• 旋转向量缺点：不够直观，奇异性
• 欧拉角缺点：奇异性
• 四元数缺点：不够直观，运算复杂

单位四元数可以描述一个三维旋转。

四元数和复数类有点像，一个实部+三个虚部。

q=q0+q1i+q2j+q3k;

q=[s,v]^T, s=q0, v=[q1,q2,q3]^T

3.4.2 四元数的运算

1. 加减
2. 乘法
3. 模长
4. 共轭
5. 逆
6. 数乘

3.4.3 用四元数表示旋转

，

其中q为旋转四元数，等于把p转到四维空间再转回来。

3.4.4 四元数到其他旋转表示的旋转

简单来说：

• 四元数和旋转向量可以相互转换
• 旋转矩阵和旋转向量可以相互转换

3.5 相似、仿射、射影变换

这些变化都是前面的欧式变换的拓展，思路差不多：

1. 相似变换
2. 仿射变换
3. 射影变换

3.6 编程2

Eigen几何模块

3.7 编程3

可视化演示

3.7.1 显示运动轨迹

Pangolin库：支持3D绘图

• 轨迹文件每一行格式：

，其中t为平移。

• 所谓轨迹，就是把一系列机器人坐标系原点在世界坐标系中的坐标连起来：

3.7.2 显示相机位姿

第4讲 李群与李代数

4.1 李群与李代数基础

旋转矩阵构成了特殊正交群SO(3)，变换矩阵构成了特殊欧式群SE(3)。

旋转矩阵和变换矩阵——只对乘法封闭，对加法不封闭。

群：只有一个封闭运算的集合

4.1.1 群

更严谨的定义一下群：群是一种集合加上一种运算的代数结构。
群里的集合对于群的运算满足以下性质：

• 封闭性
• 结合律
• 幺元
• 逆

李群：具有连续性质的群。

• SO(3)、SE(3)

但是由于SO(3)和SE(3)只有定义良好的乘法而没有加法，因此难以求导，故，引入李代数。

每一个李群都对应着一个李代数。

4.1.2 李代数的引出

对旋转矩阵求导引出的概念

4.1.3 李代数的定义

每个李群对应一个李代数。

李代数描述李群的局部性质，是单位元附近的正切空间。

李代数g：集合V + 数域F + 二元运算(李括号)[,]

4.1.4 李代数so(3)

SO(3)对应的李代数so(3)里的元素记作Φ：

so(3)是一个由三维向量组成的集合，每个向量对应一个反对称矩阵，可以用于表达旋转矩阵的导数。

so(3)与SO(3)关系：

4.1.5 李代数se(3)

se(3)的每个元素是一个6维向量，前三维表示平移，后三维表示旋转，其后三维就是so(3)的元素Φ。因此，se(3)可以理解为一个三维平移加上一个so(3)元素。

• ∧：指代从向量到矩阵
• ∨：指代从矩阵到向量

4.2 指数与对数映射

前面有提到so(3)和SO(3)之间可以用exp来转换，这章主要是说so(3)、se(3)和SO(3)、SE(3)之间的转换（也就是这个exp）究竟是如何计算的。

有趣的是，SO(3)对应旋转矩阵，而so(3)对应旋转向量。

4.2.1 SO(3)上的指数映射

• so(3)—>SO(3)：通过指数映射，可以将so(3)中的任意一个向量对应到SO(3)中的一个旋转矩阵上。

R=exp(Φ^)=exp(θa^), 其中θ是模长，a是方向。

• SO(3)—>so(3)：对数映射

• 需要注意的是，这种映射并不是一一对应的，一个SO(3)可能对应多个so(3)。

4.2.2 SE(3)上的指数映射

• se(3)—>SE(3)：指数映射

• SE(3)—>se(3)：略

• 总结一下转换关系：

4.3 李代数求导与扰动模型

4.3.1 BCH公式与近似模型

探索一下李群乘法和李代数加法之间的关系。

• SO(3) & so(3)：

  • 李群乘法：在李群上左乘小量，等于在李代数上加上一个左雅克比的逆乘小量

    给旋转矩阵R左乘一个微小扰动—ΔR·R（左乘右乘区别不大，可以互推）

    

    

  • 李代数加法：在李代数上加上小量，等于在李群上左乘一个左雅克比乘小量

    

• SE(3) & se(3)：

  

4.3.2 SO(3)上的李代数求导

选择矩阵R的求导在SLAM上有很强的应用性：

观测方程：z=Tp+w，相机以T位姿观察到p点，在w噪声情况下产生观测数据z。

虽然有时候我们是用观测数据和通过运动方程得到的相机位姿来估计路标点p，但有时候并没有相机位姿反而有路标点p的数据，我们也可以利用路标点和观测来估计位姿。在这种情况下，相当于寻找一个最优的T，使得整体误差最小化：

这是最小二乘估计，求解时目标函数J要对变换矩阵T求导。

总之就是SLAM里面经常出现”与位姿相关的函数对位姿求导”的情况，这时就需要对李群求导，但由于李群没有加法不太好搞，于是需要利用李代数来解决李群上的求导问题，思路包括：

1. 李代数求导模型：通过李群到李代数的转换，用李代数来表示位姿，然后对李代数求导
2. 扰动模型：对李群左乘（右乘）微小扰动，对该扰动求导

4.3.3 李代数求导

等效于

这个思路逻辑比较清晰，但是计算略麻烦，故通常并不使用这个求导思路。

4.3.4 扰动模型（左乘）

空间点p经过一次旋转R，对左扰动对应的李代数求导：设左扰动ΔR对应李代数φ

4.3.5 SE(3)上的李代数求导

扰动模型

空间点p经过一次变化T，对左扰动对应的李代数求导

编程

4.4.1 Sophus基本使用

Sophus库：李群/李代数库，本书使用带模板的Sophus库

4.4.2 评估轨迹误差

误差指标：

• 绝对轨迹误差ATE
• 绝对平移误差
• 相对位姿误差RPE
• 相对位姿平移误差

4.5 相似变换群与李代数

没看，略

第5讲 相机与图像

5.1 相机模型

相机将三维世界坐标点（m）映射到二维图像平面（像素）的过程可以用一个几何模型来描述。

相机内参数：针孔模型 + 畸变模型，外部三维点投影到相机内部的成像平面。

5.1.1 针孔相机模型

步骤：

• 现实世界P[X,Y,Z]^T
• 成像平面P’[X’,Y’,Z’]^T
  • 真实成像平面
  • 对称成像平面
  • 归一化成像平面[x,y]^T
• 像素平面[u,v]^T：在成像平面上对像进行采样和量化

内参：矩阵K为相机的内参数，有时生产厂商会告诉你，但有时也需要自己去标定。

外参：上面公式中的P实际上是相机的世界坐标根据相机位姿变换到相机坐标系下的结果。相机位姿，也就是这个变换矩阵就是相机外参。

这里的外参是SLAM中的待估计目标，代表着机器人的轨迹。

值得一提的是，这个过程中丢失了深度信息。

5.1.2 畸变模型

径向畸变（失真）：透镜形状引起的畸变

切向畸变：透镜未和成像平面严格平行引起的畸变

畸变系数：k1, k2, k3, p1, p2（前3个为径向的，后2个为切向的）

畸变变换发生在归一化成像平面[x,y]^T到像素平面[u,v]^T的过程中：

• 去畸变处理（畸变矫正）
  • 对整张图像去畸变，再用这个图像去对应空间位置
  • 对图像上的每个像素点去畸变，对应其畸变前的空间点

总的来说：

• 世界坐标Pw –+外参T–>
• 相机坐标Pc —>
• 归一化坐标 –+畸变–+内参K(α,β,c,f)–>
• 像素坐标Puv

5.1.3 双目相机模型

双目相机的原理是同步采集左右相机图像，计算图像视差，计算每个像素点的深度。

，b为基线

计算量比较大，要用GPU或FPGA来实现实时计算。

5.1.4 RGB-D 相机模型

RGB-D相机原理：主动测量每个像素深度

• 通过红外结构光原理测量 深度
• 通过飞行时间原理测量深度

主要问题是，发射-接收测量方式使得使用范围比较受限。

5.2 图像

计算机中图像存储为数值矩阵。

灰度图：每个像素位置对应一个灰度值I

1
2

unsigned char image[480][640]   //一张宽640像素，高480像素分辨率的灰度图
unsigned char pixel = image[y][x]   //访问图像像素，pixel=I(x,y)，注意下标

• 深度图
• 彩色图：多通道
  • opencv中通道顺序是B、G、R

5.3 编程：计算机中的图像

5.3.1 opencv基本使用

opencv库：图像处理算法库

本书用的是opencv3

5.3.2 图像去畸变

1

cv::Undistort() //去畸变函数

5.4 编程：3D视觉

5.4.1 双目视觉

从双目视觉的左右图像出发，计算图像对应的视差图，然后将像素点转换回相机坐标，构成物体的点云。

SGBM算法：计算视差

5.4.2 RGB-D视觉

位姿记录形式：[x,y,z,qx,qy,qz,qw]—平移向量+旋转四元数

1. 根据内参计算一对RGB-D图像对应的点云
2. 根据各张图的相机位姿，把点云加起来，组成地图

第6将 非线性优化

6.1 状态估计问题

根据第5讲的内容，观测方程可以写成：

SLAM的目标是通过带噪声的观测数据z和输入u，来推断位姿x和地图y，这就是个状态估计问题。

两种方案：

• 滤波器/增量方法
  • 在当前时刻的估计状态下，来一个新数据，更新一次状态
  • 只关心当前的xk
• 批量方法
  • 攒一波数据，然后一并处理
  • 目前的主流
  • 为了达到实时性，可以固定一些历史轨迹，仅对当前时刻附近的一些轨迹进行优化（滑动窗口估计）

6.1.1 批量状态估计与最大后验估计

MAP估计：

ML估计：在什么样的状态下(x,y)下，最可能产生当前观测到的数据

6.1.2 最小二乘的引出

先不考虑运动方程，且假设噪声是高斯分布。

这个式子等同于在最小化噪声项，因此ML估计可以等价于最小化误差项(噪声)。

• 上式的Q逆是信息矩阵
• 噪声项是一个二次型，称为马氏距离。

现在假设运动方程，考虑批量数据。

（因为独立）

上式的ML估计等同于最小化所有时刻的误差项：

这就是一个最小二乘问题了。

6.1.3 例子：批量状态估计

略

6.2 非线性最小二乘

这是一个比较正常的最小二乘的样子，一般用梯度下降来解

具体步骤：

6.2.1 一阶和二阶梯度法

J：一阶导数，梯度、雅克比矩阵

H：二阶导数，海塞矩阵

• 一阶梯度：最速下降法（并不是最小二乘）

  

  

• 二阶梯度：

  

  

• 缺点：

  • 最速过于贪心
  • 二阶里的H过于难算

6.2.2 高斯牛顿法

对误差：f(x)，而不是目标函数：F(x)，做一阶泰勒展开：

构建最小二乘问题：

得到增量方程（高斯牛顿方程）：

可以看到，高斯牛顿法用JJ^T来做H的近似，避免了H的复杂计算。

• 算法步骤：

  

• 算法缺点：

  • JJ^T不一定可逆，可能会导致无法收敛

• 补救措施：

  • 线搜索法：加上步长，即先找到方向，再确定长度。
  • 阻尼牛顿：引入信赖区域，认为近似只在区域内可靠

6.2.3 列文伯格-马夸尔特方法

又名阻尼牛顿法。

根据近似模型下降的值 如果能近似等于 函数实际下降的值，说明这个近似很nice，可以扩大近似的范围。（下式接近1）

算法步骤：

拉格朗日乘子来解，最后推导到求解：

值得注意的是，上面的非线性优化都需要提供初始值，SLAM中，通常使用ICP、PnP等算法来提供初始值x0。

6.3 编程：曲线拟合问题

6.3.1 手写高斯牛顿算法

6.3.2 使用Ceres进行曲线拟合

Cere库：最小二乘问题求解库

Ceres求解的最小二乘问题的一般形式（带边界的核函数最小二乘）：

图优化理论

G2o库：图优化库，SLAM领域广泛使用。

图优化是一种将非线性优化和图论结合起来的理论，将优化问题用图（贝叶斯图）的形式呈现。

图：顶点+边

• 顶点：表示优化变量
• 边：表示误差项

• 使用g2o拟合曲线

需要先将问题转化为图优化问题。

曲线拟合问题中只有一个优化变量，而带噪声的数据构成了诸多误差项。

步骤：

1. 定义顶点和边的类型
2. 构件图
3. 选择优化算法
4. 调用g2o进行优化，返回结果