SVO 论文与代码分析总结


[TOC]

概述

SVO: Semi-direct Monocular Visual Odometry

  • 论文: SVO: Fast Semi-Direct Monocular Visual Odometry
  • 代码(注释版):cggos/svo_cg

SVO结合了直接法和特征点法,称为 半直接单目视觉里程计

初始化

获取第一关键帧和第二关键帧的相对位姿,并建立初始地图,代码主要在 initialization.cpp 中。

(1)处理第一帧 FrameHandlerMono::processFirstFrame

  • 第一帧位姿为单位阵 new_frame_->T_f_w_ = SE3(Matrix3d::Identity(), Vector3d::Zero())
  • addFirstFrame
    • detectFeatures: 获取 Shi-Tomas得分较高 且 均匀分布 的 FAST角点,并创建 Features
    • 初始化 px_vec(Coordinates in pixels on pyramid level 0) 和 f_vec(Unit-bearing vector of the feature)
  • 将当前帧设置为关键帧,并检测5个对应的关键点
  • 添加关键帧到地图 map_.addKeyframe(new_frame_);

(2)处理第二帧 FrameHandlerMono::processSecondFrame

  • addSecondFrame
    • 光流跟踪trackKlt
    • 计算 单应性矩阵(假设局部平面场景),估计相对位姿T_cur_from_ref_
    • 创建初始地图
  • BA优化 ba::twoViewBA
  • 添加关键帧到地图 map_.addKeyframe(new_frame_);
  • 添加关键帧到深度滤波器 depth_filter_->addKeyframe

位姿估计

代码主框架在 FrameHandlerMono::processFrame()

稀疏图像对齐(直接法)

代码主要在 SparseImgAlign

使用 稀疏直接法 计算两帧之间的初始相机位姿T_cur_from_ref:根据last_frame_的特征点 和 last_frame_new_frame_的相对位姿,建立损失函数(两帧之间稀疏的4x4的patch的光度误差)SparseImgAlign::computeResiduals,使用G-N优化算法获得两帧之间的位姿变换,没有特征匹配过程,效率较高。

\[T_{k,k-1} = \arg \min_{T_{k,k-1}} \frac{1}{2} \sum_{i} \| \delta I(T_{k,k-1}, u_i) \|^2\]

其中,

