<基于欧几里德聚类的激光雷达点云分割及ROS实现> 笔记

本文实现参考Adam的博客    基于欧几里德聚类的激光雷达点云分割及ROS实现

点云聚类在激光雷达环境感知中的作用

就无人车的环境感知而言，方案很多，根据使用的传感器的不同，算法也截然不同，有单纯基于图像视觉的方法，也有基于激光雷达的方法，激光雷达以其稳定可靠、精度高并且能同时应用于定位和环境感知而被广泛采用。激光雷达环境感知一般的流程为：

     1、分割地面，从而减少地面的点对目标检测的影响
     2、点云聚类，将目标按照点的分布进行聚类，从而降低后续计算的计算量
     3、模式识别，对分割得到的点云进行特征提取，使用提取出来的特征训练分类器进行模式识别，近年来深度学习取得进展，也有不少使用深度神经网络的端到端检测算法。
     4、目标追踪，在完成模式识别以后我们实际上已经得到了目标障碍物的类别（是行人还是车辆还是别的）、障碍物的轮廓（一个3维的bounding box）、障碍物的位置（障碍物形心相对于激光雷达的xyz坐标）。为了方便规划模块完成对障碍物的预测，我们需要建立障碍物在前后帧（来自传感器的前后两次信号）的关系，即需要给障碍物一个ID，并且能够持续追踪这个障碍物，在目标追踪中，我们前面介绍的卡尔曼滤波、扩展卡尔曼滤波和无损卡尔曼滤波被广泛使用。

当然，根据使用的传感器的不同，在点云聚类完成以后，对障碍物进一步的模式识别通常有两种做法：

     ● 直接针对分割出来的点云进行模式识别
     ● 使用相机对目标进行模式识别（图像），将相机和雷达进行联合标定，将相机得到的检测目标投射到3维的点云空间，融合图像检测和点云聚类的结果实现目标检测和分类

目前来说，第一种方法依赖于密集的点云才能达到稳定可靠的效果，为了实现密集点云，通常使用高线激光雷达（如Velodyne的HDL-64），或者采用多雷达组合（单个32线雷达+多个16线雷达）来实现密集点云，这类方法要达到安全稳定的感知效果成本高昂。第二种方法依赖于图像检测的精度，由于深度神经网络的发展，基于图像的目标检测已经非常稳定可靠了，但是，多路图像的深度学习检测依赖于强大的芯片，满足车规级要求的深度学习芯片缺乏。

可见点云聚类是激光雷达环境感知中的重要步骤，实际上，在低速、简单场景下，仅使用聚类已经能够达到很好的障碍物感知效果了。

欧几里德聚类

KD Tree

在介绍欧几里德聚类之前我们首先理解欧几里德聚类中使用的基本的数据结构——KD Tree（k-维树）。k-维树是在一个欧几里德空间中组织点的基本数据结构，它本质上就是一个每个节点都未k维点的二叉树。在PCL中，由于点云的三维属性，所用到的K-维树即为3维树。在本文的代码中，我们实际上仅使用了一个2维树，我们将点云压缩成了2维——即将所有点的z值（高度）设为0，这么做的原因在于一方面我们并不关心点云簇在z方向的搜索顺序（两个物体在z方向叠在一起我们可以将其视为一个障碍物），另一方面我们希望能够加快我们的聚类速度以满足无人车感知实时性的需求。此外，一个2维树以更方便读者理解KD Tree。使用二维树对平面上的点进行划分如下图所示：

 

如上图所示，我们使用一个二叉树组织所有的点。点与点的距离表示其邻近距离，二叉树的所有非叶子节点可以视作用一个超平面把空间分割成两个半空间。节点左边的子树代表在超平面左边的点，节点右边的子树代表在超平面右边的点。选择超平面的方法如下：每个节点都与k维中垂直于超平面的那一维有关。因此，如果选择按照x轴划分，所有x值小于指定值的节点都会出现在左子树，所有x值大于指定值的节点都会出现在右子树。这样，超平面可以用该x值来确定，其法线为x轴的单位向量。

欧几里德聚类

对于欧几里德聚类的具体算法流程，PCL官方文档提供的如下伪代码：

 

之所以使用KD Tree数据结构来组织点，实际上就是为了加速在聚类过程中的搜索速度。在该算法中，最重要的参数即为dth，它表示聚类的时候的半径阈值。在这个半径内整个球体内的点将被聚类成一个点云簇。此外，在PCL库中，聚类方法还有两个重要参数——最大和最小聚类点数阈值，当聚类的点云簇的点数在这个两个阈值以内的情况下才会被返回。

基于欧几里德聚类的障碍物检测ROS实现

在上一篇博客中我们实现了一个简单的地面-非地面分割ROS节点，这个节点订阅来自 /velodyne_points 话题的点云数据，并且将分割完的点云分别发布到 /filtered_points_ground 和 /filtered_points_no_ground 两个话题上，下面我们编写一个欧几里德聚类的节点，订阅 /filtered_points_no_ground 话题，对路面以上的障碍物进行检测。

