Semi-Direct Viusal Odometry 学习笔记

引言

作为一个SLAM方向的研究人员或者从业人员，不管是学术研究还是工程实现，都需要对SLAM算法中每个艺术般的操作了然于心。在致力于掌握主流SLAM方法的设计灵魂之后，才能产生新的突破，进一步推动SLAM技术走向现实生活，成为人们生活中不可或缺的一部分。
本文将对所学习的视觉里程计SVO系统进行整理，主要参考SVO详细解读.SVO原理解析.一些大佬们走过的路，我们致以敬意。同时，也向在SLAM领域中前赴后继的无数同志们致以敬意。
SLAM算法的学习与设计，首先是数理基础的延伸，其次是对SLAM问题模型的理解与刻画，再其次，是对人类生活中的应用场景的深刻思考。

SVO是一种较适用于无人机的视觉里程计，最主要的特点就是快，其整体框架也较为明朗，适合SLAM学习者们上手。

流程图

个人理解，SVO的思想，类似于十大机器学习算法之一的EM算法的思想，通过迭代对参数求取最大似然估计，使得参数逐渐收敛到最优值。当然，在这种思想下指导的算法，都对初始值有着较高的要求。具体地，在相机相对位姿变换、特征位置、空间点位置都存在不确定性的情况下，我们基于三者的模型，首先对位姿变换进行优化，然后对匹配特征的位置进行优化，然后再反过来优化位姿变换，然后再去优化先前假设固定的空间点位置。
具体细节，看下文分析。

跟踪

目前主流的划分是直接法和特征点法就是根据在跟踪过程中所使用的特征来决定的。两种方法都有各自的优势和适用范围，都对环境条件具有一定的假设。当然，如果SLAM的目标只是能够精确的对相机位姿进行跟踪的话，那么这两种方法都是很优秀的方法，但是SLAM的终极目标远不止如此。

1. 初始化

图像刚进来的时候，获取其金字塔图像，5层，比例为2.

然后，处理第一张图像，processFirstFrame()。先检测FAST特征点和边缘特征，如果图像中间的特征点数量超过50个，就把这张图像作为第一个关键帧。

然后处理第一张之后的连续图像，proecssSecondFrame()，用于跟第一张进行三角初始化。从第一张图像开始，就用光流法持续跟踪特征点，把特征像素点转换成在相机坐标系下的深度归一化的点，并进行畸变校正，再让模变为1，映射到单位球上面。

如果匹配点的数量大于阈值，并且视差的中位数大于阈值。如果视差的方差大的话，选择计算E矩阵，如果视差的方差小的话，选择计算H矩阵。如果计算完H或E后，还有足够的内点，就认为这帧是合适的用来三角化的帧。根据H或者E恢复出来的位姿和地图点，进行尺度变换，把深度的中值调为1.

然后把这一帧，作为关键帧，送入到深度滤波器中，（就是送到的深度滤波器的updateSeedsLoop()线程中。深度滤波器来给种子点在极线上搜索匹配点，更新种子点，种子点收敛出新的候选地图点。如果是关键帧的话，就初始化出新的种子点，在这帧图像里面每层的每个25x25大小的网格里，取一个最大的fast点。在第0层图像上，找出canny边缘点。）

之后就是正常的跟踪processFrame()。

1.2 基于稀疏点亮度的位姿预估
把上一帧的位姿作为当前帧的初始位姿。

把上一帧作为参考帧。

先创建n行16列的矩阵ref_patch_cache_，n表示参考帧上的特征点的个数，16代表要取的图块的像素数量。(这里有个问题就是，单纯的做像素的对比在帧间差异较小的话是很有可能成立的)，再创建6行n*16列的矩阵jacobian_cache_，即每个像素点误差对相机位姿的雅克比。

要优化的是参考帧相对于当前帧的位姿。

把参考帧的所有图块结合地图点，往当前帧图像的金字塔图像上投影。在当前帧的金字塔图像上，从最高层开始，一层层往低层算。每次继承前一次的优化结果。如果前一次的误差相比于前前次没有减小的话，就继承前前次的优化后的位姿。每层的优化，迭代30次。

要优化的残差是，参考帧$I_{k-1}$的特征点的图块与投影到当前帧$I_k$上的位置上的图块的亮度残差。投影位置是，参考帧的特征点延伸到三维空间中到与对应的地图点深度一样的位置，然后投影到当前帧。保证延伸出来的空间点肯定也在与特征点以及光心在一条直线上。但是这里注意到，首先，参考帧上图块的位置是确定的，但是深度具有不确定性，投影到当前帧的位置也具有不确定性。

