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

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

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: 主要用于点云滤波、分割和聚类OpenCVROS：提供了一些点云数据表示、消息格式、坐标变化、可视化工具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

---

1.本站遵循行业规范，任何转载的稿件都会明确标注作者和来源；2.本站的原创文章，会注明原创字样,如未注明都非原创，如有侵权请联系删除！；3.作者投稿可能会经我们编辑修改或补充；4.本站不提供任何储存功能只提供收集或者投稿人的网盘链接。