三維SLAM算法LeGO-LOAM源碼閱讀（一）

LeGO-LOAM是LOAM的增強版，在LOAM的基礎上增加了迴環檢測。論文地址：https://github.com/RobustFieldAutonomyLab/LeGO-LOAM/blob/master/Shan_Englot_IROS_2018_Preprint.pdf

它分別由點雲分割、特徵提取、激光里程計、激光建圖組成，算法思想與RGBD-SLAM有些類似，系統通過接收來自三維激光雷達的輸入並輸出6自由度姿態估計。整個系統分爲五個模塊，第一個是分割，將單個掃描的點雲投影到一個固定範圍的圖像上進行分割，然後第二個是將分割的點雲發送到特徵提取模塊。第三個，激光雷達里程計使用從上一個模塊中提取的特徵找到在連續掃描過程中機器人位姿的轉換，這些信息最終用於激光雷達用點雲方式的建圖中。第五個模塊是融合激光雷達里程測量和建圖的姿態估計結果，並輸出最終的姿態估計。

從論文中可以得到，從地面特徵中確認機器人當前位姿z，roll，pitch，而在每一幀點雲數據匹配的過程中得到機器人位姿的x，y，yaw。

接下來看看代碼，從CMakeLists中我們可以看到，它依賴於PCL、OpenCV、GTSAM三個庫，以前沒用過GTSAM庫，它的定義是：

GTSAM是一個在機器人領域和計算機視覺領域用於平滑（smoothing）和建圖（mapping）的C++庫。它與g2o不同的是，g2o採用稀疏矩陣的方式求解一個非線性優化問題，而GTSAM是採用因子圖（factor graphs）和貝葉斯網絡（Bayes networks）的方式最大化後驗概率。

在這個工程中，具備了imageProjection（圖像投影）、featureAssociation（特徵關聯）、mapOptmization（地圖優化）、transformFusion（位姿優化）四個可執行文件，這比LOAM的四個main函數程序加上lib中的一堆文件要人性化一點。

唯一的頭文件utility.h定義了一些普適的東西，先看看頭文件。

首先PointType是帶intensity的PointXYZ：

typedef pcl::PointXYZI  PointType;

 這些參數是根據雷達品牌來定的，比如這是16線、每線1800個點的數據：

// VLP-16
extern const int N_SCAN = 16;
extern const int Horizon_SCAN = 1800;
extern const float ang_res_x = 0.2;//水平上每條線間隔0.2°
extern const float ang_res_y = 2.0;//豎直方向上每條線間隔2°
extern const float ang_bottom = 15.0+0.1;//豎直方向上起始角度是負角度，與水平方向相差15.1°
extern const int groundScanInd = 7;//以多少個掃描圈來表示地面

 在點雲分割時的必要參數：

extern const float sensorMountAngle = 0.0;
extern const float segmentTheta = 1.0472;//點雲分割時的角度跨度上限（π/3）
extern const int segmentValidPointNum = 5;//檢查上下左右連續5個點做爲分割的特徵依據
extern const int segmentValidLineNum = 3;

而 PointTypePose指的是具備姿態角的特定點：

struct PointXYZIRPYT
{
    PCL_ADD_POINT4D
    PCL_ADD_INTENSITY;
    float roll;
    float pitch;
    float yaw;
    double time;
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGEN_ALIGN16;

typedef PointXYZIRPYT  PointTypePose;

一. 根據算法思想，我們首先看到imageProjection.cpp，這個程序是對三維點雲進行投影。下列是在投影過程中主要用到的點雲：

pcl::PointCloud<PointType>::Ptr laserCloudIn;//雷達直接傳出的點雲


pcl::PointCloud<PointType>::Ptr fullCloud;//投影后的點雲
pcl::PointCloud<PointType>::Ptr fullInfoCloud;//整體的點雲

pcl::PointCloud<PointType>::Ptr groundCloud;//地面點雲
pcl::PointCloud<PointType>::Ptr segmentedCloud;//分割後的部分
pcl::PointCloud<PointType>::Ptr segmentedCloudPure;//分割後的部分的幾何信息
pcl::PointCloud<PointType>::Ptr outlierCloud;//在分割時出現的異常

另外，使用了自創的rosmsg來表示點雲信息：

cloud_msgs::cloud_info segMsg;

它的具體信息如下：

Header header 

int32[] startRingIndex
int32[] endRingIndex

float32 startOrientation
float32 endOrientation
float32 orientationDiff

bool[]    segmentedCloudGroundFlag
uint32[]  segmentedCloudColInd 
float32[] segmentedCloudRange

接下來的流程完全依賴於回調函數ImageProjection::cloudHandler：

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg)
{
    copyPointCloud(laserCloudMsg);
    findStartEndAngle();
    projectPointCloud();
    groundRemoval();
    cloudSegmentation();
    publishCloud();
    resetParameters();
}

在這個函數裏依次調用了點雲的複製、尋找始末角度、點雲投影、地面檢測、點雲分割與發佈。其中點雲的複製是將rosmsg轉化爲pcl點雲。

1.findStartEndAngle是將起始點與最末點進行角度的轉換：

void findStartEndAngle()
{
    //計算角度時以x軸負軸爲基準
    segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
    //因此最末角度爲2π減去計算值
    segMsg.endOrientation   = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y, laserCloudIn->points[laserCloudIn->points.size() - 2].x) + 2 * M_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;
}

