天天看點
傳回

SC-LEGO-LOAM 擴充以及深度解析

前言

本作者在16年大學開始接觸ROS後，逐漸向着機器人建圖導航方面擴充，尤其是對雷射雷達方向比較感興趣，目前打算針對近階段的SC-LEGO-LOAM進行分析講述。從ScanContext和Lego LOAM兩個部分進行分析闡述。一方面也是記錄自己的學習成果，另一方面也是幫助他人一起熟悉這篇20年的經典文章。

LOAM系列發展

LOAM

LOAM作為該系列的鼻祖，在前幾年kitti資料集中常年霸占榜首，文中的作者所寫的代碼由于可讀性不高，是以有很多人對代碼進行了重構。

SC-LEGO-LOAM 擴充以及深度解析
SC-LEGO-LOAM 擴充以及深度解析

論文中作者目标是使用一個三維空間中運動的多線雷射雷達來實作雷射裡程計+建圖的功能。為了可以同時獲得低漂移和低複雜度，并且不需要高精度的測距和慣性測量。本文的核心思想是将定位和建圖的分割，通過兩個算法：一個是執行高頻率的裡程計但是低精度的運動估計（定位），另一個算法在比定位低一個數量級的頻率執行比對和注冊點雲資訊（建圖和校正裡程計）。

第一個算法為了保證高頻率，文中使用scan-to-scan比對，利用前後兩幀的位姿資訊來實作粗略的位姿估計（當中包含有：特征點提取【按線束分割 → \rightarrow → 計算曲率 → \rightarrow → 删除異常點 → \rightarrow →按曲率大小篩選特征點】 ⇒ \Rightarrow ⇒ 幀間比對【投影到上一時刻，并計算損失函數】 ⇒ \Rightarrow ⇒疊代優化【利用LM來實作位姿的疊代優化，并估算出這一幀資料中的點和上一幀資料中點的對應關系】）。

第二個算法為了保證高精度，則是使用了map-to-map的比對。其優點是精度高，誤差累計小；缺點就是計算量大，實時性壓力大。當我們在第一步有了裡程計校正後的點雲資料後，接下來我們就可以做一個map-to-map的比對了。但是map-to-map存在計算量大的問題，是以 我們可以讓其執行的頻率降低。這樣的高低頻率結合就保證了計算量的同時又兼具了精度。

值得注意的是LOAM僅僅是一個雷射裡程計算法，沒有閉環檢測，也就沒有加入圖優化架構。隻是将SLAM問題分為兩個算法并行運作：一個odometry算法，10Hz；另一個mapping算法，1Hz，最終将兩者輸出的pose做整合，實作10Hz的位姿實時輸出。兩個算法都是使用點雲中提取出尖銳的邊點和平整的面點作為特征點，然後進行特征點比對，來估計lidar的位姿以及對位姿進行fine tune。

SC-LEGO-LOAM 擴充以及深度解析
SC-LEGO-LOAM 擴充以及深度解析
SC-LEGO-LOAM 擴充以及深度解析

A-LOAM

A-LOAM相較于LOAM而言舍去了IMU對資訊修正的接口，同時A-LOAM使用了Ceres庫完成了LM優化和雅克比矩陣的正逆解。A-LOAM可讀性更高，便于上手。簡而言之，A-LOAM就是LOAM的清晰簡化，版本。

SC-LEGO-LOAM 擴充以及深度解析

A-LOAM的代碼清晰度确實很高，整理的非常簡潔，主要是使用了Ceres函數庫代替了張繼手推的ICP優化求解部分（用Ceres的自動求導，代替了手推的解析求導，效率會低一些）。整個代碼目錄如下：

SC-LEGO-LOAM 擴充以及深度解析
SC-LEGO-LOAM 擴充以及深度解析

…詳情請參照古月居

SC-LeGO-LOAM算法詳解

SC-LeGO-LOAM是在LeGO-LOAM的基礎上新增了基于Scan context的回環檢測，在回環檢測的速度上相較于LeGO-LOAM有了一定的提升。下面我們将會對SC-LeGO-LOAM算法的代碼進行詳細分析。

首先我們從代碼的launch檔案開始入手進行分析，README檔案中提示，我們需要從

run.launch

檔案入手。

