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

//enviroment.cpp

    //cluster
    std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> my_cluster_vector;
    my_cluster_vector=my_processor->Clustering(seg_cloud_pair.second,1,3,30);
    std::vector<Color> Colors = {Color(255,0,0),Color(0,255,0),Color(0,0,255)};
    int clusterid = 0;
    for(pcl::PointCloud<pcl::PointXYZ>::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<typename PointT>
std::vector<typename pcl::PointCloud<PointT>::Ptr> ProcessPointClouds<PointT>::Clustering(typename pcl::PointCloud<PointT>::Ptr cloud, float clusterTolerance, int minSize, int maxSize)
{

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

    std::vector<typename pcl::PointCloud<PointT>::Ptr> clusters;

    // TODO:: Fill in the function to perform euclidean clustering to group detected obstacles
    typename  pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>);
    tree->setInputCloud (cloud);
    std::vector<pcl::PointIndices> cluster_indices;
    pcl::EuclideanClusterExtraction<PointT> 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<PointT>::Ptr cloud_temp(new pcl::PointCloud<PointT>);
        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<std::chrono::milliseconds>(endTime - startTime);
    std::cout << "clustering took " << elapsedTime.count() << " milliseconds and found " << clusters.size() << " clusters" << std::endl;

    return clusters;
}