VINS-Mono 代码详细解读——回环检测与重定位、四自由度位姿图优化

最新推荐文章于 2024-02-18 19:59:57 发布

try_again_later

最新推荐文章于 2024-02-18 19:59:57 发布

阅读量3.9k

点赞数 6

分类专栏：视觉、激光SLAM

本文链接：https://blog.csdn.net/try_again_later/article/details/105036445

版权

本文深入解析VINS-Mono的回环检测与重定位机制，以及4自由度位姿图优化。通过DBoW2进行回环检测，匹配特征，利用RANSAC剔除外点，实现紧耦合重定位。此外，还介绍了全局位姿图优化过程，涉及关键帧添加、图优化和管理。

摘要由CSDN通过智能技术生成

本文主要介绍VINS的闭环检测重定位与位姿图优化部分，作为系列文章的最后一节。

回环检测的关键就是如何有效检测出相机曾经经过同一个地方，这样可以避免较大的累积误差，使得当前帧和之前的某一帧迅速建立约束，形成新的较小的累积误差。由于回环检测提供了当前数据与所有历史数据的关联，在跟踪算法丢失后，还可以利用重定位。

论文中主要分为两部分：回环检测与重定位、4-DOF的位姿图优化。

第一部分主要是为了通过回环检测找到当前帧和候选帧的联系，并通过简单的紧耦合重定位将局部滑动窗口移动与过去的位姿对齐。

第二部分是为了保证基于重定位结果对过去的所有位姿进行全局优化。

论文中内容为

0.4 重定位

vins的重定位模块主要包含回环检测,回环候选帧之间的特征匹配,紧耦合重定位三个部分

A、回环检测（只对关键帧）

1、采用DBoW2词袋位置识别方法进行回环检测。经过时间空间一致性检验后，DBoW2返回回环检测候选帧。

2、除了用于单目VIO的角点特征外，还添加了500个角点并使用BRIEF描述子，描述子用作视觉词袋在数据库里进行搜索。这些额外的角点能用来实现更好的回环检测。

3、VINS只保留所有用于特征检索的BRIEF描述子，丢弃原始图像以减小内存。

4、单目VIO可以观测到滚动和俯仰角，VINS并不需要依赖旋转不变性。

B、回环候选帧之间的特征匹配

1、检测到回环时，通过BRIEF描述子匹配找到对应关系。但是直接的描述子匹配会导致很多外点。

2、本文提出两步几何剔除法：

1）2D-2D：使用RANSAC进行F矩阵测试，

2）3D-2D：使用RANSAC进行PnP，基于已知的滑动窗特征点的3D位置，和回路闭合候选处图像的2D观测（像素坐标）。

当内点超过一定阈值时，我们将该候选帧视为正确的循环检测并执行重定位。

C、紧耦合重定位

1、重定位过程使单目VIO维持的当前滑动窗口与过去的位姿图对齐。

2、将所有回环帧的位姿作为常量，利用所有IMU测量值、局部视觉测量和从回环中提取特征对应值，共同优化滑动窗口。

0.5 全局位姿图优化

A、位姿图中添加关键帧

B、4自由度位姿图优化

C、位姿图管理

在这里插入图片描述

相关代码都在pose_graph文件中，主要分为三个程序，分别为：

keyframe.cpp/.h 构建关键帧类、描述子计算、匹配关键帧与回环帧
pose_graph.cpp/.h 位姿图的建立与图优化、回环检测与闭环
pose_graph_node.cpp ROS 节点函数、回调函数、主线程

1、pose_graph_node.cpp

主要分为7个回调函数以及主线程process（）函数。

7个回调函数：包括关键帧的位姿（keyframe_pose）、重定位位姿（relo_relative_pose）、相机到IMU的外参估计（extrinsic）、VIO里程计信息PQV（odometry）、关键帧中的3D点云（keyframe_point）、IMU传播值（imu_propagate）。

ROS初始化，设置句柄

从launch文件读取参数和参数文件config中的参数

如果需要进行回环检测则读取词典和BRIEF描述子的模板文件，同时读取config中的其他参数、设置带回环的结果输出路径。

加载先前位姿图 loadPoseGraph()

订阅各个topic并执行各自回调函数

发布/pose_graph的topic

