PTAM视觉SLAM发展过程中非常重要的一个成果,是它首先把非线性优化应用到SLAM中,并引入关键帧,利用Bundle Adjustment的稀疏性和多线程并行的方式实现了实时运行。

概述

  1. 手动获取两个关键帧进行初始化,得到初始的map和第一个相机姿态;
  2. 对于每个新采取到的关键帧进行下采样的4级金字塔分解,并对金字塔的每一级进行FAST-10的角点检测;
  3. Tracking 线程和Mapping线程开始并行工作;
  4. Tracking线程对于每个新加入的关键帧,先用相机运动模型估计一个相机姿态,然后再把当前map中的点投影到当前的帧中得到potentially-visible-set(PVS),然后进行Patch搜索找到当前图像中的PVS中的点得到(successful observed point),找到了匹配点对就可以用来更新相机姿态。
  5. Mapping线程得到一个关键帧后需要判断该关键帧是否符合加入到map的条件,若符合根据非极大值抑制和Shi-Tomasi得分赛选出高质量的角点,然后剔除successful observed point 周围10*10像素的点,剩下的点就作为加入map的候选点;然后根据相机姿态选取最近的帧来进行三角化得到候选点的三维坐标加入到map中。

初始化

系统的初始化需要使用者的配合, 首选需要使用者在一个位置手动获取一个关键帧,然后把相机平滑的移动一段距离再手动的获取一个关键帧,对于第一帧中的每一个角点都在第二帧的角点中进行简单的像素Patch搜索以找到匹配点对,根据这些匹配点对可以进行三角化得到初始的map。利用五点法和RANSAC算法可以估计出一个本质矩阵(essential matrix),通过对该本质矩阵进行SVD分解可得到旋转矩阵和平移向量进而得到第一个相机姿态。

由于初始的map具有任意的尺度,所以AR应用不方便,所以假设相机移动了10厘米来缩放初始的map。然后根据初始的map利用RANSAC算法拟合一个平面:

  1. 随机选取很多组来3个点来假设一个平面,剩下的点用来验证与这个平面的一致性,剔除掉离群点
  2. 通过一致点集的空间均值和方差来改进获胜的平面
  3. 协方差矩阵的最小特征向量就构成了平面的法向量

Tracking

Tracking线程最主要的任务就是计算每一帧的相机姿态,对于每一帧:

  1. 用相机运动模型预估计一个相机姿态
  2. 利用预估的姿态把当前map中所有点投影到当前帧中
  3. 选取很小一部分点(60个)进行一次大规模的Patch搜索并进行一次粗略的姿态更新
  4. 选取大规模的点(1000个)进行重投影和Patch搜索,并利用得到的搜索结果更新相机姿态

相机运动模型

该模型假设相机是每秒一帧,更新步骤:

1
2
3
4
5
6
7
8
// 新的预估姿态
SE3<> newPrePose = SE3<>::exp(veclocity)*oldPose;
// 根据特征点更新相机姿态
SE3<> newPose = updataCameraPose();
// 新的相机运动
Vector<6> cameraMotion = SE3<>::ln(newPose);
// 更新速度
Vector<6> veclocity = 0.9 * (0.5*veclocity + 0.5*cameraMotion)

相机姿态和投影

假设空间中一点 \(p\) 在世界坐标系 \(W\) 中的坐标为 $ (X,Y,Z) $ 那么把它转换到相机坐标系 \(C\) 的点 \(p_c(x,y,z)\) 公式为: \[ p_c = E_{CW}p \] \[ E_{CW} = \left[\begin{matrix} R & T\\ 0^T&0\end{matrix}\right] \]

