A-LOAM(前端-1)的特征提取及均匀化-算法流程+代码注释

laserCloudHandler流程

该函数是提取特征点的处理函数，接收到雷达硬件发送过来的消息的时候就会运行这个函数，来对点云进行预处理
这个函数是个回调函数，在订阅到点云数据的时候才会运行，ALOAM中的点云接收频率为10HZ

1. 先判断初始化，等待几帧来到后才认定为初始化成功

2. 进行线束id计算与每个点的时间戳，并将点云存到其对应的第scanID个数组中

3. 标记每个线束的最左5与最右5不参与曲率计算

4. 开始曲率计算，notic：这里实际处理是将全部点云都参与曲率计算了，计算曲率的公式可以这样认为 ，这里是仅对x处理时，对y，z操作同理 $$diffX=x1+x2+x3+x4+x5-10\ast x6+x7+x8+x9+x10+x11$$
   曲率$$\sigma =diff{X}^2+diff{Y}^2+diff{Z}^2$$

5. 开始特征提取，用到第4点的曲率信息

   • 提取曲率最大的2个点
   • 提取曲率一般大的20-2个点
   • 边缘点均匀化操作
   1. 将选中点的周围5个点都置1，判断条件为，查看相邻点的距离是否差异过大，过大则说明不连续是边缘点会break跳出不置为，差异小则说明是临近点有冗余，则置为1（往左操作5个点云，往右操作5个点云，共两次for循环）
   • 提取平坦点，只要曲率小于0.1都认为是平坦点，没有最平坦和一般平坦
   • 和上面一样均匀化操作
   • 对平坦点进行体素滤波获得体素滤波后的平坦点

6. 将原始点云、曲率最大的点、曲率一般大的点、全部平坦点、体素滤波后的平坦点发布出去给odom和mapping进行计算

7. 判断点云预处理时间是否超过100ms，因为KITTY数据集是10HZ，数据处理超过100ms可能会出现丢帧的情况进而影响odom和mapping的效果

个人学习总结，有错误的地方欢迎指出一起讨论

曲率计算代码

pcl::PointCloud<PointType>::Ptr laserCloud(new pcl::PointCloud<PointType>());
    for (int i = 0; i < N_SCANS; i++)
    {
    //每个scan的前后5个点都不参与曲率计算
        scanStartInd[i] = laserCloud->size() + 5;  //最左5不加入曲率计算，scanStartInd来区分第几个线束的点，判断是否加入曲率计算
        *laserCloud += laserCloudScans[i];
        scanEndInd[i] = laserCloud->size() - 6; //最右5不加入曲率计算
    }

    printf("prepare time %f \n", t_prepare.toc());
	//开始计算曲率，这里将全部点就计算曲率了