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

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

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

目录

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

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

2. IMU预积分

3. 重定位setReloFrame()

 4. processImage()处理图像

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

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

二、getMeasurements()函数

1、图像注解

2、代码

三、3个回调函数

imu_callback()回调函数

feature_callback()函数

 restart_callback()

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() 处理图像帧:初始化,紧耦合的非线性优化    

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值