1. 这个文件整体作用这个文件的输入是原始点云subLaserCloud nh.subscribesensor_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_info3. 内存与核心数据结构代码中最重要的数据结构有三个矩阵cv::Mat rangeMat; // 距离图像每个格子存这个方向上点到雷达的距离 range cv::Mat labelMat; // 分割标签图每个格子存这个点属于哪个聚类 cv::Mat groundMat; // 地面标记图标记这个格子是否是地面点它们的尺寸都是N_SCAN × Horizon_SCAN其中N_SCAN是雷达线数比如 VLP-16 就是 16Horizon_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::vectorint 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.atfloat(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.atint8_t(i,j) 1; groundMat.atint8_t(i1,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.atint8_t(i,j) 1 || rangeMat.atfloat(i,j) FLT_MAX){ labelMat.atint(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.atint(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.atfloat(fromIndX, fromIndY), rangeMat.atfloat(thisIndX, thisIndY)); d2 std::min(rangeMat.atfloat(fromIndX, fromIndY), rangeMat.atfloat(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.atint(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.atint(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.atint(i,j) 0 || groundMat.atint8_t(i,j) 1){意思是如果这个点属于有效聚类或者它是地面点就有机会进入segmentedCloud。但是对于 outlierif (labelMat.atint(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.atint8_t(i,j) 1){ if (j%5!0 j5 jHorizon_SCAN-5) continue; }意思是大部分地面点会被跳过只保留每 5 列一个地面点。原因是地面点数量非常多如果全部保留会增加后续曲率计算和特征提取的计算量。保存到segmentedCloud前还会记录几个非常重要的信息segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] (groundMat.atint8_t(i,j) 1); segMsg.segmentedCloudColInd[sizeOfSegCloud] j; segMsg.segmentedCloudRange[sizeOfSegCloud] rangeMat.atfloat(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_{i1} ... range_{i5}如果某根 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是两个相邻点中较大的 ranged2是较小的 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 点云整理成有行列结构、有地面标记、有聚类标签、有距离信息的分割点云为后续里程计估计提供更干净、更稳定、更高效的数据基础。版权声明 辛苦码字不易转载请注明原文出处和作者信息谢谢理解欢迎分享与交流但拒绝任何形式的商业转载或洗稿。
Lego-LOAM中imageProjection详解解释
1. 这个文件整体作用这个文件的输入是原始点云subLaserCloud nh.subscribesensor_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_info3. 内存与核心数据结构代码中最重要的数据结构有三个矩阵cv::Mat rangeMat; // 距离图像每个格子存这个方向上点到雷达的距离 range cv::Mat labelMat; // 分割标签图每个格子存这个点属于哪个聚类 cv::Mat groundMat; // 地面标记图标记这个格子是否是地面点它们的尺寸都是N_SCAN × Horizon_SCAN其中N_SCAN是雷达线数比如 VLP-16 就是 16Horizon_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::vectorint 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.atfloat(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.atint8_t(i,j) 1; groundMat.atint8_t(i1,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.atint8_t(i,j) 1 || rangeMat.atfloat(i,j) FLT_MAX){ labelMat.atint(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.atint(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.atfloat(fromIndX, fromIndY), rangeMat.atfloat(thisIndX, thisIndY)); d2 std::min(rangeMat.atfloat(fromIndX, fromIndY), rangeMat.atfloat(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.atint(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.atint(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.atint(i,j) 0 || groundMat.atint8_t(i,j) 1){意思是如果这个点属于有效聚类或者它是地面点就有机会进入segmentedCloud。但是对于 outlierif (labelMat.atint(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.atint8_t(i,j) 1){ if (j%5!0 j5 jHorizon_SCAN-5) continue; }意思是大部分地面点会被跳过只保留每 5 列一个地面点。原因是地面点数量非常多如果全部保留会增加后续曲率计算和特征提取的计算量。保存到segmentedCloud前还会记录几个非常重要的信息segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] (groundMat.atint8_t(i,j) 1); segMsg.segmentedCloudColInd[sizeOfSegCloud] j; segMsg.segmentedCloudRange[sizeOfSegCloud] rangeMat.atfloat(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_{i1} ... range_{i5}如果某根 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是两个相邻点中较大的 ranged2是较小的 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 点云整理成有行列结构、有地面标记、有聚类标签、有距离信息的分割点云为后续里程计估计提供更干净、更稳定、更高效的数据基础。版权声明 辛苦码字不易转载请注明原文出处和作者信息谢谢理解欢迎分享与交流但拒绝任何形式的商业转载或洗稿。
