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

接下來是featureAssociation.cpp，里程計部分，它分爲特徵點的提取與匹配兩部分。這是一個非常長的代碼，首先進入主函數：

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

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

    FeatureAssociation FA;

    ros::Rate rate(200);
    while (ros::ok())
    {
        ros::spinOnce();

        FA.runFeatureAssociation();

        rate.sleep();
    }

    ros::spin();
    return 0;
}

我們可以看到FeatureAssociation的構造函數初始化了一些ros話題的收發，接着就是一個死循環不斷的調用runFeatureAssociation（以每秒200HZ的頻率），先在這裏懷疑一下是否需要這麼高的頻率，轉到構造函數，發現它接收上一個節點的分割點雲、異常值以及IMU信息，卻發送了很多點雲的話題：

//沒有找到接收這四個話題的節點，可能是供可視化處理的
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>("/laser_cloud_corner_last", 2);
pubLaserCloudSurfLast = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_surf_last", 2);
pubOutlierCloudLast = nh.advertise<sensor_msgs::PointCloud2>("/outlier_cloud_last", 2);
pubLaserOdometry = nh.advertise<nav_msgs::Odometry> ("/laser_odom_to_init", 5);

另外在initializationValue函數中定義了降採樣濾波器，那麼點雲名稱最後加DS的就是降採樣之後的點雲。

再轉到runFeatureAssociation函數：

    void 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;
        }
        //進行形狀特徵的調整
        adjustDistortion();
        calculateSmoothness();
        markOccludedPoints();
        //提取特徵
        extractFeatures();
        publishCloud();
        //在配準之前，檢驗LM法是否初始化，接下來就是里程計部分
        if (!systemInitedLM)
	{
            checkSystemInitialization();
            return;
        }
        //提供粗配準的先驗以供優化
        updateInitialGuess();
        //優化併發送里程計信息
        updateTransformation();
        integrateTransformation();
        publishOdometry();
        //發送點雲以供建圖使用
        publishCloudsLast();
    }

也就是說，這個函數依次調用瞭如上的這些函數完成了一次特徵的匹配與里程計的計算，在建圖過程中它們將被反覆調用。在整個建圖過程中的特徵配準，雖然是採取ICP的方法，但利用點到點的一一配準太浪費時間，在這裏作者採取了點與線之間的距離作爲優化的目標，所以以LM初始化檢查爲分界，這個程序分爲特徵提取與配準計算兩個階段。

一. 特徵提取階段

1.adjustDistortion函數，它用於計算點雲之間的相對位姿與時間差，計算出相對移動的速度，並將得到的點雲轉換到IMU的座標系下。

//在這裏是利用插值的方法，relTime指的是角度的變化率，乘上scanPeriod（0.1秒）便得到在一個掃描週期內的角度變化量
float relTime = (ori - segInfo.startOrientation) / segInfo.orientationDiff;
point.intensity = int(segmentedCloud->points[i].intensity) + scanPeriod * relTime;

