天天看點
傳回

基于lio-sam架構，教你如何進行回環檢測及位姿計算

作者：華為雲開發者聯盟

本文分享自華為雲社群《lio-sam架構：回環檢測及位姿計算-雲社群-華為雲》，作者：月照銀海似蛟龍 。

前言

圖優化本身有成形的 開源的庫 例如

  • g2o
  • ceres
  • gtsam

lio-sam 中就是 通過 gtsam 庫 進行 圖優化的，其中限制因子就包括回環檢測因子

本篇主要解析lio-sam架構下，是如何進行回環檢測及位姿計算的。

Pose Graph的概念

用一個圖(Graph 圖論)來表示SLAM問題

基于lio-sam架構，教你如何進行回環檢測及位姿計算

圖中的節點來表示機器人的位姿 二維的話即為 (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 認為裡程計累計漂移比較小，是以通過距離與時間這兩個概念進行的關鍵幀的回環檢測

點選下方，第一時間了解華為雲新鮮技術~

華為雲部落格_大資料部落格_AI部落格_雲計算部落格_開發者中心-華為雲

如何進行
上一篇: 百戰程式員--IDLE開發環境使用入門
下一篇: Nature Plants | 作物的瞬時重程式設計以提高農藝性狀表現

繼續閱讀