基于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], &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 認為里程計累計漂移比較小，所以通過距離與時間這兩個概念進行的關鍵幀的回環檢測，

點擊關注，第一時間了解華為云新鮮技術~

轉載請註明出處，本文鏈接：https://www.uj5u.com/qita/508105.html

標籤：其他

上一篇：【火熱招募】一文看懂華為云IoT Edge邊緣計算開發者大賽技術亮點

下一篇：Java反序列化之原生