LOAM筆記及A-LOAM源碼閱讀

導讀

下面是我對LOAM論文的理解以及對A-LOAM的源碼閱讀（中文注釋版的A-LOAM已經push到github，見A-LOAM-NOTED），最后也會手推一下LOAM源碼中高斯牛頓法（論文中說的是LM法）求解ICP配准的非線性最小二乘問題，這一部分在A-LOAM代碼中用Ceres優化函數庫做了簡化，CSDN也有篇不錯的博文，知乎也有篇很贊的文章，如有問題，歡迎在評論區討論。

碼字繪圖不易，轉載請注明出處：https://www.cnblogs.com/wellp/p/8877990.html

一、Paper Reading

1. 綜述

LOAM是一個激光里程計算法，沒有閉環檢測，也就沒有加入圖優化框架，該算法把SLAM問題分為兩個算法並行運行：一個odometry算法，10Hz；另一個mapping算法，1Hz，最終將兩者輸出的pose做整合，實現10Hz的位姿實時輸出。兩個算法都是使用點雲中提取出尖銳的邊點和平整的面點作為特征點，然后進行特征點匹配，來估計lidar的位姿以及對位姿進行fine tune。匹配特征點時，不是簡單的點到點，而是edge point到edge line（點到線）以及planar point到planar patch（點到面），在后續運動估計中建立的殘差距離也就是點到線的距離以及點到面的距離，所以感覺LOAM匹配的過程就像是一個點到線以及點到面相結合的ICP算法；需要注意的是odometry和mapping算法的edge line和planar patch（即匹配時的target）的確定略有不同，具體看第3小結的介紹。

2. odometry算法

1）輸入是已經消除畸變（所謂的消除畸變，也解釋為運動補償，就是把P_k點雲中的所有的點都映射到同一個時刻t_k+1，消除由於車輛運動和lidar的旋轉導致的點雲畸變現象）的上一次掃描的點雲、此次掃描過程中逐漸被觀測到的點雲P_k+1、[t_k+1, t]時刻的pose之間的增量 （也解釋為pose之間的transform）；

2）如果是一次新的掃描就讓等於0；

3）然后從點雲P_k+1中提取邊點和平面點作為特征點，分別構成邊特征點集合和面特征點集合；

4）對於集合中的每一個邊點以及平面點我們找到在的對應邊和對應平面，然后根據（11）式，構建點到線和點到面的距離殘差方程，進而估計Lidar的運動，其中優化變量是，優化目標是殘差距離，即求一個最優的，使得點到線以及點到面的距離最小；

5）每個殘差方程都分配一個權重，特征點到對應邊（或者對應面）的距離越大就分配越小的權重，如果距離大於一定閾值，就把權重設置成0；

6）利用LM法（源碼中用的是高斯牛頓法）對這個非線性最小二乘問題進行求解，得到，這樣就可以更新 ；

7）當非線性優化收斂或者迭代次數到達預設值后，break；

8）如果本次odometry算法已經到達一次掃描的結束時刻就把P_k+1的所有點映射到t_k+2時刻得到，完成對P_k+1點雲的運動補償，否則就只輸出已經更新的，為下一次的odometry算法做准備。

論文中的偽代碼如下：

3. mapping算法

1）mapping算法就是將已經消除畸變的點雲先轉換到全局坐標系變成，然后與局部地圖（local map或者稱為submap，源碼中使用的是三維柵格cube做的局部地圖管理）做特征匹配，將配准好的點雲添加到中，一旦超過預設的范圍，就刪除范圍外的點雲，源碼中submap的范圍是250m * 250m * 150m，需要注意的是，源碼中對submap的管理並不是基於時序的滑動窗口方式，而是空間范圍划分方式，這就是為什么最后的frame對應的submap會包含早期的frames：

這種submap方式比基於時序的滑動窗口方式會包含更多的歷史幀信息，對於點雲配准更友好，當然這也就導致內存占用增加；

2）mapping算法中，從中提取特征點的方式跟之前一樣，也就是根據曲率值判斷一個點是邊點還是面點，但是特征點的數量是odometry算法的10倍，具體通過調整曲率值的比較閾值實現，源碼中其實也不是10倍了，反正就是比odometry算法提取的特征點會多；

3）然后就是匹配，即在Q_k中找特征點的correspondence，在odometry算法中，correspondence的確定是為了最快的計算速度，使用的是基於最近鄰的思路來找對應線以及對應面，具體為：最近鄰的2個邊點構成對應邊edge line、最近鄰的3個面點構成對應面planar patch；而mapping算法是通過對特征點周圍的點雲簇（源碼是使用了最近鄰的5個點作為點雲簇）進行PCA主成分分析（求點雲簇的協方差矩陣的特征值和特征向量），來確定找到對應邊edge line的方向向量以及確定對應面planar patch的法向量。correspondence確定后的優化求解過程就跟之前一樣了，實現將配准到局部地圖，進而達到對odometry算法計算出的位姿進行fine tune的目的。

 4. 系統整體過程

這里主要說下mapping輸出的1Hz位姿和odometry輸出的10Hz位姿的整合過程：當mapping計算出一個位姿P_map后，就跟與之時間同步的一個odometry位姿P_odom計算增量△P = P_map * P_odom^-1，然后用△P去修正后續的高頻odometry位姿，修正后的10Hz高頻位姿就是LOAM最終輸出的實時位姿，然后直到mapping算法再計算出一個位姿后重復上述過程。

二、A-LOAM源碼閱讀

1. 節點圖

先放一個運行A-LOAM時的ros節點圖，整體架構如代碼一般清晰。

2. 代碼整體綜述

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

	docker目錄	提供了docker環境，方便開發者搭建環境
	include目錄	包含一個簡單的通用頭文件以及一個計時器的類TicToc，計時單位為ms
	launch目錄	ros的啟動文件
	out、picture目錄	預留的編譯產出目錄、README.md中的圖片
	rviz_cfg目錄	rviz的配置文件
	src目錄	核心代碼：4個cpp分別對應了1.節點圖中的4個node；1個hpp為基於Ceres構建殘差函數時使用的各個仿函數類（是不是命名成Functor，更直觀？）

3.kittiHelper.cpp

1）對應節點：/kittiHelper

2）功能：讀取kitti odometry的數據集的數據，具體包括點雲、左右相機的圖像、以及pose的groundtruth（訓練集才有），然后分成5個topic以10Hz（可修改）的頻率發布出去，其中真正對算法有用的topic只有點雲/velodyne_points，其他四個topic都是在rviz中可視化用。

3）代碼分析：這部分代碼主要是ros的消息發布，需要注意的是，kitti的訓練集真值pose的坐標系和點雲的坐標系不相同如下圖所示，A-LOAM為了統一，坐標系均采用點雲的坐標系，所以需要將真值pose進行坐標變換，轉到點雲的坐標系下

A-LOAM在此處還存在一個坐標變換的bug，如下所示，當然這個bug只會影響可視化真值軌跡時的姿態，並不影響后續算法的運行：

Eigen::Quaterniond q_w_i(gt_pose.topLeftCorner<3, 3>());
// Eigen::Quaterniond q = q_transform * q_w_i;// 此處應該添加 * q_transform.inverse()，如下所示
Eigen::Quaterniond q = q_transform * q_w_i *q_transform.inverse();
q.normalize();
Eigen::Vector3d t = q_transform * gt_pose.topRightCorner<3, 1>();

 4.scanRegistration.cpp

1）對應節點：/ascanRegistration

2）功能：對輸入點雲進行濾波，提取4種feature，即邊緣點特征sharp、less_sharp，面特征flat、less_flat

