在第（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 cloud
	ec.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 cluster
		for (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 cloud
	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 cluster
		for (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;

}


 

无人驾驶汽车系统入门（三十一）——点云分割和聚类算法详解

本篇详细讲解点云处理中的基本分割和聚类的算法原理。

Lidar基本常识
lidar的分辨率要高于radar，但是radar可以直接测量目标的速度。通过融合两者，可以获得对目标较好的位置和速度估计。
激光雷达坐标系：右手法则，大拇指朝上为z，食指朝前为x，中指朝左为y, lidar的解析度很大程度上取决于线数，其解析度指标分为横向解析度和纵向解析度，横向解析度即一条激光束扫描一圈（360度）的反射点的数量，作为参考，Velodyne的16线激光雷达的横向解析度为1800个反射点，即：
$\frac{360}{1800} = 0.2 \  degree/point$
也就是说这一款雷达横向上每0.2度有一个反射点。纵向解析度即雷达的线束数量，线束越多解析度越高。
常用的point cloud处理工具：

PCL: 主要用于点云滤波、分割和聚类
OpenCV
ROS：提供了一些点云数据表示、消息格式、坐标变化、可视化工具
Eigen：线性运算库，可用于描述各类变换，通常用于点云的坐标变换（点云SLAM里用的比较多）

传统的点云感知算法处理流程：

点云滤波：降采样点云以加速后续计算
点云分割：分别提取出地面点和非地面点
点云聚类：将非地面点通过空间关系聚类成点云簇
目标轮廓拟合：使用设定形状（如bounding box）将点云簇拟合

本文详细讲解基础的点云分割和点云聚类算法。
点云滤波和降采样

为什么做滤波(filter)和降采样？

滤波是点云预处理的步骤，可以显著减低后续处理的计算复杂度，有些滤波方法还能够滤除点云中的噪点。PCL提供多种点云滤波方法的实现，其中应用最多的是体素滤波：Voxel grid filter. 体素滤波即将空间按照一定尺寸（比如说$1m \times 1m \times 1m$）的立方体格进行划分，每个立方格内仅保留一个点。下图是voxel size为0.3m的降采样结果。

PCL提供各类成熟的点云滤波方法，以体素滤波为例：
typename pcl::VoxelGrid<PointT> voxel_filter;
voxel_filter.setInputCloud(cloud);
voxel_filter.setLeafSize(filterRes, filterRes, filterRes);
typename pcl::PointCloud<PointT>::Ptr ds_cloud(new pcl::PointCloud<PointT>);
voxel_filter.filter(*ds_cloud);

点云分割：RANSAC 算法
RANSAC: RANdom SAmple Consensus, 通过随机采样和迭代的方法用一种模型（比如说直线模型、平面模型）拟合一群数据，在点云处理中通常用于点云数据的分割，以最简单的直线拟合为例：
给定平面内若干个点的集合$P_n: \{(x_0, y_0), (x_1, y_1), ... , (x_n, y_n)\}$，要求得一条直线 $L: ax+by+c=0$ 使得拟合尽可能多的点，也就是 $n$ 个点到直线 $L$ 的距离和最短；RANSAC的做法是通过一定的迭代次数(比如1000次)，在每次迭代中随机在点集 $P_n$ 中选取两个点 $(x1, y1), (x2, y2)$，计算 $a, b, c$:
$a = y1 - y2, \\ b = x2 - x1, \\ c = x1*y2 -x2*y1$
接着遍历 $P_n$ 每一个点$(x_i, y_i)$到直线 $L$ 的距离 $dist$,
$dist = \frac{fabs(a * x_i + b * y_i + c)}{sqrt(a*a + b*b)}$
当$dist$小于我们给定的一个距离阈值 $D_{threshold}$，则认为点在L上，穿过点最多的直线即为RANSAC搜索的最优（拟合）的直线。平面拟合同理。
PCL中已经成熟地实现了各类模型地RANSAC拟合，以平面拟合为例：
// cloud are input cloud, maxIterations and
// distanceThreshold are hyperparameters
pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients ());
pcl::PointIndices::Ptr inliers(new pcl::PointIndices());

