Autoware中的点云3D聚类算法，保姆级算法阅读注释，一看就懂，非常详细！

窃书的孔乙己

已于 2022-11-24 11:10:57 修改

阅读量1.6k

点赞数 3

分类专栏：笔记程序文章标签：算法 3d 聚类

于 2022-11-20 17:56:54 首次发布

本文链接：https://blog.csdn.net/cxkyxx/article/details/127951737

版权

笔记同时被 2 个专栏收录

26 篇文章 9 订阅

订阅专栏

程序

11 篇文章 1 订阅

订阅专栏

综述

五大模块，看完等于点云入门~（dog）
在这里插入图片描述

实际步骤

输入激光雷达获得的点云

pcl::PointCloud<pcl::PointXYZ>::Ptr

header:
	seq:序列长度
    stamp:获取点云的时刻
    frame_id:坐标系名称
points:保存点云的容器,类型为std::vector
width:类型为uint32_t,表示点云的宽度，即一行点云的数量
height:类型为uint32_t,若为1，代表无序点云，则width代表点云数量；大于1，则有序
is_dense:bool类型，若点云中数据是有限的，则为true；否则为false

剔除距离激光雷达过近的点云（水平方向，XY平面，360度剔除）
1. 通过点云中（x,y)坐标到原点的距离判断当前点云是否满足要求
对点云进行体素下采样操作
1. 点云体素下采样代码
```
pcl::VoxelGrid<pcl::PointXYZ> sor;   //设置下采样滤波器
sor.setInputCloud(in_cloud_ptr);     //输入点云数据(指针)
sor.LeafSize((float) in_leaf_size, (float) in_leaf_size, (float) in_leaf_size);                      //设置体素栅格大小(长、宽、高)
sor.filter(*out_cloud_ptr);         //设置输出点云容器(非指针)
```
2. 操作步骤
  1. 绘制体素包围盒，包裹住所有点云
    - 使用AABB包围盒，即根据x、y、z三轴的最大值和最小值创建包围盒
  2. 在包围盒内生成体素栅格，栅格大小作者设定，单位是米
  3. 计算每个栅格中包含所有点云的质心，并输出。即每个栅格只输出一个点云，大幅度缩减了点云数量。
  4. 实现点云体素下采样
剔除距离激光雷达过低或过高的点云（垂直方向，Z轴，360度剔除）
1. 通过点云的Z坐标判断当前点云是否满足要求，通常取 -1.3 <= Z <= 0.5
2. 激光雷达的安装高度已足够高，再加半米足以检测到大多数车辆
剔除距离激光雷达左右两侧过远的点云
1. 通过点云的Y坐标是否满足限制条件判断，通常取 -1.5 <= Y <= 1.5
2. 应用函数pcl::PointIndices::Ptr、pcl::ExtractIndices<pcl::PointXYZ>
  - pcl:PointIndices::Ptr 保存点云索引，用于后续点云查找、提取、删除等操作
    - 普通数组也可以保存点云索引，但是无法应用pcl库中其他函数直接对索引进行处理，所以还是需要利用pcl::PointIndices::Ptr记录点云索引
    - 该函数最主要的部分indices成员变量，类型为std::vector
```
pcl::PointIndices::Ptr far_indices(new pcl::PointIndices);
far_indices->indices.push_back((unsigned int)index);
```
  - pcl::ExtractIndices 根据作者的设置，输出是否位于索引内的点云 ——>滤波器
```
pcl::ExtractIndices<pcl::PointXYZ> extract;
extract.setInputCloud(in_cloud_ptr);    //输入点云指针
extract.setIndices(far_indices);        //输入待处理的点云索引

//设置为true，则去掉索引值代表的点云再进行输出；设置为false，则直接输出索引值代表的点云
extract.setNegative(true);              
extract.filter(*out_cloud_ptr);        //输出点云(非指针)
```
进行地面点云滤波（去除地面点云）
1. 总体流程
2. 重要部分：地面点云分割——>几何分割方法（RANSAC：随机采样一致算法），
  1. 算法步骤原理如下：
    1. 随机选择点云集合中的三个点，拟合平面模型
    2. 判断该平面模型法线与Z轴的夹角是否小于0.1弧度。若否，则不考虑，返回步骤1
    3. 计算其他点云到该平面模型的距离
      1. 若距离小于0.2（通常选取），则认定该点云是平面模型的内点，并统计内点个数
    4. 重复迭代，选择内点个数最多的平面模型作为最佳模型，并记录平面模型的点云索引
    5. 通过点云索引去掉地面点云，完成点云滤波
  2. 代码设置函数为何设置迭代次数为100，链接：
