激光雷达障碍物识别

Lidar系列文章

传感器融合是将多个传感器采集的数据进行融合处理，以更好感知周围环境；这里首先介绍激光雷达的相关内容，包括激光雷达基本介绍（本节内容），激光点云数据处理方法（点云数据显示，点云分割，点云聚类，障碍物识别实例）等。

系列文章目录

1. 激光雷达基本介绍
2. 激光点云数据显示
3. 基于RANSAC的激光点云分割
4. 点云分割入门级实例学习
5. 激光点云目标物聚类
6. 基于PCL实现欧式聚类提取
7. 激光雷达障碍物识别

---

---

在前一章节，我们已经了解如何分割和聚类简单的仿真产生的点云。这里我们将介绍如何使用分割和聚类算法处理自动驾驶车辆的真实点云数据的整个过程。同时我们将介绍点云滤波处理，以及处理pcd文件流实现障碍物检测的方法。

障碍物检测的步骤

1. 读取PCD点云数据流；
2. 点云滤波；
3. 点云分割；
4. 障碍物点云聚类；
5. 确定聚类点云的边界框；
   我们要处理的点云如下图所示。
   

PCD数据流读取

通常用于加载单帧的pcd数据，函数实现可以如下loadPcd。

template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr ProcessPointClouds<PointT>::loadPcd(std::string file)
{
   
    typename pcl::PointCloud<PointT>::Ptr cloud (new pcl::PointCloud<PointT>);

    if (pcl::io::loadPCDFile<PointT> (file, *cloud) == -1) //* load the file
    {
   
        PCL_ERROR ("Couldn't read file \n");
    }
    std::cerr << "Loaded " << cloud->points.size () << " data points from "+file << std::endl;
    return cloud;
}

然后加载pcd数据即可。

  ProcessPointClouds<pcl::PointXYZI>* pointProcessorI = new ProcessPointClouds<pcl::PointXYZI>();
  pcl::PointCloud<pcl::PointXYZI>::Ptr inputCloud = pointProcessorI->loadPcd("../src/sensors/data/pcd/data_1/0000000000.pcd");
  renderPointCloud(viewer,inputCloud,"inputCloud");

由于本项目中提供的是事先录制好的pcd点云数据流，因此我们需要逐个加载录制的pcd文件，这里首先获取pcd文件名列表。

template<typename PointT>
std::vector<boost::filesystem::path> ProcessPointClouds<PointT>::streamPcd(std::string dataPath)
{
   
    std::vector<boost::filesystem::path> paths(boost::filesystem::directory_iterator{
   dataPath}, boost::filesystem::directory_iterator{
   });

    // sort files in accending order so playback is chronological
    sort(paths.begin(), paths.end());
    return paths;
}

然后逐个加载pcd数据，流水线方式处理列表中的pcd数据。

    //Create point cloud processor
    ProcessPointClouds<pcl::PointXYZI>* pointProcessorI = new ProcessPointClouds<pcl::PointXYZI>();
    std::vector<boost::filesystem::path> stream = pointProcessorI->streamPcd("../src/sensors/data/pcd/data_1");
    auto streamIterator = stream.begin();
    pcl::PointCloud<pcl::PointXYZI>::Ptr inputCloudI;
    
    while (!viewer->wasStopped ())
    {
   
        // Clear viewer
        viewer->removeAllPointClouds();
        viewer->removeAllShapes();

        // Load pcd and run obstacle detection process
        inputCloudI = pointProcessorI->loadPcd((*streamIterator).string());
        cityBlock(viewer, pointProcessorI, inputCloudI);

        streamIterator++;
        if(streamIterator == stream.end())
            streamIterator = stream.begin();
        
        viewer->spinOnce ();
    } 

这里我们首先创建了一个ProcessPointClouds对象pointProcessorI，其中ProcessPointClouds包含我们的点云处理函数模块（pcd数据加载，滤波，点云分割，聚类，pcd数据存储等），CityBlock函数实现对点云的障碍物检测。

PCL点云滤波

下一步我们需要利用PCL库的滤波函数实现点云滤波。我们使用的点云数据为通过64线激光雷达采集所得，一帧包含120000点。这里我们将进行降采样，降低点云的精度，以提高处理速度。同时，我们有ROI感兴趣区域，集中于包含障碍物的道路区域，比如x轴车辆行驶方向30m，y轴横向10m的矩形区域。
本小节主要工作如下：

1. Voxel Grid滤波器：进行点云降采样以加快点云处理速度；
   Voxel Grid滤波器生成一系列立方体网格对点云进行滤波，通过每个体素立方体单元只保留一个点云来实现降采样，因此立体单元越大，输出的点云分辨率就越低。这里我们将使用PCL库的voxel grid filtering功能。
2. 保留ROI感兴趣区域内的点云，删除其他点云。这里我们将使用PCL库的Region of Interest功能。
3. 去除车顶的反射点；

template<typename PointT>
typename pcl::PointCloud<PointT>::Ptr ProcessPointClouds<PointT>::FilterCloud(typename pcl::PointCloud<PointT>::Ptr cloud, float filterRes, Eigen::Vector4f minPoint, Eigen::