3）代碼分析：

main函數主要是訂閱前一個節點發布的點雲topic，一旦接受到一幀點雲就執行一次回調函數laserCloudHandler，所以重點來看該回調函數

// 首先對點雲濾波，去除NaN值的無效點，以及在Lidar坐標系原點MINIMUM_RANGE距離以內的點
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);

（假設64線Lidar的水平角度分辨率是0.2deg，則每個scan理論有360/0.2=1800個點，為方便敘述，我們稱每個scan點的ID為fireID，即fireID [0~1799]，相應的scanID [0~63] ）

接下來通過Lidar坐標系下點的仰角以及水平夾角計算點雲的scanID和fireID，需要注意的是這里的計算方式其實並不適用kitti數據集的點雲，因為kitti的點雲已經被運動補償過。

point.intensity = scanID + scanPeriod * relTime;// intensity字段的整數部分存放scanID，小數部分存放歸一化后的fireID
laserCloudScans[scanID].push_back(point); // 將點根據scanID放到對應的子點雲laserCloudScans中

然后將子點雲laserCloudScans合並成一幀點雲laserCloud，注意這里的單幀點雲laserCloud可以認為已經是有序點雲了，按照scanID和fireID的順序存放

for (int i = 0; i < N_SCANS; i++)
{ 
    scanStartInd[i] = laserCloud->size() + 5;// 記錄每個scan的開始index，忽略前5個點
    *laserCloud += laserCloudScans[i];
    scanEndInd[i] = laserCloud->size() - 6;// 記錄每個scan的結束index，忽略后5個點，開始和結束處的點雲容易產生不閉合的“接縫”，對提取edge feature不利
}

 接下來計算點的曲率

for (int i = 5; i < cloudSize - 5; i++)
{ 
     float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
     float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
     float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;

     cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
     cloudSortInd[i] = i;
     cloudNeighborPicked[i] = 0;// 點有沒有被選選擇為feature點
     cloudLabel[i] = 0;// Label 2: corner_sharp
                       // Label 1: corner_less_sharp, 包含Label 2
                       // Label -1: surf_flat
                       // Label 0: surf_less_flat， 包含Label -1，因為點太多，最后會降采樣
}

下面的一個大的for loop就是根據曲率計算4種特征點

for (int i = 0; i < N_SCANS; i++)// 按照scan的順序提取4種特征點
{
    if (scanEndInd[i] - scanStartInd[i] < 6)// 如果該scan的點數少於7個點，就跳過
        continue;
    pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
    for (int j = 0; j < 6; j++)// 將該scan分成6小段執行特征檢測
    {
        int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;// subscan的起始index
        int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;// subscan的結束index

        TicToc t_tmp;
        std::sort(cloudSortInd + sp, cloudSortInd + ep + 1, comp);// 根據曲率有小到大對subscan的點進行sort
        t_q_sort += t_tmp.toc();

        int largestPickedNum = 0;
        for (int k = ep; k >= sp; k--)// 從后往前，即從曲率大的點開始提取corner feature
        {
            int ind = cloudSortInd[k];

            if (cloudNeighborPicked[ind] == 0 &&
                cloudCurvature[ind] > 0.1)// 如果該點沒有被選擇過，並且曲率大於0.1
            {
                largestPickedNum++;
                if (largestPickedNum <= 2)// 該subscan中曲率最大的前2個點認為是corner_sharp特征點
                {
                    cloudLabel[ind] = 2;
                    cornerPointsSharp.push_back(laserCloud->points[ind]);
                    cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                }
                else if (largestPickedNum <= 20)// 該subscan中曲率最大的前20個點認為是corner_less_sharp特征點
                {
                    cloudLabel[ind] = 1;
                    cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                }
                else
                {
                    break;
                }

                cloudNeighborPicked[ind] = 1;// 標記該點被選擇過了

                // 與當前點距離的平方 <= 0.05的點標記為選擇過，避免特征點密集分布
                for (int l = 1; l <= 5; l++)
                {
                    float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                    float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                    float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                    if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                    {
                        break;
                    }

                    cloudNeighborPicked[ind + l] = 1;
                }
                for (int l = -1; l >= -5; l--)
                {
                    float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                    float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                    float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                    if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                    {
                        break;
                    }

                    cloudNeighborPicked[ind + l] = 1;
                }
            }
        }

        // 提取surf平面feature，與上述類似，選取該subscan曲率最小的前4個點為surf_flat
        int smallestPickedNum = 0;
        for (int k = sp; k <= ep; k++)
        {
            int ind = cloudSortInd[k];

            if (cloudNeighborPicked[ind] == 0 &&
                cloudCurvature[ind] < 0.1)
            {

                cloudLabel[ind] = -1;
                surfPointsFlat.push_back(laserCloud->points[ind]);

                smallestPickedNum++;
                if (smallestPickedNum >= 4)
                {
                    break;
                }

                cloudNeighborPicked[ind] = 1;
                for (int l = 1; l <= 5; l++)
                {
                    float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                    float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                    float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                    if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                    {
                        break;
                    }

                    cloudNeighborPicked[ind + l] = 1;
                }
                for (int l = -1; l >= -5; l--)
                {
                    float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                    float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                    float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                    if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05)
                    {
                        break;
                    }

                    cloudNeighborPicked[ind + l] = 1;
                }
            }
        }

        // 其他的非corner特征點與surf_flat特征點一起組成surf_less_flat特征點
        for (int k = sp; k <= ep; k++)
        {
            if (cloudLabel[k] <= 0)
            {
                surfPointsLessFlatScan->push_back(laserCloud->points[k]);
            }
        }
    }

    // 最后對該scan點雲中提取的所有surf_less_flat特征點進行降采樣，因為點太多了
    pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
    pcl::VoxelGrid<PointType> downSizeFilter;
    downSizeFilter.setInputCloud(surfPointsLessFlatScan);
    downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
    downSizeFilter.filter(surfPointsLessFlatScanDS);

    surfPointsLessFlat += surfPointsLessFlatScanDS;
}

最后就是將當前幀點雲提取出的4種特征點與濾波后的當前幀點雲進行publish

5.laserOdometry.cpp

1）對應節點：/alaserOdometry

2）功能：基於前述的4種feature進行幀與幀的點雲特征配准，即簡單的Lidar Odometry

3）代碼分析：

a. 先明確幾個關鍵變量的含義

// Lidar Odometry線程估計的frame在world坐標系的位姿P，Transformation from current frame to world frame
Eigen::Quaterniond q_w_curr(1, 0, 0, 0);
Eigen::Vector3d t_w_curr(0, 0, 0);

// 點雲特征匹配時的優化變量
double para_q[4] = {0, 0, 0, 1};
double para_t[3] = {0, 0, 0};

// 下面的2個分別是優化變量para_q和para_t的映射：表示的是兩個world坐標系下的位姿P之間的增量，例如△P = P0.inverse() * P1
Eigen::Map<Eigen::Quaterniond> q_last_curr(para_q);
Eigen::Map<Eigen::Vector3d> t_last_curr(para_t);

b. 再介紹一下main函數之前的幾個函數

TransformToStart：將當前幀Lidar坐標系下的點雲變換到上一幀Lidar坐標系下（也就是當前幀的初始位姿，起始位姿，所以函數名是TransformToStart），因為kitti點雲已經去除了畸變，所以不再考慮運動補償。（如果點雲沒有去除畸變，用slerp差值的方式計算出每個點在fire時刻的位姿，然后進行TransformToStart的坐標變換，一方面實現了變換到上一幀Lidar坐標系下；另一方面也可以理解成點都將fire時刻統一到了開始時刻，即去除了畸變，完成了運動補償）

