Stereo Parallel Tracking and Mapping for robot localization（S-PTAM）

1. 介绍

按照并行跟踪与映射（PTAM）的方法，S-PTAM将问题分为两个主要的并行任务：摄像机跟踪和地图优化。跟踪线程匹配特征、创建新点并估计每个新帧的相机姿势，映射线程迭代地细化组成地图的附近点地标。
S-PTAM特点：
1）利用SLAM的并行性实现实时性能，同时最小化线程间依赖性。
2) 使用立体摄像机，避免单目自举问题，并允许在没有任何先验信息的情况下计算映射环境的度量尺度。
3) 在独立线程中执行一个局部可观察区域中迭代地运行映射优化（Bundle Adjustment）操作，从而提高全局一致性。
4) 可用于早期姿势预测和更有效的特征匹配。
5) 对姿势和地图优化算法实施立体约束，提高了稳健性。
6) 二进制特征用于描述可视点地标，从而减少存储需求并提高匹配速度。

2. 方法

S-PTAM从世界参考帧的标准姿势开始，并通过对第一对立体图像中的匹配特征进行三角化来初始化。跟踪线程为每个新进入的立体帧估计当前姿态，从而最小化图像上的特征与其对应的地图点之间的重新投影误差。同时，映射优化线程不断地试图通过BA中的所有点和关键帧来最小化重新投影错误。

1. 跟踪

1）特征提取：BRIEF 描述子，Shi-Tomasi 特征提取，速度不如FAST快，但通过非最大抑制在图像上“良好”的选定点的空间分布，显著地提高了跟踪过程中的姿势细化的鲁棒性。
2）特征匹配：
3）姿态优化：使用Levenberg-Marquardt算法，通过非线性最小二乘方程迭代最小化重新投影误差
4）键帧选择：被跟踪点的数量小于最后一个关键帧中被跟踪点的90%时，当前帧被选择为关键帧

2. 映射

BA优化最小化方程：
考虑到Jacobian的稀疏块特性，使用了稀疏Levenberg-Marquardt实现：

3. 实验

1. MIT数据集

如图1，机器人跟随的近50米轨迹中，最大计算误差为0.6米。

图1 估计轨迹与地面真实情况

图2，相对跟踪误差不会随着行驶距离的增加而增加，且不会超过20cm。在图3中，在14秒处，峰值显示80度的误差，之后，该方法通过跟踪先前构建的地图立即恢复，S-PTAM对这种异常值表现出了很强的鲁棒性。

图2 MIT序列1相对于地面真位姿计算的轨迹欧氏距离

图3 轨迹相对于地面真实位姿的角度偏差

2. KITTI数据集

相对定位误差不会随着行驶距离的增加而增加。在车辆跟随的4km轨道上，最大绝对定位误差为16m。

图4 KITTI估计轨迹与地面真实情况

图5 地面真实位姿的欧几里德轨迹距离