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

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

Lidar基本常识

lidar的分辨率要高于radar，但是radar可以直接测量目标的速度。通过融合两者，可以获得对目标较好的位置和速度估计。

激光雷达坐标系：右手法则，大拇指朝上为z，食指朝前为x，中指朝左为y, lidar的解析度很大程度上取决于线数，其解析度指标分为横向解析度和纵向解析度，横向解析度即一条激光束扫描一圈（360度）的反射点的数量，作为参考，Velodyne的16线激光雷达的横向解析度为1800个反射点，即：

$$\frac{360}{1800}=0.2degree/point$$

也就是说这一款雷达横向上每0.2度有一个反射点。纵向解析度即雷达的线束数量，线束越多解析度越高。

常用的point cloud处理工具：

• PCL: 主要用于点云滤波、分割和聚类
• OpenCV
• ROS：提供了一些点云数据表示、消息格式、坐标变化、可视化工具
• Eigen：线性运算库，可用于描述各类变换，通常用于点云的坐标变换（点云SLAM里用的比较多）

传统的点云感知算法处理流程：

1. 点云滤波：降采样点云以加速后续计算
2. 点云分割：分别提取出地面点和非地面点
3. 点云聚类：将非地面点通过空间关系聚类成点云簇
4. 目标轮廓拟合：使用设定形状（如bounding box）将点云簇拟合

本文详细讲解基础的点云分割和点云聚类算法。

点云滤波和降采样

为什么做滤波(filter)和降采样？

滤波是点云预处理的步骤，可以显著减低后续处理的计算复杂度，有些滤波方法还能够滤除点云中的噪点。PCL提供多种点云滤波方法的实现，其中应用最多的是体素滤波：Voxel grid filter. 体素滤波即将空间按照一定尺寸（比如说$1m\times 1m\times 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, 通过随机采样和迭代的方法用一种模型（比如说直线模型、平面模型）拟合一群数据，在点云处理中通常用于点云数据的分割，以最简单的直线拟合为例：

给定平面内若干个点的集合Pn:{(x0,y0),(x1,y1),...,(xn,yn)}P_n: \{(x_0, y_0), (x_1, y_1), ... , (x_n, y_n)\}Pn​:{(x0​,y0​),(x1​,y1​),...,(xn​,yn​)}，要求得一条直线 $L:ax+by+c=0$ 使得拟合尽可能多的点，也就是 $n$ 个点到直线 $L$ 的距离和最短；RANSAC的做法是通过一定的迭代次数(比如1000次)，在每次迭代中随机在点集 $P_n$ 中选取两个点 $(x1,y1),(x2,y2)$，计算 $a,b,c$:

$$\begin{gathered} a=y1-y2, \\ b=x2-x1, \\ c=x1\ast y2-x2\ast y1 \end{gathered}$$

接着遍历 $P_n$ 每一个点$(x_i,y_i)$到直线 $L$ 的距离 $dist$,

$$dist=\frac{fabs(a\ast x_i+b\ast y_i+c)}{sqrt(a\ast a+b\ast b)}$$

当$dist$小于我们给定的一个距离阈值 $D_{threshold}$，则认为点在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)$,在x方向对平面进行划分如下（横轴为x，纵轴为y）：

接着依次插入第二个和第三个点，第二个点是$(5,4)$, 5小于7所有该点在左边，$(9,6)$的$x$大于7所以在右边，基于y对空间进行第二次划分（图中蓝线）：

2D KD树的划分顺序为: x->y->x->y…, 所以到第四个点$(4,7)$, 其$x$小于$(7,2)$所以在根节点的左边， $y$大于$(5,4)$的$y$所以在它的右边，依次类推得到对二维空间的划分图：

构造的树如下：

因为按区域进行了划分，kd树显著的降低了搜索的步数，以上图为例，假定我们聚类的距离阈值为4，要搜索出点$(2,3)$的距离阈值内的点，先从根节点$(7,2)$开始,算得距离大于4，类似于插入点的流程，查找左边的点$(5,4)$， 算得距离小于4，得到一个近距离点，接着搜索，虽然$(2,3)$在$(5,4)$左侧，但是为其本身，且无叶子节点，所以左侧搜索结束，看$(5,4)$右侧节点，计算得$(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