LeGo-LOAM 源碼解析

這篇具有很好參考價(jià)值的文章主要介紹了LeGo-LOAM 源碼解析。希望對大家有所幫助。如果存在錯誤或未考慮完全的地方，請大家不吝賜教，您也可以點(diǎn)擊"舉報(bào)違法"按鈕提交疑問。

A lightweight and ground optimized lidar odometry and mapping (LeGO-LOAM) system for ROS compatible UGVs. The system takes in point cloud from a Velodyne VLP-16 Lidar (placed horizontal) and optional IMU data as inputs. It outputs 6D pose estimation in real-time.

LeGO-LOAM（激光SLAM，IMU+LiDAR），以LOAM為基礎(chǔ)，實(shí)現(xiàn)與其同等的精度同時(shí)大大提高了速度。利用點(diǎn)云分割區(qū)分噪聲，提取平面與邊特征，使用兩步LM優(yōu)化方法求解位姿，并且添加回環(huán)檢測減小漂移。

github：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM

0、整體框架

LeGo-LOAM 源碼解析,ROS,lego-loam

首先看一下代碼結(jié)構(gòu)，cloud_msgs、launch、include、src（核心程序），接下來具體看一下：

CMakeList.txt

...
find_package(GTSAM REQUIRED QUIET)
find_package(PCL REQUIRED QUIET)
find_package(OpenCV REQUIRED QUIET)
...

依賴庫：GTSAM、PCL、OPenCV

GTSAM 是一個在機(jī)器人領(lǐng)域和計(jì)算機(jī)視覺領(lǐng)域用于平滑（smoothing）和建圖（mapping）的C++庫，采用因子圖（factor graphs）和貝葉斯網(wǎng)絡(luò)（Bayes networks）的方式最大化后驗(yàn)概率。

launch/run.launch

節(jié)點(diǎn)：rviz、camera_init_to_map、base_link_to_camera、imageProjection、featureAssociation、mapOptmization、transformFusion

主要節(jié)點(diǎn)就是后4個，也是對應(yīng)src目錄下的核心程序。

include/utility.h

定義數(shù)據(jù)結(jié)構(gòu)：點(diǎn)云、話題、激光雷達(dá)參數(shù)、圖像分割參數(shù)、特征提取參數(shù)、優(yōu)化參數(shù)、建圖參數(shù)

...
/*
    * A point cloud type that has "ring" channel
    */
struct PointXYZIR
{
   
    PCL_ADD_POINT4D;
    PCL_ADD_INTENSITY;
    uint16_t ring;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

POINT_CLOUD_REGISTER_POINT_STRUCT (PointXYZIR,  
                                   (float, x, x) (float, y, y)
                                   (float, z, z) (float, intensity, intensity)
                                   (uint16_t, ring, ring)
)
...

這里定義了一種包含 ring_id 的點(diǎn)云數(shù)據(jù)類型，velodyne 的激光雷達(dá)是有這個數(shù)據(jù)的，其它廠商的不一定有；區(qū)別于laser_id，ring_id是指從最下面的激光線依次向上遞增的線號（0-15 for VLP-16）。

...
// Using velodyne cloud "ring" channel for image projection (other lidar may have different name for this channel, change "PointXYZIR" below)
extern const bool useCloudRing = false; // if true, ang_res_y and ang_bottom are not used

// VLP-16
extern const int N_SCAN = 16;//number of lasers
extern const int Horizon_SCAN = 1800;//number of points from one laser of one scan
extern const float ang_res_x = 0.2;//angle resolution of horizon
extern const float ang_res_y = 2.0;//angle resolution of vertical
extern const float ang_bottom = 15.0+0.1;
extern const int groundScanInd = 7;//表示地面需要的線圈數(shù);
...

針對不同激光雷達(dá)的參數(shù)設(shè)置，要根據(jù)自己的實(shí)際情況去調(diào)整，假如使用數(shù)據(jù)集的數(shù)據(jù)，比如KITTI，就需要針對HDL-64E進(jìn)行調(diào)整，而且由于該雷達(dá)垂直角分辨率不是均勻的，還需要自己編寫后續(xù)的投影程序。

LeGo-LOAM 源碼解析,ROS,lego-loam

1、imageProjection —— 點(diǎn)云分割

將激光點(diǎn)云處理成圖像

總體流程：訂閱點(diǎn)云數(shù)據(jù)回調(diào)處理 -> 點(diǎn)云轉(zhuǎn)換到pcl預(yù)處理 -> 截取一幀激光數(shù)據(jù) -> 投影映射到圖像 -> 地面移除 -> 點(diǎn)云分割 -> 發(fā)布點(diǎn)云數(shù)據(jù) -> 重置參數(shù);

0. main()

int main(int argc, char **argv)
{
   

    ros::init(argc, argv, "lego_loam");

    ImageProjection IP;

    ROS_INFO("\033[1;32m---->\033[0m Image Projection Started.");

    ros::spin();
    return 0;
}

在 main 函數(shù)中，只是實(shí)例化了一個對象 IP ，所以一旦創(chuàng)建了一個對象，就進(jìn)入到了 Class 中去執(zhí)行構(gòu)造函數(shù)。

