写点什么

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

  • 2022 年 9 月 14 日
    中国香港
  • 本文字数:4324 字

    阅读完需:约 14 分钟

基于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 认为里程计累计漂移比较小,所以通过距离与时间这两个概念进行的关键帧的回环检测


点击关注,第一时间了解华为云新鲜技术~


发布于: 刚刚阅读数: 3
用户头像

提供全面深入的云计算技术干货 2020.07.14 加入

华为云开发者社区,提供全面深入的云计算前景分析、丰富的技术干货、程序样例,分享华为云前沿资讯动态,方便开发者快速成长与发展,欢迎提问、互动,多方位了解云计算! 传送门:https://bbs.huaweicloud.com/

评论

发布
暂无评论
基于lio-sam框架,教你如何进行回环检测及位姿计算_人工智能_华为云开发者联盟_InfoQ写作社区