天天看點
傳回

A-LOAM源碼解析——scanRegistration.cpp

A-LOAM主要包括四個主要的源程式，剛看完scanRegistration，順便分享總結一下，剩餘三個以後看完會及時更新，本檔案主要是通過曲率來對角點以及平面點進行提取。

1.首先是對後面用到的一些變量進行定義，主要包括，雷射雷達掃描周期，初始化标志，雷達線數，以及6類點等等。

//定義雷射雷達的掃描周期，頻率為10Hz，周期為0.1s
const double scanPeriod = 0.1;
const int systemDelay = 0;
//計數過了多少幀
int systemInitCount = 0;
//系統初始化标志
bool systemInited = false;
//雷射雷達線數 初始值為0
int N_SCANS = 0;
float cloudCurvature[400000];
int cloudSortInd[400000];
int cloudNeighborPicked[400000];
int cloudLabel[400000];

//兩個點曲率比較
bool comp (int i,int j) { return (cloudCurvature[i]<cloudCurvature[j]); }
ros::Publisher pubLaserCloud;//整體點雲
ros::Publisher pubCornerPointsSharp;//大曲率特征點——角點
ros::Publisher pubCornerPointsLessSharp;//小曲率特征點——降采樣角點
ros::Publisher pubSurfPointsFlat;//平面點
ros::Publisher pubSurfPointsLessFlat;//降采樣平面點
ros::Publisher pubRemovePoints;//剔除點
std::vector<ros::Publisher> pubEachScan;

//是否釋出每行掃描
bool PUB_EACH_LINE = false;
//定義最小距離參數，用于根據距離遠近剔除點
double MINIMUM_RANGE = 0.1;

 2.定義了一個剔除點的函數，主要删除一些距離比較近的點，并重新定義點雲的size。

//定義剔除點函數（距離較近的點）
void removeClosedPointCloud(const pcl::PointCloud<PointT> &cloud_in,
                              pcl::PointCloud<PointT> &cloud_out, float thres)
{//統一header（時間戳）和size
    if (&cloud_in != &cloud_out)
    {
        cloud_out.header = cloud_in.header;
        cloud_out.points.resize(cloud_in.points.size());
    }

    size_t j = 0;
//逐個點進行距離比較
    for (size_t i = 0; i < cloud_in.points.size(); ++i)
    {//如果在這個範圍内，跳出這次循環，下一個點進來
        if (cloud_in.points[i].x * cloud_in.points[i].x + cloud_in.points[i].y * cloud_in.points[i].y + cloud_in.points[i].z * cloud_in.points[i].z < thres * thres)
            continue;
        cloud_out.points[j] = cloud_in.points[i];//如果不在這個範圍，正常執行
        j++;
    }
    if (j != cloud_in.points.size())//如果有點剔除，size發生改變
    {
        cloud_out.points.resize(j);//重新定義size
    }

    cloud_out.height = 1;//無序資料為1
    cloud_out.width = static_cast<uint32_t>(j);//無序資料為總點數
    cloud_out.is_dense = true;//點雲數目是否有限
}

3.定義點雲的回調函數，也就是核心部分，每來一幀點雲，執行一次。先将ros的點雲轉換成pcl，然後對點進行曲率計算，進行分類劃分，最後再将劃分出來的點轉換回ros形式進行釋出。