pcl::SACSegmentation<PointT> seg;
seg.setOptimizeCoefficients(true);
seg.setModelType(pcl::SACMODEL_PLANE);
seg.setMethodType(pcl::SAC_RANSAC);
seg.setMaxIterations(maxIterations);
seg.setDistanceThreshold(distanceThreshold);

seg.setInputCloud(cloud);
// the indices in inliers are the points in line
seg.segment(*inliers, *coefficients);

点云聚类之用于加速邻近点搜索的数据结构——KD树
KD树是一种用于加速最邻近搜索的二叉树，主要通过划分区域以实现加速，以2D的KD树为例，只需考虑x和y两个划分，假定点集如下：
$\{(7, 2), (5, 4), (9, 6), (4, 7), (8, 1), (2, 3)\}$
往kd树中插入一个点$(7,2)$,在x方向对平面进行划分如下（横轴为x，纵轴为y）：

接着依次插入第二个和第三个点，第二个点是$(5, 4)$, 5小于7所有该点在左边，$(9, 6)$的$x$大于7所以在右边，基于y对空间进行第二次划分（图中蓝线）：

2D KD树的划分顺序为: x->y->x->y…, 所以到第四个点$(4, 7)$, 其$x$小于$(7, 2)$所以在根节点的左边， $y$大于$(5, 4)$的$y$所以在它的右边，依次类推得到对二维空间的划分图：

构造的树如下：

因为按区域进行了划分，kd树显著的降低了搜索的步数，以上图为例，假定我们聚类的距离阈值为4，要搜索出点$(2,3)$的距离阈值内的点，先从根节点$(7, 2)$开始,算得距离大于4，类似于插入点的流程，查找左边的点$(5, 4)$， 算得距离小于4，得到一个近距离点，接着搜索，虽然$(2,3)$在$(5,4)$左侧，但是为其本身，且无叶子节点，所以左侧搜索结束，看$(5,4)$右侧节点，计算得$(4, 7)$到目标点的距离大于4，搜索结束。
以下是一个用c++实现的2维KD树的例子：
// Structure to represent node of kd tree
struct Node{
	std::vector<float> point;
	int id;
	Node* left;
	Node* right;

	Node(std::vector<float> arr, int setId)
	: point(arr), id(setId), left(NULL), right(NULL){}
};

struct KdTree
{
	Node* root;

	KdTree(): root(NULL){}

	void insert(std::vector<float> point, int id){
		recursive_insert(&root, 0, point, id);
	}

	void recursive_insert(Node **root, int depth, std::vector<float>  point, int id){
	    if (*root!= NULL){
	        int i = depth%2;
	        if(point[i] < (*root)->point[i]){
	            // left
	            recursive_insert(&(*root)->left, depth+1, point, id);
	        } else{
	            //right
	            recursive_insert(&(*root)->right, depth+1, point, id);
	        }
	    }else{
            *root = new Node(point, id);
	    }
	}

	void recursive_search(Node * node, int depth, std::vector<int> &ids, 
                       std::vector<float> target, float distanceTol){
	    if(node != NULL){
	        // compare current node to target
	        if ((node->point[0] >= (target[0]-distanceTol)) && (node->point[0] <= (target[0]+distanceTol)) &&
                    (node->point[1] >= (target[1]-distanceTol)) && (node->point[1] <= (target[1]+distanceTol))){
	            
	            float dis = sqrt((node->point[0]-target[0]) * (node->point[0]-target[0]) +
	                    (node->point[1]-target[1]) * (node->point[1]-target[1]));
	            
	            if (dis <= distanceTol){
	                ids.push_back(node->id);
	            }
	        }
	        if((target[depth%2] - distanceTol)<node->point[depth%2]){
	            // go to left
                recursive_search(node->left, depth + 1, ids, target, distanceTol);
            }
            if((target[depth%2] + distanceTol)>node->point[depth%2]){
                // go to right
                recursive_search(node->right, depth+1, ids, target, distanceTol);
            }
	    }
	}