设置主线程 process() 和键盘控制线程 command()

int main(int argc, char **argv)
{
    // 1.ROS初始化，设置句柄
    ros::init(argc, argv, "pose_graph");
    ros::NodeHandle n("~");
    posegraph.registerPub(n);

    // 2.读取参数
    n.getParam("visualization_shift_x", VISUALIZATION_SHIFT_X);
    n.getParam("visualization_shift_y", VISUALIZATION_SHIFT_Y);
    n.getParam("skip_cnt", SKIP_CNT);
    n.getParam("skip_dis", SKIP_DIS);
    std::string config_file;
    n.getParam("config_file", config_file);
    cv::FileStorage fsSettings(config_file, cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        std::cerr << "ERROR: Wrong path to settings" << std::endl;
    }
    double camera_visual_size = fsSettings["visualize_camera_size"];
    cameraposevisual.setScale(camera_visual_size);
    cameraposevisual.setLineWidth(camera_visual_size / 10.0);
    LOOP_CLOSURE = fsSettings["loop_closure"];
    std::string IMAGE_TOPIC;
    int LOAD_PREVIOUS_POSE_GRAPH;

    //3.如果需要进行回环检测则读取词典和BRIEF描述子的模板文件，同时读取config中的其他参数、设置带回环的结果输出路径。
    if (LOOP_CLOSURE)
    {
        ROW = fsSettings["image_height"];
        COL = fsSettings["image_width"];

        // 3.1读取字典
        std::string pkg_path = ros::package::getPath("pose_graph");
        string vocabulary_file = pkg_path + "/../support_files/brief_k10L6.bin";
        cout << "vocabulary_file" << vocabulary_file << endl;
        posegraph.loadVocabulary(vocabulary_file);

        // 3.2读取BRIEF描述子的模板文件
        BRIEF_PATTERN_FILE = pkg_path + "/../support_files/brief_pattern.yml";
        cout << "BRIEF_PATTERN_FILE" << BRIEF_PATTERN_FILE << endl;
        m_camera = camodocal::CameraFactory::instance()->generateCameraFromYamlFile(config_file.c_str());

        fsSettings["image_topic"] >> IMAGE_TOPIC;        
        fsSettings["pose_graph_save_path"] >> POSE_GRAPH_SAVE_PATH;
        fsSettings["output_path"] >> VINS_RESULT_PATH;
        fsSettings["save_image"] >> DEBUG_IMAGE;
        VISUALIZE_IMU_FORWARD = fsSettings["visualize_imu_forward"];
        LOAD_PREVIOUS_POSE_GRAPH = fsSettings["load_previous_pose_graph"];
        FAST_RELOCALIZATION = fsSettings["fast_relocalization"];
        VINS_RESULT_PATH = VINS_RESULT_PATH + "/vins_result_loop.csv";
        std::ofstream fout(VINS_RESULT_PATH, std::ios::out);
        fout.close();
        fsSettings.release();

        // 3.3加载先前的位姿图
        if (LOAD_PREVIOUS_POSE_GRAPH)
        {
            printf("load pose graph\n");
            m_process.lock();
            posegraph.loadPoseGraph();
            m_process.unlock();
            printf("load pose graph finish\n");
            load_flag = 1;
        }
        else
        {
            printf("no previous pose graph\n");
            load_flag = 1;
        }
    }

    fsSettings.release();

    // 4.订阅话题topic并执行各自回调函数
    ros::Subscriber sub_imu_forward = n.subscribe("/vins_estimator/imu_propagate", 2000, imu_forward_callback);
    ros::Subscriber sub_vio = n.subscribe("/vins_estimator/odometry", 2000, vio_callback);
    ros::Subscriber sub_image = n.subscribe(IMAGE_TOPIC, 2000, image_callback);
    ros::Subscriber sub_pose = n.subscribe("/vins_estimator/keyframe_pose", 2000, pose_callback);
    ros::Subscriber sub_extrinsic = n.subscribe("/vins_estimator/extrinsic", 2000, extrinsic_callback);
    ros::Subscriber sub_point = n.subscribe("/vins_estimator/keyframe_point", 2000, point_callback);
    ros::Subscriber sub_relo_relative_pose = n.subscribe("/vins_estimator/relo_relative_pose", 2000, relo_relative_pose_call