Lego-LOAM中imageProjection详解解释

1. 这个文件整体作用

这个文件的输入是原始点云：

subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>( pointCloudTopic, 1, &ImageProjection::cloudHandler, this);

这里pointCloudTopic是原始雷达点云话题。代码订阅一帧sensor_msgs::PointCloud2后，会先转成 PCL 点云，再根据每个点的垂直角和水平角，把三维点投影到二维矩阵里。这个二维矩阵就是rangeMat，也可以理解成一张 LiDAR 深度图。

输出主要包括：

pubSegmentedCloud // /segmented_cloud，后续里程计主要使用的分割点云 pubSegmentedCloudInfo // /segmented_cloud_info，保存每个点的 range、列索引、是否地面等信息 pubGroundCloud // /ground_cloud，地面点云，可视化用 pubOutlierCloud // /outlier_cloud，离群点 pubFullCloud // /full_cloud_projected，完整投影点云 pubFullInfoCloud // /full_cloud_info，完整投影点云，但 intensity 存 range

最关键的是/segmented_cloud和/segmented_cloud_info。后面的FeatureAssociation会用这两个东西来计算曲率、判断遮挡、提取角点和平面点。

2. 主流程cloudHandler()

整帧点云进入后，核心流程在cloudHandler()里：

void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){ copyPointCloud(laserCloudMsg); // 1. ROS PointCloud2 转 PCL 点云，并移除 NaN 点 findStartEndAngle(); // 2. 计算当前 scan 的起始角和结束角 projectPointCloud(); // 3. 把三维点云投影到 range image 上 groundRemoval(); // 4. 根据相邻 scan line 的几何关系判断地面点 cloudSegmentation(); // 5. 对非地面点做连通域分割，过滤小簇离群点 publishCloud(); // 6. 发布分割点云、地面点云、离群点云和辅助信息 resetParameters(); // 7. 清空本帧临时变量，为下一帧点云做准备 }

这段就是整个模块的骨架。可以把它理解成：

原始点云 ↓ 转 PCL + 去 NaN ↓ 计算起止水平角 ↓ 投影成 range image ↓ 地面检测 ↓ 非地面点分割 ↓ 输出 segmented cloud 和 cloud_info

3. 内存与核心数据结构

代码中最重要的数据结构有三个矩阵：

cv::Mat rangeMat; // 距离图像，每个格子存这个方向上点到雷达的距离 range cv::Mat labelMat; // 分割标签图，每个格子存这个点属于哪个聚类 cv::Mat groundMat; // 地面标记图，标记这个格子是否是地面点

它们的尺寸都是：

N_SCAN × Horizon_SCAN

其中N_SCAN是雷达线数，比如 VLP-16 就是 16；Horizon_SCAN是水平方向分辨率，比如 1800，表示一圈水平角被分成 1800 个格子。

初始化代码如下：

rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX)); // 初始化距离图，默认是无效距离 groundMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_8S, cv::Scalar::all(0)); // 初始化地面图，0 表示未判断 labelMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32S, cv::Scalar::all(0)); // 初始化标签图，0 表示未分割 labelCount = 1; // 聚类标签从 1 开始

rangeMat默认填FLT_MAX，意思是这个格子暂时没有有效点。groundMat有三个状态：-1表示无效点，0表示非地面或未判断，1表示地面点。labelMat也有几个重要状态：0表示还没分割，-1表示不参与分割，比如地面点或无效点，999999表示无效的小聚类离群点，正数表示有效聚类标签。

4. 点云复制与 NaN 移除

函数copyPointCloud()负责把 ROS 点云转成 PCL，并移除 NaN 点：

