SVO源码解读(三):运动跟踪—Feature Alignment

为什么要做这一步?

通过上一步sparse model-based image alignment我们已经能够估计位姿了,但是这个位姿肯定不是完美的。导致重投影预测的特征点在IkIk中的位置并不和真正的吻合,也就是还会有残差的存在。如下图所示,图中灰色的特征块为真实位置,蓝色特征块为预测位置。

怎样做这一步?

从上图中可以看出,虽然真实位置和预测位置有偏差,但是偏差不大,可以构造残差目标函数,和上面直接法类似,不过优化变量不再是相机位姿,而是像素的位置(u′,v′),通过迭代对特征块的预测位置进行优化。这就是svo中提到的Feature Alignment。

已知:利用初始估计的粗略位姿

目标:上一帧特征块在当前帧的投影位置(u',v'),寻找更多地图点(3D)在当前帧投影点(2D)的对应关系。

通过地图我们保存了很多三维空间点,很明显,每一个new frame都是可能看到地图中的某些点的。由于new frame的位姿通过上一步的直接法已经计算出来了,按理来说这些被看到的地图上的点可以被投影到这个new frame中,即图中的蓝色方框块。上图中分析了,所有位姿误差导致这个方框块在new frame中肯定不是真正的特征块所处的位置。所以需要Feature Alignment来找到地图中特征块在new frame中应该出现的位置,根据这个位置误差为进一步的优化做准备。基于光度不变性假设,特征块在以前参考帧中的亮度应该和new frame中的亮度差不多。所以可以重新构造一个残差,对特征预测位置进行优化:

注意这里的优化变量是像素位置,这过程就是光流法跟踪嘛。并且注意,光度误差的前一部分是当前图像中的亮度值,后一部分不是Ik−1Ik−1而是IrIr,即它是根据投影的3d点追溯到的这个3d点所在的key frame中的像素值,而不是相邻帧。由于是特征块对比并且3d点所在的KF可能离当前帧new frame比较远,所以光度误差和前面不一样的是还加了一个仿射变换,需要对KF帧中的特征块进行旋转拉伸之类仿射变换后才能和当前帧的特征块对比。

这时候的迭代量计算方程和之前是一样的,只不过雅克比矩阵变了,这里的雅克比矩阵很好计算:

下面进入代码环节

由于这部分代码分布在多个文件中,比较杂乱,所以我们不放出具体的代码了,重点分析程序中的流程。

先看下与这个功能相关的文件包括哪些:

  • depth_filter.h/.cpp 像素深度估计(基于概率)
  • feature_alignment.h/.cpp 特征匹配
  • frame_handler_base.h/.cpp 视觉前端基础类
  • frame_handler_mono.h/.cpp 视觉前端原理
  • matcher.h/.cpp 重投影匹配与极线搜索
  • point.h/.cpp 3D点的定义
  • reprojector.h/.cpp 重投影

下面直接给出该功能的流程:

1)使用创建关键帧时生成的5个keyPoints投影进行寻找,当前帧与Map中存储的关键帧有共视的关键帧,取前10个共视最好的关键帧

注意,为了使feature对齐更准确(因为关键帧中的point都是比较准确的),使用的<当前帧>和<与当前有共视关系的关键帧>,调整的是当前帧feature(初始位置:由共视关键帧对应的point投影得到)的位置

2)遍历每一个栅格(grid_size=30)中每一个地图点,grid_.cells中的每一个cell包括两个成员变量:point(由关键帧观测到)、point在当前帧上的投影,注意这里不含feature,因为这一步只是寻找point而已,feature要在下一步通过getCloseViewObs()确定

3)针对每一个地图点,通过优化寻找直接法匹配,即使用findMatchDirect()

  • 使用getCloseViewObs(),选出夹角最小的那个关键帧作为参考关键帧,以及对应的feature: ref_ftr_
  • 计算从参考关键帧到当前帧的仿射变换,这是因为如果帧间图像发生了旋转,我们还用同样的窗口,将出错,这等价于给fast(其实是图像块)加上旋转不变性
  • 确定search_level_,这是因为当前帧很大可能与参考关键帧的距离比较远,所以feature的大小很有可能发生变化,这就等于加上了尺度不变形

4)仿射变换矩阵是A_ref_cur,表示从当前帧的<0层>转换到参考关键帧的<0层>的仿射变换,warpAffine()用于将在当前帧的search_level_层上取到的10x10的图像块,投影到feature<提取层>对应的参考关键帧ref_ftr_->frame->img_pyr_[ref_ftr_->level]上,然后就可以使用align2D()或者align1D(edgelet特征)进行优化。

5)根据上一步的结果筛选point

6)如果findMatchDirect()匹配成功了,则向当前帧添加feature,通过下面代码生成新的feature,使用的层是search_level_search_level_是针对每一个point在findMatchDirect()确定

参考资料: