A-LOAM 数据处理
Kong Liangqian Lv6
 2022-01-10 18:38:29  

文件结构

该项目主要有四个launch文件，

  • kitti_helper.launch 可以把kitti数据集上的数据转换为bag包
  • 其他launch 运行三个可执行文件

可执行文件

该项目有三个可执行文件，启动每一个launch的时候都会调用，分别为ascanRegistration，alaserOdometry和alaserMapping

alaserOdometry为高频低精的节点，alaserMapping为低频高精的节点。ascanRegistration是前端配准的节点。

ascanRegistration

这个主要对线特征和面特征进行提取

参数：

  • minimum_range: 表示什么范围内的点不用
  • scan_line: 多少线的雷达

雷达数据预处理

转换为pcl格式的数据

1
2
3
pcl::PointCloud<pcl::PointXYZ> laserCloudIn;
 // convert to pcl datatype
 pcl::fromROSMsg(*laserCloudMsg, laserCloudIn);

移除Nan、过滤近点

那些发射到天上去的，以及和发射到的墙面反射不回来的点都移除掉，距离雷达太近的点(三维上d的) 移除掉，这样车身就看不见了

1
2
3
4
pcl::removeNaNFromPointCloud(laserCloudIn, laserCloudIn, indices);
// remove the point whose distance shorter than MINIMUM_RANGE
// height = 1 unordered pointcloud
removeClosedPointCloud(laserCloudIn, laserCloudIn, MINIMUM_RANGE);

定义旋转初始和结束角度

逆时针旋转作为正方向，所以添加负号

1
2
3
4
5
6
int cloudSize = laserCloudIn.points.size();
// The laser rotates clockwise, adding - to artificially become counterclockwise
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;

end_ori 和 start_ori在最理想的状态下，是一样的，从哪儿开始从哪儿结束，

有一种情况，譬如一圈是从-179到179度，那么其实数据就不需要角度补偿

1
2
3
4
if (endOri - startOri > 3 * M_PI)
{
    endOri -= 2 * M_PI;
}

另一种情况是，179度顺时针又一圈到-179度，这样的话加了2pi还不够，需要再加

1
2
3
4
else if (endOri - startOri < M_PI)
{
    endOri += 2 * M_PI;
}

计算scanID, 第几根线

循环遍历每一个点，通过俯仰角来计算点属于第几根线，计算水平角计算相对位置，把scanID 和 相对位置 以小数和整数的形式保存在point.intensity中

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
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;
    // compute the number of scans
    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();
    }
    //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);
    point.intensity = scanID + scanPeriod * relTime;
    laserCloudScans[scanID].push_back(point); 
}

把所有的点全部存入pcl::PointCloud\::Ptr laserCloud 中，把每一个scan的雷达数据，一股脑儿全部塞入laserCloud中，并且记录下每一个scan数据的起始和结束的位置，注意这里为了计算曲率，舍弃计算每一段scan的前面的和后面的点

1
2
3
4
5
6
7
pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
for (int i = 0; i < N_SCANS; i++)
{ 
    scanStartInd[i] = laserCloud->size() + 5;
    *laserCloud += laserCloudScans[i];
    scanEndInd[i] = laserCloud->size() - 6;
}

特征提取以及均匀化

计算曲率

遍历所有的点，即laserCloud，以及所有这些点的曲率，以及这些点原来的位置，索引

角点：曲率比较大

面点：曲率比较小

1
2
3
4
5
6
7
8
9
10
11
12

for (int i = 5; i < cloudSize - 5; 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;
}

角点计算cornerPointsSharp

在此之前，我们提前定义一些变量

