msckf_vio代码解读--msckf_vio部分

代码结构

├── CMakeLists.txt
├── LICENSE.txt
├── README.md
├── cmake
│ └── FindSuiteSparse.cmake
├── config
│ ├── camchain-imucam-euroc-noextrinsics.yaml
│ ├── camchain-imucam-euroc.yaml
│ └── camchain-imucam-fla.yaml
├── include
│ └── msckf_vio
│ ├── cam_state.h
│ ├── feature.hpp
│ ├── image_processor.h
│ ├── image_processor_nodelet.h
│ ├── imu_state.h
│ ├── math_utils.hpp
│ ├── msckf_vio.h
│ ├── msckf_vio_nodelet.h
│ └── utils.h
├── launch
│ ├── image_processor_euroc.launch
│ ├── image_processor_fla.launch
│ ├── msckf_vio_euroc.launch
│ ├── msckf_vio_euroc_noextrinsics.launch
│ ├── msckf_vio_fla.launch
│ └── reset.launch
├── msg
│ ├── CameraMeasurement.msg
│ ├── FeatureMeasurement.msg
│ └── TrackingInfo.msg
├── nodelets.xml
├── package.xml
├── rviz
│ ├── rviz_euroc_config.rviz
│ └── rviz_fla_config.rviz
├── src
│ ├── image_processor.cpp
│ ├── image_processor_nodelet.cpp
│ ├── msckf_vio.cpp
│ ├── msckf_vio_nodelet.cpp
│ └── utils.cpp
└── test
├── feature_initialization_test.cpp
└── math_utils_test.cpp
本篇文章主要介绍src msckf_vio部分，也就是论文中关于状态向量的部分。

介绍

看过源码的同学可能已经知道，msckf_vio是目前msckf应用到双目vio中比较成熟的代码。他由两部分组成，分别是image_processor和msckf_vio两部分组成，image_processor 主要负责视觉数据的特征提取，匹配与跟踪，并且并且剔除外点。最后将结果发布到feature这个topic上。

这里的feature topic上的消息实际上对应一个camera state ，所以为了预测Filter State，并且计算卡尔曼增益K以及更新状态，代码中设计了msckf_vio_nodelet.cpp来，msckf_vio这个类就是通过msckf_vio_nodelet.cpp这个nodelet进行初始化。
所以msckf_vio部分是通过MsckfVioNodelet::onInit()这个函数初始化MsckfVio类，然后调用MsckfVio中的initialize()函数。对MsckfVio类进行初始化。
接下来请读者把目光放在msckf_vio.cpp上面。

一 MsckfVIO类的initialize()函数

这个函数被调用时主要发生以下事情：

1. 调用loadParameters() 对一些参数进行初始化，若初始化失败，则程序返回。
2. 1执行完之后，有了关于noise的参数，根据参数对state_server.continuous_noise_cov进行初始化。
3. 初始化卡方检验表。
4. 调用createRosIO() 初始化nodelet的关于消息发布和接受的topic。

1.1 createRosIO()

createRosIO()这个函数很好理解，就是创建相关publisher以及subscriber。其中

1. pub有feature_pub，和odom_pub。分别发布特征点点云信息，以及odometry 里程信息。
2. sub有feature_sub，imu_sub ，分别监听image_processor发布的feature信息，以及imu信息
3. reset_srv是重制按钮的监听器。

二 MsckfVIO类的imuCallback()函数

每当imu topic有信息时，这个回调函数会被调用。
所有imu msg是会被push_back到imu_msg_buffer后面，而不是立即处理
然后判断gravity是否已经被初始化，若未被初始化，判断是否有足够的（imu_msg_buffer.size()==200）imu msg信息进行初始化.若足够。
调用

2.1 initializeGravityAndBias（）