之后的5個Handler函數為接受上游5個topic的回調函數，作用是將消息緩存到對應的queue中，以便后續處理。

c. 然后就是main函數，即Odometry線程的核心代碼部分：

if (!systemInited)// 第一幀不進行匹配，僅僅將 cornerPointsLessSharp 保存至 laserCloudCornerLast
                  //                   將 surfPointsLessFlat    保存至 laserCloudSurfLast，以及更新對應的kdtree
                  // 為下次匹配提供target
{
    systemInited = true;
    std::cout << "Initialization finished \n";
}

之后一個關鍵的大for loop：

for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)// 點到線以及點到面的ICP，迭代2次
{
    ...
}

接着介紹這個for loop里面的內容，關於Ceres的使用，就不介紹了，可以參考Ceres函數庫的說明文檔，或者高翔的《視覺SLAM十四講》中的Ceres示例，注意一下優化變量是兩個world坐標系下的位姿P的增量para_q和para_t

for (size_t opti_counter = 0; opti_counter < 2; ++opti_counter)// 點到線以及點到面的ICP，迭代2次
{
    corner_correspondence = 0;
    plane_correspondence = 0;

    //ceres::LossFunction *loss_function = NULL;
    ceres::LossFunction* loss_function = new ceres::HuberLoss(0.1);
    ceres::LocalParameterization* q_parameterization =
        new ceres::EigenQuaternionParameterization();
    ceres::Problem::Options problem_options;

    ceres::Problem problem(problem_options);
    problem.AddParameterBlock(para_q, 4, q_parameterization);
    problem.AddParameterBlock(para_t, 3);

    pcl::PointXYZI pointSel;
    std::vector<int> pointSearchInd;
    std::vector<float> pointSearchSqDis;

    TicToc t_data;
    // 基於最近鄰原理建立corner特征點之間關聯，find correspondence for corner features
    for (int i = 0; i < cornerPointsSharpNum; ++i)
    {
        TransformToStart(&(cornerPointsSharp->points[i]), &pointSel);// 將當前幀的corner_sharp特征點O_cur，從當前幀的Lidar坐標系下變換到上一幀的Lidar坐標系下（記為點O，注意與前面的點O_cur不同），以利於尋找corner特征點的correspondence
        kdtreeCornerLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);// kdtree中的點雲是上一幀的corner_less_sharp，所以這是在上一幀
                                                                                        // 的corner_less_sharp中尋找當前幀corner_sharp特征點O的最近鄰點（記為A）

        int closestPointInd = -1, minPointInd2 = -1;
        if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)// 如果最近鄰的corner特征點之間距離平方小於閾值，則最近鄰點A有效
        {
            closestPointInd = pointSearchInd[0];
            int closestPointScanID = int(laserCloudCornerLast->points[closestPointInd].intensity);

            double minPointSqDis2 = DISTANCE_SQ_THRESHOLD;
            // 尋找點O的另外一個最近鄰的點（記為點B） in the direction of increasing scan line
            for (int j = closestPointInd + 1; j < (int)laserCloudCornerLast->points.size(); ++j)// laserCloudCornerLast 來自上一幀的corner_less_sharp特征點,由於提取特征時是
            {                                                                                   // 按照scan的順序提取的，所以laserCloudCornerLast中的點也是按照scanID遞增的順序存放的
                // if in the same scan line, continue
                if (int(laserCloudCornerLast->points[j].intensity) <= closestPointScanID)// intensity整數部分存放的是scanID
                    continue;

                // if not in nearby scans, end the loop
                if (int(laserCloudCornerLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
                    break;

                double 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 (pointSqDis < minPointSqDis2)// 第二個最近鄰點有效，更新點B
                {
                    // find nearer point
                    minPointSqDis2 = pointSqDis;
                    minPointInd2 = j;
                }
            }

            // 尋找點O的另外一個最近鄰的點B in the direction of decreasing scan line
            for (int j = closestPointInd - 1; j >= 0; --j)
            {
                // if in the same scan line, continue
                if (int(laserCloudCornerLast->points[j].intensity) >= closestPointScanID)
                    continue;

                // if not in nearby scans, end the loop
                if (int(laserCloudCornerLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
                    break;

                double 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 (pointSqDis < minPointSqDis2)// 第二個最近鄰點有效，更新點B
                {
                    // find nearer point
                    minPointSqDis2 = pointSqDis;
                    minPointInd2 = j;
                }
            }
        }
        if (minPointInd2 >= 0) // both closestPointInd and minPointInd2 is valid
        {                      // 即特征點O的兩個最近鄰點A和B都有效
            Eigen::Vector3d curr_point(cornerPointsSharp->points[i].x,
                cornerPointsSharp->points[i].y,
                cornerPointsSharp->points[i].z);
            Eigen::Vector3d last_point_a(laserCloudCornerLast->points[closestPointInd].x,
                laserCloudCornerLast->points[closestPointInd].y,
                laserCloudCornerLast->points[closestPointInd].z);
            Eigen::Vector3d last_point_b(laserCloudCornerLast->points[minPointInd2].x,
                laserCloudCornerLast->points[minPointInd2].y,
                laserCloudCornerLast->points[minPointInd2].z);

            double s;// 運動補償系數，kitti數據集的點雲已經被補償過，所以s = 1.0
            if (DISTORTION)
                s = (cornerPointsSharp->points[i].intensity - int(cornerPointsSharp->points[i].intensity)) / SCAN_PERIOD;
            else
                s = 1.0;
            // 用點O，A，B構造點到線的距離的殘差項，注意這三個點都是在上一幀的Lidar坐標系下，即，殘差 = 點O到直線AB的距離
            // 具體到介紹lidarFactor.cpp時再說明該殘差的具體計算方法
            ceres::CostFunction* cost_function = LidarEdgeFactor::Create(curr_point, last_point_a, last_point_b, s);
            problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
            corner_correspondence++;
        }
    }
    // 下面說的點符號與上述相同
    // 與上面的建立corner特征點之間的關聯類似，尋找平面特征點O的最近鄰點ABC，即基於最近鄰原理建立surf特征點之間的關聯，find correspondence for plane features
    for (int i = 0; i < surfPointsFlatNum; ++i)
    {
        TransformToStart(&(surfPointsFlat->points[i]), &pointSel);
        kdtreeSurfLast->nearestKSearch(pointSel, 1, pointSearchInd, pointSearchSqDis);

        int closestPointInd = -1, minPointInd2 = -1, minPointInd3 = -1;
        if (pointSearchSqDis[0] < DISTANCE_SQ_THRESHOLD)// 找到的最近鄰點A有效
        {
            closestPointInd = pointSearchInd[0];

            // get closest point's scan ID
            int closestPointScanID = int(laserCloudSurfLast->points[closestPointInd].intensity);
            double minPointSqDis2 = DISTANCE_SQ_THRESHOLD, minPointSqDis3 = DISTANCE_SQ_THRESHOLD;

            // search in the direction of increasing scan line
            for (int j = closestPointInd + 1; j < (int)laserCloudSurfLast->points.size(); ++j)
            {
                // if not in nearby scans, end the loop
                if (int(laserCloudSurfLast->points[j].intensity) > (closestPointScanID + NEARBY_SCAN))
                    break;

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

                // if in the same or lower scan line
                if (int(laserCloudSurfLast->points[j].intensity) <= closestPointScanID && pointSqDis < minPointSqDis2)
                {
                    minPointSqDis2 = pointSqDis;// 找到的第2個最近鄰點有效，更新點B，注意如果scanID准確的話，一般點A和點B的scanID相同
                    minPointInd2 = j;
                }
                // if in the higher scan line
                else if (int(laserCloudSurfLast->points[j].intensity) > closestPointScanID && pointSqDis < minPointSqDis3)
                {
                    minPointSqDis3 = pointSqDis;// 找到的第3個最近鄰點有效，更新點C，注意如果scanID准確的話，一般點A和點B的scanID相同,且與點C的scanID不同，與LOAM的paper敘述一致
                    minPointInd3 = j;
                }
            }

            // search in the direction of decreasing scan line
            for (int j = closestPointInd - 1; j >= 0; --j)
            {
                // if not in nearby scans, end the loop
                if (int(laserCloudSurfLast->points[j].intensity) < (closestPointScanID - NEARBY_SCAN))
                    break;

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

                // if in the same or higher scan line
                if (int(laserCloudSurfLast->points[j].intensity) >= closestPointScanID && pointSqDis < minPointSqDis2)
                {
                    minPointSqDis2 = pointSqDis;
                    minPointInd2 = j;
                }
                else if (int(laserCloudSurfLast->points[j].intensity) < closestPointScanID && pointSqDis < minPointSqDis3)
                {
                    // find nearer point
                    minPointSqDis3 = pointSqDis;
                    minPointInd3 = j;
                }
            }

            if (minPointInd2 >= 0 && minPointInd3 >= 0)// 如果三個最近鄰點都有效
            {

                Eigen::Vector3d curr_point(surfPointsFlat->points[i].x,
                    surfPointsFlat->points[i].y,
                    surfPointsFlat->points[i].z);
                Eigen::Vector3d last_point_a(laserCloudSurfLast->points[closestPointInd].x,
                    laserCloudSurfLast->points[closestPointInd].y,
                    laserCloudSurfLast->points[closestPointInd].z);
                Eigen::Vector3d last_point_b(laserCloudSurfLast->points[minPointInd2].x,
                    laserCloudSurfLast->points[minPointInd2].y,
                    laserCloudSurfLast->points[minPointInd2].z);
                Eigen::Vector3d last_point_c(laserCloudSurfLast->points[minPointInd3].x,
                    laserCloudSurfLast->points[minPointInd3].y,
                    laserCloudSurfLast->points[minPointInd3].z);

                double s;
                if (DISTORTION)
                    s = (surfPointsFlat->points[i].intensity - int(surfPointsFlat->points[i].intensity)) / SCAN_PERIOD;
                else
                    s = 1.0;
                // 用點O，A，B，C構造點到面的距離的殘差項，注意這三個點都是在上一幀的Lidar坐標系下，即，殘差 = 點O到平面ABC的距離
                // 同樣的，具體到介紹lidarFactor.cpp時再說明該殘差的具體計算方法
                ceres::CostFunction* cost_function = LidarPlaneFactor::Create(curr_point, last_point_a, last_point_b, last_point_c, s);
                problem.AddResidualBlock(cost_function, loss_function, para_q, para_t);
                plane_correspondence++;
            }
        }
    }

    printf("data association time %f ms \n", t_data.toc());

    if ((corner_correspondence + plane_correspondence) < 10)
    {
        printf("less correspondence! *************************************************\n");
    }

    TicToc t_solver;
    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.max_num_iterations = 4;
    options.minimizer_progress_to_stdout = false;
    ceres::Solver::Summary summary;
    // 基於構建的所有殘差項，求解最優的當前幀位姿與上一幀位姿的位姿增量：para_q和para_t
    ceres::Solve(options, &problem, &summary);
    printf("solver time %f ms \n", t_solver.toc());
}
printf("optimization twice time %f \n", t_opt.toc());

經過2次點到線以及點到面的ICP點雲配准之后，得到最優的位姿增量para_q和para_t，即，q_last_curr和t_last_curr

// 用最新計算出的位姿增量，更新上一幀的位姿，得到當前幀的位姿，注意這里說的位姿都指的是世界坐標系下的位姿
t_w_curr = t_w_curr + q_w_curr * t_last_curr;
q_w_curr = q_w_curr * q_last_curr;

最后就是：

（1）publish由當前Odometry線程計算出的當前幀在世界坐標系在的位姿、corner_less_sharp特征點、surf_less_flat特征點、濾波后的點雲（原封不動的轉發上一個node處理出（簡單濾波）的當前幀點雲）

（2） 用cornerPointsLessSharp和surfPointsLessFlat 更新 laserCloudCornerLast和laserCloudSurfLast以及相應的kdtree，為下一次點雲特征匹配提供target

最后的最后，說明一下各個節點的計算頻率問題：

（1）第1個節點/kittiHelper發布topic的頻率設置為10Hz（當然是可以修改的），

（2）下游節點即第2個節點/ascanRegistration，沒有設置頻率，接收到上游的消息后，進行特征提取，提取完畢后立即publish相關topic，因為特征提取時間一般小於100ms，所以可以認為該節點publish的頻率也是10Hz

（3）下下游節點即第3個節點/alaserOdometry，大循環設置的ros::Rate rate(100); 即 100Hz執行一次Odometry（可以理解成100Hz刷一次Odometry，不一定執行，要看buffer中有沒有數據，有數據就真正執行1次Odometry），並且為各個接受到的各個topic設置了作為緩存容器的queue，旨在防止執行一次Odometry的時間超過100ms，所以該節點publish的頻率允許<=10Hz；需要注意的是在publish特征點和點雲時有一個publish頻率的設置，這是為了控制最后一個節點的執行頻率。

if (frameCount % skipFrameNum == 0)
{
    ...
}

（4）最后一個節點即第4個節點/alaserMapping，沒有設置頻率，直接ros::spin();了，也就是接受到一次上游節點發布的topic就執行一次回調函數。需要注意的是，該節點的處理時間很有可能會超過100ms，為了實現LOAM算法整體的實時性，該節點雖然也設置了接受topic的緩存容器，但是會清空沒有來得及處理的歷史緩存消息，詳見下一節的介紹。

6.laserMapping.cpp

1）對應節點：/alaserMapping

2）功能：基於前述的corner_less_sharp特征點和surf_less_flat特征點，進行幀與submap的點雲特征配准，對Odometry線程計算的位姿進行finetune

3）代碼分析：

a. 同樣地，先明確幾個關鍵變量的含義

// 點雲特征匹配時的優化變量
double parameters[7] = {0, 0, 0, 1, 0, 0, 0};

// Mapping線程估計的frame在world坐標系的位姿P,因為Mapping的算法耗時很有可能會超過100ms，所以
// 這個位姿P不是實時的，LOAM最終輸出的實時位姿P_realtime,需要Mapping線程計算的相對低頻位姿和
// Odometry線程計算的相對高頻位姿做整合，詳見后面laserOdometryHandler函數分析。此外需要注意
// 的是，不同於Odometry線程，這里的位姿P，即q_w_curr和t_w_curr，本身就是匹配時的優化變量。
Eigen::Map<Eigen::Quaterniond> q_w_curr(parameters);
Eigen::Map<Eigen::Vector3d> t_w_curr(parameters + 4);

// 下面的兩個變量是world坐標系下的Odometry計算的位姿和Mapping計算的位姿之間的增量（也即變換，transformation）
// wmap_odom * wodom_curr = wmap_curr(即前面的q/t_w_curr)
// transformation between odom's world and map's world frame
Eigen::Quaterniond q_wmap_wodom(1, 0, 0, 0);
Eigen::Vector3d t_wmap_wodom(0, 0, 0);

// Odometry線程計算的frame在world坐標系的位姿
Eigen::Quaterniond q_wodom_curr(1, 0, 0, 0);
Eigen::Vector3d t_wodom_curr(0, 0, 0);

b. 再介紹一下main函數之前的幾個函數

// set initial guess，上一幀的增量wmap_wodom * 本幀Odometry位姿wodom_curr，旨在為本幀Mapping位姿w_curr設置一個初始值
void transformAssociateToMap()
{
    q_w_curr = q_wmap_wodom * q_wodom_curr;
    t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom;
}

// 用在最后，當Mapping的位姿w_curr計算完畢后，更新增量wmap_wodom，旨在為下一次執行transformAssociateToMap函數時做准備
void transformUpdate()
{
    q_wmap_wodom = q_w_curr * q_wodom_curr.inverse();
    t_wmap_wodom = t_w_curr - q_wmap_wodom * t_wodom_curr;
}

// 用Mapping的位姿w_curr，將Lidar坐標系下的點變換到world坐標系下
void pointAssociateToMap(PointType const *const pi, PointType *const po)
{
    Eigen::Vector3d point_curr(pi->x, pi->y, pi->z);
    Eigen::Vector3d point_w = q_w_curr * point_curr + t_w_curr;
    po->x = point_w.x();
    po->y = point_w.y();
    po->z = point_w.z();
    po->intensity = pi->intensity;
    //po->intensity = 1.0;
}

// 這個沒有用到，是上面pointAssociateToMap的逆變換，即用Mapping的位姿w_curr，將world坐標系下的點變換到Lidar坐標系下
void pointAssociateTobeMapped(PointType const *const pi, PointType *const po)
{
    Eigen::Vector3d point_w(pi->x, pi->y, pi->z);
    Eigen::Vector3d point_curr = q_w_curr.inverse() * (point_w - t_w_curr);
    po->x = point_curr.x();
    po->y = point_curr.y();
    po->z = point_curr.z();
    po->intensity = pi->intensity;
}

之后的幾個Handler函數為接受到上游publish的topic后的回調函數，同樣會將消息緩存到對應的queue中，以便后續處理，需要注意的是

// receive odomtry
void laserOdometryHandler(const nav_msgs::Odometry::ConstPtr &laserOdometry)
{
    mBuf.lock();
    odometryBuf.push(laserOdometry);
    mBuf.unlock();

    // high frequence publish
    Eigen::Quaterniond q_wodom_curr;
    Eigen::Vector3d t_wodom_curr;
    q_wodom_curr.x() = laserOdometry->pose.pose.orientation.x;
    ...
    t_wodom_curr.z() = laserOdometry->pose.pose.position.z;

    // 為了保證LOAM整體的實時性，防止Mapping線程耗時>100ms導致丟幀，用上一次的增量wmap_wodom來更新
    // Odometry的位姿，旨在用Mapping位姿的初始值（也可以理解為預測值）來實時輸出，進而實現LOAM整體的實時性
    Eigen::Quaterniond q_w_curr = q_wmap_wodom * q_wodom_curr;
    Eigen::Vector3d t_w_curr = q_wmap_wodom * t_wodom_curr + t_wmap_wodom; 
    ...
    pubOdomAftMappedHighFrec.publish(odomAftMapped);
}

c. 然后就是核心函數process以及main函數：

main函數比較簡單，接收上游節點的topic，定義對應的消息回調函數，然后就進入核心函數process()了，進行Mapping，即幀與submap的匹配，對Odometry計算的位姿進行finetune

process函數中，我個人覺得最不方便理解的是submap的定義和維護，所以在代碼分析之前，先介紹一下LOAM中這個submap，直接上圖

LOAM中Mapping線程中的幀與submap的特征匹配，用到的submap就是上圖中的黃色區域，submap中的corner特征和surf特征在匹配中作為target；而當前幀的單幀點雲中的兩種特征在匹配中作為source。

下面說明一下幾個與維護submap有關的全局變量的含義，需要注意的是LOAM對submap中cube的管理都是以1維數組的形式

// cube的總數量，也就是上圖中的所有小格子個總數量 21 * 21 * 11 = 4851
const int laserCloudNum = laserCloudWidth * laserCloudHeight * laserCloudDepth;

// 下面兩個變量是一模一樣的，有點冗余，記錄submap中的有效cube的index，注意submap中cube的最大數量為 5 * 5 * 5 = 125
int laserCloudValidInd[125];
int laserCloudSurroundInd[125];

// 存放cube點雲特征的數組，數組大小4851，points in every cube
pcl::PointCloud<PointType>::Ptr laserCloudCornerArray[laserCloudNum];
pcl::PointCloud<PointType>::Ptr laserCloudSurfArray[laserCloudNum];

為了保證LOAM算法整體的實時性，Mapping線程每次只處理cornerLastBuf.front()及其他與之時間同步的消息，而因為Mapping線程耗時>100ms導致的歷史緩存都會被清空，具體操作如下：

while (!cornerLastBuf.empty() && !surfLastBuf.empty() &&
    !fullResBuf.empty() && !odometryBuf.empty())
{
    mBuf.lock();
    // odometryBuf只保留一個與cornerLastBuf.front()時間同步的最新消息
    while (!odometryBuf.empty() && odometryBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
        odometryBuf.pop();
    if (odometryBuf.empty())
    {
        mBuf.unlock();
        break;
    }

    // surfLastBuf也如此
    while (!surfLastBuf.empty() && surfLastBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
        surfLastBuf.pop();
    if (surfLastBuf.empty())
    {
        mBuf.unlock();
        break;
    }

    // fullResBuf也如此
    while (!fullResBuf.empty() && fullResBuf.front()->header.stamp.toSec() < cornerLastBuf.front()->header.stamp.toSec())
        fullResBuf.pop();
    if (fullResBuf.empty())
    {
        mBuf.unlock();
        break;
    }

    ...

    // 清空cornerLastBuf的歷史緩存，為了LOAM的整體實時性
    while (!cornerLastBuf.empty())
    {
        cornerLastBuf.pop();
        printf("drop lidar frame in mapping for real time performance \n");
    }

    mBuf.unlock();
}

 然后是關於submap位置的維護，直接看下面的注釋吧：

// 上一幀的增量wmap_wodom * 本幀Odometry位姿wodom_curr，旨在為本幀Mapping位姿w_curr設置一個初始值
transformAssociateToMap();

TicToc t_shift;
// 下面這是計算當前幀位置t_w_curr（在上圖中用紅色五角星表示的位置）IJK坐標（見上圖中的坐標軸），
// 參照LOAM_NOTED的注釋，下面有關25呀，50啥的運算，等效於以50m為單位進行縮放，因為LOAM用1維數組
// 進行cube的管理，而數組的index只用是正數，所以要保證IJK坐標都是正數，所以加了laserCloudCenWidth/Heigh/Depth
// 的偏移，來使得當前位置盡量位於submap的中心處，也就是使得上圖中的五角星位置盡量處於所有格子的中心附近，
// 偏移laserCloudCenWidth/Heigh/Depth會動態調整，來保證當前位置盡量位於submap的中心處。
int centerCubeI = int((t_w_curr.x() + 25.0) / 50.0) + laserCloudCenWidth;
int centerCubeJ = int((t_w_curr.y() + 25.0) / 50.0) + laserCloudCenHeight;
int centerCubeK = int((t_w_curr.z() + 25.0) / 50.0) + laserCloudCenDepth;

// 由於計算機求余是向零取整，為了不使（-50.0,50.0）求余后都向零偏移，當被求余數為負數時求余結果統一向左偏移一個單位，也即減一
if (t_w_curr.x() + 25.0 < 0)
    centerCubeI--;
if (t_w_curr.y() + 25.0 < 0)
    centerCubeJ--;
if (t_w_curr.z() + 25.0 < 0)
    centerCubeK--;

// 以下注釋部分參照LOAM_NOTED，結合我畫的submap的示意圖說明下面的6個while loop的作用：要
// 注意世界坐標系下的點雲地圖是固定的，但是IJK坐標系我們是可以移動的，所以這6個while loop
// 的作用就是調整IJK坐標系（也就是調整所有cube位置），使得五角星在IJK坐標系的坐標范圍處於
// 3 < centerCubeI < 18， 3 < centerCubeJ < 8, 3 < centerCubeK < 18，目的是為了防止后續向
// 四周拓展cube（圖中的黃色cube就是拓展的cube）時，index（即IJK坐標）成為負數。
while (centerCubeI < 3)
{
    for (int j = 0; j < laserCloudHeight; j++)
    {
        for (int k = 0; k < laserCloudDepth; k++)
        {
            int i = laserCloudWidth - 1;
            pcl::PointCloud<PointType>::Ptr laserCloudCubeCornerPointer =
                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
            pcl::PointCloud<PointType>::Ptr laserCloudCubeSurfPointer =
                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
            for (; i >= 1; i--)// 在I方向cube[I]=cube[I-1],清空最后一個空出來的cube，實現IJK坐標系向I軸負方向移動一個cube的
                               // 效果，從相對運動的角度看是圖中的五角星在IJK坐標系下向I軸正方向移動了一個cube，如下面動圖所示，所
                               // 以centerCubeI最后++，laserCloudCenWidth也++，為下一幀Mapping時計算五角星的IJK坐標做准備
            {
                laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                    laserCloudCornerArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
                laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                    laserCloudSurfArray[i - 1 + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k];
            }
            laserCloudCornerArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudCubeCornerPointer;
            laserCloudSurfArray[i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k] =
                laserCloudCubeSurfPointer;
            laserCloudCubeCornerPointer->clear();
            laserCloudCubeSurfPointer->clear();
        }
    }

    centerCubeI++;
    laserCloudCenWidth++;
}

while (centerCubeI >= laserCloudWidth - 3)
{
    ...
}

while (centerCubeJ < 3)
{
    ...
}

while (centerCubeJ >= laserCloudHeight - 3)
{
    ...
}

while (centerCubeK < 3)
{
    ...
}

while (centerCubeK >= laserCloudDepth - 3)
{
    ...
}

I軸方向調整一次示意圖

生成submap的特征點雲如下，需要注意的是，LOAM這種方式生成的submap並不是基於時序的滑動窗口方式，而是基於空間范圍划分的方式：

// 下面的兩個定義可以認為是清空submap

int laserCloudValidNum = 0;
int laserCloudSurroundNum = 0;

// 向IJ坐標軸的正負方向各拓展2個cube，K坐標軸的正負方向各拓展1個cube，上圖中五角星所在的藍色cube就是當前位置
// 所處的cube，拓展的cube就是黃色的cube，這些cube就是submap的范圍
for (int i = centerCubeI - 2; i <= centerCubeI + 2; i++)
{
    for (int j = centerCubeJ - 2; j <= centerCubeJ + 2; j++)
    {
        for (int k = centerCubeK - 1; k <= centerCubeK + 1; k++)
        {
            if (i >= 0 && i < laserCloudWidth &&
                j >= 0 && j < laserCloudHeight &&
                k >= 0 && k < laserCloudDepth)// 如果坐標合法
            {
                // 記錄submap中的所有cube的index，記為有效index
                laserCloudValidInd[laserCloudValidNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
                laserCloudValidNum++;
                laserCloudSurroundInd[laserCloudSurroundNum] = i + laserCloudWidth * j + laserCloudWidth * laserCloudHeight * k;
                laserCloudSurroundNum++;
            }
        }
    }
}

laserCloudCornerFromMap->clear();
laserCloudSurfFromMap->clear();
for (int i = 0; i < laserCloudValidNum; i++)
{
    // 將有效index的cube中的點雲疊加到一起組成submap的特征點雲
    *laserCloudCornerFromMap += *laserCloudCornerArray[laserCloudValidInd[i]];
    *laserCloudSurfFromMap += *laserCloudSurfArray[laserCloudValidInd[i]];
}

接下來就是以降采樣后的submap特征點雲為target，以當前幀降采樣后的特征點雲為source的ICP過程了，同樣地也分為點到線和點到面的匹配，基本流程與Odometry線程相同，不同的是建立correspondence（關聯）的方式不同，具體看下面的介紹，先是點到線的ICP過程：

for (int i = 0; i < laserCloudCornerStackNum; i++)
{
    pointOri = laserCloudCornerStack->points[i];
    // 需要注意的是submap中的點雲都是world坐標系，而當前幀的點雲都是Lidar坐標系，所以
    // 在搜尋最近鄰點時，先用預測的Mapping位姿w_curr，將Lidar坐標系下的特征點變換到world坐標系下
    pointAssociateToMap(&pointOri, &pointSel);
    // 在submap的corner特征點（target）中，尋找距離當前幀corner特征點（source）最近的5個點
    kdtreeCornerFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

    if (pointSearchSqDis[4] < 1.0)
    {
        std::vector<Eigen::Vector3d> nearCorners;
        Eigen::Vector3d center(0, 0, 0);
        for (int j = 0; j < 5; j++)
        {
            Eigen::Vector3d tmp(laserCloudCornerFromMap->points[pointSearchInd[j]].x,
                laserCloudCornerFromMap->points[pointSearchInd[j]].y,
                laserCloudCornerFromMap->points[pointSearchInd[j]].z);
            center = center + tmp;
            nearCorners.push_back(tmp);
        }
        // 計算這個5個最近鄰點的中心
        center = center / 5.0;

        // 協方差矩陣
        Eigen::Matrix3d covMat = Eigen::Matrix3d::Zero();
        for (int j = 0; j < 5; j++)
        {
            Eigen::Matrix<double, 3, 1> tmpZeroMean = nearCorners[j] - center;
            covMat = covMat + tmpZeroMean * tmpZeroMean.transpose();
        }

        // 計算協方差矩陣的特征值和特征向量，用於判斷這5個點是不是呈線狀分布，此為PCA的原理
        Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> saes(covMat);

        // if is indeed line feature
        // note Eigen library sort eigenvalues in increasing order
        Eigen::Vector3d unit_direction = saes.eigenvectors().col(2);// 如果5個點呈線狀分布，最大的特征值對應的特征向量就是該線的方向向量
        Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
        if (saes.eigenvalues()[2] > 3 * saes.eigenvalues()[1])// 如果最大的特征值 >> 其他特征值，則5個點確實呈線狀分布，否則認為直線“不夠直”
        {
            Eigen::Vector3d point_on_line = center;
            Eigen::Vector3d point_a, point_b;
            // 從中心點沿着方向向量向兩端移動0.1m，構造線上的兩個點
            point_a = 0.1 * unit_direction + point_on_line;
            point_b = -0.1 * unit_direction + point_on_line;

            // 然后殘差函數的形式就跟Odometry一樣了，殘差距離即點到線的距離，到介紹lidarFactor.cpp時再說明具體計算方法
            ceres::CostFunction* cost_function = LidarEdgeFactor::Create(curr_point, point_a, point_b, 1.0);
            problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
            corner_num++;
        }
    }
}

上面的點到線的ICP過程就比Odometry中的要魯棒和准確一些了（當然也就更耗時一些），因為不是簡單粗暴地搜最近的兩個corner點作為target的線，而是PCA計算出最近鄰的5個點的主方向作為線的方向，而且還會check直線是不是“足夠直”，接着看點到面的ICP過程：

for (int i = 0; i < laserCloudSurfStackNum; i++)
{
    pointOri = laserCloudSurfStack->points[i];

    pointAssociateToMap(&pointOri, &pointSel);
    kdtreeSurfFromMap->nearestKSearch(pointSel, 5, pointSearchInd, pointSearchSqDis);

    // 求面的法向量就不是用的PCA了（雖然論文中說還是PCA），使用的是最小二乘擬合，是為了提效？不確定
    // 假設平面不通過原點，則平面的一般方程為Ax + By + Cz + 1 = 0，用這個假設可以少算一個參數，提效。
    Eigen::Matrix<double, 5, 3> matA0;
    Eigen::Matrix<double, 5, 1> matB0 = -1 * Eigen::Matrix<double, 5, 1>::Ones();
    // 用上面的2個矩陣表示平面方程就是 matA0 * norm（A, B, C） = matB0，這是個超定方程組，因為數據個數超過未知數的個數
    if (pointSearchSqDis[4] < 1.0)
    {
        for (int j = 0; j < 5; j++)
        {
            matA0(j, 0) = laserCloudSurfFromMap->points[pointSearchInd[j]].x;
            matA0(j, 1) = laserCloudSurfFromMap->points[pointSearchInd[j]].y;
            matA0(j, 2) = laserCloudSurfFromMap->points[pointSearchInd[j]].z;
        }
        // 求解這個最小二乘問題，可得平面的法向量，find the norm of plane
        Eigen::Vector3d norm = matA0.colPivHouseholderQr().solve(matB0);
        // Ax + By + Cz + 1 = 0，全部除以法向量的模長，方程依舊成立，而且使得法向量歸一化了
        double negative_OA_dot_norm = 1 / norm.norm();
        norm.normalize();

        // Here n(pa, pb, pc) is unit norm of plane
        bool planeValid = true;
        for (int j = 0; j < 5; j++)
        {
            // 點(x0, y0, z0)到平面Ax + By + Cz + D = 0 的距離公式 = fabs(Ax0 + By0 + Cz0 + D) / sqrt(A^2 + B^2 + C^2)
            if (fabs(norm(0) * laserCloudSurfFromMap->points[pointSearchInd[j]].x +
                norm(1) * laserCloudSurfFromMap->points[pointSearchInd[j]].y +
                norm(2) * laserCloudSurfFromMap->points[pointSearchInd[j]].z + negative_OA_dot_norm) > 0.2)
            {
                planeValid = false;// 平面沒有擬合好，平面“不夠平”
                break;
            }
        }
        Eigen::Vector3d curr_point(pointOri.x, pointOri.y, pointOri.z);
        if (planeValid)
        {
            // 構造點到面的距離殘差項，同樣的，具體到介紹lidarFactor.cpp時再說明該殘差的具體計算方法
            ceres::CostFunction* cost_function = LidarPlaneNormFactor::Create(curr_point, norm, negative_OA_dot_norm);
            problem.AddResidualBlock(cost_function, loss_function, parameters, parameters + 4);
            surf_num++;
        }
    }
}

上面的點到面的ICP過程同樣比Odometry中的要魯棒和准確一些了（當然也就更耗時一些），因為不是簡單粗暴地搜最近的3個surf點作為target的平面點，而是對最近鄰的5個點進行基於最小二乘的平面擬合，而且還會check擬合出的平面是不是“足夠平”，再然后就是匹配之后的一些操作，主要包括更新位姿增量、更新submap：

// 完成ICP（迭代2次）的特征匹配后，用最后匹配計算出的優化變量w_curr，更新增量wmap_wodom，為下一次Mapping做准備
transformUpdate();

TicToc t_add;
// 下面兩個for loop的作用就是將當前幀的特征點雲，逐點進行操作：轉換到world坐標系並添加到對應位置的cube中
for (int i = 0; i < laserCloudCornerStackNum; i++)
{
    // Lidar坐標系轉到world坐標系
    pointAssociateToMap(&laserCloudCornerStack->points[i], &pointSel);

    // 計算本次的特征點的IJK坐標，進而確定添加到哪個cube中
    int cubeI = int((pointSel.x + 25.0) / 50.0) + laserCloudCenWidth;
    int cubeJ = int((pointSel.y + 25.0) / 50.0) + laserCloudCenHeight;
    int cubeK = int((pointSel.z + 25.0) / 50.0) + laserCloudCenDepth;

    if (pointSel.x + 25.0 < 0)
        cubeI--;
    if (pointSel.y + 25.0 < 0)
        cubeJ--;
    if (pointSel.z + 25.0 < 0)
        cubeK--;

    if (cubeI >= 0 && cubeI < laserCloudWidth &&
        cubeJ >= 0 && cubeJ < laserCloudHeight &&
        cubeK >= 0 && cubeK < laserCloudDepth)
    {
        int cubeInd = cubeI + laserCloudWidth * cubeJ + laserCloudWidth * laserCloudHeight * cubeK;
        laserCloudCornerArray[cubeInd]->push_back(pointSel);
    }
}

for (int i = 0; i < laserCloudSurfStackNum; i++)
{
    ...
}
printf("add points time %f ms\n", t_add.toc());

TicToc t_filter;
// 因為新增加了點雲，對之前已經存有點雲的cube全部重新進行一次降采樣
// 這個地方可以簡單優化一下：如果之前的cube沒有新添加點就不需要再降采樣
for (int i = 0; i < laserCloudValidNum; i++)
{
    int ind = laserCloudValidInd[i];

    pcl::PointCloud<PointType>::Ptr tmpCorner(new pcl::PointCloud<PointType>());
    downSizeFilterCorner.setInputCloud(laserCloudCornerArray[ind]);
    downSizeFilterCorner.filter(*tmpCorner);
    laserCloudCornerArray[ind] = tmpCorner;

    pcl::PointCloud<PointType>::Ptr tmpSurf(new pcl::PointCloud<PointType>());
    downSizeFilterSurf.setInputCloud(laserCloudSurfArray[ind]);
    downSizeFilterSurf.filter(*tmpSurf);
    laserCloudSurfArray[ind] = tmpSurf;
}
printf("filter time %f ms \n", t_filter.toc());

最后就是publish一些topic，可以在rviz中接受可視化點雲、軌跡的結果。需要注意的最后求出的位姿不是KITTI真值位姿的坐標系，如果需要利用真值位姿進行評測還得進行坐標變換。

7.lidarFactor.hpp

1）對應節點：無

2）功能：計算點到線、點到面的距離殘差項

3）代碼分析：

// 點到線的殘差距離計算
struct LidarEdgeFactor
{
    LidarEdgeFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_a_,
                    Eigen::Vector3d last_point_b_, double s_)
        : curr_point(curr_point_), last_point_a(last_point_a_), last_point_b(last_point_b_), s(s_) {}

    template <typename T>
    bool operator()(const T *q, const T *t, T *residual) const
    {
        Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
        Eigen::Matrix<T, 3, 1> lpa{T(last_point_a.x()), T(last_point_a.y()), T(last_point_a.z())};
        Eigen::Matrix<T, 3, 1> lpb{T(last_point_b.x()), T(last_point_b.y()), T(last_point_b.z())};

        //Eigen::Quaternion<T> q_last_curr{q[3], T(s) * q[0], T(s) * q[1], T(s) * q[2]};
        Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
        Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
        // 考慮運動補償，ktti點雲已經補償過所以可以忽略下面的對四元數slerp插值以及對平移的線性插值
        q_last_curr = q_identity.slerp(T(s), q_last_curr);
        Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};

        Eigen::Matrix<T, 3, 1> lp;
        // Odometry線程時，下面是將當前幀Lidar坐標系下的cp點變換到上一幀的Lidar坐標系下，然后在上一幀的Lidar坐標系計算點到線的殘差距離
        // Mapping線程時，下面是將當前幀Lidar坐標系下的cp點變換到world坐標系下，然后在world坐標系下計算點到線的殘差距離
        lp = q_last_curr * cp + t_last_curr;

        // 點到線的計算如下圖所示
        Eigen::Matrix<T, 3, 1> nu = (lp - lpa).cross(lp - lpb);
        Eigen::Matrix<T, 3, 1> de = lpa - lpb;

        // 最終的殘差本來應該是residual[0] = nu.norm() / de.norm(); 為啥也分成3個，我也不知
        // 道，從我試驗的效果來看，確實是下面的殘差函數形式，最后輸出的pose精度會好一點點，這里需要
        // 注意的是，所有的residual都不用加fabs，因為Ceres內部會對其求 平方 作為最終的殘差項
        residual[0] = nu.x() / de.norm();
        residual[1] = nu.y() / de.norm();
        residual[2] = nu.z() / de.norm();

        return true;
    }

    static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_a_,
                                       const Eigen::Vector3d last_point_b_, const double s_)
    {
        return (new ceres::AutoDiffCostFunction<
                LidarEdgeFactor, 3, 4, 3>(
//                               ^  ^  ^
//                               |  |  |
//                  殘差的維度 ____|  |  |
//              優化變量q的維度 _______|  |
//              優化變量t的維度 __________|
            new LidarEdgeFactor(curr_point_, last_point_a_, last_point_b_, s_)));
    }

    Eigen::Vector3d curr_point, last_point_a, last_point_b;
    double s;
};

點到線的距離計算圖解：

// 計算Odometry線程中點到面的殘差距離
struct LidarPlaneFactor
{
    LidarPlaneFactor(Eigen::Vector3d curr_point_, Eigen::Vector3d last_point_j_,
                     Eigen::Vector3d last_point_l_, Eigen::Vector3d last_point_m_, double s_)
        : curr_point(curr_point_), last_point_j(last_point_j_), last_point_l(last_point_l_),
          last_point_m(last_point_m_), s(s_)
    {
        // 點l、j、m就是搜索到的最近鄰的3個點，下面就是計算出這三個點構成的平面ljlm的法向量
        ljm_norm = (last_point_j - last_point_l).cross(last_point_j - last_point_m);
        // 歸一化法向量
        ljm_norm.normalize();
    }

    template <typename T>
    bool operator()(const T *q, const T *t, T *residual) const
    {

        Eigen::Matrix<T, 3, 1> cp{T(curr_point.x()), T(curr_point.y()), T(curr_point.z())};
        Eigen::Matrix<T, 3, 1> lpj{T(last_point_j.x()), T(last_point_j.y()), T(last_point_j.z())};
        Eigen::Matrix<T, 3, 1> ljm{T(ljm_norm.x()), T(ljm_norm.y()), T(ljm_norm.z())};

        Eigen::Quaternion<T> q_last_curr{q[3], q[0], q[1], q[2]};
        Eigen::Quaternion<T> q_identity{T(1), T(0), T(0), T(0)};
        q_last_curr = q_identity.slerp(T(s), q_last_curr);
        Eigen::Matrix<T, 3, 1> t_last_curr{T(s) * t[0], T(s) * t[1], T(s) * t[2]};

        Eigen::Matrix<T, 3, 1> lp;
        lp = q_last_curr * cp + t_last_curr;

        // 計算點到平面的殘差距離，如下圖所示
        residual[0] = (lp - lpj).dot(ljm);

        return true;
    }

    static ceres::CostFunction *Create(const Eigen::Vector3d curr_point_, const Eigen::Vector3d last_point_j_,
                                       const Eigen::Vector3d last_point_l_, const Eigen::Vector3d last_point_m_,
                                       const double s_)
    {
        return (new ceres::AutoDiffCostFunction<
                LidarPlaneFactor, 1, 4, 3>(
//                                ^  ^  ^
//                                |  |  |
//                   殘差的維度 ____|  |  |
//              優化變量q的維度 ________|  |
//               優化變量t的維度 __________|
            new LidarPlaneFactor(curr_point_, last_point_j_, last_point_l_, last_point_m_, s_)));
    }

    Eigen::Vector3d curr_point, last_point_j, last_point_l, last_point_m;
    Eigen::Vector3d ljm_norm;
    double s;
};

點到面的距離計算圖解：

最后一個struct LidarPlaneNormFactor，是計算Mapping線程中點到平面的殘差距離，因為輸入了平面方程的參數（），所以直接使用點到面的距離公式進行計算：

點(x0, y0, z0)到平面Ax + By + Cz + D = 0 的距離 = fabs(A*x0 + B*y0 + C*z0 + D) / sqrt(A^2 + B^2 + C^2)，因為法向量（A, B, C）已經歸一化了，所以距離公式可以簡寫為：距離 = fabs(A*x0 + B*y0 + C*z0 + D) ，即：

residual[0] = norm.dot(point_w) + T(negative_OA_dot_norm);

三、手推LOAM源碼中的高斯牛頓迭代

1.kitti數據集中的坐標變換

如前所述，kitti odometry真值pose是左相機坐標系相對於其第1幀的transform；A-LOAM直接計算出來的pose是lidar坐標系相對於其第1幀的transform；還需要注意的是，sensor的世界坐標系本質就是第1幀的sensor坐標系。

記：

lidar to left camera的外參矩陣為T_r；

（

這里mark一種Tr的快速估計方法（與本文無關）：假如不考慮xyz的平移，只有旋轉的話，由上圖知：

x_camera = -y_lidar

y_camera = -z_lidar

z_camera = x_lidar

(x_camera，y_camera，z_camera)^T = （0  -1  0

                                                     0  0  -1

                                                     1  0   0）* (x_lidar，y_lidar，z_lidar)^T

即 Tr = （0  -1  0

               0  0  -1

               1  0   0）

）

第i幀lidar坐標系在其世界坐標系的pose（也即transform）為T_i^lidar_w；

第i幀camera坐標系在其世界坐標系的pose為T_i^camera_w；

第i幀lidar坐標系下的點雲為P_{i^lidar_l}；

第i幀camera坐標系下的點雲為P_{i^camera_l}；

第i幀lidar世界坐標系下的點雲為P_{i^lidar_w}；

第i幀camera世界坐標系下的點雲為P_{i^camera_w}；

則有 T_i^camera_w * T_r *  P_{i^lidar_l} = T_r * T_i^lidar_w * P_{i^lidar_l}    (1)

                |            |_____|         |            |______|  

                |                 |               |                  |

                |            P_{i^camera_l}        |              P_{i^lidar_w}

                |________|                |_________|

                        |                                  |

                 P_{i^camera_w}        ==        P_{i^camera_w}

由（1）式可得T_i^camera_w * T_r = T_r * T_i^lidar_w （2）

由（2）式可得T_i^camera_w = T_r * T_i^lidar_w * T_r^-1（3）

則（3）式就是A-LOAM計算出的pose轉換到kitti真值pose坐標系的公式了

2.手推雅克比矩陣

 帖鏈接了，=_=  ，https://wykxwyc.github.io/2019/08/01/The-Math-Formula-in-LeGO-LOAM/