      ImageProjection() : nh("~")
    {
   
		// 訂閱原始的激光點(diǎn)云
        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 1, &ImageProjection::cloudHandler, this);
		
        // 轉(zhuǎn)換成圖片的點(diǎn)云
        pubFullCloud = nh.advertise<sensor_msgs::PointCloud2>("/full_cloud_projected", 1);
         // 轉(zhuǎn)換成圖片的并帶有距離信息的點(diǎn)云
        pubFullInfoCloud = nh.advertise<sensor_msgs::PointCloud2>("/full_cloud_info", 1);
		
        // 發(fā)布提取的地面特征
        pubGroundCloud = nh.advertise<sensor_msgs::PointCloud2>("/ground_cloud", 1);
        
        // 發(fā)布已經(jīng)分割的點(diǎn)云
        pubSegmentedCloud = nh.advertise<sensor_msgs::PointCloud2>("/segmented_cloud", 1);
         // 具有幾何信息的分割點(diǎn)云
        pubSegmentedCloudPure = nh.advertise<sensor_msgs::PointCloud2>("/segmented_cloud_pure", 1);
        pubSegmentedCloudInfo = nh.advertise<cloud_msgs::cloud_info>("/segmented_cloud_info", 1);
          
         // 含有異常信息的點(diǎn)云
        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();
    }

class 中的構(gòu)造函數(shù)中，訂閱了原始的激光點(diǎn)云數(shù)據(jù)（subLaserCloud）然后就進(jìn)入到了回調(diào)函數(shù)（ImageProjection::cloudHandler）對點(diǎn)云數(shù)據(jù)進(jìn)行處理。

1. cloudHandler()

訂閱激光雷達(dá)點(diǎn)云信息之后的回調(diào)函數(shù)

    void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
   
        // 1. Convert ros message to pcl point cloud    將ros消息轉(zhuǎn)換為pcl點(diǎn)云
        copyPointCloud(laserCloudMsg);	
        // 2. Start and end angle of a scan     掃描的起始和結(jié)束角度
        findStartEndAngle();
        // 3. Range image projection        距離圖像投影
        projectPointCloud();
        // 4. Mark ground points        標(biāo)記地面點(diǎn)
        groundRemoval();
        // 5. Point cloud segmentation      點(diǎn)云分割
        cloudSegmentation();
        // 6. Publish all clouds        發(fā)布點(diǎn)云
        publishCloud();
        // 7. Reset parameters for next iteration       重置下一次迭代的參數(shù)
        resetParameters();
    }

回調(diào)函數(shù)中按照以上 7 個邏輯步驟對點(diǎn)云進(jìn)行處理，主要的邏輯步驟通過一個個封裝好的函數(shù)呈現(xiàn)出來。

2. copyPointCloud()

