传感器融合是将多个传感器采集的数据进行融合处理,以更好感知周围环境;这里首先介绍激光雷达的相关内容,包括激光雷达基本介绍(本节内容),激光点云数据处理方法(点云数据显示,点云分割,点云聚类,障碍物识别实例)等。
系列文章目录
1. 激光雷达基本介绍
2. 激光点云数据显示
3. 基于RANSAC的激光点云分割
4. 点云分割入门级实例学习
目前我已经实现点云分割,分离出障碍物点云。如果可以将这些障碍物进行区分,分离出同一物体反射的点云,则将会对我们的多目标跟踪(车辆,行人,自行车等)非常有帮助。本节主要介绍激光点云聚类算法。
聚类:分离出属于同一障碍物的激光点云,比如车辆,行人等;
欧氏聚类:Euclidean clustering,根据点之间的距离进行分组关联;
KD-Tree:通过将搜索空间分块并将点云分类进不同的区域,减小了不相邻点云间的距离计算,KD-Tree数据结构通常可以将查找时间从O(n)缩短到O(log(n)),实现快速的最近邻搜索(nearest neighbors search)。
KD-Tree是一种以二叉树形式表示的数据结构,在k维空间(如三维空间X, Y , Z维度)中进行维度切换对比将点插入二叉树进行存储,以便对其进行快速检索。通过区域分割来划分区域,使用欧氏聚类等算法进行聚类时最近邻搜索将极大加速。
我们将以下图包含11点的2D点云聚类进行介绍,可以看到包含3组距离较近的点。
首先需要将点插入KD-Tree中,主要步骤如下:
insertHelper
函数实现插入点及更新树的节点。void insertHelper(Node** node, unsigned int depth, std::vector<float> point, int id) { //Tree is empty if(*node == NULL) *node = new Node(point,id); else { //Calculate current dim uint cd = depth%2; //use 3 if we are working in 3 dimension(x,y,z) kdtree if(point[cd]<((*node)->point[cd])) insertHelper(&((*node)->left),depth+1,point,id); else insertHelper(&((*node)->right),depth+1,point,id); } } void insert(std::vector<float> point, int id) { // TODO: Fill in this function to insert a new point into the tree // the function should create a new node and place correctly with in the root insertHelper(&root,0,point,id); }
所有点插入到KD-Tree中后,下一步是通过与给定的目标对比搜索最近的点。距离目标点给定距离distanceTol
(目标点附近2*distanceTol
区域)的点可视为可能的临近点,再计算其距离判定是否作为近邻点。KD-Tree通过将区域进行分割可以忽略部分区域的点,加快最近邻搜索过程。
具体实现如下:
void searchHelper(std::vector<float> target, Node* node,int depth, float distanceTol, std::vector<int>& ids) { if(node!=NULL) { //Below check 3 dimensions if we are working in 3 dimension(x,y,z) kdtree 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 distance = sqrt(pow((node->point[0]-target[0]),2)+pow((node->point[1]-target[1]),2)); //this calculation change a little bit in a 3d environment. if(distanceTol<=distanceTol) ids.push_back(node->id); } //check accross boundary if((target[depth%2]-distanceTol)<node->point[depth%2]) searchHelper(target,node->left,depth+1,distanceTol,ids); if((target[depth%2]+distanceTol)>node->point[depth%2]) searchHelper(target,node->right,depth+1,distanceTol,ids); } } // 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; searchHelper(target,root,0,distanceTol,ids); return ids; }
使用KD-Tree进行聚类的伪代码:
Proximity(point,cluster): mark point as processed add point to cluster nearby points = tree(point) Iterate through each nearby point If point has not been processed Proximity(cluster) EuclideanCluster(): list of clusters Iterate through each point If point has not been processed Create cluster Proximity(point, cluster) cluster add clusters return clusters
void clusterHelper(int indice, const std::vector<std::vector<float>> points,std::vector<int>& cluster, std::vector<bool>& processed, KdTree* tree, float distanceTol) { processed[indice] = true; //mark that point as being processed cluster.push_back(indice); //push that point back into clusters //check which point are nearby to this indice std::vector<int> nearst = tree->search(points[indice],distanceTol); //iterate through those nearby indices for(int id : nearst) { //if hasn't been processed yet if(!processed[id]) clusterHelper(id, points, cluster, processed, tree, distanceTol);//recursive } } std::vector<std::vector<int>> euclideanCluster(const std::vector<std::vector<float>>& points, KdTree* tree, float distanceTol) { // TODO: Fill out this function to return list of indices for each cluster //list of cluster indices std::vector<std::vector<int>> clusters; //keep track of which points have been processed or not std::vector<bool> processed(points.size(),false); int i = 0; // point id // iterate through all points while(i<points.size()) { // skip and move on to next point if have been processed if(processed[i]) { i++; continue; } // not been processed, create a new cluster std::vector<int>cluster; clusterHelper(i,points,cluster,processed,tree,distanceTol); clusters.push_back(cluster); i++; } return clusters; }
下面我们使用PCL内置的欧氏聚类函数。
欧氏聚类对象ec
需要设定距离容差(setClusterTolerance
),该容差范围内的点将分为同一类;同时也需要设定最小点云数量(数量过少可能是噪音)和最大点云数量(过大可能存在点云重叠的情况);同时也需要设定使用输入的障碍物点云生成的KD-Tree。
参考代码如下。
// Creating the KdTree object for the search method of the extraction pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ>); tree->setInputCloud (cloud_filtered); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<pcl::PointXYZ> ec; ec.setClusterTolerance (0.02); // 2cm ec.setMinClusterSize (100); ec.setMaxClusterSize (25000); ec.setSearchMethod (tree); ec.setInputCloud (cloud_filtered); ec.extract (cluster_indices);
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) { // Time clustering process auto startTime = std::chrono::steady_clock::now(); std::vector<typename pcl::PointCloud<PointT>::Ptr> clusters; // TODO:: Fill in the function to perform euclidean clustering to group detected obstacles // Creating the KdTree object for the search method of the extraction typename pcl::search::KdTree<PointT>::Ptr tree (new pcl::search::KdTree<PointT>); tree->setInputCloud (cloud); std::vector<pcl::PointIndices> cluster_indices; pcl::EuclideanClusterExtraction<PointT> ec; ec.setClusterTolerance (clusterTolerance); ec.setMinClusterSize (minSize); ec.setMaxClusterSize (maxSize); ec.setSearchMethod (tree); ec.setInputCloud (cloud); ec.extract (cluster_indices); for (pcl::PointIndices getIndices : cluster_indices) { typename pcl::PointCloud<PointT>::Ptr cloud_cluster (new pcl::PointCloud<PointT>); for (int index:getIndices.indices) { cloud_cluster->points.push_back (cloud->points[index]); //* } cloud_cluster->width = cloud_cluster->points.size (); cloud_cluster->height = 1; cloud_cluster->is_dense = true; clusters.push_back(cloud_cluster); } auto endTime = std::chrono::steady_clock::now(); auto elapsedTime = std::chrono::duration_cast<std::chrono::milliseconds>(endTime - startTime); std::cout << "clustering took " << elapsedTime.count() << " milliseconds and found " << clusters.size() << " clusters" << std::endl; return clusters; }
以下代码实现不同点云显示:
std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr> cloudClusters = pointProcessor.Clustering(segmentCloud.first, 1.0, 3, 30); int clusterId = 0; std::vector<Color> colors = {Color(1,0,0), Color(0,1,0), Color(0,0,1)}; for(pcl::PointCloud<pcl::PointXYZ>::Ptr cluster : cloudClusters) { std::cout << "cluster size "; pointProcessor.numPoints(cluster); renderPointCloud(viewer,cluster,"obstCloud"+std::to_string(clusterId),colors[clusterId]); ++clusterId; }
显示如下:
目前我们已经理解点云如何实现聚类,并使用PCL库函数实现点云聚类,下面就需要使用边界框将点云框出来。边界框也可视为车辆占用的空间区域,此空间区域内车辆不能进入,否则可能产生碰撞。
BoundingBox
函数的实现比较简单,查找聚类点云各维度的最小值最大值,作为边界框的参数。
template<typename PointT> Box ProcessPointClouds<PointT>::BoundingBox(typename pcl::PointCloud<PointT>::Ptr cluster) { // Find bounding box for one of the clusters PointT minPoint, maxPoint; pcl::getMinMax3D(*cluster, minPoint, maxPoint); Box box; box.x_min = minPoint.x; box.y_min = minPoint.y; box.z_min = minPoint.z; box.x_max = maxPoint.x; box.y_max = maxPoint.y; box.z_max = maxPoint.z; return box; }
BoundingBox
函数的函数调用如下,即可实现上图的边界框显示。
Box box = pointProcessor->BoundingBox(cluster); renderBox(viewer,box,clusterId);
以上我们看到的Bounding box,始终是x,y轴方向。如果大部分聚类的点是这个方向,这是可以的。但如果聚类点云是与x轴成45°角的长条矩形时,此时产生的Bounding BOX将不可避免的非常大,从而限制了车辆的可行驶区域,如下图左侧所示。解决办法是考虑绕Z轴的旋转。下图中使用的方法是PCA主成分分析。
资源地址:Clustering