<launch>
	........

    <node pkg="lego_loam" type="imageProjection" name="imageProjection" output="screen"/>
    <node pkg="lego_loam" type="featureAssociation" name="featureAssociation" output="screen"/>
    <node pkg="lego_loam" type="mapOptmization" name="mapOptmization" output="screen"/>
    <node pkg="lego_loam" type="transformFusion" name="transformFusion" output="screen"/>

</launch>

從中我們可以看到launch檔案中主要依賴四個node，其中最後一個node主要輸出了一些資料坐标系的轉換，對文章的了解影響不會很大，主要功能的是由前面3個node來實作的，而且是存在資料流的依次傳遞和處理。

imageProjection.cpp

首先我們先來看一下

imageProjection

這一個node節點，這個部分主要是對雷射雷達資料進行預處理。包括雷射雷達資料擷取、點雲資料分割、點雲類别标注、資料釋出。

該檔案中訂閱了雷射雷達釋出的資料，并釋出了多個分類點雲結果，初始化

lego_loam::ImageProjection

構造函數中，

nodehandle

使用

~

來表示在預設空間中。

ImageProjection::ImageProjection() : nh("~") {
        // init params
        InitParams();
        // subscriber
        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic.c_str(), 1,
                                                               &ImageProjection::cloudHandler, this);
        // publisher
        pubFullCloud = nh.advertise<sensor_msgs::PointCloud2>("/full_cloud_projected", 1);
        pubFullInfoCloud = nh.advertise<sensor_msgs::PointCloud2>("/full_cloud_info", 1);

        pubGroundCloud = nh.advertise<sensor_msgs::PointCloud2>("/ground_cloud", 1);
        pubSegmentedCloud = nh.advertise<sensor_msgs::PointCloud2>("/segmented_cloud", 1);
        pubSegmentedCloudPure = nh.advertise<sensor_msgs::PointCloud2>("/segmented_cloud_pure", 1);
        pubSegmentedCloudInfo = nh.advertise<cloud_msgs::cloud_info>("/segmented_cloud_info", 1);
        pubOutlierCloud = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud", 1);  // 離群點或異常點

        nanPoint.x = std::numeric_limits<float>::quiet_NaN();
        nanPoint.y = std::numeric_limits<float>::quiet_NaN();
        nanPoint.z = std::numeric_limits<float>::quiet_NaN();
        nanPoint.intensity = -1;

        allocateMemory();
        resetParameters();
    }

在構造函數中除了使用

allocateMemory

對點雲進行

reset、resize、assign

等重置、指派等操作以外。主要的函數是

ImageProjection::cloudHandler

，該函數内部清晰記錄了雷射雷達資料流的走向

void ImageProjection::cloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg) {
        // 1. Convert ros message to pcl point cloud
        copyPointCloud(laserCloudMsg);
        // 2. Start and end angle of a scan
        findStartEndAngle();
        // 3. Range image projection
        projectPointCloud();
        // 4. Mark ground points
        groundRemoval();
        // 5. Point cloud segmentation
        cloudSegmentation();
        // 6. Publish all clouds
        publishCloud();
        // 7. Reset parameters for next iteration
        resetParameters();
    }

該回調函數調用了七個函數，完成了對單幀雷射雷達資料的處理。下面我們對該流程進行梳理，并詳細介紹地面分割方法。

…詳情請參照古月居

featureAssociation.cpp

其次

featureAssociation

這一個node節點，主要是特征提取。代碼中先初始化了

lego_loam::FeatureAssociation

，用來訂閱了上一節點發出來的分割出來的點雲，點雲的屬性，外點以及IMU消息，并設定了回調函數。其中IMU消息的訂閱函數較為複雜，它從IMU資料中提取出姿态，角速度和線加速度，其中姿态用來消除重力對線加速度的影響。然後函數

FeatureAssociation::AccumulateIMUShiftAndRotation

用來做積分，包括根據姿态，将加速度往世界坐标系下進行投影。再根據勻加速度運動模型積分得到速度和位移，同時，對角速度也進行了積分。