	// return a list of point ids in the tree that are within distance of target
	std::vector<int> search(std::vector<float> target, float distanceTol){
		std::vector<int> ids;
		recursive_search(root, 0, ids, target, distanceTol);
		return ids;
	}
};

欧几里得聚类算法
点云聚类中常用的欧几里得聚类算法就是基于KD树实现的，聚类的目的是搜索出点云中“聚在一起”的点云簇，从而得到感知目标的位置和轮廓，以2D点欧几里得聚类为例，其处理过程如下：
std::vector<std::vector<int>> euclideanCluster(const std::vector<std::vector<float>>& points, KdTree* tree, float distanceTol){
	
    std::vector<std::vector<int>> clusters;
    std::vector<bool> processed(points.size(), false);

    for (int i = 0; i < points.size(); ++i) {
        if (processed[i] == true){
            continue;
        } else{
            std::vector<int> cluster;
            Proximity(cluster, points, processed, distanceTol, tree, i);
            clusters.push_back(cluster);
        }
    }
	return clusters;
}

// 递归聚类
void Proximity(std::vector<int> & cluster, std::vector<std::vector<float>> point,
               std::vector<bool> &processed, float distanceTol, KdTree* tree, int ind){
    processed[ind] = true;
    cluster.push_back(ind);
    std::vector<int> nearby_points = tree->search(point[ind], distanceTol);
    for (int i = 0; i < nearby_points.size(); ++i) {
        if(processed[nearby_points[i]]){
            continue;
        } else {
            Proximity(cluster, point, processed, distanceTol, tree, nearby_points[i]);
        }
    }
}

PCL中的欧几里得聚类
以上是关于KD树和欧几里得聚类的基本介绍，实际业务中我们并不需要自行实现欧几里得聚类，而且采用PCL中实现的欧几里得聚类，其基本用法如下：
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)
{

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

    // perform euclidean clustering to group detected obstacles
    typename pcl::search::KdTree<PointT>::Ptr kd_tree(new pcl::search::KdTree<PointT>());
    kd_tree->setInputCloud(cloud);
    typename pcl::EuclideanClusterExtraction<PointT> eu_cluster;
    eu_cluster.setClusterTolerance(clusterTolerance);
    eu_cluster.setMinClusterSize(minSize);
    eu_cluster.setMaxClusterSize(maxSize);
    eu_cluster.setSearchMethod(kd_tree);
    eu_cluster.setInputCloud(cloud);

    std::vector<pcl::PointIndices> cluster_list;
    eu_cluster.extract(cluster_list);

    for (auto cluster_indices : cluster_list){
        typename pcl::PointCloud<PointT>::Ptr cluster_points (new pcl::PointCloud<PointT>());
        for(auto ind : cluster_indices.indices){
            cluster_points->points.push_back(cloud->points[ind]);
        }
        cluster_points->width = cluster_points->points.size();
        cluster_points->height = 1;
        cluster_points->is_dense = true;
        clusters.push_back(cluster_points);
    }

    return clusters;
}

更详细的ros实现可以参考我的这篇博客：https://blog.csdn.net/AdamShan/article/details/83015570?spm=1001.2014.3001.5501

Lidar系列文章
传感器融合是将多个传感器采集的数据进行融合处理，以更好感知周围环境；这里首先介绍激光雷达的相关内容，包括激光雷达基本介绍（本节内容），激光点云数据处理方法（点云数据显示，点云分割，点云聚类，障碍物识别实例）等。
系列文章目录
1. 激光雷达基本介绍

2. 激光点云数据显示

3. 基于RANSAC的激光点云分割

4. 点云分割入门级实例学习

5. 激光点云目标物聚类

6. 基于PCL实现欧式聚类提取

7. 激光雷达障碍物识别

文章目录
Lidar系列文章
基于PCL实现欧式聚类提取

基于PCL实现欧式聚类提取
本节我们将介绍如何用PCL EuclideanClusterExtraction类采用欧氏聚类对三维点云组成的场景进行分割。

原始点云如下：


