代码详情参考了知乎的一篇文章:
https://zhuanlan.zhihu.com/p/489882447
下面有些代码解释写在注释中
先看main函数：
int main(int argc, char** argv){
ros::init(argc, argv, "lego_loam");
//初始化 ROS 系统，并为新节点 ("lego_loam") 设置唯一标识符
ImageProjection IP;
ROS_INFO("\033[1;32m---->\033[0m Image Projection Started.");
//ROS_INFO 是 ROS 中用于输出信息日志的宏。它的作用是将一条信息打印到控制台，通常用于调试或记录程序运行状态。
//\033[1;32m 是一个 ANSI 转义序列，用于控制终端输出的格式
//\033 表示转义字符（ESC）
//[1;32m 设置文本为绿色（32），并且加粗（1）。
//\033[0m 是重置格式的 ANSI 转义序列，它会将后续输出的文本恢复为默认格式。
ros::spin();//ros::spin() 将会使程序保持在运行状态，等待并处理所有待处理的消息和回调
return 0;
}
在初始化ros后，只实例化出一个ImageProjection对象：IP，没进行其他相关操作，因此调用该类的构造函数
注意以下代码是按照调用的顺序进行分析，所以在分析构造函数时会分析其他函数
类中定义
这些定义在分析函数时会逐渐明确其含义
ros::NodeHandle nh;//NodeHandle 是ROS中与节点通信的主要接口之一
ros::Subscriber subLaserCloud;
//ros::Subscriber 是 ROS (Robot Operating System) 中用于创建消息订阅者的一个类，允许节点从特定的话题（topic）接收消息。
//主要用于：
//订阅一个话题，从中获取发布的消息。
//指定一个回调函数来处理接收到的消息
//此处用于接收来自激光雷达（Lidar）传感器的数据
ros::Publisher pubFullCloud;
ros::Publisher pubFullInfoCloud;
ros::Publisher pubGroundCloud;
ros::Publisher pubSegmentedCloud;
ros::Publisher pubSegmentedCloudPure;
ros::Publisher pubSegmentedCloudInfo;
ros::Publisher pubOutlierCloud;
pcl::PointCloud<PointType>::Ptr laserCloudIn;
//ROS 消息格式的点云转换为 PCL 格式，并存储在 laserCloudIn 中
//声明了一个指向点云对象的智能指针
//typedef pcl::PointXYZI PointType;
//pcl::PointXYZI是PCL库中定义的一种点云数据结构。它表示一个三维空间中的点
//包括xyz坐标和intensity: 点的强度值（通常是激光雷达测量的反射强度）
//Ptr 是 PCL 提供的一个智能指针类型,用于自动管理内存，避免手动释放内存的麻烦。这样，当指针不再被使用时，内存会自动释放，有效防止内存泄漏。
pcl::PointCloud<PointXYZIR>::Ptr laserCloudInRing;
pcl::PointCloud<PointType>::Ptr fullCloud; // projected velodyne raw cloud, but saved in the form of 1-D matrix
pcl::PointCloud<PointType>::Ptr fullInfoCloud; // same as fullCloud, but with intensity - range
pcl::PointCloud<PointType>::Ptr groundCloud;
pcl::PointCloud<PointType>::Ptr segmentedCloud;
pcl::PointCloud<PointType>::Ptr segmentedCloudPure;
pcl::PointCloud<PointType>::Ptr outlierCloud;
PointType nanPoint; // fill in fullCloud at each iteration
cv::Mat rangeMat; // range matrix for range image
cv::Mat labelMat; // label matrix for segmentaiton marking
cv::Mat groundMat; // ground matrix for ground cloud marking
int labelCount;
float startOrientation;
float endOrientation;
cloud_msgs::cloud_info segMsg; // info of segmented cloud
std_msgs::Header cloudHeader;
/*
std_msgs::Header:
这是一个数据结构，定义在 ROS 的标准消息库中。它包含了与时间、序列号和坐标框架相关的信息。这个结构体非常重要，因为它用于描述其他消息的元数据，例如传感器数据的时间戳和发送顺序。
通过设置 cloudHeader 的字段，可以为后续的消息提供时间戳、序列号和坐标框架等上下文信息，从而确保消息的正确性和可追溯性*/
std::vector<std::pair<int8_t, int8_t> > neighborIterator; // neighbor iterator for segmentaiton process
uint16_t *allPushedIndX; // array for tracking points of a segmented object
uint16_t *allPushedIndY;
uint16_t *queueIndX; // array for breadth-first search process of segmentation, for speed
uint16_t *queueIndY;
构造函数
nh(“~”)
ImageProjection():
nh("~"){
//nh("~") 意味着创建一个私有的节点句柄。
//使用 "~" 作为参数意味着这个节点句柄将只访问与当前节点相关的参数，而不是全局参数。这对于避免命名冲突和管理参数非常有用。
节点名称:
在你调用 ros::init(argc, argv, “lego_loam”); 时，你实际上设置了该节点的全局名称为 “lego_loam”。
这个名称在 ROS 系统中是唯一的，其他节点可以通过这个名称来识别和通讯。
句柄：
当你创建 ros::NodeHandle nh; 时，这个对象用于与 ROS 系统进行交互，但它本身并不具有特定的名称属性。句柄，用于发布、订阅消息、服务等操作。
订阅原始的激光点云
subLaserCloud 是一个订阅者对象，它订阅了 ROS 中一个特定的话题（pointCloudTopic），并将接收到的点云数据（类型为 sensor_msgs::PointCloud2）传递给类 ImageProjection 的成员函数 cloudHandler 进行处理。
subLaserCloud 本身并不存储经过 cloudHandler 处理后的数据，而是负责订阅原始的点云数据。
处理逻辑和结果是在 cloudHandler 方法中完成的，具体如何使用接收到的数据，完全取决于你在 cloudHandler 中的实现。
如果需要对处理后的数据进行进一步操作，可以在 cloudHandler 中保存结果到类成员变量，或者发布到新的话题上
// 订阅原始的激光点云
subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 1, &ImageProjection::cloudHandler, this);
/*
当有新的 PointCloud2 消息发布到该话题时，
订阅者会调用 cloudHandler 函数，并将接收到的消息作为参数传入给 laserCloudMsg
subLaserCloud
这是一个成员变量，类型是ros::Subscriber。
它用于保存创建的订阅者实例，以便在需要时能够调用和管理这个订阅者
nh 是一个已经定义好的 ROS 节点句柄（ros::NodeHandle），用于与 ROS 系统进行交互
subscribe 是节点句柄的方法，用于订阅特定话题（topic）。
<sensor_msgs::PointCloud2> 是模板参数，指定了要订阅的消息类型。
在这个例子中，消息类型为PointCloud2，它通常用于表示激光雷达或其他传感器生成的点云数据。
pointCloudTopic 是一个字符串，表示要订阅的话题名称。是 “/velodyne_points”
extern const string pointCloudTopic = "/velodyne_points";
1是队列大小，队列只会保留最新的一条消息
&ImageProjection::cloudHandler
这是回调函数的指针，当有新的 PointCloud2 消息到达时，将调用此函数进行处理。
cloudHandler 是 ImageProjection 类的一个成员函数，负责处理接收到的点云数据。
this 指针传递给回调函数，表示当前对象的实例，使得回调函数能够访问类的成员变量和其他成员函数。
*/
回调函数
就是调用一系列函数，对接受到的数据进行处理
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
// 1. Convert ros message to pcl point cloud
copyPointCloud(laserCloudMsg);
//将 laserCloudMsg 中的点云数据复制到另一个数据结构中，以便后续的处理或分析
//将来自传感器的原始点云数据转换成自己定义的点云格式，或者进行必要的预处理
// 2. Start and end angle of a scan// 2. 找到开始时刻和结束时刻的方向角度
findStartEndAngle();
// 3. Range image projection
projectPointCloud();
// 4. Mark ground points
groundRemoval();
// 5. Point cloud segmentation
cloudSegmentation();
// 6. Publish all clouds
publishCloud();
// 7. Reset parameters for next iteration
resetParameters();
}
copyPointCloud(laserCloudMsg);
对接收到的数据进行处理， 由于激光雷达点云消息在传递过程中使用的是ROS 类型的消息，所以在处理的过错中统一需要对消息类型转换，通常的办法就是使用 PCL 库，转换为pcl点云，并移除无效点。
void copyPointCloud(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
cloudHeader = laserCloudMsg->header;//复制消息头
// cloudHeader.stamp = ros::Time::now(); // Ouster lidar users may need to uncomment this line
pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);//转换 PointCloud2 消息到 PCL 格式
//ROS 消息格式的点云转换为 PCL 格式，并存储在 laserCloudIn 中
// Remove Nan points
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
//移除点云中的无效（NaN）点，以确保数据的准确性和完整性
// have "ring" channel in the cloud // 判断是不是使用了 velodyne 的激光雷达
if (useCloudRing == true){
pcl::fromROSMsg(*laserCloudMsg, *laserCloudInRing);
if (laserCloudInRing->is_dense == false) {
ROS_ERROR("Point cloud is not in dense format, please remove NaN points first!");
ros::shutdown();
}
}
/*
这段代码检查 useCloudRing 是否为真，如果是，则再次将点云数据转换为 laserCloudInRing。
然后，它检查 laserCloudInRing 是否是稠密格式（即没有 NaN 点）。如果不是，则输出错误信息并关闭 ROS 节点。这通常是为了防止在后续处理中出现问题。
*/
}
findStartEndAngle();
第一步处理后转化为pcl格式并清除无效点后的数据，找到其初始部分和结尾部分的角度
void findStartEndAngle(){
// start and end orientation of this cloud
segMsg.startOrientation = -atan2(laserCloudIn->points[0].y, laserCloudIn->points[0].x);
/*
laserCloudIn->points[0]:
这是访问存储在 laserCloudIn 中的第一个点。这是一个包含多个三维点的点云数据结构，每个点都有 x, y, 和 z 坐标。
atan2(y, x):
atan2 返回从 x 轴到点 (x, y) 的角度。它接受两个参数：y 和 x，并返回弧度单位的角度。这个函数的好处是可以正确处理所有四个象限（即考虑正负值），因此它比简单的 atan(y/x) 更加可靠。
在这里，laserCloudIn->points[0].y 和 laserCloudIn->points[0].x 分别对应点云中第一个点的 Y 和 X 坐标。
代码中使用了负号 - 来反转方向，这可能是因为采用的坐标系或应用逻辑要求角度朝向与默认的数学坐标系相反。
在一些情况下，机器人运动的方向可能是逆时针为正，而在数学上，atan2 返回的角度是顺时针的，因此需要进行反转。
将计算得到的角度值赋值给 segMsg 结构中的 startOrientation 字段。
segMsg 可能是用于描述某一特定分割或运动状态的自定义消息结构（如某个激光扫描或路径规划过程中的状态）。
*/
segMsg.endOrientation = -atan2(laserCloudIn->points[laserCloudIn->points.size() - 1].y,
laserCloudIn->points[laserCloudIn->points.size() - 1].x) + 2 * M_PI;
/*
加上 2 * M_PI:
M_PI 是一个表示 π 的常量，因此 2 * M_PI 表示一个完整的圆周（360度）。通过加上这个值，可以确保结果为正值。
这在某些情况下是必要的，因为 atan2 的输出范围是 [-π, π]，而通过加上 2 * π 可以将负角度转换为正角度范围 [0, 2π]。
*/
//保证 所有角度 落在 [M_PI , 3M_PI] 上
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;
}
projectPointCloud();
将点云数据投影到一个范围图像上。这个函数对每个点进行处理，以计算其在图像中的行列索引，并存储相应的距离和强度信息
将三维点云数据映射到一个二维范围图像上，根据每个点的空间位置计算出其在图像中的对应行列索引，并存储距离和强度的信息。
void projectPointCloud(){
// range image projection
float verticalAngle, horizonAngle, range;
size_t rowIdn, columnIdn, index, cloudSize;
PointType thisPoint;
cloudSize = laserCloudIn->points.size();
for (size_t i = 0; i < cloudSize; ++i){//遍历每个点
thisPoint.x = laserCloudIn->points[i].x;
thisPoint.y = laserCloudIn->points[i].y;
thisPoint.z = laserCloudIn->points[i].z;
// find the row and column index in the iamge for this point
if (useCloudRing == true){
rowIdn = laserCloudInRing->points[i].ring;
}
else{
verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;//相对于水平面的垂直角度
/*
计算出的垂直角度 verticalAngle、底部角度 ang_bottom 和垂直分辨率 ang_res_y 来确定一个行索引 rowIdn
ang_bottom:
这通常表示在某种坐标系统中，视场的底部角度（也可能是负值）。它可以用来调整垂直角度，使其与特定的视场（比如摄像机或传感器的视野）对齐。
例如，如果这个值为-30度，表示视场从水平面向下偏移30度
ang_res_y:
这是垂直方向上的角分辨率，通常也以度为单位。它表示每一行所对应的角度变化量。例如，如果 ang_res_y 为1度，则每一行代表1度的视场变化。
*/
rowIdn = (verticalAngle + ang_bottom) / ang_res_y;
/*将 verticalAngle 与 ang_bottom 相加，可以获得相对于视场基准的实际垂直角度
将结果除以 ang_res_y 以转换成行索引。这样可以确定在某个图像或数据网格中，给定角度应对应于哪一行*/
}
if (rowIdn < 0 || rowIdn >= N_SCAN)
continue;
horizonAngle = atan2(thisPoint.x, thisPoint.y) * 180 / M_PI;
columnIdn = -round((horizonAngle-90.0)/ang_res_x) + Horizon_SCAN/2;
/*horizonAngle 是通过前面的计算得到的角度。减去 90 度的目的通常是为了将角度转换为与某个特定方向（例如，正下方或正上方）相关的表示方式。
在这个例子中，可能是为了把 0 度指向右侧，90 度指向上侧。
round 函数用于将计算结果四舍五入到最接近的整数
Horizon_SCAN 可能代表传感器的扫描总列数。Horizon_SCAN / 2 将索引平移，使得中心列处于数组的中间位置
*/
if (columnIdn >= Horizon_SCAN)
columnIdn -= Horizon_SCAN;
if (columnIdn < 0 || columnIdn >= Horizon_SCAN)
continue;
range = sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y + thisPoint.z * thisPoint.z);
//点到原点的距离
if (range < sensorMinimumRange)
continue;//小于设定值时，舍去
rangeMat.at<float>(rowIdn, columnIdn) = range;
/*// 将计算下来的距离传感器的 数值保存到 rangeMat 中
// 这是一个 16 * 1800 的矩阵 rowIdn为线束数值 columnIdn 是 一圈圆形 滩平之后的数值
// range 是特征点云点到原点的数值
// 这样就将一个三维的坐标 转换到一个 矩阵中了.*/
//将距离信息存储到一个矩阵中，在计算机处理图像时把图像看为一个矩阵，因此相当于把距离信息转化为图片
//存放的数值就是图片相应位置上面的点到激光雷达的距离。
// 将 index 和 横坐标存储在 intensity 中 整数部分是线束数值 小数部分是方向角度
thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;
//综合起来，这行代码的目的是根据点的行索引和列索引来计算并设置这个点的强度值。
//由于 rowIdn 的权重更高，而 columnIdn 的贡献则通过除法被缩小，因此这一策略可能旨在使 rowIdn 更加显著地影响结果
//深度图的索引值 index = 列号 + 行号 * 1800
index = columnIdn + rowIdn * Horizon_SCAN;
fullCloud->points[index] = thisPoint;
fullInfoCloud->points[index] = thisPoint;
fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity"
}
}
groundRemoval();
用于从点云数据中去除地面点。通过分析每个点的空间位置和与传感器的安装角度的关系，该函数能够判断哪些点属于地面并做出相应标记。
void groundRemoval(){
size_t lowerInd, upperInd;
float diffX, diffY, diffZ, angle;
// groundMat
// -1, no valid info to check if ground of not
// 0, initial value, after validation, means not ground
// 1, ground
for (size_t j = 0; j < Horizon_SCAN; ++j){
for (size_t i = 0; i < groundScanInd; ++i){ 前7个激光雷达扫描线束足够满足地面点的检测 所以只遍历 7 次
lowerInd = j + ( i )*Horizon_SCAN;
upperInd = j + (i+1)*Horizon_SCAN;
if (fullCloud->points[lowerInd].intensity == -1 ||
fullCloud->points[upperInd].intensity == -1){
// no info to check, invalid points
groundMat.at<int8_t>(i,j) = -1;
continue;
}
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;
}
}
}
// extract ground cloud (groundMat == 1)
// mark entry that doesn't need to label (ground and invalid point) for segmentation
// note that ground remove is from 0~N_SCAN-1, need rangeMat for mark label matrix for the 16th scan
for (size_t i = 0; i < N_SCAN; ++i){
for (size_t j = 0; j < Horizon_SCAN; ++j){
if (groundMat.at<int8_t>(i,j) == 1 || rangeMat.at<float>(i,j) == FLT_MAX){
labelMat.at<int>(i,j) = -1;//地面点标记为-1
}
}
}
if (pubGroundCloud.getNumSubscribers() != 0){
/*pubGroundCloud 通常是一个 ros::Publisher 类型的对象，代表用于发布数据的主题（topic）
getNumSubscribers() 是 ros::Publisher 类中的一个成员函数，用于获取当前订阅该发布者主题的订阅者数量*/
for (size_t i = 0; i <= groundScanInd; ++i){
for (size_t j = 0; j < Horizon_SCAN; ++j){
if (groundMat.at<int8_t>(i,j) == 1)
groundCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
//将从完整点云数据 fullCloud 中提取的特定点，添加到 groundCloud 点云容器中。
//具体来说，它根据行列索引计算出相应的点，并将该点压入到 groundCloud 中。
}
}
}
}
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);
调用了 void labelComponents(int row, int col)
来处理非地面点，主要用于在点云数据中标记连通组件（或称为区域）。该函数使用队列的方式进行广度优先搜索（BFS），将相邻的点根据一定条件归为同一类别
void labelComponents(int row, int col){
// use std::queue std::vector std::deque will slow the program down greatly
float d1, d2, alpha, angle;
int fromIndX, fromIndY, thisIndX, thisIndY;
bool lineCountFlag[N_SCAN] = {false};
/*
d1, d2 存储两个点的距离。
alpha 表示当前处理的方向的角度。
angle 用于计算当前点与相邻点之间的夹角。
fromIndX, fromIndY 是当前处理的点的坐标。
lineCountFlag 用以标记哪些行参与了分段。
*/
queueIndX[0] = row;
queueIndY[0] = col;
int queueSize = 1;
int queueStartInd = 0;
int queueEndInd = 1;
allPushedIndX[0] = row;
allPushedIndY[0] = col;
int allPushedIndSize = 1;
while(queueSize > 0){//队列不为空时
// Pop point
// 从队列中取出当前处理的点
fromIndX = queueIndX[queueStartInd];
fromIndY = queueIndY[queueStartInd];
--queueSize; // 将队列中的这个点移出
++queueStartInd;
// Mark popped point
labelMat.at<int>(fromIndX, fromIndY) = labelCount; // 标记当前点
// Loop through all the neighboring grids of popped grid // 遍历当前点的所有邻居
for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){
// new index // 计算邻居的索引
thisIndX = fromIndX + (*iter).first;
thisIndY = fromIndY + (*iter).second;
// index should be within the boundary // 检查索引是否在有效范围内
if (thisIndX < 0 || thisIndX >= N_SCAN)
continue;
// at range image margin (left or right side) // 处理行列的环绕情况
if (thisIndY < 0)
thisIndY = Horizon_SCAN - 1;
if (thisIndY >= Horizon_SCAN)
thisIndY = 0;
// prevent infinite loop (caused by put already examined point back)
if (labelMat.at<int>(thisIndX, thisIndY) != 0) // 跳过已标记的点
continue;
// 计算当前点和邻居之间的距离和角度
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));
if ((*iter).first == 0)
alpha = segmentAlphaX;
else
alpha = segmentAlphaY;
angle = atan2(d2*sin(alpha), (d1 -d2*cos(alpha)));
//d1 和 d2 分别是当前点与邻居点的最大和最小距离
if (angle > segmentTheta){
//只有当计算出的角度大于设定阈值 segmentTheta 时，才认为这两个点是可以连接的
//将符合条件的邻居加入队列
queueIndX[queueEndInd] = thisIndX;
queueIndY[queueEndInd] = thisIndY;
++queueSize;
++queueEndInd;
labelMat.at<int>(thisIndX, thisIndY) = labelCount;
lineCountFlag[thisIndX] = true;
allPushedIndX[allPushedIndSize] = thisIndX;
allPushedIndY[allPushedIndSize] = thisIndY;
++allPushedIndSize;
}
}
}
// check if this segment is valid
bool feasibleSegment = false;// 初始化为不可行的段
if (allPushedIndSize >= 30)
feasibleSegment = true;
//如果当前段包含的点数（allPushedIndSize）大于等于 30，
//则直接将 feasibleSegment 设为 true。这个阈值可能基于经验数据来决定一个段是否足够大
else if (allPushedIndSize >= segmentValidPointNum){//若点数大于等于 segmentValidPointNum，则进一步进行检查
int lineCount = 0;
for (size_t i = 0; i < N_SCAN; ++i)
if (lineCountFlag[i] == true)
++lineCount;
if (lineCount >= segmentValidLineNum)
feasibleSegment = true;
}
/*次要条件检查：若点数大于等于 segmentValidPointNum，则进一步进行检查。
线计数：定义一个 lineCount 变量用以统计满足条件的扫描行（线）。使用 lineCountFlag 数组来判断某一行是否有有效标记，如果满足条件，则增加 lineCount。
判断线数：如果有效线数 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<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
}// 标记为 999999 表示无效
}
/*
有效段处理：如果 feasibleSegment 为 true，那么增加 labelCount，表示发现了一个新的有效段。
无效段处理：如果段被判定为无效，则循环遍历 allPushedIndSize 中的所有点，将它们在 labelMat 中标记为 999999，表示这些点不属于任何有效段。
*/
}
回到cloudSegmentation();
seg cloud
segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
// size of seg cloud
++sizeOfSegCloud;
}
}
//更新起始和结束索引
segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;
}
//可视化分割结果
// extract segmented cloud for visualization
if (pubSegmentedCloudPure.getNumSubscribers() != 0){
/*检查是否有其他程序或节点正在订阅 pubSegmentedCloudPure。
只有在存在订阅者时，才会执行后续的处理和发布操作。这是一种常见的做法，以防止在没有接收者的情况下浪费计算资源。*/
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 && labelMat.at<int>(i,j) != 999999){
segmentedCloudPure->push_back(fullCloud->points[j + i*Horizon_SCAN]);
segmentedCloudPure->points.back().intensity = labelMat.at<int>(i,j);
}
}
/*
在确认当前点是有效的之后，将其添加到 segmentedCloudPure 中，这是一个新的点云对象，包含了经过分割和标记的有效点。
将该点的强度（intensity）设置为它在 labelMat 中对应的标签，这样可以在可视化时展示不同的类别或特征。
*/
}
}
}
publishCloud()
将处理后的点云数据发布到 ROS (Robot Operating System) 网络中
void publishCloud(){
// 1. Publish Seg Cloud Info
segMsg.header = cloudHeader;
pubSegmentedCloudInfo.publish(segMsg);
/*
设置消息头: 将 cloudHeader （通常包含时间戳和坐标系信息）赋值给 segMsg 的头部。
发布分割云信息: 通过 pubSegmentedCloudInfo 发布这个消息，通知其他节点有关分割云的信息，例如检测到的物体或特征的数量*/
// 2. Publish clouds
sensor_msgs::PointCloud2 laserCloudTemp;
pcl::toROSMsg(*outlierCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubOutlierCloud.publish(laserCloudTemp);
/*
转换格式: 使用 pcl::toROSMsg 将 outlierCloud 转换为适合 ROS 使用的 PointCloud2 格式。
设置时间戳与坐标系: 将消息的时间戳和坐标系（如 base_link）设置为当前的 cloudHeader 的值。
发布点云: 通过 pubOutlierCloud 发布异常点云数据。
*/
// segmented cloud with ground
pcl::toROSMsg(*segmentedCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubSegmentedCloud.publish(laserCloudTemp);
// projected full cloud
if (pubFullCloud.getNumSubscribers() != 0){
pcl::toROSMsg(*fullCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubFullCloud.publish(laserCloudTemp);
}
// original dense ground cloud
if (pubGroundCloud.getNumSubscribers() != 0){
pcl::toROSMsg(*groundCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubGroundCloud.publish(laserCloudTemp);
}
// segmented cloud without ground
if (pubSegmentedCloudPure.getNumSubscribers() != 0){
pcl::toROSMsg(*segmentedCloudPure, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubSegmentedCloudPure.publish(laserCloudTemp);
}
// projected full cloud info
if (pubFullInfoCloud.getNumSubscribers() != 0){
pcl::toROSMsg(*fullInfoCloud, laserCloudTemp);
laserCloudTemp.header.stamp = cloudHeader.stamp;
laserCloudTemp.header.frame_id = "base_link";
pubFullInfoCloud.publish(laserCloudTemp);
}
}
};
