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\)接近1的程度,可确定该地图点应该搜索的金字塔层数。比如说当前帧离地图点很近,画面面积比源关键帧放大了64倍,那么\(l\)就应该等于3,表示应该在高层(层3)搜索匹配,相当于把画面尺度减小一点以和源patch相适应。
接着对源patch进行变换,变换矩阵为\(\mathbf A/2^l\)(行列式接近1),因为通过金字塔已经弥补了尺度上的差异,相当于只是对观测角度进行一个修正。然后可以重采样出一个新的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中还引入了第三个线程,即闭环。
个人理解错误的地方还请不吝赐教,转载请标明出处