精华内容
下载资源
问答
  • 输入Velodyne Lidar数据,对点云进行聚类,基于Qt图像界面开发,算法满足实时性,分割效果好,可用于16线,32线,64线激光雷达数据.
  • 使用聚类技术从RGB,高光谱和LiDAR图像中分割树冠 数据集 培训输入(GeoTIFF格式) 37张RGB图像(320,320,3) 43张高光谱图像(80,80,420) 43张LiDAR图像(80,80) 43张LiDAR PointCloud 3D地图(20665,20665,20665...
  • 基于KD树聚类的机载LiDAR数据输电线提取方法,梁静,张继贤,论文提出并实现了一种基于kd树近邻域点云聚类法从LiDAR点云数据中自动提取多根电力线。首先利用高程直方图统计法去除地面点,接着�
  • 类似matlab矩阵功能的c代码 ...聚类结果基于聚类半径和聚类长宽比的剔除 聚类质心和真值物方值的匹配 各种预处理数据输出 ##bugs known## 聚类效率捉急 分布式?应该可以 但是软件对象是standalone版本的
  • 在第(3)实现了地面点与障碍物的分离,此部分要实现的是聚类聚类是指把不同物体的点云分别组合聚集起来, 从而能让你跟踪汽车, 行人等多个目标. 其中一种对点云数据进行分组和聚类的方法称为欧氏聚类。 欧式聚类...

    在第(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中插入4个点(-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可以直接测量目标的速度。通过融合两者,...

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

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

    Lidar基本常识

    lidar的分辨率要高于radar,但是radar可以直接测量目标的速度。通过融合两者,可以获得对目标较好的位置和速度估计。

    激光雷达坐标系:右手法则,大拇指朝上为z,食指朝前为x,中指朝左为y, lidar的解析度很大程度上取决于线数,其解析度指标分为横向解析度和纵向解析度,横向解析度即一条激光束扫描一圈(360度)的反射点的数量,作为参考,Velodyne的16线激光雷达的横向解析度为1800个反射点,即:

    3601800=0.2 degree/point\frac{360}{1800} = 0.2 \ degree/point

    也就是说这一款雷达横向上每0.2度有一个反射点。纵向解析度即雷达的线束数量,线束越多解析度越高。

    常用的point cloud处理工具:

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

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

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

    本文详细讲解基础的点云分割和点云聚类算法。

    点云滤波和降采样

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

    滤波是点云预处理的步骤,可以显著减低后续处理的计算复杂度,有些滤波方法还能够滤除点云中的噪点。PCL提供多种点云滤波方法的实现,其中应用最多的是体素滤波:Voxel grid filter. 体素滤波即将空间按照一定尺寸(比如说1m×1m×1m1m \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, 通过随机采样和迭代的方法用一种模型(比如说直线模型、平面模型)拟合一群数据,在点云处理中通常用于点云数据的分割,以最简单的直线拟合为例:

    给定平面内若干个点的集合Pn:{(x0,y0),(x1,y1),...,(xn,yn)}P_n: \{(x_0, y_0), (x_1, y_1), ... , (x_n, y_n)\},要求得一条直线 L:ax+by+c=0L: ax+by+c=0 使得拟合尽可能多的点,也就是 nn 个点到直线 LL 的距离和最短;RANSAC的做法是通过一定的迭代次数(比如1000次),在每次迭代中随机在点集 PnP_n 中选取两个点 (x1,y1),(x2,y2)(x1, y1), (x2, y2),计算 a,b,ca, b, c:

    a=y1y2,b=x2x1,c=x1y2x2y1 a = y1 - y2, \\ b = x2 - x1, \\ c = x1*y2 -x2*y1

    接着遍历 PnP_n 每一个点(xi,yi)(x_i, y_i)到直线 LL 的距离 distdist,

    dist=fabs(axi+byi+c)sqrt(aa+bb)dist = \frac{fabs(a * x_i + b * y_i + c)}{sqrt(a*a + b*b)}

    distdist小于我们给定的一个距离阈值 DthresholdD_{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)} \{(7, 2), (5, 4), (9, 6), (4, 7), (8, 1), (2, 3)\}

    往kd树中插入一个点(7,2)(7,2),在x方向对平面进行划分如下(横轴为x,纵轴为y):

    在这里插入图片描述

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

    在这里插入图片描述

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

    在这里插入图片描述

    构造的树如下:

    在这里插入图片描述

    因为按区域进行了划分,kd树显著的降低了搜索的步数,以上图为例,假定我们聚类的距离阈值为4,要搜索出点(2,3)(2,3)的距离阈值内的点,先从根节点(7,2)(7, 2)开始,算得距离大于4,类似于插入点的流程,查找左边的点(5,4)(5, 4), 算得距离小于4,得到一个近距离点,接着搜索,虽然(2,3)(2,3)(5,4)(5,4)左侧,但是为其本身,且无叶子节点,所以左侧搜索结束,看(5,4)(5,4)右侧节点,计算得(4,7)(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

    展开全文
  • 激光雷达点云聚类1

    2019-04-29 21:15:54
    激光雷达点云聚类1
  • 地面点云分割与欧式聚类.zip
  • lidar点云边缘线提取

    2015-04-15 09:54:07
    基于坡度和聚类的算法,提取lidar点云的地物边缘线。最终得到地物的轮廓
  • Lidar系列文章 传感器融合是将多个传感器采集的数据进行融合处理,以更好感知周围环境;...文章目录Lidar系列文章基于PCL实现欧式聚类提取 基于PCL实现欧式聚类提取 本节我们将介绍如何用PCL Euclid

    Lidar系列文章

    传感器融合是将多个传感器采集的数据进行融合处理,以更好感知周围环境;这里首先介绍激光雷达的相关内容,包括激光雷达基本介绍(本节内容),激光点云数据处理方法(点云数据显示,点云分割,点云聚类,障碍物识别实例)等。

    系列文章目录

    1. 激光雷达基本介绍
    2. 激光点云数据显示
    3. 基于RANSAC的激光点云分割
    4. 点云分割入门级实例学习
    5. 激光点云目标物聚类
    6. 基于PCL实现欧式聚类提取
    7. 激光雷达障碍物识别


    基于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从入门到精通》第十二章入门级实例解析。

    展开全文
  • 为了解决此问题,提出一种基于点云射线角度约束的改进欧氏聚类算法,使障碍物检测更加快速准确,所提算法有效解决了点云密度不均匀导致的检测障碍物成功率较低的问题,同时对所提算法进行实车实验。实验结果表明,与传统...
  • ros实现地面过滤+欧式聚类(boundingbox)

    千次阅读 2020-05-22 22:39:45
    这里写目录标题代码解析地面分割欧式聚类结果可视化地面滤除欧式聚类遇到的问题调用PCL库时出现segmentation fault(core ...论文:Fast Segmentation of 3D Point Clouds: A Paradigm on LiDAR Data for Autonomous V

    代码解析

    注:完整代码请点击这篇博客

    地面分割

    根据论文和开源代码进行了修改,主要实现过程是这样的
    在这里插入图片描述

    1. 首先是对点云根据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;
    }
    
    
    1. 对特别低的点进行滤除
        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;
    
    1. 提取大致的地面点
    // 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
    }
    
    
    1. 循环拟合大致的地面点,
        // 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实现

    展开全文
  • 激光点云目标物聚类

    2021-05-04 08:36:23
    文章目录系列文章目录前言欧氏聚类KD-TreeKD-Tree中搜索点使用PCL进行欧氏聚类 前言 目前我已经实现点云分割,分离出障碍物点云。如果可以将这些障碍物进行区分,分离出同一物体反射的点云,则将会对我们的多目标...
  • 无人驾驶汽车系统入门(二十五)——基于欧几里德聚类的激光雷达点云分割及ROS实现 上一篇文章中我们介绍了一种基于射线坡度阈值的地面分割方法,并且我们使用pcl_ros实现了一个简单的节点,在完成了点云的地面分割...
  • Lidar SLAMTraditional Lidar Traditional Lidar
  • Lidar SLAM

    2021-05-28 16:40:47
    LIO-SAM: Tightly-coupled Lidar Inertial Odometry via Smoothing and Mapping 文中提到,LIO-SAM可以实时,比LOAM/LIOM效果好,而且速度快。 基于因子图优化提出了一个紧耦合的激光雷达惯性里程计框架,...
  • LiDAR Pointcloud聚类/语义分割/平面提取 任务:道路/地面提取,平面提取,语义分割,开放集实例分割,聚类 3D点云的快速分割:自动驾驶汽车应用的LiDAR数据范例ICRA 2017 [ , ] 用于自动驾驶的时间序列LIDAR数据...
  • 分割聚类的细节

    2020-09-30 11:50:47
    4.BFS搜索分割、聚类 分割 队列的第一个点是满足angle,后续邻居点加入是看是否满足差值小于ground_remove_angle=5 聚类 队列的第一个点是遍历整个非地面点云图,访问过的加visited标记, 在bfs外面加一个label = ...
  • 1. DBSCAN聚类的基本原理 详细原理可以参考链接: https://www.cnblogs.com/pinard/p/6208966.html 这是找到的相对很详细的介绍了,此链接基本仍是周志华《机器学习》中的内容,不过这个链接更通俗一点,且算法流程...
  • vlp-16点云聚类(14)

    千次阅读 2018-12-31 17:28:48
    将分割后的地面上的点云进行聚类提取。 并以box的形式包围起来。 参考:https://blog.csdn.net/AdamShan/article/details/83015570   //xx...
  • 系列文章目录 ...本节我们将通过从2D图像检测获取到的ROI(Region of interests)来对激光点云进行聚类,避免错误聚类。基于此初步的3D点云聚类,我们可以提取最近点的一些信息,比如距离,激光点云数量,目
  • 在生成数字正射影像的基础上,首先利用K均值(K-means)聚类算法对影像进行聚类和图像增强,然后将增强后的影像和对应区域的点云数据进行融合,最后通过影像处理结果对机载LiDAR植被点云进行分类。选取某城市的机载LiDAR...
  • 自适应聚类 一种轻量级且精确的点云聚类方法(请检查分支以进一步... title = {Online learning for 3D LiDAR-based human detection: Experimental analysis of point cloud clustering and classification methods
  • 今天尝试跑一下欧式聚类,看看在路侧场景下的聚类效果。 【代码】 #include <stdlib.h> #include <boost/bind.hpp> #include <ros/ros.h> #include <sensor_msgs/PointCloud2.h> #...
  • 激光雷达——目标识别(滤波、分割、聚类,包围盒) 参考地址:https://github.com/williamhyin/SFND_Lidar_Obstacle_Detection 压缩包SFND_Lidar_Obstacle_Detection-master.zip在百度网盘
  • 提出了一种基于k平面聚类算法的新分类方法,该方法将建筑物屋顶的点云从机载光检测测距仪(LiDAR)获得。 在激光点聚类的操作中,将点云中激光点的3D坐标直接用作聚类对象。 根据所获得的聚类解生成簇中激光点的拟合...
  • 我们通常会对非地面点云进行进一步的分割,也就是对地面以上的障碍物的点云进行聚类,通过聚类,我们可以检测出障碍物的边缘,然后使用3维的Bounding Box将障碍物从三维点云中框出来。本章将讲解Euclidean 聚类算法...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 751
精华内容 300
关键字:

lidar聚类