```
pcl::SACSegmentation<pcl::PointXYZ> seg;   //pcl库中自带的采样一致分割算法，即几何分割算法
pcl::PointIndices::Ptr inliers(new Pcl::PointIndices); //创建点云索引指针
pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); //创建模型参数

seg.setOptimizaCoefficients(true);      //进行参数优化

//============对要分割的平面模型进行设置============
seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE); //设置分割模型，此时设置的分割模型为垂直于轴线的平面模型
seg.setAxis(Eigen::Vector3f(0 ,0, 1));  //设置分割模型垂直的轴线。因此要对地面进行滤波，所以此处平面模型设置为垂直于Z轴
seg.setEpsAngle(0.1);                   //允许平面模型法线与Z轴的最大偏差为0.1弧度，也就是说大于0.1弧度的平面模型不考虑
//==============================================

seg.setMethodType(pcl::SAC_RANSAC);     //设置分割方法为随机采样一致算法
seg.setMaxIteration(100);               //设置最大迭代次数为100
seg.setDistanceThreshold(0.2);          //设置点云到平面的最大距离阈值。若小于该阈值，则认定为内点
seg.setOptimizeCoefficients(true);      //再次进行参数优化
seg.setInputCloud(in_cloud_ptr);        //输入待处理点云集合
seg.segment(*inliers, *coefficients);   //向分割算法中输入模型参数，输出分割模型的点云索引对象
```
滤除特征不明显的点云
1. 判定特征是否明显的原理：点云曲率越大，对应弧的弯曲程度越大，采样点越多，特征越明显；反之，特征越不明显
2. 核心操作：去除曲率小于0.5的点云数据
3. 操作流程：
  - 核心函数：
    1. 建立KD搜索树对象：pcl::search::KdTree<pcl::PointXYZ> tree
    2. 建立KD搜索树：tree->setInputCloud(int_cloud_ptr)
    3. 进行法向量粗估计（OMP代表多线程估计，更快）：pcl::NormalEstimationOMP<pcl::PointXYZ, pcl::PointNormal> normal_estimation
      1. 设置输入
      2. 设置搜索方式：normal_estimation.setSearchMethod(tree)
      3. 设置视点（用于统一法线方向）：normmal_estimation.setViewPoint(0, 0, 0)
      4. 设置搜索半径，进行KD树半径R邻域搜索：normal_estimation.setRadiusSearch(3)
      5. 拟合平面，并计算点云法向量：normal_estimation.compute(*normal_point_ptr)
    4. 进行法向量精估计：基于法向量微分的点云分割（对点云的大小半径做差）–>pcl::DiffenceOfNormalEstimation<pcl::PointXYZ, pcl::PointNormal, pcl::PointNormal> diffnormals_estimator
      1. diffnormals_estimator中的PointXYZ、PointNormal、PointNormal分别代表待分割的点云数据、大半径点云法向量、小半径点云法向量
      2. 输入点云、大半径法向量、小半径法向量
        diffnormals_estimator.setInputCloud(in_cloud_ptr)
        diffnormals_estimator.setNormalScaleLarge(normal_large_ptr)
        diffnormals_estimator.setNormalScaleSmall(normal_small_ptr)
      3. 初始化计算：diffnormals_estimator.initcompute()
      4. 计算点云法向量：diffnormals_estimator.computeFeature(*out_cloud_ptr)
        更新out_cloud_ptr中的法线和曲率，其数据类型为：PointNormal
    5. 构建点云条件滤波对象：pcl::ConditionalRemoval<pcl::PointNormal> cond_remove
      1. 构建滤波条件对象：pcl::ConditionOr<pcl::PointNormal>::ptr range_cond(new pcl::ConditionOr<pcl::PointNormal>())
      2. 构建条件滤波（点云曲率大于0.5保留）：range_cond->addComparison(pcl::FieldComparison<pcl::PointNormal>::ConstPtr(new pcl::FieldComparison<pcl::PointNormal>("curvature", pcl::ComparisonOps::GT, 0.5))
    6. 点云数据复制
```
pcl::PointCloud<pcl::PointNormal>::ptr diffnormals_cloud(new pcl::PointCloud<pcl::PointNormal>)
pcl::copyPointCloud<pcl::PointXYZ, pcl::PointNormal>(*in_cloud_ptr, *diffnormals_cloud) //将in_cloud_ptr中的点云XYZ数据复制到diffnormals_cloud中点云pcl::PointNormal的XYZ上
```
点云欧式聚类，获得聚类信息
1. 简述欧式聚类原理：
  1. 构建点云空间的KD树，便于后续点云半径邻域搜索
  2. 随机选取点云空间内的某个点云p1作为初始点，并以该点云p1为中心进行半径（人为设定）邻域搜索，获得领域点云集合P1
  3. 在点云集合P1中去除点云p1，再随机挑选一个点云p2进行邻域搜索，获得邻域点云集合P2，并将P2集合中有而P1集合中没有的点云补充到P1集合中，即 $P1=P1\cup P2$ ，P1不断扩大。
  4. 不断循环上述3步骤，直到遍历完P1集合中的所有点云，此时点云集合P1即为聚类得到的某个物体。
  5. 重新在点云空间中随机选取某个不属于点云集合P1的点云，进行上述步骤2、3、4，获得聚类物体。
  6. 当点云空间中没有单独的点云时，聚类结束，完成点云聚类。
2. 实际流程：
  1. 获取点云的二维信息，即x，y坐标，进行二维空间的点云欧式聚类
  2. 利用二维空间的点云聚类结果找到对应的三维点云，即不考虑点云z坐标，只要点云（x,y)坐标属于一个聚类物体，那个该三维点云就属于同一个聚类物体。
    - 从该部分可以看出3D聚类的结果存在瑕疵。如果现有两个物体，其（x,y）坐标相同，而高度z不同，按照该方法聚类，只会聚成一个物体，后续需改进
  3. 根据聚类物体的三维点云坐标获取聚类物体的信息
    - 聚类物体的长宽高
      - 长：length = max_x - min_x
      - 宽：width = max_y - min_y
      - 高：height = max_z - min_z
    - 聚类物体的顶点
      - 注：聚类结果显示为矩形。上下顶点只有Z坐标不同（只需求出上顶点或下顶点即可）
      - 上下顶点的计算使用凸包算法获得
        
        利用凸包算法获得聚类物体在二维平面中的凸点，如下所示，即将最外层点连接
        
        根据上述凸包，计算凸包的最小内接矩形，得到如下图所示，则紫色矩形的4个顶点，即为聚类物体的顶点，加上高度信息，即得到了聚类物体的8个顶点。
    - 聚类物体的姿态
      1. 类物体的姿态等于聚类物体相对于激光雷达坐标系的偏航角
      - 利用凸包算法计算得到内接矩阵的angle，如下图所示，angle应该为 $\theta_1$ .angle选择两个角度中绝对值较小的角度
      - 创造聚类物体的四元数tf::createQuaternionFromRPY(0.0, 0.0, θ1)，完成聚类物体的姿态获取
将过近的聚类物体合并成一个聚类物体
1. 合并流程：
  1. 第一次合并：
    1. 计算聚类物体的质心
    2. 如果两个质心的距离小于一定阈值，则将两个质心对应的聚类物体合并
  2. 第二次合并：
    1. 重复第一次合并操作
2. 两次合并的原因：防止第一次合并后出现物体过近的情况
输出聚类结果，完成点云3D聚类