void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){ cloudHeader = laserCloudMsg->header; // 保存当前点云 header，包括时间戳和坐标系 pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn); // ROS PointCloud2 转成 PCL 点云 laserCloudIn std::vector<int> indices; // 用于接收有效点索引 pcl::removeNaNFromPointCloud( // 移除点云中的 NaN 点 *laserCloudIn, // 输入点云 *laserCloudIn, // 输出点云，覆盖原点云 indices); // 输出有效点索引 if (useCloudRing == true){ // 如果点云自带 ring 通道 pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing); // 再转成带 ring 字段的点云 if (laserCloudInRing->is_dense == false) { // 如果点云不是 dense 格式 ROS_ERROR("Point cloud is not in dense format..."); // 报错提示 ros::shutdown(); // 关闭 ROS 节点 } } }

这里ring字段很重要。Velodyne 这类雷达通常会给每个点带上ring，表示这个点来自第几根激光线。如果点云没有ring，代码就会通过垂直角自己计算这个点应该属于哪一根 scan line。

5. 计算一帧点云的起始角和结束角

函数findStartEndAngle()用来计算一帧 scan 的起始水平角和结束水平角：

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

对应公式是：

startOrientation = -atan2(y_0, x_0) endOrientation = -atan2(y_last, x_last) + 2π

这里x_0、y_0是当前帧第一个点的水平坐标，x_last、y_last是最后一个点的水平坐标。atan2(y, x)用来计算点在水平平面上的方位角。前面加负号，是为了适配 LeGO-LOAM 内部的角度方向定义。

后面这段代码用于保证一帧的角度跨度合理：

if (segMsg.endOrientation - segMsg.startOrientation > 3 * M_PI) { segMsg.endOrientation -= 2 * M_PI; } else if (segMsg.endOrientation - segMsg.startOrientation < M_PI) segMsg.endOrientation += 2 * M_PI; segMsg.orientationDiff = segMsg.endOrientation - segMsg.startOrientation;

正常一帧 360° 扫描的角度差应该接近2π。如果角度差大于3π或小于π，说明角度跨越了-π / π边界，需要加减2π修正。orientationDiff后面会给特征关联模块使用，用来估计每个点在一帧 scan 内的相对时间。

6. 点云投影到 Range Image

这是这个文件最核心的部分之一：projectPointCloud()。

每个三维点p = (x, y, z)会被投影到二维图像格子(rowIdn, columnIdn)。其中rowIdn表示第几根激光线，columnIdn表示水平角方向的第几列。

6.1 计算行索引 rowIdn

如果点云自带ring字段：

if (useCloudRing == true){ rowIdn = laserCloudInRing->points[i].ring; // 直接使用雷达驱动给出的 ring 作为行索引 }

如果没有ring，就通过垂直角计算：

verticalAngle = atan2( thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y) ) * 180 / M_PI; rowIdn = (verticalAngle + ang_bottom) / ang_res_y;

对应公式是：

verticalAngle = atan2(z, sqrt(x² + y²)) × 180 / π rowIdn = (verticalAngle + ang_bottom) / ang_res_y

这里verticalAngle是点相对水平面的垂直角，ang_bottom是雷达最低线束角度的绝对偏移，ang_res_y是相邻 scan line 的垂直角分辨率。比如 16 线雷达，从下到上每根线对应一个不同垂直角，所以可以通过垂直角反推点属于哪一根线。

如果rowIdn超出[0, N_SCAN)，说明点不在有效线束范围内，直接跳过：

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

6.2 计算列索引 columnIdn

水平角计算如下：

horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI; columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;

对应公式是：

horizonAngle = atan2(x, y) × 180 / π columnIdn = -round((horizonAngle - 90°) / ang_res_x) + Horizon_SCAN / 2

这里horizonAngle是当前点在水平平面的方位角，ang_res_x是水平角分辨率，比如 360° / 1800 = 0.2°。通过这个公式，就能把一个角度映射成水平图像中的列编号。

如果列号超过右边界，需要绕回：

if (columnIdn >= Horizon_SCAN) columnIdn -= Horizon_SCAN;

因为 LiDAR 是 360° 扫描，水平图像左右边界本质上是相连的。

6.3 计算 range 并保存

距离公式是：

range = sqrt(x² + y² + z²)

代码如下：

range = sqrt( thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z); if (range < sensorMinimumRange) continue;

如果点太近，小于sensorMinimumRange，说明可能是雷达盲区或车体自身点，直接跳过。

然后保存到矩阵和点云里：

rangeMat.at<float>(rowIdn, columnIdn) = range; // 距离图中保存该点距离 thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0; // intensity 编码行列信息 index = columnIdn + rowIdn * Horizon_SCAN; // 二维索引转一维索引 fullCloud->points[index] = thisPoint; // 保存投影后的完整点云 fullInfoCloud->points[index] = thisPoint; // 保存带 range 信息的完整点云 fullInfoCloud->points[index].intensity = range; // fullInfoCloud 的 intensity 改存 range

这里thisPoint.intensity = rowIdn + columnIdn / 10000.0是一个小技巧。整数部分保存rowIdn，小数部分保存columnIdn，后面可以从 intensity 反推这个点在 range image 中的位置。fullInfoCloud里 intensity 不再存行列，而是存range，方便后面查看每个投影点的距离。

7. 地面检测 groundRemoval()

LeGO-LOAM 适合地面车，一个重要原因就是它显式利用地面约束。groundRemoval()会比较相邻 scan line 在同一列上的两个点，如果它们连线的倾角接近传感器安装角，就认为这两个点是地面点。

代码核心如下：

diffX = fullCloud->points[upperInd].x - fullCloud->points[lowerInd].x; diffY = fullCloud->points[upperInd].y - fullCloud->points[lowerInd].y; diffZ = fullCloud->points[upperInd].z - fullCloud->points[lowerInd].z; angle = atan2(diffZ, sqrt(diffX*diffX + diffY*diffY)) * 180 / M_PI; if (abs(angle - sensorMountAngle) <= 10){ groundMat.at<int8_t>(i,j) = 1; groundMat.at<int8_t>(i+1,j) = 1; }

对应公式是：

diff = P_upper - P_lower angle = atan2(diffZ, sqrt(diffX² + diffY²)) × 180 / π

这里P_lower是较低 scan line 上的点，P_upper是相邻较高 scan line 上的点。diffZ表示两点高度差，sqrt(diffX² + diffY²)表示两点水平距离。angle就是这两个点连线相对水平面的倾角。

判断条件是：

|angle - sensorMountAngle| ≤ 10°

其中sensorMountAngle是传感器安装角。如果两个相邻线束点形成的倾角接近传感器安装角，说明它们大概率落在同一个地面平面上，于是标记为地面点。

地面点和无效点会被标记为不参与后续非地面聚类：

if (groundMat.at<int8_t>(i,j) == 1 || rangeMat.at<float>(i,j) == FLT_MAX){ labelMat.at<int>(i,j) = -1; }

这里labelMat = -1的意思是：这个格子不参与后面的连通域分割。原因是地面点后面会单独作为平面特征候选，不需要和非地面障碍物一起做聚类。

8. 非地面点聚类分割 cloudSegmentation()

地面点处理完后，cloudSegmentation()会对剩余非地面点做连通域分割：

for (size_t i = 0; i < N_SCAN; ++i) for (size_t j = 0; j < Horizon_SCAN; ++j) if (labelMat.at<int>(i,j) == 0) labelComponents(i, j);

这里labelMat == 0表示这个点还没有被分割，也不是地面点或无效点。于是调用labelComponents(i, j)从这个点开始做 BFS 聚类。

9. BFS 聚类 labelComponents()

labelComponents()是整个分割模块最关键的函数。它不是用 PCL 的欧式聚类，而是在 range image 上做四邻域 BFS。邻域方向在构造函数里已经定义：

neighbor.first = -1; neighbor.second = 0; neighborIterator.push_back(neighbor); // 上 neighbor.first = 0; neighbor.second = 1; neighborIterator.push_back(neighbor); // 右 neighbor.first = 0; neighbor.second = -1; neighborIterator.push_back(neighbor); // 左 neighbor.first = 1; neighbor.second = 0; neighborIterator.push_back(neighbor); // 下

也就是说，每个格子只和上下左右四个格子比较。这样计算很快，适合实时运行。

9.1 聚类判断公式

对于当前点和邻居点，代码先取两个距离：

d1 = std::max(rangeMat.at<float>(fromIndX, fromIndY), rangeMat.at<float>(thisIndX, thisIndY)); d2 = std::min(rangeMat.at<float>(fromIndX, fromIndY), rangeMat.at<float>(thisIndX, thisIndY));

这里d1是两个点中较远的距离，d2是较近的距离。

然后根据邻接方向选择角分辨率：

if ((*iter).first == 0) alpha = segmentAlphaX; else alpha = segmentAlphaY;

如果是水平方向邻居，用segmentAlphaX，也就是水平角分辨率；如果是垂直方向邻居，用segmentAlphaY，也就是垂直角分辨率。

核心公式是：

angle = atan2(d2*sin(alpha), (d1 - d2*cos(alpha)));

对应公式：

angle = atan2(d₂ sin α, d₁ - d₂ cos α)

这里α是两个相邻激光束之间的角分辨率，d1是较远点距离，d2是较近点距离。这个公式来自两条相邻激光束和两个测距点形成的三角关系。它本质上衡量这两个点在空间上是否可能属于同一个物体表面。

如果：

if (angle > segmentTheta)

说明两个点之间的夹角关系足够连续，它们可能属于同一个物体，于是把邻居点加入当前聚类：

queueIndX[queueEndInd] = thisIndX; // 邻居点行索引入队 queueIndY[queueEndInd] = thisIndY; // 邻居点列索引入队 ++queueSize; // 队列大小加一 ++queueEndInd; // 队尾后移 labelMat.at<int>(thisIndX, thisIndY) = labelCount; // 标记为当前聚类标签 lineCountFlag[thisIndX] = true; // 记录这个聚类覆盖了哪根 scan line allPushedIndX[allPushedIndSize] = thisIndX; // 保存该聚类所有点的行索引 allPushedIndY[allPushedIndSize] = thisIndY; // 保存该聚类所有点的列索引 ++allPushedIndSize; // 聚类点数加一

9.2 为什么不用std::queue

代码注释里写了：

// use std::queue std::vector std::deque will slow the program down greatly

所以作者用预分配数组queueIndX / queueIndY和allPushedIndX / allPushedIndY手写队列，避免频繁动态分配内存，提高实时性。这是工程实现上的优化。

10. 有效聚类与离群点判断

BFS 完成后，需要判断这个聚类是不是有效物体。如果聚类太小，说明很可能是噪声点或离群点。

代码如下：

bool feasibleSegment = false; if (allPushedIndSize >= 30) feasibleSegment = true; else if (allPushedIndSize >= segmentValidPointNum){ int lineCount = 0; for (size_t i = 0; i < N_SCAN; ++i) if (lineCountFlag[i] == true) ++lineCount; if (lineCount >= segmentValidLineNum) feasibleSegment = true; }

判断逻辑是：

如果聚类点数 ≥ 30： 直接认为是有效聚类。 否则，如果聚类点数 ≥ segmentValidPointNum： 再检查它覆盖了多少根 scan line。 如果覆盖线数 ≥ segmentValidLineNum： 认为是有效聚类。

这个设计很合理。一个物体可能点数不是特别多，但如果它跨越多根激光线，说明它有一定空间结构，不太像随机噪声。反过来，如果只有几个点，并且集中在一根线上，通常更像离群点。

如果聚类有效：

if (feasibleSegment == true){ ++labelCount; }

说明当前标签使用完成，下一个聚类用新的labelCount。

如果无效：

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

这些点被标记为999999，后面会被当成 outlier 处理。

对非地面点做四邻域 BFS 聚类，主要是为了把空间上连续、结构稳定的非地面点和零散噪声、小碎片、离群点区分开。LeGO-LOAM 先把原始点云投影成 range image，其中行表示激光线束，列表示水平角位置，这样每个点天然可以找到上下左右四个相邻点。BFS 聚类就是从一个未标记的非地面点出发，检查它的上下左右邻居，如果两个点在 range、角分辨率和几何关系上满足连续性条件，就把它们归为同一个 cluster。这样做的意义是：墙、柱子、箱子、路沿等真实环境结构通常会形成较大的连续点簇，而反光点、远处稀疏点、扫描边缘点、动态残留点往往只形成很小的零散点簇。代码后面会根据聚类点数和跨越的 scan line 数判断这个 cluster 是否有效，有效的保留到segmentedCloud，无效的小簇标记为999999或放入outlierCloud。这样后续FeatureAssociation计算曲率、提取角点和平面点时，就不会被大量孤立噪声干扰，避免噪声点因为 range 突变被误选成角点，也能减少计算量。简单说，四邻域 BFS 聚类不是为了识别物体类别，而是利用 range image 的结构快速做非地面结构筛选和噪声过滤，让后续 SLAM 匹配使用的点更加连续、稳定、可靠。

11. 提取 segmentedCloud 和 outlierCloud

聚类完成后，cloudSegmentation()会把地面点和有效非地面点提取出来，组成segmentedCloud，供后面的特征提取使用。

关键代码如下：

if (labelMat.at<int>(i,j) > 0 || groundMat.at<int8_t>(i,j) == 1){

意思是：如果这个点属于有效聚类，或者它是地面点，就有机会进入segmentedCloud。

但是对于 outlier：

if (labelMat.at<int>(i,j) == 999999){ if (i > groundScanInd && j % 5 == 0){ outlierCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]); continue; }else{ continue; } }