#include <pcl/ModelCoefficients.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/features/normal_3d.h>
#include <pcl/kdtree/kdtree.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
#include <pcl/segmentation/extract_clusters.h>


int 
main (int argc, char** argv)
{
  // Read in the cloud data 读入点云数据
  pcl::PCDReader reader;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>), cloud_f (new pcl::PointCloud<pcl::PointXYZ>);
  reader.read ("../table_scene_lms400.pcd", *cloud);
  std::cout << "PointCloud before filtering has: " << cloud->points.size () << " data points." << std::endl; //*

  // Create the filtering object: downsample the dataset using a leaf size of 1cm 降采样
  pcl::VoxelGrid<pcl::PointXYZ> vg;
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered (new pcl::PointCloud<pcl::PointXYZ>);
  vg.setInputCloud (cloud);
  vg.setLeafSize (0.01f, 0.01f, 0.01f);
  vg.filter (*cloud_filtered);
  std::cout << "PointCloud after filtering has: " << cloud_filtered->points.size ()  << " data points." << std::endl; //*

  // Create the segmentation object for the planar model and set all the parameters SANSAC点云分割
  pcl::SACSegmentation<pcl::PointXYZ> seg;
  pcl::PointIndices::Ptr inliers (new pcl::PointIndices);
  pcl::ModelCoefficients::Ptr coefficients (new pcl::ModelCoefficients);
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_plane (new pcl::PointCloud<pcl::PointXYZ> ());
  pcl::PCDWriter writer;
  seg.setOptimizeCoefficients (true);
  seg.setModelType (pcl::SACMODEL_PLANE);
  seg.setMethodType (pcl::SAC_RANSAC);
  seg.setMaxIterations (100);
  seg.setDistanceThreshold (0.02);

  int i=0, nr_points = (int) cloud_filtered->points.size ();
  while (cloud_filtered->points.size () > 0.3 * nr_points)
  {
    // Segment the largest planar component from the remaining cloud
    seg.setInputCloud (cloud_filtered);
    seg.segment (*inliers, *coefficients);
    if (inliers->indices.size () == 0)
    {
      std::cout << "Could not estimate a planar model for the given dataset." << std::endl;
      break;
    }

    // Extract the planar inliers from the input cloud 提取平面点云
    pcl::ExtractIndices<pcl::PointXYZ> extract;
    extract.setInputCloud (cloud_filtered);
    extract.setIndices (inliers);
    extract.setNegative (false);

    // Write the planar inliers to disk
    extract.filter (*cloud_plane);
    std::cout << "PointCloud representing the planar component: " << cloud_plane->points.size () << " data points." << std::endl;

    // Remove the planar inliers, extract the rest 移去平面局内点，提取剩余点云
    extract.setNegative (true);
    extract.filter (*cloud_f);
    cloud_filtered = cloud_f;
  }

  // Creating the KdTree object for the search method of the extraction 点云提取
  //为提取点云时使用的搜索对象利用输入点云cloud_filtered创建Kd树对象tree
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);  
  tree->setInputCloud (cloud_filtered);//创建点云索引向量，用于存储实际的点云信息

  //创建一个点云索引向量cluster_indices，用于存储检测到的点云聚类的点云索引信息，如cluster_indices[0]包含点云中第一个聚类中的点集的所有索引。
  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;  //创建一个点云类型为PointXYZ的欧氏聚类对象
  //设置提取的参数和变量
  ec.setClusterTolerance (0.02); // 设置近邻搜索的搜索半径为2cm
  ec.setMinClusterSize (100);  //设置一个聚类所需要的最小点数目为100
  ec.setMaxClusterSize (25000);  //设置一个聚类需要的最大点数目为25000
  ec.setSearchMethod (tree);  //设置点云的搜索机制
  ec.setInputCloud (cloud_filtered);
  ec.extract (cluster_indices);  //从点云中提取聚类，并将点云索引保存在cluster_indices中

  //迭代访问点云索引cluster_indices，直到分割出所有聚类
  int j = 0;
  for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
  {
    //创建新的点云数据集，将所有当前聚类写入到点云数据集中
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster (new pcl::PointCloud<pcl::PointXYZ>);
    for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); pit++)
      cloud_cluster->points.push_back (cloud_filtered->points[*pit]); //*
    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 << "cloud_cluster_" << j << ".pcd";
    writer.write<pcl::PointXYZ> (ss.str (), *cloud_cluster, false); //*
    j++;
  }

  return (0);
}