SVO的一个创新点是，以当前帧上的投影点的像素值为基准，通过优化调整参考帧投影过来的像素点的位置，以此来优化这两者像素值残差。注意，这里的优化对象是参考帧相对当前帧的位姿。以前的做法是反向的。残差用公式表示为
$$\sigma I(u_i)=I_k(\pi ({\hat{T}}_{k,k-1}{p}_{k-1}))-I_{k-1}(\pi (T_{k-1,k-1}{p}_{k-1}))$$
假设给参考帧的相机位姿增加一个扰动
$$T_{k,(k-1{)}^{\prime }}=T_{k,k-1}{T}_{k-1,(k-1{)}^{\prime }}=T_{k,k-1}T(\Psi )$$
则残差与扰动的关系可以表达为
$$\sigma I(\Psi ,u_i)=I_k(\pi ({\hat{T}}_{k,k-1}{p}_{k-1}))-I_{k-1}(\pi (T_{(k-1{)}^{\prime },k-1}{p}_{k-1}))=I_k(\pi ({\hat{T}}_{k,k-1}{p}_{k-1}))-I_{k-1}(\pi (T(\Psi {)}^{-1}{p}_{k-1}))$$
令$T(\xi )=T(\Psi {)}^{-1}$,以方便计算导数。所以原式转化为
$$\sigma I(\xi ,u_i)=I_k(\pi ({\hat{T}}_{k,k-1}{p}_{k-1}))-I_{k-1}(\pi (T(\xi )p_{k-1}))$$
雅克比计算如下
$$J=\frac{\partial \sigma I(\xi ,u_i)}{\partial \xi }=-\frac{\partial I_{k-1}(a)}{\partial a}{|}_{a=u_i}\cdot \frac{\partial \pi (b)}{\partial b}{|}_{b=p_i}\cdot \frac{\partial T(\xi )}{\partial \xi }|\xi =0\cdot p_{k-1}=-\frac{\partial I}{\partial u}\cdot \frac{\partial u}{\partial b}\cdot \frac{\partial b}{\partial \xi }$$
对于上一帧的每个特征点，都进行这样的计算，在自己本来的层数上，取那个特征点左上角的$4\times 4$图块。如果特征点映射回原来的层数时，坐标不是整数，就进行插值。

最后，得到一个巨大的雅克比矩阵，以及残差矩阵，用高斯牛顿法计算扰动$\xi$。
$$\xi =-H^{-1}{J}^T\sigma I$$
然后，得到$T(\xi )$，逆矩阵得到$T(\Psi )$,再更新出$T_{k,(k-1{)}^{\prime }}$。然后，在新的位置上，再从像素点坐标，投影出新的点$p_{k-1}$

每一层迭代30次，因为这种inverse-compositional方法，用这种近似的思想，雅克比就可以不用再重新计算了。这样逐层下去，重复之间的步骤。
对于每个图块的每个像素，其雅克比计算如下
$$\frac{\partial I}{\partial u}=[dx,dy]$$
$\frac{\partial u}{\partial b}$的计算如下
$$\begin{gathered} u=\pi (b)=K\left[\begin{array}{c}\frac{b_x}{b_z}\\ \frac{b_y}{b_z}\\ \frac{b_z}{b_z}\end{array}\right]=\left[\begin{array}{ccc}{f}_x& 0& u_0\\ 0& f_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}\frac{b_x}{b_z}\\ \frac{b_y}{b_z}\\ 1\end{array}\right]=\left[\begin{array}{c}{f}_x{b}_x/b_z+u_0\\ f_y{b}_y/b_z+v_0\\ 1\end{array}\right]\to \\ \frac{\partial u}{\partial b}=\left[\begin{array}{ccc}{f}_x/b_z& 0& -f_x{b}_x/b_z^2\\ 0& f_y/b_z& -f_y{b}_y/b_z^2\end{array}\right] \\ \frac{\partial b}{\partial \xi }=\frac{\partial T(\xi )p}{\xi }=\frac{\partial (\sigma {\varphi }^{\wedge }p+\sigma \rho )}{\partial [\delta \rho ,\delta \varphi ]}=\left[\begin{array}{cc}I& -p^{\wedge }\end{array}\right]=\left[\begin{array}{cccccc}1& 0& 0& 0& p_z& -p_y\\ 0& 1& 0& -p_z& 0& p_x\\ 0& 0& 1& p_y& -p_x& 0\end{array}\right] \end{gathered}$$
为了方便计算,虽然$b=T(\xi )p$，但因为$T(\xi )$是一个很小的扰动，所以可以认为$b=p,\frac{\partial u}{\partial b}\approx \frac{\partial u}{\partial p}$,故有
$$\frac{\partial u}{\partial b}\frac{\partial b}{\partial \xi }=\left[\begin{array}{ccc}{f}_x/p_z& 0& -f_x{p}_x/p_z^2\\ 0& f_y/p_z& -f_y{p}_y/p_z^2\end{array}\right]\left[\begin{array}{cccccc}1& 0& 0& 0& p_z& -p_y\\ 0& 1& 0& -p_z& 0& p_x\\ 0& 0& 1& p_y& -p_x& 0\end{array}\right]$$
对于每个特征点，根据像素位置计算出$\frac{\partial I}{\partial u}$，再根据反投影出来的空间点位置$p$，根据特征点所在的层数算出$f$，然后，相乘，就得到了一行的雅克比矩阵，然后进行位姿的更新与优化。
最后，当前帧的位姿计算为
$$T_{k,w}=T_{k,k-1}{T}_{k-1,w}$$