這一塊對IMU的處理非常巧妙，在每次處理點雲時，需要遍歷每一個點，把第一個點作爲初始座標，隨後的點均以第一個點爲參考做插值計算。

            if (imuPointerLast >= 0)
	    {
                float pointTime = relTime * scanPeriod;
                imuPointerFront = imuPointerLastIteration;
                while (imuPointerFront != imuPointerLast)
		{
                    if (timeScanCur + pointTime < imuTime[imuPointerFront])
		    {
                        break;
                    }
                    //imuQueLength是200，imuPointerFront在0~199之間輪轉
                    imuPointerFront = (imuPointerFront + 1) % imuQueLength;
                }
                //要是已經是最新數據那就拿下標爲imuPointerFront的數據作爲Cur
                if (timeScanCur + pointTime > imuTime[imuPointerFront])
	        {
                    imuRollCur = imuRoll[imuPointerFront];
                    imuPitchCur = imuPitch[imuPointerFront];
                    imuYawCur = imuYaw[imuPointerFront];

                    imuVeloXCur = imuVeloX[imuPointerFront];
                    imuVeloYCur = imuVeloY[imuPointerFront];
                    imuVeloZCur = imuVeloZ[imuPointerFront];

                    imuShiftXCur = imuShiftX[imuPointerFront];
                    imuShiftYCur = imuShiftY[imuPointerFront];
                    imuShiftZCur = imuShiftZ[imuPointerFront];
                }
                //否則用插值計算當前數據
		else
		{
                    int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
                    float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack])
                                                     / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
                    float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime)
                                                    / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);

                    imuRollCur = imuRoll[imuPointerFront] * ratioFront + imuRoll[imuPointerBack] * ratioBack;
                    imuPitchCur = imuPitch[imuPointerFront] * ratioFront + imuPitch[imuPointerBack] * ratioBack;
                    if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] > M_PI)
		    {
                        imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] + 2 * M_PI) * ratioBack;
                    }
		    else if (imuYaw[imuPointerFront] - imuYaw[imuPointerBack] < -M_PI)
		    {
                        imuYawCur = imuYaw[imuPointerFront] * ratioFront + (imuYaw[imuPointerBack] - 2 * M_PI) * ratioBack;
                    }
		    else
		    {
                        imuYawCur = imuYaw[imuPointerFront] * ratioFront + imuYaw[imuPointerBack] * ratioBack;
                    }

                    imuVeloXCur = imuVeloX[imuPointerFront] * ratioFront + imuVeloX[imuPointerBack] * ratioBack;
                    imuVeloYCur = imuVeloY[imuPointerFront] * ratioFront + imuVeloY[imuPointerBack] * ratioBack;
                    imuVeloZCur = imuVeloZ[imuPointerFront] * ratioFront + imuVeloZ[imuPointerBack] * ratioBack;

                    imuShiftXCur = imuShiftX[imuPointerFront] * ratioFront + imuShiftX[imuPointerBack] * ratioBack;
                    imuShiftYCur = imuShiftY[imuPointerFront] * ratioFront + imuShiftY[imuPointerBack] * ratioBack;
                    imuShiftZCur = imuShiftZ[imuPointerFront] * ratioFront + imuShiftZ[imuPointerBack] * ratioBack;
                }
                //如果是在點雲中的第一個點，就將數據作爲初始座標系
                if (i == 0)
		{
                    imuRollStart = imuRollCur;
                    imuPitchStart = imuPitchCur;
                    imuYawStart = imuYawCur;

                    imuVeloXStart = imuVeloXCur;
                    imuVeloYStart = imuVeloYCur;
                    imuVeloZStart = imuVeloZCur;

                    imuShiftXStart = imuShiftXCur;
                    imuShiftYStart = imuShiftYCur;
                    imuShiftZStart = imuShiftZCur;

                    if (timeScanCur + pointTime > imuTime[imuPointerFront])
		    {
                        imuAngularRotationXCur = imuAngularRotationX[imuPointerFront];
                        imuAngularRotationYCur = imuAngularRotationY[imuPointerFront];
                        imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront];
                    }
		    else
		    {
                        int imuPointerBack = (imuPointerFront + imuQueLength - 1) % imuQueLength;
                        float ratioFront = (timeScanCur + pointTime - imuTime[imuPointerBack])
                                                         / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
                        float ratioBack = (imuTime[imuPointerFront] - timeScanCur - pointTime)
                                                        / (imuTime[imuPointerFront] - imuTime[imuPointerBack]);
                        imuAngularRotationXCur = imuAngularRotationX[imuPointerFront] * ratioFront + imuAngularRotationX[imuPointerBack] * ratioBack;
                        imuAngularRotationYCur = imuAngularRotationY[imuPointerFront] * ratioFront + imuAngularRotationY[imuPointerBack] * ratioBack;
                        imuAngularRotationZCur = imuAngularRotationZ[imuPointerFront] * ratioFront + imuAngularRotationZ[imuPointerBack] * ratioBack;
                    }

                    imuAngularFromStartX = imuAngularRotationXCur - imuAngularRotationXLast;
                    imuAngularFromStartY = imuAngularRotationYCur - imuAngularRotationYLast;
                    imuAngularFromStartZ = imuAngularRotationZCur - imuAngularRotationZLast;

                    imuAngularRotationXLast = imuAngularRotationXCur;
                    imuAngularRotationYLast = imuAngularRotationYCur;
                    imuAngularRotationZLast = imuAngularRotationZCur;

                    updateImuRollPitchYawStartSinCos();
                }
                //如果不是第一個點
                else
		{
                    //把速度與位移轉換回IMU的初始座標系下
                    VeloToStartIMU();
                    TransformToStartIMU(&point);
                }
            }

