继续来看回调函数
void cloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudMsg){
// 1. Convert ros message to pcl point cloud
copyPointCloud(laserCloudMsg);
// 2. Start and end angle of a scan
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();
}
一、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){
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;
}
}
}
if (pubGroundCloud.getNumSubscribers() != 0){
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]);
}
}
}
}//extern const int groundScanInd =12;
size_t i = 0; i < groundScanInd; ++i;在这一步循环中用到了头文件定义的groundScanInd，这个参数是lego-loam设置的分割地面点的最大线束，在小于12的线束中提取地面点。
lowerInd = j + ( i )*Horizon_SCAN;
upperInd = j + (i+1)*Horizon_SCAN;//通过两个相邻线束，计算在r-z面上两个点的方向向量和水平面的夹角。
具体可以看这篇文章，lego-loam采用的方法是这篇文章方法的简化版，从图上对梯度的理解就是OSo方向上，两个点pi-1和pi的方向向量。
fullCloud->points[lowerInd].intensity == -1 ||fullCloud->points[upperInd].intensity == -1
//如果在点云投影到深度相片上的时候，出现了问题会导致points[lowerInd].intensity == -1，这个时候不确定是不是地面点，将其在GroundMat中的值设为-1。
计算xyz方向的差分，计算xy面上的半径r，计算diffz与r的夹角。
//extern const float sensorMountAngle = 0.0;
if (abs(angle - sensorMountAngle) <= 10){
groundMat.at<int8_t>(i,j) = 1;
groundMat.at<int8_t>(i+1,j) = 1;
}//如果angle角小于10°，认为是地面点。
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;}}}
//FLT_MAX这个值是单精度浮点数能表示的最大正数，但不包括无穷大。FLT_MAX是rangeMat的初始化值；
将距离为无穷大的点和地面点在labelMat中标记为-1，这一步应该是为后续操作准备的。
最后保存并发布地面点！！
二、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);//要搞清楚这个循环的作用：
首先来看嵌套在cloudSegmentation中的函数labelComponents
labelComponents()
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};
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;
//labelCount未赋值，初值为0。
// 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)));
if (angle > 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;
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<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
}
}
}
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;这是几个定义
了解一下什么是广度优先搜索：借用一下大佬的图片，有一个初步的了解即可
lego-loam使用广度优先搜索可以提高代码的运行效率。
深度优先搜索（DFS）和广度优先搜索（BFS）_深度优先搜索和广度优先搜索对比-CSDN博客
初始化队列中的第一个元素，弹出(pop)队列中的第一个元素。
fromIndX = queueIndX[queueStartInd];
fromIndY = queueIndY[queueStartInd];
通过当前点寻找相邻点
for (auto iter = neighborIterator.begin(); iter != neighborIterator.end(); ++iter){
//neighborIterator.begin()关于neighborIterator的定义在代码中没有体现，可能藏在哪一个头文件里？？猜测可能是四邻域，即【0，0】，【0，1】，【1，0】，【1，1】。
thisIndX = fromIndX + (*iter).first;
thisIndY = fromIndY + (*iter).second;
如果邻居点已经被标记（即 labelMat.at<int>(thisIndX, thisIndY) != 0），则使用 continue 语句跳过当前循环的剩余部分，直接开始下一次循环。这样做可以避免将已经检查过的点再次加入队列，从而防止无限循环。
if ((*iter).first == 0)：这是一个条件判断，用于检查当前邻居点的 x 坐标偏移量是否为0。iter 是一个迭代器，指向 neighborIterator 集合中的一个元素，该元素是一个 std::pair<int, int>，其中 first 成员表示 x 坐标的偏移量，second 成员表示 y 坐标的偏移量。
extern const float segmentAlphaX = ang_res_x / 180.0 * M_PI;
extern const float segmentAlphaY = ang_res_y / 180.0 * M_PI;
//extern const float ang_res_x = 0.2;
//extern const float ang_res_y = 2.0;
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)));这一步是在算什么？参考大佬的文章，我们来看一张图片
LeGO-LOAM源码解析2： imageProjection_lego loam imageprojection-CSDN博客
这样我们就很清楚这一步计算的angle是什么了，然后我们来看angle有什么用？
extern const float segmentTheta = 60.0/180.0*M_PI; // decrese this value may improve accuracy
通过判断angle是否大于60°，来判断索引点与相邻点是否在一个面上。
如果angle大于60°，就将相邻点加入队列，labelMat标记为0，下一次再遍历相邻点的相邻点。
将所有angle大于60°的相邻点（前提是从一个点开始搜索的子子孙孙），加入allPushedInd，为后面聚类做准备。lineCountFlag[thisIndX] = true;如果大于60°的相邻点数目比较少，就考虑在同一行上的大于60°的相邻点数目，如果这个数目大于3，则也认为是一个有效聚类。
if (allPushedIndSize >= 30)
feasibleSegment = true;
如果allPushedInd点数目大于30，认为是一个有效聚类。
//extern const int segmentValidPointNum = 5;
//extern const int segmentValidLineNum = 3;
如果allPushedInd数目小于30，大于5
else if (allPushedIndSize >= segmentValidPointNum){
int lineCount = 0;
for (size_t i = 0; i < N_SCAN; ++i)
if (lineCountFlag[i] == true)
++lineCount;//这个数目大于3，则也认为是一个有效聚类。
if (lineCount >= segmentValidLineNum)
feasibleSegment = true;
}
完成一个聚类之后，lableCount加1。也就是这个聚类标记为1，下一个聚类标记为2、3、4....
如果数目都不满足聚类条件，就标记为99999。
然后我们在看cloudSegmentation()！！
cloudSegmentation()
void cloudSegmentation(){
// segmentation process
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);
int sizeOfSegCloud = 0;
// extract segmented cloud for lidar odometry
for (size_t i = 0; i < N_SCAN; ++i) {
segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5;
for (size_t j = 0; j < Horizon_SCAN; ++j) {
if (labelMat.at<int>(i,j) > 0 || groundMat.at<int8_t>(i,j) == 1){
// outliers that will not be used for optimization (always continue)
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;
}
}
// majority of ground points are skipped
if (groundMat.at<int8_t>(i,j) == 1){
if (j%5!=0 && j>5 && j<Horizon_SCAN-5)
continue;
}
// mark ground points so they will not be considered as edge features later
segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1);
// mark the points' column index for marking occlusion later
segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
// save range info
segMsg.segmentedCloudRange[sizeOfSegCloud] = rangeMat.at<float>(i,j);
// save 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){
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);
}
}
}
}
}处理不会被优化的离群点！！
for (size_t i = 0; i < N_SCAN; ++i) {
segMsg.startRingIndex[i] = sizeOfSegCloud-1 + 5;//去除每束激光的前5个点。
segMsg.endRingIndex[i] = sizeOfSegCloud-1 - 5;//去除每束激光的后5个点。
1、对于已经被标记的点，其中数目不满足聚类条件的点，选择能被5整除的加入到离群点中，其他的跳过。（至于为什么是被5整除，我感觉是一种抽稀）
2、// majority of ground points are skipped大部分地面点被跳过
if (groundMat.at<int8_t>(i,j) == 1){
if (j%5!=0 && j>5 && j<Horizon_SCAN-5)？？只处理十几个地面点吗？
continue;}
3、排除零散的点和大部分地面点之后，保存分割后的点云数据！！
//记录一些分割后点云的信息，首先记录点是地面点
segMsg.segmentedCloudGroundFlag[sizeOfSegCloud] = (groundMat.at<int8_t>(i,j) == 1);
// 记录点的列索引
segMsg.segmentedCloudColInd[sizeOfSegCloud] = j;
// save range info
segMsg.segmentedCloudRange[sizeOfSegCloud] = rangeMat.at<float>(i,j);
// 保存分割后的点云
segmentedCloud->push_back(fullCloud->points[j + i*Horizon_SCAN]);
// size of seg cloud
++sizeOfSegCloud;分割后点云数目加1
4、提取不包括地面点的分割后点云，用于可视化！！、
// extract segmented cloud for visualization
if (pubSegmentedCloudPure.getNumSubscribers() != 0){
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的区别在于不包括地面点！！