当imu_msg_buffer.size()==200时被调用，对gravity以及bias进行初始化。
对于state_server.imu_state.gyro_bias表示状态向量里的bg，这里主要是用imu_msg_buffer里面信息的平均值作为初始化值。同理gravity是用acc的平均值进行初始化。
值得一提的是此时得到的gravity并不是对应世界坐标下的重力加速度。所以

double gravity_norm = gravity_imu.norm();
IMUState::gravity = Vector3d(0.0, 0.0, -gravity_norm);

上述就是将重力加速度转化为世界坐标系下。

Quaterniond q0_i_w = Quaterniond::FromTwoVectors(gravity_imu, -IMUState::gravity);
state_server.imu_state.orientation =rotationToQuaternion(q0_i_w.toRotationMatrix().transpose());//初始化0时刻imustate的orientation部分

三 MsckfVIO类的MsckfVio::featureCallback

这是特征点（实际上是新一帧图片被track的特征点）
每当新进来一帧图像，该回调函数就会被调用。

3.1首先判断重力是否被初始化，未初始化则返回。

3.2 调用batchImuProcessing()

该函数被调用时主要发生以下事情。
通过当前state_server.imu_state的时间判断当前imu_msg_buffer中哪些信息不被使用。并且用
int used_imu_msg_cntr = 0;这个变量来记录在imu_msg_buffer中已被使用的imu_msg的个数。
将imu_msg的转化成eigen形式。
然后调用MsckfVio::processModel函数

3.2.1 MsckfVio::processModel函数

每当这个函数被调用时，状态向量的协方差矩阵（state_server.state_cov）以及imus_state

1. 计算F矩阵以及G矩阵
2. 根据F矩阵计算Phi（$\Phi (t+T,t)$）矩阵
3. 调用predictNewState()函数：RK算法积分出下一时刻IMU状态的预测值
4. 调整Phi矩阵以应对yaw值不客观的问题得到新的Phi
5. 用新的Phi矩阵更新协方差矩阵（state_server.state_cov）

3.2.2 循环执行完processModel之后，根据used_imu_msg_cntr 擦除已经使用的imu_msg。

3.3 调用void MsckfVio::stateAugmentation

这个函数主要是对协方差进行增广

1. 根据imu状态id，在camstates中注册新的camstate并用当前的imu状态值对其进行初始化。
2. 与论文相符，构建雅可比矩阵J 维度6x21
3. 根据J更新协方差矩阵
4. 调整协方差矩阵，使其保持对称性

3.4 调用MsckfVio::addFeatureObservations

执行该函数时，会判断是否有新观测特征点，并将其加入到map_server中。

3.5 调用MsckfVio::removeLostFeatures

如函数名，删除跟踪丢失的feature
定义两个vector，invalid_feature_ids用来记录不合法的feature id，processed_feature_ids负责记录继续保留的feature id。
并且同时记录jacobian_row_size 雅可比矩阵的行数，对于每一个processed_feature_ids里的特征点执行。 jacobian_row_size += 4*feature.observations.size() - 3;

然后根据invalid_feature_ids里面的feature id ，从map_server里面删除掉。

循环遍历 processed_feature_ids，获取每一个特征点，然后调用featureJacobian这个函数，得到对于该特征点的H_xj以及r_j.并且根据计算结果更新H_x 和r

调用measurementUpdate函数，如函数名。更新measurement，对应论文中的update部分。

3.6 调用MsckfVio::pruneCamStateBuffer

这里主要做的是对camstates进行瘦身

1. 判断state_server.cam_states.size() < max_cam_state_size，小于滑动窗口，不执行
2. 大于滑动窗口size，则调用findRedundantCamStates函数寻找可以被删除的两个camstate
3. 遍历每一个feature。检查该feature是否与被删除的camstate有关联，无则继续，有则更新H_xj ,r_j并且计算新的H_x 和r。
4. 执行一次measurementUpdate
5. 调整协方差
6. 删除camstate

结语

到此基本上结束，update发生在两处，camstates瘦身时，删除不再被跟踪特征点时。