【2】激光点云处理聚类（Clustering）

//enviroment.cpp

    //cluster
    std::vector::Ptr> my_cluster_vector;
    my_cluster_vector=my_processor->Clustering(seg_cloud_pair.second,1,3,30);
    std::vector Colors = {Color(255,0,0),Color(0,255,0),Color(0,0,255)};
    int clusterid = 0;
    for(pcl::PointCloud::Ptr cluster: my_cluster_vector){
        std::cout<<"size of cluster"+std::to_string(clusterid)<<":";
        my_processor->numPoints(cluster);
        renderPointCloud(viewer, cluster, "obcluster"+std::to_string(clusterid), Colors[clusterid % Colors.size()]);

        Box box = my_processor->BoundingBox(cluster);
        renderBox(viewer,box,clusterid,Color(0,20,20));
        clusterid++;
    }

//ProcressPointClouds.cpp

template
std::vector::Ptr> ProcessPointClouds::Clustering(typename pcl::PointCloud::Ptr cloud, float clusterTolerance, int minSize, int maxSize)
{

    // Time clustering process
    auto startTime = std::chrono::steady_clock::now();

    std::vector::Ptr> clusters;

    // TODO:: Fill in the function to perform euclidean clustering to group detected obstacles
    typename  pcl::search::KdTree::Ptr tree (new pcl::search::KdTree);
    tree->setInputCloud (cloud);
    std::vector cluster_indices;
    pcl::EuclideanClusterExtraction ec;
    ec.setClusterTolerance (clusterTolerance); // 2cm
    ec.setMinClusterSize (minSize);
    ec.setMaxClusterSize (maxSize);
    ec.setSearchMethod (tree);
    ec.setInputCloud (cloud);
    ec.extract (cluster_indices);
    for(auto x:cluster_indices)
    {
        typename pcl::PointCloud::Ptr cloud_temp(new pcl::PointCloud);
        for(auto p:x.indices)
        {
            cloud_temp->points.push_back(cloud->points[p]);
        }
        clusters.push_back(cloud_temp);
    }

    auto endTime = std::chrono::steady_clock::now();
    auto elapsedTime = std::chrono::duration_cast(endTime - startTime);
    std::cout <<"clustering took " <" milliseconds and found " <" clusters" << std::endl;

    return clusters;
}