PCL点云库学习笔记点云的欧式聚类

ゞ浴缸里的玫瑰 2022-09-08 14:00 506阅读 0赞

欧式聚类详解（点云数据处理）  
欧式聚类是一种基于欧氏距离度量的聚类算法。基于KD-Tree的近邻查询算法是加速欧式聚类算法的重要预处理方法。

KD-Tree最近邻搜索

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0pBVDA5Mjk_size_16_color_FFFFFF_t_70][]  
Kd-树是K-dimension tree的缩写，是对数据点在k维空间中划分的一种数据结构；Kd-树是一种平衡二叉树。为了能有效的找到最近邻，Kd-树采用分而治之的思想，即将整个空间划分为几个小部分。k-d树算法的应用可以分为两方面，一方面是有关k-d树本身这种数据结构建立的算法，另一方面是在建立的k-d树上如何进行最邻近查找的算法。  
k-d tree是每个节点均为k维数值点的二叉树，其上的每个节点代表一个超平面，该超平面垂直于当前划分维度的坐标轴，并在该维度上将空间划分为两部分，一部分在其左子树，另一部分在其右子树。即若当前节点的划分维度为d，其左子树上所有点在d维的坐标值均小于当前值，右子树上所有点在d维的坐标值均大于等于当前值，本定义对其任意子节点均成立。  
构建开始前，对比数据点在各维度的分布情况，数据点在某一维度坐标值的方差越大分布越分散，方差越小分布越集中。从方差大的维度开始切分可以取得很好的切分效果及平衡性。

KD-Tree构建原理  
常规的k-d tree的构建过程为：循环依序取数据点的各维度来作为切分维度，取数据点在该维度的中值作为切分超平面，将中值左侧的数据点挂在其左子树，将中值右侧的数据点挂在其右子树。递归处理其子树，直至所有数据点挂载完毕。

KD-Tree近邻查询  
给定点p，查询数据集中与其距离最近点的过程即为最近邻搜索

图中对于给定的查询数据点m，须从KD-Tree的根结点开始进行比较，其中m(k)为当前结点划分维度k上数据点m对应的值，d为当前结点划分的阈值。若 m(k)小于d，则访问左子树；否则访问右子树，直至到达叶子结点Q。此时Q就是当m前最近邻点，而m与Q之间的距离就是当前最小距离Dmin 。随后沿着原搜索路径回退至根结点，若此过程中发现和m之间距离小于 Dmin的点，则须将未曾访问过的子节点均纳入搜索范畴，并及时更新最近邻点，直至所有的搜索路径都为空，整个基于KD-Tree结构的最近邻点查询过程便告完成。

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0pBVDA5Mjk_size_16_color_FFFFFF_t_70 1][]

欧式聚类

对于欧式聚类来说，距离判断准则为前文提到的欧氏距离。对于空间某点P，通过KD-Tree近邻搜索算法找到k个离p点最近的点，这些点中距离小于设定阈值的便聚类到集合Q中。如果Q中元素的数目不在增加，整个聚类过程便结束；否则须在集合Q中选取p点以外的点，重复上述过程，直到Q中元素的数目不在增加为止。  
  
原文链接：https://blog.csdn.net/JAT0929/article/details/104258461

pcl::EuclideanClusterExtraction是基于欧式距离提取集群的方法，仅依据距离，将小于距离阈值的点云作为一个集群。  
　　具体的实现方法大致是：  
　　(1) 找到空间中某点p10，由kdTree找到离他最近的n个点，判断这n个点到p的距离；  
　　(2) 将距离小于阈值r的点p12、p13、p14…放在类Q里；  
　　(3) 在 Q\\p10 里找到一点p12，重复1；  
　　(4) 在 Q\\p10、p12 找到一点，重复1，找到p22、p23、p24…全部放进Q里；  
　　(5) 当 Q 再也不能有新点加入了，则完成搜索了。

后面有自定义代码：

