基于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框架,教你如何进行回环检测及位姿计算相关推荐
- lio-sam框架:回环检测及位姿计算
lio-sam框架:回环检测及位姿计算 前言 Pose Graph的概念 回环检测及位姿计算 总结 前言 图优化本身有成形的 开源的库 例如 g2o ceres gtsam lio-sam 中就是 通 ...
- segMatch:基于3D点云分割的回环检测
该论文的地址是:https://arxiv.org/pdf/1609.07720.pdf segmatch是一个提供车辆的回环检测的技术,使用提取和匹配分割的三维激光点云技术.分割的例子可以在下面的图 ...
- 经典激光slam配准及回环检测框架:ScanContext
论文名称: Scan context: Egocentric spatial descriptor for place recognition within 3d point cloud map 论文 ...
- lio-sam框架:后端里程计、回环、gps融合
lio-sam框架:后端里程计.回环.gps融合 前言 代码解析 添加激光雷达帧间里程计因子 添加GPS因子 添加回环检测因子 isam 更新 调整全局轨迹 总结 前言 LIO-SAM的全称是:Tig ...
- 基于EP4CE10F17C8的以太网数据回环(UDP)
目录 一.理论知识(部分来自野火.正点原子资料) (一)TCP/IP五层模型 (二)以太网(自底向上描述) 1.物理层 2.MAC子层 3.网络层(IP.ICMP.ARP) 4.传输层(TCP.UDP ...
- 基于 2D 激光雷达和实时回环优化的 SLAM 算法
基于 2D 激光雷达和实时回环优化的 SLAM 算法 原文: https://www.ixueshu.com/document/771d692c7d3c0c40318947a18e7f9386.htm ...
- 基于Java毕业设计早教课程管理系统源码+系统+mysql+lw文档+部署软件
基于Java毕业设计早教课程管理系统源码+系统+mysql+lw文档+部署软件 基于Java毕业设计早教课程管理系统源码+系统+mysql+lw文档+部署软件 本源码技术栈: 项目架构:B/S架构 开 ...
- 基于java ssm框架实大学生心理健康系统设计与开发演示【附项目源码+论文说明】
基于java ssm框架实大学生心理健康系统设计与开发演示 摘要 近些年来,我国高等教育规模不断扩大,在校大学生人数屡创新高,大学生心理健康问题也日益严峻.虽然各高等学校对心理健康教育的重视程度较高, ...
- 源码推荐:基于uni-app前端框架,开源版本还开源免费商用
今天要给大家介绍一款电商软件,目前有两个主流版本:免费开源版.商业开源版.首先需要和大家普及下什么是开源软件? 提到开源,一定绕不开Linux.Linux 是一款开源软件,我们可以随意浏览和修改它的源 ...
- 基于ACE Proactor框架下高并发、大容量吞吐程序设计既最近的一个产品开发总结
Reactor与 Proactor 基本概念 在高性能的I/O设计中,有两个比较著名的模式Reactor和Proactor模式,其中Reactor模式用于同步I/O,而Proactor运用于异步I/O ...
最新文章
- 深入理解Spring系列之六:bean初始化
- Mybatis学习错误之:重复加载mapper.xml
- php mysql生成excel文件,PHP导出MySQL数据到Excel文件简单示例
- 104. Maximum Depth of Binary Tree 二叉树的最大深度
- qt-项目部署(某些情况下编译器中运行异常的话可以使用命令windeployqt 程序名在安装的qt dos命令下补全部分依赖库在运行项目或发布)
- Spark streaming java代码
- 解决ssh使用一段时间断开的问题
- Java 8 –按值对HashMap进行升序和降序排序
- Spring Boot中表格的请求以及表格界面的显示
- 5个数中取三个数组合 不重复 php,PHP产生不重复随机数的5个方法总结
- TCP协议的三次握手+四次断开
- 树莓派C语言超声波传感器测距
- Adobe Illustrator CS6 已停止工作 的问题
- Linux下安装docker与kubernetes(k8s)
- shp文件中polyline是什么_polyline怎么读用法大全_polyline是什么意思
- MTD系列 - 关于linux MTD的一些理解
- 二叉树的非递归遍历和递归遍历
- Winsock 函数
- 满分作文生成器网页版
- Discuz!论坛教程之修改admin.php防止直接恶意访问
热门文章
- Excel表格之道 学习笔记(四)
- Leetcode. Largest Rectangle in Histogram
- C语言解决找零钱问题
- 小众但口碑好的便签软件
- maxscript rollout
- [网络安全自学篇] 七十八.XSS跨站脚本攻击案例分享及总结(二)
- Python实战——外星人入侵游戏
- latch mysql_MySQL中的latch(閂鎖)詳解——易產生的問題以及原因分析
- SAP Excel 已完成文件级验证和修复。此工作簿的某些部分可能已被修复或丢弃。
- 活久见:都 2203 年了,你还在使用 word 调试 API