【SLAM】VINS-MONO解析——vins_estimator流程

最新推荐文章于 2026-03-30 10:05:34 发布

原创

最新推荐文章于 2026-03-30 10:05:34 发布 · 2.2w 阅读

101

深入解析VINS-MONO视觉惯性里程计系统，涵盖初始化、IMU预积分、非线性优化等核心模块，详细阐述VINS-MONO的运行原理与实现细节。

5.vins_estimator

基本上VINS里面绝大部分功能都在这个package下面，包括IMU数据的处理(前端)，初始化(我觉得可能属于是前端)，滑动窗口(后端)，非线性优化(后端)，关键帧的选取(部分内容)(前端)。我第一次看的时候，总是抱有一个疑问，就是为什么把这么多内容全都放在这一个node里面。为了回答这个问题，那么首先先搞清楚vins_estimator里面分别具体都是什么，为什么要有这些数据结构/函数，这些函数是怎样工作的。
在这里插入图片描述

这个package下面主要以下文件：
factor——主要用于非线性优化对各个参数块和残差块的定义，VINS采用的是ceres，所以这部分需要对一些状态量和因子进行继承和重写。
initial——主要用于初始化，VINS采用的初始化策略是先SfM进行视觉初始化，再与IMU进行松耦合。
estimator.cpp——vins_estimator需要的所有函数都放在这里，是一个鸿篇巨制。
estimator_node.cpp——vins_estimator的入口，是一个ROS的node，实际上运行的是这个cpp文件。
feature_manager.cpp——负责管理滑窗内的所有特征点。
parameters.cpp——读取参数，是一个辅助函数。
utility——里面放着用于可视化的函数和tictok计时器。

然后就是要注意CmakeLists.txt和package.xml文件的写法，这两个文件也是相当于套公式，写错了就不能实现正常的ROS功能了。

各个部分的讲解如下链接：

【SLAM】VINS-MONO解析——综述

【SLAM】VINS-MONO解析——feature_tracker

【SLAM】VINS-MONO解析——IMU预积分

【SLAM】VINS-MONO解析——vins_estimator

【SLAM】VINS-MONO解析——初始化(理论部分)

【SLAM】VINS-MONO解析——初始化(代码部分)

【SLAM】VINS-MONO解析——后端优化(理论部分)

【SLAM】VINS-MONO解析——后端优化(代码部分)

【SLAM】VINS-MONO解析——sliding window

【SLAM】VINS-MONO解析——回环检测

【SLAM】VINS-Fusion解析——流程

【SLAM】VINS-MONO解析——对vins-mono的修改使流程逻辑更清晰

【SLAM】VINS-MONO解析——基于vins-mono的slam系统开发

5.1 estimator_node

5.1.1 main()主入口
在这里插入图片描述
1、ROS初始化、设置句柄

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();

这个estimator.setParameter()需要注意一下，它在estimator.cpp里面：

void Estimator::setParameter()
{
   
   
    for (int i = 0; i < NUM_OF_CAM; i++)
    {
   
   
        tic[i] = TIC[i];
        ric[i] = RIC[i];
    }
    f_manager.setRic(ric);
    ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity();
    td = TD;
}

它读取了每一个相机到IMU坐标系的旋转/平移外参数和非线性优化的重投影误差部分的信息矩阵。

3、发布用于RVIZ显示的Topic，本模块具体发布的内容详见输入输出

registerPub(n);

这个函数定义在utility/visualization.cpp里面：void registerPub(ros::NodeHandle &n)。

4、订阅IMU_TOPIC，执行imu_callback

ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());

这个回调函数比较重要，如下代码所示：

