A-LOAM 源码阅读一 scanRegistration

scanRegistration 的内容是 corner 与 surface 特征点的提取。

一、main()函数

订阅:velodyne_points,激光雷达点云数据。

发布:velodyne_cloud_2,由 velodyne_points 经过处理后的有序点云;

           laser_cloud_sharp、laser_cloud_less_sharp、laser_cloud_flat、laser_cloud_less_flat,特征点云。

二、laserCloudHandler()函数

laserCloudHandler 是对 velodyne_points 点云数据的处理。

1. 去除无效点云

// 首先对点云滤波,去除 NaN 值的无效点云,以及在 Lidar 坐标系原点 MINIMUM_RANGE 距离以内的点
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);

2. 根据仰角确定 scanID

根据多线激光雷达 scan 的角度分布情况,结合仰角,即可确定所有点是由哪个 scan 扫描得到的。

float angle = atan(point.z / sqrt(point.x * point.x + point.y * point.y)) * 180 / M_PI;
int scanID = 0;
// 确定每一个点对应的 scanID,即确定该点是由哪个扫描线扫描得到的
if (N_SCANS == 16)
{
    scanID = int((angle + 15) / 2 + 0.5);
    if (scanID > (N_SCANS - 1) || scanID < 0)
    {
        count--;
        continue;
    }
}
else if (N_SCANS == 32)
{
    scanID = int((angle + 92.0/3.0) * 3.0 / 4.0);
    if (scanID > (N_SCANS - 1) || scanID < 0)
    {
        count--;
        continue;
    }
}
else if (N_SCANS == 64)
{   
    if (angle >= -8.83)
        scanID = int((2 - angle) * 3.0 + 0.5);
    else
        scanID = N_SCANS / 2 + int((-8.83 - angle) * 2.0 + 0.5);

    // use [0 50]  > 50 remove outlies 
    if (angle > 2 || angle < -24.33 || scanID > 50 || scanID < 0)
    {
        count--;
        continue;
    }
}
else
{
    printf("wrong scan number\n");
    ROS_BREAK();
}

3. 根据水平方位角确定 fireID

计算各个点相对于起始点的时间差,便于后续消除点云数据的运动畸变。

1) 计算起始点与终止点的方位角

// 点云的方位角,起始角度和终止角度
// 扫描起始的方位角,atan2范围 [-pi,pi],计算方位角时取负号是因为 velodyne 顺时针旋转
float startOri = -atan2(laserCloudIn.points[0].y, laserCloudIn.points[0].x);
// 扫描结束点的方位角,加 2*pi 使点云旋转周期为 2*pi
float endOri = -atan2(laserCloudIn.points[cloudSize - 1].y, laserCloudIn.points[cloudSize - 1].x) + 2 * M_PI;

// 终止方位角与起始方位角之间的角度差在 (pi, 3*pi), 允许雷达不是一个圆周扫描
if (endOri - startOri > 3 * M_PI)
    endOri -= 2 * M_PI;
else if (endOri - startOri < M_PI)
    endOri += 2 * M_PI;

2) 计算各个点相对于起始点的时间差

// 确定每一个点的水平方位角
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);
// 点强度 = scanID + 点相对时间
point.intensity = scanID + scanPeriod * relTime;    // intensity 字段的整数部分存放 scanID,小数部分存放归一化后的 fireID
laserCloudScans[scanID].push_back(point);           // 将点根据 scanID 放到对应的子点云 laserCloudScans 中

intensity 用于存放 scanID 和 fireID。

4. 有序点云

// 将子点云 laserCloudScans 合并成一帧点云 laserCloud
// 这里的单帧点云 laserCloud 可以认为已经是有序点云了,按照 scanID 和 fireID 的顺序存放
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++)
{   // 确定每一个 scanline 对应点云的起始和结束索引
    // 记录每个scan的开始index,忽略前5个点
    scanStartInd[i] = laserCloud->size() + 5;
    *laserCloud += laserCloudScans[i];
    // 记录每个 scan 的结束 index,忽略后5个点,开始和结束处的点云 scan 容易产生不闭合的“接缝”,对提取 edge feature 不利
    scanEndInd[i] = laserCloud->size() - 6;
}

5. 特征点提取

1) 计算点的曲率

遍历每个点(除了前五个点和后五个点)差值的平方代表曲率。

