IMU预积分

关于IMU预积分的详细内容可参考VINS-Mono详解。

VINS-Mono后端

VINS-Mono将VIO和后端封装为一个ROS节点estimator_node，IMU预积分、松耦合初始化、local BA和回环检测都是在vins_estimator中完成的，后端节点的实现在vins_estimator目录下的src中。vins_estimator代码目录：

vins_estimator
- src
  - factor 计算IMU和视觉观测残差，涉及到ceres优化和雅可比矩阵的计算。
  - initial VIO的松耦合初始化。
  - loop-closure 回环检测，使用了DBOW2作者的一个Demo
  - utility 实现了四元数转换、显示等功能。
    - utility.h
    - utility.cpp 实现了一些四元数和旋转矩阵之间的转换
    - tic_toc.h
    - CameraPoseVisualization.h
    - CameraPoseVisualization.cpp 定义了CameraPoseVisualization类
    - visualization.h
    - visualization.cpp 用于在vins_estimator节点中注册一些发布器，发布数据供Rviz显示
  - parameters.h
  - parameters.cpp 从外部读取配置参数，而且parameters.h中也指定了一些参数
  - feature_manager.h
  - feature_manager.cpp 主要通过定义FeatureManager类，封装了特征点管理，三角化以及关键帧相关的操作。
  - estimator.h
  - estimator.cpp 实现了estimator类，封装了预积分、松耦合初始化和非线性优化等操作
  - estimator_node.cpp（main函数所在文件）开启3个线程：measurement_process、loop_detection和pose_graph。

入口函数main

后端入口函数为estimator_node.cpp中的main()函数。在main()函数中，创建名为“vins_estimator”的节点。

接着定义三个订阅器sub_imu、sub_image和sub_raw_imag：

订阅器sub_imu从话题IMU_TOPIC中订阅原始IMU数据，回调函数为imu_callback()。
订阅器sub_image从话题”/feature_tracker/feature”中订阅feature_tracker节点跟踪的特征点数据，回调函数为feature_callback()。
订阅器sub_raw_imag从话题IMAGE_TOPIC中订阅相机图像数据，回调函数为raw_image_callback()。

	name	topic	type	消息内容
subscriber	sub_imu	IMU_TOPIC		原始IMU数据
subscriber	sub_image	"/feature_tracker/ feature"	sensor_msgs:: PointCloud	feature_tracker节点跟踪的特征点数据
subscriber	sub_raw_image	IMAGE_TOPIC	sensor_msgs::Image	相机图像数据

imu_callback()

imu_callback()是订阅原始IMU数据的回调函数，首先将IMU数据的指针保存到imu_buf中，然后执行con.notify_one()，唤醒作用于process线程中的获取观测值数据的函数。每得到一帧IMU数据，就调用estimator_node.cpp中定义的predict()函数，通过中值积分计算出标称状态下本体坐标系相对于世界坐标系的位移p、速度v和旋转q。并通过utility/visualization.cpp中定义的发布器来发布p、v、q，似乎是为了用于Rviz显示。

feature_callback()

feature_callback()是订阅feature_tracker节点所跟踪特征点的回调函数，该函数的功能是：将feature_tracker节点发布的特征点数据保存到feature_buf中去。

raw_image_callback()

raw_image_callback()是订阅相机图像数据的回调函数，这里订阅相机图像数据是为了回环检测服务的,该回调函数的功能就是：若开启回环闭合模块，则将图像数据保存到一个队列（queue）image_buf中去。

多线程处理

vins_estimator中开启了3个线程：

measurement_process线程。线程函数为process(),该函数实现了视觉惯性里程计，包括IMU预积分、松耦合初始化和local BA；
loop_detection线程。线程函数为process_loop_detection()，该函数完成回环检测；
pose_graph线程。线程函数为process_pose_graph，该函数完成全局优化。

