无人驾驶传感器融合系列（二）——激光雷达点云的聚类原理及实现

无人驾驶传感器融合系列（二）——激光雷达点云的聚类原理及实现

本章摘要：在上一章，我们采用RANSAC算法分割出了地面点云，非地面点云。我们通常会对非地面点云进行进一步的分割，也就是对地面以上的障碍物的点云进行聚类，通过聚类，我们可以检测出障碍物的边缘，然后使用3维的Bounding Box将障碍物从三维点云中框出来。本章将讲解Euclidean 聚类算法、PCL实现，并对其所利用的基本的数据结构kd-tree进行讲解。

Euclidean 聚类效果展示：

欧几里德聚类（Euclidean Clustering），本质上是找到最近点，只要点与点之间的距离，在一定半径范围内就认为这些点属于一个聚类。那么上一章中，非地面点云聚类之后的效果如下：

通过代码来直观理解Euclidean聚类算法实现过程:

伪代码如下：

clusthelp(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
            clusthelp(cluster)

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

对应的C++代码实现如下：

//参数里面的 tree为kd-tree,里面存储了点云points.
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
	std::vector<std::vector<int>> clusters;
	std::vector<bool> processed (points.size(), false);

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

void clusthelp(int indice, const std::vector<std::vector<float>>& points, KdTree* tree, float distanceTol, std::vector<bool> &processed, std::vector<int> &cluster)
{	
	processed[indice] = true;
	cluster.push_back(indice);

	std::vector<int> rawclust = tree->search(points[indice],distanceTol); //利用kd-tree来搜索points[indice]点一定范围内（distanceTol）的点云。
	for(int k : rawclust)
	{
		if(!processed[k]) 
			clusthelp(k, points, tree, distanceTol, processed, cluster);
	}
}

Euclidean 聚类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)
{

    // 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);                   //通过kd-tree的方式搜索。
    ec.setInputCloud (cloud);
    ec.extract (cluster_indices);

    for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin (); it != cluster_indices.end (); ++it)
    {
        typename pcl::PointCloud<PointT>::Ptr cloud_cluster (new pcl::PointCloud<PointT>);
        for (std::vector<int>::const_iterator pit = it->indices.begin (); pit != it->indices.end (); ++pit)
            cloud_cluster->points.push_back (cloud->points[*pit]); 

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

kd-tree讲解

上面聚类算法中，用到了数据结构kd-tree，采用kd-tree主要是为了加快数据的搜索查找速度。关于kd-tree的基础原理讲解见这里。这里主要通过c++代码来直观的理解kd-tree如何实现二维数据的插入、搜索。

数据的插入

void inserthelper(Node** node, uint depth, std::vector<float> point, int id)
	{
		if(*node==NULL)
			*node = new Node(point, id);
		else
		{
			uint cd = depth % 2;
			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)
	{
		inserthelper(&root, 0, point, id);
	}

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

搜索目标点一点范围内的所有点。

从树根开始搜索，看点是否在目标点distanceTol所占的范围内，如果在范围内就将其加入到ids，如果不在则舍弃一半，然后接着往下搜索，采用同样的方式迭代，知道搜索完所有的点。

void searchhelp(std::vector<float> target, Node* node, int depth, float distanceTol, std::vector<int> &ids)
	{
		if(node!=NULL)
		{
			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 dist = sqrt(pow((node->point[0]-target[0]),2) + pow((node->point[1]-target[1]),2));
					if(dist<=distanceTol)
						ids.push_back(node->id);
				}
			if((target[depth%2]-distanceTol) < node->point[depth%2])
				searchhelp(target, node->left, depth+1, distanceTol, ids);
			if((target[depth%2]+distanceTol) > node->point[depth%2])
				searchhelp(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;
		searchhelp(target, root, 0, distanceTol, ids);
		return ids;
	}

给聚类添加Bounding Box框

对每一聚类，确定一个能够将点云簇包裹的三维Bounding Box，将其视为一个物体处理。这里采用，聚类点云中所有点的最小x, y, z值，最大x, y, z值来确定Bounding Box的外框边界。

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

struct Box
{
	float x_min;
	float y_min;
	float z_min;
	float x_max;
	float y_max;
	float z_max;
};

输出BoundingBox，在environment.cpp/ simpleHighway下做如下设置。

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

完整代码见：github

显示不同的输出结果见 sensors/ environment.cpp/ simpleHighway, 可以根据注释自己调整输出。