二十五. 智能驾驶之基于点云分割和聚类的障碍物检测

        在智能驾驶领域, 根据使用的传感器的不同，对障碍物的检测和识别通常有3种做法：

1.一种是基于相机图像和点云鸟瞰图的纯图像障碍物检测, 比如YOLO三维;

2.一种是将相机和雷达进行联合标定, 使用相机图像,利用基于深度学习的物体检测网络识别图像中物体，将相机得到的检测目标投射到3维的点云空间，融合图像检测和点云聚类实现目标检测和分类;

3.另一种是,直接利用激光雷达的点云,利用传统方法进行点云分割和聚类,也有利用基于点云的神经网络如PointNet++等, 识别出路面和障碍物;

       本例采用第3种方法中较传统的方法, 直接通过对点云的分割和聚类实现障碍物检测.

技术难点有两点:

1>. 地面和非地面点云分割;

        基本思路是: 将基于3维笛卡尔坐标系的所有点云点坐标(x,y,z), 映射为以雷达为中心,以地面平面X-Y为基面的(d,θ,z)坐标系.

        这里的d:  为点云中某点在X-Y平面的投影点,到雷达中心点的距离.  计算公式很简单:

       这里的θ:   为点云点 投影到X-Y平面的点,与X轴正方向轴的夹角, 其取值范围为: [0,360]. 假设都是基于角度的话, 其计算公式为:  

      这里的z: 为点云点在其原3维笛卡尔坐标系中的坐标z值. 

      下一步就是给这些无序的(d,θ,z)点分门别类了.  其思路如下:

        其原理非常类似于电脑磁盘的扇区和柱面.  一个硬盘有若干个盘面, 相当于(x,y,z),坐标系的不同的z坐标值; 每个盘面又以一定的角度区间划分为不同的扇区,  相当于不同θ扇区; 每个扇面上又划分不同的同心径向磁道(所有盘面叠在一起就是一个同心径向柱),相当于公式中的d值.

        一般基于雷达扫描一圈的角度分辨率,比如单线一圈2000个扫描点, 那么紧邻两点之间的角度变化就是: 360/2000 = 0.18度.  基于此,我们把X-Y平面以雷达为中心分成2000个扇区, 按照径向距离的不同,分出若干的同心环道(类似磁盘的磁道),比如64线激光雷达,我们可以分出64个同心环道. 这样所有的点云中的点, 就被我们有序地分布到基于不同θ的扇区, 不同径向距离d的径向环道, 而其z值 仍然是在原始点云中的高度值z.

        类似下图的效果:

        有了上述的(d,θ,z)坐标系有序点, 下一步只需要对每个径向扇区内由近到远的各个径向环道内点,进行基于斜率阈值的直线拟合即可.

2>. 点云聚类;

        这块主要是利用pcl库的,基于KD树的欧几里德聚类, 具体理论这里就不阐述了, 主要接口类:

pcl::search::KdTree<pcl::PointXYZ>和pcl::EuclideanClusterExtraction<pcl::PointXYZ>

一. 获取原始点云信息

二. 原始点云数据量较大,为降低后续计算量,对点云进行体素(Voxel)降采样或称稀疏化.

降采样前:

降采样后:

//下采样后点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());

//体素滤波
pcl::VoxelGrid<pcl::PointXYZ> voxelFilter;
voxelFilter.setInputCloud(originPointCloud);
voxelFilter.setLeafSize(0.1f,0.1f,0.1f);
voxelFilter.filter(*cloud_filtered);

三. 裁剪点云, 去除高于车身顶部的点云点, 同样为了降低后续计算量.

裁剪顶部以上点云前:

裁剪顶部以上点云后:

        这里基本是基于z坐标高度和x-y的进行距离过滤, 代码很简单.

四. 分离出地面和非地面点云

为了视觉直观, 切换个合适角度:

分离出的地面点云:

分离出的非地面点云,  用于后续障碍物检测,做点云分割和聚类:

五. 点云分割和聚类

换个合适角度,俯视全局点云:

从俯视角度,查看分割和聚类结果:

回到地面,换个角度,查看点云:

从地面角度,查看分割和聚类结果:

走近查看细节点云:

走仅查看细节分割和聚类结果:

    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);

    //create 2d point cloud
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2d(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud(*pointCloud, *cloud_2d);
    
    for (size_t i = 0; i < cloud_2d->points.size(); i++)
    {
        cloud_2d->points[i].z = 0; //仅在X-Y平面聚类,不需要考虑z坐标
    }

    if (cloud_2d->points.size() > 0)
        tree->setInputCloud(cloud_2d);

    std::vector<pcl::PointIndices> pointIndices;

    pcl::EuclideanClusterExtraction<pcl::PointXYZ> euclid;
    euclid.setInputCloud(cloud_2d);
    euclid.setClusterTolerance(in_max_cluster_distance);
    euclid.setMinClusterSize(MIN_CLUSTER_SIZE);
    euclid.setMaxClusterSize(MAX_CLUSTER_SIZE);
    euclid.setSearchMethod(tree);
    euclid.extract(pointIndices);