VINS-Mono 代码解析二、初始化第1部分

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

原创

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

标签

#javascript

本文深入解读了VINS-mono估计器模块，涉及状态初始化、非线性优化、关键帧选择及重定位过程。通过源码解析，揭示了IMU预积分、视觉特征融合和闭环检测的核心算法。

一、前言

VINS的状态估计器模块（estimator），主要在代码中 /vins_estimator节点的相关部分实现。
这个模块可以说是VINS的最核心模块，从论文的内容上来说，里面的内容包括了VINS的估计器初始化、基于滑动窗口的非线性优化实现紧耦合。此外还包括了关键帧的选择内容。该模块的代码放在文件夹vins_estimator中，可以看到，除了上述内容外，还包括有外参标定、可视化等其他功能的实现，内容实在是太多了！

本人主要按照程序的运行步骤来讲解，理论和代码 结合的方式，可能有点多，后面可能会分两部分。

因为这是我第3此仔细看这个代码了，一方面加深理解，另一方面也是一种记录和复习吧！结束后会自己写双目和加GPS数据。

这一部分的整体结构图：

其中：

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计时器。

下面是系统的闭环优化流程图：

上图中,蓝线为正常的闭环优化流程,即通过后端的非线性优化来更新滑窗内所有相机的位姿。紫线为闭环检测模块,当后端优化完成后,会将滑窗内的次新帧进行闭环检测,即首先提取新角点并进行描述,然后与数据库进行检索,寻找闭环帧,并将该帧添加到数据库中。红线为快速重定位模块,当检测到闭环帧后,会将闭环约束添加到后端的整体目标函数中进行非线性优化,得到第 i 帧(注意这里的帧为闭环帧中的老帧)经过滑窗优化后的位姿,这时可以直接根据计算结果修正滑窗内所有相机的位姿,但是因为此处计算的 T i←i_opt 并不准确 , 因此我们会在检测到闭环成功后 , 进行 PoseGraph 的四自由度优化PoseGraph::optimize4DoF(),来计算 T i←i_opt ,并将此值 (r_drift、t_drift) 传回给后端优化线程,来更新滑窗内的相机位姿 Estimator::update_loop_correction()。

二、程序解析

程序入口在 /vins_estimator/src/estimator_node.cpp 的main.cpp 里面，代码如下：

int main(int argc, char **argv)
{
    //1、ROS初始化 
    ros::init(argc, argv, "vins_estimator"); //许ROS通过命令行进行名称重映射，指定节点名称的地方
    ros::NodeHandle n("~");                  //创建节点句柄-实际上将执行节点的初始化
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME, ros::console::levels::Info);
    
    //2、读取参数，设置状态估计器参数
    readParameters(n);
    //它读取了每一个相机到IMU坐标系的旋转/平移外参数和非线性优化的重投影误差部分的信息矩阵。
    estimator.setParameter();
    
#ifdef EIGEN_DONT_PARALLELIZE
    ROS_DEBUG("EIGEN_DONT_PARALLELIZE");
#endif
    ROS_WARN("waiting for image and imu...");

    //3、发布用于  RVIZ显示 和 pose_graph_node.cpp 接收 的Topic，本模块具体发布的内容详见输入输出
    //这个函数定义在utility/visualization.cpp里面：void registerPub(ros::NodeHandle &n)。
    registerPub(n); 

    //4、创建消息订阅者，订阅IMU、feature、restart、match_points的topic，执行各自回调函数
    ros::Subscriber sub_imu = n.subscribe(IMU_TOPIC, 2000, imu_callback, ros::TransportHints().tcpNoDelay());
    
    //这一部分接收的是feature_tracker_node发布的在cur帧的所有特征点的信息
    ros::Subscriber sub_image = n.subscribe("/feature_tracker/feature", 2000, feature_callback);
    
    //restart_callback干了一件事，就是把所有状态量归零，把buf里的数据全部清空。
    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);

    //5、创建VIO主线程process()(VINS核心！)，这一部分是最重要的，包含了VINS绝大部分内容和最难的内容
    std::thread measurement_process{process};
    ros::spin();//所有用户的调用程序将从 ros::spin()开始调用

    return 0;
}

说明：1、2部分我们就不看了，和前端的道理是一样的，ROS的初始化和参数的读取，我们从第3部分开始看！

2.1 主题发布

我们看了主函数代码很奇怪，只有订阅的话题，没有发布的话题，其实话题发布在第3步骤！程序如下“

void registerPub(ros::NodeHandle &n)
{
    pub_latest_odometry    = n.advertise<nav_msgs::Odometry>("imu_propagate", 1000);
    pub_path               = n.advertise<nav_msgs::Path>("path", 1000);
    pub_relo_path          = n.advertise<nav_msgs::Path>("relocalization_path", 1000);
    pub_odometry           = n.advertise<nav_msgs::Odometry>("odometry", 1000);
    pub_point_cloud        = n.advertise<sensor_msgs::PointCloud>("point_cloud", 1000);
    pub_margin_cloud       = n.advertise<sensor_msgs::PointCloud>("history_cloud", 1000);
    pub_key_poses          = n.advertise<visualization_msgs::Marker>("key_poses", 1000);
    pub_camera_pose        = n.advertise<nav_msgs::Odometry>("camera_pose", 1000);
    pub_camera_pose_visual = n.advertise<visualization_msgs::MarkerArray>("camera_pose_visual", 1000);
    pub_keyframe_pose      = n.advertise<nav_msgs::Odometry>("keyframe_pose", 1000);
    pub_keyframe_point     = n.advertise<sensor_msgs::PointCloud>("keyframe_point", 1000);
    pub_extrinsic          = n.advertise<nav_msgs::Odometry>("extrinsic", 1000);
    pub_relo_relative_pose =  n.advertise<nav_msgs::Odometry>("relo_relative_pose", 1000);

    cameraposevisual.setScale(1);
    cameraposevisual.setLineWidth(0.05);
    keyframebasevisual.setScale(0.1);
    keyframebasevisual.setLineWidth(0.01);
}

先不用知道每个发布信息的意思，后面在慢慢去理解，知道发布话题在这即可！

2.2 订阅话题信息

（1）订阅IMU话题信息

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); // 只要有IMU数据就调用这个程序
        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);
    }
}