草庐IT

基于lio-sam框架，教你如何进行回环检测及位姿计算

华为云开发者社区 2023-03-28 原文
摘要：本篇主要解析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], &copy_cloudKeyPoses6D->points[keyNear]);
 *nearKeyframes += *transformPointCloud(surfCloudKeyFrames[keyNear], &copy_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-samspancolorstyle人工智能

有关基于lio-sam框架，教你如何进行回环检测及位姿计算的更多相关文章

  1. ruby - 如何使用 Nokogiri 的 xpath 和 at_xpath 方法 - 2

    我正在学习如何使用Nokogiri，根据这段代码我遇到了一些问题:require'rubygems'require'mechanize'post_agent=WWW::Mechanize.newpost_page=post_agent.get('http://www.vbulletin.org/forum/showthread.php?t=230708')puts"\nabsolutepathwithtbodygivesnil"putspost_page.parser.xpath('/html/body/div/div/div/div/div/table/tbody/tr/td/div

  2. ruby - 如何从 ruby​​ 中的字符串运行任意对象方法？ - 2

    总的来说，我对ruby​​还比较陌生，我正在为我正在创建的对象编写一些rspec测试用例。许多测试用例都非常基础，我只是想确保正确填充和返回值。我想知道是否有办法使用循环结构来执行此操作。不必为我要测试的每个方法都设置一个assertEquals。例如:describeitem,"TestingtheItem"doit"willhaveanullvaluetostart"doitem=Item.new#HereIcoulddotheitem.name.shouldbe_nil#thenIcoulddoitem.category.shouldbe_nilendend但我想要一些方法来使用

  3. ruby-on-rails - 使用 Ruby on Rails 进行自动化测试 - 最佳实践 - 2

    很好奇，就使用ruby​​onrails自动化单元测试而言，你们正在做什么？您是否创建了一个脚本来在cron中运行rake作业并将结果邮寄给您？git中的预提交Hook？只是手动调用？我完全理解测试，但想知道在错误发生之前捕获错误的最佳实践是什么。让我们理所当然地认为测试本身是完美无缺的，并且可以正常工作。下一步是什么以确保他们在正确的时间将可能有害的结果传达给您？ 最佳答案 不确定您到底想听什么，但是有几个级别的自动代码库控制:在处理某项功能时，您可以使用类似autotest的内容获得关于哪些有效，哪些无效的即时反馈。要确保您的提

  4. python - 如何使用 Ruby 或 Python 创建一系列高音调和低音调的蜂鸣声？ - 2

    关闭。这个问题是opinion-based.它目前不接受答案。想要改进这个问题？更新问题，以便editingthispost可以用事实和引用来回答它.关闭4年前。Improvethisquestion我想在固定时间创建一系列低音和高音调的哔哔声。例如:在150毫秒时发出高音调的蜂鸣声在151毫秒时发出低音调的蜂鸣声200毫秒时发出低音调的蜂鸣声250毫秒的高音调蜂鸣声有没有办法在Ruby或Python中做到这一点？我真的不在乎输出编码是什么(.wav、.mp3、.ogg等等)，但我确实想创建一个输出文件。

  5. ruby-on-rails - 如何验证 update_all 是否实际在 Rails 中更新 - 2

    给定这段代码defcreate@upgrades=User.update_all(["role=?","upgraded"],:id=>params[:upgrade])redirect_toadmin_upgrades_path,:notice=>"Successfullyupgradeduser."end我如何在该操作中实际验证它们是否已保存或未重定向到适当的页面和消息？ 最佳答案 在Rails3中，update_all不返回任何有意义的信息，除了已更新的记录数(这可能取决于您的DBMS是否返回该信息)。http://ar.ru

  6. ruby-on-rails - 按天对 Mongoid 对象进行分组 - 2

    在控制台中反复尝试之后，我想到了这种方法，可以按发生日期对类似activerecord的(Mongoid)对象进行分组。我不确定这是完成此任务的最佳方法，但它确实有效。有没有人有更好的建议，或者这是一个很好的方法？#eventsisanarrayofactiverecord-likeobjectsthatincludeatimeattributeevents.map{|event|#converteventsarrayintoanarrayofhasheswiththedayofthemonthandtheevent{:number=>event.time.day,:event=>ev

  7. ruby-on-rails - 'compass watch' 是如何工作的/它是如何与 rails 一起使用的 - 2

    我在我的项目目录中完成了compasscreate.和compassinitrails。几个问题:我已将我的.sass文件放在public/stylesheets中。这是放置它们的正确位置吗？当我运行compasswatch时，它不会自动编译这些.sass文件。我必须手动指定文件:compasswatchpublic/stylesheets/myfile.sass等。如何让它自动运行？文件ie.css、print.css和screen.css已放在stylesheets/compiled。如何在编译后不让它们重新出现的情况下删除它们？我自己编译的.sass文件编译成compiled/t

  8. ruby - 如何将脚本文件的末尾读取为数据文件(Perl 或任何其他语言) - 2

    我正在寻找执行以下操作的正确语法(在Perl、Shell或Ruby中):#variabletoaccessthedatalinesappendedasafileEND_OF_SCRIPT_MARKERrawdatastartshereanditcontinues. 最佳答案 Perl用__DATA__做这个:#!/usr/bin/perlusestrict;usewarnings;while(){print;}__DATA__Texttoprintgoeshere 关于ruby-如何将脚

  9. ruby - 如何指定 Rack 处理程序 - 2

    Rackup通过Rack的默认处理程序成功运行任何Rack应用程序。例如:classRackAppdefcall(environment)['200',{'Content-Type'=>'text/html'},["Helloworld"]]endendrunRackApp.new但是当最后一行更改为使用Rack的内置CGI处理程序时，rackup给出“NoMethodErrorat/undefinedmethod`call'fornil:NilClass”:Rack::Handler::CGI.runRackApp.newRack的其他内置处理程序也提出了同样的反对意见。例如Rack

  10. ruby - 如何每月在 Heroku 运行一次 Scheduler 插件？ - 2

    在选择我想要运行操作的频率时，唯一的选项是“每天”、“每小时”和“每10分钟”。谢谢!我想为我的Rails3.1应用程序运行调度程序。 最佳答案 这不是一个优雅的解决方案，但您可以安排它每天运行，并在实际开始工作之前检查日期是否为当月的第一天。 关于ruby-如何每月在Heroku运行一次Scheduler插件？，我们在StackOverflow上找到一个类似的问题： https://stackoverflow.com/questions/8692687/

随机推荐