激光点云目标物聚类

2021/5/16 10:28:25

编程Tag： Cluster vector 聚类 point std 点云 激光 物聚类

本文主要是介绍激光点云目标物聚类，对大家解决编程问题具有一定的参考价值，需要的程序猿们随着小编来一起学习吧！

Lidar系列文章

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

系列文章目录

1. 激光雷达基本介绍
2. 激光点云数据显示
3. 基于RANSAC的激光点云分割
4. 点云分割入门级实例学习

文章目录

    • Lidar系列文章
    • 欧氏聚类
    • KD-Tree
    • KD-Tree中搜索点
    • 欧氏聚类实现
    • 使用PCL进行欧氏聚类
    • Bounding Boxes边界框
    • PCA Boxes

目前我已经实现点云分割，分离出障碍物点云。如果可以将这些障碍物进行区分，分离出同一物体反射的点云，则将会对我们的多目标跟踪（车辆，行人，自行车等）非常有帮助。本节主要介绍激光点云聚类算法。

欧氏聚类

聚类：分离出属于同一障碍物的激光点云，比如车辆，行人等；
欧氏聚类：Euclidean clustering，根据点之间的距离进行分组关联；
KD-Tree：通过将搜索空间分块并将点云分类进不同的区域，减小了不相邻点云间的距离计算，KD-Tree数据结构通常可以将查找时间从O(n)缩短到O(log(n))，实现快速的最近邻搜索（nearest neighbors search）。

KD-Tree

KD-Tree是一种以二叉树形式表示的数据结构，在k维空间（如三维空间X, Y , Z维度）中进行维度切换对比将点插入二叉树进行存储，以便对其进行快速检索。通过区域分割来划分区域，使用欧氏聚类等算法进行聚类时最近邻搜索将极大加速。
我们将以下图包含11点的2D点云聚类进行介绍，可以看到包含3组距离较近的点。
在这里插入图片描述
首先需要将点插入KD-Tree中，主要步骤如下：

  1. 插入任一点（-6.2,7）作为树的根节点，分割x方向，下图绿线所示；
    在这里插入图片描述
  2. 插入下一点 (-6.3, 8.4)，如果其x值小于根节点，插入根节点左侧，否则根节点右侧；该点分割y区域，下图红线所示；
    在这里插入图片描述
  3. 继续插入另外两点(-5.2, 7.1) ，(-5.7, 6.3)，重新分割x区域。 在这里插入图片描述
    这里用于对比的（x，y）维度是根据其深度进行判断。
    在这里插入图片描述
    下图是通过直线分割好的KD-Tree，绿色划分x轴区域，红色划分y轴区域。
    在这里插入图片描述
    为改善KD-Tree的结构，可以通过X/Y交替插入中值点的方法，使树结构更加均匀，缩短搜索时间。
    以下是KD-Tree点插入的代码实现。这里通过递归调用insertHelper函数实现插入点及更新树的节点。
	void insertHelper(Node** node, unsigned int depth, std::vector<float> point, int id)
	{
		//Tree is empty
		if(*node == NULL)
			*node = new Node(point,id);
		else
		{
			//Calculate current dim
			uint cd = depth%2; //use 3 if we are working in 3 dimension(x,y,z) kdtree
			if(point[cd]<((*node)->point[cd]))
				insertHelper(&((*node)->left),depth+1,point,id);
			else
				insertHelper(&((*node)->right),depth+1,point,id);
		}
	}

	void insert(std::vector<float> point, int id)
	{
		// TODO: Fill in this function to insert a new point into the tree
		// the function should create a new node and place correctly with in the root 
		insertHelper(&root,0,point,id);
	}

KD-Tree中搜索点

所有点插入到KD-Tree中后，下一步是通过与给定的目标对比搜索最近的点。距离目标点给定距离distanceTol（目标点附近2*distanceTol区域）的点可视为可能的临近点，再计算其距离判定是否作为近邻点。KD-Tree通过将区域进行分割可以忽略部分区域的点，加快最近邻搜索过程。
在这里插入图片描述
具体实现如下：

	void searchHelper(std::vector<float> target, Node* node,int depth, float distanceTol, std::vector<int>& ids)
	{
		if(node!=NULL)
		{	
			//Below check 3 dimensions if we are working in 3 dimension(x,y,z) kdtree
			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 distance = sqrt(pow((node->point[0]-target[0]),2)+pow((node->point[1]-target[1]),2));
				//this calculation change a little bit in a 3d environment.
				
				if(distanceTol<=distanceTol)
					ids.push_back(node->id);
			}

			//check accross boundary
			if((target[depth%2]-distanceTol)<node->point[depth%2])
				searchHelper(target,node->left,depth+1,distanceTol,ids);
			if((target[depth%2]+distanceTol)>node->point[depth%2])
				searchHelper(target,node->right,depth+1,distanceTol,ids);
		}
	}

	// 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;
		searchHelper(target,root,0,distanceTol,ids);
		return ids;
	}

欧氏聚类实现

使用KD-Tree进行聚类的伪代码：

Proximity(point,cluster):
    mark point as processed
    add point to cluster
    nearby points = tree(point)
    Iterate through each nearby point
        If point has not been processed
            Proximity(cluster)

EuclideanCluster():
    list of clusters 
    Iterate through each point
        If point has not been processed
            Create cluster
            Proximity(point, cluster)
            cluster add clusters
    return clusters

