UIE
UIE
泡芙3级
泡芙3级
  • UID14
  • 粉丝3
  • 关注0
  • 发帖数25
阅读:65回复:0

位姿估计 | SVO 论文与代码分析分讲

楼主#
更多 发布于:2019-05-31 11:07
位姿估计
代码主框架在 FrameHandlerMono::processFrame()。
稀疏图像对齐(直接法)
代码主要在 SparseImgAlign。

图片:1.jpg



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

图片:22.png



其中,

图片:33.jpg





  • 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。

图片:44.jpg



通过上一步的帧间匹配能够得到当前帧相机的位姿,但是这种 frame to frame 估计位姿的方式不可避免的会带来累计误差从而导致漂移。
稀疏图像对齐之后使用 特征对齐,即通过地图向当前帧投影,并使用 逆向组合光流 以稀疏图像对齐的结果为初始值,完成基于 patch 的特征匹配,得到更精确的特征位置。
基于光度不变性假设,特征块在以前参考帧中的亮度应该和 new frame 中的亮度差不多,所以重新构造一个残差,对特征预测位置进行优化(代码主要在Matcher::findMatchDirect):

图片:55.png





  • 优化变量是 像素位置
  • 光度误差的前一部分是当前图像中的亮度值;后一部分不是

    图片:11.png

     而不是

    图片:1111.png

     ,即它是根据投影的 3D 点追溯到其所在的 KF 中的像素值
  • 选取了 8x8 的 patch ,由于是特征块对比并且 3D 点所在的 KF 可能离当前帧 new frame 比较远,所以光度误差还加了一个仿射变换

    图片:1112.png

    ,可以得到亚像素级别的精度
  • 对于每个特征点单独考虑,找到和当前帧视角最接近的共视关键帧getCloseViewObs(这个关键帧和当前帧的视角差别越小,patch的形变越小,越可以更精准地匹配)

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


位姿和结构优化(特征点法)
代码主要在pose_optimizer::optimizeGaussNewton和FrameHandlerBase::optimizeStructure

图片:66.jpg



motion-only BA

图片:77.png





  • 优化变量是 相机位姿

structure-only BA
  • 优化变量是 三维点坐标

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

重定位
代码主要在 FrameHandlerMono::relocalizeFrame。重
定位效果一般,有待改进。
游客

返回顶部