void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg)
{
   
   
    if (imu_msg->header.stamp.toSec() <= last_imu_t)
    {
   
   
        ROS_WARN("imu message in disorder!");
        return;
    }
    last_imu_t = imu_msg->header.stamp.toSec();
    m_buf.lock();
    imu_buf.push(imu_msg);
    m_buf.unlock();
    con.notify_one();
    last_imu_t = imu_msg->header.stamp.toSec();
    {
   
   
        std::lock_guard<std::mutex> lg(m_state);
        predict(imu_msg);
        std_msgs::Header header = imu_msg->header;
        header.frame_id = "world";
        if (estimator.solver_flag == Estimator::SolverFlag::NON_LINEAR)
            pubLatestOdometry(tmp_P, tmp_Q, tmp_V, header);
    }
}

imu_callback主要干了3件事，
第一件事就是往imu_buf里放IMU数据，缓存起来；
第二件事就是IMU预积分获得当前时刻的PVQ，见4.1.2；
第三件事就是如果当前处于非线性优化阶段的话，需要把第二件事计算得到的PVQ发布到rviz里去，见utility/visualization.cpp的pubLatestOdometry()函数。

在这个函数里，第一次出现了上锁操作，因为这个程序是多线程，需要考虑线程安全问题，对imu_buf的操作是一个生产者-消费者模型，加入和读取的时候不可以中断，必须加锁以保证数据安全，后续部分有多个地方都有上锁操作。
这部分出现了几个新的数据结构，解释如下：

数据结构: 
tmp_Q，tmp_P，tmp_V：当前时刻的PVQ
header：当前时刻时间戳

5、订阅/feature_tracker/feature，执行feature_callback

ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);

这一部分接收的是feature_tracker_node发布的在cur帧的所有特征点的信息，见3.3.6.。
feature_callback就只干了一件事，就是把cur帧的所有特征点放到feature_buf里，同样需要上锁。注意，cur帧的所有特征点都是整合在一个数据里的，也就是sensor_msgs::PointCloudConstPtr &feature_msg。

6、订阅/feature_tracker/restart，执行restart_callback

ros::Subscriber sub_restart = n.subscribe("/feature_tracker/restart", 2000, restart_callback);

restart_callback干了一件事，就是把所有状态量归零，把buf里的数据全部清空。

7、订阅/pose_graph/match_points，执行relocalization_callback

ros::Subscriber sub_relo_points = n.subscribe("/pose_graph/match_points", 2000, relocalization_callback);

8、创建VIO主线程process()(VINS核心！)

std::thread measurement_process{
   
   process};

这一部分是最重要的，包含了VINS绝大部分内容和最难的内容。

5.1.2 process()主线程
1、对imu和图像数据进行对齐并配对
这一部分卡了我很久很久，终于最近彻底弄明白了。

 std::vector<std::pair<std::vector<sensor_msgs::ImuConstPtr>, sensor_msgs::PointCloudConstPtr>> measurements;
              
        std::unique_lock<std::mutex> lk(m_buf);
        con.wait(lk, [&]{
   
   return (measurements = getMeasurements()).size() != 0; });
        lk.unlock();

一上来就是一个非常复杂的数据结构measurements！在这里先解释一波。

数据结构: measurements
1、首先，measurements他自己就是一个vector；
2、对于measurements中的每一个measurement，又由2部分组成；
3、第一部分，由sensor_msgs::ImuConstPtr组成的vector；
4、第二部分，一个sensor_msgs::PointCloudConstPtr；
5、这两个sensor_msgs见3.1-6部分介绍。
6、为什么要这样配对(一个PointCloudConstPtr配上若干个ImuConstPtr)？
因为IMU的频率比视觉帧的发布频率要高，所以说在这里，需要把一个视觉帧和之前的一串IMU帧的数据配对起来。

容器measurements有了，接下来就是配对IMU和视觉帧的数据，放到容器里去。配对过程也需要上锁，而且是一个条件锁。作者在这里用了一个lambda表达式，也就是说，在return里面的部分是false时，保持上锁状态，继续配对数据；如果return里面时true，说明配对完成，释放锁，measurements完成了，以供后续使用。

接下来分析一下measurements()具体的功能：