LOAM 系列(2): ascanRegistration node 分析

scofield_ly

已于 2022-02-20 15:13:23 修改

阅读量816

点赞数 1

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

于 2021-10-27 23:12:54 首次发布

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

版权

lidar_slam 专栏收录该内容

8 篇文章 8 订阅

订阅专栏

一、对应源文件

add_executable(ascanRegistration src/scanRegistration.cpp)

src/scanRegistration.cpp

二、原理

1. 特征提取

对于单个线束扫描的点云

计算平面光滑度

平面点：平滑度低于某个阈值

边缘点：平滑度高于某个阈值

2. 其他限制

2.1 选取点数量限制

每个线束的扫描分四段，每段最多取两个边缘点和四个平面点

2.2 非极大抑制

被选取的点的周围点避免被选取

2.3 去除不可靠的特征点

平面点：避免与雷达扫描束过于平行，例如下图

边缘点：避免位于遮挡区域边缘，例如下图

3. 可视化效果

三、代码分析

1. 点云预处理

1.1 格式转换

pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);

1.2 去除 nan 点云

std::vector<int> indices;
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);

1.3 去除近处点云

// 去除距离小于 MINIMUM_RANGE 的点云
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);

2. 点云线数分类

2.1 按线数分组保存点云，且更新 intensity

int cloudSize = laserCloudIn.points.size();
// 点云的起始、终止旋转角。保证二者之间差值 pi-3pi
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, 
                      laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;
if (endOri - startOri > 3 * M_PI) {
    endOri -= 2 * M_PI;
} else if (endOri - startOri < M_PI) {
    endOri += 2 * M_PI;
}

//雷达扫描线是否过半
bool halfPassed = false;
int count = cloudSize;
PointType point;
std::vector<pcl::PointCloud<PointType>> laserCloudScans(N_SCANS);
for (int i = 0; i < cloudSize; i++) {
    point.x = laserCloudIn.points[i].x;
    point.y = laserCloudIn.points[i].y;
    point.z = laserCloudIn.points[i].z;
    // 计算仰角，获取垂直方向的序号即线号
    float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
    int scanID = 0;
    if (N_SCANS == 16) {   // 16 线数
        scanID = int((angle + 15) / 2 + 0.5);
    } else if (N_SCANS == 32)  {  // 32 线数
        ...
    } else if (N_SCANS == 64) {  // 64 线数   
        ...
    } else {
        printf("wrong scan number\n");
        ROS_BREAK();
    }
    //printf("angle %f scanID %d \n", angle, scanID);


    // 当前点云旋转角
    float ori = -atan2(point.y, point.x);
    if (!halfPassed) { 
        if (ori < startOri - M_PI / 2) {
            ori += 2 * M_PI;
        } else if (ori > startOri + M_PI * 3 / 2) {
            ori -= 2 * M_PI;
        }
        if (ori - startOri > M_PI) {
            halfPassed = true;
        }
    } else { 
        ori += 2 * M_PI;
        if (ori < endOri - M_PI * 3 / 2)  {
            ori += 2 * M_PI;
        } else if (ori > endOri + M_PI / 2) {
            ori -= 2 * M_PI;
        }
    }

    // 点云的相对时间，运动补偿的时候会用到
    float relTime = (ori - startOri) / (endOri - startOri);
    // intensity: 小数点前面是 scanID, 小数点后面是相对时间*0.1
    // double scanPeriod = 0.1
    point.intensity = scanID + scanPeriod * relTime;
    // 压入对应的线数点云数组
    laserCloudScans[scanID].push_back(point); 
}

2.2 更新各个线组点云索引

std::vector<int> scanStartInd(N_SCANS, 0);
std::vector<int> scanEndInd(N_SCANS, 0);
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++) { 
    scanStartInd[i] = laserCloud->size() + 5;  // 忽略该组的前5个点云
    *laserCloud += laserCloudScans[i];  // 所有点云按照线束顺序先后放入 laserCloud
    scanEndInd[i] = laserCloud->size() - 6;  // 忽略该组的后5个点云
}

