SLAM之PTAM学习笔记

概述

PTAM，全称Parallel Tracking And Mapping，是最早提出将Track和Map分开作为两个线程的一种SLAM算法，是一种基于关键帧的单目视觉SLAM算法。PTAM主要分为这几部分：

1. Track线程

• FAST特征提取
• 地图初始化
• 跟踪定位
• 选取添加关键帧到缓存队列
• 重定位

2. Map线程

• 局部BundleAdjustment
• 全局BundleAdjustment
• 从缓存队列取出关键帧到地图
• 极线搜索加点到地图

另一方面，按照一般的视觉SLAM框架，PTAM也可分为

• 传感器数据获取（摄像头输入图像数据）
• 前端视觉里程计（跟踪定位、重定位）
• 后端优化（Bundle Adjustment）
• 建图（极线搜索加点）
• 没有回环检测

代码：cggos/ptam_cg，对原始PTAM代码进行了部分改动（将原来的makefile工程改成了cmake工程，改变了部分代码结构，添加带有公式的Doxygen文档注释，某些本文档提及的算法公式可能会出现代码注释中），核心算法没变。

FAST特征提取

• 为了SLAM的实时性，选择FAST作为特征提取的方法；
• 对于从数据流中输入的每一帧图像先进行金字塔分层(4层均值金字塔)，每一层都要进行FAST特征提取；

地图初始化

对于每帧图像提取的FAST特征点，因经常出现“扎堆”现象，再进行非极大值抑制，选出较好的特征点，然后对每个特征点计算Shi-Tomas得分，选出得分较高的特征点（不超过1000个，设置数量阈值），作为特征匹配的候选特征点。

先选择一帧图像，再通过基于SSD的块匹配选出第二帧图像，作为两帧关键帧；根据两帧图像间的匹配特征点，计算出两帧间的单应性矩阵，然后分解出对应的旋转平移矩阵，作为相机的初始位姿。

因单目的尺度不确定问题，根据经验设定一个尺度，作用于初始两帧间的旋转平移矩阵，并作为全局的尺度。

根据初始两帧间的旋转平移矩阵和特征点像素坐标，利用线性三角法深度估计算法估算出第一帧坐标系下的世界点三维坐标，再通过BundleAdjustment方法对世界点和相机初始位姿进行优化；因先前计算出的世界点数量可能不够，再通过极线搜索添加世界点，再通过BundleAdjustment方法对世界点和相机初始位姿进行优化。

根据现在的世界点，通过RANSAC找出主平面，作为系统的世界坐标系，同时计算出质心$C$；计算出内点和主平面质心的协方差矩阵，通过PCA主成分分析得出主平面的法向量$N$，然后通过Gram-Schmidt正交化计算出第一帧坐标系和主平面坐标系旋转矩阵$R$，再根据质心$C$和公式$P_w=R(P_c-C)=R{P}_c-RC$计算出平移向量$t=-RC$。

根据主平面计算出的旋转平移矩阵，将第一帧坐标系下的世界点和两帧的旋转平移矩阵变换到主平面对应的世界坐标系下，第二帧对应的旋转平移矩阵作为当前相机的位姿。

跟踪定位

（1）根据上一帧的相机位姿（旋转平移矩阵），通过作用运动模型和基于ESM的视觉跟踪算法对当前帧的相机位姿进行预测。

（2）根据预测的相机位姿，将当前所有世界点根据小孔成像原理进行投影，投影后的像素点记为$p_i$，并计算出对应的金字塔层级。

（3）根据金字塔高层优先原则，选取一定数量世界点（通常，粗搜索选取30～60个，细搜索选取1000个左右）。

（4）遍历选取的世界点，对于每一个世界点对应源帧图像中已经进行Warp变换的8x8的模板块，和以当前帧图像点$p_i$一定范围内的每一个FAST特征点为中心选取的8x8像素块，进行基于SSD的相似度计算，选择具有最小SSD值的FAST特征点，并记录查找到的特征点数量，用于后期跟踪质量评估；出于精确考虑，可通过反向合成图像对齐算法求取该特征点的亚像素坐标，记为$p_i^{\prime }$，这样选取的每个世界点都对应$p_i$和$p_i^{\prime }$，重投影误差即为$p_i^{\prime }-p_i$。

（5）根据重投影误差建立起误差函数，以预测的相机位姿作为初始值，通过高斯-牛顿非线性优化方法计算出当前帧的相机位姿，其中每次迭代的位姿增量为李代数形式。

实际上，PTAM中位姿计算分粗跟踪和细跟踪两个阶段，每个阶段均进行上述的（3）～（5）过程，主要差别在于选取世界点进行计算的点数。

关键帧选取

关键帧选取的指标主要有：

• 跟踪质量（主要根据跟踪过程中搜索到的点数和搜索的点数比例）
• 距离最近关键帧的距离是否足够远（空间）
• 距离上一关键帧的帧数是否足够多（时间）
• 关键帧缓存队列是否已满

重定位

构建关键帧时，每一帧都会生成一个高斯模糊小图（大小为顶层金字塔尺寸的一半，并进行了0.75的高斯卷积，以及去中心化）。

重定位时，基于SSD算法 计算 当前帧的高斯模糊小图 和 地图中所有的关键帧的高斯模糊小图 的 相似度，选择相似度最高的关键帧的相机位姿，根据基于ESM的视觉跟踪算法计算出相机位姿作为当前帧的相机位姿。

Bundle Adjustment

Bundle Adjustment（以下简称BA），中文翻译“光束法平差”，本质是一个优化模型，目的是最小化重投影误差，用于最后一步优化，优化相机位姿和世界点。PTAM中BA主要在Map线程中，分为局部BA和全局BA，是其中比较耗时的操作。

局部BA用于优化局部的相机位姿，提高跟踪的精确度；全局BA用于全局过程中的相机位姿，使相机经过长时间、长距离的移动之后，相机位姿还比较准确。

BA是一个图优化模型，一般选择LM(Levenberg-Marquardt)算法并在此基础上利用BA模型的稀疏性进行计算；可以直接计算，也可以使用g2o或者Ceres等优化库进行计算。

极线搜索

选择关键帧容器中最后一帧作为源帧，然后在所有关键帧中找到距离其最近的一帧作为目标帧。

通过源帧中像素特征点、场景平均深度和场景深度方差，根据对极几何原理，找出源帧中平均深度附近一定范围光束，并将其投影到目标帧成像平面，为一段极线。

遍历该段极线附近所有的候选特征点，通过基于SSD的块匹配方法查找出与源帧图像匹配的特征点，再通过反向合成算法求取其亚像素坐标，然后三角法计算世界点。

分析

优化算法：基本是基于最小二乘的非线性优化算法，跟踪部分使用G-N求解基于权重的最小二乘，BA使用L-M；

矩阵求逆：Cholesky分解；

线性方程组求解：SVD分解 ；

初始化相机位姿求解，用的是基于2D-2D的对极几何计算单应性矩阵，适用于共面特征点；后面跟踪部分相机位姿求解，用的是基于2D-3D的PnP算法；

块匹配：基于SSD的相似度计算；