把点从相机坐标系转换到图像坐标 \((u,v)\) : \[ \bigl( \begin{matrix} u\\ v\end{matrix} \bigr) = CamProj\left(\begin{smallmatrix}x\\y\\z\\1\end{smallmatrix}\right) = \bigl(\begin{matrix} c_x\\c_y\end{matrix}\bigr) + \left[\begin{matrix}f_u & 0\\0&f_v\end{matrix}\right]\frac{r'}{r}\bigl(\begin{matrix} \frac{x}{z}\\\frac{y}{z}\end{matrix}\bigr) \]

其中 \(r 和 r'\) 计算方式如下: \[ r = \sqrt{\frac{x^2+y^2}{z^2}} \]

\[ r' = \frac{1}{w}\arctan{(2r\tan\frac{w}{2})} \]

\(w\) 是PTAM所使用的FOV模型的畸变系数,相机的姿态变化可以通过当前姿态左乘一个 \(4\times4\) 的矩阵M: \(\left[\begin{matrix} R & T\\ 0^T&0\end{matrix}\right]\) 实现,M是三维欧氏群 \(SE(3)\) 的元素,那它就可以用一个对应的六维向量 \(\mu\) 的指数映射来表示,它的前三个元素表示平移,后三个元素代表旋转: \[ E_{CW} ’ = ME_{CW} = exp(\mu)E_{CW}\tag{1} \]

Patch搜索

对map中的点,仅在它在当前帧的预计位置的固定范围内搜索,因此要考虑到第一次观察到该点的关键帧和当前关键帧之间的视点的变化而引起的扭曲(warp),首先需要计算一个仿射变换矩阵A: \[ A = \left[\begin{matrix}\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{matrix}\right] \] \((u_s,v_s)\) 表示在patch的源金字塔level中的像素的横纵偏移,\((u_c,v_c)\) 表示在当前帧的第0级金字塔上的像素偏移。求解A的步骤如下,假设map中的点为 \(p\)

  1. \(p\) 在当前相机坐标系中的位置
  2. \(p\) 在源关键帧中右单位向量和下单位向量投影到当前帧
  3. 计算源图像中像素运动的导数

在计算得到A之后,利用A的行列式值来确定应该在那一层进行搜索:

1
2
3
4
while(det(A) > 3 && searchLevel < 3) {
searchlevel++;
det(A) *= 0.25;
}

右单位向量,下单位向量,可以围城一个平行四边形,利用双线性插值得到一个 \(8\times8\) 像素的搜索模板,在预估位置的固定半径内的角点取模板计算零均值SSD来寻找最佳匹配。

相机姿态更新

Patch搜索会得到一个点集 \(S\) ,用相机运动模型预估的姿态显然是相当不靠谱的,假设 \(S\) 中的点 $p_i $ 利用预估姿态计算得到的当前帧中的估计值为: \[ (\hat u_i,\hat v_i)^T = CamProj(exp(\mu)E_{CW}P_i) \] 该点的实际观测值,既是Patch搜索得到的最佳匹配值为 \((u_i, v_i)^T\) 那么测量残差为 \(e_i\) 为: \[ e_i = (u_i,v_i)^T - (\hat u_i, \hat v_i)^T \] 姿态更新实质就是要求得一组参数 \(\mu\) 使得 \(\sum_{i \in S}e_i ^2\) 最小,同时使用Tukey biweight objective 函数进行M-estimator。用高斯牛顿迭代来求解这个最小二乘问题。

M-estimator

所谓的M估计方法就是用一个残差 $e_i $ 的函数来替换 \(e_i ^2\) ,减小异常残差对系的印象,Tukey biweight objective函数如下: \[ f(x) = \begin{eqnarray} \begin{cases} x(1-\frac{x^2}{c^2} )&|x| < c \\ 0 & |x|> c \end{cases} \end{eqnarray} \] PTAM中貌似只是用它来给每个残差添加一个权重…

高斯牛顿迭代

高斯牛顿法的基本思想为用一系列的线性最小二乘问题来求解非线性最小二乘问题:假设 \(x(k)\) 是解的第 \(k\) 次逼近,那么在 \(x(k)\) 处将目标函数线性化(求导并令等于0),求解第 \(k+1\) 次的近似解,重复该过程直到达到迭代准则,用矩阵表示如下: \[ J_k ^T J_k P_k ^{GN} = -J_K ^T r_k \] 上式中,\(r_k\) 表示残差,$ P_k ^{GN}$ 表示第K次的最佳修正因子。

  1. 取初值,设置迭代终止条件:迭代次数K和精度要求 \(\omega\)
  2. 计算雅可比矩阵
  3. \(||J_Kr|| \leq \omega\) 或者达到迭代次数,终止迭代,\(x(k)\) 就为最优解,否则解上面的方程组,求得 \(P_k ^{GN}\)
  4. \(x(k+1) = x(k) + P_k ^{GN},k++\) 循环

雅克比矩阵

雅克比矩阵的行代表信息,列代表约束,每一行是一个点在该位姿下的误差向量,每一列代表的是函数对某一分量的偏导数,参数向量 \(\mu\) 可以表示为 \((T_x, T_y, T_z,\alpha, \beta,\gamma)\) ,遍历所有点对参数向量求偏导数就可以得到雅克比矩阵: \[ \left[ \begin{matrix} \frac{\partial \hat u_1}{\partial T_x} & \frac{\partial \hat u_1}{\partial T_y} & \frac{\partial \hat u_1}{\partial T_z} &\frac{\partial \hat u_1}{\partial \alpha} & \frac{\partial \hat u_1}{\partial \beta} & \frac{\partial \hat u_1}{\partial \gamma} \\ \frac{\partial \hat v_1}{\partial T_x} & \frac{\partial \hat v_1}{\partial T_y} & \frac{\partial \hat v_1}{\partial T_z} &\frac{\partial \hat v_1}{\partial \alpha} & \frac{\partial \hat v_1}{\partial \beta} & \frac{\partial \hat v_1}{\partial \gamma} \\ \vdots & \vdots & \vdots & \vdots & \vdots & \vdots \\ \frac{\partial \hat u_i}{\partial T_x} & \frac{\partial \hat u_i}{\partial T_y} & \frac{\partial \hat u_i}{\partial T_z} &\frac{\partial \hat u_i}{\partial \alpha} & \frac{\partial \hat u_i}{\partial \beta} & \frac{\partial \hat u_i}{\partial \gamma} \\ \frac{\partial \hat v_i}{\partial T_x} & \frac{\partial \hat v_i}{\partial T_y} & \frac{\partial \hat v_i}{\partial T_z} &\frac{\partial \hat v_i}{\partial \alpha} & \frac{\partial \hat v_i}{\partial \beta} & \frac{\partial \hat v_i}{\partial \gamma} \\ \end{matrix} \right] \] 由于非线性最小二乘计算很复杂,在精细搜索的时候需要计算上千个点,每次迭代都进行非线性计算的话实时性得不到保障,所以在进行高斯牛顿迭代的过程中第0、4、9次进行非线性计算,其他时候进行线性计算:

1
v2Image += m26Jacobian * v6;

跟踪质量评价

在Patch搜索的步骤中,会对所有能够投影到当前帧中的Map点进行Patch搜索以找到在当前帧中的对应点,假设对每个金字塔level,所有能投影到当前帧中点的总数为 \(Total_{level}\) ,经过patch搜索能找到对应点的数量为 \(Found_{level}\) ,那么跟踪质量评价的方式为:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
// 在金字塔第0级和第1级中的所有点和所有找到点的总数
int Total01 = 0;
int Found01 = 0;
// 在金字塔第2级和第3级中的所有点和所有找到点的总数
int Total23 = 0;
int Found23 = 0;
for (int i = 0;i < 4; i++) {
Total01 += Total[i];
found01 += Found[i];
if (i>=2){
Total23 += Total[i];
found23 += Found[i];
}
}
if (Found01/Total01 > 0.3)
Good;
else if (Found23/Total23 <0.13)
BAD;
else
DODGY;
if (DODGY)
// 当前帧与map中最近帧的距离
if DistanceToNearestKeyFrame > 0.1 *10;
BAD;

丢失找回

如果连续三帧图像的跟踪质量都被判定为BAD,那么系统就判定跟踪丢失了,启动跟踪找回机制。在Mapping线程每次添加关键帧时,都会计算一个金字塔最上层图像一半大小的图像,并做高斯模糊保存起来,当跟踪丢失时,给当前帧也进行同样的处理,然后到与所有关键帧的模糊图像进行ZSSD匹配,选出匹配度最高的关键帧,然后计算当前帧相对于最相似帧的旋转和位移(SE2),然后进而得到重定位的位置和姿态(SE3)。

Mapping

并不是视频流中的每一帧都要加入到map中,因此需要对视频帧进行选择,同时满足三个条件的关键帧才可以加入到map中:

  1. 跟踪质量评价必须为good

  2. 时间上距离上一关键帧必须相隔至少20帧
  3. 距离map中已经存在的点的最小距离满足一定值

当一个关键确定要加入到map中首先选出其中的候选点,假设它为目标帧,然后在map中选取和当前帧相机姿态最接近的关键帧(源帧)来进行三角化,这一步必然涉及到匹配点的搜索,利用极线搜索可以大大的减小搜索的复杂度。

过滤角点

Shi-Tomasi得分

Shi-Tomasi算法是一种Harris角点检测算法的改进,和Harris算法一样,也需要计算一个角点的领域窗口的海森矩阵(Hessian)矩阵H,H矩阵的形式如下: \[ H = \sum_{p\in S} \left[\begin{matrix}d_x ^2(p)&d_x(p)d_y(p)\\d_x(p)d_y(p)&d_y ^2(p)\end{matrix}\right] \] 其中,p表示角点的坐标,\(S\) 表示p周围指定范围内的所有像素,在PTAM中S表示一个以点p为中心的一个 \(7\times7\) 的patch ,\(d_x(p)、d_y(p)\) 分别表示点p在 \(x\)\(y\) 方向上的导数,那么__矩阵H较小的特征值__就是点p的Shi-Tomasi得分值。如果一个角点的Shi-Tomasi得分值小于70,那么就认为该点不满足要求,丢弃掉。

非极大值抑制

非极大值抑制首先需要计算每个角点的得分,既是在以角点为中心,半径为3的Bresenham圆上所有像素和角点的灰度的差值,正负分别求和,正负两个值的绝对值中较大的值就为该角点的得分。然后再检查每个角点的周围 \(3\times 3\) 的范围内是否具有比它得分更高的角点,如果有就删除该角点。

极线搜索

假设源帧中的点 \(p_s\) 投影到源相机的摄像机坐标系下 \(z=1\) 的平面上得到 \(p_{sc}\) ,然后转换到目标帧的摄像机坐标系下: \[ p_{tc} = R_tR_s ^{-1}p_{sc};O_t = -M_tR_s ^{-1}T_s \] 根据源帧中所有点的深度均值(dMean)和深度的标准差(dSigma)来确定搜索范围: \[ lineStart = O_t + max(10, dMean-dSigma) \]

\[ lineEnd = O_t + min(40*10 , dMean +dSigma) \]

把起点和终点都投影到图像平面就得到了需要搜索的极线了。把极线附近的点加入到带匹配集中,用patch搜索计算ZSSD,得分值最低的就是匹配点。

三角化

在已知对应点和两个视图的摄像机矩阵,根据三角原理构建出方程: \[ \left(\begin{matrix} P_2 ^T & - &vP_3T \\ P_1 ^T & - & uP_3 ^T \\ P_2 ' ^T & - & v'P_3 ' ^T \\ P_1 '^T & - & v'P_3 ' ^T \end{matrix}\right)X = 0 \]

\[ P = K\left(\begin{matrix}R&T\end{matrix}\right) \]

其中X就是所有的三维点坐标。但是无法直接求解,所以利用svd分解来求解。

Bundle Adjustment

Bundle Adjustment依然是计算雅可比矩阵,用LM迭代求解,虽然它可以有比较好的优化效果,但是但是计算量非常大,所以PTAM有局部和全局两种策略。局部策略是选取当前帧之前的4帧对当前帧进行优化。

数据关联优化

当系统闲下来时(没有新关键帧加入,Bundle Adjustment全局和局部都收敛了)就优化map:

  1. 在进行极线搜索添加新点的时候只考虑了两个视图,但是很有可能该点在多个视图中可见,所以这时就利用这些视图重新计算该点;
  2. 在具有重复纹理的地方,Tracking系统的计算可能出错,所以重新计算被Tracking系统标记为离群点的点,如果满足添加到map的条件就添加到map中否者彻底删除。