SVO算法学习总结

简介

SVO: Fast Semi-Direct Monocular Visual Odometry
SVO：半直接法单目视觉里程计

SVO: Semi-Direct Visual Odometry for Monocular and Multi-Camera Systems
SVO: 用于单目和多目的半直接法里程计

SVO算法最初是基于无人机应用提出的一种半直接法视觉里程计。

1.算法流程简介

算法主要分为两个并行的线程：跟踪和建图;
跟踪线程主要分为3步，包括帧间匹配，3D空间点投影匹配和BA优化三个部分，先根据最小光度误差优化相机位姿，再单独优化特征点像素块位置，最后根据新的位置和初始位姿对当前帧位姿进行BA优化。

• 帧间匹配是将上一帧作为参考帧，将上一帧位姿作为初始位姿，通过光流跟踪得到特征点在当前帧的位置，通过对参考帧3D点重投影可以得到上一帧特征点在当前帧的投影坐标，通过优化图像所有特征点的像素块(4x4)的最小光度误差可以得到当前帧的初始位姿。
• 在得到帧间匹配的初始位姿后，会进一步细化当前帧特征的像素坐标。通过将共视关键帧中对应的地图点投影到当前帧图像，通过最小化参考关键帧中的特征点像素块和当前帧特征点像素块的光度误差，可以得到特征点在当前帧的精确坐标
• 然后固定匹配点的坐标，通过最小化地图点在当前帧上的重投影残差获得当前帧位姿。

建图线程主要负责关键帧创建和维护一个深度滤波器，每当跟踪到一帧图像，会选择当前时刻的一些地图点计算地图点在上一参考帧与当前帧的视差，如果视差或者地图点与上一参考关键帧之间的距离大于一定阈值，就将当前帧设置为关键帧。

SVO仅对关键帧提取特征点，并将特征点初始化为深度滤波器种子点，并基于极限搜索和图像块匹配原理估计种子点对应深度，当经过多次观测后种子点深度的概率分布的不确定性如果小于一定阈值则认为收敛，将其判定为candidate点，如果该点被成功观测10次就认为是好的地图点。

2.图像对齐/帧间匹配

1. 主要过程
   （1）找到前一帧（K-1）看到的地图点p1,p2,p3。
   （2）重投影至当前帧（k）二维图像上，得到2D像素坐标
   （3）然后最小化灰度误差函数（优化的是光流跟踪到的像素块和重投影得到的像素块之间的灰度差），得到位姿
   该过程主要是固定匹配点坐标，优化帧间运动T
   

在使用高斯牛顿法迭代优化时，选择每次优化参考帧在当前帧的投影位置，这样就可以每次在求导时在同一位置展开，进而将雅格比矩阵进行固定。

SVO认为相邻帧图像之间的运动较小，所以在计算光度误差时取特征点周围4x4的像素块。

3.特征点对齐

该过程是单独对当前帧特征点坐标的进一步修正，之前得到的位姿是通过光流跟踪得到的，这里是固定优化好的3D点，通过最小化3D点在关键帧中对应像素块和当前帧中对应的像素块进一步细化当前帧中特征点的位置。

3.1 共视关键帧与局部地图点的选择

SVO这里采用了一个5点法选择共视关键帧，就是在关键帧的四个角落和中心位置各选一个特征点，如果这5个特征点任意一个可以投影到当前帧则初步认为这该关键帧与当前帧共视。

然后，按照关键帧距离从近到远的顺序，依次把这些关键帧上面的特征点对应的地图点都往当前帧上面投影，同一个地图点只被投影一次。如果地图点在当前帧上的投影位置，能取到8x8的图块，就把这个地图点存入到当前帧投影位置的网格中。

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

3.2 地图点与关键帧的对应

找到地图点后就该找地图点对应2D特征的像素块了，每个地图点会被多个关键帧观测到，连接地图点和关键帧的光心得到一个方向向量，选择与当前帧方向向量夹角最小的关键帧，然后选择3D点在该关键帧上对应的像素块进行特征点对齐。

3.3 基于逆光流法的最小化光度误差

根据逆光流法采取的策略是每次都在参考图像的同一位置处展开，这样就避免了求导等操作的反复进行。每次优化后会在参考帧上得到特征点u1调整后的特征点坐标u1’，然后将对应当前帧的特征点u2移动相同的变化量，对于每次迭代都通过上述过程不断逼近u2的真实坐标。