void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr &laserCloudMsg)
{
    if (!systemInited)//如果系統沒初始化
    { 
        systemInitCount++;//計數+1
        if (systemInitCount >= systemDelay)//當大于這個數
        {
            systemInited = true;//系統初始化設為真
        }
        else
            return;
    }

    TicToc t_whole;//整體時間
    TicToc t_prepare;//計算曲率前的預處理時間
   //定義一個容器，記錄每次掃描有曲率的點開始和結束索引
    std::vector<int> scanStartInd(N_SCANS, 0);
    std::vector<int> scanEndInd(N_SCANS, 0);
   //定義一個pcl點雲類型
    pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
    pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);//把ros的點雲，轉換為pcl形式
    std::vector<int> indices;
   //去除過遠點（無效點）函數，下面會遇到詳細的定義
    pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
   //去除離雷射雷達太近的點 0.1
    removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);

   //定義該次掃描的點數
    int cloudSize = laserCloudIn.points.size();
   //atan2的範圍-pi到+pi，負号是因為雷射雷達是順時針旋轉，而角度逆時針才為正
    float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
    float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y,
                          laserCloudIn.points[cloudSize - 1].x) +
                   2 * M_PI;
   //保證雷射掃描的範圍在pi-3pi
    if (endOri - startOri > 3 * M_PI)
    {
        endOri -= 2 * M_PI;
    }
    else if (endOri - startOri < M_PI)
    {
        endOri += 2 * M_PI;
    }
    //printf("end Ori %f\n", endOri);

    bool halfPassed = false;//掃描是否過半
    int count = cloudSize;//總點數
    PointType point;
    std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);//定義容器，按照線數存儲點雲
   //周遊目前掃描幀的全部點，讀取每個點的坐标
    for (int i = 0; i < cloudSize; i++)
    {
        point.x = laserCloudIn.points[i].x;
        point.y = laserCloudIn.points[i].y;
        point.z = laserCloudIn.points[i].z;
   //計算垂直俯仰角（後文詳細推導）
        float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;//16線雷射雷達垂直視場為-15到15
        int scanID = 0;//雷射幀序号

        if (N_SCANS == 16)//如果為16線雷射，進入
        {
            scanID = int((angle + 15) / 2 + 0.5);//0.5為了四舍五入 int隻能保留整數部分，angle的範圍為-pi/2到pi/2，/2是每兩個掃描之間差2度,angle範圍為-15到15,帶入求出scanID範圍為0-15
            if (scanID > (N_SCANS - 1) || scanID < 0)//如果在16以上或者為負數，總點數-1結束本次循環
            {
                count--;
                continue;
            }
        }
        else if (N_SCANS == 32)
        {
            scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
            if (scanID > (N_SCANS - 1) || scanID < 0)
            {
                count--;
                continue;
            }
        }
        else if (N_SCANS == 64)
        {   
            if (angle >= -8.83)
                scanID = int((2 - angle) * 3.0 + 0.5);
            else
                scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);

            // use [0 50]  > 50 remove outlies 
            if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
            {
                count--;
                continue;
            }
        }
        else
        {
            printf("wrong scan number\n");
            ROS_BREAK();
        }
        //printf("angle %f scanID %d \n", angle, scanID);
        float ori = -atan2(point.y, point.x);//計算這個點的水準旋轉角度
        if (!halfPassed)//判斷是否掃描一半，進行補償，保證掃描終止起始掃描範圍在pi-2pi
        { //如果沒過半，選擇起始位置進行內插補點計算，來補償，確定-pi/2 < ori - startOri < 3*pi/2
            if (ori < startOri - M_PI / 2)
            {
                ori += 2 * M_PI;
            }
            else if (ori > startOri + M_PI * 3 / 2)
            {
                ori -= 2 * M_PI;
            }

            if (ori - startOri > M_PI)
            {
                halfPassed = true;
            }
        }
        else//如果過半，選擇終止位置進行內插補點計算，來補償，確定-3*pi/2 < ori - endOri < pi/2
        {
            ori += 2 * M_PI;
            if (ori < endOri - M_PI * 3 / 2)
            {
                ori += 2 * M_PI;
            }
            else if (ori > endOri + M_PI / 2)
            {
                ori -= 2 * M_PI;
            }
        }
        float relTime = (ori - startOri) / (endOri - startOri); //旋轉的角度/整個周期角度，看旋轉了多大
        point.intensity = scanID + scanPeriod * relTime;//一個整數+一個小數，整數部分是線号（0-15），小數部分是相對時間（旋轉了多少，對應的時間），scanPeriod為0.1，也就是一圈0.1s，小數部分不會超過0.1,完成了按照時間排序的需求
        laserCloudScans[scanID].push_back(point); //把目前點，放入對應的scanid的線上
    }
    
    cloudSize = count;//把總點數給回cloudSize
    printf("points size %d \n", cloudSize);
    //定義一個資料集合，把所有的線儲存到裡面
    pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
    for (int i = 0; i < N_SCANS; i++)//讀取每個線
    { 
        scanStartInd[i] = laserCloud->size() + 5;//忽略前五個點
        *laserCloud += laserCloudScans[i];//将scanID為i的scan放入laserCloud
        scanEndInd[i] = laserCloud->size() - 6;//忽略後五個點，開始和結束處的點雲scan容易産生不閉合的“接縫”，對提取edge feature不利
    }

    printf("prepare time %f \n", t_prepare.toc());

    //計算曲率，使用每個點相鄰的前五個和後五個，最前五個點和最後五個已經忽略
    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;//點沒被選擇過，設定為0.，後續當被選為特征點之後，将被設定為1
        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，因為點太多，最後會降采樣

    }


    TicToc t_pts;//用于計時
    //定義角點，降采樣角點，面點，降采樣面點
    pcl::PointCloud<PointType> cornerPointsSharp;
    pcl::PointCloud<PointType> cornerPointsLessSharp;
    pcl::PointCloud<PointType> surfPointsFlat;
    pcl::PointCloud<PointType> surfPointsLessFlat;

    float t_q_sort = 0;
    for (int i = 0; i < N_SCANS; i++)//按照scan的順序，提取四種特征點
    {
        if( scanEndInd[i] - scanStartInd[i] < 6)//如果點數少于6個，則跳過，下一個掃描進來，確定将每個掃描進行6等分
            continue;
        pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
        for (int j = 0; j < 6; j++)//6小段進行特征檢測
        {
            int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; //起始索引，六等分起點
            int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;//結束索引，-1避免重複，六等分終點

            TicToc t_tmp;
            std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);//利用sort，按照曲率從小到大進行排序
            t_q_sort += t_tmp.toc();
            //篩選每個分段，曲率很大和比較大的點
            int largestPickedNum = 0;
            for (int k = ep; k >= sp; k--)//倒序查找，曲率從大到小
            {
                int ind = cloudSortInd[k]; 
                //如果沒有被選擇過，并且曲率較大
                if (cloudNeighborPicked[ind] == 0 &&
                    cloudCurvature[ind] > 0.1)
                {

                    largestPickedNum++;//曲率大的點計數+1
                    //曲率大的前兩個點，為角點，最多為兩個
                    if (largestPickedNum <= 2)
                    {                        
                        cloudLabel[ind] = 2;//角點 為2
                        cornerPointsSharp.push_back(laserCloud->points[ind]);
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    //曲率最大的前20個為降采樣角點，包括角點
                    else if (largestPickedNum <= 20)
                    {                        
                        cloudLabel[ind] = 1; //降采樣角點 為1
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    else
                    {
                        break;
                    }

                    cloudNeighborPicked[ind] = 1; //該點被選擇處理過了，标志設為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)//如果距離的平方大于0.05,跳過
                        {
                            break;
                        }
                        //否則，距離的平方小于等于0.05,為比較近的點，标記為1,也就是選擇處理過
                        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;
                    }
                }
            }
            //提取曲率比較小的點（平面點）
            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; //-1表示曲率很小的點
                    surfPointsFlat.push_back(laserCloud->points[ind]);

                    smallestPickedNum++;
                    if (smallestPickedNum >= 4)//選取曲率最小的前四個點
                    { 
                        break;
                    }

                    cloudNeighborPicked[ind] = 1;//該點被選擇處理過了，标志設為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;//距離小于等于0.05的被設定為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;
                    }
                }
            }
            //将剩餘的點（非角點（2和1）），組成surf_less_flat進行降采樣
            for (int k = sp; k <= ep; k++)
            {
                if (cloudLabel[k] <= 0)//-1 0 也就是除了角點之外的所有點
                {
                    surfPointsLessFlatScan->push_back(laserCloud->points[k]);
                }
            }
        }
        //因為點比較多，進行降采樣（體素栅格濾波器），執行結果放入到scanDS
        pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
        pcl::VoxelGrid<PointType> downSizeFilter;
        downSizeFilter.setInputCloud(sufPointsLessFlatScan);//設定需要降采樣處理的點雲
        downSizeFilter.setLeafSize(0.2, 0.2, 0.2);//設定濾波時建立的體素體積
        downSizeFilter.filter(surfPointsLessFlatScanDS);//處理結果放入DS
        surfPointsLessFlat += surfPointsLessFlatScanDS;//每次彙總降采樣處理之後的點雲
    }
    printf("sort q time %f \n", t_q_sort);//輸出分類排序的時間
    printf("seperate points time %f \n", t_pts.toc());//輸出降采樣的時間

    //進行點雲格式轉換，便于釋出
    sensor_msgs::PointCloud2 laserCloudOutMsg;//定義ros點雲格式
    pcl::toROSMsg(*laserCloud, laserCloudOutMsg);//将pcl的*laserCloud轉換成ros的laserCloudOutMsg
    laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;//時間戳
    laserCloudOutMsg.header.frame_id = "/camera_init";//rviz中參考坐标系
    pubLaserCloud.publish(laserCloudOutMsg);//釋出總的點雲

    sensor_msgs::PointCloud2 cornerPointsSharpMsg;
    pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
    cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
    cornerPointsSharpMsg.header.frame_id = "/camera_init";
    pubCornerPointsSharp.publish(cornerPointsSharpMsg);

    sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
    pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
    cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
    cornerPointsLessSharpMsg.header.frame_id = "/camera_init";
    pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);

    sensor_msgs::PointCloud2 surfPointsFlat2;
    pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
    surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
    surfPointsFlat2.header.frame_id = "/camera_init";
    pubSurfPointsFlat.publish(surfPointsFlat2);

    sensor_msgs::PointCloud2 surfPointsLessFlat2;
    pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
    surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
    surfPointsLessFlat2.header.frame_id = "/camera_init";
    pubSurfPointsLessFlat.publish(surfPointsLessFlat2);

    // pub each scam
    if(PUB_EACH_LINE)
    {
        for(int i = 0; i< N_SCANS; i++)
        {
            sensor_msgs::PointCloud2 scanMsg;
            pcl::toROSMsg(laserCloudScans[i], scanMsg);
            scanMsg.header.stamp = laserCloudMsg->header.stamp;
            scanMsg.header.frame_id = "/camera_init";
            pubEachScan[i].publish(scanMsg);
        }
    }
    //總的時間輸出
    printf("scan registration time %f ms *************\n", t_whole.toc());
    if(t_whole.toc() > 100)
        ROS_WARN("scan registration process over 100ms");
}