./cluster_extraction 

PointCloud before filtering has: 460400 data points.
PointCloud after filtering has: 41049 data points.
PointCloud representing the planar component: 20536 data points.
PointCloud representing the planar component: 12442 data points.
PointCloud representing the Cluster: 4857 data points.
PointCloud representing the Cluster: 1386 data points.
PointCloud representing the Cluster: 321 data points.
PointCloud representing the Cluster: 291 data points.
PointCloud representing the Cluster: 123 data points.


分割结果如下：




资源链接：点云分割入门级实例学习
参考：

《点云库PCL从入门到精通》第十二章入门级实例解析。

我是一个标题
代码解析
地面分割
欧式聚类
结果可视化
地面滤除
欧式聚类
遇到的问题
调用PCL库时出现segmentation fault(core dumped)错误
参考

代码解析
注:完整代码请点击这篇博客
地面分割
根据论文和开源代码进行了修改，主要实现过程是这样的



首先是对点云根据z值进行排序

    // 1. 复制一份点云
    pcl::PointCloud<VPoint> cloud_org(cloud_in);  // 复制一份点云
    // cout << "点云复制完成" << endl;

    // 2.Sort on Z-axis value
    sort(cloud_in.points.begin(), cloud_in.end(), point_cmp);  // 排序

// 用户排序的参数
bool point_cmp(VPoint a, VPoint b){
    return a.z<b.z;
}



对特别低的点进行滤除

    pcl::PointCloud<VPoint>::iterator it = cloud_in.points.begin();
    for (size_t i = 0; i < cloud_in.points.size(); i++) {    // 统计小于-1.5*sensor_height_的点数目
        if (cloud_in.points[i].z < -1.5 * sensor_height_) {
            it++;
        } else {
            break;
        }
    }
    cloud_in.points.erase(cloud_in.points.begin(), it);  // 清除 在地面下的点 ,因为之前是根据z排序了 所以很方便
    // std::cout<<"清除地面下的点后的点数: " <<cloud_in.points.size()<<std::endl;


提取大致的地面点

// 4. Extract init ground seeds.
    extract_initial_seeds_(cloud_in);  // 获得待拟合的地面点
    g_ground_pc = g_seeds_pc;   // 待拟合的地面点


// 获得待拟合的地面点
void
GroundFit::extract_initial_seeds_(const pcl::PointCloud<VPoint>& p_sorted)
{
    // LPR is the mean of low point representative
    double sum = 0;
    int cnt = 0;
    // Calculate the mean height value.
    for(size_t i=0;i<p_sorted.points.size() && cnt<num_lpr_;i++){  // 提取20个最低的点作为种子点
        sum += p_sorted.points[i].z;
        cnt++;
    }
    double lpr_height = cnt!=0?sum/cnt:0;// in case divide by 0   // 最低的20个种子点的平均值
    g_seeds_pc->clear();
    // iterate pointcloud, filter those height is less than lpr.height+th_seeds_   // 小于lpr_height + th_seeds_ 的点作为待拟合的地面点
    for(size_t i=0;i<p_sorted.points.size();i++){
        if(p_sorted.points[i].z < lpr_height + th_seeds_){
            g_seeds_pc->points.push_back(p_sorted.points[i]);
        } else {
            break;  // 因为是按顺序遍历的 所以可以直接退出循环
        }
    }
    // return seeds points  g_seeds_pc
}



