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

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

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

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里用的比较多)

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

  1. 点云滤波:降采样点云以加速后续计算
  2. 点云分割:分别提取出地面点和非地面点
  3. 点云聚类:将非地面点通过空间关系聚类成点云簇
  4. 目标轮廓拟合:使用设定形状(如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=y1y2,b=x2x1,c=x1y2x2y1

接着遍历 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(aa+bb)fabs(axi+byi+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

AdamShan CSDN认证博客专家 图像处理 深度学习 TensorFlow
奔驰高级自动驾驶扫地僧,谷歌认证机器学习专家,兰州大学无人驾驶团队创始人,主攻深度学习,无人驾驶汽车方向,著有《无人驾驶原理与实践》一书。
已标记关键词 清除标记
相关推荐
©️2020 CSDN 皮肤主题: 大白 设计师:CSDN官方博客 返回首页
实付 29.90元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、C币套餐、付费专栏及课程。

余额充值