void FeatureAssociation::imuHandler(const sensor_msgs::Imu::ConstPtr &imuIn) {
        double roll, pitch, yaw;
        tf::Quaternion orientation;
        tf::quaternionMsgToTF(imuIn->orientation, orientation);
        tf::Matrix3x3(orientation).getRPY(roll, pitch, yaw);

        // 對加速度進行坐标變換
        // 進行加速度坐标交換時将重力加速度去除，然後再進行xxx到zzz,yyy到xxx,zzz到yyy的變換。
        // 去除重力加速度的影響時，需要把重力加速度分解到三個坐标軸上，然後分别去除他們分量的影響，在去除的過程中需要注意加減号（預設右手坐标系的旋轉方向來看）。
        // 在上面示意圖中，可以簡單了解為紅色箭頭實線分解到紅色箭頭虛線上（根據pitchpitchpitch進行分解）,然後再按找rollrollroll角進行分解。
        // 原文連結：https://blog.csdn.net/wykxwyc/article/details/98317544
        float accX = imuIn->linear_acceleration.y - sin(roll) * cos(pitch) * 9.81;
        float accY = imuIn->linear_acceleration.z - cos(roll) * cos(pitch) * 9.81;
        float accZ = imuIn->linear_acceleration.x + sin(pitch) * 9.81;

        imuPointerLast = (imuPointerLast + 1) % imuQueLength;

        // 将歐拉角，加速度，速度儲存到循環隊列中
        imuTime[imuPointerLast] = imuIn->header.stamp.toSec();

        imuRoll[imuPointerLast] = roll;
        imuPitch[imuPointerLast] = pitch;
        imuYaw[imuPointerLast] = yaw;

        imuAccX[imuPointerLast] = accX;
        imuAccY[imuPointerLast] = accY;
        imuAccZ[imuPointerLast] = accZ;

        imuAngularVeloX[imuPointerLast] = imuIn->angular_velocity.x;
        imuAngularVeloY[imuPointerLast] = imuIn->angular_velocity.y;
        imuAngularVeloZ[imuPointerLast] = imuIn->angular_velocity.z;

        // 積分計算得到位移
        AccumulateIMUShiftAndRotation();
    }

上面的回調函數僅僅是考慮該如何讀取資料，而未涉及點雲該如何處理，也沒有說怎麼與IMU資料做融合。由于代碼太多而且很多都是純粹的數學推導計算，為此我們選取

runFeatureAssociation

中的一些重要的思想來詳寫。

//  主程式入口
    void FeatureAssociation::runFeatureAssociation() {
        // 有新資料進來才執行
        if (newSegmentedCloud && newSegmentedCloudInfo && newOutlierCloud &&
            std::abs(timeNewSegmentedCloudInfo - timeNewSegmentedCloud) < 0.05 &&
            std::abs(timeNewOutlierCloud - timeNewSegmentedCloud) < 0.05) {

            newSegmentedCloud = false;
            newSegmentedCloudInfo = false;
            newOutlierCloud = false;
        } else {
            return;
        }
        /**
            1. Feature Extraction
        */
        adjustDistortion();  //  imu去畸變

        calculateSmoothness(); //  計算光滑性

        markOccludedPoints();  // 距離較大或者距離變動較大的點标記

        extractFeatures();  //  特征提取（未看懂）

        publishCloud(); // cloud for visualization

        /**
        2. Feature Association
        */
        if (!systemInitedLM) {
            checkSystemInitialization();
            return;
        }

        updateInitialGuess();  //  更新初始位姿

        //  一個是找特征平面，通過面之間的對應關系計算出變換矩陣。
        // 另一個部分是通過角、邊特征的比對，計算變換矩陣。
        updateTransformation();

        integrateTransformation();  //  計算旋轉角的累積變化量。

        publishOdometry();

        publishCloudsLast(); // cloud to mapOptimization
    }

…詳情請參照古月居

SC-LEGO-LOAM 擴充以及深度解析

到這裡對SC-LeGO-LOAM的所有流程已經理清楚，同時這部分代碼可以在我個人的Github項目中看到，如果各位感興趣的可以下載下傳測試學習。

這裡感謝26.3分的HITer、攻城獅の家、shuang_yu_等人的相關博文提供的指導和幫助。

–>

ros lego_loam loam SLAM 雷射裡程計
上一篇: 接口-java的筆記小結接口
下一篇: [C++]LeetCode: 15 3Sum Closest (plus JAVA Code)

繼續閱讀