无人驾驶汽车系统入门(三十一)——点云分割和聚类算法详解
本篇详细讲解点云处理中的基本分割和聚类的算法原理。
Lidar基本常识
lidar的分辨率要高于radar,但是radar可以直接测量目标的速度。通过融合两者,可以获得对目标较好的位置和速度估计。
激光雷达坐标系:右手法则,大拇指朝上为z,食指朝前为x,中指朝左为y, lidar的解析度很大程度上取决于线数,其解析度指标分为横向解析度和纵向解析度,横向解析度即一条激光束扫描一圈(360度)的反射点的数量,作为参考,Velodyne的16线激光雷达的横向解析度为1800个反射点,即:
360 1800 = 0.2 d e g r e e / p o i n t \frac{360}{1800} = 0.2 \ degree/point 1800360=0.2 degree/point
也就是说这一款雷达横向上每0.2度有一个反射点。纵向解析度即雷达的线束数量,线束越多解析度越高。
常用的point cloud处理工具:
- PCL: 主要用于点云滤波、分割和聚类
- OpenCV
- ROS:提供了一些点云数据表示、消息格式、坐标变化、可视化工具
- Eigen:线性运算库,可用于描述各类变换,通常用于点云的坐标变换(点云SLAM里用的比较多)
传统的点云感知算法处理流程:
- 点云滤波:降采样点云以加速后续计算
- 点云分割:分别提取出地面点和非地面点
- 点云聚类:将非地面点通过空间关系聚类成点云簇
- 目标轮廓拟合:使用设定形状(如bounding box)将点云簇拟合
本文详细讲解基础的点云分割和点云聚类算法。
点云滤波和降采样
为什么做滤波(filter)和降采样?
滤波是点云预处理的步骤,可以显著减低后续处理的计算复杂度,有些滤波方法还能够滤除点云中的噪点。PCL提供多种点云滤波方法的实现,其中应用最多的是体素滤波:Voxel grid filter. 体素滤波即将空间按照一定尺寸(比如说 1 m × 1 m × 1 m 1m \times 1m \times 1m 1m×1m×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 ) } P_n: \{(x_0, y_0), (x_1, y_1), ... , (x_n, y_n)\} Pn:{(x0,y0),(x1,y1),...,(xn,yn)},要求得一条直线 L : a x + b y + c = 0 L: ax+by+c=0 L:ax+by+c=0 使得拟合尽可能多的点,也就是 n n n 个点到直线 L L L 的距离和最短;RANSAC的做法是通过一定的迭代次数(比如1000次),在每次迭代中随机在点集 P n P_n Pn 中选取两个点 ( x 1 , y 1 ) , ( x 2 , y 2 ) (x1, y1), (x2, y2) (x1,y1),(x2,y2),计算 a , b , c a, b, c a,b,c:
a = y 1 − y 2 , b = x 2 − x 1 , c = x 1 ∗ y 2 − x 2 ∗ y 1 a = y1 - y2, \\ b = x2 - x1, \\ c = x1*y2 -x2*y1 a=y1−y2,b=x2−x1,c=x1∗y2−x2∗y1
接着遍历 P n P_n Pn 每一个点 ( x i , y i ) (x_i, y_i) (xi,yi)到直线 L L L 的距离 d i s t dist dist,
d i s t = f a b s ( a ∗ x i + b ∗ y i + c ) s q r t ( a ∗ a + b ∗ b ) dist = \frac{fabs(a * x_i + b * y_i + c)}{sqrt(a*a + b*b)} dist=sqrt(a∗a+b∗b)fabs(a∗xi+b∗yi+c)
当 d i s t dist dist小于我们给定的一个距离阈值 D t h r e s h o l d D_{threshold} Dthreshold,则认为点在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)\} {(7,2),(5,4),(9,6),(4,7),(8,1),(2,3)}
往kd树中插入一个点 ( 7 , 2 ) (7,2) (7,2),在x方向对平面进行划分如下(横轴为x,纵轴为y):
接着依次插入第二个和第三个点,第二个点是 ( 5 , 4 ) (5, 4) (5,4), 5小于7所有该点在左边, ( 9 , 6 ) (9, 6) (9,6)的 x x x大于7所以在右边,基于y对空间进行第二次划分(图中蓝线):
2D KD树的划分顺序为: x->y->x->y…, 所以到第四个点 ( 4 , 7 ) (4, 7) (4,7), 其 x x x小于 ( 7 , 2 ) (7, 2) (7,2)所以在根节点的左边, y y y大于 ( 5 , 4 ) (5, 4) (5,4)的 y y y所以在它的右边,依次类推得到对二维空间的划分图:
构造的树如下:
因为按区域进行了划分,kd树显著的降低了搜索的步数,以上图为例,假定我们聚类的距离阈值为4,要搜索出点 ( 2 , 3 ) (2,3) (2,3)的距离阈值内的点,先从根节点 ( 7 , 2 ) (7, 2) (7,2)开始,算得距离大于4,类似于插入点的流程,查找左边的点 ( 5 , 4 ) (5, 4) (5,4), 算得距离小于4,得到一个近距离点,接着搜索,虽然 ( 2 , 3 ) (2,3) (2,3)在 ( 5 , 4 ) (5,4) (5,4)左侧,但是为其本身,且无叶子节点,所以左侧搜索结束,看 ( 5 , 4 ) (5,4) (5,4)右侧节点,计算得 ( 4 , 7 ) (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