LeGO-LOAM(2):lego-loam源码imageProjection.cpp学习

目录

一、imageProjection

1.1 各种标记的含义

1.1.1 groundMat 地面属性矩阵

1.1.2 rangeMat 距离矩阵

1.1.3 labelMat 标签矩阵

1.2 点云分割函数

1.3 发布的话题

1.3.1 `"/full_cloud_projected"(sensor_msgs::PointCloud2)`

1.3.2 `"/full_cloud_info"(sensor_msgs::PointCloud2)`

1.3.3 `"/ground_cloud"(sensor_msgs::PointCloud2)`

 1.3.4 `"/segmented_cloud"(sensor_msgs::PointCloud2)`

1.3.5 `"/segmented_cloud_pure"(sensor_msgs::PointCloud2)`

1.3.6 `"/segmented_cloud_info"(cloud_msgs::cloud_info)`

1.3.7 `"/outlier_cloud"(sensor_msgs::PointCloud2)`

# 参考链接

## 博客1

## 博客2

---

一、imageProjection

1.1 各种标记的含义

数据类型： cv::Mat

1.1.1 groundMat 地面属性矩阵

0 初始值

1 有效的地面点，判断条件：与相邻线束上点的夹角不超过10°

-1 无效的地面点

1.1.2 rangeMat 距离矩阵

FLT_MAX 浮点数的最大值，初始化信息

range 距离值

1.1.3 labelMat 标签矩阵

作用：用于将激光雷达点标记成不同类，除了无效点和异常点，剩下的非0标签的点均是具有“局部相邻点聚成一类”的特点

0 初始值

-1 无效点，判断条件：

groundMat.at(i,j) == 1 || rangeMat.at(i,j) == FLT_MAX

labelCount 类别标签，不同数值表示不同类别，大于0的数值可不必纠结具体属于哪一类

999999 相邻点角度变化较大且不构成聚类的点，判断条件有两个：

if (angle > segmentTheta){

        // check if this segment is valid
        bool feasibleSegment = false;
        // 如果allPushedIndSize 累加的数量增加到了30个，则判断这部分点云属于一个聚类
        if (allPushedIndSize >= 30)
            feasibleSegment = true;
        // 如果垂直方向上点的数量大于5个，默认是一个有效的聚类
        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;            
        }
        // segment is valid, mark these points
        if (feasibleSegment == true){
            ++labelCount;
        }else{ // segment is invalid, mark these points
            for (size_t i = 0; i < allPushedIndSize; ++i){
                labelMat.at(allPushedIndX[i], allPushedIndY[i]) = 999999;
            }
        }

1.2 点云分割函数

参考论文：

Effificient Online Segmentation for Sparse 3D Laser Scans

Fast Range Image-Based Segmentation of Sparse 3D Laser Scans for Online Operation

知乎复现：

《地面点障碍物快速分割聚类》 https://zhuanlan.zhihu.com/p/72932303

1.3 发布的话题

rqt_graph图

1.3.1 `"/full_cloud_projected"(sensor_msgs::PointCloud2)`

作用：存放三维空间坐标信息和投影后的二维行列信息

（1）初始化

初始化为无效点，并设置初始点个数大小：

nanPoint.x = std::numeric_limits::quiet_NaN();

nanPoint.y = std::numeric_limits::quiet_NaN();

nanPoint.z = std::numeric_limits::quiet_NaN();

nanPoint.intensity = -1;
//--------------------------

fullCloud->points.resize(N_SCAN*Horizon_SCAN);

（2）存放信息

保存的信息为三维坐标信息和投影后的二维行列信息：

thisPoint.x = laserCloudIn->points[i].x;

thisPoint.y = laserCloudIn->points[i].y;

thisPoint.z = laserCloudIn->points[i].z;

//-------------------------------

// 将行索引和列索引存储在 intensity 中，整数部分是线束数值，小数部分是方向角度

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

fullCloud->points[index] = thisPoint;

1.3.2 `"/full_cloud_info"(sensor_msgs::PointCloud2)`

作用：存放三维空间坐标信息和距离信息

（1）初始化

初始化部分和1.3.1一样

（2）存放信息

保存的信息为三维坐标信息和距离信息：

range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);

//----------------------------------

fullInfoCloud->points[index] = thisPoint;

fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity"

1.3.3 `"/ground_cloud"(sensor_msgs::PointCloud2)`

含义：原始稠密的地面点云（没有进行下采样）

初始化：只进行类型初始化，没有进行大小初始化

判断条件：

groundMat.at(i,j) == 1

地面点云特点：有0~7共8个扫描圈:

 1.3.4 `"/segmented_cloud"(sensor_msgs::PointCloud2)`

含义：去除无效的点，并对地面点进行下采样（没有标签）

两个条件满足其一即可：

（1）`groundMat.at(i,j) == 1`时选择列索引为5的倍数的点

if (j%5!=0 && j>5 && j

（2）`labelMat.at(i,j) > 0`且不满足`labelMat.at(i,j) == 999999`

1.3.5 `"/segmented_cloud_pure"(sensor_msgs::PointCloud2)`

条件：不包括地面点和无效点：

labelMat.at(i,j) > 0 && labelMat.at(i,j) != 999999

含义：三维坐标值和标签值

segmentedCloudPure->push_back(fullCloud->points[j + i*Horizon_SCAN]);

segmentedCloudPure->points.back().intensity = labelMat.at(i,j);

1.3.6 `"/segmented_cloud_info"(cloud_msgs::cloud_info)`

（1）初始化

segMsg.startRingIndex.assign(N_SCAN, 0);

segMsg.endRingIndex.assign(N_SCAN, 0);

//----------------
segMsg.segmentedCloudGroundFlag.assign(N_SCAN*Horizon_SCAN, false);

segMsg.segmentedCloudColInd.assign(N_SCAN*Horizon_SCAN, 0);

segMsg.segmentedCloudRange.assign(N_SCAN*Horizon_SCAN, 0);

（2）赋值

        获取一帧点云的开始角度和结束角度：

segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);

// 加2*M_PI表示已经转了一圈

segMsg.endOrientation = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y, laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI;

        对每一个线束的起点和终点索引进行赋值：表示的是将第0线和第15线点云横着排列后。每一线点云有一个startRingIndex和endRingIndex,表示这一线点云中的一部分，如下图绿色部分。黑色部分是整体这一线点云中筛选出来满足条件的。(暂时不知道为什么这么选？）

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


segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;

        对是否为地面点、列信息、距离信息进行赋值：

// 是否为地面点

segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at(i,j) == 1);

// 列信息

segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;

// 距离信息

segMsg.segmentedCloudRange[sizeOfSegCloud] = rangeMat.at(i,j);

1.3.7 `"/outlier_cloud"(sensor_msgs::PointCloud2)`

条件：

if (labelMat.at(i,j) == 999999){

    if (i > groundScanInd && j % 5 == 0){

        outlierCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);

        i > groundScanInd 表示 在第8线线束开始进行判断，前8线（0~7）线束主要是打到地面的，不需要怎么判断

        j % 5 是为了进行降采样

# 参考链接

建议多个博客一块看

## 博客1

这个博客时间比较新，和我现在看的github源码(时间2022年10月2日 )是一致的

https://blog.csdn.net/m0_50610065/article/details/123834057

## 博客2

这个博客时间比较旧，但比较全有较多的图解

https://blog.csdn.net/wykxwyc/article/details/98317100