4.位姿优化

在得到精确的匹配坐标后，将当前帧特征点坐标固定，然后对相机在当前帧的位姿进行优化，依然是3D重投影光度误差，最终可以得到精确的位姿。
残差函数如下：

在得到新的位姿后会进一步对地图点进行优化，即计算3D地图点对应的重投影误差。

5.地图点提取

SVO只在图像是关键帧时提取关键点，然后通过极线搜索和块匹配在当前帧寻找与参考关键帧匹配特征点坐标。然后基于深度滤波器恢复3D点深度信息，将新创建的3D点作为种子点(种子就是深度未收敛的像素点)

6.基于深度滤波器对种子进行更新

在进行特征点深度时，单次计算出来的深度具有一定的误差，对于同一个特征点的深度需要进行多次测量恢复误差较小的深度值。SVO假定像素点的深度信息服从高斯分布，每来一帧图像都会计算一次特征点对应深度，然后用新的测量不断更新种子点的深度信息，直到种子点深度不确定性收敛。

对于一个种子，其每一次深度滤波（也叫深度融合，利用新的一帧来更新种子的深度值），都行需要经以下过程：

7.逆光流法浅谈

总体上讲，逆光流法的优势主要体现在gauss-newton求解过程中的雅格比和Hessian矩阵计算上，对于常规gauss-newton在迭代过程中是不断对自变量x在目标图像中的位置处进行泰勒展开，因为每次展开点都不一样所以每次迭代都需要重新求导计算雅格比和Hessian矩阵等。根据逆光流法采取的策略是每次都在参考图像的同一位置处展开，这样就避免了求导等操作的反复进行。

7.1 光流法中的最小二乘问题

光流法是根据光度不变性假设（同一空间点在各图像上的成像点的灰度值相同）实现，也就是下面的式子：

这个公式的物理意义是参考图像T中某个点x经过一个坐标变换p得到x’，在当前帧I图像x’坐标处的像素灰度和图像T的x处像素灰度值相同。

式中T(x)表示参考图象中点x的像素灰度值。W表示关于x和p的一个函数，W(x;p)表示x点坐标经过变换（变换可以是纯平移或仿射）后的坐标。p是一个参数向量，用于构建相关的变换。I(.)表示当前图像中某点的像素灰度值。最终通过调整p的取值来使上述公式成立。

式(1)只针对一个点，但在实际中会考虑多个点对p的求解约束。因此一般的光流法求解的是下面这样的一个式子：

这是求出p关于式(2)的最小二乘解的过程，可由迭代的方法解决（如高斯牛顿法）。对于式(2)来说，p和W(.)的更新公式为（这些更新公式不难理解，就是对p和W(.)进行的一种简单叠加）：

迭代求解不可避免地要对原公式进行求导。先对式(2)求在I(W(x;p+△p))处的一阶泰勒展开：

式中的▽I(W)是I图像中W(x;p)像素坐标点处的像素梯度，它与当前像素坐标有关，也就是和p有关。同时W关于p的雅可比矩阵也与p的当前值有关。所以上述方法需要在每次迭代优化后求一次新的导数。之后就是通过构建Hessian矩阵来求出△p增量，完成迭代求解操作。大致步骤如下图：

做一个小小的总结： 可以看到上述在构建好损失函数后就是一个基于高斯牛顿法解最小二乘的过程，基于参考帧处x的坐标，不断地迭代变换p在当前帧寻找新的匹配点x’，在这个不断迭代过程中是在当前帧新的点x’处进行更新的，每次更新都需要计算相应的雅格比矩阵和Hessian矩阵，这一过程浪费了很多计算资源的消耗，逆向光流法就是针对这一现象提出的。

7.2 逆向光流法中的最小二乘问题

逆向光流法也是基于灰度不变性假设实现，但它求解的迭代优化问题不同。它求解的目标是下面这个式子:

与式子(2)相比，式(6)中图象T和I换了个位置。△p的作用对象从图像I变成了图像T（这应该是算法被称为逆向光流的原因吧）。此时，W(.)的更新公式变成了：