2.calculateSmoothness函數檢測了點雲特徵點的曲率，具體方法是將點雲的每一層拿出分析，計算每個點左右各五個點的偏差作爲曲率，將曲率保存在cloudSmoothness中。

3.markOccludedPoints是去除一些瑕點，這些瑕點指的是在點雲中可能出現的互相遮擋的情況，如果兩個特徵點距離過近（小於某個閾值），則取更近的那個點做下一步處理。

void markOccludedPoints()
{
    int cloudSize = segmentedCloud->points.size();
    //遍歷每一個點
    for (int i = 5; i < cloudSize - 6; ++i)
    {
        //depth1和depth2分別是兩點的深度
        float depth1 = segInfo.segmentedCloudRange[i];
        float depth2 = segInfo.segmentedCloudRange[i+1];
        //將可能存在遮擋的點去除，將遠側的點視爲瑕點，這裏的處理方法和LOAM不太一樣，是直接採取比較點的下標，去掉其中一側的6個點
        int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[i+1] - segInfo.segmentedCloudColInd[i]));

        if (columnDiff < 10)
        {
            if (depth1 - depth2 > 0.3)
            {
                 cloudNeighborPicked[i - 5] = 1;
                 cloudNeighborPicked[i - 4] = 1;
                 cloudNeighborPicked[i - 3] = 1;
                 cloudNeighborPicked[i - 2] = 1;
                 cloudNeighborPicked[i - 1] = 1;
                 cloudNeighborPicked[i] = 1;
            }
	    else if (depth2 - depth1 > 0.3)
	    {
                 cloudNeighborPicked[i + 1] = 1;
                 cloudNeighborPicked[i + 2] = 1;
                 cloudNeighborPicked[i + 3] = 1;
                 cloudNeighborPicked[i + 4] = 1;
                 cloudNeighborPicked[i + 5] = 1;
                 cloudNeighborPicked[i + 6] = 1;
            }
        }
        //diff1和diff2是當前點距離前後兩個點的距離
        float diff1 = std::abs(segInfo.segmentedCloudRange[i-1] - segInfo.segmentedCloudRange[i]);
        float diff2 = std::abs(segInfo.segmentedCloudRange[i+1] - segInfo.segmentedCloudRange[i]);
        //如果當前點距離左右鄰點都過遠，則視其爲瑕點，因爲入射角可能太小導致誤差較大
        if (diff1 > 0.02 * segInfo.segmentedCloudRange[i] && diff2 > 0.02 * segInfo.segmentedCloudRange[i])
        cloudNeighborPicked[i] = 1;
    }
}

4.extractFeatures函數，不加註釋導致看了很久。。。。首先它的做法是將每一層點雲分成6份，每一份中，對點的曲率進行排序，sp和ep分別是這段點雲的起始點與終止點。。。從而判斷出角點與平面點，根據label值，分類爲sharp，lesssharp，flat，lessflat。分類後分別提供給ros話題。下面是對角點特徵的篩選：

for (int k = ep; k >= sp; k--)
{
    int ind = cloudSmoothness[k].ind;
    //曲率大於edgeThreshold（0.1）則視爲角點，而且不能是地面點
    if (cloudNeighborPicked[ind] == 0 &&
    cloudCurvature[ind] > edgeThreshold &&
    segInfo.segmentedCloudGroundFlag[ind] == false)
    {
        largestPickedNum++;
        if (largestPickedNum <= 2)
	{
            cloudLabel[ind] = 2;
            cornerPointsSharp->push_back(segmentedCloud->points[ind]);
            cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
        }
											else if (largestPickedNum <= 20)
	{
            cloudLabel[ind] = 1;
            cornerPointsLessSharp->push_back(segmentedCloud->points[ind]);
        }
	else
	{
            break;
        }
        //防止重複處理
        cloudNeighborPicked[ind] = 1;
        for (int l = 1; l <= 5; l++)
	{
            int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l - 1]));
            if (columnDiff > 10)
                break;
            cloudNeighborPicked[ind + l] = 1;
        }
        for (int l = -1; l >= -5; l--)
	{
            int columnDiff = std::abs(int(segInfo.segmentedCloudColInd[ind + l] - segInfo.segmentedCloudColInd[ind + l + 1]));
            if (columnDiff > 10)
                break;
            cloudNeighborPicked[ind + l] = 1;
        }
    }
}

