点云数据传统处理算法综述
一、点云传统算法检测流程
这里主要介绍的是自动驾驶方向中传统算法在对点云数据的操作,传统方式的点云目标识别流程较为复杂,涉及算法较多。从点云接收到输出目标整个流程涉及多个环节与算法。
附赠自动驾驶最全的学习资料和量产经验:链接
可以看到,传统算法检测流程中包含两个主要模块,分别是 点云预处理 和 点云聚类算法 。
●点云预处理
点云预处理模块主要是去除不感兴趣的区域点云,只留下感兴趣区域的点云。因此,这里主要是过滤掉非 ROI 区域的点云,以此来为后面的聚类算法更加有效的生成目标,同时一定程度上降低聚类算法耗时。 点云数据预处理中涉及到的几个主要研究内容,即 相关滤波算法,地面与非地面点云分割、ROI区域点云获取 等。
●点云聚类算法
聚类就是按照某种特定的标准把一个数据集分割成不同的簇,使得同一个簇内的数据尽可能相似,不在同一个簇的数据差异尽可能大。聚类是一种无监督算法。 点云聚类就是在已经分割出地面的情况下,在预处理后点云中针对点云数据的空间性进行聚类。 点云聚类可分成在 俯视图X-Y平面的二维聚类,和 三维空间聚类。二维聚类速度快。但是依赖点云预处理的效果,因为二维方法丢弃了高度z轴信息。三维聚类,速度慢但是聚类的三维几何空间较为准确。 常用的聚类算法有 欧式聚类 与 DBSCAN聚类 等。
二、点云预处理
2.1 点云滤波算法
一般下面这几种情况需要进行点云滤波处理:
-
点云数据密度不规则需要平滑
-
因为遮挡等问题造成离群点需要去除
-
大量数据需要下采样
-
噪声数据需要去除
点云滤波算法有很多种,本节只对其中使用频率较高的 直通滤波 / 体素滤波 / 半径滤波 / 双边滤波 展开讨论。
2.1.1 点云下采样(体素滤波)
点云的数目越大,储存、操作都是个大问题,而通过按一定的规则从里面抽取有代表性的样本,可以代替原来的样本,节省计算开销。
PCL 库实现的 VoxelGrid 类通过输入的点云数据创建一个三维体素栅格(可把体素栅格想象为微小的空间三维立方体的集合),然后在每个体素(即,三维立方体)内,用 体素中所有点的重心来近似显示体素中其他点,这样该体素就内所有点就用一个重心点最终表示,对于所有体素处理后得到过滤后的点云。其实现代码如下:
void VoxelGrid_ApplyFilter(vector<Point3D>& InputCloudPoint,
vector<Point3D>& OutPointCloud,
float X_Voxel,
float Y_Voxel,
float Z_Voxel)
{
//先判断输入的点云是否为空
if (InputCloudPoint.size()==0)
{
cout << "输入点云为空!" << endl;
return;
}
//存放输入点云的最大与最小坐标
Array4f min_p, max_p;
GetMaxMin(InputCloudPoint, min_p, max_p); // 具体实现略过
Array4f inverse_leaf_size_;
inverse_leaf_size_.x = 1 / X_Voxel;
inverse_leaf_size_.y = 1 / Y_Voxel;
inverse_leaf_size_.z = 1 / Z_Voxel;
inverse_leaf_size_.C = 1;
//计算最小和最大边界框值
Array4f min_b_, max_b_, div_b_, divb_mul_;
min_b_.x = (int)(floor(min_p.x * inverse_leaf_size_.x));
max_b_.x = (int)(floor(max_p.x * inverse_leaf_size_.x));
min_b_.y = (int)(floor(min_p.y * inverse_leaf_size_.y));
max_b_.y = (int)(floor(max_p.y * inverse_leaf_size_.y));
min_b_.z = (int)(floor(min_p.z * inverse_leaf_size_.z));
max_b_.z = (int)(floor(max_p.z * inverse_leaf_size_.z));
//计算沿所有轴所需的分割数
div_b_.x = max_b_.x - min_b_.x + 1;
div_b_.y = max_b_.y - min_b_.y + 1;
div_b_.z = max_b_.z - min_b_.z + 1;
div_b_.C= 0;
//设置除法乘数
divb_mul_.x = 1;
divb_mul_.y = div_b_.x;
divb_mul_.z =div_b_.x * div_b_.y;
divb_mul_.C = 0;
//用于计算 idx 和 pointcloud 索引的存储,
//cloud_point_index_idx 是个自定义结构体,本文没列出来
//主要包含两个元素: grid_idx 和 point_idx
std::vector<cloud_point_index_idx> index_vector;
index_vector.reserve(InputCloudPoint.size());
//第一步:遍历所有点并将它们插入到具有计算 idx 的 index_vector 向量中
//具有相同idx值的点将有助于产生 CloudPoint 的相同点
for (int i = 0; i < InputCloudPoint.size();i++)
{
int ijk0 = (int)(floor(InputCloudPoint[i].x * inverse_leaf_size_.x) -
static_cast<float> (min_b_.x));
int ijk1 = (int)(floor(InputCloudPoint[i].y * inverse_leaf_size_.y) -
static_cast<float> (min_b_.y));
int ijk2 = (int)(floor(InputCloudPoint[i].z * inverse_leaf_size_.z) -
static_cast<float> (min_b_.z));
//计算质心叶索引
int idx = ijk0 * divb_mul_.x +
ijk1 * divb_mul_.y +
ijk2 * divb_mul_.z;
index_vector.push_back(cloud_point_index_idx(idx, i));
}
//第二步:使用表示目标单元格的值作为索引对index_vector向量进行排序;
//实际上属于同一输出单元格的所有点都将彼此相邻
std::sort(index_vector.begin(), index_vector.end(), std::less<cloud_point_index_idx>());
//第三步:计数输出单元格,我们需要跳过所有相同的,相邻的 idx 值
int total = 0;
int index = 0;
int min_points_per_voxel_ = 0;
std::vector<std::pair<int, int>> first_and_last_indices_vector;
first_and_last_indices_vector.reserve(index_vector.size());
while (index < index_vector.size())
{
int i = index + 1;
while (i < index_vector.size() && index_vector[i].idx == index_vector[index].idx)
++i;
if (i - index >= min_points_per_voxel_)
{
++total;
first_and_last_indices_vector.push_back(std::pair<int, int>(index, i));
}
index = i;
}
//第四步:计算质心,将它们插入最终位置
//OutPointCloud.resize(total); //给输出点云分配内存空间
float x_Sum, y_Sum, z_Sum;
Point3D PointCloud;
int first_index, last_index;
for (int cp = 0; cp < first_and_last_indices_vector.size(); ++cp)
{
// 计算质心 - 来自所有输入点的和值,这些值在index_vector数组中具有相同的idx值
first_index = first_and_last_indices_vector[cp].first;
last_index = first_and_last_indices_vector[cp].second;
x_Sum = y_Sum = z_Sum = 0;
for (int li = first_index; li < last_index; ++li)
{
x_Sum += InputCloudPoint[index_vector[li].cloud_point_index].x;
y_Sum += InputCloudPoint[index_vector[li].cloud_point_index].y;
z_Sum += InputCloudPoint[index_vector[li].cloud_point_index].z;
}
PointCloud.x = x_Sum / (last_index - first_index);
PointCloud.y = y_Sum / (last_index - first_index);
PointCloud.z = z_Sum / (last_index - first_index);
OutPointCloud.push_back(PointCloud);
}
}
下面是 原始点云图片 和 经过下采样后的点云图片:
2.1.2 去除点云的离群点
由于设备精度、环境因素等影响,点云数据中出现一些噪声点,属于随机误差。此外,外界干扰如遮挡,障碍物等因素的影响,往往导致一些距离主题点云较远的离散点,即离群点。
对于这种离群点,一般有两种常用的滤波方法,即基于邻域统计的方法和基于半径点数的方法,在 PCL 库中对应的类是 StatisticalOutlierRemoval 和 RadiusOutlierRemoval(半径滤波)。
StatisticalOutlierRemoval
顾名思义,使用统计分析技术,从一个点云数据中集中移除测量噪声点。对每个点的邻域进行统计分析,剔除不符合一定标准的邻域点。具体来说:
-
对于每个点,计算它到最近 k 个邻域的平均距离。假设得到的分布是高斯分布,我们可以计算出一个均值 μ 和一个标准差 σ。
-
这个邻域点集中所有点与其邻域距离大于 μ + std_mul * σ 区间之外的点都可以被视为离群点,并可从点云数据中去除。std_mul 是标准差倍数的一个阈值,可以自己指定。
RadiusOutlierRemoval
在点云数据中,设定每个点一定半径范围内周围至少有足够多的近邻,不满足就会被删除。如指定参数半径 d,然后指定该半径内至少有 k 个邻居,则近邻点数量小于 k 的点即为离群点。
PCL 库中的使用方法如下,其具体实现不再展开。
pcl::RadiusOutlierRemoval<pcl::PointXYZ> outrem;
outrem.setInputCloud(pointCloud_raw);
outrem.setRadiusSearch(0.02); //设置在0.02半径的范围内找邻近点
outrem.setMinNeighborsInRadius(5); //设置查询点的邻近点集数小于5的删除
outrem.filter(*pointCloud_filter);
2.1.3 滤波算法
2.1.3.1 直通滤波
直通滤波算作最为简单、粗暴的一种滤波方式,就是直接对点云的 X、Y、Z 轴的点云坐标约束来进行滤波,可以约束只在 Z 轴,或者 XYZ 三个坐标轴共同约束来达到点云滤波效果。PCL 库中的使用方法如下,其具体实现不再展开。
pcl::PassThrough<pcl::PointXYZ> pass;
pass.setInputCloud(cloud); // 传入点云数据
pass.setFilterFieldName("z"); // 设置操作的坐标轴
pass.setFilterLimits(0.0f, 3.0f); // 设置坐标范围
pass.filter(*cloud_filtered); // 进行滤波输出
2.1.3.2 双边滤波
双边滤波(Bilateral filter)是一种非线性的滤波方法,是结合空间邻近度和像素值相似度的一种折中处理,同时考虑空域信息和灰度相似性,达到保边去噪的目的。具有简单、非迭代、局部的特点 。点云的双边滤波与图像的双边滤波作用类似,都是剔除噪声点的同时能够很好的保留原有特征,包括边缘、角点信息等。
首先看看图像的双边滤波算法,该算法的假设是图像边缘、角点的高频信息要远高于噪声的高频信息,可以说在一块区域内,边缘的平均高频信息远大于带有噪声的平坦区域,跳出该假设,噪声滤除效果不好。
简单来说双边滤波计算卷积内两个权值,一个用于空间域核(高斯滤波),一个用于像素域核,如果卷积在边缘,那么作用于像素域核的权值大于空间域核,可以保留边缘信息,反之,空间域核的权值更大,高斯滤波做主导,用于平滑。
所以对于点云来说,双边滤波是提取邻近采样点(用到k-d tree(后面介绍),八叉树做搜索,减少算法时间复杂度)计算两个权值进行滤波。
注意:
-
能使用双边滤波的点云必须得包含强度字段。在 PCL 库现有的 points 类型中,只有 PointXYZI 和 PointXYZINormal 有强度信息。
-
PCL 中实现的双边滤波 FastBilateralFilter 只适用于有序点云,且运算速度比较慢,不适用于较大的点云。
2.2 点云分割算法
2.2.1 地面点云分割
过滤地面是激光雷达感知中一步基础的预处理操作,因为我们环境感知通常只对路面上的障碍物感兴趣,且地面的点对于障碍物聚类容易产生影响。
目前已知的地面点云分割大致思路有如下几个:
-
基于几何特征提取方法
-
利用 RANSAC 算法
-
基于深度学习的方法
2.2.1.1 基于几何特征提取方法
传统的几何特征基于假设是地面点云所构成的地面为平面,而不是弧面,当然对于有倾斜角度的地面也是可以先通过水平面校准然后再进行后处理来达到目标。下面是常用的几种几何特征:
水平面校准
水平面校准顾名思义就是通过找到地面点所在平面,然后进行校准点云的方法。通过此步可将数据采集阶段,采集的地面点云相对于激光雷达 z 轴不平行校准为与之平行。目的是为后续处理提供更易于处理的点云。大致流程如下:
-
分割出大致地面点(可以使用绝对高度等方法,不用精确)
-
找到地点所在平面(通过大致地面点估计地平面方程ax+by+cz+d=0,得到法向量a,b,c)
-
通过变换矩阵校准平面(通过地平面的法向量 a,b,c 和 雷达坐标系法向量 0,0,1 求解变换矩阵)
法向量
法向量方法基于假设为计算得到的地面点法向量为竖直向上或向下,即地面点法向量值为 (0, 0, 1) 或 (0, 0, -1) 。根据法向量方法的假设,一定要先对点云进行校正,如果不进行校正,那么很可能出现某一帧没有地面点被分割出来的极端情况(激光雷达倾斜角度过大)。大致流程如下:
-
计算点法向量
-
设定法向量阈值 threshold 进行点分类
绝对高度
绝对高度方法根据校准后点云高度进行分割,通过设定阈值将点云分为地面点和障碍物点。大致流程如下:
-
校准点云
-
根据高度阈值 threshold 对点进行分类
平均高度
平均高度方法是对预处理后已经包含大部分地面点进行的处理,而不能单独进行使用。该方法基于假设为,预处理分割后得到点中地面点为绝大部分点,从而可根据平均高度作为进一步滤波。大致流程如下:
-
栅格最低点高度以上约 d(m) 分割出地面点,d 根据采集的实际点云数据设定
-
计算过程 1. 得到地面点的平均高度 h
-
以 h 为阈值再进行分割得到地面点
2.2.1.2 基于 RANSAC 方法分割地面
RANSAC 算法原理
RANSAC 是 RANdom SAmple Consensus 的缩写,即随机抽样一致的意思。它表示可以从一组包含“局外点”的数据集中,通过迭代的方式估计某个数学模型的参数。这是一种包含概率统计思想的“不确定算法”,它的每次迭代并不能保证获得一个正确合理的结果,这是带有概率性的,要想提高获得这种合理结果的概率,就需要增加迭代次数。
- RANSAC 的基本假设
-
整个数据集中同时包含好的点和不好的点,我们将它们分别称为局内点和局外点;
-
数据的分布可以通过某个数学模型来描述,而局内点就是可以适应该模型的点,局外点是不可以适应该模型的点;
-
随意给定一组点(可能是局内点也有可能是局外点),我们假设这组点都是局内点,它们满足某个数学模型,我们利用这个数学模型去估算其他的点,如果有足够多的点通过计算能被归类为假设的局内点,那么这个模型就足够合理。
RANSAC 在 PCL 库中的使用
PCL点云库中有标准的RANSAC算法接口,我们可以通过调用它实现更加快速,稳定地滤除地面点云,其示例代码如下:
#include <iostream>
#include <pcl/io/io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <unordered_set>
using namespace std;
int main()
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PCDReader reader;
reader.read("cloudRegion.pcd", *cloud);
int num_points = cloud->points.size();
std::unordered_set<int> inliersResult;
int maxIterations = 40; //迭代次数
while (maxIterations--)
{
std::unordered_set<int> inliers; //存放平面的内点,平面上的点也属于平面的内点
//随机在原始点云中选取三个点作为点云的求取平面系数的初始点
while (inliers.size() < 3)
{
inliers.insert(rand() % num_points);
}
// 需要至少三个点 才能找到地面
float x1, y1, z1, x2, y2, z2, x3, y3, z3;
auto itr = inliers.begin();
x1 = cloud->points[*itr].x;
y1 = cloud->points[*itr].y;
z1 = cloud->points[*itr].z;
itr++;
x2 = cloud->points[*itr].x;
y2 = cloud->points[*itr].y;
z2 = cloud->points[*itr].z;
itr++;
x3 = cloud->points[*itr].x;
y3 = cloud->points[*itr].y;
z3 = cloud->points[*itr].z;
//计算平面系数
float a, b, c, d, sqrt_abc;
a = (y2 - y1) * (z3 - z1) - (z2 - z1) * (y3 - y1);
b = (z2 - z1) * (x3 - x1) - (x2 - x1) * (z3 - z1);
c = (x2 - x1) * (y3 - y1) - (y2 - y1) * (x3 - x1);
d = -(a * x1 + b * y1 + c * z1);
sqrt_abc = sqrt(a * a + b * b + c * c);
//分别计算这些点到平面的距离
for (int i = 0; i < num_points; i++)
{
if (inliers.count(i) > 0) //判断一下有没有内点
continue;
pcl::PointXYZ point = cloud->points[i];
float x = point.x;
float y = point.y;
float z = point.z;
float dist = fabs(a * x + b * y + c * z + d) / sqrt_abc; // 计算点到平面距离
float distanceTol = 0.3;
if (dist < distanceTol)
inliers.insert(i); //如果点距离平面小于阈值,那么该点则视为内点
//将 inliersResult 中的内容不断更新,因为地面的点一定是最多的,
//所以迭代 40 次 找到的 inliersResult 最大时,也就相当于找到了地面
//inliersResult 中存储的也就是地面上的点云
if (inliers.size() > inliersResult.size())
inliersResult = inliers;
}
//cout << inliers.size() << endl;
}
//迭代结束后,所有属于平面上的内点都存放在 inliersResult 中
cout << inliersResult.size() << endl; //1633
//创建两个点云,一片存放地面,一片存放其他点
pcl::PointCloud<pcl::PointXYZ>::Ptr out_plane(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr in_plane(new pcl::PointCloud<pcl::PointXYZ>);
for (int i = 0; i < num_points; i++)
{
pcl::PointXYZ pt = cloud->points[i];
if (inliersResult.count(i))
out_plane->points.push_back(pt);
else
in_plane->points.push_back(pt);
}
/*pcl::PCDWriter writer;
writer.write("tree-2-Rend.pcd", *cloud_filtered1);*/
pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("显示窗口")); //窗口显示点云
viewer->addPointCloud(in_plane, "*cloud");
viewer->resetCamera();
viewer->spin();
system("pause");
return (0);
}
三、点云聚类算法
聚类就是按照某种特定的标准把一个数据集分割成不同的簇,使得同一个簇内的数据尽可能相似,不在同一个簇的数据差异尽可能大。聚类是一种无监督算法,一个好的聚类算法具有高的类内距离,同时具有低的类间距离。通常可以通过内在方法和外在方法进行评估。
-
内部评价是一种无监督的方式,采用类内聚集程度和类间离散程度衡量,比如轮廓系数、DB指数等。
-
外部评价是一种有监督的方式,用一定的度量评判聚类结果与基准数据的相似程度。比如归一化信息,兰德指数等。
三维点云通常利用其特征属性进行聚类,对每个点空间或者局部空间进行特征的提取或者转换,得到多种属性,比如:法向量,密度,距离,高程,强度等等,将不同属性的点云分割出来。
在点云处理中常见的有 欧式聚类,密度聚类,超体聚类,针对不同的场景其各有优缺点,从算法耗时角度出发超体聚类最慢,欧式聚类最快;从算法效果角度出发欧式距离最暴力直接,超体聚类对特殊形态的物体表征最好。下图是一张经过点云预处理后的障碍物聚类图:
点云聚类中常用的 欧几里得聚类算法(欧式聚类) 是基于 KD树 实现的,本节将就 KD树和欧式聚类展开介绍。
3.1 KD-Tree(KD树)
点云数据,具有数据量大、分布不均匀等特点。点云不具备传统网格数据的集合拓扑信息。所以点云数据处理中最为核心的问题就是建立离散点间的拓扑关系,实现基于邻域关系的快速查找。建立空间索引在点云数据处理中已被广泛应用,常见空间索引一般是自顶向下逐级划分空间的各种空间索引结构,比较有代表性的包括 BSP-Tree、KD-Tree、R-Tree、四叉树和八叉树等索引结构,而在这些结构中 KD-Tree 在3D点云数据组织中应用较为广泛。
KD-Tree,是一种对 k 维空间中的实例点进行存储以便对其进行快速检索的树形数据结构。主要应用于多维空间关键数据的搜索(如:范围搜索和最近邻搜索)。K-D树是二进制空间分割树的特殊的情况。
KD-Tree 的原理
选择其中某一个维度比较,根据这个维度进行空间划分。需要做的是两件事:
-
判断出在哪一个维度比较,也就是说,我们所要切割的面在哪一个维度上。当然这种切割需要遵循一个基本要求,那就是尽量通过这个维度的切割,使得数据集均分(为二);
-
判断以哪个数据条目分依据划分。上面我们说,要使得数据集均分为二,那当然要选择一个合适的数据项,充当这个划分的“点”。
针对上面两件事,只需要按照如下解决方法即可:
-
确定划分维度:尽量要使这个维度上所有数据项数值的分布有 最大方差,也就是说,数据在这个维度上最为分散。
-
选择充当切割标准的数据项:那么只需要求得这个维度上所有数值的 中位数 即可;
至此,可以设计出 KD-Tree 的构建算法了:
-
对于一个 n 维数据集,首先寻找方差最大的维度 d,找出维度 d 上的中位数 m,按 m 把数据集一分为二,记这两个数据子集为 Dl, Dr。建立树节点,存储这次划分的情况(记录划分的维度 d 以及中位数 m);
-
对 Dl, Dr 重复进行以上的划分,并且将新生成的树节点设置为上一次划分的左右孩子;
-
递归地进行以上两步,直到不能再划分为止。不能再划分时,将对应的数据保存至最后的节点中,这些最后的节点也就是叶子节点。
-
以下是一个用 c++ 实现的 2维KD树的例子:
// KD-Tree 节点
struct Node
{
std::vector 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]) // 左 recursive_insert(&(*root)->left, depth+1, point, id); else // 右 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) { // 目标节点与当前节点比较 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]) // 前往左分支 recursive_search(node->left, depth + 1, ids, target, distanceTol); if((target[depth % 2] + distanceTol) > node->point[depth % 2]) // 前往右分支 recursive_search(node->right, depth+1, ids, target, distanceTol); } } // 返回 KD-Tree 中距离小于阈值的 point-id 的 list std::vector<int> search(std::vector<float> target, float distanceTol) { std::vector<int> ids; recursive_search(root, 0, ids, target, distanceTol); return ids; }
};
3.2 欧几里得聚类
欧式聚类依据欧式距离作为判定准则,对于空间内的一点 P,通过 KD-Tree 在领域搜索 k 个距离 P 点最近的点,其中距离小于设定阈值的便放入集合 Q 中,如果 Q 中元素不在增加,则聚类过程结束;否则在集合Q中选取P点以外的点,重复以上步骤。
欧式聚类算法在调参过程中有一个比较关键,就是点与点之间的距离阈值的设定。激光雷达在采样过程中具有近密远稀的特点,所以近处的点集之间距离较小,远处的点集之间距离较大。
经常遇到的一种现象就是,设定了距离阈值后,近处的目标聚类效果较好,但是远处的目标出现欠分割或者截断的问题。如下图所示:顶部车辆中间区域点距离较远导致被聚类成两个目标。
为了解决欧式聚类在不同距离段,点的间距不同的问题。在工程上也可以考虑采用分段聚类,在不同距离段设置不同的阈值,多线程并行运算每一段的聚类算法,最后在分段间隔处对聚类目标进行融合。
以 2D点欧几里得聚类为例,其 c++ 实现代码如下:
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(points, cluster, processed, distanceTol, tree, i);
clusters.push_back(cluster);
}
}
return clusters;
}
// 递归聚类
void Proximity(std::vector<std::vector<float>> point,
std::vector<int> &cluster,
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(point, cluster, processed, distanceTol, tree, nearby_points[i]);
}
}
以上是关于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;
// 欧式聚类对检测到的障碍物进行分组
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;
}
3.3 密度聚类
密度聚类是将簇定义为密度相连的点的最大集合,能够把具有足够高密度的区域划分为簇,并可在噪声的空间数据库中发现任意形状的聚类。
3.3.1 DBSCAN 算法
DBSCAN 算法,全称为 “Density-Based Spatial Clustering of Applications with Node”,也就是 “基于密度的聚类”。此类算法是假设聚类结构能通过样本分布的紧密程度确定,从样本密度的角度来考察样本之间的可连续性,并基于可连接样本不断扩展聚类簇以获得最终的聚类结果。
DBSCAN 算法使用两个参数来描述样本的紧密程度,其一是邻域半径,用来描述当前点的邻域距离阈值;其二是点的个数,用来描述在邻域范围内数据点的最小个数。
几个基本概念:
●核心对象:对于空间任一点 P,如果其邻域内至少包含 k 个样本点,则 P 是核心对象;
●密度直达:如果点 Q 位于点 P 的邻域内,且 P 是核心对象,则 Q 由 P 密度直达;
●密度可达:如果在样本序列中 P1,P2,P3,P2 由 P1 密度直达,P3 由 P2 密度直达,则 P3 由 P1 密度可达;
●密度直连:对于空间点 P 和 Q,如果存在核心对象 T 使得 P 和 Q 均由 T 密度可达,则称 P 和 Q 密度相连;
算法流程如下:
1初始化核心对象并记录其邻接点循环遍历每一个数据点,记录每一个数据点邻域内 <= Distance 参数(距离参数)的其他数据点,并判断符合距离参数的邻接点个数是否 >= MinPts 参数(点数量参数),如果符合添加则将该数据点添加到核心对象的集合之中。
2聚类先任选数据集中的一个核心对象为 “种子”,再由此出发找出其所有的密度可达的样本集合从而生成相应的聚类簇,直到所有的核心对象均被访问过为止。
判断核心对象 c++ 代码参考如下实现:
for (size_t i = 0; i < pointCloud.size(); i++)
{
// 指定半径范围查找近邻
kdtree.radiusSearch(cloud->points[i],
Neighbourhood, // 半径
neighbourPoints[i], // 近邻 vector
neighbourDistance[i]); // 距离 vector
if (neighbourPoints[i].size() >= MinPts) // 满足近邻点数量,即为核心对象
kernelObj.push_back(i);
else
pointCloud[i].cluster = NOISE;
}
聚类 c++ 代码参考如下实现(非递归方式):
//迭代标记同一聚类点
int k = -1; //初始化聚类簇数
for (int i = 0; i < kernelObj.size(); i++)
{
if (pointCloud[kernelObj[i]].cluster != NOT_CLASSIFIED)
continue;
std::vector<T> queue;
queue.push_back(kernelObj[i]);
pointCloud[kernelObj[i]].cluster = ++k;
while (!queue.empty())
{
size_t index = queue.back();
queue.pop_back();
if (neighbourPoints[index].size() > MinPts)
{
for (int j = 0; j < neighbourPoints[index].size(); j++)
{
if (k==pointCloud[neighbourPoints[index][j]].cluster)
continue;
queue.push_back(neighbourPoints[index][j]);
pointCloud[neighbourPoints[index][j]].cluster = k;
}
}
}
}
3.4 两种聚类方法总结
欧式聚类算法实现相对简单,采用 KD-Tree 搜索空间点,运算效率相对较高。但是对离群点和噪声点敏感,在道路场景对障碍物聚类时,会对灌木,树枝等点云分布不规则静态障碍物,在连续帧中聚类出不同的效果,导致外接框拟合抖动。并且欧式聚类的距离阈值对近处和远处的目标点集难以有通用性。
密度聚类对噪声点不敏感,选择合适的阈值可以将目标点和离群点分离。但是在大规模的点云数据中,每个三维点都必须与其他对象点进行比较,算法耗时严重。而且对于线束较少的激光雷达点云,由于线束之间采样点间隔较大,邻域内的点云稀疏,采用密度聚类很容易造成目标点被当成噪声丢失掉。
点云采用聚类算法进行障碍物分割经常出现欠分割和过分割的问题。一般聚类算法需要设置一个搜索半径,如果搜索半径过小,会将一个对象分割成多个聚类目标;如果搜索半径太大,会将多个对象分割为一个聚类目标。如下图所示:
左边的卡车被聚类成两块,右边的卡车被聚类成三块。
实际工程开发中会对根据使用的激光雷达数据对典型算法进行改进,比如为了降低密度聚类逐点比较的时间成本,可以将三维点云数据按照维度划分为体素栅格为单元进行聚类,减少在空间中以点为单位的邻域搜索时间。
传统聚类算法的参数设定需要根据不同的应用场景进行调节,操作算法鲁棒性较差,针对阈值参数的设定采用自适应的方式会是一直算法优化的趋势。