PTAM笔记

[TOC]

概述

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

• Track线程

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

• Map线程

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

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

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

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

初始化

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

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

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

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

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

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

Tracking

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

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

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

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

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

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

FAST特征提取

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

关键帧选取

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

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

旋转预测 (ESM-tracking)

• paper: Real-time image-based tracking of planes using Efficient Second-order Minimization

• PTAM跟踪过程中的旋转预测方法

std::pair<SE3<>,double> SmallBlurryImage::CalcSBIRotation(
  SmallBlurryImage *pSBIRef, ATANCamera camera, int nIterations) {
    std::pair<SE2<>, double> pair_ret = IteratePosRelToTarget(*pSBIRef, nIterations);

    std::pair<SE3<>, double> result_pair;
    result_pair.first  = SE3fromSE2(pair_ret.first, camera);
    result_pair.second = pair_ret.second;

    return result_pair;
}

TODO

仿射变换与图像扭曲(Image Warping)

$ {u_s,v_s} $ 对应源帧金字塔层水平和竖直方向的像素位移，$ {u_c,v_c} $ 对应当前帧金字塔0层水平和竖直方向的像素位移，$ u_c,v_c $ 分别为 $ u_s,v_s $ 的函数，则 $ u_c,v_c $ 的微分（矩阵形式）为：

\[ \begin{bmatrix} du_c \\ dv_c \end{bmatrix} = \begin{bmatrix} \frac{\partial{u_c}}{\partial{u_s}} & \frac{\partial{u_c}}{\partial{v_s}} \\ \frac{\partial{v_c}}{\partial{u_s}} & \frac{\partial{v_c}}{\partial{v_s}} \end{bmatrix} \begin{bmatrix} du_s \\ dv_s \end{bmatrix} \]

Affine Warp Matrix (Jacobian) 为：

\[ A = J = \begin{bmatrix} \frac{\partial{u_c}}{\partial{u_s}} & \frac{\partial{u_c}}{\partial{v_s}} \\ \frac{\partial{v_c}}{\partial{u_s}} & \frac{\partial{v_c}}{\partial{v_s}} \end{bmatrix} \]

在二维情况，雅可比 \(J\) 行列式 代表 当前c平面上的面积微元 与 源s平面上的面积微元的比值。

行列式$ |A^{-1}| $ 的几何意义： 向量 $ du_s , dv_s $ 与 向量 $ du_c , dv_c $ 分别张成的平行四边形的面积之比

// in PatchFinder::CalcSearchLevelAndWarpMatrix()

Vector<3> v3Cam = se3CFromW * p.v3WorldPos;
double dOneOverCameraZ = 1.0 / v3Cam[2];

// Project the source keyframe's one-pixel-right and one-pixel-down vectors into the current view
Vector<3> v3MotionRight = se3CFromW.get_rotation() * p.v3PixelRight_W;
Vector<3> v3MotionDown  = se3CFromW.get_rotation() * p.v3PixelDown_W;

// Calculate in-image derivatives of source image pixel motions:
mm2WarpInverse.T()[0] = 
  m2CamDerivs * (v3MotionRight.slice<0,2>() - v3Cam.slice<0,2>() * v3MotionRight[2] * dOneOverCameraZ) * dOneOverCameraZ;
mm2WarpInverse.T()[1] = 
  m2CamDerivs * (v3MotionDown.slice<0,2>() - v3Cam.slice<0,2>() * v3MotionDown[2] * dOneOverCameraZ) * dOneOverCameraZ;

TODO

重定位

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

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

Mapping

极线搜索

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

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

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

Bundle Adjustment

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

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

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

总结

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

矩阵求逆：Cholesky分解；

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

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

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