（四）ORBSLAM运动估计
ORBSLAM2的运动估计简介
ORBSLAM2中的运动估计核心方法就是3D-2D的PNP，而在跟踪过程主要分为三种类型：
- 无运动模型的跟踪，即基于参考帧的跟踪；
- 基于匀速运动模型的跟踪；
- 重定位；
上述三种方案，我们只介绍前两种，重定位由于需要用到回环检测，我们会在之后讲解。
PNP运动估计
在介绍ORBSLAM2的跟踪策略之前，我们先了解一下他所用的运动估计方法——PNP。
PNP是一种将匹配点从三维空间投影到像平面并与观测数据计算误差来估计相机运动的方法，我们也管这种方法叫重投影误差。基于解析的PNP方法只采用了少量的匹配对即可估计相对运动，然而，在SLAM问题当中，通常约束条件较多，因此基于解析的方法通常不能更好地利用约束条件。另外，若将错误匹配对纳入解析方程中，会错误估计相机运动。尽管可以通过随机选取多组匹配对进行估算多个相机运动并根据一定条件进行筛选出最合适的相机运动，但是笔者更倾向于用PNP解析解估算出一个初值，随后通过非线性优化的方法来优化相机运动。不过，值得一提的是，ORBSLAM2并不用PNP求初值，而是直接用参考帧的运动作为当前帧的运动初值进行优化。因此，本文只介绍基于非线性优化的PNP方法。
与之前一样，为了让大家更直观的看到问题的描述，笔者为大家提供了一个图：
其中，空间点 $P$ 齐次坐标为 $P=[X,Y,Z,1]^{T}$，其在图像 $I_1$ 中投影的像素坐标为 $p_1=[u_1,v_1]^{T}$，在图像 $I_2$ 中的重投影像素坐标为 $p_2^{'}=[u_2^{'},v_2^{'}]^{T}$，而空间点 $P$ 在图像 $I_2$ 中的观测值为 $p_2=[u_2,v_2]^{T}$ ,$e=p_2-p_2^{'}$ 表示重投影误差。
图中的理想重投影过程用公式$(1)$表达如下：
\begin{equation}
s_2u_2=Kexp(\xi^{\land})P
\end{equation}
其中，$s_2$ 表示空间点 $P$ 在图像 $I_2$ 所在相机坐标系的深度。$K$ 表示相机内参数，$exp( \xi^{\land} )$ 表示相机从图像 $I_1$ 到图像 $I_2$ 的姿态变换阵，也可以用 $T$ 表示，$\xi$ 表示 $T$ 对应的李代数。
然而，由于相机姿态初值估计不够好，因此重投影时通常会与真实值存在一定的误差，这个误差我们定义如下：
\begin{equation}
e_2 = u_2 - \frac{1}{s_2}Kexp(\xi^{\land})P
\end{equation}
一个相机姿态下观测到的特征点通常不止一个，假设我们有 $N$ 个特征点，则构成求相机姿态 $\xi$ 的最小二乘问题：
\begin{equation}
\xi^{\ast} = \mathop{\arg\min}_{\xi}\frac{1}{2}\sum_{i=1}^{N}\left\|e_i\right\|_2^{2} = \mathop{\arg\min}_{\xi}\frac{1}{2}\sum_{i=1}^{N}\left\|u_i - \frac{1}{s_i}Kexp(\xi^{\land})P_i\right\|_2^{2}
\end{equation}
为了满足所有重投影误差最小，我们通过不断调整$\xi$的值，使得上述误差不断下降，最终得到一个对当前匹配对最优的相机姿态。但是 $\xi$ 应该怎么调整？我们知道最小二乘优化是通过不断调整增量，使得整体的误差不断下降，直到收敛的过程。在这个过程中，我们需要选择增量的方向和步长。根据优化理论知识我们可知，求出误差相对于位姿的雅克比，这个优化问题我们基本上就算解决了。如果用了牛顿法，我们还需要再求个海塞阵，但通常我们都会用高斯牛顿法来替代牛顿法，避免计算海塞阵。
通过链式法则，我们可以求出误差相对于位姿的雅克比：
\begin{equation}
\frac{\partial e}{\partial \delta\xi}=\frac{\partial e }{\partial P^{'}}\frac{\partial P^{'}}{\partial\delta\xi}
\end{equation}
其中，$P^{'}$ 表示 $P$ 位于图像 $I_2$ 处的相机坐标系下的坐标。
上述雅克比为：
\begin{equation}
\frac{\partial e}{\partial \delta\xi} = -{\left[ \begin{array}{cccccc}\frac{f_{x}}{Z^{'}} & 0 & -\frac{f_{x}X^{'}}{Z^{'2}} & -\frac{f_{x}X^{'}Y^{'}}{Z^{'2}} & f_{x}+\frac{f_{x}X^{'2}}{Z^{'2}} & -\frac{f_{x}Y^{'}}{Z^{'}} \\
0 & \frac{f_{y}}{Z^{'}} & -\frac{f_{y}Y^{'}}{Z^{'2}} & -f_{y}-\frac{f_{y}Y^{'2}}{Z^{'2}} & \frac{f_{y}X^{'}Y^{'}}{Z^{'2}} & \frac{f_{y}X^{'}}{Z^{'}} \end{array} \right]}
\end{equation}
得到雅克比以后我们就可以通过g2o或者ceres来求解公式 $(3)$ 的最小二乘问题了。（具体源码，之后笔者会更新在github上，并将在此附上链接）
那么，到这里，基于非线性优化的PNP方法就差不多介绍完了。如果我们公式 $(3)$ 拓展一下，将参考帧的位姿和所有共同观测点作为优化变量加入优化函数中，我们就得到了完整的bundle adjustment，可以同时优化两个姿态和三维空间点。
对基于PNP解析方法比较感兴趣的同学可以参考：
涂金戈同学的博客：https://www.cnblogs.com/JingeTU/p/10646322.html ；
或者参考高翔博士的《视觉SLAM十四讲》。
--------------------------------------------------------------
稍微加个插曲：
在优化算法上，笔者根据自己的理解写出部分优化算法的优缺点（这也是视觉SLAM十四讲中的课后思考题）：
实际上，由于梯度下降法过于贪心，通常会导致锯齿状下降，导致收敛速度缓慢；
而牛顿法尽管使用了二阶近似，提高了下降速度，但对于SLAM问题而言，求解海塞阵的计算量太大，非常不适用；
基于牛顿法改进的高斯牛顿法，通过二次近似二阶泰勒展开，利用雅克比的二次方 $J^{T}J$ 来近似海塞阵，在计算量和下降速度上可以较好满足SLAM的需求，但存在的隐患是增量的范围超出泰勒展开时限定的微小邻域，导致近似失败；
列文伯格-马夸尔特法是基于高斯牛顿法进一步改进的，通过限定搜索区域，防止出现近似失败的情况，LM方法也是SLAM中最常用的优化方法。
基于参考帧的跟踪
当没有运动模型时，ORBSLAM2采用跟踪参考帧的方式估计相机的相对运动。
在ORBSLAM2中，参考关键帧的大部分特征点在地图中都有对应三维点。输入当前帧后，通过上一讲所介绍的特征匹配后，我们确定了当前帧和参考关键帧之间的二维特征匹配关系。已知参考关键帧特征点对应的地图三维点，我们很容易就可以得到匹配的三维空间点与当前帧特征点的对应关系。即我们有多对匹配的3D-2D点，于是我们很容易想到利用前面我们介绍的PNP方法来优化位姿。
在前面我们提到过，ORBSLAM2并没有用解析解的方法求解一个相机位姿作为初值。在基于参考帧的跟踪中，ORBSLAM2用上一帧的相机姿态作为初值进行迭代优化。在优化阶段，有一些细节需要稍微注意一下：
1. 初始化阶段，设置当前帧相机位姿作为节点。空间三维点都只作为观测信息，在此阶段不对空间三维点进行优化；
2. 优化过程循环了4次，前两次优化调用了鲁棒核，防止误差值太过发散，更主要的原因是抑制错误匹配对的影响。后两次关闭了鲁棒核，认为前两次的优化基本上抑制了错误匹配的影响。每次优化迭代诗词；
3. 每次优化结束后，根据预设误差阈值判断每一个特征点是内点还是外点。
优化结束后，根据判断条件，将所有匹配的外点全部删除，统计内点（观测次数大于0）数量。若内点数量大于10，则认为跟踪成功，否则跟踪失败。
基于匀速运动模型的跟踪
ORBSLAM2中每次跟踪成功后，都会将两帧间的相对运动记录下来作为运动模型。在估计下一帧运动时，将前一帧的姿态，乘上这个运动模型，就得到了当前帧的姿态初值。能用公式的地方，我们坚决不含糊，毕竟数学公式是没有歧义性的，因此先上公式：
假设前一帧的相机姿态是 $T_{lw}$ ，表示从世界坐标系到前一帧相机坐标系的相对变换。运动模型为 $T_{cl}$ ，表示从前一帧到当前帧的相对变换。则当前帧的相机姿态初值可以描述为：
\begin{equation}
T_{cw} = T_{cl}T_{lw}
\end{equation}
确定了姿态初值，我们还需要查找匹配对。由于我们根据前一帧的相机姿态加上运动模型得到了当前帧的姿态，所以我们在需要在前一帧中查找匹配对。
查找匹配对的方法非常有意思，有很值得大家借鉴：
1. 从相机姿态中取出前一帧和当前帧对应的旋转和位移；
2. 逐个取出前一帧特征点 $f_{i}$ 对应的地图点 $pM_{i}$，并将其投影到当前帧的像平面中；
3. 根据设置的搜索区域阈值，确定搜索窗口半径 $r$；
4. 获取当前帧中处于搜索窗口区域的所有候选特征点 $\left\{ f_{c}^{1}, f_{c}^{2}, \dots, f_{c}^{m}\right\}$；
5. 将前一帧的特征点 $f_{i}$ 与上述所有候选特征点进行匹配，并根据阈值条件筛选最优匹配；
6. 同样统计匹配对的特征主方向角度差值，取数量最多的前三个角度区间对应的匹配对作为最终匹配对；
7. 若匹配对数量低于20，扩大搜索区域，重复步骤4-6，若仍然小于20，则跟踪失败。
除了上述姿态初值的选择方式和匹配对的选择方式不同之外，两种跟踪方法在优化相机位姿时采用的步骤是一致的。因此，优化的方法就不再赘述了。（后续笔者会实现这部分功能，届时会将源码公布在github上，并在此提供源码链接）
总结：
本文主要介绍了ORBSLAM2中的跟踪方案：
涉及的运动估计方法主要是PNP，因此我们详细介绍了利用非线性优化的PNP方法；
此外，我们还分析了两种跟踪方案的不同特点，大家可以根据实际需要选择跟踪方案，亦或是像ORBSLAM2一样，二者结合。
另外，由于本文中已经涉及了PNP的非线性优化方法，局部优化我们就不单独写一讲了，详细的优化内容在本文中查看。之后介绍闭环时，我们也会细致地分析相关优化内容。
局部优化：我们通过扩大关键帧的数目，将当前帧关联的所有关键帧以及对应的所有观测点全部包含进来优化问题中，构成了更大的优化问题。利用同样的优化策略，我们就实现了局部的优化，防止相机姿态随着时间变化而漂移。
下一讲，笔者将为大家介绍前端中一个很重要的模块：关键帧的筛选和插入。
参考文献：
[1] 视觉SLAM十四讲
[2] 机器人学中的状态估计
PS：
如果您觉得我的博客对您有所帮助，欢迎关注我的博客。此外，欢迎转载我的文章，但请注明出处链接。
对本文有任何问题可以在留言区进行评论，也可以在泡泡机器人论坛：http://paopaorobot.org/bbs/index.php?c=cate&fid=1中的SLAM技术交流模块发帖提问。
我的github链接是：https://github.com/yepeichu123/orbslam2_learn。
