【点云处理】pcl中使用欧式聚类

        今天尝试跑一下欧式聚类，看看在路侧场景下的聚类效果。

【代码】

#include <stdlib.h>
#include <boost/bind.hpp>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <functional>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/ModelCoefficients.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/console/time.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/pcl_visualizer.h>

void pc_callback(sensor_msgs::PointCloud2::ConstPtr msg_pc_ptr,
                 const ros::Publisher& pc_pub,
                 const float& cluster_tolerance,
                 const float& min_cluster_size) {
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZRGB>);
    pcl::fromROSMsg(*msg_pc_ptr,*cloud);

    pcl::console::TicToc tt;
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZRGB>);
    {
        tt.tic(); //before voxel grid
        // Create the filtering object: downsample the dataset using a leaf size of 1cm
        pcl::VoxelGrid<pcl::PointXYZRGB> vg;
        vg.setInputCloud (cloud);
        float leaf=0.1f;
        vg.setLeafSize(leaf,leaf,leaf);
        vg.setDownsampleAllData(true); //work on all points?
        vg.filter(*cloud_filter);
        std::cerr << "voxel grid done,cost " << tt.toc() << " ms," \
            << cloud->points.size() << " -> " << cloud_filter->points.size() << " points" << std::endl;
    }

    // Creating the KdTree object for the search method of the extraction
    pcl::search::KdTree<pcl::PointXYZRGB>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGB>);
    tree->setInputCloud(cloud_filter);

    tt.tic(); //before voxel grid
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<pcl::PointXYZRGB> ec;
    ec.setClusterTolerance(cluster_tolerance); //设置近邻搜索的搜索半径,e.g. 0.5
    ec.setMinClusterSize(min_cluster_size);    //设置一个聚类需要的最少点数
    ec.setMaxClusterSize(cloud_filter->size()/5.);  //设置一个聚类需要的最大点数目
    ec.setSearchMethod(tree);     //设置点云的搜索机制
    ec.setInputCloud(cloud_filter); //设置原始点云
    ec.extract(cluster_indices);  //从点云中提取聚类
    std::cerr << "cluster done,cost " << tt.toc() << " ms," << cluster_indices.size() << " clusters" << std::endl;

    for (const auto& c : cluster_indices) {
        int r = 255. * std::rand() / RAND_MAX;
        int g = 255. * std::rand() / RAND_MAX;
        int b = 255. * std::rand() / RAND_MAX;
        for (const auto& idx : c.indices) {
            cloud_filter->at(idx).r = r;
            cloud_filter->at(idx).g = g;
            cloud_filter->at(idx).b = b;
        }
    }
    sensor_msgs::PointCloud2 msg_cloud_filter_cluster;
    pcl::toROSMsg(*cloud_filter, msg_cloud_filter_cluster);
    msg_cloud_filter_cluster.header.frame_id = "livox_frame";
    pc_pub.publish(msg_cloud_filter_cluster);
};

int main (int argc, char** argv) {
    ros::init(argc, argv, "pcl_euclidean_cluster");
    float cluster_tolerance = std::atof(argv[1]);
    float min_cluster_size = std::atof(argv[2]);
    ros::NodeHandle nd;
    ros::Publisher pc_pub = nd.advertise<sensor_msgs::PointCloud2>("euclidean_cluster",1);
    ros::Subscriber pc_sub = nd.subscribe<sensor_msgs::PointCloud2>("/livox/lidar",1,
                                        boost::bind(&pc_callback,_1,pc_pub,cluster_tolerance,min_cluster_size));
    ros::spin();
    return 0;
}

【实验场景1-说明】

雷达：Livox Horizon

安装方式：路侧，非水平安装，向地面倾斜约30度。

【实验场景1-测试】

rosrun n_lidar_learn n_lidar_learn_node 0.5 10
voxel grid done,cost 1 ms,24000 -> 13831 points
cluster done,cost 31 ms,63 clusters
voxel grid done,cost 1 ms,24000 -> 14037 points
cluster done,cost 32 ms,73 clusters
voxel grid done,cost 2 ms,24000 -> 13901 points
cluster done,cost 41 ms,73 clusters
voxel grid done,cost 2 ms,24000 -> 13800 points
cluster done,cost 39 ms,73 clusters
voxel grid done,cost 2 ms,24000 -> 13815 points
cluster done,cost 41 ms,69 clusters
voxel grid done,cost 2 ms,24000 -> 13915 points
cluster done,cost 33 ms,78 clusters
.....

我所用的livox horizon一帧是24000个点，测试的时候我先做了一个体素滤波，过滤调大概40%的点，主要是速度上的考虑。欧式聚类中需要设置的两个重要参数cluster_tolerance和min_cluster_size分别为0.5和10。

这里暂时没有过滤地面点，我就想着一步步来观察效果。我的场景中样本其实还是比较有代表性的，有人，车，挡墙，树木，标识牌等。因为没有分离地面点，人和车基本上就没有单独聚类出来。