由于激光雷達(dá)點(diǎn)云消息在傳遞過程中使用的是ROS 類型的消息，所以在處理的過程中統(tǒng)一需要對消息類型轉(zhuǎn)換，通常的辦法就是使用 PCL 庫

    // 復(fù)制點(diǎn)云   將ros消息轉(zhuǎn)換為pcl點(diǎn)云
    // 使用pcl庫函數(shù);
    
    void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
   
         // 1. 讀取ROS點(diǎn)云轉(zhuǎn)換為PCL點(diǎn)云
        cloudHeader = laserCloudMsg->header;
        // cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
        pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);

        //2.移除無效的點(diǎn)云 Remove Nan points
        std::vector<int> indices;
        pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);

        // 3. have "ring" channel in the cloud or not
        // 如果點(diǎn)云有"ring"通過，則保存為laserCloudInRing
        // 判斷是不是使用了 velodyne 的激光雷達(dá)
        if (useCloudRing == true){
   
            pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
            if (laserCloudInRing->is_dense == false) {
   
                ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
                ros::shutdown();
            }  
        }
    }

?這個函數(shù)主體內(nèi)容分為三個步驟，其中的第三步注意區(qū)分自己使用的激光雷達(dá)是否存在 CloudRing，沒有這個的激光雷達(dá)接下來的步驟都不會去執(zhí)行，所以在實(shí)際跑這個框架的時(shí)候一定要注意這個地方

3. findStartEndAngle()

    // 找到開始結(jié)束角度
    // 1.一圈數(shù)據(jù)的角度差，使用atan2計(jì)算;
    // 2.注意計(jì)算結(jié)果的范圍合理性
    
    void findStartEndAngle(){
   
        // start and end orientation of this cloud
        // 1.開始點(diǎn)和結(jié)束點(diǎn)的航向角 (負(fù)號表示順時(shí)針旋轉(zhuǎn))   
        segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);

        // 加 2 * M_PI 表示已經(jīng)轉(zhuǎn)轉(zhuǎn)了一圈
        segMsg.endOrientation   = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,
                                                     laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI;
        
        // 2.保證所有角度落在 [M_PI , 3M_PI] 上
        if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI) {
   
            segMsg.endOrientation -= 2 * M_PI;
        } else if (segMsg.endOrientation - segMsg.startOrientation < M_PI)
            segMsg.endOrientation += 2 * M_PI;
        segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation;
    }

這個函數(shù)在 LOAM 代碼里面也存在。
首先通過一個反正切函數(shù)，找到一幀激光點(diǎn)云起始時(shí)刻和結(jié)束時(shí)刻的航向角，第二步的作用是將一幀激光點(diǎn)云的航向角度限制在 [pi , 3pi] 之間，方便后續(xù)計(jì)算出一幀激光點(diǎn)云的時(shí)間。

4. projectPointCloud()

