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

综述

五大模块，看完等于点云入门~（dog）

实际步骤

1. 输入激光雷达获得的点云

   1. pcl::PointCloud::Ptr

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

2. 剔除距离激光雷达过近的点云（水平方向，XY平面，360度剔除）

   1. 通过点云中（x,y)坐标到原点的距离判断当前点云是否满足要求

3. 对点云进行体素下采样操作

   1. 点云体素下采样代码

      pcl::VoxelGrid 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);         //设置输出点云容器(非指针)
      1
      2
      3
      4

   2. 操作步骤

      1. 绘制体素包围盒，包裹住所有点云
         • 使用AABB包围盒，即根据x、y、z三轴的最大值和最小值创建包围盒
      2. 在包围盒内生成体素栅格，栅格大小作者设定，单位是米
      3. 计算每个栅格中包含所有点云的质心，并输出。即每个栅格只输出一个点云，大幅度缩减了点云数量。
      4. 实现点云体素下采样

4. 剔除距离激光雷达过低或过高的点云（垂直方向，Z轴，360度剔除）

   1. 通过点云的Z坐标判断当前点云是否满足要求，通常取 -1.3 <= Z <= 0.5
   2. 激光雷达的安装高度已足够高，再加半米足以检测到大多数车辆

5. 剔除距离激光雷达左右两侧过远的点云

   1. 通过点云的Y坐标是否满足限制条件判断，通常取 -1.5 <= Y <= 1.5

   2. 应用函数pcl::PointIndices::Ptr、pcl::ExtractIndices

      • 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);
          1
          2

      • pcl::ExtractIndices 根据作者的设置，输出是否位于索引内的点云 ——>滤波器

        pcl::ExtractIndices extract;
        extract.setInputCloud(in_cloud_ptr);    //输入点云指针
        extract.setIndices(far_indices);        //输入待处理的点云索引
        
        //设置为true，则去掉索引值代表的点云再进行输出；设置为false，则直接输出索引值代表的点云
        extract.setNegative(true);              
        extract.filter(*out_cloud_ptr);        //输出点云(非指针)
        1
        2
        3
        4
        5
        6
        7

6. 进行地面点云滤波（去除地面点云）

   1. 总体流程

   2. 重要部分：地面点云分割——>几何分割方法（RANSAC：随机采样一致算法），

      1. 算法步骤原理如下：

         1. 随机选择点云集合中的三个点，拟合平面模型
         2. 判断该平面模型法线与Z轴的夹角是否小于0.1弧度。若否，则不考虑，返回步骤1
         3. 计算其他点云到该平面模型的距离
            1. 若距离小于0.2（通常选取），则认定该点云是平面模型的内点，并统计内点个数
         4. 重复迭代，选择内点个数最多的平面模型作为最佳模型，并记录平面模型的点云索引
         5. 通过点云索引去掉地面点云，完成点云滤波

      2. 代码设置函数 为何设置迭代次数为100，链接：

         pcl::SACSegmentation 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
         3
         4
         5
         6
         7
         8
         9
         10
         11
         12
         13
         14
         15
         16
         17
         18

7. 滤除特征不明显的点云

   1. 判定特征是否明显的原理：点云曲率越大，对应弧的弯曲程度越大，采样点越多，特征越明显；反之，特征越不明显

   2. 核心操作：去除曲率小于0.5的点云数据

   3. 操作流程：

      • 核心函数：

        1. 建立KD搜索树对象：pcl::search::KdTree tree

        2. 建立KD搜索树：tree->setInputCloud(int_cloud_ptr)

        3. 进行法向量粗估计（OMP代表多线程估计，更快）：pcl::NormalEstimationOMP 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 diffnormals_estimator

           1. diffnormals_estimator中的PointXYZ、PointNormal、PointNormal分别代表待分割的点云数据、大半径点云法向量、小半径点云法向量
           2. 输入点云、大半径法向量、小半径法向量
              1. diffnormals_estimator.setInputCloud(in_cloud_ptr)
              2. diffnormals_estimator.setNormalScaleLarge(normal_large_ptr)
              3. diffnormals_estimator.setNormalScaleSmall(normal_small_ptr)
           3. 初始化计算：diffnormals_estimator.initcompute()
           4. 计算点云法向量：diffnormals_estimator.computeFeature(*out_cloud_ptr)
              1. 更新out_cloud_ptr中的法线和曲率，其数据类型为：PointNormal

        5. 构建点云条件滤波对象：pcl::ConditionalRemoval cond_remove

           1. 构建滤波条件对象：pcl::ConditionOr::ptr range_cond(new pcl::ConditionOr())

           2. 构建条件滤波（点云曲率大于0.5保留）：range_cond->addComparison(pcl::FieldComparison::ConstPtr(new pcl::FieldComparison("curvature", pcl::ComparisonOps::GT, 0.5))

              

        6. 点云数据复制

           pcl::PointCloud::ptr diffnormals_cloud(new pcl::PointCloud)
           pcl::copyPointCloud(*in_cloud_ptr, *diffnormals_cloud) //将in_cloud_ptr中的点云XYZ数据复制到diffnormals_cloud中点云pcl::PointNormal的XYZ上
           1
           2

8. 点云欧式聚类，获得聚类信息

   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)，完成聚类物体的姿态获取

9. 将过近的聚类物体合并成一个聚类物体

   1. 合并流程：
      1. 第一次合并：
         1. 计算聚类物体的质心
         2. 如果两个质心的距离小于一定阈值，则将两个质心对应的聚类物体合并
      2. 第二次合并：
         1. 重复第一次合并操作
   2. 两次合并的原因：防止第一次合并后出现物体过近的情况

10. 输出聚类结果，完成点云3D聚类