一文详解回环检测与重定位
标题:VINS-Mono代码解读—回环检测与重定位 pose graph loop closing
作者:Manii
来源:https://blog.csdn.net/qq_41839222/category_9286052.html
排版:点云PCL
前言
本文主要介绍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;//当前帧的 posequeue<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]匹配状态,成功为1const 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置为1if (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;}
本文仅做学术分享,如有侵权,请联系删文。
下载1
在「3D视觉工坊」公众号后台回复:3D视觉,即可下载 3D视觉相关资料干货,涉及相机标定、三维重建、立体视觉、SLAM、深度学习、点云后处理、多视图几何等方向。
下载2
在「3D视觉工坊」公众号后台回复:3D视觉github资源汇总,即可下载包括结构光、标定源码、缺陷检测源码、深度估计与深度补全源码、点云处理相关源码、立体匹配源码、单目、双目3D检测、基于点云的3D检测、6D姿态估计源码汇总等。
下载3
在「3D视觉工坊」公众号后台回复:相机标定,即可下载独家相机标定学习课件与视频网址;后台回复:立体匹配,即可下载独家立体匹配学习课件与视频网址。
重磅!3DCVer-学术论文写作投稿 交流群已成立
扫码添加小助手微信,可申请加入3D视觉工坊-学术论文写作与投稿 微信交流群,旨在交流顶会、顶刊、SCI、EI等写作与投稿事宜。
同时也可申请加入我们的细分方向交流群,目前主要有3D视觉、CV&深度学习、SLAM、三维重建、点云后处理、自动驾驶、多传感器融合、CV入门、三维测量、VR/AR、3D人脸识别、医疗影像、缺陷检测、行人重识别、目标跟踪、视觉产品落地、视觉竞赛、车牌识别、硬件选型、学术交流、求职交流、ORB-SLAM系列源码交流、深度估计等微信群。
一定要备注:研究方向+学校/公司+昵称,例如:”3D视觉 + 上海交大 + 静静“。请按照格式备注,可快速被通过且邀请进群。原创投稿也请联系。
▲长按加微信群或投稿
▲长按关注公众号
3D视觉从入门到精通知识星球:针对3D视觉领域的知识点汇总、入门进阶学习路线、最新paper分享、疑问解答四个方面进行深耕,更有各类大厂的算法工程人员进行技术指导。与此同时,星球将联合知名企业发布3D视觉相关算法开发岗位以及项目对接信息,打造成集技术与就业为一体的铁杆粉丝聚集区,近3000星球成员为创造更好的AI世界共同进步,知识星球入口:
学习3D视觉核心技术,扫描查看介绍,3天内无条件退款
圈里有高质量教程资料、可答疑解惑、助你高效解决问题
觉得有用,麻烦给个赞和在看~
一文详解回环检测与重定位相关推荐
- 【SLAM】VINS-MONO解析——回环检测和重定位
9. 回环检测与重定位 本部分内容涉及到的代码大部分在pose_graph文件夹下,少部分在vins_estimator里. 原创内容,转载请先与我联系并注明出处,谢谢! 系列内容请点击:[SLAM] ...
- 一文详解车道线检测技术分析
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 针对车道线检测的任务,我们需要弄清楚几个问题: 1.车道线的表示形式? 输出类型:掩码/点集/矢量线条 ...
- 一文详解基于先验地图的视觉定位
作者丨cc.fy@知乎 来源丨https://zhuanlan.zhihu.com/p/460001686 编辑丨3D视觉工坊 问题定义 给定一份高精度地图或环境模型,给定相机(不限于单目,双目,多目 ...
- 一文详解单目VINS论文与代码解读目录
本文旨在对前一阶段学习vins-mono开源框架的总结.结合暑假秋招之前报名的深蓝学院的<从零开始手写VIO>课程,本文从VIO原理以及开源代码分析两部分进行详细介绍.PS:提升代码能力最 ...
- 视觉SLAM⑪----回环检测
目录 11.0 本章目标 11.1 概述 11.1.1 回环检测的意义 11.1.2 回环检测的方法 11.1.3 准确率和召回率 11.2 词袋模型 11.3 字典 11.3.1 字典的结构 11. ...
- 详解 | SLAM回环检测问题
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达 本文经知乎作者fishmarch授权转载,二次转载请联系作者 原文 ...
- 一文详解 YOLO 2 与 YOLO 9000 目标检测系统
一文详解 YOLO 2 与 YOLO 9000 目标检测系统 from 雷锋网 雷锋网 AI 科技评论按:YOLO 是 Joseph Redmon 和 Ali Farhadi 等人于 2015 年提出 ...
- 一文详解VarScan肿瘤体细胞突变检测的的安装和实践
目录 VarScan 简介 VarScan 安装和使用说明:安装.说明.配置.运行 VarScan 案例实战:数据下载.配置.运行.输出 使用sixbox快速运行 hello,大家好,今天为大家带来 ...
- 综述 | SLAM回环检测方法
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 作者丨任旭倩 来源丨计算机视觉life 在视觉SLAM问题中,位姿的估计往往是一个递推的过程,即由上一 ...
最新文章
- 学C++,80%都是为了搞竞赛、凑一门语言?网友:莫钓鱼
- php self this parent
- 某互联网公司校园招聘的小组面试题
- ubuntu终端下快捷键,字体放大缩小等【逐渐完善篇】
- 性能提升约7倍!Apache Flink 与 Apache Hive 的集成
- 【AI视野·今日CV 计算机视觉论文速览 第179期】Tue, 25 Feb 2020
- Cloudflare通过UnstoppableDomains添加对“.crypto”域名的支持
- 【百度地图API】圣诞节里不会迷路的麋鹿——驾车导航
- Camtasia Recorder 2020如何确定录制区域
- 利用vertical-align:middle实现在整个页面居中
- Zookeeper节点监听结合Spring
- red5源码分析---1
- 非结构化数据分析技术是忽悠
- Write a program that gives count of common characters presented in an array of strings..(or array of
- usbip--局域网内共享的USB设备
- html监控页面大小,JQuery实时监控窗口大小(无需插件)
- 如何发布自己的npm包(超详细步骤,博主都在用)
- 联发科MT6580_datasheet/规格书资料分享
- 深信服下一代防火墙介绍
- 超级账本(版本2.2):编写第一个应用
热门文章
- 13 微积分——级数
- QTreeWidget 勾选三种状态
- ajax 参数data问题 data中的 参数名 参数值为string 提交到后台后,会自动转换参数名相同的 类型 和 js字符串拼接...
- easyrecovery有免费版吗 如何获得easy recovery密钥?easyrecovery激活密钥在哪里
- 20220114软件测试学习第一天
- 对城市名搜索(汉字、拼音)功能的改进 / 小程序城市区县定位模块改进
- python类内部方法调用_python如何调用类中的方法
- Python自动浏览器页面,Mac系统安装Chromedriver
- Microsoft Office Visio Professional 之用例图
- 线性代数计算器C语言程序,新手作品:行列式计算C语言版