距離圖像投影
將3D point cloud投影映射到2D range image
將激光雷達(dá)數(shù)據(jù)投影成一個16x1800（依雷達(dá)角分辨率而定）的點(diǎn)云陣列

    // 距離圖像投影
    // 將3D point cloud投影映射到2D range image
    // 將激光雷達(dá)數(shù)據(jù)投影成一個16x1800（依雷達(dá)角分辨率而定）的點(diǎn)云陣列
    void projectPointCloud(){
   
        // range image projection
        float verticalAngle, horizonAngle, range;
        size_t rowIdn, columnIdn, index, cloudSize; 
        PointType thisPoint;

        cloudSize = laserCloudIn->points.size();

        // 遍歷整個點(diǎn)云 
        for (size_t i = 0; i < cloudSize; ++i){
   
            // 提取點(diǎn)云中 x y z 坐標(biāo)數(shù)值
            thisPoint.x = laserCloudIn->points[i].x;
            thisPoint.y = laserCloudIn->points[i].y;
            thisPoint.z = laserCloudIn->points[i].z;

            // find the row and column index in the iamge for this point
            // 判斷是不是使用了 velodyne 的雷達(dá)
            if (useCloudRing == true){
   
                // 提取激光雷達(dá)線束到 rowIdn 
                rowIdn = laserCloudInRing->points[i].ring;
            }
            //  是其他的雷達(dá) 就通過俯仰角 確定當(dāng)前的激光點(diǎn)是來自哪個線束 index 
            else{
   
                verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
                rowIdn = (verticalAngle + ang_bottom) / ang_res_y;  //確定行索引
            }
            if (rowIdn < 0 || rowIdn >= N_SCAN)
                continue;

            horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;

            // 保證 columnIdn 的大小在 [0 , 1800)
            columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2; //確定列索引
            if (columnIdn >= Horizon_SCAN)
                columnIdn -= Horizon_SCAN;

            if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
                continue;

            // 如果距離小于 1米 則過濾掉 通常是過濾自身（距離傳感器比較近的點(diǎn)）
            range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);    //確定深度
            if (range < sensorMinimumRange)
                continue;
            
            // 將計(jì)算下來的距離傳感器的 數(shù)值保存到 rangeMat 中 
            // 這是一個 16 * 1800 的矩陣 rowIdn為線束數(shù)值  columnIdn 是 一圈圓形 灘平之后的數(shù)值
            // range  是特征點(diǎn)云點(diǎn)到原點(diǎn)的數(shù)值
            // 這樣就將一個三維的坐標(biāo) 轉(zhuǎn)換到一個 矩陣中了.
            rangeMat.at<float>(rowIdn, columnIdn) = range;

            // 將 index 和 橫坐標(biāo)存儲在 intensity 中 整數(shù)部分是線束數(shù)值  小數(shù)部分是方向角度
            thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;

            //深度圖的索引值  index = 列號 +  行號 * 1800 
            index = columnIdn  + rowIdn * Horizon_SCAN;

            // fullCloud 存放的是坐標(biāo)信息（二維的）
            // fullInfoCloud 增加了點(diǎn)到傳感器的距離信息（三維的） 
            fullCloud->points[index] = thisPoint;
            fullInfoCloud->points[index] = thisPoint;

            // intensity 中存放的是點(diǎn)云點(diǎn)到傳感器的距離
            fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity"
        }
    }

此處對激光點(diǎn)云的處理是 LEGO-LOAM 和 LOAM 的第一個區(qū)別。這里將一幀激光點(diǎn)云處理成一個 16 * 1800 像素的圖片。 16代表的是激光雷達(dá)的線束，1800代表的是激光掃描一圈時(shí)候是間隔0.5度發(fā)射一個激光束所以 360 * 0.5 = 1800

? 再者，rangeMat.at(rowIdn, columnIdn) 這里調(diào)用了一個 opencv 的多維數(shù)組，數(shù)組中的行與列對應(yīng)的就是圖片的長和寬，然后數(shù)組中存放的數(shù)值就是圖片相應(yīng)位置上面的點(diǎn)到激光雷達(dá)的距離。

? 最后將點(diǎn)云的坐標(biāo)信息存儲在 intensity 中，采用的方法是：整數(shù)+小數(shù) 存儲。整數(shù)部分存儲的是激光線束數(shù)值，小數(shù)部分存儲的水平航向值。

LeGo-LOAM 源碼解析,ROS,lego-loam

atan2(y，x);

反正切的角度等于X軸與通過原點(diǎn)和給定坐標(biāo)點(diǎn)(x, y)的直線之間的夾角，結(jié)果為正表示從X軸逆時(shí)針旋轉(zhuǎn)的角度，結(jié)果為負(fù)表示從X軸順時(shí)針旋轉(zhuǎn)的角度;

LeGo-LOAM 源碼解析,ROS,lego-loam

確定行索引

根據(jù)ring id或者根據(jù)坐標(biāo)計(jì)算：
確定列索引([ ? π , π ] 轉(zhuǎn)換為 [ 0 , 1800 ])：
確定深度值：（注意數(shù)據(jù)的有效范圍）

5. groundRemoval()