如果是无效小聚类点，大部分会被跳过。只有在非地面 scan line 上，并且每隔 5 列采样一个，才加入outlierCloud。这样既保留少量离群点用于可视化或后续参考，又不会让大量噪声进入优化。

对于地面点，代码也做了降采样：

if (groundMat.at<int8_t>(i,j) == 1){ if (j%5!=0 && j>5 && j<Horizon_SCAN-5) continue; }

意思是大部分地面点会被跳过，只保留每 5 列一个地面点。原因是地面点数量非常多，如果全部保留，会增加后续曲率计算和特征提取的计算量。

保存到segmentedCloud前，还会记录几个非常重要的信息：

segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1); segMsg.segmentedCloudColInd[sizeOfSegCloud] = j; segMsg.segmentedCloudRange[sizeOfSegCloud] = rangeMat.at<float>(i,j); segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]); ++sizeOfSegCloud;

这里segmentedCloudGroundFlag表示这个点是不是地面点，后面提取角点时会避免把地面点当角点。segmentedCloudColInd记录这个点在 range image 中的列号，后面遮挡判断需要用。segmentedCloudRange记录这个点的距离，后面曲率计算和遮挡判断也需要用。

12. startRingIndex 和 endRingIndex 的作用

在提取每根 scan line 的 segmented cloud 时，代码记录了每根线在segmentedCloud中的起止索引：

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

