理论二、MSCKF 前端特征跟踪

感谢：紫薯萝卜 https://zhuanlan.zhihu.com/p/76375967

 

VIO算法的核心在后端，不管是优化的方法还是滤波的方法，都是靠后端的紧耦合融合获得较高的VIO精度，为了节省计算量一般前端都会做得很简单，基本都是FAST、Harris等角点加LK光流跟踪（S-MSCKF中使用的FAST角点，VINS_MONO中使用的goodFeatureToTrack）。如果计算资源充裕的话，也可以换成 ORB、SURF、SIFT 等描述子特征，精度应该会有所提升。

这里以S-MSCKF中的双目特征跟踪为例进行介绍，单目特征跟踪步骤很类似，可以参照VINS-MONO中的前端实现。

 

1. 特征跟踪流程

S-MSCKF前端主要步骤：

• A. 双目特征跟踪

  • a) 左图前后帧跟踪：对上一帧左图特征点，在当前帧左图进行跟踪，获得当前帧左图的跟踪特征点
  • b) 左右图跟踪：对当前帧左图特征点，在当前帧右图进行跟踪，获得当前帧右图的跟踪特征点
  • c) 左右图前后帧RANSAC：分别对左右图，进行前后帧2-points RANSAC，剔除外点。只保留左右目都通过RANSAC的特征点。

• B. 双目特征新增

  • a) 提取左图特征：在当前帧左图上，构建mask，已有特征附近不提取，在图像剩下区域提取FAST角点，将新的角点投射到网格中，如果网格中特征点满了则删除该新角点。
  • b) 左右图跟踪：对当前帧左图新提取的特征点，在当前帧右图进行跟踪，获得当前帧右图的跟踪特征点

• C. 特征均匀化采样

  • 为了使得特征点均匀分布在图像上，将图像分成了4*5个网格(grid)，每个网格中最多4个特征点，这样避免了特征点集中在图像上的某一小块。跟踪的特征点可能会使得单个网格中的特征点数量超过限制，需要对网格特征点进行修剪，修剪的原则是尽量保留生存时间（特征点被跟踪的帧数）较长的特征点。附加的好处：环境中可能会出现运动的物体，如果此时从运动物体上提取的特征点过多，那么相机的状态估计精度很差，因为运动物体大多是无规律的，特征点均匀化可以减小动态物体对视觉里程计的影响，如果最大可能的去除，解决办法是加深度学习

这里实现时需要注意的细节是左右图的特征点是成对出现的，所以对于那些在右图中跟踪失败的点，在左图特征点会被删除。

特征跟踪流程中的跟踪主要有两种：前后帧跟踪和左右图跟踪，二者都是调用LK光流。区别在于前后帧跟踪用了IMU估计相机旋转来预测特征点初值，左右图跟踪用的相机外参，而且左右图跟踪还加了极线约束，所以左右图匹配准确性更高。

 

2. 前后帧跟踪

• A. IMU旋转估计

收集前后两帧之间所有的IMU数据，计算平均角速度 ，通过外参分别转到左右相机系，再乘以时间得到相对旋转：

• B. 特征点预测

对于前一帧特征点 ，投影到后一帧为 

S为尺度， d 为特征点深度，  未知，直接令 （这样做肯定会有误差，但这只是预测特征点的初始位置，后面会有LK光流来对位置进行refine）因为假设所有特征点都在深度为1的normalize平面，所以可以直接计算Homography， ，从而投影公式简化为

• C. LK光流跟踪

调用opencv中的calcOpticalFlowPyrLK进行跟踪。

 

3. 左右图跟踪

A. 特征点预测

由于特征点深度未知，同样假设特征点在normalize平面，计算 。这里还考虑了图像畸变参数，所以会预测得更准一点。具体实现上就是先调用undistortPoints()转到normalized平面，再乘以旋转矩阵，最后调用distortPoints()投到右相机平面。

B. LK光流跟踪

调用opencv中的calcOpticalFlowPyrLK进行跟踪。

C. 极线约束判断

左右图跟踪中由于 已知，所以可以计算Essential Matrix ，对于左右图特征点先undistort到normalized平面 ，然后根据极线约束 ，将不满足极线约束的特征删除。

 

4.  2-point RANSAC

设前后两帧的特征点匹配对为 ，特征都已经转到了各自相机的normalize平面，则有极线约束

根据叉乘性质对公式进行变换