13、基于ROS的欧几里德聚类的点云聚类的实现

基于ROS的欧几里德聚类的点云聚类的实现

激光雷达环境感知一般的流程为：

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

一、 欧几里德聚类

KD Tree
简单来说，就是在K维度上进行K次二分查找。
在本文的代码中使用了一个2维树，将点云压缩成了2维——即将所有点的z值（高度）设为0，这么做的原因在于一方面我们并不关心点云簇在z方向的搜索顺序（两个物体在z方向叠在一起我们可以将其视为一个障碍物），另一方面我们希望能够加快我们的聚类速度以满足无人车感知实时性的需求。此外，一个2维树以更方便读者理解KD Tree。使用二维树对平面上的点进行划分如下图所示（有在点上划分的，有在中点划分的，原理是一致的）：

关于欧式距离聚类具体算法流程，PCL官方文档提供了如下伪代码：

二、基于欧几里德聚类的障碍物检测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);
}
//leaf_size为栅格边长

按距离分割点云
所以，我们首先将扫描的点云按照其到雷达的聚类切分成五个点云，以下是分类以及聚类的距离大小：

0-15m d=0.5
15-30 d=1
30-45 d=1.6
45-60 d=2.1
>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);
        }
    }

聚类并计算障碍物中心和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
        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 ，使用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);

三、运行以及效果：

1、在pcl_test下进行编译和更新：

catkin_make
source devel/setup.sh

2、启动这个节点：

roslaunch pcl_test  pcl_test.launch
roslaunch euclidean_cluster euclidean_cluster.launch

3、启动rviz(要在launch之后运行):

rviz

4、运行包：

rosbag play 11-12-boliu2.bag
//需要↑这个包可私信
/*
或者：from AdamShan:
rosbag play --clock test.bag
测试bag下载链接：https://pan.baidu.com/s/1HOhs9StXUmZ_5sCALgKG3w
*/

rviz设置：

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

在这里插入图片描述

实验结果如下：
原始点云地图：

检测结果：


对移动的行人（红框内）也能检测到，合理分类。本文的方法虽然能够实现点云的聚类，但是受非道路元素的影响颇大。