为什么要+5和-5？因为后面FeatureAssociation计算曲率时，会用当前点前后各 5 个点：

curvature_i ≈ range_{i-5} + ... + range_{i-1} - 10 range_i + range_{i+1} + ... + range_{i+5}

如果某根 scan line 的开头或结尾没有足够邻居点，直接计算会越界或者不稳定。所以这里提前把每根线的有效起点向后挪 5 个点，终点向前挪 5 个点。

13. 发布结果 publishCloud()

最后publishCloud()发布所有结果。最重要的是：

segMsg.header = cloudHeader; pubSegmentedCloudInfo.publish(segMsg);

这一步发布/segmented_cloud_info，它包含后续特征提取需要的辅助信息。

然后发布离群点：

pcl::toROSMsg(*outlierCloud, laserCloudTemp); laserCloudTemp.header.stamp = cloudHeader.stamp; laserCloudTemp.header.frame_id = "base_link"; pubOutlierCloud.publish(laserCloudTemp);

发布分割点云：

pcl::toROSMsg(*segmentedCloud, laserCloudTemp); laserCloudTemp.header.stamp = cloudHeader.stamp; laserCloudTemp.header.frame_id = "base_link"; pubSegmentedCloud.publish(laserCloudTemp);

这里 frame_id 写成"base_link"，说明输出点云被认为在机器人基坐标系下。后续模块会基于这些点云继续做特征提取和里程计估计。