1.3 基于图块的特征点匹配
因为当前帧有了预估的位姿，对于关键帧链表里面的那些关键帧，把它们图像上的分散的5点往当前帧上投影，看是否能投影成功，如果能够投影成功，就认为共视。再把所有的共视关键帧，按照与当前帧的距离远近来排序。然后，按照关键帧距离从近到远的顺序，依次把这些关键帧上面的特征点对应的地图点都往当前帧上面投影，同一个地图点只被投影一次。如果地图点在当前帧上的投影位置，能取到$8\times 8$的图块，就把这个地图点存入到当前帧投影位置的网格中。

再把候选地图点都往当前帧上投影，如果在当前帧上的投影位置，能取到8x8的图块，就把这个候选地图点存入到当前帧投影位置的网格中。如果一个候选点有10帧投影不成功，就把这个候选点删除掉。

然后，对于每一个网格，把其中对应的地图点，按照地图点的质量进行排序（TYPE_GOOD> TYPE_UNKNOWN> TYPE_CANDIDATE> TYPE_DELETED）。如果是TYPE_DELETED，则在网格中把它删除掉。

遍历网格中的每个地图点，找到这个地图点被观察到的所有的关键帧。获取那些关键帧光心与这个地图点连线，与，地图点与当前帧光心连线，的夹角。选出夹角最小的那个关键帧作为参考帧，以及对应的特征点。（注意，这里的这种选夹角的情况，是只适合无人机那样的视角一直朝下的情况的，应该改成ORBSLAM那样，还要再把视角转换到对应的相机坐标系下，再筛选一遍）。这个对应的特征点，必须要在它自己的对应的层数上，能获取10x10的图块。

然后，计算仿射矩阵。首先，获取地图点在参考帧上的与光心连线的模。然后它的对应的特征点，在它对应的层数上，取右边的第5个像素位置和下边的第5个像素位置，再映射到第0层。再转换到单位球上，再映射到三维空间中，直到与地图点的模一样的长度。把对应的特征点也映射到三维空间中，直到与地图点的模一样的长度。然后，再把这3个点映射到当前帧的（有畸变的）图像上。根据它们与中心投影点的位置变换，算出了仿射矩阵A_cur_ref。A_cur_ref.col(0) = (px_du - px_cur)/halfpatch_size; A_cur_ref.col(1) = (px_dv - px_cur)/halfpatch_size;。（www.cnblogs.com/ilekoaiq）仿射矩阵A，就是把参考帧上的图块在它自己对应的层数上，转换到当前帧的第0层上。（这种把比例变换转换成矩阵表示的方法，很好）。

然后，计算在当前帧的目标搜索层数。通过计算仿射矩阵A_cur_ref的行列式，其实就是面积放大率。如果面积放大率超过3，就往上一层，面积放大率变为原来的四分之一。知道面积放大率不再大于3，或者到最高层。就得到了目标要搜索的层数。

然后，计算仿射矩阵的逆仿射矩阵A_ref_cur。然后，这样子，如果以投影点为中心（5,5），取10x10的图块，则图块上每个像素点的（相对中心点的）位置，都可以通过逆仿射矩阵，得到对应的参考帧上的对应层数图像上的（相对中心点的）像素位置。进行像素插值。就是，把参考帧上的特征点附近取一些像素点过来，可以组成，映射到当前帧上的对应层数的投影点位置的附近，这些映射到的位置刚好组成10x10的图块。