4.文檔最後，為整個程式的主函數，先啟動ROS，判斷雷射雷達線數是否符合标準，最後定義要釋出的topic名稱，以及緩沖區大小。

int main(int argc, char **argv)
{
    ros::init(argc, argv, "scanRegistration");//初始化節點
    ros::NodeHandle nh;//建立句柄，啟動ROS
    nh.param<int>("scan_line", N_SCANS, 16);//scan_line為參數名，N_SCANS為儲存參數的值，沒有參數值的時候預設值設為16
    nh.param<double>("minimum_range", MINIMUM_RANGE, 0.1);//定義最小距離的參數，沒有參數值的時候預設值設定為0.1
    printf("scan line number %d \n", N_SCANS);
    if(N_SCANS != 16 && N_SCANS != 32 && N_SCANS != 64)//确定雷射雷達的線數符合标準
    {
        printf("only support velodyne with 16, 32 or 64 scan line!");
        return 0;
    }
    //接收雷射雷達信号
    ros::Subscriber subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>("/velodyne_points", 100, laserCloudHandler);
    //全部點雲
    pubLaserCloud = nh.advertise<sensor_msgs::PointCloud2>("/velodyne_cloud_2", 100);
    //角點
    pubCornerPointsSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_sharp", 100);
    //降采樣角點
    pubCornerPointsLessSharp = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_sharp", 100);
    //平面點
    pubSurfPointsFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_flat", 100);
    //釋出除了角點之外全部點  經過了濾波處理
    pubSurfPointsLessFlat = nh.advertise<sensor_msgs::PointCloud2>("/laser_cloud_less_flat", 100);
    //去除點
    pubRemovePoints = nh.advertise<sensor_msgs::PointCloud2>("/laser_remove_points", 100);
    //每行掃描到的點
    if(PUB_EACH_LINE)
    {
        for(int i = 0; i < N_SCANS; i++)
        {
            ros::Publisher tmp = nh.advertise<sensor_msgs::PointCloud2>("/laser_scanid_" + std::to_string(i), 100);
            pubEachScan.push_back(tmp);
        }
    }
    ros::spin();//到這，一直執行回調函數，直到ros主程序關閉，且不往下繼續執行
    return 0;
}

