VINS-Mono 代码详细解读——Estimator_node.cpp

最新推荐文章于 2025-10-25 18:08:55 发布

原创

最新推荐文章于 2025-10-25 18:08:55 发布 · 3.4k 阅读

29 ·

CC 4.0 BY-SA版权

本文详细解读VINS-Mono中的Estimator_node.cpp，涵盖process()函数的观测值处理，包括IMU预积分、重定位、图像处理及参数更新。同时介绍getMeasurements()函数、3个回调函数（imu_callback、feature_callback、restart_callback）以及predict()和update()方法，讨论了多线程中数据同步和时间对齐策略。

一、process() 函数处理观测值线程

1、先通过getMeasurements（）函数获取imu和相机帧的信息。

2. IMU预积分

3. 重定位setReloFrame（）

4. processImage（）处理图像

5、向RVIZ发布里程计信息、关键位姿、相机位姿、点云和TF关系

6、更新IMU参数[P,Q,V,ba,bg,a,g]

二、getMeasurements（）函数

relocalization_callback（）

四、predict（）

update（）

estimator_node.cpp

int main(int argc, char **argv)
{
    // 1.ROS初始化设置节点vins_estimator，设置句柄n
    ros::init(argc, argv, "vins_estimator");
    ros::NodeHandle n("~");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
    
    // 2.读取参数，设置估计器参数
    readParameters(n);
    estimator.setParameter();

#ifdef EIGEN_DONT_PARALLELIZE
    ROS_DEBUG("EIGEN_DONT_PARALLELIZE");
#endif
    ROS_WARN("waiting for image and imu...");

    //用于RVIZ显示的Topic
    registerPub(n);

    // 3.订阅IMU、feature、restart、match_points的话题topic,执行各自回调函数
    // 每当订阅的节点由数据送过来就会进入到相应的回调函数中。
    ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
    ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
    ros::Subscriber sub_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);
    ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);

    // 4.创建VIO主线程
    std::thread measurement_process{process};
    ros::spin();

    return 0;
}

参数

Estimator estimator;// Estimator类的对象

std::condition_variable con;//条件变量
double current_time = -1;

// 三个buf
queue<sensor_msgs::ImuConstPtr> imu_buf; 
queue<sensor_msgs::PointCloudConstPtr> feature_buf;
queue<sensor_msgs::PointCloudConstPtr> relo_buf;

int sum_of_wait = 0;// 等待IMU刷新时间

// 互斥量，锁
std::mutex m_buf;
std::mutex m_state;
std::mutex i_buf;
std::mutex m_estimator;

double latest_time;

//IM U项[P,Q,B,Ba,Bg,a,g]
Eigen::Vector3d tmp_P;
Eigen::Quaterniond tmp_Q;
Eigen::Vector3d tmp_V;
Eigen::Vector3d tmp_Ba;
Eigen::Vector3d tmp_Bg;
Eigen::Vector3d acc_0;
Eigen::Vector3d gyr_0;

// 初始化feature、imu、last_imu_t
bool init_feature = 0;
bool init_imu = 1;
double last_imu_t = 0;

函数名	功能
void process()	VIO主线程
vector<pair<vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> getMeasurements()	对imu和图像数据进行对齐并组合
Estimator::processIMU（）本节末尾	IMU预积分
Estimator::setReloFrame（）	设置重定位帧
Estimator::processImage（）下一节再讲	处理图像帧
imu_callback（）	imu回调函数，将imu_msg保存到imu_buf, IMU状态递推并发布PVQ，header
feature_callback（）	feature回调函数，将feature_msg放入feature_buf
restart_callback（）	restart回调函数，清空feature_buf和imu_buf，估计器重置，时间重置
relocalization_callback（）	重定位回调函数，将points_msg放入relo_buf
update（）	imu_buf中PVQ递推更新
predict（）	离散中值积分从IMU测量值imu_msg和上一个PVQ递推得到下一个 tmp_Q，tmp_P，tmp_V

一、process() 函数处理观测值线程

等待并获取measurements：(IMUs, img_msg)s，计算dt

estimator.processIMU() 进行IMU预积分

estimator.setReloFrame() 设置重定位帧

estimator.processImage() 处理图像帧：初始化，紧耦合的非线性优化

最低0.47元/天解锁文章