<基於歐幾里德聚類的激光雷達點雲分割及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