1
2
3
4
5
6
7
ros::Publisher pubLaserCloud;   //整体点云
ros::Publisher pubCornerPointsSharp;    //角点，曲率很大的点
ros::Publisher pubCornerPointsLessSharp;    //曲率稍微大一点的点
ros::Publisher pubSurfPointsFlat;   //曲率最小的面点，平坦的点
ros::Publisher pubSurfPointsLessFlat;   //降采样面点
ros::Publisher pubRemovePoints;     //剔除点
std::vector<ros::Publisher> pubEachScan;    //ros形式的一线扫描

对每一个scan进行六等分，在每一等分里面对点根据曲率的大小进行排序。

1
2
3
4
5
6
7
8
9
10
11
12
for (int i = 0; i < N_SCANS; i++)
    {
        if( scanEndInd[i] - scanStartInd[i] < 6)
            continue;
        pcl::PointCloud<PointType>::Ptr surfPointsLessFlatScan(new pcl::PointCloud<PointType>);
        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;

            TicToc t_tmp;
            std::sort (cloudSortInd + sp, cloudSortInd + ep + 1, comp);

对当前区间的第一个点，加入到角点cornerPointsSharp的vector中，并且把该点周围的十个数看情况标记cloudNeighborPicked[ind + l]] = 1; 表示该点周围的五个数范围内是有 角点了，来避免角点太集中

当该点周围的一些点差异过大，即不太连续，是特征边缘，可以不需要设置，

角点标记为2；曲率稍微大的，小于角点的点标记为1

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
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)
                    {                        
                        cloudLabel[ind] = 2;
                        cornerPointsSharp.push_back(laserCloud->points[ind]);
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    else if (largestPickedNum <= 20)
                    {                        
                        cloudLabel[ind] = 1; 
                        cornerPointsLessSharp.push_back(laserCloud->points[ind]);
                    }
                    else
                    {
                        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;
                    }
                }
            }

平坦点计算surfPointsFlat

平坦点的曲率是确保可以小于0.1的，平坦的点，标志为-1

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
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;
                 }
             }
         }

面点计算surfPointsLessFlatScan

只要不是角点的所有点，全部都推送到面点里面去

1
2
3
4
5
6
7
for (int k = sp; k <= ep; k++)
{
    if (cloudLabel[k] <= 0)
    {
        surfPointsLessFlatScan->push_back(laserCloud->points[k]);
    }
}

体素滤波surfPointsLessFlat

把所有的面点surfPointsLessFlatScan进行一个体素滤波，在0.2x0.2x0.2的体积方格内，只能有一个体素点，全部保存到surfPointsLessFlat中。

1
2
3
4
5
6
7
pcl::PointCloud<PointType> surfPointsLessFlatScanDS;
pcl::VoxelGrid<PointType> downSizeFilter;
downSizeFilter.setInputCloud(surfPointsLessFlatScan);
downSizeFilter.setLeafSize(0.2, 0.2, 0.2);
downSizeFilter.filter(surfPointsLessFlatScanDS);

surfPointsLessFlat += surfPointsLessFlatScanDS;

外点剔除

相邻两个点，距离如果偏大的话，说明扫描到的墙面和雷达发出的角度是接近平行的，这个点就不要了，

相邻两个点，距离偏大的，遮挡点，如果左边的大就设置该点左边的五个点全部为遮挡点，反之就设置右边的五个点全部为遮挡点

发送数据

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
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(f);

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

topic数据含义

可以得到所有的topic含义即为

  • /velodyne_cloud_2： 原始点云
  • /laser_cloud_sharp：曲率最大的角点的点云
  • /laser_cloud_less_sharp：曲率次大的角点的点云，数量会大于/laser_cloud_sharp
  • /laser_cloud_flat：曲率最小的点的点云
  • /laser_cloud_less_flat：除了角点之外的，经过体素滤波之后的面点
  • /laser_remove_points：被移除的点

数据处理时间

1
2
if(t_whole.toc() > 100)
    ROS_WARN("scan registration process over 100ms");

雷达的频率在10hz左右，所以当ROS发出警告太多的时候，就需要注意是不是有资源不够

 Comments