# 一些标志位，比如是否是地面点,以及在深度图中的索引 bool[] segmentedCloudGroundFlag # true - ground point, false - other points uint32[] segmentedCloudColInd # point column index in range image float32[] segmentedCloudRange # point range 点的深度
在cloudHandler中可以看到此模块的主流程，分别是以下几步：
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16
// 1. Convert ros message to pcl point cloud // 点云转换成pcl,去NAN copyPointCloud(laserCloudMsg); // 2. Start and end angle of a scan // 计算一帧点云水平方向的起始旋转角度, 角度差/360 * 0.1用来得到每个点在当前帧时间内运动的相对时间 findStartEndAngle(); // 3.点云投影成深度图, 计算点的深度， 确定点的索引方式 projectPointCloud(); // 4.粗略标记地面点云 groundRemoval(); // 5. 点云分割,先聚类再分割,方便提取特征 cloudSegmentation(); // 6. Publish all clouds publishCloud(); // 7. Reset parameters for next iteration resetParameters();
// 点云逐一计算深度，将具有深度的点云保存至fullInfoCloud intensity字段中 index = columnIdn + rowIdn * Horizon_SCAN; // index = 列数 + 行数×1800 fullCloud->points[index] = thisPoint; fullInfoCloud->points[index] = thisPoint; fullInfoCloud->points[index].intensity = range; // the corresponding range of a point is saved as "intensity" } }
void ImageProjection::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;
// queueSize指的是在特征处理时还未处理好的点的数量，因此该while循环是在尝试检测该特定点的周围的点的几何特征 // 以当前点(row,col)为中心，在深度图中向四周扩散，进行聚类 while (queueSize > 0) { // Pop point fromIndX = queueIndX[queueStartInd]; fromIndY = queueIndY[queueStartInd]; --queueSize; ++queueStartInd; // Mark popped point labelMat.at<int>(fromIndX, fromIndY) = labelCount;
//检查上下左右四个邻点，判定其在xoz平面的俯仰角是否满足要求，否则纳入聚类簇 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与d2分别是该点与某邻点的深度 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));
// check if this segment is valid // 当邻点数目达到30后，则该帧雷达点云的几何特征配置成功 bool feasibleSegment = false; if (allPushedIndSize >= 30) feasibleSegment = true; elseif (allPushedIndSize >= segmentValidPointNum) { // 聚类簇之后5-30个点时,统计垂直方向的点数lineCount,数量满足要求则认为此聚类粗有效 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; } } }