循环拟合大致的地面点，

    // 5. Ground plane fitter mainloop
    for (int i = 0; i < num_iter_; i++) {    // 迭代3次

        // cout<<" -----------iter"<<"["<<i+1<<"]"<<"------------" <<endl;
        estimate_plane_();     // g_ground_pc 进行平面拟合 得到法向量normal_ 和 th_dist_d_
        g_ground_pc->clear();
        g_not_ground_pc->clear();

        //pointcloud to matrix
        MatrixXf points(cloud_org.points.size(), 3);
        int j = 0;
        for (auto p:cloud_org.points) {
            points.row(j++) << p.x, p.y, p.z;
        }

        // 得到所有点到平面的距离相关的 result
        // ground plane model
        VectorXf result = points * normal_;  // result=Ax+By+Cz
        // threshold filter
        for (int r = 0; r < result.rows(); r++) {
            if (result[r] < th_dist_d_) {      // 到拟合的平面的距离小于th_dist_的点 作为最后的地面点
                g_ground_pc->points.push_back(cloud_org[r]);
            } else {                          // 非地面点
                g_not_ground_pc->points.push_back(cloud_org[r]);
            }
        }

        // 每次迭代输出结果
        // cout<<"["<<i+1<<"]"<<" "<<"地面点: "<<g_ground_pc->points.size()<<", "<<"非地面点: "<<g_not_ground_pc->points.size()<<"\n\n\n"<<endl;
    }

将确定的地面点放到g_ground_pc中，然后再使用GroundFit::estimate_plane_()再计算得到新的大致的地面点，不断拟合下去。
// 更新拟合平面的A B C D
void
GroundFit::estimate_plane_()
{
    // Create covarian matrix in single pass.
    // TODO: compare the efficiency.
    Eigen::Matrix3f cov;
    Eigen::Vector4f pc_mean;   // 归一化坐标值
    pcl::computeMeanAndCovarianceMatrix(*g_ground_pc, cov, pc_mean); // 对地面点(最小的n个点)进行计算协方差和平均值
    // Singular Value Decomposition: SVD
    JacobiSVD<MatrixXf> svd(cov,Eigen::DecompositionOptions::ComputeFullU);
    // use the least singular vector as normal
    normal_ = (svd.matrixU().col(2));   // 取最小的特征值对应的特征向量作为法向量
    // cout<<"normal_ \n"<<normal_<<endl;
    // mean ground seeds value
    Eigen::Vector3f seeds_mean = pc_mean.head<3>();   // seeds_mean 地面点的平均值
    // cout<<"seeds_mean \n"<<seeds_mean<<endl;

    // according to normal.T*[x,y,z] = -d
    d_ = -(normal_.transpose()*seeds_mean)(0,0);  // 计算d   D=d
//    std::cout<<"d_: "<<d_<<std::endl;
    // set distance threhold to `th_dist - d`
    th_dist_d_ = th_dist_ - d_;   // ------------------------------? // 这里只考虑在拟合的平面上方的点 小于这个范围的点当做地面
//    std::cout<<"th_dist_d_=th_dist_ - d_ : "<<th_dist_d_<<std::endl;

    // return the equation parameters

}


欧式聚类
这一块比较简单，可以参考原博主，主要是为了解决激光点的稀疏性不同的问题，将原始点云划分为5个范围，分别根据不同的阈值进行欧式聚类
结果可视化
地面滤除


代码实现:https://github.com/VincentCheungM/Run_based_segmentation


论文：Fast Segmentation of 3D Point Clouds: A Paradigm on LiDAR Data for Autonomous Vehicle Applications




欧式聚类



遇到的问题
调用PCL库时出现segmentation fault(core dumped)错误
当我在欧式聚类中设置KdTree时,
        tree->setInputCloud(cloud_2d);

这句话会导致segmentation fault(core dumped)这个错误.

最终我把pcl使用的1.7的版本,就没报错了.
参考:

C+11编译调用PCL库时出现segmentation fault(core dumped)错误

ROS初学笔记 - C++11与PCL库冲突问题

用了pcl的地方， 程序直接崩溃 挂掉

PCL：1.7.2使用时的一个问题（core dumped与-std=c++11)

Segfault on app start using Trusty Beta with packaged PCL
参考
点云地面检测算法
无人驾驶汽车系统入门（二十五）——基于欧几里德聚类的激光雷达点云分割及ROS实现