使用Voxel Grid对点云降采样
由于点云聚类的实时性要求，我们需要通过减少点云的密度来加速聚类。一种简单的方法就是使用我们前文提到的Voxel Grid Filter对点云进行降采样，代码如下：

void EuClusterCore::voxel_grid_filer(pcl::PointCloud<pcl::PointXYZ>::Ptr in, pcl::PointCloud<pcl::PointXYZ>::Ptr out, double leaf_size)
{
    pcl::VoxelGrid<pcl::PointXYZ> filter;
    filter.setInputCloud(in);
    filter.setLeafSize(leaf_size, leaf_size, leaf_size);
    filter.filter(*out);
}

需要注意的是，这里的Voxel Grid Filter的Leaf Size应该尽可能小，在实例中我们使用的Leaf Size为0.1m，过大的Leaf Size虽然会使得速度变快，但是聚类的结果相对会变得更差，尤其是对于反射较为微弱的物体（如远处的行人）。

按距离分割点云
如上文所提，欧几里德聚类最重要的参数是聚类半径阈值，为了达到更好的聚类效果，我们在不同距离的区域使用不同的聚类半径阈值，如下图所示：

 

所以，我们首先将扫描的点云按照其到雷达的聚类切分成五个点云：

void EuClusterCore::cluster_by_distance(pcl::PointCloud<pcl::PointXYZ>::Ptr in_pc, std::vector<Detected_Obj> &obj_list)
{
    //cluster the pointcloud according to the distance of the points using different thresholds (not only one for the entire pc)
    //in this way, the points farther in the pc will also be clustered

    //0 => 0-15m d=0.5
    //1 => 15-30 d=1
    //2 => 30-45 d=1.6
    //3 => 45-60 d=2.1
    //4 => >60   d=2.6

    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> segment_pc_array(5);

    for (size_t i = 0; i < segment_pc_array.size(); i++)
    {
        pcl::PointCloud<pcl::PointXYZ>::Ptr tmp(new pcl::PointCloud<pcl::PointXYZ>);
        segment_pc_array[i] = tmp;
    }

    for (size_t i = 0; i < in_pc->points.size(); i++)
    {
        pcl::PointXYZ current_point;
        current_point.x = in_pc->points[i].x;
        current_point.y = in_pc->points[i].y;
        current_point.z = in_pc->points[i].z;

        float origin_distance = sqrt(pow(current_point.x, 2) + pow(current_point.y, 2));

        // 如果点的距离大于120m, 忽略该点
        if (origin_distance >= 120)
        {
            continue;
        }

        if (origin_distance < seg_distance_[0])
        {
            segment_pc_array[0]->points.push_back(current_point);
        }
        else if (origin_distance < seg_distance_[1])
        {
            segment_pc_array[1]->points.push_back(current_point);
        }
        else if (origin_distance < seg_distance_[2])
        {
            segment_pc_array[2]->points.push_back(current_point);
        }
        else if (origin_distance < seg_distance_[3])
        {
            segment_pc_array[3]->points.push_back(current_point);
        }
        else
        {
            segment_pc_array[4]->points.push_back(current_point);
        }
    }

    std::vector<pcl::PointIndices> final_indices;
    std::vector<pcl::PointIndices> tmp_indices;

    for (size_t i = 0; i < segment_pc_array.size(); i++)
    {
        cluster_segment(segment_pc_array[i], cluster_distance_[i], obj_list);
    }
}

这里我们忽略了距离大于120m的点，原因在于一方面我们近期主要做的低速场景，对于远距离的环境感知并无要求，此外我们采用的Velodyne VLP-32C雷达线束仍不密集，在远处实际上反射已经非常微弱了，聚类效果不佳。

聚类并计算障碍物中心和Bounding Box
接下来我们正对这五个点云分别使用不同的半径阈值进行欧几里德聚类，对聚类完以后的一个个点云簇，我们计算其形心作为该障碍物的中心，同时计算点云簇的长宽高，从而确定一个能够将点云簇包裹的三维Bounding Box，代码如下：