二. 配準計算階段

1.updateInitialGuess函數將當前時刻保存的IMU數據作爲先驗數據。

2.updateTransformation分別對平面特徵與角點特徵進行了匹配計算，首先找到各自對應的特徵，下面是角部特徵的函數：

    void findCorrespondingCornerFeatures(int iterCount)
    {
        int cornerPointsSharpNum = cornerPointsSharp->points.size();
        //對角部特徵點依次進行處理
        for (int i = 0; i < cornerPointsSharpNum; i++)
	{
            //根據IMU數據的將點雲轉換到上一次掃描的位置中
            TransformToStart(&cornerPointsSharp->points[i], &pointSel);
            //每五次迭代尋找一次鄰域點，否則使用上次的鄰域查找
            if (iterCount % 5 == 0)
	    {
                kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);
                int closestPointInd = -1, minPointInd2 = -1;
                //pointSearchSqDis是平方距離，閾值是25
                if (pointSearchSqDis[0] < nearestFeatureSearchSqDist)
		{
                    closestPointInd = pointSearchInd[0];
                    //得到最近鄰所在的層數
                    int closestPointScan = int(laserCloudCornerLast->points[closestPointInd].intensity);

                    float pointSqDis, minPointSqDis2 = nearestFeatureSearchSqDist;
                    for (int j = closestPointInd + 1; j < cornerPointsSharpNum; j++)
		    {
                        //最近鄰需要在上下兩層之內否則失敗
                        if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan + 2.5)
			{
                            break;
                        }

                        pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
                                     (laserCloudCornerLast->points[j].x - pointSel.x) +
                                     (laserCloudCornerLast->points[j].y - pointSel.y) *
                                     (laserCloudCornerLast->points[j].y - pointSel.y) +
                                     (laserCloudCornerLast->points[j].z - pointSel.z) *
                                     (laserCloudCornerLast->points[j].z - pointSel.z);

                        if (int(laserCloudCornerLast->points[j].intensity) > closestPointScan)
			{
                            if (pointSqDis < minPointSqDis2)
			    {
                                minPointSqDis2 = pointSqDis;
                                minPointInd2 = j;
                            }
                        }
                    }
                    for (int j = closestPointInd - 1; j >= 0; j--)
		    {
                        if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan - 2.5)
			{
                            break;
                        }

                        pointSqDis = (laserCloudCornerLast->points[j].x - pointSel.x) *
                                     (laserCloudCornerLast->points[j].x - pointSel.x) +
                                     (laserCloudCornerLast->points[j].y - pointSel.y) *
                                     (laserCloudCornerLast->points[j].y - pointSel.y) +
                                     (laserCloudCornerLast->points[j].z - pointSel.z) *
                                     (laserCloudCornerLast->points[j].z - pointSel.z);

                        if (int(laserCloudCornerLast->points[j].intensity) < closestPointScan)
			{
                            if (pointSqDis < minPointSqDis2)
			    {
                                minPointSqDis2 = pointSqDis;
                                minPointInd2 = j;
                            }
                        }
                    }
                }

                pointSearchCornerInd1[i] = closestPointInd;
                pointSearchCornerInd2[i] = minPointInd2;
            }
            //這裏是開始計算點到直線的距離，tripod即三角形，根據三角形餘弦定理計算距離並求偏導數
            if (pointSearchCornerInd2[i] >= 0)
	    {
                tripod1 = laserCloudCornerLast->points[pointSearchCornerInd1[i]];
                tripod2 = laserCloudCornerLast->points[pointSearchCornerInd2[i]];

                float x0 = pointSel.x;
                float y0 = pointSel.y;
                float z0 = pointSel.z;
                float x1 = tripod1.x;
                float y1 = tripod1.y;
                float z1 = tripod1.z;
                float x2 = tripod2.x;
                float y2 = tripod2.y;
                float z2 = tripod2.z;

                float m11 = ((x0 - x1)*(y0 - y2) - (x0 - x2)*(y0 - y1));
                float m22 = ((x0 - x1)*(z0 - z2) - (x0 - x2)*(z0 - z1));
                float m33 = ((y0 - y1)*(z0 - z2) - (y0 - y2)*(z0 - z1));

                float a012 = sqrt(m11 * m11  + m22 * m22 + m33 * m33);

                float l12 = sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2) + (z1 - z2)*(z1 - z2));

                float la =  ((y1 - y2)*m11 + (z1 - z2)*m22) / a012 / l12;

                float lb = -((x1 - x2)*m11 - (z1 - z2)*m33) / a012 / l12;

                float lc = -((x1 - x2)*m22 + (y1 - y2)*m33) / a012 / l12;

                float ld2 = a012 / l12;

                float s = 1;
                if (iterCount >= 5)
		{
                    s = 1 - 1.8 * fabs(ld2);
                }

                if (s > 0.1 && ld2 != 0)
		{
                    coeff.x = s * la;
                    coeff.y = s * lb;
                    coeff.z = s * lc;
                    coeff.intensity = s * ld2;

                    laserCloudOri->push_back(cornerPointsSharp->points[i]);
                    coeffSel->push_back(coeff);
                }
            }
        }
    }