14. 这个模块与后续 FeatureAssociation 的关系

这个文件本身不做位姿估计，但它为后面的里程计模块准备数据。它输出的/segmented_cloud会进入FeatureAssociation，后者再做：

calculateSmoothness() 根据 segmentedCloudRange 计算曲率 markOccludedPoints() 根据 segmentedCloudColInd 和 segmentedCloudRange 判断遮挡点 extractFeatures() 根据曲率和 segmentedCloudGroundFlag 提取角点和平面点

所以ImageProjection的质量会直接影响后面的特征质量。如果这里投影错了、地面误判了、分割错误了，后面角点和平面点都会受影响。

15. 核心公式汇总

这个文件中最关键的公式有四类。

第一类是水平角和垂直角：

verticalAngle = atan2(z, sqrt(x² + y²)) × 180 / π horizonAngle = atan2(x, y) × 180 / π

它们用于把三维点投影到二维 range image。

第二类是距离：

range = sqrt(x² + y² + z²)

它用于填充rangeMat，也用于后续地面判断、分割和曲率计算。

第三类是地面判断角：

angle_ground = atan2(diffZ, sqrt(diffX² + diffY²)) × 180 / π

如果|angle_ground - sensorMountAngle| ≤ 10°，就认为相邻两线上的点属于地面。