首先。判斷地面點(diǎn)的標(biāo)志就是相鄰兩個激光線束掃描到點(diǎn)的坐標(biāo)，如果兩個坐標(biāo)之間的差值或者兩個坐標(biāo)之間的斜率大于一個設(shè)定的值，則將該點(diǎn)

判斷是地面點(diǎn)。所以此處用到了激光點(diǎn)云圖片的深度信息

    // 移除地面點(diǎn)
    // 根據(jù)上下兩線之間點(diǎn)的位置計(jì)算兩線之間俯仰角判斷，小于10度則為地面點(diǎn)
    
    void groundRemoval(){
   
        size_t lowerInd, upperInd;
        float diffX, diffY, diffZ, angle;
        
        // groundMat
        // -1, no valid info to check if ground of not  沒有有效的信息確認(rèn)是不是地面
        //  0, initial value, after validation, means not ground    確認(rèn)不是地面點(diǎn)
        //  1, ground
        for (size_t j = 0; j < Horizon_SCAN; ++j){
   
            //  前7個激光雷達(dá)掃描線束足夠滿足地面點(diǎn)的檢測 所以只遍歷 7 次
            for (size_t i = 0; i < groundScanInd; ++i){
   
                //groundScanInd定義打到地面上激光線的數(shù)目
                lowerInd = j + ( i )*Horizon_SCAN;
                upperInd = j + (i+1)*Horizon_SCAN;

                // 如果之前計(jì)算過的 intensity 是 -1 則直接默認(rèn)為是一個無效點(diǎn)
                if (fullCloud->points[lowerInd].intensity == -1 ||
                    fullCloud->points[upperInd].intensity == -1){
   
                    // no info to check, invalid points  對這個無效點(diǎn)直接進(jìn)行貼標(biāo)簽
                    groundMat.at<int8_t>(i,j) = -1;
                    continue;
                }
                    
                diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x;
                diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y;
                diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z;

                // 計(jì)算相鄰兩個線束之間的夾角 
                angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY) ) * 180 / M_PI;

                 //垂直方向相鄰兩點(diǎn)俯仰角小于10度就判定為地面點(diǎn)；相鄰掃描圈
                if (abs(angle - sensorMountAngle) <= 10){
   
                    groundMat.at<int8_t>(i,j) = 1;
                    groundMat.at<int8_t>(i+1,j) = 1;
                }
            }
        }

        // extract ground cloud (groundMat == 1)
        // mark entry that doesn't need to label (ground and invalid point) for segmentation
        // note that ground remove is from 0~N_SCAN-1, need rangeMat for mark label matrix for the 16th scan

        // 給地面點(diǎn) 標(biāo)記一個符號 為 -1 
        for (size_t i = 0; i < N_SCAN; ++i){
   
            for (size_t j = 0; j < Horizon_SCAN; ++j){
   
                if (groundMat.at<int8_t>(i,j) == 1 || rangeMat.at<float>(i,j) == FLT_MAX){
   
                    labelMat.at<int>(i,j) = -1;
                }
            }
        }

        // 發(fā)布點(diǎn)云信息，只發(fā)布地面點(diǎn)云信息
        if (pubGroundCloud.getNumSubscribers() != 0){
   
            for (size_t i = 0; i <= groundScanInd; ++i){
   
                for (size_t j = 0; j < Horizon_SCAN; ++j){
   
                    if (groundMat.at<int8_t>(i,j) == 1)
                        groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                }
            }
        }
    }

LeGo-LOAM 源碼解析,ROS,lego-loam

垂直相鄰兩點(diǎn)俯仰角小于10度就判定為地面點(diǎn)。

6. cloudSegmentation()