下面谈谈我对公式(7)的理解。首先在两张图T、I中标记出正在进行匹配两个点对，将它们分别记为Pt和Pi。由于两幅图像目前估计的仿射变换为W(x;p)，所以我们可以用下图来表示当前的情况：

这时我们在图像T中进行一次微调，也就是对Pt执行W(x;△p)操作，将变换后的点记为Pt’。此时情况如下图所示：

我们假设经过这次△p的更新之后， Pt’ 与Pi之间的灰度误差最小，可认为 Pt’ 就是Pi对应的匹配点。得，弄了半天找了个 Pt’ 的匹配点。不过没有关系，至少我们知道Pi不会是Pt的匹配点。因此要在图像I中继续搜索，即移动Pi的位置，获得新的点Pi’。怎么移呢？我们这样想，既然 Pt’ 是通过△p变换获得的，那么对Pi使用与△p方向相反的变换（从这儿可以看出逆向光流法的限制是需要W(.;△p)是可逆的！）就有可能获得与Pt匹配的点。这一步由式(9)来实现，情况如下图所示：

那么这样的方法有什么好处呢？我们先对式(8)进行一阶泰勒展开，从展开式中就能够看出这个方法的优势：

式中的▽T代表的是W(x;0)点处的像素梯度，它是一个固定的量。W关于p的雅可比矩阵是在(x;0)处求得的（这就说明p对雅可比的计算是没有影响的），也是个固定量。之后就是通过构建Hessian矩阵来求出△p增量，完成迭代求解操作。大致步骤如下图：

从图中可以看出，由于有些量在整个迭代过程中是固定的，所以能够在算法的一开始就计算出来它们的值，且在迭代过程中可以一直使用，不需要更新。这固定的量就是降低整个算法计算量的关键所在。

8.深度滤波专项

8.1 极线搜索与块匹配

下图为极限搜索的示意图。

1.极线搜索
已知参考帧(左图)的某个特征点p1，根据相机几何原理p1对应的3D空间点在连接光心O1和p1的射线上的某一处，假设3D点的深度范围是(d_min, +∞)。

同样的该3D点在相邻的当前帧图像上也存在一个投影点p2,p2也是3D点与当前帧光心O2连线的交点。因为3D点的位置是未知的，所以对于(d_min, +∞)范围内所有3D点可能的位置在当前帧的投影会形成一条极线l2，我们知道p2一定在这条极线上，只需要沿着极线进行搜索就可以找到p1在当前帧的匹配点p2。这个过程就是极限搜索。

2.块匹配
按照直接法的思想，可以通过比较极线l2上每个像素点与p1的灰度值，灰度值之差越小是匹配点的可能性就越大。但是如果极线上有很多和p1相似的点，比较单个像素之间的差异可能会出现很大的误匹配，所以就引入了块匹配的概念。即通过比较特征点邻域内的像素块之间的差异来提高区分度，寻找相应的匹配点p2。

3.块匹配方法
现在取p1周围的小像素块,假设像素块的大小为w×w；并且在极线上也取了很多个小块。不妨把p1周围的小块记成 A ∈ Rw×w,把极线上的n个小块记成Bi, i=1,…,n。

（1）SAD(Sum of Absolute Difference)。顾名思义,即取两个小块的差的绝对值之和

（2）SSD(Sum of Squared Distance),即两个像素块灰度值差异的平方和:

（3）NCC(Normalized Cross Correlation)(归一化互相关)。这种方式计算两个小块的相关性。
分子为A矩阵与B矩阵对应元素相乘的和，分母为A矩阵元素平方和与B矩阵元素平方和的平方跟。

同时可以进行去均值化操作，对AB矩阵分别进行去均值操作再使用上述方法，以应对相邻帧之间光线的明暗变化。

8.2.高斯分布的深度滤波器

现在,我们在极线上,计算了A与每一个Bi的相似性度量。为了方便叙述,假设我们用了NCC,那么,我们将得到一个沿着极线的NCC分布。这个分布的形状严重取决于图像本身的样子。在搜索距离较长的情况下,我们通常会得到一个非凸函数:这个分布存在着许多峰值,然而真实的对应点必定只有一个。在这种情况下,我们会倾向于使用概率分布来描述深度值,而非用某个单一个的数值来描述深度。于是,我们的问题就转到了,在不断对不同图像进行极线搜索时,我们估计的深度分布将发生怎样的变化——这就是所谓的深度滤波器。

