PTAM算法是2007年提出的经典的单目特征点法SLAM，同时也是早期将SLAM和AR结合起来的工作之一。虽然PTAM几乎已经过时，但其在整个SLAM发展过程中占有重要地位：
- PTAM首先提出将定位(Tracking)和建图(Mapping)分为两个线程并行进行
- 计算资源因此得到了释放，所以PTAM也是第一个使用非线性优化的方案，精度自然更高
- PTAM引入关键帧机制，不必精细处理每一张图
在今天的众多先进的SLAM算法中仍可见PTAM的影子，因此学习一下PTAM还是很有必要的。在学习算法之前，先跑一下代码，对算法有一个直观的感受。我使用的环境是ubuntu16.04+ROS kinetic。PTAM主页见PTAM-ox，其代码见PTAM-github
I 跑PTAM
1.安装ROS kinetic
这个地方一般不会有问题，请参考ROS wiki或者一些博客，如博客，根据博客安装、初始化并测试ROS。如果出错，请换个源或者VPN试试。
2.编译PTAM
1.创建ROS工作空间
不同于上次使用rosbuild，这次我们使用catkin创建ROS工作空间。
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18mkdir -p ~/catkin_ws/src
cd ~/catkin_ws/src
catkin_init_workspace
# 在工作空间根目录编译，编译后自动出现build和devel文件夹及几个脚本文件，用于设置环境变量等
cd ~/catkin_ws/
catkin_make
# 运行脚本文件使其生效
source devel/setup.bash
# 查看环境变量是否生效
echo $ROS_PACKAGE_PATH
# 如果没添加上使用sudo gedit ~/.bashrc手动添加。或者使用下面命令。
# 这种方式对所有终端都有效，source只对当前终端有效！
echo "source ~/catkin_ws/devel/setup.bash">> ~/.bashrc
source ~/.bashrc
2.获取源码并编译
这里使用的是PTAM的ROS移植版本，如果编译出错，可以参考issues中的解答。编译之前可能需要提前安装一些依赖库，这个可以在网上找到。虽然编译过程中会出一些warning，但无大碍。当看到[100%]
Built target ptam信息表示编译成功。 1
2
3
4
5
6
7# 进入代码空间，获取源码，源码中包含若干功能包
cd ~/catkin_ws/src
git clone -b kineticbuild https://github.com/gjgjh/ethzasl_ptam
# 工作空间根目录下编译
cd ~/catkin_ws
catkin_make
3.标定相机
相机标定已有很多开源的工具，比如Matlab工具箱、Matlab标定工具、OpenCV和ROS都提供了一些工具包。这里我们尝试使用PTAM自带的工具进行标定。 首先在一个新的终端打开roscore，然后启动相机节点并发布消息。在之前的博客中，已经提到如何启动手机相机和IMU并发布消息了，这里就以手机相机为例进行标定（也可以使用电脑自带相机或usb相机）。在一个新的终端输入roslaunch android_cam-imu.launch启动相机节点以后，可以使用rostopic list命令查看当前已发布的话题：
1 | /android/imu |
因为PTAM标定要求输入为灰度图像，必须首先将彩色的image_raw转换为灰度图，否则会出现issues#78
这样的图像重影模糊问题。因此使用的自带的image_proc节点来转换：
1
2# 这里我的image_raw位于camera命名空间下
ROS_NAMESPACE=camera rosrun image_proc image_proc1
2ImageSizeX: 640
ImageSizeY: 4801
rosrun ptam cameracalibrator image:=camera/image_mono
4.运行PTAM
1 | roslaunch ptam ptam.launch |
现在就可以成功运行了！但是现在还是只能跑灰度图，不知道怎么解决。
II 算法学习
PTAM由论文Parallel Tracking and Mapping for Small AR Workspaces提出，下面我就自己对算法的理解进行一些总结。在本文中，主要参考了以下几个链接：
【2】ilotuo的博客
【3】PTAM slides
【4】快乐勇敢闯天涯的博客
PTAM整体流程图如下(图片来自[1])。按照高博的话说，单目特征点法就是“初始化－PnP－PnP－...”的过程。那么初始化部分，PTAM使用的是五点法求解本质矩阵，虽然现在一般用八点法求解更多一点。而PnP部分PTAM用的就是由粗到精两轮求解BA问题。在后端优化部分，使用了局部BA和全局BA进行优化。每次有新的关键帧插入时，则停止手头优化工作，以生成新的地图点。虽然大体上思路如此，但每个SLAM方案总是有大量的tricks，正是这些tricks让系统变得更加稳定有效，所以下面来看一下细节部分。
1 Tracking
Tracking负责相机位姿的估计和增强现实图像的绘制，Tracking部分必须实时进行，每秒30Hz。在Tracking线程，地图(由地图点和关键帧组成)是已知且固定的，初始的地图可由RANSAC+五点法＋三角化＋BA优化得到，后来的地图通过Mapping部分扩展和优化。这里按照原论文的顺序，先说Tracking部分。
1.1 预处理
将每一帧图像(640x480)的灰度图用于后面的Tracking计算，而将其彩色图用于AR的绘制展示。很多算法都会用到金字塔的概念，这里PTAM为了在求位姿时更好更快收敛，也用到了金字塔图像，进行了由粗到精(coarse-to-fine)两轮求解位姿，具体在1.5节会介绍。PTAM通过降采样构建了四层金字塔图像，并对每一层都进行了FAST角点检测，如下图所示，从左到右记为层0、层1、层2、层3，层数越高，分辨率越低。
1.2 投影地图点
在迭代求解相机位姿前，一般要给一个位姿的初始值。论文使用速度衰减模型估计当前帧的初始位姿(但没有查到具体的表达式)。
然后将地图点重投影到当前帧。这里重投影用的就是普通的针孔相机模型，只不过与我们常用的多项式径向畸变模型不同，PTAM用了一个叫FOV模型。重投影时需要确定哪些地图点是当前帧可视的，还要确定对应应该搜索的金字塔层数(确定方法见下一节)。
1.3 Patch匹配
重投影后，我们还没解决数据关联（data association）的问题，如果不知道地图点重投影像素和当前帧像素一一对应关系，就无法求解位姿(这和一般的特征点法SLAM步骤不太一样，见1.7部分)。因此，必须进行特征匹配。以地图点重投影后的位置为圆心，在一个半径范围内进行特征的搜索匹配。但是PTAM没有使用特征点，而是使用一块8x8小区域(即patch)进行匹配，因此自然没有ORB等特征那样的旋转、尺度不变性了。为了去除观测位置姿态不同的影响，在匹配patch前必须先做仿射变换(我认为这里是在小区域内用仿射变换近似实际的透视变换，在一阶导近似，不确定对不对)。仿射变换矩阵\(\mathbf A\)定义为： \[ \mathbf A=\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} \] 其中，变量\(u_s,v_s\)表示在patch的源关键帧层0上水平和竖直方向的像素点位移，变量\(u_c,v_c\)表示对应在当前帧层0上水平和竖直方向的像素点位移。\(\mathbf A\)由各偏导数项组成，其计算分两步，先把源关键帧层0上单位像素反向投影到地图点所在平面(把地图点近似看成一个小平面)，然后再从这个平面投影到当前帧层0上，然后就得到了对应的像素位移值，即一阶偏导。具体的示意图可以参考链接[2]。
当前帧比源关键帧位置更靠近某地图点时，地图点会显得更大，此时画面尺度变大，根据这个面积值大小决定搜索在当前帧哪一层进行，以弥补尺度变化。 \(\mathbf A\)的行列式表示源关键帧层0上一个像素在当前帧层0上占的面积，因此行列式表示变换后该点面积的放大倍数。根据\(det(\mathbf{A})/4^l\)接近１的程度，可确定该地图点应该搜索的金字塔层数。比如说当前帧离地图点很近，画面面积比源关键帧放大了64倍，那么\(l\)就应该等于3，表示应该在高层(层3)搜索匹配，相当于把画面尺度减小一点以和源patch相适应。
接着对源patch进行变换，变换矩阵为\(\mathbf A/2^l\)(行列式接近１)，因为通过金字塔已经弥补了尺度上的差异，相当于只是对观测角度进行一个修正。然后可以重采样出一个新的8x8的patch作为匹配模板。接下来以地图点重投影后的位置为圆心，在一个半径范围内进行匹配，其中只在FAST角点位置进行匹配(FAST角点为中心的8x8patch)。匹配的相似度用的是均值归一化SSD，可抵抗光照变化影响。
最后，当搜索层数大于0时，特别是在高层匹配到的patch位置不确定性比较大，而重投影误差计算时需要它在层0的图像坐标。因此可以迭代误差最小化来获得patch精确位置(亚像素)，即通过最小化平均patch灰度差(平移是变量)来求精确位置。论文使用的是反向合成法做图像对齐，然后获得层0亚像素位置。但是为了节省计算量，只是对其中一部分patch做了精确匹配。
1.4 更新相机位姿
解决了匹配的问题，就可以进行优化了。优化是在李代数\(\mathfrak{se}(3)\)上进行，使用加权最小二乘法，迭代10次最小化重投影误差来求解。为了对粗差鲁棒，还使用了Tukey核函数。
1.5 两轮求解
PTAM为了加速计算，设计了从粗到细两轮求解过程，粗测阶段只对少量(50个)对应最金字塔最高层的地图点进行搜索匹配，搜索的半径设置大一些，优化出的位置姿态作为精测阶段的初值；精测阶段会纳入更多点(1000个)和金字塔所有层，搜索的半径相对小一点。之前在1.3节提到的精确匹配只在粗测阶段进行。
1.6 Tracking质量
PTAM根据patch匹配时成功匹配的比率，用三个级别评判Tracking质量：好、不好、丢失。只会在“好”的状态下插入新关键帧和地图点；不好的时候只对当前帧Tracking；如果“丢失”，会有简单的重定位功能(在所有关键帧中找相似的)。
1.7 小结
一般我们是先配准，再重投影，再优化的一个过程，但PTAM的顺序是先重投影，再配准，再优化。前两步共同完成了数据关联。直接法将数据关联与位姿估计放在了一个统一的非线性优化问题中，而特征点法则分步求解即，先通过匹配特征点求出数据之间关联，再根据关联来估计位姿。这两步通常是独立的。因此可以看出PTAM属于特征点法。虽然作为特征点法的一种，但它使用的仅仅是8x8的一个patch，甚至感觉称不上是特征。因此本身不具有像ORB这种特征的旋转和缩放不变性，所以要通过仿射变换矩阵来弥补视角变换的影响。
2 Mapping
Mapping负责生成和不断优化地图，不必实时进行，只对关键帧进行处理，但仍会耗费大量计算资源。
2.1 初始化
初始的地图由RANSAC+五点法＋三角化＋BA优化得到。用户需要点击两下来确定两个初始关键帧，因为单目尺度未知，所以两立体像对距离当成常数(10cm)固定下来。然后为了方便将AR物体绘制在一个平面上，PTAM对当前地图做RANSAC估计主平面(可参考链接[2])，并将地图的坐标系变换至这个主平面上。
2.2 关键帧判断
PTAM从以下几个角度判断当前帧是否是关键帧：
- 质量。关键帧必须跟踪的质量为“好”，见1.6节；
- 时间。距离上一个关键帧至少20帧图片；
- 空间。距离最近的地图点大于一个阈值，这是为了保证基线足够大，测图精度高。这个阈值与观测的平均深度有关，比如当相机离一个表面很近时，这个阈值减小，关键帧更密一些；而当相机离一面墙很远时，阈值变大，关键帧就稀疏一点。
当判断当前帧不是关键帧，则做BA优化(2.3节)；当判断当前帧是关键帧，则将关键帧插入到地图中(2.4节)。
2.3 BA优化
PTAM注意到BA可以通过其稀疏性来极大地减少计算，因此首次将非线性优化引入SLAM中。PTAM将BA分为局部BA和全局BA两部分，也是为了由粗到精计算。另外在BA时仍使用了Tukey核函数来增加鲁棒性。
在局部BA阶段，只考虑滑动窗内的关键帧(5帧)，以及它们能观测到的所有地图点。全局BA阶段，优化对象纳入所有的关键帧和地图点。
在空闲时间Mapping线程可以利用旧的关键帧改善地图，要么从旧的关键帧观察新添加的地图点，要么重新测量之前被剔除的粗差点，如果被成功观测并收敛，则作为新的地图点插入地图。
2.4 关键帧插入
当判断加入新的关键帧，则停止手头优化工作。在计算资源支持的条件下，Mapping线程希望得到的地图越丰富、越精确越好。因此做两件事情：
把所有地图点投影到这个新的关键帧（Tracking线程处于计算量的考虑只投影了一部分地图点），为之后的BA做准备。
生成新的地图点：
- 对新关键帧的FAST角点做非极大值抑制，并筛选出最显著（Shi-Tomasi分数）的一批角点。然后通过与成功匹配角点的距离判断是否已经在地图中有该点了，如果已经存在则删除
- 在最近的关键帧上沿极线搜索匹配点，只要能找到匹配点，就三角化出地图点
- 对金字塔四层重复进行上述操作
2.5 小结
PTAM没有回环检测，远距离会出现尺度漂移，因此只能局限于小的场景使用。为了解决这个问题，在ORB-SLAM中还引入了第三个线程，即闭环。
个人理解错误的地方还请不吝赐教，转载请标明出处