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

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

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

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

Lidar点云分割聚类算法是一种利用激光雷达扫描数据进行地物分类和分析的算法。激光雷达通过发射激光束并接收反射回来的信号,可以获取到地物的三维坐标信息。而激光雷达扫描得到的原始数据是一组离散点的信息,需要进行分割聚类处理才能得到有意义的结果。 点云分割算法的主要目标是将原始点云数据分割成不同的地物部分。常用的分割算法有基于几何特征和基于特征提取的方法。基于几何特征的算法主要依靠点云中点的相邻关系进行分割,比如根据点间距、法线方向等信息来判断是否属于同一地物。而基于特征提取的算法则通过对点云进行特征提取,比如曲率、形状描述子等,根据不同特征之间的相似性进行聚类,从而实现点云分割点云聚类算法则是对分割后的点云进行进一步的聚类,将属于同一地物的点划分为一个簇。聚类算法常用的方法包括基于密度和基于连通关系的方法。基于密度的聚类算法通过确定点的密度来判断是否属于同一簇,如DBSCAN算法。而基于连通关系的聚类算法则依靠点之间的连通关系进行划分,比如基于区域生长的算法。 通过点云分割聚类算法,可以有效地提取地物的相关信息,如建筑物、树木、道路等,为后续的地物识别、地物分类、场景分析等应用提供有价值的数据基础。
评论 5
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

AdamShan

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

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

余额充值