3. 提取特征

3.1 曲率计算

for (int i = 5; i < cloudSize - 5; i++) { 
    // 用前后10个点云计算曲率
    float diffX = laserCloud->points[i - 5].x + laserCloud->points[i - 4].x + laserCloud->points[i - 3].x + laserCloud->points[i - 2].x + laserCloud->points[i - 1].x - 10 * laserCloud->points[i].x + laserCloud->points[i + 1].x + laserCloud->points[i + 2].x + laserCloud->points[i + 3].x + laserCloud->points[i + 4].x + laserCloud->points[i + 5].x;
    float diffY = laserCloud->points[i - 5].y + laserCloud->points[i - 4].y + laserCloud->points[i - 3].y + laserCloud->points[i - 2].y + laserCloud->points[i - 1].y - 10 * laserCloud->points[i].y + laserCloud->points[i + 1].y + laserCloud->points[i + 2].y + laserCloud->points[i + 3].y + laserCloud->points[i + 4].y + laserCloud->points[i + 5].y;
    float diffZ = laserCloud->points[i - 5].z + laserCloud->points[i - 4].z + laserCloud->points[i - 3].z + laserCloud->points[i - 2].z + laserCloud->points[i - 1].z - 10 * laserCloud->points[i].z + laserCloud->points[i + 1].z + laserCloud->points[i + 2].z + laserCloud->points[i + 3].z + laserCloud->points[i + 4].z + laserCloud->points[i + 5].z;

    cloudCurvature[i] = diffX * diffX + diffY * diffY + diffZ * diffZ;
    cloudSortInd[i] = i;
    cloudNeighborPicked[i] = 0;
    cloudLabel[i] = 0;
}

3.2 提取特征

每个线束单独处理

每个线束分为 6 组

每组 sharp 点不超过 2 个，flat 点不超过 4 个

pcl::PointCloud<PointType> cornerPointsSharp;   // 曲率最大的点云
pcl::PointCloud<PointType> cornerPointsLessSharp;   // 曲率稍微大的点云
pcl::PointCloud<PointType> surfPointsFlat;   // 曲率最小的点云
pcl::PointCloud<PointType> surfPointsLessFlat;   // 曲率稍微小的点云

