一、对应源文件
add_executable(imageProjection src/imageProjection.cpp)
二、原理
1. 转换为深度图
分辨率:1800 x16
VLP-16 横向角度分辨率 ,所以列数:
纵向角度分辨率 ,16 个扫描线束,所以列数为 16
每个像素的像素值:点云测距的距离
2. 地面点云提取
(1). 选定初始范围
对于 16 线束,0-7 扫描线束是在水平线以下。所以只用在 0-7 线束的点云中选择地面点云
(2). 通过俯仰角进行二次校验
依据相邻线束点云计算俯仰角,俯仰角和传感器水平角度接近平行的,才选择为地面点
3. 非地面点云分割聚类
BFS 聚类处理
每一个聚类结果,作为一个新的类别
地面点云在处理时被忽略
同时过剔除小面积的聚类结果,可以过滤掉一部分噪声
结果可视化:
a:原始点云
b:分割聚类后结果
三、ROS interface
1.1 subscribe topic
subLaserCloud = nh.subscribe<sensor_msgs::PointCloud2>(pointCloudTopic, 1, &ImageProjection::cloudHandler, this);
1.2. publish topic
pubFullCloud = nh.advertise<sensor_msgs::PointCloud2> ("/full_cloud_projected", 1);
pubFullInfoCloud = nh.advertise<sensor_msgs::PointCloud2> ("/full_cloud_info", 1);
pubGroundCloud = nh.advertise<sensor_msgs::PointCloud2> ("/ground_cloud", 1);
pubSegmentedCloud = nh.advertise<sensor_msgs::PointCloud2> ("/segmented_cloud", 1);
pubSegmentedCloudPure = nh.advertise<sensor_msgs::PointCloud2> ("/segmented_cloud_pure", 1);
pubSegmentedCloudInfo = nh.advertise<cloud_msgs::cloud_info> ("/segmented_cloud_info", 1);
pubOutlierCloud = nh.advertise<sensor_msgs::PointCloud2> ("/outlier_cloud", 1);
四、算法 pipeline
1. copyPointCloud
1.1 作用
将 ros msg 转换为 pcl 点云,并进行预处理
1.2 处理流程
// 1. 格式转换
pcl::fromROSMsg(*laserCloudMsg, *laserCloudIn);
// 2. 去除 nan 值
std::vector<int> indices;
pcl::removeNaNFromPointCloud(*laserCloudIn, *laserCloudIn, indices);
2. findStartEndAngle
2.1 作用
计算起始终止旋转角度
2.2 处理流程
// 更新 segMsg 的起始、终止、delta 旋转角
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;
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;
3. projectPointCloud
3.1 作用
将点云映射为深度图
3.2 相关变量
cv::Mat rangeMat; // range matrix for range image
3.3 初始化
rangeMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32F, cv::Scalar::all(FLT_MAX));
3.4 赋值
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;
// 依据纵向角度,查找对应的行索引
verticalAngle = atan2(thisPoint.z, sqrt(thisPoint.x * thisPoint.x + thisPoint.y * thisPoint.y)) * 180 / M_PI;
rowIdn = (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;
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;
// cv::Mat 赋值
rangeMat.at<float>(rowIdn, columnIdn) = range;
// 同时将结果填充至 fullCloud, fillInfoCloud
thisPoint.intensity = (float)rowIdn + (float)columnIdn / 10000.0;
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"
}
4. groundRemoval
4.1 作用
识别地面点云,标记为单独类别,用于后续处理
4.2 相关变量
// groundMat
// -1, no valid info to check if ground of not
// 0, initial value, after validation, means not ground
// 1, ground
cv::Mat groundMat; // ground matrix for ground cloud marking
// labelMat
// -1: ground pts or invalid pts, skip for label segmentation
// 999999: invalid segment result
// 0, initial value, invalid
// >0: valid segment result
cv::Mat labelMat; // label matrix for segmentaiton marking
4.3 初始化
groundMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_8S, cv::Scalar::all(0));
labelMat = cv::Mat(N_SCAN, Horizon_SCAN, CV_32S, cv::Scalar::all(0));
4.4 选择初始范围点云
for (size_t j = 0; j < Horizon_SCAN; ++j){
for (size_t i = 0; i < groundScanInd; ++i){ // 仅仅校验 0-groundScanInd 扫描束范围内的点云
......
}
}
4.5 依据俯仰角二次校验
将 groundMat 对应位置设置为 1
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;
}
4.5 更新 ground pts label
将 地面点云 和 距离无效的点云 label 设置为 -1,不用于后面的分割聚类
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;
}
}
}
5. cloudSegmentation
5.1 作用
点云聚类处理
5.2 预备知识
广度优先遍历:广度优先搜索算法(BFS)详解_Amelie_xiao的博客-CSDN博客_bfs算法
5.3 初始化
// 聚类索引
labelCount = 1;
// 邻域迭代器
std::vector<std::pair<int8_t, int8_t> > neighborIterator; // neighbor iterator for segmentaiton process
std::pair<int8_t, int8_t> neighbor;
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);
// 通过动态数组实现队列
queueIndX = new uint16_t[N_SCAN*Horizon_SCAN];
queueIndY = new uint16_t[N_SCAN*Horizon_SCAN];
// 存储遍历的所有像素点
allPushedIndX = new uint16_t[N_SCAN*Horizon_SCAN];
allPushedIndY = new uint16_t[N_SCAN*Horizon_SCAN];
5.4 labelComponents
(1). 目的:往外拓展,聚类分析
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);
(2). 对单个像素 bfs 聚类
// 输入: 聚类起始位置: rwo, 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;
// 四邻域遍历
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;
}
}
}
(3). 剔除错误过滤区域
条件1:聚类点云个数 >= 30
条件2:聚类点云个数 < 30 && 聚类的点云分布行数 >= segmentValidPointNum
否则,聚类标签设置为 999999,代表无效聚类结果
// check if this segment is valid
bool feasibleSegment = false;
if (allPushedIndSize >= 30) // 满足条件1
feasibleSegment = true;
else if (allPushedIndSize >= segmentValidPointNum) { // 满足条件2
int lineCount = 0;
for (size_t i = 0; i < N_SCAN; ++i)
if (lineCountFlag[i] == true)
++lineCount;
if (lineCount >= segmentValidLineNum)
feasibleSegment = true;
}
if (feasibleSegment == true) { // segment is valid, mark these points
++labelCount;
}else{ // segment is invalid, mark these points
for (size_t i = 0; i < allPushedIndSize; ++i){
labelMat.at<int>(allPushedIndX[i], allPushedIndY[i]) = 999999;
}
}
5.5 统计分割聚类结果
// 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]);
5.6 填充可视化结果
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);
}
}
}
6. 其他操作
// 6. Publish all clouds
publishCloud();
// 7. Reset parameters for next iteration
resetParameters();