平面特徵與角部特徵的計算類似，都使用了LM法進行迭代，具體計算在calculateTransformationSurf與calculateTransformationCorner當中，按下不表。

3.integrateTransformation函數是將IMU信息融入到位姿更新當中。

    void integrateTransformation()
    {
        float rx, ry, rz, tx, ty, tz;
        //transformSum是IMU的累計變化量，0、1、2分別是pitch、yaw、roll，transformCur是當前的IMU數據，AccumulateRotation是爲了使局部座標轉換爲全局座標
        AccumulateRotation(transformSum[0], transformSum[1], transformSum[2],
                           -transformCur[0], -transformCur[1], -transformCur[2], rx, ry, rz);

        float x1 = cos(rz) * (transformCur[3] - imuShiftFromStartX)
                 - sin(rz) * (transformCur[4] - imuShiftFromStartY);
        float y1 = sin(rz) * (transformCur[3] - imuShiftFromStartX)
                 + cos(rz) * (transformCur[4] - imuShiftFromStartY);
        float z1 = transformCur[5] - imuShiftFromStartZ;

        float x2 = x1;
        float y2 = cos(rx) * y1 - sin(rx) * z1;
        float z2 = sin(rx) * y1 + cos(rx) * z1;

        tx = transformSum[3] - (cos(ry) * x2 + sin(ry) * z2);
        ty = transformSum[4] - y2;
        tz = transformSum[5] - (-sin(ry) * x2 + cos(ry) * z2);
        //加入IMU當前數據更新位姿
        PluginIMURotation(rx, ry, rz, imuPitchStart, imuYawStart, imuRollStart,
                          imuPitchLast, imuYawLast, imuRollLast, rx, ry, rz);

        transformSum[0] = rx;
        transformSum[1] = ry;
        transformSum[2] = rz;
        transformSum[3] = tx;
        transformSum[4] = ty;
        transformSum[5] = tz;
    }

4.publishOdometry即是將上一條中計算出的位姿變爲里程計信息與tf變換髮送出去。

這就是第二個程序的主要流程，我們需要的就是里程計的信息。代碼裏面有很多坑，也沒有註釋，今後慢慢學習。。。

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

PDManer [元數建模]-v4.9.0 發佈：一款簡單好用的數據庫建模平臺

使用neovim打造go ide(支持代碼跳轉, 代碼補全, 實時語法檢查)

sql求連續值問題

cs01 CSS Syntax

挑戰程序設計競賽 2.3章習題 poj 3046 Ant Counting

[MASM拾遺]Offset僞指令

h30 HTML Layout Elements

瞭解顯卡

一款基於C#開發的通訊調試工具（支持Modbus RTU、MQTT調試）

Linux/Golang/glibC系統調用

重讀數據結構之爬樓梯問題（動態規劃）

軟件開發隨筆：用C++實現上下層級的類的數據共享

ros源碼分析（四）service的實時反饋機制

ros源碼分析（三）ros::spin( )背後發生的事

ros源碼探索（二）publish之後發生了什麼？

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結