第四类是分割连通性角：

angle_segment = atan2(d₂ sin α, d₁ - d₂ cos α)

其中d1是两个相邻点中较大的 range，d2是较小的 range，α是水平或垂直角分辨率。如果angle_segment > segmentTheta，说明两个点空间上连续，可以归为同一聚类。

16. 总结

这个ImageProjection文件是 LeGO-LOAM 前端链路中的第一个核心模块，主要负责把原始三维 LiDAR 点云转换成结构化的range image，并在range image上完成地面提取和非地面点分割。它的输入是原始/points_raw类似的点云话题，输出是/segmented_cloud、/segmented_cloud_info、/ground_cloud、/outlier_cloud等结果。其中真正给后续里程计使用的主要是/segmented_cloud和/segmented_cloud_info。前者保存经过地面处理和聚类过滤后的点云，后者保存每个点是否地面、所在列号、距离、每根scan line的起止索引等辅助信息。

代码首先通过copyPointCloud()把 ROS 点云转成 PCL 点云，并移除 NaN 点。如果点云自带ring字段，就直接用ring作为scan line行索引；如果没有ring，就通过垂直角verticalAngle = atan2(z, sqrt(x² + y²))反推出点属于哪根线。然后在findStartEndAngle()中计算当前scan的起止水平角，并修正跨越-π / π边界的问题。这个起止角并不直接用于本模块的投影，但会被保存到segMsg，供后续FeatureAssociation估计点在一帧扫描中的相对时间，从而进行运动去畸变。

projectPointCloud()是第一大核心函数，它把每个三维点根据垂直角和水平角投影到二维矩阵rangeMat中。行索引rowIdn表示点属于第几根激光线，列索引columnIdn表示点对应的水平角位置。投影完成后，rangeMat保存每个方向上的距离，fullCloud保存投影后的完整点云，fullInfoCloud则把intensity改成range，方便后续模块使用距离信息。这个过程把无序点云变成了规则图像结构，因此后续地面检测和聚类分割都可以在二维矩阵上高效完成。

groundRemoval()是第二大核心函数。它利用地面车场景下的地面几何假设，比较相邻scan line在同一列上的两个点，计算它们连线相对水平面的夹角。如果这个夹角接近传感器安装角，就认为这两个点属于地面。地面点会在groundMat中标记为1，同时在labelMat中标记为-1，表示它们不再参与非地面物体聚类。这样做的好处是可以把大量地面点从障碍物分割中排除，减少计算量，也避免地面点把不同非地面物体错误连接成一个大簇。