\[\delta I(T, u) = I_k(\pi(T \cdot {\pi}^{-1} (u,d_u))) - I_{k-1}(u)\]
  • patch选取了4x4的大小,忽略patch的变形,并且没有做仿射变换(affine warp),加快了计算速度
  • 在几个图像金字塔层之间迭代优化,从金字塔的最顶层(默认是第五层klt_max_level)逐层计算(默认计算到第3层klt_min_level
  • 广泛使用指针,比直接取值速度更快
  • 逆向组合(inverse compositional)算法,预先计算雅克比矩阵precomputeReferencePatches,节省计算量
  • 几处双线性插值方法对提高精度有帮助

最后,cur_frame_->T_f_w_ = T_cur_from_ref * ref_frame_->T_f_w_;

特征对齐(光流法)

代码入口在 Reprojector::reprojectMap

通过上一步的帧间匹配能够得到当前帧相机的位姿,但是这种frame to frame估计位姿的方式不可避免的会带来累计误差从而导致漂移。

稀疏图像对齐之后使用 特征对齐,即通过地图向当前帧投影,并使用 逆向组合光流 以稀疏图像对齐的结果为初始值,完成基于patch的特征匹配,得到更精确的特征位置。

基于 光度不变性假设,特征块在以前参考帧中的亮度应该和new frame中的亮度差不多,所以重新构造一个残差,对特征预测位置进行优化(代码主要在 Matcher::findMatchDirect):

\[u_i' = \arg \min_{u_i'} \frac{1}{2} \| I_k(u_i') - A_i \cdot I_r(u_i) \|^2\]
  • 优化变量是 像素位置
  • 光度误差的前一部分是当前图像中的亮度值;后一部分不是 $I_{k-1}$ 而是 $I_r$,即它是根据投影的3D点追溯到其所在的KF中的像素值
  • 选取了8x8的patch,由于是特征块对比并且3D点所在的KF可能离当前帧new frame比较远,所以光度误差还加了一个仿射变换 $A_i$,可以得到亚像素级别的精度
  • 对于每个特征点单独考虑,找到和当前帧视角最接近的共视关键帧getCloseViewObs(这个关键帧和当前帧的视角差别越小,patch的形变越小,越可以更精准地匹配)

该过程通过 inverse compositional Lucas-Kanade algorithm 求解,得到优化后更加准确的特征点预测位置。

位姿和结构优化(特征点法)

代码主要在 pose_optimizer::optimizeGaussNewtonFrameHandlerBase::optimizeStructure

motion-only BA

\[T_{k,w} = \arg \min_{T_{k,w}} \frac{1}{2} \sum_{i} \| u_i - \pi(T_{k,w} \cdot p_i) \|^2\]
  • 优化变量是 相机位姿

structure-only BA

  • 优化变量是 三维点坐标

local BA

  • 附近关键帧和可见的地图点都被优化了,这一小步在fast模式下是不做的

重定位

代码主要在 FrameHandlerMono::relocalizeFrame

重定位效果一般,有待改进。

建图(深度滤波器)

SVO把像素的深度误差模型看做概率分布,使用 高斯——均匀混合分布的逆深度(深度值服从高斯分布,局外点的概率服从Beta分布),称为 深度滤波器Depth Filter,每个特征点作为 种子Seed(深度未收敛的像素点)有一个单独的深度滤波器。

  • 初始化种子:如果进来一个 关键帧,就提取关键帧上的新特征点,初始化深度滤波器,作为种子点放进一个种子队列中
  • 更新种子:如果进来一个 普通帧,就用普通帧的信息,更新所有种子点的概率分布;如果某个种子点的深度分布已经收敛,就把它放到地图中,供追踪线程使用

初始化种子

代码主要在 DepthFilter::initializeSeeds

  • 当前帧上已经有的特征点,占据住网格
  • 多层金字塔FAST特征检测并进行非极大值抑制,映射到第0层网格,每个网格保留Shi-Tomas分数最大的点
  • 对于所有的新的特征点,初始化成种子点Seed
    • 高斯分布均值:mu = 1.0/depth_mean
    • 高斯分布方差:sigma2 = z_range*z_range/36,其中 z_range = 1.0/depth_min

更新种子(深度滤波)

深度滤波 DepthFilter::updateSeeds 主要过程:

  • 极线搜索与三角测量 Matcher::findEpipolarMatchDirect
  • 计算深度不确定度 DepthFilter::computeTau
  • 深度融合,更新种子 DepthFilter::updateSeed
  • 初始化新的地图点,移除种子点

深度估计值(极线搜索)

  • 根据 深度均值mu和深度方差sigma2确定深度范围 $[d_{min},d_{max}]$,计算极线
  • 计算仿射矩阵 warp::getWarpMatrixAffine(帧间图像发生旋转)
  • 极线上搜索匹配:通过RANSAC计算ZMSSD获取最佳匹配点坐标
    • 当计算的极线长度小于两个像素时,直接采用下一步的图像对齐
    • 否则,继续沿极线搜索匹配
  • 光流法亚像素精度提取(图像对齐) feature_alignment::align2D
  • 三角法恢复深度Z depthFromTriangulationTriangulate in SVO

深度估计不确定度

REMODE 对由于特征定位不准导致的三角化深度误差进行了分析

假设 焦距为 $f$,像素扰动为 px_noise = 1.0,角度变化量 px_error_angle 为 $\delta \beta$ 则

\[\tan \frac{\delta \beta}{2} = \frac{1.0}{2f}\]

\[\delta \beta = 2 \arctan \frac{1.0}{2f}\]

\[\beta^{+} = \beta + \delta \beta\] \[\gamma^{+} = \pi - \alpha - \beta^{+}\]

已知 $t$ 为 T_ref_cur.translation(),根据正弦定理

\[Z^{+} = \|t\| \frac{\sin \beta^{+}}{\sin \gamma^{+}}\]

所以,深度不确定度 tau 为 $\tau = Z^{+} - Z$

逆深度不确定度 tau_inverse 为:

double tau_inverse = 0.5 * (1.0/max(0.0000001, z-tau) - 1.0/(z+tau));

逆深度标准差

\[\tau_{inv} = \frac{1}{2} (\frac{1}{Z-\tau} - \frac{1}{Z+\tau})\]

深度融合

代码主要在 void DepthFilter::updateSeed(const float x, const float tau2, Seed* seed)

SVO的融合是不断利用最新时刻深度的观测值,来融合上一时刻深度最优值,直至深度收敛。

通过上面两步得到 逆深度测量值 $x$ 1./z逆深度不确定度 $\tau_{inv}$,则 逆深度 服从 高斯分布

\[N(x, \tau_{inv}^2)\]

SVO 采用 Vogiatzis的论文 Video-based, real-time multi-view stereo 提到的概率模型,使用 高斯–均匀混合分布的深度滤波器,根据 $x$ 和 $\tau_{inv}$ 更新以下四个参数:

  • 逆深度高斯分布的均值 $\mu$ seed->mu
  • 逆深度高斯分布的方差 $\sigma^2$ seed->sigma2
  • Beta分布的 $a$ seed->a
  • Beta分布的 $b$ seed->b

更新过程(Vogiatzis的Supplementary matterial)如下:

其中,论文公式19有误(代码正确),应为

\[\frac{1}{s^2} = \frac{1}{\sigma^2} + \frac{1}{\tau_{inv}^2}\]

最终,得到收敛的逆深度的最佳估计值 seed->mu

新的地图点

如果种子点的方差seed->sigma2,小于深度范围/200的时候,就认为收敛了,它就不再是种子点,而是TYPE_CANDIDATE点。

TYPE_CANDIDATE点被成功观察到1次,就变成TYPE_UNKNOWN点;TYPE_UNKNOWN被成功观察到10次,就变成TYPE_GOOD点;如果多次应该观察而没有被观察到,就变成TYPE_DELETED点。

总结与讨论

  • 特征点提取在地图线程,跟踪使用光流法
  • 后端的特征点只在关键帧上提取,用FAST加金字塔
  • 上一个关键帧的特征点在这一个关键帧上找匹配点的方法,是用极线搜索,寻找亮度差最小的点,最后再用深度滤波器把这个地图点准确地滤出来
  • 重定位比较简单,没有回环
  • 深度滤波器使用的是高斯–均匀混合分布
  • SVO对PTAM的改进主要在两个方面:高效的特征匹配、鲁棒的深度滤波器
  • 在Tracking线程,当前帧的特征点是从上一帧用光流法传递过来的,只有在Mapping线程DepthFilter插入新关键帧时才需要提取特征点,不需要每一帧都提取特征点
  • PTAM和ORB-SLAM只用两帧图像三角化出地图点,只要地图点没有被判定为外点,就固定不变了(除非BA阶段调整);而SVO的深度滤波器会根据多帧图片不断收敛地图点的不确定度,从而得到更可靠的地图点。因为地图点更可靠,所以SVO只需要维护更少的地图点(PTAM一般维护约160到220个特征点,SVO在fast模式下维护约120个地图点),从而加快了计算速度。

参考文献




^