void clusterHelper(int indice, const std::vector<std::vector<float>> points,std::vector<int>& cluster, std::vector<bool>& processed, KdTree* tree, float distanceTol)
{
  processed[indice] = true;  //mark that point as being processed
  cluster.push_back(indice); //push that point back into clusters

  //check which point are nearby to this indice
  std::vector<int> nearst = tree->search(points[indice],distanceTol);
  //iterate  through those nearby indices
  for(int id : nearst)
  {
    //if hasn't been processed yet
    if(!processed[id])
      clusterHelper(id, points, cluster, processed, tree, distanceTol);//recursive
  }
}

std::vector<std::vector<int>> euclideanCluster(const std::vector<std::vector<float>>& points, KdTree* tree, float distanceTol)
{

  // TODO: Fill out this function to return list of indices for each cluster
  //list of cluster indices
  std::vector<std::vector<int>> clusters; 
	
  //keep track of which points have been processed or not
  std::vector<bool> processed(points.size(),false);

  int i = 0; // point id
  // iterate through all points
  while(i<points.size())
  {
    // skip and move on to next point if have been processed
    if(processed[i])
    {
      i++;
      continue;
    }
    // not been processed, create a new cluster
    std::vector<int>cluster;
    clusterHelper(i,points,cluster,processed,tree,distanceTol);
    clusters.push_back(cluster);
    i++;
  } 
	return clusters;
}

在这里插入图片描述

使用PCL进行欧氏聚类

下面我们使用PCL内置的欧氏聚类函数。
欧氏聚类对象ec需要设定距离容差(setClusterTolerance)，该容差范围内的点将分为同一类；同时也需要设定最小点云数量（数量过少可能是噪音）和最大点云数量（过大可能存在点云重叠的情况）；同时也需要设定使用输入的障碍物点云生成的KD-Tree。
参考代码如下。

  // Creating the KdTree object for the search method of the extraction
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>);
  tree->setInputCloud (cloud_filtered);

  std::vector<pcl::PointIndices> cluster_indices;
  pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec;
  ec.setClusterTolerance (0.02); // 2cm
  ec.setMinClusterSize (100);
  ec.setMaxClusterSize (25000);
  ec.setSearchMethod (tree);
  ec.setInputCloud (cloud_filtered);
  ec.extract (cluster_indices);

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
    // Creating the KdTree object for the search method of the extraction
    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); 
    ec.setMinClusterSize (minSize);
    ec.setMaxClusterSize (maxSize);
    ec.setSearchMethod (tree);
    ec.setInputCloud (cloud);
    ec.extract (cluster_indices);

    for (pcl::PointIndices getIndices : cluster_indices)
    {
        typename pcl::PointCloud<PointT>::Ptr cloud_cluster (new pcl::PointCloud<PointT>);
        for (int index:getIndices.indices)
        {
            cloud_cluster->points.push_back (cloud->points[index]); //*
        }
        cloud_cluster->width = cloud_cluster->points.size ();
        cloud_cluster->height = 1;
        cloud_cluster->is_dense = true;

        clusters.push_back(cloud_cluster);
    }

    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;
}

以下代码实现不同点云显示：

std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudClusters = pointProcessor.Clustering(segmentCloud.first, 1.0, 3, 30);

int clusterId = 0;
std::vector<Color> colors = {Color(1,0,0), Color(0,1,0), Color(0,0,1)};

for(pcl::PointCloud<pcl::PointXYZ>::Ptr cluster : cloudClusters)
{
      std::cout << "cluster size ";
      pointProcessor.numPoints(cluster);
      renderPointCloud(viewer,cluster,"obstCloud"+std::to_string(clusterId),colors[clusterId]);
      ++clusterId;
}

显示如下：
在这里插入图片描述

Bounding Boxes边界框

目前我们已经理解点云如何实现聚类，并使用PCL库函数实现点云聚类，下面就需要使用边界框将点云框出来。边界框也可视为车辆占用的空间区域，此空间区域内车辆不能进入，否则可能产生碰撞。
在这里插入图片描述
BoundingBox函数的实现比较简单，查找聚类点云各维度的最小值最大值，作为边界框的参数。

template<typename PointT>
Box ProcessPointClouds<PointT>::BoundingBox(typename pcl::PointCloud<PointT>::Ptr cluster)
{
    // Find bounding box for one of the clusters
    PointT minPoint, maxPoint;
    pcl::getMinMax3D(*cluster, minPoint, maxPoint);

    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;

    return box;
}

BoundingBox函数的函数调用如下，即可实现上图的边界框显示。

Box box = pointProcessor->BoundingBox(cluster);
renderBox(viewer,box,clusterId);

PCA Boxes

以上我们看到的Bounding box，始终是x，y轴方向。如果大部分聚类的点是这个方向，这是可以的。但如果聚类点云是与x轴成45°角的长条矩形时，此时产生的Bounding BOX将不可避免的非常大，从而限制了车辆的可行驶区域，如下图左侧所示。解决办法是考虑绕Z轴的旋转。下图中使用的方法是PCA主成分分析。
在这里插入图片描述
资源地址：Clustering



这篇关于激光点云目标物聚类的文章就介绍到这儿，希望我们推荐的文章对大家有所帮助，也希望大家多多支持为之网！


相关编程文章

更多>
本站官方QQ
官方QQ:
23002807

沪ICP备2023033072号-1

     

扫一扫关注最新编程教程