1.基于高斯分布的深度滤波器
核心思想是假设特征点的深度值和观测到的深度值(由多帧的三角化得到)均服从高斯概率分布，通过对深度的观测去不断更新特征点深度值的概率分布，当深度的不确定性小于阈值时则认为收敛，此时的深度值即为特征点深度。但是在进行深度估计时要借助于两帧之间的平移，由于单目匹配计算的平移没有真实的尺度信息，因此这时估计出的深度也是没有尺度的。

主要过程：

1. 假设所有像素的深度满足某个初始的高斯分布；
2. 当新数据产生时，通过极线搜索和块匹配确定投影点位置；
3. 根据几何关系计算三角化后的深度以及不确定性；
4. 将当前观测融合进上一次的估计中。若收敛则停止计算，否则返回2。

假设某特征点的深度服从均值为μ方差为σ的高斯分布：P(d) ~ N(μ，σ^2)

当新的数据到来(新的匹配产生)，可以根据几何原理得到观测值，假设观测值同样服从概率分布P(d_obs) ~ N(μ_obs，σ_obs^2)

然后可以使用当前的观测信息更新之前关于特征点的概率分布，根据高斯分布的性质可以进行以下更新：

当融合后的分布不确定性收敛到一定阈值之后，认为此时的估计值为对应特征点的深度。

2.贺一家对深度滤波器的解释
最基本的深度估计就是三角化，这是多视角几何的基础内容。我们知道通过两帧图像的匹配点就可以计算出这一点的深度值，如果有多幅图像，那就能计算出这一点的多个深度值。这就像对同一个状态变量我们进行了多次测量，因此，可以用贝叶斯估计来对多个测量值进行融合，使得估计的不确定性缩小。如下图所示：

一开始深度估计的不确定性较大(浅绿色部分)，通过三角化得到一个深度估计值以后，能够极大的缩小这个不确定性(墨绿色部分)。

8.3 SVO中深度滤波器的使用

以下内容参考自：https://zhuanlan.zhihu.com/p/85190014

在这里，先简单介绍下svo中的三角化计算深度的过程，主要是极线搜索确定匹配点。在参考帧Ir中，我们知道了一个特征的图像位置，假设它的深度值在[d_min,d_max]之间，那么根据这两个端点深度值，我们能够计算出他们在当前帧Ik中的位置，如上图中草绿色圆圈中的线段。

确定了特征出现的极线段位置，就可以进行特征搜索匹配了。如果极线段很短，小于两个像素，那直接使用上面求位姿时提到的Feature Alignment光流法就可以比较准确地预测特征位置。

如果极线段很长，那分两步走，第一步在极线段上间隔采样，对采样的多个特征块一一和参考帧中的特征块匹配，用Zero mean Sum of Squared Differences 方法对各采样特征块评分，哪个得分最高，说明他和参考帧中的特征块最匹配。第二步就是在这个得分最高点附近使用Feature Alignment得到次像素精度的特征点位置。像素点位置确定了，就可以三角化计算深度了。

最后，得到一个新的深度估计值以后，就可以用贝叶斯概率模型对深度值更新。

在深度估计的过程中，除了计算深度值外，这个深度值的不确定性也是需要计算的，它在很多地方都会用到，如极线搜索中确定极线的起始位置和长度，如用贝叶斯概率更新深度的过程中用它来确定更新权重(就像卡尔曼滤波中协方差矩阵扮演的角色)，如判断这个深度点是否收敛了，如果收敛就插入地图等等。

8.4 逆深度

在进行极线搜索时，可能会存在多个极值，为了确定真实深度值，通过使用深度的概率分布描述深度值。

（1）逆深度误差可以降低真实世界中较远的深度点造成的误差。比如对于100m和50m处的不同深度点，可能在图像中表现为几个像素的距离，如果使用深度误差的话是50的差距，但是采用逆深度误差只有1/50-1/100=0.01的差距
（2）当采用逆深度时，通过将坐标归一化可以得到逆深度因子以及特征的齐次表达形式，优化变量减少
（3）逆深度的表达形式能够表达更远的点，而且更接近于高斯分布