基于lio-sam框架,教你如何进行回环检测及位姿计算
摘要:本篇主要解析lio-sam框架下,是如何进行回环检测及位姿计算的。
本文分享自华为云社区《lio-sam框架:回环检测及位姿计算》,作者:月照银海似蛟龙 。
前言
图优化本身有成形的开源的库,例如
- g2o
- ceres
- gtsam
lio-sam 中就是 通过 gtsam 库 进行 图优化的,其中约束因子就包括回环检测因子
本篇主要解析lio-sam框架下,是如何进行回环检测及位姿计算的。
Pose Graph的概念
用一个图(Graph 图论)来表示SLAM问题
图中的节点来表示机器人的位姿 二维的话即为 (x,y,yaw)
两个节点之间的边表示两个位姿的空间约束(相对位姿关系以及对应方差或线性矩阵)
边分为了两种边
- 帧间边:连接的前后,时间上是连续的
- 回环边:连接的前后,时间上是不连续的,但是直接也是两个位姿的空间约束
构建了回环边才会有误差出现,没有回环边是没有误差的
图优化的基本思想:
出现回环边,有了误差之后.构建图,并且找到一个最优的配置(各节点的位姿),让预测与观测的误差最小
一旦形成回环即可进行优化消除误差
里程积分的相对位姿视为预测值 图上的各个节点就是通过里程(激光里程计\轮速里程计)积分得到的
回环计算的相对位姿视为观测值 图上就是说通过 X2和X8的帧间匹配作为观测值
图优化要干的事:
构建图并调整各节点的位姿,让预测与观测的误差最小
回环检测及位姿计算
在点云匹配之后,可以来看回环检测部分的代码了
这部分的代码入口在 main函数中
std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
单独开了一个回环检测的线程
下面来看loopClosureThread这个函数
void loopClosureThread()
{
if (loopClosureEnableFlag == false)
return;
如果不需要进行回环检测,那么就退出这个线程
ros::Rate rate(loopClosureFrequency);
设置回环检测的频率 loopClosureFrequency默认为 1hz
没有必要太频繁
while (ros::ok())
{
rate.sleep();
performLoopClosure();
visualizeLoopClosure();
}
设置完频率后,进行一个while的死循环。
执行完一次就必须sleep一段时间,否则该线程的cpu占用会非常高,通过performLoopClosure visualizeLoopClosure 执行回环检测
下面来看performLoopClosure 函数的具体内容
void performLoopClosure()
{
if (cloudKeyPoses3D->points.empty() == true)
return;
如果没有关键帧,就没法进行回环检测了
就直接退出
mtx.lock();
*copy_cloudKeyPoses3D = *cloudKeyPoses3D;
*copy_cloudKeyPoses6D = *cloudKeyPoses6D;
mtx.unlock();
把存储关键帧额位姿的点云copy出来,避免线程冲突 cloudKeyPoses3D就是关键帧的位置 cloudKeyPoses6D就是关键帧的位姿
if (detectLoopClosureExternal(&loopKeyCur, &loopKeyPre) == false)
首先看一下外部通知的回环信息
if (detectLoopClosureDistance(&loopKeyCur, &loopKeyPre) == false)
return;
然后根据里程计的距离来检测回环
如果还没有则直接返回
来看detectLoopClosureDistance 函数的具体内容
int loopKeyCur = copy_cloudKeyPoses3D->size() - 1;
int loopKeyPre = -1;
检测最新帧是否和其它帧形成回环,取出最新帧的索引
auto it = loopIndexContainer.find(loopKeyCur);
if (it != loopIndexContainer.end())
return false;
检查一下较晚帧是否和别的形成了回环,如果有就算了
因为当前帧刚刚出现,不会和其它帧形成回环,所以基本不会触发
kdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);
把只包含关键帧位移信息的点云填充kdtree
kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);
根据最后一个关键帧的平移信息,寻找离他一定距离内的其它关键帧
historyKeyframeSearchRadius 搜索范围 15m
for (int i = 0; i < (int)pointSearchIndLoop.size(); ++i)
{
遍历找到的候选关键帧
int id = pointSearchIndLoop[i];
if (abs(copy_cloudKeyPoses6D->points[id].time - timeLaserInfoCur) > historyKeyframeSearchTimeDiff)
{
loopKeyPre = id;
break;
}
历史帧,必须比当前帧间隔30s以上
必须满足时间上超过一定阈值,才认为是一个有效的回环
historyKeyframeSearchTimeDiff 时间阈值 30s
如果时间上满足要做就找到了历史回环帧,那么赋值id 并且 break
一次找一个回环帧就行了
if (loopKeyPre == -1 || loopKeyCur == loopKeyPre)
return false;
如果没有找到回环或者回环找到自己身上去了,就认为是本次回环寻找失败
*latestID = loopKeyCur;
*closestID = loopKeyPre;
return true;
}
至此则找到了当真关键帧和历史回环帧
赋值当前帧和历史回环帧的id
如果在一个地方静止不动的时候,那么按照这个逻辑也会形成关键帧,可以通过以关键帧序列号的方式加以改进
如果检测回环存在了,那么则可以进行下面内容,就是计算检测出这两帧的位姿变换
pcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());
声明当前关键帧的点云
声明历史回环帧周围的点云(局部地图)
loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);
当前关键帧把自己取了出来
来看 loopFindNearKeyframes 这个函数
void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& searchNum)
{
for (int i = -searchNum; i <= searchNum; ++i)
{
searchNum 是搜索范围 ,遍历帧的范围
int keyNear = key + i;
找到这个 idx
if (keyNear < 0 || keyNear >= cloudSize )
continue;
如果超出范围了就算了
*nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[keyNear], ©_cloudKeyPoses6D->points[keyNear]);
*nearKeyframes += *transformPointCloud(surfCloudKeyFrames[keyNear], ©_cloudKeyPoses6D->points[keyNear]);
否则吧对应角点和面点的点云转到世界坐标系下去
if (nearKeyframes->empty())
return;
如果没有有效的点云就算了
pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());
downSizeFilterICP.setInputCloud(nearKeyframes);
downSizeFilterICP.filter(*cloud_temp);
*nearKeyframes = *cloud_temp;
吧点云下采样
然后会到之前的地方:
loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);
回环帧把自己周围一些点云取出来,也就是构成一个帧局部地图的一个匹配问题
historyKeyframeSearchNum 25帧
if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)
return;
如果点云数目太少就算了
if (pubHistoryKeyFrames.getNumSubscribers() != 0)
publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);
把局部地图发布出来供rviz可视化使用
现在有了当前关键帧投到地图坐标系下的点云和历史回环帧投到地图坐标系下的局部地图,那么接下来就可以进行两者的icp位姿变换求解
static pcl::IterativeClosestPoint<PointType, PointType> icp;
使用简单的icp来进行帧到局部地图的配准
icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);
设置最大相关距离
historyKeyframeSearchRadius 15m
icp.setMaximumIterations(100);
最大优化次数
icp.setTransformationEpsilon(1e-6);
单次变换范围
icp.setEuclideanFitnessEpsilon(1e-6);
icp.setRANSACIterations(0);
残差设置
icp.setInputSource(cureKeyframeCloud);
icp.setInputTarget(prevKeyframeCloud);
设置两个点云
pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());
icp.align(*unused_result);
执行配准
if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)
return;
检测icp是否收敛 且 得分是否满足要求
if (pubIcpKeyFrames.getNumSubscribers() != 0)
{
pcl::PointCloud<PointType>::Ptr closed_cloud(new pcl::PointCloud<PointType>());
pcl::transformPointCloud(*cureKeyframeCloud, *closed_cloud, icp.getFinalTransformation());
publishCloud(&pubIcpKeyFrames, closed_cloud, timeLaserInfoStamp, odometryFrame);
}
把修正后的当前点云发布供可视化使用
correctionLidarFrame = icp.getFinalTransformation();
获得两个点云的变换矩阵结果
Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);
取出当前帧的位姿
Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;
将icp结果补偿过去,就是当前帧的更为准确的位姿结果
pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);
将当前帧补偿后的位姿 转换成 平移和旋转
gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));
gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);
将当前帧补偿后的位姿 转换成 gtsam的形式
From 和 To相当于帧间约束的因子,To是历史回环帧的位姿
gtsam::Vector Vector6(6);
float noiseScore = icp.getFitnessScore();
noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);
使用icp的得分作为他们的约束噪声项
loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));//两帧索引
loopPoseQueue.push_back(poseFrom.between(poseTo));//当前帧与历史回环帧相对位姿
loopNoiseQueue.push_back(constraintNoise);//噪声
将两帧索引,两帧相对位姿和噪声作为回环约束 送入对列
loopIndexContainer[loopKeyCur] = loopKeyPre;
保存已经存在的约束对
总结
lio-sam回环检测的方式
构建关键帧,将关键帧的位姿存储。以固定频率进行回环检测。每次处理最新的关键帧,通过kdtree寻找历史关键帧中距离和时间满足条件的一个关键帧。然后就认为形成了回环。
形成回环后,历史帧周围25帧,构建局部地图,与当前关键帧进行icp匹配求解位姿变换。
lio-sam 认为里程计累计漂移比较小,所以通过距离与时间这两个概念进行的关键帧的回环检测。
基于lio-sam框架,教你如何进行回环检测及位姿计算的更多相关文章
- segMatch:基于3D点云分割的回环检测
该论文的地址是:https://arxiv.org/pdf/1609.07720.pdf segmatch是一个提供车辆的回环检测的技术,使用提取和匹配分割的三维激光点云技术.分割的例子可以在下面的图 ...
- 一个基于深度学习回环检测模块的简单双目 SLAM 系统
转载请注明出处,谢谢 原创作者:Mingrui 原创链接:https://www.cnblogs.com/MingruiYu/p/12634631.html 写在前面 最近在搞本科毕设,关于基于深度学 ...
- 基于Java Netty框架构建高性能的部标808协议的GPS服务器
使用Java语言开发一个高质量和高性能的jt808 协议的GPS通信服务器,并不是一件简单容易的事情,开发出来一段程序和能够承受数十万台车载接入是两码事,除去开发部标808协议的固有复杂性和几个月长周 ...
- 基于Typecho CMS框架开发大中型应用
基于Typecho CMS框架开发大中型应用 大中型应用暂且定义为:大于等于3个数据表的应用!汗吧! Typecho原本是一款博客系统,其框架体系有别于市面上一般意义MVC框架,主体代码以自创的Wid ...
- 基于AForge.Net框架的扑克牌识别
原文:基于AForge.Net框架的扑克牌识别 © 版权所有 野比 2012 原文地址:点击查看 作者:Nazmi Altun Nazmi Altun著,野比 译 下载源代码 - 148.61 KB ...
- revel框架教程之CSRF(跨站请求伪造)保护
revel框架教程之CSRF(跨站请求伪造)保护 CSRF是什么?请看这篇博文“浅谈CSRF攻击方式”,说的非常清楚. 现在做网站敢不防CSRF的我猜只有两种情况,一是没什么人访问,二是局域网应用.山 ...
- 基于BrokerPattern服务器框架
基于BrokerPattern服务器框架 RedRabbit 经典网游服务器架构 该图省略了专门用途的dbserver.guildserver等用于专门功能的server,该架构的优点有: l Log ...
- 手工搭建基于ABP的框架(2) - 访问数据库
为了防止不提供原网址的转载,特在这里加上原文链接: http://www.cnblogs.com/skabyy/p/7517397.html 本篇我们实现数据库的访问.我们将实现两种数据库访问方法来访 ...
- 手工搭建基于ABP的框架 - 工作单元以及事务管理
一个业务功能往往不只由一次数据库请求(或者服务调用)实现.为了功能的完整性,我们希望如果该功能执行一半时出错,则撤销前面已执行的改动.在数据库层面上,事务管理实现了这种完整性需求.在ABP中,一个完整 ...
- 基于Kafka Connect框架DataPipeline可以更好地解决哪些企业数据集成难题?
DataPipeline已经完成了很多优化和提升工作,可以很好地解决当前企业数据集成面临的很多核心难题. 1. 任务的独立性与全局性. 从Kafka设计之初,就遵从从源端到目的的解耦性.下游可以有很多 ...
随机推荐
- 分享一个Redis自带的压测工具:redis-benchmark
前言 今天给xdm分享一个Redis自带的压测工具:redis-benchmark. 介绍 redis-benchmark 是一个用于测试 Redis性能的基准测试工具,可以帮助开发人员评估和比较 R ...
- Unity - Windows获取屏幕分辨率、可用区域
直接搜索最多的就是使用System.Windows.Form.Screen类,但因为unity用的是mono,不能正常使用这个方法 可使用win32api获取,这里只尝试了获取主要屏幕的分辨率,而且没 ...
- 合唱队形(lgP1091)
思路: 先从左到右求一遍最长不下降子序列,再同样方法从右到左求一遍. 然后我们枚举分界点,则总人数就是左边一半加上右边一半的人数. 取最大值,输出答案. 见注释. #include<bits/s ...
- mysql查看索引利用率
-- mysql查看索引利用率 -- 如果很慢把排序去掉,加上limit 并且在where条件中限定表名. -- cardinality越接近0,利用率越低 SELECT t.TABLE_SCHEMA ...
- arm架构docker安装nacos
前言 搞了个hk1box,装了armbian系统,想用这个当服务器调试微服务,需要安装nacos.尝试安装非docker版本的nacos,去github下载arm版本的并且放到linux下面,运行的时 ...
- 夯实JAVA基本之一——泛型详解(2):高级进阶(转)
上一篇给大家初步讲解了泛型变量的各种应用环境,这篇将更深入的讲解一下有关类型绑定,通配符方面的知识. 一.类型绑定1.引入我们重新看上篇写的一个泛型:class Point<T> { pr ...
- 快速教程|如何在 AWS EC2上使用 Walrus 部署 GitLab
Walrus 是一款基于平台工程理念的开源应用管理平台,致力于解决应用交付领域的深切痛点.借助 Walrus 将云原生的能力和最佳实践扩展到非容器化环境,并支持任意应用形态统一编排部署,降低使用基础设 ...
- 使用 Hexo 搭建个人博客并部署到云服务器
目录 1 整体流程 2. 本地环境准备 2.1 安装 Node.js 和 Git 2.2 安装 Hexo 3. 服务端环境准备 3.1 Nginx 环境配置 3.1.1 安装 Nginx 3.1.2 ...
- 基于 TCP 协议写的 FTP 管理小工具
这是一个FTP(文件传输协议)管理工具,能够支持文件上传下载以及操作服务端的文件. 该工具由客户端和服务端组成.客户端与服务端通过Socket连接实现通信,客户端发送命令,服务端解析并执行相应的操作. ...
- Linux下^m符号删除
Linux下^m符号删除 从Windows上复制的代码到Linux尾会有M字符,通过下命令可以删除. :%s/\r//