其中loop_detection和pose_graph线程受配置参数LOOP_CLOSURE的控制。如果LOOP_CLOSURE置0，则不会创建loop_detection和pose_graph线程。

measurement_process线程

measurement_process的线程函数是estimator_node.cpp中定义的process()函数。在process()函数中，首先调用getMeasurements()函数，从缓存imu_buf和feature_buf中读入IMU数据和图像特征点数据。

getMeasurements()函数

getMeasurements()函数，从imu_buf和feature_buf中读入IMU数据和图像特征点数据，并在该函数中对IMU数据和图像特征点数据进行初步的时间戳对齐。初步的时间戳对齐需要满足两个条件： 1. IMU缓存队列中队尾数据的时间戳，要晚于图像特征点数据缓存队列中队首数据的时间戳； 2. IMU缓存队列中队首数据的时间戳，要早于图像特征点数据缓存队列中队首数据的时间戳。

时间戳初步对齐的结果是：一帧图像特征点数据与多帧IMU数据对应，这些IMU数据为：该帧图像与上一帧图像的时间间隔内的所有IMU数据。这里只保证了相邻的feature数据之间有完整的IMU数据。每一组IMU数据和对应的图像特征点数据组成measurement，多组measurement组成measurements，作为getMeasurements()的返回值。

值得一提的是，在获得观测数据的过程中，使用了互斥锁和条件等待的功能。当回调函数imu_callback()或者feature_callback()从话题消息中接收数据时，互斥锁会锁住imu_buf或者feature_buf，此时getMeasurements()就无法从imu_buf、feature_buf中获取观测数据。当imu_callback()或者feature_callback()接收话题数据完成时，条件等待就会唤醒getMeasurements()从imu_buf、feature_buf中读取观测数据，在读取过程中两个回调函数imu_callback()和feature_callback()是无法接收数据的。当getMeasurements()提取完成后，回调函数就可以继续接收话题消息数据。

send_imu()函数

使用for循环对measurements中的measurement进行遍历，对于measurement中的IMU数据imu_msg，通过一个for循环进行遍历，对每一帧IMU数据调用send_imu()函数进行预积分：

1
2
3
4


//遍历各帧imu数据，进行预积分
for (auto &imu_msg : measurement.first)
    send_imu(imu_msg);

在send_imu()函数中调用Estimator::processIMU()函数进行处理：

1
2


estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz));

Estimator::processIMU()函数

在Estimator::processIMU()函数中调用IntegrationBase::push_back()函数

1
2


pre_integrations[frame_count]->push_back(dt, linear_acceleration, angular_velocity);

IntegrationBase::push_back()

每一帧IMU数据都对应一个预积分，对于每一帧IMU数据，在IntegrationBase::push_back()函数中调用IntegrationBase::propagate()函数：

1
2
3
4
5
6
7
8


void push_back(double dt, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr)
{
    dt_buf.push_back(dt);
    acc_buf.push_back(acc);
    gyr_buf.push_back(gyr);
    propagate(dt, acc, gyr);
}

IntegrationBase::propagate()

在IntegrationBase::propagate()函数中，调用IntegrationBase::midPointIntegration()函数：

1
2
3
4
5


midPointIntegration(_dt, acc_0, gyr_0, _acc_1, _gyr_1, delta_p, delta_q, delta_v,
                    linearized_ba, linearized_bg,
                    result_delta_p, result_delta_q, result_delta_v,
                    result_linearized_ba, result_linearized_bg, 1);

IntegrationBase::midPointIntegration()

计算误差状态的转移矩阵F和噪声系数矩阵V，更新雅可比矩阵jacobian和协方差矩阵covariance

Estimator::processImage()函数

对于measurement中的图像特征点数据img_msg，先将其存到一个关联容器map中，然后调用Estimator::processImage()函数进行处理。

1
2


estimator.processImage(image, img_msg->header); // 处理图像特征点数据

VINS-Mono（二）vins_estimator_node

Table of Contents