cloudSegmentation()和labelComponents()是第三大核心部分，也是这段代码中很重要的结构筛选环节。它们不是对所有点做普通欧式聚类，而是在range image上对非地面点做四邻域 BFS 聚类。由于点云已经被投影成二维矩阵，每个有效点都具有明确的行列关系：上下邻居对应相邻激光线，左右邻居对应相邻水平角方向。因此，四邻域 BFS 只需要检查当前点的上、下、左、右四个相邻格子，就可以快速判断哪些点在扫描几何上相邻，这比在原始三维点云中用 KD-tree 做邻域搜索更轻量，也更适合实时 SLAM。这里选择四邻域而不是八邻域，是因为上下左右邻接关系物理意义更明确，斜对角邻居同时跨越水平和垂直方向，容易把两个本来不连续的结构误连接起来，四邻域更保守、更稳定，也能减少错误聚类。

在 BFS 扩展过程中，代码判断两个相邻点是否属于同一个 cluster，并不是简单看欧式距离，而是使用angle = atan2(d₂ sin α, d₁ - d₂ cos α)这个几何连续性公式。其中d1和d2分别是两个相邻点中较远和较近的range，α是相邻激光束之间的角分辨率。如果angle > segmentTheta，说明这两个点在 LiDAR 扫描几何上更可能来自同一个连续表面，比如墙面、柱子侧面、箱体边缘或路沿结构，于是就把邻居点加入当前 cluster；如果不满足阈值，说明两点之间可能存在明显深度断层，例如近处柱子后面是远处墙面，或者一个点是孤立反光点，就不会把它们连在一起。这样 BFS 聚类实际完成的是一种基于距离图像和激光束几何关系的连续表面分割，而不是简单的点云距离聚类。

聚类完成后，代码还会进一步判断这个 cluster 是否有效。点数很多的聚类会直接保留，因为它通常对应较大的真实结构；点数较少但跨越多根scan line的聚类也会保留，因为跨线说明它在垂直方向上有一定结构，不太像随机噪声；而点数很少、又只集中在少数线束上的小簇，则会被认为是不可靠的离群结构，并标记为999999。这些点后面大多不会进入/segmented_cloud，只会少量采样进入/outlier_cloud。这个设计的意义非常关键：真实环境中的墙、柱子、货架、箱体、路沿等非地面结构通常能形成连续点簇，而远处稀疏点、反光点、扫描边缘点、动态物体残留点往往只形成很小、很不稳定的 cluster。通过四邻域 BFS 和有效性筛选，代码可以把稳定结构点和零散噪声点区分开，为后续特征提取提供更干净的输入。

最后，代码会从labelMat和groundMat中提取出有效的segmentedCloud。有效非地面聚类点会保留，地面点会按列降采样后保留，过小的离群聚类大多丢弃，只少量加入outlierCloud。同时，代码记录segmentedCloudGroundFlag、segmentedCloudColInd、segmentedCloudRange、startRingIndex和endRingIndex。这些信息对后续特征提取非常关键：地面标志可以避免地面点被选成角点，列号和距离可以用于遮挡判断，起止索引可以保证曲率计算时不会越界，而经过 BFS 聚类过滤后的非地面点则更可能来自稳定的几何结构，能降低噪声点被误选为角点的概率。

总体来看，这个文件完成的是 LeGO-LOAM 中“点云结构化 + 地面检测 + 聚类过滤”的预处理任务。它不直接输出位姿，也不做优化，但它决定了后续特征点提取的输入质量。尤其是四邻域 BFS 聚类这一步，它的作用不是识别物体类别，而是利用range image的行列邻接关系和 LiDAR 扫描几何连续性，把空间上连续、结构稳定的非地面点保留下来，把孤立噪声、小碎片点和不可靠离群点过滤掉。如果没有这一步，大量零散噪声点可能会因为range突变在后续曲率计算中被误认为角点，导致错误特征进入 scan-to-scan 或 scan-to-map 匹配，最终影响位姿估计稳定性。因此，ImageProjection可以理解成 LeGO-LOAM 的“前端入口清洗器”：它把原始、无序、包含地面和噪声的 LiDAR 点云，整理成有行列结构、有地面标记、有聚类标签、有距离信息的分割点云，为后续里程计估计提供更干净、更稳定、更高效的数据基础。

版权声明： 辛苦码字不易，转载请注明原文出处和作者信息，谢谢理解！

欢迎分享与交流，但拒绝任何形式的商业转载或洗稿。