【PCL】—基于邻近信息的点云分割算法详解（欧几里得、区域生长）

参考：
https://www.cnblogs.com/ironstark/p/5000147.html
https://blog.csdn.net/m0_37957160/article/details/105856478

分割给人最直观的影响大概就是邻居和我不一样。也就是说，除了之前提到的基于采样一致性的分割方式以外，应该还存在基于邻近搜索的分割方式。通过对比某点和其最近一点的某些特征，来实现点云的分割。图像所能提供的分割信息仅是灰度或RGB向量，而三维点云却能够提供更多的信息。故点云在分割上的优势是图像所无法比拟的（重要的事情要说三遍）。

1. 搜索邻近点——kdTree & OcTree

• 由于分割工作需要对点云的邻近点进行操作，不断对比和访问某个点的邻居，所以表示点云的相邻关系是非常重要的。对于Scan来说，邻居关系是天然的。但对于很多杂乱点云，或者滤波、分割后的点云来说，邻居关系就已经被破坏了。
• 确定一个点云之间的相邻关系可以通过”树”来完成，目前比较主流的方法包括：kdTree 和 OcTree，这两种方法各有特点。

1.1 kdTree——一种递归的邻近搜索策略

• 关于 kdTree 到底是怎么工作的，https://en.wikipedia.org/wiki/K-d_tree，这里有非常详细的说明，我不再赘述。
• kdTree 实际上包括两个部分：
  1.建立 kdTree；
  2.在 kdTree 中查找。
  建立 kdTree 实际上是一个不断划分的过程，首先选择最sparse的维度，然后找到该维度上的中间点，垂直该维度做第一次划分。此时k维超平面被一分为二，在两个子平面中再找最sparse的维度，依次类推知道最后一个点也被划分。那么就形了一个不断二分的树。如图所示：
  
• 显然，一般情况下一个点的邻近点只需要在其父节点和子节点中搜索即可，大大缩小了邻近点的搜索规模。并且 kdtree 可以有效的对插入点进行判断其最近点在哪个位置。对于低层次视觉来说 kdTree 算法是非常重要的。在很多情况下需要给出某个点，再查k临近点的编号，或者差某半径范围内的点。
• PCL已经实现了kdtree算法，其调用接口如下：

#include 
#include 

//创建kdtree 结构
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree;
//传入点云
kdtree.setInputCloud(cloud);
//设置输入点
pcl::PointXYZ searchPoint;

//    k邻近搜索    //
int K = 10;
//设置两个容器，第一个放点的标号，第二个放点到SearchPoint的距离
std::vector<int> pointIdxNKNSearch(K);
std::vector<float> pointNKNSquaredDistance(K);
//进行搜索，注意，此函数有返回值，>0为找到，<0则没找到
kdtree.nearestKSearch(searchPoint, K, pointIdxNKNSearch, pointNKNSquaredDistance);

//    基于距离的搜索    //
//两个未知大小的容器，作用同上
std::vector<int> pointIdxRadiusSearch;
std::vector<float> pointRadiusSquaredDistance;
// 搜索半径
float radius = 3;
//搜索，效果同上
kdtree.radiusSearch(searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance)

显然，我们还需要一个算法把Idx里的点云数据提取出来进行重新着色之类的工作，代码可以写作：

pcl::PointCloud<pcl::PointXYZ>::Ptr Npoints(new pcl::PointCloud<pcl::PointXYZ>);
Npoints->height=1;
Npoints->width=searchindice.size();
Npoints->resize (searchindice.size());
//注意此清空操作非常极其以及特别重要，否则Npoints中会有莫名奇妙的点。
Npoints->clear();
int PointNUM = 0;
for(int i=0;i<searchindice.size();++i)
{   
    PointNUM = searchindice[i];
    Npoints->push_back(cloud->points[PointNUM]);
//    cout<points[PointNUM].x<<"  "<points[PointNUM].y<<"  "<points[PointNUM].z<<"  "<
}

pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> Npoints_color_handler (Npoints, 0, 255, 0);
viewer->addPointCloud(Npoints,Npoints_color_handler,"Npoints");
viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "Npoints");

1.2 OcTree

• OcTree 是一种更容易理解也更自然的思想。对于一个空间，如果某个角落里有个盒子我们却不知道在哪儿。但是"神"可以告诉我们这个盒子在或者不在某范围内，显而易见的方法就是把空间化成8个卦限，然后询问在哪个卦限内。再将存在的卦限继续化成8个。意思大概就是太极生两仪，两仪生四象，四象生八卦，就这么一直划分下去，最后一定会确定一个非常小的空间。对于点云而言，只要将点云的立方体凸包用 octree 生成很多很多小的卦限，那么在相邻卦限里的点则为相邻点。
• 显然，对于不同点云应该采取不同的搜索策略。如果点云是疏散的，分布很广泛，且没什么规律（如 lidar 测得的点云或双目视觉捕捉的点云），kdTree 能更好的划分，而 octree 则很难决定最小立方体应该是多少，太大则一个立方体里可能有很多点云，太小则可能立方体之间连不起来。如果点云分布非常规整，是某个特定物体的点云模型，则应该使用 ocTree，因为很容易求解凸包并且点与点之间相对距离无需再次比对父节点和子节点，更加明晰。典型的例子是斯坦福的兔子。

2. 计算与邻近点之间的关系——欧几里得与区域生长算法

• 基于欧式距离的分割和基于区域生长的分割本质上都是用区分邻里关系远近来完成的。由于点云数据提供了更高维度的数据，故有很多信息可以提取获得。
• 欧几里得算法使用邻居之间距离作为判定标准，而区域生长算法则利用了法线、曲率、颜色等信息来判断点云是否应该聚成一类。

2.1 欧几里得算法

2.1.1 算法原理

• 算法的原理在之前讲述《PCL—欧式聚类算法详解》的博客中已经说的比较清楚了。
• 这里我想用一个故事来讲讲这个问题。从前有一个脑筋急转弯，说一个锅里有两粒豆子，如果不用手，要怎么把它们分开。当时的答案是豆子本来就是分开的，又没黏在一起，怎么不叫分开。OK，实际上 欧几里德算法就是这个意思。两团点云就像是两粒豆子，只要找到某个合适的度量方式，就有办法把点云和点云分开。区分豆子我们用的方法可以归结为，两个豆子之间的距离小于分子距离，所以它们并没有连在一起。如果两团点云之间最近两点的距离小于单个点云内部点之间的距离，则可以由算法判断其分为两类。假设总点云集合为A，聚类所得点云团为Q，具体的实现方法大致是：

1. 找到空间中某点p10，用kdTree找到离他最近的n个点，判断这n个点到p的距离。将距离小于阈值r的点p12、p13、p14…放在类Q里；
2. 在 Q\p10 里找到一点p12，重复1；
3. 在 Q\p10、p12 找到找到一点，重复1，找到p22、p23、p24…全部放进Q里；
4. 当 Q 再也不能有新点加入了，则完成搜索了。

• 听起来好像这个算法并没什么用，因为点云总是连成片的，很少有什么东西会浮在空中让你来分。但是如果和前面介绍的内容联系起来就会发现这个算法威力巨大了。比如：
  半径滤波删除离群点
  采样一致找到桌面
  抽掉桌面…
• 显然，一旦桌面被抽，桌上的物体就自然成了一个个的浮空点云团。就能够直接用欧几里德算法进行分割了。如图所示：
  

2.1.2 PCL实现

PCL对欧几里德算法进行了很好的封装，其代码如下：

//被分割出来的点云团（标号队列）
  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);

2.2 区域生长算法

2.2.1 算法原理

