简单的V-SLAM介绍-电子发烧友网

【导读】SLAM是“Simultaneous Localization And Mapping”的缩写，可译为同步定位与建图。最早，SLAM 主要用在机器人领域，是为了在没有任何先验知识的情况下，根据传感器数据实时构建周围环境地图，同时根据这个地图推测自身的定位。因此本文以简单清晰的文字为大家介绍了视觉V-SLAM。

简单的V-SLAM介绍，就当入门：）。

首先，从双目立体几何（stereo geometry）原理开始来定义外极（epipolar）约束：两个摄像头光心分别是 c0 和 c1，3-D 空间点 p 在两个图像平面的投影点分别是 x0 和 x1，那么直线 c0c1 和两个图像平面的交点即外极点（epipole）e0 和 e1，pc0c1 平面称为外极平面（epipolar plane），它和两个图像平面的交线 l0 和 l1 即外极线（epipolar line）；可以看到两个摄像头坐标系之间的转换满足（R，t），同时说明摄像头 c0 的图像点 x0 在摄像头 c1 的图像对应点 x1 一定落在其外极线 l1，反之依然；

（a）对应一条光线的外极线（b）对应的外极线集合和外极平面外极线约束

这里需要定义一个本质矩阵（essential matrix）E 和一个基础矩阵（fundamental matrix）F：E = [t]×R，其中 [t]× 是反对称矩阵，即定义向量

那么

而 F = A−TEA−1，其中 A 是摄像头内参数矩阵；对矩阵 E 和矩阵 F 来说，均满足外极约束方程x1TEx0 =0，x1TFx0 =0

前者是摄像头已标定情况下图像特征点的对应关系，后者是摄像头未标定情况下图像特征点的对应关系；

其次，得到外极线约束（以F矩阵为例，E矩阵同样）如下

l1=Fx0，l0=x1F，

以及，外极点约束（以F矩阵为例，E矩阵同样）如下

Fe0=0，FTe1=0；

根据上面图像特征点的外极约束方程，有8 点算法求解 F 和 E，以 F 为例，给定一组匹配特征点（n> 7）

定义F矩阵元素fij（i=1～3，j=1～3），那么线性齐次方程组为

简记为

Qf = 0

做SVD得到Q = USVT，而最终F的解是V最右的奇异向量（singular vector）；

另外，因为E矩阵完全由摄像头外参数（R和t共6个未知参数，但有一个不确定平移尺度量）决定，所以存在5 点算法求解E矩阵；

可以分解E得到摄像头外参数，其步骤如下：

i. 同样对E矩阵做奇异值分解（SVD）：

ii. 那么第一个摄像头投影矩阵简单设为

而第二个摄像头矩阵P2有四种可能情况，如图（a）-（d）所示：

其中

从E矩阵分解得到R和t

根据视图方向与摄像头中心到3-D点的方向之间夹角可以发现，四个可能中只有情况（a）是合理的解；

确定两个视角的姿态之后，匹配的特征点 x，x’可以重建其 3-D 坐标X，即三角化（triangulation）理论；首先存在一个线性解：设两个摄像头投影矩阵为 P 和 P’，相应的它们列向量为pi，pi’，i=1～3，则有方程组：AX= 0

其中

但一些误差干扰的存在，上述线性解是不存在的；所以需要一个非线性的解，这里采用F 矩阵定义的外极约束方程xTFx’= 0，得到最小化误差函数为

下面采用外极线 l, l’的来约束定义误差，如图所示，将目标函数重写为

外极线定义的误差

另外，在已知重建的 3-D 点集，如何和新视角的 2-D 图像特征点匹配呢？这个问题解法称为PnP（Perspective n Points），算法如下：

i. 首先，3D 点表示为 4 个控制点的加权和；

ii. 控制点坐标是求解的（12）未知数；

iii. 3D点投影到图像上以控制点坐标建立线性方程；

iv. 控制点坐标表达为零特征向量（null eigenvectors）线性组合；

v. 上面组合的权重（bi）是新未知数（<4）；

vi. 增加刚体（rigidity）约束以得到bi二次方程；

