在第（3）实现了地面点与障碍物的分离，此部分要实现的是聚类，聚类是指把不同物体的点云分别组合聚集起来, 从而能让你跟踪汽车, 行人等多个目标. 其中一种对点云数据进行分组和聚类的方法称为欧氏聚类。
欧式聚类是指将距离紧密度高的点云聚合起来. 为了有效地进行最近邻搜索, 可以使用 KD-Tree 数据结构, 这种结构平均可以加快从 o (n)到 o (log (n))的查找时间. 这是因为Kd-Tree允许你更好地分割你的搜索空间. 通过将点分组到 KD-Tree 中的区域中, 您可以避免计算可能有数千个点的距离, 因为你知道它们不会被考虑在一个紧邻的区域中.
作者罗列了两种方式的欧式聚类第一种是自己重写了欧式聚类跟k-dtree，第二种是直接调用PCL库里边的欧式聚类。以下是两种方式的记录便于学习。
第一种：Manual Euclidean clustering
除此之外我们也可以直接使用KD-Tree进行欧氏聚类.
在此我们首先对KD-Tree的原理进行介绍. KD-Tree是一个数据结构, 由二进制树表示, 在不同维度之间对插入的点的数值进行交替比较, 通过分割区域来分割空间, 如在3D数据中, 需要交替在X,Y,Z不同轴上比较. 这样会使最近邻搜索可以快得多.
首先我们在试着二维空间上建立KD-Tree, 并讲述欧氏聚类的整个在二维空间上的实现过程, 最终我们将扩展到三维空间.
在KD-Tree中插入点(这是将点云输入到树中创建和构建KD-Tree的步骤)
假设我们需要在KD-Tree中插入４个点(-6.3, 8.4), (-6.2, 7), (-5.2, 7.1), (-5.7, 6.3)
首先我们得确定一个根点(-6.2, 7), 第0层为x轴, 需要插入的点为(-6.3, 8.4), (-5.2, 7.1), 因为-6.3<-6.2,(-6.3, 8.4)划分为左子节点, 而-5.2>-6.2, (-5.2, 7.1)划分为右子节点. (-5.7, 6.3)中x轴-5.7>-6.2,所以放在(-5.2, 7.1)节点下, 接下来第1层使用y轴, 6.3<7.1, 因此放在(-5.2, 7.1)的左子节点. 同理, 如果我们想插入第五个点(7.2, 6.1), 你可以交替对比x,y轴数值, (7.2>-6.2)->(6.1<7.1)->(7.2>-5.7), 第五点应插入到(-5.7, 6.3)的右子节点C.
下图是KD-Tree的结构.
第二种：PCL Euclidean clustering
首先我们使用PCL内置的欧式聚类函数. 点云聚类的具体细节推荐查看PCL的官网文档。
欧氏聚类对象 ec 具有距离容忍度. 在这个距离之内的任何点都将被组合在一起. 它还有用于表示集群的点数的 min 和 max 参数. 如果一个集群真的很小, 它可能只是噪音, min参数会限制使用这个集群. 而max参数允许我们更好地分解非常大的集群, 如果一个集群非常大, 可能只是许多其他集群重叠, 最大容差可以帮助我们更好地解决对象检测. 欧式聚类的最后一个参数是 Kd-Tree. Kd-Tree是使用输入点云创建和构建的, 这些输入点云是初始点云过滤分割后得到障碍物点云.
聚类结果可视化显示：
#include <iostream>#include <pcl/io/io.h>#include <pcl/io/pcd_io.h>#include <pcl/point_types.h>#include <pcl/visualization/cloud_viewer.h>#include <pcl/segmentation/sac_segmentation.h>#include <pcl/segmentation/extract_clusters.h>#include <pcl/kdtree/kdtree.h>#include <pcl/common/common.h>#include <unordered_set>using namespace std;using namespace pcl;int main(){pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::PCDReader reader;reader.read("F:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection\\data\\pcd\\pcd\\out_plane.pcd", *cloud);pcl::PointCloud<pcl::PointXYZ>::Ptr obstCloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::PCDReader reader2;reader2.read("F:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection\\data\\pcd\\pcd\\obstCloud.pcd", *obstCloud);pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);//tree->setInputCloud(cloud);tree->setInputCloud(obstCloud);std::vector<pcl::PointCloud<pcl::PointXYZ>> clusters;//保存分割后的所有类 每一类为一个点云( //创建点云索引向量，用于存储实际的点云信息)// 欧式聚类对检测到的障碍物进行分组float clusterTolerance = 0.5;int minsize = 10;int maxsize = 140;std::vector<pcl::PointIndices> clusterIndices;// 创建索引类型对象pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; // 欧式聚类对象ec.setClusterTolerance(clusterTolerance);//设置近邻搜索半径ec.setMinClusterSize(minsize);//设置一个类需要的最小的点数ec.setMaxClusterSize(maxsize);//设置一个类需要的最大的点数ec.setSearchMethod(tree);//设置搜索方法//ec.setInputCloud(cloud); // feed point cloudec.setInputCloud(obstCloud);//输入的点云不同，聚类的结果不同ec.extract(clusterIndices); // 得到所有类别的索引 clusterIndices 11类// 将得到的所有类的索引分别在点云中找到，即每一个索引形成了一个类for (pcl::PointIndices getIndices : clusterIndices){pcl::PointCloud<pcl::PointXYZ> cloudCluster;//PtCdtr<PointT> cloudCluster(new pcl::PointCloud<PointT>);// For each point indice in each clusterfor (int index : getIndices.indices){cloudCluster.push_back(cloud->points[index]);//cloudCluster.push_back(obstCloud->points[index]);}cloudCluster.width = cloudCluster.size();cloudCluster.height = 1;cloudCluster.is_dense = true;clusters.push_back(cloudCluster);}/*为了从点云索引向量中分割出每个聚类，必须迭代访问点云索引，每次创建一个新的点云数据集，并且将所有当前聚类的点写入到点云数据集中。*///迭代访问点云索引clusterIndices，直到分割出所有聚类int j = 0;for (std::vector<pcl::PointIndices>::const_iterator it = clusterIndices.begin(); it != clusterIndices.end(); ++it){pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);//创建新的点云数据集cloud_cluster，将所有当前聚类写入到点云数据集中for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit)//cloud_cluster->points.push_back(cloud->points[*pit]); //查找的是定义的cloud里边对应的点云cloud_cluster->points.push_back(obstCloud->points[*pit]); //查找的是定义的obstCloud里边对应的点云cloud_cluster->width = cloud_cluster->points.size();cloud_cluster->height = 1;cloud_cluster->is_dense = true;std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size() << " data points." << std::endl;std::stringstream ss;ss << "F:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection\\data\\pcd\\pcd\\cluster\\" << j << ".pcd";pcl::PCDWriter writer;//保存提取的点云文件writer.write<pcl::PointXYZ>(ss.str(), *cloud_cluster, false); //*j++;}int clusterId = 0;pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));//viewer->addPointCloud<pcl::PointXYZ>(obstCloud, "obstCLoud" ); for (pcl::PointCloud<pcl::PointXYZ> cluster : clusters){//viewer->addPointCloud<pcl::PointXYZ>(cloud, "obstCLoud" + std::to_string(clusterId));//cloud可视化显示的是平面的聚类viewer->addPointCloud<pcl::PointXYZ>(obstCloud, "obstCLoud" + std::to_string(clusterId));//obstCloud可视化显示的是车的聚类viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "obstCLoud" + std::to_string(clusterId));viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 1, "obstCLoud" + std::to_string(clusterId));++clusterId;}viewer->resetCamera();//相机点重置viewer->spin();cout << clusters.size() << endl;system("pause");return 0;}
给聚类结果画框：
结果展示：
#include <iostream>#include <pcl/io/io.h>#include <pcl/io/pcd_io.h>#include <pcl/point_types.h>#include <pcl/visualization/cloud_viewer.h>#include <pcl/segmentation/sac_segmentation.h>#include <pcl/segmentation/extract_clusters.h>#include <pcl/kdtree/kdtree.h>#include <pcl/common/common.h>#include <unordered_set>using namespace std;using namespace pcl;struct Box{float x_min;float y_min;float z_min;float x_max;float y_max;float z_max;};int main(){pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::PCDReader reader;reader.read("D:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection代码分解\\out_plane.pcd", *cloud);pcl::PointCloud<pcl::PointXYZ>::Ptr all_cloud(new pcl::PointCloud<pcl::PointXYZ>);pcl::PCDReader reader2;reader2.read("D:\\SFND_Lidar_Obstacle_Detection\\SFND_Lidar_Obstacle_Detection代码分解\\cloudRegion.pcd", *all_cloud);pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);tree->setInputCloud(cloud);std::vector<pcl::PointCloud<pcl::PointXYZ>> clusters;//保存分割后的所有类 每一类为一个点云// 欧式聚类对检测到的障碍物进行分组float clusterTolerance = 0.5;int minsize = 10;int maxsize = 140;std::vector<pcl::PointIndices> clusterIndices;// 创建索引类型对象pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; // 欧式聚类对象ec.setClusterTolerance(clusterTolerance);//设置近邻搜索半径ec.setMinClusterSize(minsize);//设置一个类需要的最小的点数ec.setMaxClusterSize(maxsize);//设置一个类需要的最大的点数ec.setSearchMethod(tree);//设置搜索方法ec.setInputCloud(cloud); // feed point cloudec.extract(clusterIndices); // 得到所有类别的索引 clusterIndices 11类// 将得到的所有类的索引分别在点云中找到，即每一个索引形成了一个类for (pcl::PointIndices getIndices : clusterIndices){pcl::PointCloud<pcl::PointXYZ> cloudCluster;//PtCdtr<PointT> cloudCluster(new pcl::PointCloud<PointT>);// For each point indice in each clusterfor (int index : getIndices.indices) {cloudCluster.push_back(cloud->points[index]);}cloudCluster.width = cloudCluster.size();cloudCluster.height = 1;cloudCluster.is_dense = true;clusters.push_back(cloudCluster);}int clusterId = 0;pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));//viewer->addPointCloud<pcl::PointXYZ>(all_cloud, "obstCLoud" ); //整个点云for (pcl::PointCloud<pcl::PointXYZ> cluster : clusters){viewer->addPointCloud<pcl::PointXYZ>(cloud, "obstCLoud" + std::to_string(clusterId));viewer->setPointCloudRenderingProperties( pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "obstCLoud" + std::to_string(clusterId) );viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, 0, 0, 1, "obstCLoud" + std::to_string(clusterId));pcl::PointXYZ minPoint, maxPoint;pcl::getMinMax3D(cluster, minPoint, maxPoint);//想得到它x,y,z三个轴上的最大值和最小值Box box;box.x_min = minPoint.x;box.y_min = minPoint.y;box.z_min = minPoint.z;box.x_max = maxPoint.x;box.y_max = maxPoint.y;box.z_max = maxPoint.z;std::string cube = "box" + std::to_string(clusterId);viewer->addCube(box.x_min, box.x_max, box.y_min, box.y_max, box.z_min, box.z_max, 1, 0, 0, cube);viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_REPRESENTATION, pcl::visualization::PCL_VISUALIZER_REPRESENTATION_WIREFRAME, cube); //只显示线框++clusterId;}viewer->resetCamera();//相机点重置viewer->spin();cout << clusters.size() << endl;system("pause");return 0;}
