视觉VIO:S-MSCKF算法学习(一)

1.算法简介

Mingyang Li博士于2007年提出MSCKF (Multi-State Constraint Kalman Filter),一直是filter-based SLAM的经典之作.据说这也是谷歌tango(室内外手机大规模重建)里面的算法。

在传统的EKF-SLAM框架中，特征点的信息会加入到特征向量和协方差矩阵里,这种方法的缺点是特征点的信息会给一个初始深度和初始协方差，如果不正确的话，极容易导致后面不收敛，出现inconsistent的情况。MSCKF维护一个pose的FIFO，按照时间顺序排列，可以称为滑动窗口，一个特征点在滑动窗口的几个位姿都被观察到的话，就会在这几个位姿间建立约束，从而进行KF的更新。如下图所示, 左边代表的是传统EKF SLAM, 红色五角星是old feature,这个也是保存在状态向量中的,另外状态向量中只保存最新的相机姿态; 中间这张可以表示的是keyframe-based SLAM, 它会保存稀疏的关键帧和它们之间相关联的地图点; 最右边这张则可以代表MSCKF的一个基本结构, MSCKF中老的地图点和滑窗之外的相机姿态是被丢弃的, 它只存了滑窗内部的相机姿态和它们共享的地图点.

S-MSCKF是宾大Vijay Kumar实验室开源的双目版本MSCKF算法，Kumar无人机领域应该家喻户晓了，它们开源的S-MSCKF精度挺高，代码质量也很高，非常适合入门学习。

2.算法调试

S-MSCKF是在ROS下运行的package，依赖与ROS，这里假设已经安装了ROS并创建的catkin_ws
首先下载源码并编译，执行

sudo apt-get install libsuitesparse-dev
cd ~/catkin_ws/src
git clone KumarRobotics/msckf_vio
cd ..
catkin_make --pkg msckf_vio --cmake-args -DCMAKE_BUILD_TYPE=Release -DCMAKE_EXPORT_COMPILE_COMMANDS=Yes

注意: catkin_make时一定要加”-DCMAKE_BUILD_TYPE=Release”, 编译时没有添加release,运行后Rviz中只有特征跟踪的图像, 没有地图点或者有一些很乱的地图点,是因为观测更新耗时太长，导致IMU数据丢失。
-DCMAKE_EXPORT_COMPILE_COMMANDS=Yes
是为了在vscode上调试方便

运行算法,执行run.launch:

<?xml version="1.0"?>
><launch>   
    <!--- Run Rviz-->
        <include  file="$(find msckf_vio)/launch/rviz.launch"/>
    <!--- Run msckf_vio_euroc -->
        <include  file="$(find msckf_vio)/launch/msckf_vio_euroc.launch"/>
</launch>

正常算法运行效果图

注意:添加显示轨迹的功能参考:我以前的博文

3.代码解读

3.1 前端 ImageProcessor

ImageProcessor中两个回调函数作为数据入口:

imuCallback：接收IMU数据，将IMU数据存到imu_msg_buffer中，这里并不处理数据。stereoCallback：接收双目图像，进行双目特征跟踪，先光流跟踪上一帧的特征点，然后提取新的特征点。将跟踪到的特征点publish出去，供后端接收使用。

前端中比较关键的操作是双目特征点跟踪trackFeatures()，它分成三步：

1.左图特征点前后帧跟踪
2.得到当前帧左图特征点当前帧左右图跟踪
3.得到当前帧右图特征点左右图分别做前后帧RANSAC剔除外点

前后帧跟踪和左右图跟踪都是用的LK光流，前后帧跟踪会用IMU积分的相对旋转预测特征点在当前帧的位置作为初值(integrateImuData, predictFeatureTracking)；左右图跟踪会用相机外参预测右图特征点位置作为初值。

代码将图像分成了4*5个网格(grid)，每个网格中最多4个特征点，这样能够使特征点均匀分布在图像上。

3.2 后端 Msckf-Vio

msckf_vio中两个回调函数作为数据入口:

• imuCallback：接收IMU数据，将IMU数据存到imu_msg_buffer中，这里只会利用开头200帧IMU数据进行静止初始化，不做其他处理。
• featureCallback：接收双目特征，进行后端处理。利用IMU进行EKF Propagation，利用双目特征进行EKF Update。

静止初始化(initializeGravityAndBias)：将前200帧加速度和角速度求平均, 平均加速度的模值g作为重力加速度, 平均角速度作为陀螺仪的bias, 计算重力向量(0,0,-g)和平均加速度之间的夹角(旋转四元数), 标定初始时刻IMU系与world系之间的夹角. 因此MSCKF要求前200帧IMU是静止不动的

后端代码流程：

• 1.batchImuProcessing: 首先对两帧观测之间的所有IMU数据做预积分
• 2.stateAugmentation: 然后对MSCKF的估计状态进行扩充, 将当前的imu_state加入到状态向量cam_states中, 并扩充6*6的covariance
• 3.matrixaddFeatureObservations: 将特征添加到map_server, 将特征添加到对应feature.id的observations(std::map)中, 并计算跟踪已有特征的比例.
• 4 . removeLostFeatures: 对于那些lost(当前帧未track)的特征, 剔除掉观测小于3个的特征, 如果没有初始化尝试进行初始化, 剔除掉初始化失败的特征. 对于剩下的特征进行观测更新(measurementUpdate), 然后从map_server中移除.也就是说只有跟丢后的特征才会用于观测更新
• 5 . pruneCamStateBuffer: 当state_server中cam_states超过最大个数时, 需要移除冗余的states，也会调用measurementUpdate
  • 6 . findRedundantCamStates:取倒数第四个状态左右key state, 如果倒数第3,2个cam_state与key state之间距离和角度都很小, 则将倒数第3,2个cam_state移除, 否则移除最历史的cam_state。移除2个cam states存在rm_cam_state_ids中。
  • 7.map_server中所有的feature，查找rm_cam_state_ids中的观测，放在involved_cam_state_ids中，当待移除观测为1时，直接将该观测移除，否则，尝试初始化feature，如果初始化失败，则移除所有待删除的观测。
  • 8.构建Jacobian，进行measurementUpdate。

注意:measurementUpdate在两种情况下触发：

特征跟丢了需要移除特征时（removeLostFeatures）
相机状态数量达到最大限制需要剔除掉相机状态时（pruneCamStateBuffer）

代码流程框图

参考:
http://www.xinliang-zhong.vip/msckf_notes/