// 使用每个点的前后五个点计算曲率,因此前五个点和后五个点跳过
for (int i = 5; i < cloudSize - 5; i++)
{ 
    // 该点与周围10个点的偏差
    // diff = sum(p(i-5:i-1)) + sum(p(i+1:i+5)) - 10*p(i)
    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; // Label 2: corner_sharp
                       // Label 1: corner_less_sharp, 包含Label 2
                       // Label -1: surf_flat
                       // Label 0: surf_less_flat, 包含Label -1,因为点太多,最后会降采样
}

2) 根据曲率计算4种特征点

TicToc t_pts;

pcl::PointCloud<PointType> cornerPointsSharp;
pcl::PointCloud<PointType> cornerPointsLessSharp;
pcl::PointCloud<PointType> surfPointsFlat;
pcl::PointCloud<PointType> surfPointsLessFlat;

float t_q_sort = 0;
for (int i = 0; i < N_SCANS; i++) // 按照 scan 的顺序提取4种特征点
{
    if( scanEndInd[i] - scanStartInd[i] < 6) // 如果该 scan 的点数少于7个点,就跳过
        continue;

    pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
    for (int j = 0; j < 6; j++)  // 将该 scan 分成6小段(subscan)执行特征检测
    {
        // subscan 的起始 index
        int sp = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * j / 6;
        // subscan 的结束 index
        int ep = scanStartInd[i] + (scanEndInd[i] - scanStartInd[i]) * (j + 1) / 6 - 1; 

        TicToc t_tmp;
        // 根据曲率由小到大对 subscan 的点进行 sort
        std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);
        t_q_sort += t_tmp.toc();

        int largestPickedNum = 0;
        // 从后往前,即从曲率大的点开始提取 corner feature
        for (int k = ep; k >= sp; k--)
        {
            int ind = cloudSortInd[k];          // ind != k ?

            if (cloudNeighborPicked[ind] == 0 && cloudCurvature[ind] > 0.1)
            { // 如果该点没有被选择过,并且曲率大于 0.1
                largestPickedNum++;
                if (largestPickedNum <= 2)
                { // 该 subscan 中曲率最大的前2个点认为是 corner_sharp 特征点  
                    cloudLabel[ind] = 2;
                    cornerPointsSharp.push_back(laserCloud->points[ind]);
                    cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                }
                else if (largestPickedNum <= 20)
                { // 该 subscan 中曲率最大的前20个点认为是 corner_less_sharp 特征点
                    cloudLabel[ind] = 1; 
                    cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                }
                else
                    break;

                cloudNeighborPicked[ind] = 1;   // 标记该点被选择过了

                // 与当前点距离的平方 <= 0.05的点标记为选择过,避免特征点密集分布
                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;
                }
            }
        }

        // 提取 surf 平面 feature,与上述类似,选取该 subscan 曲率最小的前4个点为 surf_flat
        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;
                surfPointsFlat.push_back(laserCloud->points[ind]);

                smallestPickedNum++;
                if (smallestPickedNum >= 4)
                    break;

                cloudNeighborPicked[ind] = 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;
                }
                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;
                }
            }
        }

        // 其他的非 corner 特征点与 surf_flat 特征点一起组成 surf_less_flat 特征点
        for (int k = sp; k <= ep; k++)
        {
            if (cloudLabel[k] <= 0)
                surfPointsLessFlatScan->push_back(laserCloud->points[k]);
        }
    }

    // 最后对该 scan 点云中提取的所有 surf_less_flat 特征点进行降采样,因为点太多了 体素滤波下采样
    pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
    pcl::VoxelGrid<PointType> downSizeFilter;
    downSizeFilter.setInputCloud(surfPointsLessFlatScan);
    downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
    downSizeFilter.filter(surfPointsLessFlatScanDS);

    surfPointsLessFlat += surfPointsLessFlatScanDS;
       
/* sharp corner 和 flat surface 特征作为激光里程计中 source 点云,less sharp corner 和 less flat surface 特征作为激光里程计中的 target 点云 */
/* 添加less sharp、less flat ? :
 * 基于曲率的方法并不能准确、稳定地检出 corner 和 surface 特征,尤其是 corner 特征,
 * 将数量更多的 less sharp corner 和 less flat surface 特征作为激光里程计中的 target 点云,有利于提高有效关联的数量,以提高激光里程计的精度
 */
}

3) publish特征点云

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);

// pub each scam
if(PUB_EACH_LINE)
{
    for(int i = 0; i< N_SCANS; i++)
    {
        sensor_msgs::PointCloud2 scanMsg;
        pcl::toROSMsg(laserCloudScans[i], scanMsg);
        scanMsg.header.stamp = laserCloudMsg->header.stamp;
        scanMsg.header.frame_id = "/camera_init";
        pubEachScan[i].publish(scanMsg);
    }
}

 

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值