for (int i = 0; i < N_SCANS; i++) {   // 每个线束
    if( scanEndInd[i] - scanStartInd[i] < 6)
        continue;
    
    // raw lessflat pointcloud
    pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
    // 该线束点云分为 6 组
    for (int j = 0; j < 6; j++)  {
        int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6; 
        int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1;

        // 对索引排序，使得曲率从小到大
        std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);

        // 筛选曲率大的点 corner pts
        int largestPickedNum = 0;
        for (int k = ep; k >= sp; k--) {
            int ind = cloudSortInd[k]; 
            // 临近点没有被选取且曲率大于阈值
            if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > 0.1) {
                largestPickedNum++;
                if (largestPickedNum <= 2) { // 每组 sharp 点不超过2个                        
                    cloudLabel[ind] = 2;  
                    cornerPointsSharp.push_back(laserCloud->points[ind]);
                    cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                } else if (largestPickedNum <= 20) { // 每组 less sharp 加 sharp 点 不超过 20个                       
                    cloudLabel[ind] = 1;   // less sharp
                    cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                } else {
                    break;
                }

                cloudNeighborPicked[ind] = 1; 
                // 非极大抑制，避免 sharp 点过于集中
                for (int l = 1; l <= 5; l++) {
                    float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                    float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                    float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                    if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
                        break;
                    }
                    cloudNeighborPicked[ind + l] = 1;
                }
                for (int l = -1; l >= -5; l--) {
                    float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                    float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                    float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                    if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
                        break;
                    }
                    cloudNeighborPicked[ind + l] = 1;
                }
            }
        }

        // 筛选曲率小的点 flat pts
        int smallestPickedNum = 0;
        for (int k = sp; k <= ep; k++)  {
            int ind = cloudSortInd[k];
            // 临近点没有被选取且曲率小于阈值
            if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] < 0.1) {
                cloudLabel[ind] = -1; // flat
                surfPointsFlat.push_back(laserCloud->points[ind]);

                smallestPickedNum++;
                if (smallestPickedNum >= 4) {  // 每组 flat 点不超过4个 
                    break;
                }

                cloudNeighborPicked[ind] = 1;
                // 非极大抑制，避免 flat点过于集中
                for (int l = 1; l <= 5; l++) { 
                    float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l - 1].x;
                    float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l - 1].y;
                    float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l - 1].z;
                    if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
                        break;
                    }
                    cloudNeighborPicked[ind + l] = 1;
                }
                for (int l = -1; l >= -5; l--) {
                    float diffX = laserCloud->points[ind + l].x - laserCloud->points[ind + l + 1].x;
                    float diffY = laserCloud->points[ind + l].y - laserCloud->points[ind + l + 1].y;
                    float diffZ = laserCloud->points[ind + l].z - laserCloud->points[ind + l + 1].z;
                    if (diffX * diffX + diffY * diffY + diffZ * diffZ > 0.05) {
                        break;
                    }
                    cloudNeighborPicked[ind + l] = 1;
                }
            }
        }

        // 其余不属于 sharp, less-sharp, flat 的点云为  less-flat pointcloud
        for (int k = sp; k <= ep; k++) {
            if (cloudLabel[k] <= 0) {
                surfPointsLessFlatScan->push_back(laserCloud->points[k]);
            }
        }
    }

    // less-flat pts sample (体素栅格滤波)
    pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
    pcl::VoxelGrid<PointType> downSizeFilter;
    downSizeFilter.setInputCloud(surfPointsLessFlatScan);
    downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
    downSizeFilter.filter(surfPointsLessFlatScanDS);
    // 压入滤波后点云
    surfPointsLessFlat += surfPointsLessFlatScanDS;
}

4. publish topic

sensor_msgs::PointCloud2 laserCloudOutMsg;
pcl::toROSMsg(*laserCloud, laserCloudOutMsg);
laserCloudOutMsg.header.stamp = laserCloudMsg->header.stamp;
laserCloudOutMsg.header.frame_id = "/camera_init";
pubLaserCloud.publish(laserCloudOutMsg);

sensor_msgs::PointCloud2 cornerPointsSharpMsg;
pcl::toROSMsg(cornerPointsSharp, cornerPointsSharpMsg);
cornerPointsSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsSharpMsg.header.frame_id = "/camera_init";
pubCornerPointsSharp.publish(cornerPointsSharpMsg);

sensor_msgs::PointCloud2 cornerPointsLessSharpMsg;
pcl::toROSMsg(cornerPointsLessSharp, cornerPointsLessSharpMsg);
cornerPointsLessSharpMsg.header.stamp = laserCloudMsg->header.stamp;
cornerPointsLessSharpMsg.header.frame_id = "/camera_init";
pubCornerPointsLessSharp.publish(cornerPointsLessSharpMsg);

sensor_msgs::PointCloud2 surfPointsFlat2;
pcl::toROSMsg(surfPointsFlat, surfPointsFlat2);
surfPointsFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsFlat2.header.frame_id = "/camera_init";
pubSurfPointsFlat.publish(surfPointsFlat2);

sensor_msgs::PointCloud2 surfPointsLessFlat2;
pcl::toROSMsg(surfPointsLessFlat, surfPointsLessFlat2);
surfPointsLessFlat2.header.stamp = laserCloudMsg->header.stamp;
surfPointsLessFlat2.header.frame_id = "/camera_init";
pubSurfPointsLessFlat.publish(surfPointsLessFlat2);

四、参考

1. A-loam学习笔记——scanRegistration过程代码详解_英雄小摔哥的博客-CSDN博客

scofield_ly

关注

1
点赞
踩
2

收藏

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

一、对应源文件add_executable(ascanRegistration src/scanRegistration.cpp)src/scanRegistration.cpp二、代码分析1.
复制链接

扫一扫