2.projectPointCloud是將點雲逐一計算深度，將具有深度的點雲保存至fullInfoCloud中。

    void projectPointCloud()
    {
        float verticalAngle, horizonAngle, range;
        size_t rowIdn, columnIdn, index, cloudSize;
        PointType thisPoint;

        cloudSize = laserCloudIn->points.size();

        for (size_t i = 0; i < cloudSize; ++i)
        {
            thisPoint.x = laserCloudIn->points[i].x;
            thisPoint.y = laserCloudIn->points[i].y;
            thisPoint.z = laserCloudIn->points[i].z;
            //計算豎直方向上的點的角度以及在整個雷達點雲中的哪一條水平線上
            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;
            //round是四捨五入取偶
            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;
            //當前點與雷達的深度
            range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
            //在rangeMat矩陣中保存該點的深度，保存單通道像素值
            rangeMat.at<float>(rowIdn, columnIdn) = range;

            thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;

            index = columnIdn  + rowIdn * Horizon_SCAN;
            fullCloud->points[index] = thisPoint;

            fullInfoCloud->points[index].intensity = range;
        }
    }

3.groundRemoval是利用不同的掃描圈來表示地面，進而檢測地面是否水平。例如在源碼中的七個掃描圈，每兩個圈之間進行一次比較，角度相差10°以內的我們可以看做是平地。並且將掃描圈中的點加入到groundCloud點雲。

4.cloudSegmentation作爲本程序的關鍵部分，首先調用了labelComponents函數，該函數對特徵的檢測進行了詳細的描述，並且是針對於某一特定的點與其鄰點的計算過程。

    void labelComponents(int row, int col)
    {
        float d1, d2, alpha, angle;
        int fromIndX, fromIndY, thisIndX, thisIndY;
        bool lineCountFlag[N_SCAN] = {false};

        queueIndX[0] = row;
        queueIndY[0] = col;
        int queueSize = 1;
        int queueStartInd = 0;
        int queueEndInd = 1;

        allPushedIndX[0] = row;
        allPushedIndY[0] = col;
        int allPushedIndSize = 1;
        //queueSize指的是在特徵處理時還未處理好的點的數量，因此該while循環是在嘗試檢測該特定點的周圍的點的幾何特徵
        while(queueSize > 0)
        {
            fromIndX = queueIndX[queueStartInd];
            fromIndY = queueIndY[queueStartInd];
            --queueSize;
            ++queueStartInd;
            labelMat.at<int>(fromIndX, fromIndY) = labelCount;
            //檢查上下左右四個鄰點
            for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter)
            {
                thisIndX = fromIndX + (*iter).first;
                thisIndY = fromIndY + (*iter).second;

                if (thisIndX < 0 || thisIndX >= N_SCAN)
                    continue;

                if (thisIndY < 0)
                    thisIndY = Horizon_SCAN - 1;
                if (thisIndY >= Horizon_SCAN)
                    thisIndY = 0;

                if (labelMat.at<int>(thisIndX, thisIndY) != 0)
                    continue;
                //d1與d2分別是該特定點與某鄰點的深度
                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));
                //該迭代器的first是0則是水平方向上的鄰點，否則是豎直方向上的
                if ((*iter).first == 0)
                    alpha = segmentAlphaX;
                else
                    alpha = segmentAlphaY;
                //這個angle其實是該特定點與某鄰點的連線與XOZ平面的夾角，這個夾角代表了局部特徵的敏感性
                angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
                //如果夾角大於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;
                }
            }
        }


        bool feasibleSegment = false;
        //當鄰點數目達到30後，則該幀雷達點雲的幾何特徵配置成功
        if (allPushedIndSize >= 30)
            feasibleSegment = true;
        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;
        }

        if (feasibleSegment == true)
        {
            ++labelCount;
        }
        else
        {
            for (size_t i = 0; i < allPushedIndSize; ++i)
            {
                labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
            }
        }
    }

再回到cloudSegmentation當中，可以看到這是對點雲分爲地面點與可被匹配的四周被掃描的點，將其篩選後分別納入被分割點雲。

    void cloudSegmentation()
    {
        //這是在排除地面點與異常點之後，逐一檢測鄰點特徵並生成局部特徵
        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)
                    labelComponents(i, j);

        int sizeOfSegCloud = 0;
        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)
                {
                    //離羣點或異常點的處理
                    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;
                        }
                    }
                    if (groundMat.at<int8_t>(i,j) == 1)
                    {
                        //地面點雲每隔5個點納入被分割點雲
                        if (j%5!=0 && j>5 && j<Horizon_SCAN-5)
                            continue;
                    }
                    //segMsg是自定義rosmsg
                    //是否是地面點
                    segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1);
                    //當前水平方向上的行數
                    segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
                    //深度
                    segMsg.segmentedCloudRange[sizeOfSegCloud]  = rangeMat.at<float>(i,j);
                    //把當前點納入分割點雲中
                    segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
                    ++sizeOfSegCloud;
                }
            }

            segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;
        }
        //如果在當前有節點訂閱便將分割點雲的幾何信息也發佈出去
        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);
                    }
                }
            }
        }
    }

5.在publishCloud函數中我們可以看到，在我們計算的過程中參考系均爲機器人自身參考系，frame_id自然是base_link。

imageProjection程序中，輸入是雷達數據傳入的點雲信息，輸出的是被分割後的點雲，用來匹配特徵。接下來需要調用到featureAssociation.cpp中的函數，這個程序用來特徵聚類從而達到里程計的作用，避免博客太長，下一篇再寫。