然后，从映射过来的10x10的图块中取出8x8的图块，作为参考图块。对这个图块的位置进行优化调整，使得它与目标位置的图块最匹配。残差表达式为。
$$\delta I=I_{cur}(u^{\prime })-I_r(u)-m$$
其中，$\delta I$为一个像素点对应的残差，$u^{\prime }$表示在这个像素点对应的当前图像上的对应图块的位置，$u$表示这个像素点在参考图块上的位置，$m$表示两个图块的均值差。

在这里，SVO有两个创新点。

第一个创新的地方是。因为一般情况下，是基于自己图块不变，通过优化使得投影位置的图块跟自己最接近。而SVO是投影位置的图块不变，通过优化使得自己图块与投影位置的图块最接近。这样的话，就可以避免重复计算投影位置图块像素关于位置的雅克比了。因为自己图块是固定的，所以雅克比是固定的，所以只需要计算一次。注意这里是对图块上的像素点进行计算，因此图块固定，雅可比就是固定的其实，这个创新点与1.2中的反向创新点一样，都是用近似优化的方法来。因为，如果是一般的方法的话，计算目标投影位置的图块的雅克比，是知道自己参考图块重新移动后，会遇到怎样的目标图块。而，这个反向的方法，并不知道重新移动后会遇到怎样的图块，只知道移动后，对当前的目标图块可以匹配得更好。也是一种迭代，近似优化的方法，但速度可以块很多，避免了重复计算雅克比。

这里的反向构造的方法，具体的理解就是，首先计算在参考帧上像素的梯度，构建雅克比矩阵，然后求解出需要移动的变量，然后再把这个变量通过反仿射变换投影到当前帧上，形成当前帧上图块的移动量，然后对$u^{\prime }$进行移动。接下来再进行迭代。

第二个创新的地方是。一般情况下，两图块的均值差$m$，都是直接把两个图块的均值相减的。但是，这样子的话，可能容易受某些极端噪声的影响。所以，SVO中，直接把也作为优化变量了。

雅可比计算如下
$$J=\frac{\partial \delta I}{\partial [u,m]}=[\frac{\partial I_r(u)}{u},1]$$
接下来对图块上所有像素点进行这样的操作，然后用高斯牛顿法进行迭代。而，如果是对于那些边缘上的点。则只在梯度方向上进行调整，只调整这1个维度，即梯度方向上的长度。右左下上像素的变化，映射到梯度方向上，得到在梯度方向上的像素变化。最后优化完后，再从这个维度上映射出横纵坐标。最后，也得到最佳匹配点的位置。

1.4 进一步优化位姿
然后，对于1.3中得到的当前帧的新的特征点$u^{\prime }$,如果它指向的是地图点$p$的话，通过优化当前帧的相机位姿$T_{k,w}$使得地图点在对应的层数上的预测投影位置和最佳位置的残差$\delta$最小
$$\delta =u^{\prime }-\pi (T(\xi )T_{k,w}p)$$
然后计算其对应的雅可比矩阵。然后用高斯牛顿法进行优化。优化结束后，对优化后的位姿计算协方差。

1.5 优化地图点
针对每个地图点，优化地图点的三维位置，使得它在被它观察到的每个关键帧上的重投影误差最小。每个地图点优化5次，如果这次优化的误差平方和小于上次优化的误差平方和，就接受这次的优化结果。
重投影误差为
$$\begin{gathered} \delta =u^{\prime }-\pi (T_{k,w}(p+\sigma )) \\ J=\frac{\partial \delta }{\partial \sigma }=-\frac{\partial (\pi (b))}{\partial \sigma }=-\frac{\partial (\pi (b))}{\partial b}\frac{\partial b}{\partial sigma}=-\frac{\partial (\pi (b))}{\partial b}\frac{\partial (T_{k,w}p+T_{k,w}\sigma )}{\partial \sigma } \\ =-\frac{\partial (\pi (b))}{\partial b}\frac{\partial (T_{k,w}p+T_{k,w}\sigma )}{\partial \sigma }=-\frac{\partial (\pi (b))}{\partial b}{R}_{k,w} \end{gathered}$$

然后用高斯牛顿进行优化地图点。

后面对地图点的创建过程以及不确定性建模进行分析。