LIO-SAM后端中的回环检测及位姿计算
2024-06-01 21:25:54
文章目录
- performLoopClosure()
- 寻找回环关键帧detectLoopClosureDistance
- 检出回环之后开始计算两帧位姿变换
回环检测函数调用在mapOptmization.cpp 的main中单独开了一个线程运行，1秒运行一遍回环检测
std::thread loopthread(&mapOptimization::loopClosureThread, &MO);
回环检测思路描述：主要是通过里程计判断回环，根据最后一个关键帧的平移信息，寻找离他15米内的其它关键帧，并且两帧的时间戳相差要大于30s，上面两个条件都满足则认为是找到了回环帧，开始用icp匹配计算位姿，算完后把两帧索引，两帧相对位姿，噪声（icp得分）放入回环约束队列中，对回环因子的使用，和因子图的更新在函数saveKeyFramesAndFactor()中，后面会详解
流程综述：
- 如果不需要进行回环检测就退出这个线程，有标志位设置
// 回环检测线程void loopClosureThread(){// 如果不需要进行回环检测，那么就退出这个线程if (loopClosureEnableFlag == false)return;// 设置回环检测的频率ros::Rate rate(loopClosureFrequency);while (ros::ok()){// 执行完一次就必须sleep一段时间，否则该线程的cpu占用会非常高rate.sleep();// 执行回环检测performLoopClosure();visualizeLoopClosure();}}
- 执行回环检测
performLoopClosure()
- 如果没有关键帧就无法退出回环检测
if (cloudKeyPoses3D->points.empty() == true)return;
- 把存储关键帧的位姿的点云copy出来，避免线程冲突，这里开了把锁防止数据调用冲突
mtx.lock();
// 把存储关键帧的位姿的点云copy出来，避免线程冲突
*copy_cloudKeyPoses3D = *cloudKeyPoses3D;
*copy_cloudKeyPoses6D = *cloudKeyPoses6D;
mtx.unlock()
寻找回环关键帧detectLoopClosureDistance
主要是通过里程计判断回环，根据最后一个关键帧的平移信息，寻找离他15米内的其它关键帧，并且两帧的时间戳相差要大于30s，上面两个条件都满足则认为是找到了回环帧
bool detectLoopClosureDistance(int *latestID, int *closestID){// 检测最新帧是否和其他帧形成回环，所以后面一帧的id就是最后一个关键帧int loopKeyCur = copy_cloudKeyPoses3D->size() - 1;int loopKeyPre = -1;// check loop constraint added before// 检查一下较晚帧是否和别的形成了回环，如果有就算了auto it = loopIndexContainer.find(loopKeyCur);if (it != loopIndexContainer.end())return false;// find the closest history key framestd::vector<int> pointSearchIndLoop;std::vector<float> pointSearchSqDisLoop;// 把只包含关键帧位移信息的点云填充kdtreekdtreeHistoryKeyPoses->setInputCloud(copy_cloudKeyPoses3D);// 根据最后一个关键帧的平移信息，寻找离他一定距离内的其他关键帧kdtreeHistoryKeyPoses->radiusSearch(copy_cloudKeyPoses3D->back(), historyKeyframeSearchRadius, pointSearchIndLoop, pointSearchSqDisLoop, 0);// 遍历找到的候选关键帧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;}}// 如果没有找到回环或者回环找到自己身上去了，就认为是此次回环寻找失败if (loopKeyPre == -1 || loopKeyCur == loopKeyPre)return false;*latestID = loopKeyCur;*closestID = loopKeyPre;return true;}
检出回环之后开始计算两帧位姿变换
- 把回环帧的前后各25帧都取出来，共51帧，构建一个局部点云地图，然后用当前帧与这个局部点云地图进行匹配
- 构建回环帧的局部地图用loopFindNearKeyframes()
void loopFindNearKeyframes(pcl::PointCloud<PointType>::Ptr& nearKeyframes, const int& key, const int& searchNum){// extract near keyframesnearKeyframes->clear();int cloudSize = copy_cloudKeyPoses6D->size();// searchNum是搜索范围for (int i = -searchNum; i <= searchNum; ++i){// 找到这个idxint keyNear = key + i;// 如果超出范围就算了if (keyNear < 0 || keyNear >= cloudSize )continue;// 否则把对应角点和面点的点云转到世界坐标系下去*nearKeyframes += *transformPointCloud(cornerCloudKeyFrames[keyNear], ©_cloudKeyPoses6D->points[keyNear]);*nearKeyframes += *transformPointCloud(surfCloudKeyFrames[keyNear], ©_cloudKeyPoses6D->points[keyNear]);}// 如果没有有效的点云就算了if (nearKeyframes->empty())return;// downsample near keyframes// 把点云下采样pcl::PointCloud<PointType>::Ptr cloud_temp(new pcl::PointCloud<PointType>());downSizeFilterICP.setInputCloud(nearKeyframes);downSizeFilterICP.filter(*cloud_temp);*nearKeyframes = *cloud_temp;}
- 将当前帧的点云cureKeyframeCloud和回环帧的局部地图点云prevKeyframeCloud加入icp的类中，直接用库进行点云配准，然后检查icp的收敛得分是否满足要求和把修正后的当前点云发布供可视化使用
- icp匹配如下图，一般情况下的正确匹配是红色圈圈，绿色圈圈是显示匹配到最近距离的其它点的情况，但是通过内部得分优化，会去掉绿色这个错误的匹配，这些优化都在pcl的icp库里面自己进行
// 设置两个点云icp.setInputSource(cureKeyframeCloud);icp.setInputTarget(prevKeyframeCloud);pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());// 执行点云配准icp.align(*unused_result);// 检查icp是否收敛且得分是否满足要求if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)return;// publish corrected cloud// 把修正后的当前点云发布供可视化使用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);}
- 获取当前帧到局部地图的位姿变换矩阵结果Tcorrect−curT_{correct-cur}Tcorrect−cur，再获取当前帧的世界坐标的位姿Tcur−wT_{cur-w}Tcur−w，然后Tcorrect−cur∗Tcur−wT_{correct-cur}*T_{cur-w}Tcorrect−cur∗Tcur−w补偿后获取当前帧正确的位姿结果
// Get pose transformationfloat x, y, z, roll, pitch, yaw;Eigen::Affine3f correctionLidarFrame;// 获得两个点云的变换矩阵结果correctionLidarFrame = icp.getFinalTransformation();// transform from world origin to wrong pose// 找到稍早点云的位姿结果Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);// transform from world origin to corrected pose// 将icp的结果补偿过去，就是稍晚帧的更为准确的位姿结果Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;// pre-multiplying -> successive rotation about a fixed frame// 将稍晚帧的位姿转成平移+欧拉角pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);
- 然后将两帧索引（当前帧cur和回环帧pre），校正后的两帧相对位姿，噪声（icp的得分作为噪声）加入回环约束队列中，使得整个图根据最优值进行全局误差优化
其中校正后的相对位姿计算，回环帧为Tpre−wT_{pre-w}Tpre−w，校正后的当前帧为Tcorrect−wT_{correct-w}Tcorrect−w，Tcorrect−pre=Tcorrect−w∗Tpre−w−1T_{correct-pre}=T_{correct-w}*T_{pre-w}^{-1}Tcorrect−pre=Tcorrect−w∗Tpre−w−1
// from是修正后的稍晚帧的点云位姿gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));// to是修正前的稍早帧的点云位姿gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);gtsam::Vector Vector6(6);// 使用icp的得分作为他们的约束的噪声项float noiseScore = icp.getFitnessScore();Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);// Add pose constraintmtx.lock();// 将两帧索引，两帧相对位姿和噪声作为回环约束送入队列loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));loopPoseQueue.push_back(poseFrom.between(poseTo));loopNoiseQueue.push_back(constraintNoise);mtx.unlock();// add loop constriant// 保存已存在的约束对loopIndexContainer[loopKeyCur] = loopKeyPre;
对回环因子的使用，和因子图的更新在函数saveKeyFramesAndFactor()中，后面会详解
整体代码如下：
void performLoopClosure(){// 如果没有关键帧，就没法进行回环检测了if (cloudKeyPoses3D->points.empty() == true)return;mtx.lock();// 把存储关键帧的位姿的点云copy出来，避免线程冲突*copy_cloudKeyPoses3D = *cloudKeyPoses3D;*copy_cloudKeyPoses6D = *cloudKeyPoses6D;mtx.unlock();// find keysint loopKeyCur;int loopKeyPre;// 首先看一下外部通知的回环信息if (detectLoopClosureExternal(&loopKeyCur, &loopKeyPre) == false)// 然后根据里程记的距离来检测回环if (detectLoopClosureDistance(&loopKeyCur, &loopKeyPre) == false)return;// 检出回环之后开始计算两帧位姿变换// extract cloudpcl::PointCloud<PointType>::Ptr cureKeyframeCloud(new pcl::PointCloud<PointType>());pcl::PointCloud<PointType>::Ptr prevKeyframeCloud(new pcl::PointCloud<PointType>());{// 稍晚的帧就把自己取了出来loopFindNearKeyframes(cureKeyframeCloud, loopKeyCur, 0);// 稍早一点的就把自己和周围一些点云取出来，也就是构成一个帧到局部地图的一个匹配问题loopFindNearKeyframes(prevKeyframeCloud, loopKeyPre, historyKeyframeSearchNum);// 如果点云数目太少就算了if (cureKeyframeCloud->size() < 300 || prevKeyframeCloud->size() < 1000)return;if (pubHistoryKeyFrames.getNumSubscribers() != 0)// 把局部地图发布出去供rviz可视化使用publishCloud(&pubHistoryKeyFrames, prevKeyframeCloud, timeLaserInfoStamp, odometryFrame);}// ICP Settings// 使用简单的icp来进行帧到局部地图的配准static pcl::IterativeClosestPoint<PointType, PointType> icp;icp.setMaxCorrespondenceDistance(historyKeyframeSearchRadius*2);icp.setMaximumIterations(100);icp.setTransformationEpsilon(1e-6);icp.setEuclideanFitnessEpsilon(1e-6);icp.setRANSACIterations(0);// Align clouds// 设置两个点云icp.setInputSource(cureKeyframeCloud);icp.setInputTarget(prevKeyframeCloud);pcl::PointCloud<PointType>::Ptr unused_result(new pcl::PointCloud<PointType>());// 执行点云配准icp.align(*unused_result);// 检查icp是否收敛且得分是否满足要求if (icp.hasConverged() == false || icp.getFitnessScore() > historyKeyframeFitnessScore)return;// publish corrected cloud// 把修正后的当前点云发布供可视化使用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);}// Get pose transformationfloat x, y, z, roll, pitch, yaw;Eigen::Affine3f correctionLidarFrame;// 获得两个点云的变换矩阵结果correctionLidarFrame = icp.getFinalTransformation();// transform from world origin to wrong pose// 找到稍早点云的位姿结果Eigen::Affine3f tWrong = pclPointToAffine3f(copy_cloudKeyPoses6D->points[loopKeyCur]);// transform from world origin to corrected pose// 将icp的结果补偿过去，就是稍晚帧的更为准确的位姿结果Eigen::Affine3f tCorrect = correctionLidarFrame * tWrong;// pre-multiplying -> successive rotation about a fixed frame// 将稍晚帧的位姿转成平移+欧拉角pcl::getTranslationAndEulerAngles (tCorrect, x, y, z, roll, pitch, yaw);// from是修正后的稍晚帧的点云位姿gtsam::Pose3 poseFrom = Pose3(Rot3::RzRyRx(roll, pitch, yaw), Point3(x, y, z));// to是修正前的稍早帧的点云位姿gtsam::Pose3 poseTo = pclPointTogtsamPose3(copy_cloudKeyPoses6D->points[loopKeyPre]);gtsam::Vector Vector6(6);// 使用icp的得分作为他们的约束的噪声项float noiseScore = icp.getFitnessScore();Vector6 << noiseScore, noiseScore, noiseScore, noiseScore, noiseScore, noiseScore;noiseModel::Diagonal::shared_ptr constraintNoise = noiseModel::Diagonal::Variances(Vector6);// Add pose constraintmtx.lock();// 将两帧索引，两帧相对位姿和噪声作为回环约束送入队列loopIndexQueue.push_back(make_pair(loopKeyCur, loopKeyPre));loopPoseQueue.push_back(poseFrom.between(poseTo));loopNoiseQueue.push_back(constraintNoise);mtx.unlock();// add loop constriant// 保存已存在的约束对loopIndexContainer[loopKeyCur] = loopKeyPre;}
LIO-SAM后端中的回环检测及位姿计算相关推荐
- lio-sam框架：回环检测及位姿计算
lio-sam框架:回环检测及位姿计算 前言 Pose Graph的概念 回环检测及位姿计算 总结 前言 图优化本身有成形的 开源的库 例如 g2o ceres gtsam lio-sam 中就是 通 ...
- Karto的后端优化与回环检测功能对比测试与分析
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达 上篇文章讲解了Karto的前端是如何工作的. 这篇文章将slam_karto中的后端优化部分的代码添加 ...
- SC-A-LOAM：在A-LOAM中加入回环检测
Thanks to LOAM, A-LOAM, and LIO-SAM code authors. The major codes in this repository are borrowed fr ...
- SLAM前端中的视觉里程计和回环检测
1. 通常的惯例是把 VSLAM 分为前端和后端.前端为视觉里程计和回环检测,相当于是对图像数据进行关联:后端是对前端输出的结果进行优化,利用滤波或非线性优化理论,得到最优的位姿估计和全局一致性地图. ...
- SLAM之视觉里程计和回环检测
1. 通常的惯例是把 VSLAM 分为前端和后端.前端为视觉里程计和回环检测,相当于是对图像数据进行关联:后端是对前端输出的结果进行优化,利用滤波或非线性优化理论,得到最优的位姿估计和全局一致性地图. ...
- 一文详解回环检测与重定位
标题:VINS-Mono代码解读-回环检测与重定位 pose graph loop closing 作者:Manii 来源:https://blog.csdn.net/qq_41839222/cate ...
- 最全综述 | SLAM中回环检测方法 收藏
在视觉SLAM问题中,位姿的估计往往是一个递推的过程,即由上一帧位姿解算当前帧位姿,因此其中的误差便这样一帧一帧的传递下去,也就是我们所说的累积误差.一个消除误差有效的办法是进行回环检测.回环检测判断 ...
- SLAM笔记（七）回环检测中的词袋BOW
1.词频 (摘自阮一峰博客,参见附录参考) 如果某个词很重要,它应该在这篇文章中多次出现.于是,我们进行"词频"(Term Frequency,缩写为TF)统计.考虑到文章有长短之 ...
- 【SLAM十四讲】ch11 回环检测 词袋法实验 得出相似分数后计算PR曲线 VPR实验 编辑中
[SLAM十四讲]ch11 回环检测 词袋法实验 得出相似分数后计算PR曲线 [SLAM十四讲]ch11 回环检测 词袋法实验 得出相似分数后计算PR曲线 DBow3库安装 ch11编译 ch11 词 ...
- M2DP：一种新的三维点云描述子及其在回环检测中的应用
文章:M2DP: A Novel 3D Point Cloud Descriptor and Its Application in Loop Closure Detection 作者:Li He , ...
最新文章
- HTML5的本地存储详解
- 【微信小程序】：实现轮播图3秒滚动
- oracle编程艺术在线,oracle编程艺术笔记-1
- mysql约束sex_MySQL笔记--约束
- php 爬虫 类,php爬虫原型
- 循环-20. 猜数字游戏(15)
- cvs数据导入工具 oracle_为中国企业打造的研发项目管理工具ONES Project已支持Jira数据导入...
- 由m种数字组成的n位数有多少个
- NOIP2016提高A组 B题 【HDU3072】【JZOJ4686】通讯
- 手机上编程python的软件_盘点几个在手机上可以用来学习编程的软件
- 局域网网站服务器dns设置,局域网设置dns的方法
- 华硕台式计算机编号,怎么查看华硕电脑设备序列号
- This Python interpreter is in a conda environment, but the environment hasnot been activated. 如何解决？
- JS与CSS实现区域内容自动左右滑动
- 大学应用计算机二级,大学计算机二级ps考试试题内容(3)
- wxPython的界面设计wxformbuilde初学笔记
- Photoshop cc 2018基础
- 安装卡巴斯基个人网络版KIS 7.0之后的蓝屏解决方案
- Kubernetes安装系列之kubctl exec权限设定
- 1.Postman之发送get请求