void EuClusterCore::cluster_segment(pcl::PointCloud<pcl::PointXYZ>::Ptr in_pc,
                                    double in_max_cluster_distance, std::vector<Detected_Obj> &obj_list)
{

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

    //create 2d pc
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_2d(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::copyPointCloud(*in_pc, *cloud_2d);
    //make it flat
    for (size_t i = 0; i < cloud_2d->points.size(); i++)
    {
        cloud_2d->points[i].z = 0;
    }

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

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

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

    for (size_t i = 0; i < local_indices.size(); i++)
    {
        // the structure to save one detected object
        Detected_Obj obj_info;

        float min_x = std::numeric_limits<float>::max();
        float max_x = -std::numeric_limits<float>::max();
        float min_y = std::numeric_limits<float>::max();
        float max_y = -std::numeric_limits<float>::max();
        float min_z = std::numeric_limits<float>::max();
        float max_z = -std::numeric_limits<float>::max();

        for (auto pit = local_indices[i].indices.begin(); pit != local_indices[i].indices.end(); ++pit)
        {
            //fill new colored cluster point by point
            pcl::PointXYZ p;
            p.x = in_pc->points[*pit].x;
            p.y = in_pc->points[*pit].y;
            p.z = in_pc->points[*pit].z;

            obj_info.centroid_.x += p.x;
            obj_info.centroid_.y += p.y;
            obj_info.centroid_.z += p.z;

            if (p.x < min_x)
                min_x = p.x;
            if (p.y < min_y)
                min_y = p.y;
            if (p.z < min_z)
                min_z = p.z;
            if (p.x > max_x)
                max_x = p.x;
            if (p.y > max_y)
                max_y = p.y;
            if (p.z > max_z)
                max_z = p.z;
        }

        //min, max points
        obj_info.min_point_.x = min_x;
        obj_info.min_point_.y = min_y;
        obj_info.min_point_.z = min_z;

        obj_info.max_point_.x = max_x;
        obj_info.max_point_.y = max_y;
        obj_info.max_point_.z = max_z;

        //calculate centroid, average
        if (local_indices[i].indices.size() > 0)
        {
            obj_info.centroid_.x /= local_indices[i].indices.size();
            obj_info.centroid_.y /= local_indices[i].indices.size();
            obj_info.centroid_.z /= local_indices[i].indices.size();
        }

        //calculate bounding box
        double length_ = obj_info.max_point_.x - obj_info.min_point_.x;
        double width_ = obj_info.max_point_.y - obj_info.min_point_.y;
        double height_ = obj_info.max_point_.z - obj_info.min_point_.z;

        obj_info.bounding_box_.header = point_cloud_header_;

        obj_info.bounding_box_.pose.position.x = obj_info.min_point_.x + length_ / 2;
        obj_info.bounding_box_.pose.position.y = obj_info.min_point_.y + width_ / 2;
        obj_info.bounding_box_.pose.position.z = obj_info.min_point_.z + height_ / 2;

        obj_info.bounding_box_.dimensions.x = ((length_ < 0) ? -1 * length_ : length_);
        obj_info.bounding_box_.dimensions.y = ((width_ < 0) ? -1 * width_ : width_);
        obj_info.bounding_box_.dimensions.z = ((height_ < 0) ? -1 * height_ : height_);

        obj_list.push_back(obj_info);
    }
}

需要注意的是，我们放到 pcl::EuclideanClusterExtraction 是一个已经平面化的二维点云，这种做法能够带来速度的提升。这里我们定义了一个结构体 Detected_Obj ，用于存储检测到的障碍物的信息，内容如下：

  struct Detected_Obj
  {
    jsk_recognition_msgs::BoundingBox bounding_box_;

    pcl::PointXYZ min_point_;
    pcl::PointXYZ max_point_;
    pcl::PointXYZ centroid_;
  };

最后，我们将检测的障碍物的Bounding Box发布到 /detected_bounding_boxs 话题上：

    jsk_recognition_msgs::BoundingBoxArray bbox_array;

    for (size_t i = 0; i < global_obj_list.size(); i++)
    {
        bbox_array.boxes.push_back(global_obj_list[i].bounding_box_);
    }
    bbox_array.header = point_cloud_header_;

    pub_bounding_boxs_.publish(bbox_array);

完整代码见文末链接。

检测结果
我们仍然使用上一篇博客中的rosbag来完成实践，首先运行rosbag并且按空格暂停：

rosbag play test.bag

使用catkin_make编译我们这个ROS节点，使用roslaunch运行我们上一篇文章中写的节点和这个节点：

roslaunch pcl_test pcl_test.launch
roslaunch euclidean_cluster euclidean_cluster.launch

启动Rviz，继续play rosbag，在Rviz中添加如下display：

 

其中，第三个为 jsk_rvize_plugins 中的BoundingBoxArray，添加方式如下：

 

得到的Detect效果：

 

我们放大看聚类的效果：

笔记

如果编译过程中出现下面报错，是ROS没有安装全，缺少 jsk_recognition_msgs包

 Could not find a package configuration file provided by
  "jsk_recognition_msgs" with any of the following names:

    jsk_recognition_msgsConfig.cmake
    jsk_recognition_msgs-config.cmake

  Add the installation prefix of "jsk_recognition_msgs" to CMAKE_PREFIX_PATH
  or set "jsk_recognition_msgs_DIR" to a directory containing one of the
  above files.  If "jsk_recognition_msgs" provides a separate development
  package or SDK, be sure it has been installed.

解决方法：

sudo apt-get install ros-kinetic-jsk-recognition-msgs 
sudo apt-get install ros-kinetic-jsk-rviz-plugins

后记

本文的方法虽然能够实现点云的聚类，但是受非道路元素的影响颇大，一种方法是采用高精度地图彻底剔除不在可行驶区域上的点，这样不仅聚类的计算量更小，同时能够排除很多道旁障碍物（道旁的大树，电线杆）的干扰。

完整代码：https://gitee.com/itachi1121/lidar_euclidean_cluster.git