?從論文中可以看到，分割函數(shù)的來源參考了兩篇論文（ Effificient Online Segmentation for Sparse 3D Laser Scans ，另外一篇 Fast Range Image-Based Segmentation of Sparse 3D Laser Scans for Online Operation）的方法。方法的原理可以參考知乎文章《地面點(diǎn)障礙物快速分割聚類》。

    // 點(diǎn)云分割
    // 首先對點(diǎn)云進(jìn)行聚類標(biāo)記，根據(jù)標(biāo)簽進(jìn)行對應(yīng)點(diǎn)云塊存儲;
    
    void cloudSegmentation(){
   
        // segmentation process
        for (size_t i = 0; i < N_SCAN; ++i)     // 16線的 一個線束一個的遍歷
            for (size_t j = 0; j < Horizon_SCAN; ++j)       // 水平 的 1800
                if (labelMat.at<int>(i,j) == 0)     // 對非地面點(diǎn)進(jìn)行點(diǎn)云分割
                    labelComponents(i, j);      //  調(diào)用這個函數(shù)對點(diǎn)云進(jìn)行分割聚類

        int sizeOfSegCloud = 0;

        // extract segmented cloud for lidar odometry
        // 提取分割點(diǎn)云 用于激光雷達(dá)里程計(jì)
        for (size_t i = 0; i < N_SCAN; ++i) {
   

            segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5;

            for (size_t j = 0; j < Horizon_SCAN; ++j) {
   
                if (labelMat.at<int>(i,j) > 0 || groundMat.at<int8_t>(i,j) == 1){
   
                    // outliers that will not be used for optimization (always continue)
                    // 勾勒出優(yōu)化過程中不被使用的值

                    // 1. 如果label為999999則跳過
                    if (labelMat.at<int>(i,j) == 999999){
   
                        if (i > groundScanInd && j % 5 == 0){
   
                            outlierCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                            continue;
                        }else{
   
                            continue;
                        }
                    }

                    // majority of ground points are skipped
                    // 2. 如果為地，跳過index不是5的倍數(shù)的點(diǎn)
                    if (groundMat.at<int8_t>(i,j) == 1){
   
                        if (j%5!=0 && j>5 && j<Horizon_SCAN-5)
                            continue;
                    }
                    // mark ground points so they will not be considered as edge features later
                    segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1);
                    // mark the points' column index for marking occlusion later
                    segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
                    // save range info
                    segMsg.segmentedCloudRange[sizeOfSegCloud]  = rangeMat.at<float>(i,j);
                    // save seg cloud
                    segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                    // size of seg cloud
                    ++sizeOfSegCloud;
                }
            }

            segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;
        }
        
        // extract segmented cloud for visualization
        if (pubSegmentedCloudPure.getNumSubscribers() != 0){
   
            for (size_t i = 0; i < N_SCAN; ++i){
   
                for (size_t j = 0; j < Horizon_SCAN; ++j){
   
                    if (labelMat.at<int>(i,j) > 0 && labelMat.at<int>(i,j) != 999999){
   
                        segmentedCloudPure->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                        segmentedCloudPure->points.back().intensity = labelMat.at<int>(i,j);
                    }
                }
            }
        }
    }

