无人驾驶传感器融合系列（二）——激光雷达点云的聚类原理及实现
本章摘要：在上一章，我们采用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, 可以根据注释自己调整输出。
386
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
