【VINS论文笔记】系列之回环检测与重定位

点云PCL博主

发布于 2021-03-08 15:43:45

2.6K0

发布于 2021-03-08 15:43:45

文章被收录于专栏：点云PCL点云PCL

本文仅做学术分享，已获得作者授权转载，未经允许请勿二次转载！欢迎各位加入免费知识星球，获取PDF文档，欢迎转发朋友圈，分享快乐。

希望有更多的小伙伴能够加入我们，一起开启论文阅读，相互分享的微信群。参与和分享的方式：dianyunpcl@163.com

前言

本文主要介绍VINS的重定位模块（relocalization），主要在代码中/pose_graph节点的相关部分实现。

从论文的内容上来说，主要包括了VINS中的回环检测、特征匹配与检验、重定位等内容，即论文第七章（VII. RELOCALIZATION）。先简要介绍下论文中的内容：

A. 回环检测 1、利用DBoW2进行回环检测。 2、除了用于单目VIO的角点特征外，还添加了500个角点并使用BRIEF描述子描述。额外的角点特征用于在回环检测中实现更好的召回率。 3、DBoW2在时间和空间一致性检查后返回回环检测候选帧。 4、VINS保留所有用于特征检索的BRIEF描述子，丢弃原始图像以减少内存消耗 5、由于单目VIO可以观测到滚动和俯仰角，VINS并不需要依赖旋转不变性，如ORB SLAM中使用的ORB特性。

B. 特征恢复 1、检测到回环时，通过BRIEF描述子匹配找到对应关系，建立局部滑动窗口与回环候选帧之间的连接。 2、直接描述子匹配可能会造成大量异常值，使用两步进行几何上的异常值剔除。 1）2D-2D：RANSAC的基本矩阵检验。 2）3D-2D：RANSAC的PNP检验。当内点超过一定阈值时，我们将该候选帧视为正确的循环检测并执行重定位。

C. 紧耦合重定位 1、重定位过程使单目VIO维持的当前滑动窗口与过去的位姿图对齐。 2、将所有回环帧的位姿作为常量，利用所有IMU测量值、局部视觉测量和从回环中提取特征对应值，共同优化滑动窗口。

流程图

代码实现

pose_graph文件夹

keyframe.cpp/.h 构建关键帧类、描述子计算、匹配关键帧与回环帧。

pose_graph.cpp/.h 位姿图的建立与图优化、回环检测与闭环。

pose_graph_node.cpp ROS 节点函数、回调函数、主线程。

输入输出

输入： 1、订阅了/vins_estimator节点发布的多个topic，包括关键帧的位姿（keyframe_pose）、重定位位姿（relo_relative_pose）、相机到IMU的外参估计（extrinsic）、VIO里程计信息PQV（odometry）、关键帧中的3D点云（keyframe_point）、IMU传播值（imu_propagate）。 2、图像，即订阅了传感器或者rosbag发布的topic：“/cam0/image_raw”。

pose_graph_node.cpp

注意此cpp在开头全局变量定义的时候，构建了一个全局的位姿图优化对象，另外介绍一下在之后回调函数和process线程中会用到的几个队列：

PoseGraph posegraph;
queue<sensor_msgs::ImageConstPtr> image_buf;//原始图像数据
queue<sensor_msgs::PointCloudConstPtr> point_buf;//世界坐标系下的地图点云
queue<nav_msgs::Odometry::ConstPtr> pose_buf;//当前帧的 pose
queue<Eigen::Vector3d> odometry_buf;

程序入口 int main(int argc, char **argv)

1、ROS初始化，设置句柄。

2、从launch文件读取参数和参数文件config中的参数。其中： VISUALIZATION_SHIFT_X、VISUALIZATION_SHIFT_Y为可视化界面中图像x轴y轴的偏移量，一般设置为0； SKIP_CNT为之后运行process()内循环的间隔； SKIP_DIS为判断是否构建关键帧的距离标准； visualize_camera_size为可视化界面图像的尺寸； loop_closure=1即进行回环检测。

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

4、按需求加载先前位姿图

6、发布/pose_graph的topic。

7、设置主线程和键盘控制线程。

主线程 process()

如果LOOP_CLOSURE为0，即不需要进行回环检测就直接返回；如果需要则通过while (true)不断循环以下过程：（注意在使用每个队列buf的时候要加锁m_buf）。

1、得到具有相同时间戳的pose_msg、image_msg、point_msg。

2、构建pose_graph中用到的关键帧：这里用到的策略是先剔除最开始的SKIP_FIRST_CNT帧，然后每隔SKIP_CNT，将将距上一关键帧距离（平移向量的模）超过SKIP_DIS的图像创建为关键帧。

3、在posegraph中添加关键帧，将flag_detect_loop=1即设置回环检测。

4、休眠5ms

可以看到，process()的最重要的内容在于如何构建keyframe对象，以及将其通过addKeyFrame添加到posegraph对象中，而这部分分别在KeyFrame和pose_graph文件中。

pose_graph.cpp/.h

该文件主要构建了位姿图类：class PoseGraph，以及其他功能性函数，比如： YawPitchRollToRotationMatrix将欧拉角转换为旋转矩阵； RotationMatrixTranspose对矩阵进行转置； RotationMatrixRotatePoint将Rt矩阵相乘等。还构造了四自由度残差的结构，这部分留到四自由度位姿图优化中再讨论。这里主要讨论PoseGraph中的函数，值得注意的是PoseGraph的构造函数中创建了一个4自由度位姿图优化线程。