vii. 根据bi数目（无论线性化，或重新线性化）求解。

（注：有时候3-D-2-D匹配比3-D之间匹配的精度高）

PnP求解的示意图

这里需要补充两个概念，一是鲁棒估计的随机样本共识法（RANSAC，RANdom SAmple Consensus），另一个是全局优化的集束修正法（BA，bundle adjustment）：

i. RANSAC的目的是在包含异常点（outlier）的数据集上鲁棒地拟合一个模型，如图 2-12 所示：

1. 随机选择（最小）数据点子集并实例化（instantiate）模型;

2. 基于此模型，将所有数据点分类为内点（inlier）或异常点；

3. 迭代重复 1-2 步；

4. 选择最大的内地集，以此重新估计最终模型。

RANSAC示意图

ii. BA的目的是优化全局参数估计，对 SLAM 来说，结构重建的 3-D 点X和其对应的 2-D 特征点x，还有估计的视角变换参数（甚至包括摄像头内参数）P，位于一个重投影（reprojection）误差函数 D 最小化框架下（如图所示），即

BA示意图

这里集束（Bundle）指2-D点和3-D点之间的光线集，而修正（Adjustment）是指全局优化过程；其解法是非线性迭代的梯度下降法，如Gauss-Newton 方法和其修正 Levenberg-Marquardt 方法，因为问题自身的特性，这里的雅可比矩阵是非常稀疏的；另外，只取重建 3-D 点修正的话，称为 structure only BA，而只取视角变换修正的话，称为 motion-only BA；

在以上基础之上，再重新审视SLAM过程：

i. 首先要提到概念 VO，即视觉里程计（visual odometry）；VO 是 SLAM 的一部分，VO 主要是估计视角之间的变换，或者运动参数，它不需要输出制图（mapping）的结果，而且 BA 就是 motion-only 的模式；

ii. SLAM 方法分两种途径，一种是递推滤波器方法，另一种是批处理估计方法；

a)滤波器方法，比如卡尔曼滤波递推估计，实际上建立一个状态空间的观测模型和状态转换（运动）模型；观察模型描述当摄像头姿态和地标位置（来自于地图）已知时观测（地标）的概率; 运动模型是系统状态（摄像头姿态）转换的概率分布，即马尔可夫过程; 那么在递归贝叶斯估计中，同时更新系统状态和建立的地图，其中融合来自不同视角的观测来完成制图，而估计系统状态可计算摄像头的姿态，即定位问题;

b)批处理估计方法，也称“关键帧”方法，其步骤是：

1）首先通过选择的头两（关键）帧，采用双目几何的特征点匹配得到初始的3-D点云重建；

2）正常模式：假设 3D 地图可用，并且估计出摄像头增量运动，跟踪特征点并使用 PnP 进行摄像头姿态估计；

3）恢复模式：假设 3D 地图可用，但跟踪失败故没有增量运动，可相对于先前重建的地图重新定位（re-localize）摄像头姿势；

4）关键帧 BA：保持一个“关键帧”子集，状态向量是所有关键帧的 3D 地标和对应摄像头姿势，BA 可以在与跟踪模块并列的线程中调整状态估计；

（注意：关键帧的选择策略是算法性能很重要的一个因素）

SLAM 中的闭环（loop closure）问题：当摄像头又回到原来位置，称为闭环，需要检测闭环，并以此在 BA 框架下优化结构和运动估计；闭环检测和重定位是类似的，可以基于图像特征的匹配实现，俗称“基于图像的重定位（image-based re-localization），当关键帧子集较大的时候，需要对特征匹配进行压缩和加速，比如词包（bag of words）法和K维-树（KD-tree）数据结构等等；

SLAM 中的传感器可以是单目、双目、深度传感器（RGB-D）甚至激光雷达，也可以和 IMU 融合，称为 VINS（visual inertial navigation system）。

附录：G-N 和 L-M 的非线性最小二乘算法

假设有观测向量 zi’，其预测模型为 zi = zi(x)，其中x为模型参数；那么最小二乘（LS）法就是最小化如下代价函数：平方误差加权和（weighted Sum of Squared Error，SSE)