视觉SLAM简介（限于初学者）
点击上方“3D视觉工坊”，选择“星标”
干货第一时间送达
本文由知乎作者黄浴授权转载，不得擅自二次转载。原文链接：https://zhuanlan.zhihu.com/p/112124572
经典的运动重建（SFM，Structure From Motion），也叫做同步定位和制图（simultaneous localization and mapping，SLAM），基本上是假设场景是静态情况下通过摄像机的运动来获取图像序列并得到场景3-D结构的估计，也是计算机视觉的重要任务；在机器人领域，这个任务还会估计现场摄像头的姿态和位置，即定位任务；下面介绍一下视觉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为例，给定一组匹配特征点（n7）
定义F矩阵元素fij（i=1～3，j=1～3），那么线性齐次方程组为
简记为
Qf = 0
做SVD得到Q = USVT，而最终F的解是V最右的奇异向量（singular vector）；
另外，因为E矩阵完全由摄像头外参数（R和t共6个未知参数，但有一个不确定平移尺度量）决定，所以存在5点算法求解E矩阵。
可以分解E得到摄像头外参数，其步骤如下：
1）同样对E矩阵做奇异值分解（SVD）：
2）那么第一个摄像头投影矩阵简单设为P1=[I|0]，而第二个摄像头矩阵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），算法如下（如图所示）：
首先，3D点表示为4个控制点的加权和；
控制点坐标是求解的（12）未知数；
3D点投影到图像上以控制点坐标建立线性方程；
控制点坐标表达为零特征向量（null eigenvectors）线性组合；上面组合的权重（i）是新未知数（<4）；
5）增加刚体（rigidity）约束以得到i二次方程；
6）根据i数目（无论线性化，或重新线性化）求解。
（注：有时候3-D-2-D匹配比3-D之间匹配的精度高）
PnP求解的示意图
这里需要补充两个概念，一是鲁棒估计的随机样本共识法（RANSAC，RANdom SAmple Consensus），另一个是全局优化的集束修正法（BA，bundle adjustment）。
RANSAC的目的是在包含异常点（outlier）的数据集上鲁棒地拟合一个模型，如图所示：
随机选择（最小）数据点子集并实例化（instantiate）模型;
基于此模型，将所有数据点分类为内点（inlier）或异常点；
迭代重复1-2步；
选择最大的内地集，以此重新估计最终模型。
RANSAC示意图
BA的目的是优化全局参数估计，对SLAM来说，结构重建的3-D点X和其对应的2-D特征点x，还有估计的视角变换参数（甚至包括摄像头内参数）P，位于一个重投影（reprojection）误差函数D最小化框架下（如图所示），即
这里集束（Bundle）指2-D点和3-D点之间的光线集，而修正（Adjustment）是指全局优化过程；其解法是非线性迭代的梯度下降法，如Gauss-Newton方法和其修正Levenberg-Marquardt方法，因为问题自身的特性，这里的雅可比矩阵是非常稀疏的；另外，只取重建3-D点修正的话，称为structure only BA，而只取视角变换修正的话，称为motion-only BA。
BA示意图
在以上基础之上，再重新审视SLAM过程：
首先要提到概念VO，即视觉里程计（visual odometry）；VO是SLAM的一部分，VO主要是估计视角之间的变换，或者运动参数，它不需要输出制图（mapping）的结果，而且BA就是motion-only的模式。
SLAM方法分两种途径，一种是递推滤波器方法，另一种是批处理估计方法：
滤波器方法，比如卡尔曼滤波递推估计，实际上建立一个状态空间的观测模型和状态转换（运动）模型；观察模型描述当摄像头姿态和地标位置（来自于地图）已知时观测（地标）的概率; 运动模型是系统状态（摄像头姿态）转换的概率分布，即马尔可夫过程; 那么在递归贝叶斯估计中，同时更新系统状态和建立的地图，其中融合来自不同视角的观测来完成制图，而估计系统状态可计算摄像头的姿态，即定位问题。
批处理估计方法，也称“关键帧”方法，其步骤是：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），对应的视觉里程计VO部分是VIO（visual-inertial odometry）。
上述内容，如有侵犯版权，请联系作者，会自行删文。
推荐阅读：
吐血整理|3D视觉系统化学习路线
那些精贵的3D视觉系统学习资源总结（附书籍、网址与视频教程）
超全的3D视觉数据集汇总
大盘点|6D姿态估计算法汇总(上)
大盘点|6D姿态估计算法汇总（下）
机器人抓取汇总|涉及目标检测、分割、姿态识别、抓取点检测、路径规划
汇总|3D点云目标检测算法
汇总|3D人脸重建算法
那些年，我们一起刷过的计算机视觉比赛
总结|深度学习实现缺陷检测
深度学习在3-D环境重建中的应用
汇总|医学图像分析领域论文
大盘点|OCR算法汇总
汇总|3D点云目标检测算法
重磅！3DCVer-知识星球和学术交流群已成立
3D视觉从入门到精通知识星球：针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕，更有各类大厂的算法工程人员进行技术指导，650+的星球成员为创造更好的AI世界共同进步，知识星球入口：
学习3D视觉核心技术，扫描查看介绍，3天内无条件退款
圈里有高质量教程资料、可答疑解惑、助你高效解决问题
欢迎加入我们公众号读者群一起和同行交流，目前有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、CV入门、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别等微信群，请扫描下面微信号加群，备注：”研究方向+学校/公司+昵称“，例如：”3D视觉 + 上海交大 + 静静“。请按照格式备注，否则不予通过。添加成功后会根据研究方向邀请进去相关微信群。原创投稿也请联系。
▲长按加群或投稿