7. labelComponents()

    // 標(biāo)簽組件
    // 局部特征檢測聚類
    // 平面點(diǎn)與邊緣點(diǎn)
    
    void labelComponents(int row, int col){
   
        // use std::queue std::vector std::deque will slow the program down greatly
        float d1, d2, alpha, angle;
        int fromIndX, fromIndY, thisIndX, thisIndY; 
        bool lineCountFlag[N_SCAN] = {
   false};

        // 傳進(jìn)來的兩個參數(shù)，按照坐標(biāo)不同分別給他們放到 X 與 Y 的數(shù)組中
        queueIndX[0] = row;
        queueIndY[0] = col;
        int queueSize = 1;      // 需要計(jì)算角度的點(diǎn)的數(shù)量
        int queueStartInd = 0;
        int queueEndInd = 1;

        allPushedIndX[0] = row;
        allPushedIndY[0] = col;
        int allPushedIndSize = 1;
        
        while(queueSize > 0){
   
            // Pop point
            fromIndX = queueIndX[queueStartInd];
            fromIndY = queueIndY[queueStartInd];
            --queueSize;
            ++queueStartInd;
            // Mark popped point
            labelMat.at<int>(fromIndX, fromIndY) = labelCount;
            // Loop through all the neighboring grids of popped grid

            // 遍歷整個點(diǎn)云，遍歷前后左右四個鄰域點(diǎn)，求點(diǎn)之間的角度數(shù)值 
            for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){
   
                // new index
                thisIndX = fromIndX + (*iter).first;
                thisIndY = fromIndY + (*iter).second;
                // index should be within the boundary
                if (thisIndX < 0 || thisIndX >= N_SCAN)
                    continue;
                // at range image margin (left or right side)
                if (thisIndY < 0)
                    thisIndY = Horizon_SCAN - 1;
                if (thisIndY >= Horizon_SCAN)
                    thisIndY = 0;
                // prevent infinite loop (caused by put already examined point back)
                if (labelMat.at<int>(thisIndX, thisIndY) != 0)
                    continue;

                 //比較得出較大深度與較小深度
                d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY), 
                              rangeMat.at<float>(thisIndX, thisIndY));
                d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY), 
                              rangeMat.at<float>(thisIndX, thisIndY));

                if ((*iter).first == 0)
                    alpha = segmentAlphaX;
                else
                    alpha = segmentAlphaY;

                angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));      //平面聚類公式，角度越大兩點(diǎn)越趨向于平面
                //segmentTheta=60度
                if (angle > segmentTheta){
   

                    queueIndX[queueEndInd] = thisIndX;
                    queueIndY[queueEndInd] = thisIndY;
                    ++queueSize;
                    ++queueEndInd;

                    labelMat.at<int>(thisIndX, thisIndY) = labelCount;
                    lineCountFlag[thisIndX] = true;

                    allPushedIndX[allPushedIndSize] = thisIndX;
                    allPushedIndY[allPushedIndSize] = thisIndY;
                    ++allPushedIndSize;
                }
            }
        }

        // check if this segment is valid
        bool feasibleSegment = false;

        // 如果是 allPushedIndSize 累加的數(shù)量增加到了30 個 則判斷這部分點(diǎn)云屬于一個聚類
        if (allPushedIndSize >= 30)
            feasibleSegment = true;     //面點(diǎn)

        // 如果垂直 方向上點(diǎn)的數(shù)量大于 5個 默認(rèn)是一個有效的聚類
        else if (allPushedIndSize >= segmentValidPointNum){
   
            int lineCount = 0;
            for (size_t i = 0; i < N_SCAN; ++i)
                if (lineCountFlag[i] == true)
                    ++lineCount;
            if (lineCount >= segmentValidLineNum)
                feasibleSegment = true;     //邊點(diǎn)  
        }
        
        // segment is valid, mark these points
        if (feasibleSegment == true){
   
            ++labelCount;
        }else{
    // segment is invalid, mark these points
            for (size_t i = 0; i < allPushedIndSize; ++i){
   
                labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
            }
        }
    }

取去除地面的 labelMat，逐點(diǎn)開始計(jì)算鄰域4點(diǎn)（鄰域的索引要注意有效范圍），與中心點(diǎn)比較出距離的最大值與最小值，通過下面公式結(jié)合閾值π / 3，大于該值則構(gòu)成平面特征為平面點(diǎn)，隊(duì)列存儲索引，labelMat 存儲標(biāo)簽值，同時(shí) lineCountFlag 置 true，直到不滿足閾值跳出進(jìn)行下步。

LeGo-LOAM 源碼解析,ROS,lego-loam

聚類，超過30個點(diǎn)聚為一類，labelCount++；
小于30超過5，統(tǒng)計(jì)垂直方向聚類點(diǎn)數(shù)，超過3個也標(biāo)記為一類；
若都不滿足，則賦值999999表示需要舍棄的聚類點(diǎn)。

8. publishCloud()

發(fā)布各類點(diǎn)云數(shù)據(jù)

9. resetParameters()

參數(shù)重置