t_optimization = std::thread(&PoseGraph::optimize4DoF, this);

void PoseGraph::addKeyFrame(KeyFrame* cur_kf, bool flag_detect_loop)

1、如果sequence_cnt != cur_kf->sequence，则新建一个新的图像序列

2、获取当前帧的位姿vio_P_cur、vio_R_cur并更新

3、进行回环检测，返回回环候选帧的索引

4、如果存在回环候选帧，即loop_index != -1： 1）将当前帧与回环帧进行描述子匹配，如果成功则确定存在回环

2）计算当前帧与回环帧的相对位姿，纠正当前帧位姿w_P_cur、w_R_cur

3）如果存在多个图像序列，则将所有图像序列都合并到世界坐标系下

4）将当前帧放入优化队列中

5、获取VIO当前帧的位姿P、R，根据偏移量计算得到实际位姿。并进行位姿更新

6、发布path[sequence_cnt]

7、保存闭环轨迹到VINS_RESULT_PATH

8、绘制可视化轨迹中帧间的连线，发布topic：pub_pg_path、pub_path、pub_base_path

int PoseGraph::detectLoop(KeyFrame* keyframe, int frame_index)

该函数用于检测当前帧与先前帧是否可能存在回环，若存在则返回回环候选帧的索引。在函数中使用大量DEBUG条件语句，用于在调试时对当前状态进行可视化输出，这里就不介绍了。 1、查询字典数据库，得到与每一帧的相似度评分ret

2、添加当前关键帧到字典数据库中

3、通过相似度评分判断是否存在回环候选帧

4、如果在先前检测到回环候选帧再判断：当前帧的索引值是否大于50，即系统开始的前50帧不进行回环；返回评分大于0.015的最早的关键帧索引min_index，如果不存在回环或判断失败则返回-1

keyframe.cpp/.h

该文件主要构建了两个类： 1、class BriefExtractor，构建Brief产生器，用于通过Brief模板文件对图像特征点计算Brief描述子，

2、class KeyFrame，构建关键帧，通过BRIEF描述子匹配关键帧和回环候选帧。其成员函数包括：（省去了部分get和set函数）

void KeyFrame::searchByBRIEFDes

该函数的作用是将此关键帧对象与某个回环帧进行BRIEF描述子匹配，其参数包括：

void KeyFrame::searchByBRIEFDes(std::vector<cv::Point2f> &matched_2d_old,//param[out]回环帧匹配后的二维坐标
std::vector<cv::Point2f> &matched_2d_old_norm,//param[out]回环帧匹配后的二维归一化坐标
                                std::vector<uchar> &status,//param[out]匹配状态，成功为1
                                const std::vector<BRIEF::bitset> &descriptors_old,//param[in]回环帧的描述子
                                const std::vector<cv::KeyPoint> &keypoints_old,//param[in]回环帧的二维坐标
                                const std::vector<cv::KeyPoint> &keypoints_old_norm)//param[in]回环帧的二维归一化坐标
{
//vector<BRIEF::bitset> window_brief_descriptors为这个关键帧所有特征点对应的brief描述子
    for(int i = 0; i < (int)window_brief_descriptors.size(); i++)
    {
        cv::Point2f pt(0.f, 0.f);
        cv::Point2f pt_norm(0.f, 0.f);
        //对关键帧中每个特征点的描述子与回环帧的所有描述子匹配，如果能找到汉明距离小于80的最小值和索引即为该特征点的最佳匹配，相应的status置为1
        if (searchInAera(window_brief_descriptors[i], descriptors_old, keypoints_old, keypoints_old_norm, pt, pt_norm))
          status.push_back(1);
        else
          status.push_back(0);
        matched_2d_old.push_back(pt);
        matched_2d_old_norm.push_back(pt_norm);
    }
}

bool KeyFrame::findConnection(KeyFrame* old_kf)

该函数的主要目的是寻找并建立关键帧与回环帧之间的匹配关系，返回True即为确定构成回环。 1、将关键帧与回环帧进行BRIEF描述子匹配，并剔除匹配失败的点

2、如果能匹配的特征点能达到最小回环匹配个数，则用RANSAC PnP检测再去除误匹配的点，

3、将此关键帧和回环帧拼接起来，将对应的匹配点相连以绘制回环匹配图，并发布为pub_match_img。

4、如果在PNP检验后仍能达到最小回环匹配点数则进行先对位姿检验，通过则确定构成回环，将回环帧索引和相对位姿存入loop_index、loop_info，并返回True。

if ((int)matched_2d_cur.size() > MIN_LOOP_NUM)
{
    relative_t = PnP_R_old.transpose() * (origin_vio_T - PnP_T_old);
    relative_q = PnP_R_old.transpose() * origin_vio_R;
    relative_yaw = Utility::normalizeAngle(Utility::R2ypr(origin_vio_R).x() - Utility::R2ypr(PnP_R_old).x());
    //相对位姿检验
    if (abs(relative_yaw) < 30.0 && relative_t.norm() < 20.0)
    {
    has_loop = true;
    loop_index = old_kf->index;
    loop_info << relative_t.x(), relative_t.y(), relative_t.z(),
                 relative_q.w(), relative_q.x(), relative_q.y(), relative_q.z(),
                 relative_yaw;
    if(FAST_RELOCALIZATION)
    {
    快速重定位功能，略
    }
        return true;
    }
}
return false;
}

资源

本文参与腾讯云自媒体分享计划，分享自微信公众号。

原始发表：2021-03-05，如有侵权请联系 cloudcommunity@tencent.com 删除

c++