5.重點内容詳細解析

（1）垂直俯仰角angle的計算：主要應用了幾何關系以及弧度與角度之間的轉換

A-LOAM源碼解析——scanRegistration.cpp

（2）16線雷射雷達介紹：Velodyne VLP 16每秒可以高達30萬個點資料輸出，可以實作水準360掃描，垂直視場-15到+15，是以在計算scanID的時候，angle帶入的其實是[-15,15]。

（3）曲率的計算：論文中的計算公式為

A-LOAM源碼解析——scanRegistration.cpp

         第k幀點雲中的一個點Xi，附近的點集合為S。根據公式可以看出，曲率等于該點分别與其前後點差的和的範數，與集合中點數與自身範數乘積的比值。而我們的目的是比較曲率的大小，是以代碼中簡化了，隻求一個點與周圍前後5個點差平方和作為該點的曲率，來确定各點曲率的大小關系。

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;

（4）PCL體素栅格濾波器的使用（降采樣部分）：

         作用：實作降采樣，在保持點運形狀特征的情況下，可以減少點雲數量，提高配準，曲面重建等算法的速度。

pcl::PointCloud<PointType> c;//存放濾波之後結果的點雲
        pcl::VoxelGrid<PointType> a;//濾波器
        a.setInputCloud(b);//需要降采樣處理的點雲
        a.setLeafSize(0.2, 0.2, 0.2);//設定濾波時建立的體素體積
        a.filter(c);//處理結果放入c

感謝以下部落客文章提供的幫助：

https://blog.csdn.net/suyunzzz/article/details/108961925

https://blog.csdn.net/unlimitedai/article/details/105711240

https://blog.csdn.net/qq_41586768/article/details/108123854

寫在最後：新人小白，如有問題還請見諒;

                  整理不易，覺得有幫助的可以點個贊啥的，感謝！！！

小白a-loam源碼學習 經驗分享 計算機視覺 SLAM
上一篇: （3）LOAM的安裝運作——地圖儲存及bug解決
下一篇: 儲存檢視三維點雲地圖

繼續閱讀