LeGO-LOAM首先進(jìn)行地面分割，找到地面之后，對地面之上的點(diǎn)進(jìn)行聚類。聚類的算法如下圖，主要是根據(jù)斜率對物體做切割，最后得到地面和分割后的點(diǎn)云。上述步驟的關(guān)鍵是要理解如何進(jìn)行地面分割和點(diǎn)云分割。

點(diǎn)云分割完成之后，接下來對分割后的點(diǎn)云提取特征，提取的特征的目的是進(jìn)行點(diǎn)云配準(zhǔn)，從而得出當(dāng)前位姿。

2、featureAssociation —— 特征提取與匹配

訂閱分割點(diǎn)云、外點(diǎn)、imu數(shù)據(jù)，總體流程：

（特征提取部分）訂閱傳感器數(shù)據(jù) -> 運(yùn)動畸變?nèi)コ?-> 曲率計(jì)算 -> 去除瑕點(diǎn) -> 提取邊、平面特征 -> 發(fā)布特征點(diǎn)云；
（特征關(guān)聯(lián)部分）將IMU信息作為先驗(yàn) -> 根據(jù)線特征、面特征計(jì)算位姿變換 -> 聯(lián)合IMU數(shù)據(jù)進(jìn)行位姿更新 -> 發(fā)布里程計(jì)信息

0. main()

主函數(shù)內(nèi)部做了兩件事情，一個是實(shí)例化一個對象，一個是循環(huán)執(zhí)行類方法

int main(int argc, char** argv)
{
   
    ros::init(argc, argv, "lego_loam");

    ROS_INFO("\033[1;32m---->\033[0m Feature Association Started.");

     // 1. 在構(gòu)造函數(shù)中訂閱消息和消息回調(diào)函數(shù)
    FeatureAssociation FA;

    ros::Rate rate(200);
    // 2. 循環(huán)調(diào)用runFeatureAssociation，函數(shù)的主流程所在
    while (ros::ok())
    {
   
        ros::spinOnce();

        FA.runFeatureAssociation();

        rate.sleep();
    }
    
    ros::spin();
    return 0;
}

主函數(shù)里面首選實(shí)例化了一個對象 FA ，所以和之前的ImageProjection 處理過程一樣，執(zhí)行構(gòu)造函數(shù)的內(nèi)容，所以在構(gòu)造函數(shù)中會訂閱上一個cpp 文件中 pub 出來的消息。然后轉(zhuǎn)到回調(diào)函數(shù)中處理數(shù)據(jù)，此處的回調(diào)函數(shù)都是將相應(yīng)的信息存儲到buff 中，所以不做詳細(xì)分析。

消息回調(diào)主要是接收上一個步驟（點(diǎn)云分割）中分割好的點(diǎn)云消息。文章來源地址http://www.zghlxwxcb.cn/news/detail-707404.html

    FeatureAssociation():
        nh("~")
        {
   
        // 1. 接收消息，上一步分割好的點(diǎn)云
        // 訂閱了分割之后的點(diǎn)云
        subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/segmented_cloud", 1, &FeatureAssociation::laserCloudHandler, this);
        
        // 訂閱的分割點(diǎn)云含有圖像的深度信息
        subLaserCloudInfo = nh.subscribe<cloud_msgs::cloud_info>("/segmented_cloud_info", 1, &FeatureAssociation::laserCloudInfoHandler, this);
        
        // 非聚類的點(diǎn)
        subOutlierCloud = nh.subscribe<sensor_msgs::PointCloud2>("/outlier_cloud", 1, &FeatureAssociation::outlierCloudHandler, this);
        
        // IMU傳感器的消息
        subImu = nh.subscribe<sensor_msgs::Imu>(imuTopic, 50, &FeatureAssociation::imuHandler, this);
		
		// 2. 發(fā)布消息
        // 發(fā)布面特征點(diǎn) 和 邊特征點(diǎn)
        pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 1);
        pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 1);
        pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 1);
        pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 1);

        pubLaserCloudCornerLast = nh.advertise<sensor_msgs::PointCloud2>("/l