• 区域生长算法直观感觉上和欧几里德算法相差不大，都是从一个点出发，最终占领整个被分割区域。古话说：“星星之火，可以燎原” 就是这个意思。欧几里德算法是通过距离远近，来判断烧到哪儿。区域生长算法则不然，烧到哪儿靠燃料（点）的性质是否类似来决定。对于普通点云，其可由法线、曲率估计算法获得其法线和曲率值。通过法线和曲率来判断某点是否属于该类。
• 算法的主要思想是：首先依据点的曲率值对点进行排序。之所以排序是因为，区域生长算法是从曲率最小的点开始生长的，这个点就是初始种子点，初始种子点所在的区域即为最平滑的区域，从最平滑的区域开始生长可减少分割片段的总数，提高效率。设置一空的种子点序列和空的聚类区域，选好初始种子后，将其加入到种子点序列中，并搜索邻域点，对每一个邻域点，比较邻域点的法线与当前种子点的法线之间的夹角，小于平滑阈值的将当前点加入到当前区域，然后检测每一个邻域点的曲率值，小于曲率阈值的加入到种子点序列中，删除当前的种子点，循环执行以上步骤，直到种子序列为空。
• 其算法可以总结为：

1. 种子周围的点和种子相比；
2. 法线方向是否足够相近；
3. 曲率是否足够小；
4. 如果满足1、2，则该点可用做种子点；
5. 如果只满足1，则归类而不做种子点；
6. 从某个种子出发，其“子种子”不再出现，则一类聚集完成；
7. 类的规模既不能太大也不能太小。

• 显然，上述算法是针对小曲率变化面设计的。尤其适合对连续阶梯平面进行分割：比如SLAM算法所获得的建筑走廊。
  

2.2.2 PCL实现

PCL对区域生长算法有如下封装：

//一个点云团队列，用于存放聚类结果
  std::vector<pcl::PointIndices> clusters;
  
  //区域生长分割器
  pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;  
  
  //输入分割目标
  reg.setSearchMethod(tree);
  reg.setNumberOfNeighbours(30);
  reg.setInputCloud(cloud);
  //reg.setIndices(indices);
  reg.setInputNormals(normals);
  
  //设置限制条件及先验知识
  reg.setMinClusterSize(50);
  reg.setMaxClusterSize(1000000);
  reg.setSmoothnessThreshold(3.0 / 180.0 * M_PI);
  reg.setCurvatureThreshold(1.0);
  
  reg.extract(clusters);

• 除了普通点云之外，还有一种特殊的点云，称为RGB点云。显而易见，这种点云除了有结构信息之外，还存在颜色信息。将物体通过颜色分类，是人类在辨认果实的过程中进化出的能力，颜色信息可以很好的将复杂场景中的特殊物体分割出来。而颜色点云也并不那么遥不可及，Xbox Kinect 就可以轻松的捕捉颜色点云。
• 基于颜色的区域生长分割原理上和基于曲率、法线的分割方法是一致的。只不过比较目标换成了颜色，去掉了点云规模上限的限制。可以认为，同一个颜色且挨得近，是一类的可能性很大，不需要上限来限制。所以这种方式比较适合用于室内场景分割。尤其是复杂室内场景，颜色分割可以轻松的将连续的场景点云变成不同的物体。哪怕是高低不平的地面，没法用采样一致分割器抽掉，颜色分割算法同样能完成分割任务。
  
  基于PCL的实现方式如下：

//用于存放点云团的容器
  std::vector<pcl::PointIndices> clusters;
  //颜色分割器
  pcl::RegionGrowingRGB<pcl::PointXYZRGB> reg;
  
  reg.setInputCloud(cloud);
  //点云经过了滤波器的预处理，提取了indices
  reg.setIndices(indices);
  reg.setSearchMethod(tree);
  reg.setDistanceThreshold(10);
  //点与点之间颜色容差
  reg.setPointColorThreshold(6);
  //苹果都是红的，哪怕离散的苹果也应该是一类
  reg.setRegionColorThreshold(5);
  reg.setMinClusterSize(600);

  reg.extract(clusters);