[https://blog.csdn.net/fei\_12138/article/details/109718785][https_blog.csdn.net_fei_12138_article_details_109718785]

官方教程，结果和下面的代码不一样，下次再试一下：

[https://www.pclcn.org/study/shownews.php?id=57][https_www.pclcn.org_study_shownews.php_id_57]

点云欧式聚类算法流程  
（1）点云读入；  
（2）体素化下采样（方便处理）；  
（3）去离散点；  
（4）RANSAC算法检测平面，并剔除平面点云；  
（5）欧式聚类；  
（6）结果的输出和可视化；

python代码：

[https://zhuanlan.zhihu.com/p/75117664][https_zhuanlan.zhihu.com_p_75117664]

from paper_1_v0.my_ransac import my_ransac_v5
    import numpy as np
    
    
    
    img_id = 1  #  这里读入你的kitti 雷达数据即可
    path = r'D:\KITTI\Object\training\velodyne\%06d.bin' % img_id  ## Path ## need to be changed
    points = np.fromfile(path, dtype=np.float32).reshape(-1, 4)
    
    index_ground, index_obj, best_model, alpha = my_ransac_v5(points)   # 去除地面，用pcl 的代码也可以，不做也可以
    
    
    print('points  shape', points[index_obj, :3].shape)
    
    
    s = time.time()
    cloud = pcl.PointCloud(points[index_obj, :3])
    
    tree = cloud.make_kdtree()
    
    ec = cloud.make_EuclideanClusterExtraction()
    # ec.set_ClusterTolerance(0.02)
    ec.set_ClusterTolerance(0.62)  # 三个参数设置
    ec.set_MinClusterSize(100)
    ec.set_MaxClusterSize(15000)
    ec.set_SearchMethod(tree)
    cluster_indices = ec.Extract()
    
    print('time:', time.time() - s)
    
    print('cluster_indices : ' + str( len(cluster_indices)) + "  .")
    
    cluster_points = points[index_obj,:].copy()
    cluster_points[:,3] = 1000  # 基础颜色
    
    for j, indices in enumerate(cluster_indices):
        print('indices = ' + str(len(indices)))
        cluster_points[indices,3] = (j+10) * 4000   # 设置每个类的颜色
    
    color_cloud = pcl.PointCloud_PointXYZRGB(cluster_points)
    visual = pcl.pcl_visualization.CloudViewing()
    visual.ShowColorCloud(color_cloud, b'cloud')
    
    flag = True
    while flag:
        flag != visual.WasStopped()

体素化下采样：

1.体素滤波  
PCL实现的VoxelGrid类通过输入的点云数据创建一个三维体素栅格（可把体素栅格想象为微小的空间三维立方体的集合），然后在每个体素（即，三维立方体）内，用体素中所有点的重心来近似显示体素中其他点，这样该体素就内所有点就用一个重心点最终表示，对于所有体素处理后得到过滤后的点云。  
c++代码：

#include <pcl/io/pcd_io.h>  //文件输入输出
    #include <pcl/point_types.h>  //点类型相关定义
    #include <pcl/visualization/cloud_viewer.h>  //点云可视化相关定义
    #include <pcl/filters/voxel_grid.h>  //体素滤波相关
    #include <pcl/common/common.h>  
     
    #include <iostream>
    #include <vector>
     
    using namespace std;
     
    int main()
    {
    	//1.读取点云
    	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    	if (pcl::io::loadPCDFile<pcl::PointXYZ>("data\\demo.pcd", *cloud) == -1)
    	{
    		PCL_ERROR("Cloudn't read file!");
    		return -1;
    	}
    	cout << "there are " << cloud->points.size() << " points before filtering." << endl;
     
    	//2.体素栅格滤波
    	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filter(new pcl::PointCloud<pcl::PointXYZ>);
    	pcl::VoxelGrid<pcl::PointXYZ> sor;
    	sor.setInputCloud(cloud);
    	sor.setLeafSize(0.3f, 0.3f, 0.3f);//体素大小设置为30*30*30cm
    	sor.filter(*cloud_filter);
    	
    	//3.滤波结果保存
    	pcl::io::savePCDFile<pcl::PointXYZ>("data\\demo_filter.pcd", *cloud_filter);
    	cout << "there are " << cloud_filter->points.size() << " points after filtering." << endl;
     
    	system("pause");
    	return 0;
    }
    
    原文链接：https://blog.csdn.net/qq_22170875/article/details/84980996

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzIyMTcwODc1_size_16_color_FFFFFF_t_70][]

体素下采样前

![watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzIyMTcwODc1_size_16_color_FFFFFF_t_70 1][]

体素滤波（下采样后）

欧式聚类 c++代码：

#include <vtkAutoInit.h>
    VTK_MODULE_INIT(vtkRenderingOpenGL)
    #include <pcl/io/pcd_io.h>
    #include <pcl/point_types.h>
    #include <pcl/filters/statistical_outlier_removal.h>
    #include <pcl/filters/voxel_grid.h>
    #include <pcl/segmentation/extract_clusters.h>
    #include <pcl/visualization/pcl_visualizer.h>
    #include <pcl/segmentation/sac_segmentation.h>
    #include <pcl/ModelCoefficients.h>
    #include <pcl/filters/extract_indices.h>
    #include<ctime>
    #include<cstdlib>
    #include <windows.h>
    
    using namespace pcl;
    using namespace std;
    typedef PointXYZ PoinT;
    
    int *rand_rgb(){//随机产生颜色
        int *rgb = new int[3];    
        rgb[0] = rand() % 255;
        rgb[1] = rand() % 255;
        rgb[2] = rand() % 255;
        return rgb;
    }
    int main(){
        //点云的读取*********************************************************
        PointCloud<PoinT>::Ptr cloud(new PointCloud<PoinT>);
        if (io::loadPCDFile("C:\\Users\\Administrator\\Desktop\\desk.pcd", *cloud) == -1)
        {
            PCL_ERROR("read false");
            return 0;
        }
        //体素化下采样******************************************************
        VoxelGrid<PoinT> vox;
        PointCloud<PoinT>::Ptr vox_cloud(new PointCloud<PoinT>);
        vox.setInputCloud(cloud);
        vox.setLeafSize(0.01, 0.01, 0.01);
        vox.filter(*vox_cloud);
        //去除噪声点********************************************************
        StatisticalOutlierRemoval<PoinT>sor;
        PointCloud<PoinT>::Ptr sor_cloud(new PointCloud<PoinT>);
        sor.setMeanK(10);
        sor.setInputCloud(vox_cloud);
        sor.setStddevMulThresh(0.2);
        sor.filter(*sor_cloud);
        //平面分割(RANSAC)********************************************************
        SACSegmentation<PoinT> sac;
        PointIndices::Ptr inliner(new PointIndices);
        ModelCoefficients::Ptr coefficients(new ModelCoefficients);
        PointCloud<PoinT>::Ptr sac_cloud(new PointCloud<PoinT>);
        sac.setInputCloud(sor_cloud);
        sac.setMethodType(SAC_RANSAC);
        sac.setModelType(SACMODEL_PLANE);
        sac.setMaxIterations(100);
        sac.setDistanceThreshold(0.02);
        //提取平面(展示并输出)******************************************************
        PointCloud<PoinT>::Ptr ext_cloud(new PointCloud<PoinT>);
        PointCloud<PoinT>::Ptr ext_cloud_rest(new PointCloud<PoinT>);
        visualization::PCLVisualizer::Ptr viewer(new visualization::PCLVisualizer("3d view"));
    
        int i = sor_cloud->size(), j = 0;
        ExtractIndices<PoinT>ext;
        srand((unsigned)time(NULL));//刷新时间的种子节点需要放在循环体外面
        while (sor_cloud->size()>i*0.3)//当提取的点数小于总数的3/10时，跳出循环
        {
            ext.setInputCloud(sor_cloud);
            sac.segment(*inliner, *coefficients);
            if (inliner->indices.size()==0)
            {
                break;
            }
            //按照索引提取点云*************
            ext.setIndices(inliner);
            ext.setNegative(false);
            ext.filter(*ext_cloud);
            ext.setNegative(true);
            ext.filter(*ext_cloud_rest);
            //*****************************
            *sor_cloud = *ext_cloud_rest;
            stringstream ss;
            ss <<"C:\\Users\\Administrator\\Desktop\\"<<"ext_plane_clouds" << j << ".pcd";//路径加文件名和后缀
            io::savePCDFileASCII(ss.str(), *ext_cloud);//提取的平面点云写出
            int *rgb = rand_rgb();//随机生成0-255的颜色值
            visualization::PointCloudColorHandlerCustom<PoinT>rgb1(ext_cloud,rgb[0],rgb[1],rgb[2]);//提取的平面不同彩色展示
            delete[]rgb;
            viewer->addPointCloud(ext_cloud, rgb1,ss.str());
            j++;
        }
        viewer->spinOnce(1000);
        //欧式聚类*******************************************************
        vector<PointIndices>ece_inlier;
        search::KdTree<PoinT>::Ptr tree(new search::KdTree<PoinT>);
        EuclideanClusterExtraction<PoinT> ece;
        ece.setInputCloud(sor_cloud);
        ece.setClusterTolerance(0.02);
        ece.setMinClusterSize(100);
        ece.setMaxClusterSize(20000);
        ece.setSearchMethod(tree);
        ece.extract(ece_inlier);
        //聚类结果展示***************************************************
        ext.setInputCloud(sor_cloud);
        visualization::PCLVisualizer::Ptr viewer2(new visualization::PCLVisualizer("Result of EuclideanCluster"));
        srand((unsigned)time(NULL));
        for (int i = 0; i < ece_inlier.size();i++)
        {
            PointCloud<PoinT>::Ptr cloud_copy(new PointCloud<PoinT>);
            vector<int> ece_inlier_ext = ece_inlier[i].indices;
            copyPointCloud(*sor_cloud, ece_inlier_ext, *cloud_copy);//按照索引提取点云数据
            stringstream ss1;
            ss1 <<"C:\\Users\\Administrator\\Desktop\\"<< "EuclideanCluster_clouds" << j<<".pcd";
            io::savePCDFileASCII(ss1.str(), *ext_cloud);//欧式聚类结果写出
            int *rgb1 = rand_rgb();
            visualization::PointCloudColorHandlerCustom<PoinT>rgb2(ext_cloud, rgb1[0], rgb1[1], rgb1[2]);
            delete[]rgb1;
            viewer2->addPointCloud(cloud_copy, rgb2,ss1.str());
            j++;
        }
        viewer2->spin();
        return 0;
    }

可视化结果展示：  
提取的平面（颜色随机）：  
欧式聚类的结果（颜色随机）：

写出的点云数据（平面2个，欧式聚类结果5个）：

遇到过error C4996: 'pcl::SAC\_SAMPLE\_SIZE’编译错误的问题  
解决方法参考链接：https://blog.csdn.net/lizhengze1117/article/details/86565100

#include <pcl/io/pcd_io.h>
    #include <pcl/point_types.h>
    #include <pcl/filters/statistical_outlier_removal.h>
    #include <pcl/filters/voxel_grid.h>
    #include <pcl/segmentation/extract_clusters.h>
    #include <pcl/visualization/pcl_visualizer.h>
    #include <pcl/segmentation/sac_segmentation.h>
    #include <pcl/ModelCoefficients.h>
    #include <pcl/filters/extract_indices.h>
    #include<ctime>
    #include<cstdlib>
    #include <windows.h>
    
    using namespace pcl;
    using namespace std;
    typedef PointXYZ PoinT;
    
    int main(){
        //点云的读取*********************************************************
        PointCloud<PoinT>::Ptr cloud(new PointCloud<PoinT>);
        if (io::loadPCDFile("C:\\Users\\admin\\Desktop\\保留的结果.pcd", *cloud) == -1)
        {
            PCL_ERROR("read false");
            return 0;
        }
        //欧式聚类*******************************************************
        vector<PointIndices>ece_inlier;
        search::KdTree<PoinT>::Ptr tree(new search::KdTree<PoinT>);
        EuclideanClusterExtraction<PoinT> ece;
        ece.setInputCloud(cloud);
        ece.setClusterTolerance(1);
        ece.setMinClusterSize(100);
        ece.setMaxClusterSize(20000000000000);
        ece.setSearchMethod(tree);
        ece.extract(ece_inlier);
        //聚类结果展示***************************************************
        PointCloud<PoinT>::Ptr ext_cloud(new PointCloud<PoinT>);
        ExtractIndices<PoinT>ext;
        ext.setInputCloud(cloud);
        visualization::PCLVisualizer::Ptr viewer2(new visualization::PCLVisualizer("Result of EuclideanCluster"));
        srand((unsigned)time(NULL));
        int j = 0;
        vector<unsigned char>color;
        for (int i_segment = 0; i_segment < ece_inlier.size(); i_segment++)
        {
            color.push_back(static_cast<unsigned char>(rand() % 256));
            color.push_back(static_cast<unsigned char>(rand() % 256));
            color.push_back(static_cast<unsigned char>(rand() % 256));
        }
        int color_index = 0;
        PointCloud<PointXYZRGB>::Ptr color_point(new PointCloud<PointXYZRGB>);
        for (int i_seg = 0; i_seg < ece_inlier.size(); i_seg++)
        {
            int clusters_size = ece_inlier[i_seg].indices.size();
            for (int i_idx = 0; i_idx < clusters_size; i_idx++)
            {
                PointXYZRGB point;
                point.x = cloud->points[ece_inlier[i_seg].indices[i_idx]].x;
                point.y = cloud->points[ece_inlier[i_seg].indices[i_idx]].y;
                point.z = cloud->points[ece_inlier[i_seg].indices[i_idx]].z;
                point.r = color[3 * color_index];
                point.g = color[3 * color_index + 1];
                point.b = color[3 * color_index + 2];
                color_point->push_back(point);
            }
            color_index++;
        }
        viewer2->addPointCloud(color_point);
        viewer2->spin();
        return 0;
    }

————————————————  
版权声明：本文为CSDN博主「起个名字费劲死了」的原创文章，遵循CC 4.0 BY-SA版权协议，转载请附上原文出处链接及本声明。  
原文链接：https://blog.csdn.net/qq\_40335930/article/details/91982640

[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0pBVDA5Mjk_size_16_color_FFFFFF_t_70]: /images/20220829/2abd86aaf07144fabdfc9c0def4e972f.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L0pBVDA5Mjk_size_16_color_FFFFFF_t_70 1]: /images/20220829/71691964eba24787a9b4854fc9b1a613.png
[https_blog.csdn.net_fei_12138_article_details_109718785]: https://blog.csdn.net/fei_12138/article/details/109718785
[https_www.pclcn.org_study_shownews.php_id_57]: https://www.pclcn.org/study/shownews.php?id=57
[https_zhuanlan.zhihu.com_p_75117664]: https://zhuanlan.zhihu.com/p/75117664
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzIyMTcwODc1_size_16_color_FFFFFF_t_70]: /images/20220829/1adfb55d2c0849c79cd7534fb2ffa7b5.png
[watermark_type_ZmFuZ3poZW5naGVpdGk_shadow_10_text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzIyMTcwODc1_size_16_color_FFFFFF_t_70 1]: /images/20220829/dab297ad6367402abe833fac019182f4.png