LeGO-LOAM 系列(2): imageProjection node 分析

scofield_ly

已于 2022-04-15 16:57:12 修改

阅读量565

点赞数

分类专栏： # lidar_slam 文章标签： slam

于 2022-03-13 18:07:50 首次发布

本文链接：https://blog.csdn.net/weixin_40072161/article/details/123463319

版权

lidar_slam 专栏收录该内容

8 篇文章 8 订阅

订阅专栏

一、对应源文件

add_executable(imageProjection src/imageProjection.cpp)

二、原理

1. 转换为深度图

分辨率：1800 x16

VLP-16 横向角度分辨率 $0.2^{\circ}$ ，所以列数： $360^{\circ} / 0.2^{\circ} = 1800$

纵向角度分辨率 $2^{\circ}$ ，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();

五、参考

LeGO-LOAM和LOAM的区别与联系 - 知乎

scofield_ly

关注

0
点赞
踩
4

收藏

觉得还不错? 一键收藏
0
评论
LeGO-LOAM 系列(2): imageProjection node 分析

一、对应源文件add_executable(imageProjection src/imageProjection.cpp)二、原理1. 分割2. 特征提取三、代码分析四、参考LeGO-LOAM和LOAM的区别与联系 - 知乎
复制链接

扫一扫

专栏目录