学习点云和pcl算法初步③

云和pcl算法初步③"/>

学习点云和pcl算法初步③

点云初步学习③

主要工具：visual studio 2019 和cloudcompare（自行编译）

参考引用：

1.点云关键点、特征描述与提取

2.点云分割算法

点云分割&&点云分类

点云分割（point cloud segmentation）：
根据空间、几何和纹理等特征点进行划分，同一划分内的点云拥有相似的特征。点云分割的目的是分块，从而便于单独处理。
点云分类（point cloud classification）：
为每个点分配一个语义标记。点云的分类是将点云分类到不同的点云集。同一个点云集具有相似或相同的属性，例如地面、树木、人等。也叫做点云语义分割

①随机抽样一致算法（Random Sample Consensus,RANSAC）

采用迭代的方式从一组包含离群的被观测数据中估算出数学模型的参数。假设给定一组正确的数据，存在可以计算出符合这些数据的模型参数的方法。该算法核心思想就是随机性和假设性：

随机性是根据正确数据出现概率去随机选取抽样数据，根据大数定律，随机性模拟可以近似得到正确结果。
假设性是假设选取出的抽样数据都是正确数据，然后用这些正确数据通过问题满足的模型，去计算其他点 ，然后对这次结果进行一个评分。

基于RANSAC的基本检测算法虽然具有较高的鲁棒性和效率，但是目前仅针对平面，球，圆柱体，圆锥和圆环物种基本的基元。

算法流程
要得到一个直线模型，需要两个点唯一确定一个直线方程。
1.所以第一 步随机选择两个点。
2.通过这两个点，可以计算出这两个点所表示的模型方程y=ax+b。
3.将所有的数据点套到这个模型中计算误差。
4.找到所有满足误差阈值的点。
然后我们再重复1~4这个过程，直到达到一定迭代次数后，选出那个被 支持的最多的模型，作为问题的解。

PCL中的 Sample_consensus 模块
PCL中的 Sample_consensus库实现了随机采样一致性及其泛化估计算法，以及例如平面、柱面等各种常见几何模型，用不同的估计算法和不同的几何模型自由结合估算点云中隐含的具体几何模型的系数， 实现点云中所处的几何模型的分割。

支持以下模型：

SACMODEL_PLANE - 用于确定平面模型。平面的四个系数。
SACMODEL_LINE - 用于确定线模型。直线的六个系数由直线上的一个点和直线的方向给出。
SACMODEL_CIRCLE2D - 用于确定平面中的 2D 圆。圆的三个系数由其中心和半径给出。
SACMODEL_CIRCLE3D - 用于确定平面中的 3D 圆。圆的七个系数由其中心、半径和法线给出。
SACMODEL_SPHERE - 用于确定球体模型。球体的四个系数由其 3D 中心和半径给出。
SACMODEL_CYLINDER - 用于确定气缸模型。圆柱体的七个系数由其轴上的点、轴方向和半径给出。
SACMODEL_CONE - 用于确定锥模型。锥体的七个系数由其顶点、轴方向和张角给出。
SACMODEL_TORUS - 尚未实施
SACMODEL_PARALLEL_LINE -在最大指定角度偏差内确定与给定轴平行的线的模型。线系数类似于SACMODEL_LINE。
SACMODEL_PERPENDICULAR_PLANE -在最大指定角度偏差内确定垂直于用户指定轴的平面的模型。平面系数类似于SACMODEL_PLANE。
SACMODEL_PARALLEL_LINES - 尚未实现
SACMODEL_NORMAL_PLANE - 使用附加约束确定平面模型的模型：每个内点的表面法线必须平行于输出平面的表面法线，在指定的最大角度偏差内。平面系数类似于SACMODEL_PLANE。
SACMODEL_NORMAL_SPHERE - 类似于SACMODEL_SPHERE，但具有额外的表面法线约束。
SACMODEL_PARALLEL_PLANE -在最大指定角度偏差内确定平行于用户指定轴的平面的模型。平面系数类似于SACMODEL_PLANE。
SACMODEL_NORMAL_PARALLEL_PLANE使用附加表面法线约束定义 3D 平面分割模型。平面法线必须平行于用户指定的轴。因此，SACMODEL_NORMAL_PARALLEL_PLANE 等效于SACMODEL_NORMAL_PLANE + SACMODEL_PERPENDICULAR_PLANE。平面系数类似于SACMODEL_PLANE。
SACMODEL_STICK - 3D 棒分割模型。一根棍子是一条给定用户最小/最大宽度的线。

②欧式聚类分割

聚类方法，通过特征空间确定点与点之间的亲疏程度

算法流程：

找到空间中某点p，有kdTree找到离他最近的n个点，判断这n个点到p的距离。将距离小于阈值r的点p1,p2,p3… 放在类Q里
在 Q里找到一点p1,重复1，找到p22,p23,p24 全部放进Q里
当 Q 再也不能有新点加入了，则完成搜索了
参考链接

完整代码

#include <pcl/io/pcd_io.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/visualization/cloud_viewer.h>using namespace std;typedef pcl::PointXYZRGB PointT;int main()
{pcl::PointCloud<PointT>::Ptr cloud_in(new pcl::PointCloud<PointT>);pcl::PCDReader reader;pcl::PCDWriter writer;//---------------------------------//-------------加载点云-------------//---------------------------------if (reader.read("trees.pcd", *cloud_in) < 0){PCL_ERROR("点云文件不存在！\a\n");return -1;}/// 定义颜色数组，用于可视化float colors[] = {255, 0,   0,   // red 		10,   255, 0,   // green		20,   0,   255, // blue		3255, 255, 0,   // yellow	40,   255, 255, // light blue5255, 0,   255, // magenta   6255, 255, 255, // white		7255, 128, 0,   // orange	8255, 153, 255, // pink		951,  153, 255, //			10153, 102, 51,  //			11128, 51,  153, //			12153, 153, 51,  //			13163, 38,  51,  //			14204, 153, 102, //			15204, 224, 255, //			16128, 179, 255, //			17206, 255, 0,   //			18255, 204, 204, //			19204, 255, 153, //			20};//---------------------------------//------------欧式聚类--------------//---------------------------------/// 创建kd树pcl::search::KdTree<PointT>::Ptr tree(new pcl::search::KdTree<PointT>);tree->setInputCloud(cloud_in);/// 设置分割参数vector<pcl::PointIndices> cluster_indices;pcl::EuclideanClusterExtraction<PointT> ec;ec.setClusterTolerance(0.5);	//设置近邻搜索的半径ec.setMinClusterSize(999);		//设置最小聚类点数ec.setMaxClusterSize(999999);	//设置最大聚类点数ec.setSearchMethod(tree);ec.setInputCloud(cloud_in);ec.extract(cluster_indices);//----- 可视化1/3-----↓pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D viewer"));int v1(0);viewer->createViewPort(0.0, 0.0, 0.5, 1.0, v1); //设置第一个视口在X轴、Y轴的最小值、最大值，取值在0-1之间viewer->setBackgroundColor(0.1, 0.1, 0.1, v1);viewer->addText("cloud_in", 10, 10, "v1 text", v1);viewer->addPointCloud<PointT>(cloud_in, "cloud_in", v1);viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "cloud_in", v1);int v2(0);viewer->createViewPort(0.5, 0.0, 1.0, 1.0, v2);viewer->setBackgroundColor(0.3, 0.3, 0.3, v2);viewer->addText("cloud_cluster", 10, 10, "v2 text", v2);//-----可视化1/3-----↑/// 执行欧式聚类分割，并保存分割结果int j = 0;for (vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it){pcl::PointCloud<PointT>::Ptr cloud_cluster(new pcl::PointCloud<PointT>);for (vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)cloud_cluster->points.push_back(cloud_in->points[*pit]);cloud_cluster->width = cloud_cluster->points.size();cloud_cluster->height = 1;cloud_cluster->is_dense = true;stringstream ss;ss << "tree_" << j + 1 << ".pcd";writer.write<PointT>(ss.str(), *cloud_cluster, true);cout << "-----" << ss.str() << "详情-----" << endl;cout << *cloud_cluster << endl;//-----可视化2/3-----↓viewer->addPointCloud<PointT>(cloud_cluster, ss.str(), v2);viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, ss.str(), v2);viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR, colors[j*3] / 255, colors[j*3+1] / 255, colors[j*3 + 2] / 255, ss.str(), v2);//-----可视化2/3-----↑j++;}//-----可视化3/3-----↓while (!viewer->wasStopped()){viewer->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}//-----可视化3/3-----↑return 0;
}

聚类结果：

条件聚类分割
使用类pcl::ConditionEuclideanClustering实现点云分割，与其他分割方法不同的是该方法的聚类约束条件（欧式距离、平滑度、RGB颜色等）可以由用户自己定义，即当搜索到一个近邻点时，用户可以自定义该邻域点是否合并到当前聚类的条件。

pcl::ConditionalEuclideanClustering<PointTypeFull> cec (true);//创建条件聚类分割对象，并进行初始化。cec.setInputCloud (cloud_with_normals);//设置输入点集
//用于选择不同条件函数
switch(Method)
{
case 1:
cec.setConditionFunction (&enforceIntensitySimilarity); break;
case 2:
cec.setConditionFunction (&enforceCurvatureOrIntensitySimilarity); break;
case 3:
cec.setConditionFunction (&customRegionGrowing); break;
default:
cec.setConditionFunction (&customRegionGrowing); break;}
cec.setClusterTolerance (500.0);//设置聚类参考点的搜索距离
cec.setMinClusterSize (cloud_with_normals->points.size () / 1000);//设置过小聚类的标准cec.setMaxClusterSize (cloud_with_normals->points.size () / 5);//设置过大聚类的标准cec.segment (*clusters);//获取聚类的结果，分割结果保存在点云索引的向量中
cec.getRemovedClusters (small_clusters, large_clusters);//获取无效尺寸的聚类

这个条件的设置是可以由我们自定义的，因为除了距离检查，聚类的点还需要满足一个特殊的自定义的要 求，就是以第一个点为标准作为种子点，候选其周边的点作为它的对比或者比较的对象，如果满足条件就加入到聚类的对象中。

//如果此函数返回true，则将添加候选点到种子点的簇类中。 
bool customCondition(const pcl::PointXYZ& seedPoint, const pcl::PointXYZ& candidatePoin t, float squaredDistance) {// 在这里你可以添加你自定义的条件 return false; return true;
}

③区域生长分割算法

曲率和法线的介绍和计算：
PCL 计算点云法向量并显示：

种子点：根据点的曲率值对点云进行排序，曲率最小的点叫做初始种子点，
（1）区域生长算法从曲率最小的种子点开始生长，初始种子点所在区域为最平滑区域，从初始种子点所在的区域开始生长可减小分割片段的总数，从而提高算法的效率。
（2）设置一空的聚类区域Ｃ和空的种子点序列Ｑ，聚类数组L。
（3）选好初始种子点，将其加入种子点序列Q中，并搜索该种子点的领域点，计算每一个领域点法线与种子点法线之间的夹角，小于设定的平滑阀值时，将领域点加入到Ｃ中，同时判断该领域点的曲率值是否小于曲率阀值，将小于曲率阈值的领域点加入种子点序列Ｑ中，在邻域点都判断完成后，删除当前种子点，在Ｑ中重新选择新的种子点重复上述步骤，直到Ｑ中序列为空，一个区域生长完成，将其加入聚类数组L中。
（4）利用曲率值从小到大排序，顺序选择输入点集 的点作为种子点加入到种子点序列中，重复以上生长的步骤。

#include <iostream>
#include <vector>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/console/print.h>
#include <pcl/console/parse.h>
#include <pcl/console/time.h>#include <stdio.h>using namespace pcl::console;
int
main(int argc, char** argv)
{if (argc < 2){std::cout << ".exe xx.pcd -kn 50 -bc 0 -fc 10.0 -nc 0 -st 30 -ct 0.05" << endl;return 0;}time_t start, end, diff[5], option;start = time(0);int KN_normal = 10; bool Bool_Cuting = false;float far_cuting = 10, near_cuting = 0, SmoothnessThreshold = 5.0, CurvatureThreshold = 0.05;parse_argument(argc, argv, "-kn", KN_normal);parse_argument(argc, argv, "-bc", Bool_Cuting);parse_argument(argc, argv, "-fc", far_cuting);parse_argument(argc, argv, "-nc", near_cuting);parse_argument(argc, argv, "-st", SmoothnessThreshold);parse_argument(argc, argv, "-ct", CurvatureThreshold);pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);if (pcl::io::loadPCDFile <pcl::PointXYZ>(argv[1], *cloud) == -1){std::cout << "Cloud reading failed." << std::endl;return (-1);}end = time(0);diff[0] = difftime(end, start);PCL_INFO("\tLoading pcd file takes(seconds): %d\n", diff[0]);//Noraml estimation step(1 parameter)pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> >(new pcl::search::KdTree<pcl::PointXYZ>);//����һ��ָ��kd����������Ĺ���ָ��pcl::PointCloud <pcl::Normal>::Ptr normals(new pcl::PointCloud <pcl::Normal>);pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> normal_estimator;normal_estimator.setSearchMethod(tree);normal_estimator.setInputCloud(cloud);normal_estimator.setKSearch(KN_normal);normal_estimatorpute(*normals);end = time(0);diff[1] = difftime(end, start) - diff[0];PCL_INFO("\tEstimating normal takes(seconds): %d\n", diff[1]);//optional step: cutting the part are far from the orignal point in Z direction.2 parameterspcl::IndicesPtr indices(new std::vector <int>);if (Bool_Cuting){pcl::PassThrough<pcl::PointXYZ> pass;pass.setInputCloud(cloud);pass.setFilterFieldName("z");pass.setFilterLimits(near_cuting, far_cuting);pass.filter(*indices);}pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;reg.setMinClusterSize(100);reg.setMaxClusterSize(500000);reg.setSearchMethod(tree);reg.setNumberOfNeighbours(10);//然后设置参考的邻域点数，也就是看看周边的多少个点来决定这是一个平面（这个参数至关重要，决定了你的容错率，如果设置的很大，那么从全局角度看某一个点稍微有点歪也可以接受，如果设置的很小则通常检测到的平面都会很小）reg.setInputCloud(cloud);if (Bool_Cuting)reg.setIndices(indices);reg.setInputNormals(normals);reg.setSmoothnessThreshold(5 / 180.0 * M_PI);//平滑阈值,法向量的角度差reg.setCurvatureThreshold(0.05);//曲率阈值,代表平坦的程度std::vector <pcl::PointIndices> clusters;reg.extract(clusters);end = time(0);diff[2] = difftime(end, start) - diff[0] - diff[1]; PCL_INFO("\tRegion growing takes(seconds): %d\n", diff[2]);std::cout << "Number of clusters is equal to " << clusters.size() << std::endl;std::cout << "First cluster has " << clusters[0].indices.size() << " points." << endl;//保存聚类的点云-------------------------------------------------------------------int j = 0;for (std::vector<pcl::PointIndices>::const_iterator it = clusters.begin(); it != clusters.end(); ++it){pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_cluster(new pcl::PointCloud<pcl::PointXYZ>);for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); pit++)cloud_cluster->points.push_back(cloud->points[*pit]);cloud_cluster->width = cloud_cluster->points.size();cloud_cluster->height = 1;cloud_cluster->is_dense = true;std::cout << "PointCloud representing the Cluster: " << cloud_cluster->points.size() << " data points."<< std::endl;std::stringstream ss;ss << "cloud_cluster_" << j << ".pcd";pcl::io::savePCDFileASCII(ss.str(), *cloud_cluster);cout << ss.str() << "Saved" << endl;j++;}/**** But this function doesn't guarantee that different segments will have different* color(it all depends on RNG). Points that were not listed in the indices array will have red color.*/pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud();//保存附加颜色的点云
//	pcl::io::savePCDFileASCII("colored_pointCloud.pcd",*colored_cloud);pcl::visualization::PCLVisualizer viewer("区域增长分割");//    viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,8,"coloredCloud");viewer.addPointCloud(colored_cloud);//	viewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE,8);while (!viewer.wasStopped()){viewer.spinOnce();}return (0);
}

基于颜色的区域生长分割：主要是用相机采集点云的,具有RGB信息。不做过多深入了解。

基于颜色的区域生长分割的基本思想与 基于颜色的区域生长分割 相同。
基于颜色的算法有两个主要区别：
①是它使用颜色而不是法线。
②是使用合并算法进行过分割和欠分割控制，分割后，尝试合并具有相近颜色的簇。两个相邻簇的平均颜色相差很小，它们合并在一起。然后进行第二个合并步骤。在这一步中，每个集群都会根据其包含的点数进行验证。如果此数字小于用户定义的值，则当前群集将与最近的相邻群集合并。

#include <pcl/io/pcd_io.h>
#include <pcl/search/search.h>
#include <pcl/search/kdtree.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/region_growing_rgb.h>
#include <pcl/console/time.h>using namespace std;int main()
{//------------------------------- 加载点云 -------------------------------cout << "->正在加载点云..." << endl;pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud(new pcl::PointCloud <pcl::PointXYZRGB>);if (pcl::io::loadPCDFile<pcl::PointXYZRGB>("road.pcd", *cloud) < 0){PCL_ERROR("\a点云文件不存在！\n");system("pause");return (-1);}cout << "->加载点的个数：" << cloud->points.size() << endl;//========================================================================//--------------------------- 基于颜色的区域生长 ---------------------------pcl::console::TicToc time;time.tic();cout << "->正在进行基于颜色的区域生长..." << endl;pcl::search::Search <pcl::PointXYZRGB>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZRGB>);pcl::RegionGrowingRGB<pcl::PointXYZRGB> rgr;	//创建生长对象rgr.setInputCloud(cloud);						//设置输入点云rgr.setSearchMethod(tree);						//设置搜索方法rgr.setDistanceThreshold(0.2);					//设置距离阈值，用于判断两点是否相邻，小于阈值的为相邻点，用于聚类邻域点搜索rgr.setPointColorThreshold(20);					//设置点之间的颜色阈值，类似于基于曲率的区域生长中的平滑阈值，用于提取同一区域的点rgr.setRegionColorThreshold(20);				//设置聚类之间的颜色阈值，用于聚类合并rgr.setMinClusterSize(50);						//设置满足一个聚类的最小点数，如果聚类点数小于该阈值，则将其与最近的一个聚类合并rgr.setMaxClusterSize(99999999);				//设置满足一个聚类的最大点数std::vector <pcl::PointIndices> clusters;		//聚类索引向量rgr.extract(clusters);							//获取聚类结果，并保存到索引向量中cout << "->聚类个数为" << clusters.size() << endl;cout << "->区域生长用时：" << time.toc() / 1000 << " s" << endl;//========================================================================//----------------------------- 可视化生长结果 -----------------------------pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = rgr.getColoredCloud();	//对不同的分类结果随机赋色boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("基于颜色的区域生长分割结果"));viewer->addPointCloud<pcl::PointXYZRGB>(colored_cloud, "colored_cloud");//viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "colored_cloud");while (!viewer->wasStopped()){viewer->spinOnce(100);boost::this_thread::sleep(boost::posix_time::microseconds(100000));}//========================================================================return (0);
}

④最小图割的分割

参考：

       等一系列

参考代码

#include <iostream>
#include <vector>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/visualization/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/min_cut_segmentation.h>//最小图割头文件using namespace std;
int main (int argc, char** argv)
{pcl::PointCloud <pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud <pcl::PointXYZ>);if ( pcl::io::loadPCDFile <pcl::PointXYZ> ("min_cut_segmentation_tutorial.pcd", *cloud) == -1 ){cout << "Cloud reading failed." << endl;return (-1);}pcl::IndicesPtr indices (new vector <int>);//创建一组索引//--------------------直通滤波-----------------------pcl::PassThrough<pcl::PointXYZ> pass;pass.setInputCloud (cloud);pass.setFilterFieldName ("z");pass.setFilterLimits (0.0, 1.0);pass.filter (*indices);//-------------------最小图割------------------------pcl::MinCutSegmentation<pcl::PointXYZ> seg;//创建PointXYZ类型的最小图割对象seg.setInputCloud (cloud);seg.setIndices (indices);//提供前景点云（目标物体）的中心点，该中心点需要用户自己设置，对分割结果影响较大pcl::PointCloud<pcl::PointXYZ>::Ptr foreground_points(new pcl::PointCloud<pcl::PointXYZ>);pcl::PointXYZ point;point.x = 68.97;point.y = -18.55;point.z = 0.57;foreground_points->points.push_back(point);seg.setForegroundPoints (foreground_points);//输入前景点云的中心点seg.setSigma (0.25);//设置平滑成本的Sigma值seg.setRadius (3.0433856);//设置背景惩罚权重的半径seg.setNumberOfNeighbours (14);//设置临近点数目seg.setSourceWeight (0.8);//设置前景惩罚权重vector <pcl::PointIndices> clusters;seg.extract (clusters);//获取分割结果cout << "Maximum flow is " << seg.getMaxFlow () << endl;//----------------------可视化分割结果----------------------------------pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = seg.getColoredCloud();//对前景点赋予红色，对背景点赋予白色。pcl::visualization::PCLVisualizer viewer("最小割分割方法");viewer.addPointCloud(colored_cloud);viewer.addSphere(point, 5, 100, 100, 0, "sphere");//圆心坐标，半径，RGBviewer.setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.2, "sphere");while (!viewer.wasStopped()){viewer.spin();}return (0);
}

分割结果：

⑤基于法线微分的分割

参考博文：

基于法线微分的分割
1.根据不同尺度下法向量特征的差异性，利用pcl::DifferenceOfNormalsEstimation实现点云分割，在处理有较大尺度变化的场景点云分割效果较好，利用不同支撑半径去估算同一点的两个单位法向量，单位法向量的差定义DoN特征。

2.DoN算法：
DoN特征源于观察到基于所给半径估计的表面法向量可以反映曲面的内在几何特征，因此这种分割算法是基于法线估计的，需要计算点云中某一点的法线估计。而通常在计算法线估计的时候都会用到邻域信息，很明显邻域大小的选取会影响法线估计的结果。而在DoN算法中，邻域选择的大小就被称为support radius（支持半径）。对点云中某一点选取不同的支持半径，即可以得到不同的法线估计，而法线之间的差异，就是是所说的法线差异。

代码原理：
（1）对于输入点云数据中的每一点，利用较大的支撑半径rl计算法向量；
（2）对于输入点云数据中的每一点，利用较大的支撑半径rs计算法向量；
（3）对于输入点云数据中的每一点，单位化每一点的法向量差异
（4）过滤所得的向量域（DoN特征向量），分割出目标尺寸对应的点云；

#include <string>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/search/organized.h>
#include <pcl/search/kdtree.h>
#include <pcl/features/normal_3d_omp.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/segmentation/impl/extract_clusters.hpp>#include <pcl/features/don.h>
#include <pcl/visualization/pcl_visualizer.h>//for visualizationusing namespace pcl;
using namespace std;pcl::PointCloud<pcl::PointXYZRGB>::Ptr getColoredCloud
(pcl::PointCloud<pcl::PointXYZ>::Ptr input_,std::vector <pcl::PointIndices> clusters_, float r, float g, float b)
{pcl::PointCloud<pcl::PointXYZRGB>::Ptr colored_cloud;if (!clusters_.empty()){colored_cloud = (new pcl::PointCloud<pcl::PointXYZRGB>)->makeShared();srand(static_cast<unsigned int>(time(0)));std::vector<unsigned char>colors;for (size_t i_segment = 0; i_segment < clusters_.size(); i_segment++){colors.push_back(static_cast<unsigned char> (rand() % 256));colors.push_back(static_cast<unsigned char> (rand() % 256));colors.push_back(static_cast<unsigned char> (rand() % 256));}colored_cloud->width = input_->width;colored_cloud->height = input_->height;colored_cloud->is_dense = input_->is_dense;for (size_t i_point = 0; i_point < input_->points.size(); i_point++){pcl::PointXYZRGB point;point.x = *(input_->points[i_point].data);point.y = *(input_->points[i_point].data + 1);point.z = *(input_->points[i_point].data + 2);point.r = r;point.g = g;point.b = b;colored_cloud->points.push_back(point);}std::vector< pcl::PointIndices >::iterator i_segment;int next_color = 0;for (i_segment = clusters_.begin(); i_segment != clusters_.end(); i_segment++){std::vector<int>::iterator i_point;for (i_point = i_segment->indices.begin(); i_point != i_segment->indices.end(); i_point++){int index;index = *i_point;colored_cloud->points[index].r = colors[3 * next_color];colored_cloud->points[index].g = colors[3 * next_color + 1];colored_cloud->points[index].b = colors[3 * next_color + 2];}next_color++;}}return (colored_cloud);
}
int main(int argc, char *argv[]){int VISUAL = 1, SAVE = 0;//0 indicate shows nothing, 1 indicate shows very step output 2 only shows the final results//The smallest scale to use in the DoN filter.double scale1, mean_radius;//The largest scale to use in the DoN filter.double scale2;//The minimum DoN magnitude to threshold bydouble threshold;//segment scene into clusters with given distance tolerance using euclidean clusteringdouble segradius;if (argc < 6){cerr << "usage: " << argv[0] << " inputfile smallscale(5) largescale(10) threshold(0.1) segradius(1.5) VISUAL(1) SAVE(0)" << endl;cerr << "usage: " << "smallscale largescale  segradius :multiple with mean radius of point cloud " << endl;exit(EXIT_FAILURE);}//the file to read from.string infile = argv[1];//small scaleistringstream(argv[2]) >> scale1;//large scaleistringstream(argv[3]) >> scale2;istringstream(argv[4]) >> threshold;//threshold for DoN magnitudeistringstream(argv[5]) >> segradius;//threshold for radius segmentationistringstream(argv[6]) >> VISUAL;istringstream(argv[7]) >> SAVE;// Load cloud in blob formatpcl::PointCloud<PointXYZRGB>::Ptr cloud(new pcl::PointCloud<PointXYZRGB>);pcl::io::loadPCDFile(infile.c_str(), *cloud);// Create a search tree, use KDTreee for non-organized data.pcl::search::Search<PointXYZRGB>::Ptr tree;if (cloud->isOrganized()){tree.reset(new pcl::search::OrganizedNeighbor<PointXYZRGB>());}else{tree.reset(new pcl::search::KdTree<PointXYZRGB>(false));}// Set the input pointcloud for the search treetree->setInputCloud(cloud);//caculate the mean radius of cloud and mutilply with corresponding input{int size_cloud = cloud->size();int step = size_cloud / 10;double total_distance = 0;int i, j = 1;for (i = 0;i < size_cloud;i += step, j++){std::vector<int> pointIdxNKNSearch(2);std::vector<float> pointNKNSquaredDistance(2);tree->nearestKSearch(cloud->points[i], 2, pointIdxNKNSearch, pointNKNSquaredDistance);total_distance += pointNKNSquaredDistance[1] + pointNKNSquaredDistance[0];}mean_radius = sqrt((total_distance / j));cout << "mean radius of cloud is： " << mean_radius << endl;scale1 *= mean_radius;scale2 *= mean_radius;segradius *= mean_radius;}if (scale1 >= scale2){cerr << "Error: Large scale must be > small scale!" << endl;exit(EXIT_FAILURE);}// Compute normals using both small and large scales at each pointpcl::NormalEstimationOMP<PointXYZRGB, PointNormal> ne;ne.setInputCloud(cloud);ne.setSearchMethod(tree);/*NOTE: setting viewpoint is very important, so that we can ensurenormals are all pointed in the same direction!*/ne.setViewPoint(std::numeric_limits<float>::max(), std::numeric_limits<float>::max(), std::numeric_limits<float>::max());// calculate normals with the small scalecout << "Calculating normals for small scale..." << scale1 << endl;pcl::PointCloud<PointNormal>::Ptr normals_small_scale(new pcl::PointCloud<PointNormal>);ne.setRadiusSearch(scale1);nepute(*normals_small_scale);// calculate normals with the large scalecout << "Calculating normals for large scale..." << scale2 << endl;pcl::PointCloud<PointNormal>::Ptr normals_large_scale(new pcl::PointCloud<PointNormal>);ne.setRadiusSearch(scale2);nepute(*normals_large_scale);//visualize the normalsif (VISUAL = 1){cout << "click q key to quit the visualizer and continue！！" << endl;boost::shared_ptr<pcl::visualization::PCLVisualizer> MView(new pcl::visualization::PCLVisualizer("Showing normals with different scale"));pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZRGB> green(cloud, 0, 255, 0);int v1(0), v2(0);MView->createViewPort(0.0, 0.0, 0.5, 1.0, v1);MView->createViewPort(0.5, 0.0, 1.0, 1.0, v2);MView->setBackgroundColor(0, 0, 0);MView->addPointCloud(cloud, green, "small_scale", v1);MView->addPointCloud(cloud, green, "large_scale", v2);MView->addPointCloudNormals<pcl::PointXYZRGB, pcl::PointNormal>(cloud, normals_small_scale, 100, mean_radius * 10, "small_scale_normal");MView->addPointCloudNormals<pcl::PointXYZRGB, pcl::PointNormal>(cloud, normals_large_scale, 100, mean_radius * 10, "large_scale_normal");MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "small_scale", v1);MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "small_scale", v1);MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3, "large_scale", v1);MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5, "large_scale", v1);MView->spin();}// Create output cloud for DoN resultsPointCloud<PointNormal>::Ptr doncloud(new pcl::PointCloud<PointNormal>);copyPointCloud<PointXYZRGB, PointNormal>(*cloud, *doncloud);cout << "Calculating DoN... " << endl;// Create DoN operatorpcl::DifferenceOfNormalsEstimation<PointXYZRGB, PointNormal, PointNormal> don;don.setInputCloud(cloud);don.setNormalScaleLarge(normals_large_scale);don.setNormalScaleSmall(normals_small_scale);if (!don.initCompute()){std::cerr << "Error: Could not intialize DoN feature operator" << std::endl;exit(EXIT_FAILURE);}// Compute DoNdonputeFeature(*doncloud);//对输入点集，计算每个点的DON特征向量并输出//print some differencense of curvature{cout << "You may have some sense about the input threshold（curvature） next time for your data" << endl;int size_cloud = doncloud->size();int step = size_cloud / 10;for (int i = 0;i < size_cloud;i += step) cout << " " << doncloud->points[i].curvature << " " << endl;}//show the differences of curvature with both large and small scale if (VISUAL = 1){cout << "click q key to quit the visualizer and continue！！" << endl;boost::shared_ptr<pcl::visualization::PCLVisualizer> MView(new pcl::visualization::PCLVisualizer("Showing the difference of curvature of two scale"));pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointNormal> handler_k(doncloud, "curvature");MView->setBackgroundColor(0, 0, 0);MView->addPointCloud(doncloud, handler_k);MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3);MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5);MView->spin();}// Save DoN featurespcl::PCDWriter writer;if (SAVE == 1) writer.write<pcl::PointNormal>("don.pcd", *doncloud, false);// Filter by magnitudecout << "Filtering out DoN mag <= " << threshold << "..." << endl;// Build the condition for filteringpcl::ConditionOr<PointNormal>::Ptr range_cond(new pcl::ConditionOr<PointNormal>());//确定点是否属于满足设定的条件range_cond->addComparison(pcl::FieldComparison<PointNormal>::ConstPtr(new pcl::FieldComparison<PointNormal>("curvature", pcl::ComparisonOps::GT, threshold)));//添加比较的条件// Build the filterpcl::ConditionalRemoval<PointNormal> condrem;//condrem(range_cond)改成condrem.setCondition(range_cond);condrem.setCondition(range_cond);condrem.setInputCloud(doncloud);pcl::PointCloud<PointNormal>::Ptr doncloud_filtered(new pcl::PointCloud<PointNormal>);// Apply filtercondrem.filter(*doncloud_filtered);doncloud = doncloud_filtered;// Save filtered outputstd::cout << "Filtered Pointcloud: " << doncloud->points.size() << " data points." << std::endl;if (SAVE == 1)writer.write<pcl::PointNormal>("don_filtered.pcd", *doncloud, false);//show the results of keeping relative small curvature points if (VISUAL == 1){cout << "click q key to quit the visualizer and continue！！" << endl;boost::shared_ptr<pcl::visualization::PCLVisualizer> MView(new pcl::visualization::PCLVisualizer("Showing the results of keeping relative small curvature points"));pcl::visualization::PointCloudColorHandlerGenericField<pcl::PointNormal> handler_k(doncloud, "curvature");MView->setBackgroundColor(0, 0, 0);MView->addPointCloud(doncloud, handler_k);MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3);MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5);MView->spin();}// Filter by magnitudecout << "Clustering using EuclideanClusterExtraction with tolerance <= " << segradius << "..." << endl;//最后用欧氏聚类分割提取pcl::search::KdTree<PointNormal>::Ptr segtree(new pcl::search::KdTree<PointNormal>);segtree->setInputCloud(doncloud);std::vector<pcl::PointIndices> cluster_indices;pcl::EuclideanClusterExtraction<PointNormal> ec;ec.setClusterTolerance(segradius);ec.setMinClusterSize(50);ec.setMaxClusterSize(100000);ec.setSearchMethod(segtree);ec.setInputCloud(doncloud);ec.extract(cluster_indices);if (VISUAL == 1){//visualize the clustering resultspcl::PointCloud <pcl::PointXYZ>::Ptr tmp_xyz(new pcl::PointCloud<pcl::PointXYZ>);copyPointCloud<pcl::PointNormal, pcl::PointXYZ>(*doncloud, *tmp_xyz);pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = getColoredCloud(tmp_xyz, cluster_indices, 0, 255, 0);cout << "click q key to quit the visualizer and continue！！" << endl;boost::shared_ptr<pcl::visualization::PCLVisualizer> MView(new pcl::visualization::PCLVisualizer("visualize the clustering results"));pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgbps(colored_cloud);MView->setBackgroundColor(0, 0, 0);MView->addPointCloud(colored_cloud, rgbps);MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 3);MView->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY, 0.5);MView->spin();}if (SAVE == 1){int j = 0;for (std::vector<pcl::PointIndices>::const_iterator it = cluster_indices.begin(); it != cluster_indices.end(); ++it, j++){pcl::PointCloud<PointNormal>::Ptr cloud_cluster_don(new pcl::PointCloud<PointNormal>);for (std::vector<int>::const_iterator pit = it->indices.begin(); pit != it->indices.end(); ++pit){cloud_cluster_don->points.push_back(doncloud->points[*pit]);}cloud_cluster_don->width = int(cloud_cluster_don->points.size());cloud_cluster_don->height = 1;cloud_cluster_don->is_dense = true;//Save clustercout << "PointCloud representing the Cluster: " << cloud_cluster_don->points.size() << " data points." << std::endl;stringstream ss;ss << "don_cluster_" << j << ".pcd";writer.write<pcl::PointNormal>(ss.str(), *cloud_cluster_don, false);}}
}

⑥基于超体素分割

参考：://blog.csdn/qq_41918369/article/details/114436941   一系列

所谓超像素，是由许多像素点构成的小区域，这些像素点在位置上是相邻的，并且在某些特征（图像的亮度、颜色、纹理等特征）存在一定的相似性，这些小区域大都没有破坏图像的边界信息，而且还保留了对图像进行进一步分割的有效信息。

超体素数据（supervoxel）：
（类似于超像素）是一种将具有相似特征属性的体素数据聚类成一个区域的数据。超体素的获取过程即体素数据的聚类过程，其基本原理是对局部范围内满足相似性约束的体素数据的融合，根据纹理、颜色、法线等相似属性将体素云数据划分为相应的超体素，用于研究体素簇之间的关系，便于相应的后续操作。可以作为三维体素空间后续许多任务的预处理阶段任务。

点云数据经过体素化处理后，在空间内存在三种拓扑结构：6邻接、18邻接和26邻接。

点云数据的超体素过分割算法流程：

代码：

#include <pcl/console/parse.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/segmentation/supervoxel_clustering.h>// Types
typedef pcl::PointXYZRGBA PointT;
typedef pcl::PointCloud<PointT> PointCloudT;
typedef pcl::PointNormal PointNT;
typedef pcl::PointCloud<PointNT> PointNCloudT;
typedef pcl::PointXYZL PointLT;
typedef pcl::PointCloud<PointLT> PointLCloudT;void addSupervoxelConnectionsToViewer (PointT &supervoxel_center,PointCloudT &adjacent_supervoxel_centers,std::string supervoxel_name,boost::shared_ptr<pcl::visualization::PCLVisualizer> & viewer);intmain (int argc, char ** argv)
{if (argc < 2){pcl::console::print_error ("Syntax is: %s <pcd-file> \n ""--NT Dsables the single cloud transform \n""-v <voxel resolution>\n-s <seed resolution>\n""-c <color weight> \n-z <spatial weight> \n""-n <normal_weight>\n", argv[0]);return (1);}PointCloudT::Ptr cloud = pcl::make_shared <PointCloudT> ();pcl::console::print_highlight ("Loading point cloud...\n");if (pcl::io::loadPCDFile<PointT> (argv[1], *cloud)){pcl::console::print_error ("Error loading cloud file!\n");return (1);}cout<<"point size of input: "<<cloud->size()<<endl;bool disable_transform = pcl::console::find_switch (argc, argv, "--NT");//体素大小，空间八叉树的分辨率，类kinect或xtion获取的数据0.008左右合适float voxel_resolution = 0.008f;bool voxel_res_specified = pcl::console::find_switch (argc, argv, "-v");if (voxel_res_specified)pcl::console::parse (argc, argv, "-v", voxel_resolution);//种子的分辨率，一般可设置为体素分辨率的50倍以上float seed_resolution = 0.1f;bool seed_res_specified = pcl::console::find_switch (argc, argv, "-s");if (seed_res_specified)pcl::console::parse (argc, argv, "-s", seed_resolution);//针对分割场景，如果分割场景中各个物体之间的颜色特征差异明显，可以设置较大float color_importance = 0.2f;if (pcl::console::find_switch (argc, argv, "-c"))pcl::console::parse (argc, argv, "-c", color_importance);//设置较大且其他影响较小时，基本按照空间分辨率来决定体素分割float spatial_importance = 0.4f;if (pcl::console::find_switch (argc, argv, "-z"))pcl::console::parse (argc, argv, "-z", spatial_importance);//针对分割场景，如果分割场景中各个物体连通处的法线特征差异明显，可以设置较大，但在实际使用中，需要针对数据的结构适当考虑，发现估计的准确性等因素float normal_importance = 1.0f;if (pcl::console::find_switch (argc, argv, "-n"))pcl::console::parse (argc, argv, "-n", normal_importance);//  //// This is how to use supervoxels//  //pcl::SupervoxelClustering<PointT> super (voxel_resolution, seed_resolution);if (disable_transform)super.setUseSingleCameraTransform (false);super.setInputCloud (cloud);super.setColorImportance (color_importance);super.setSpatialImportance (spatial_importance);super.setNormalImportance (normal_importance);std::map <uint32_t, pcl::Supervoxel<PointT>::Ptr > supervoxel_clusters;//该单映射容器以标签为键值存储所有超体素pcl::console::print_highlight ("Extracting supervoxels!\n");super.extract (supervoxel_clusters);pcl::console::print_info ("Found %d supervoxels\n", supervoxel_clusters.size ());boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer (new pcl::visualization::PCLVisualizer ("点云库PCL学习教程第二版-超体素分割"));viewer->setBackgroundColor (1,1,1);PointCloudT::Ptr voxel_centroid_cloud = super.getVoxelCentroidCloud ();cout<<"voxel centroids: "<<voxel_centroid_cloud->size()<<endl;if(0){//对于体素中心的可视化和保存，基本就是对原始数据的空间均匀下采样viewer->addPointCloud<PointT>(voxel_centroid_cloud,"voxel centroids");pcl::io::savePCDFile("voxel_centroids.pcd",*voxel_centroid_cloud);viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,4, "voxel centroids");viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.5, "voxel centroids");}PointLCloudT::Ptr labeled_voxel_cloud = super.getLabeledVoxelCloud ();if(1){//超体素分割结果显示与保存pcl::io::savePCDFile("labeled_voxels.pcd",*labeled_voxel_cloud);viewer->addPointCloud (labeled_voxel_cloud, "labeled voxels");cout<<"labeled voxels: "<<labeled_voxel_cloud->size()<<endl;viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_POINT_SIZE,3, "labeled voxels");// viewer->setPointCloudRenderingProperties (pcl::visualization::PCL_VISUALIZER_OPACITY,0.8, "labeled voxels");}PointNCloudT::Ptr sv_normal_cloud = super.makeSupervoxelNormalCloud (supervoxel_clusters);//if(0)//超体素对应的法线特征可视化viewer->addPointCloudNormals<pcl::PointNormal> (sv_normal_cloud,1,0.05f, "supervoxel_normals");pcl::console::print_highlight ("Getting supervoxel adjacency\n");std::multimap<uint32_t, uint32_t> supervoxel_adjacency;super.getSupervoxelAdjacency (supervoxel_adjacency);cout<<"size of supervoxel_adjacency: "<<supervoxel_adjacency.size()<<endl;//遍历多重映射容器构造邻接图std::multimap<uint32_t,uint32_t>::iterator label_itr = supervoxel_adjacency.begin ();for ( ; label_itr != supervoxel_adjacency.end (); ){//获取标签值uint32_t supervoxel_label = label_itr->first;//根据标签索引到该超体素pcl::Supervoxel<PointT>::Ptr supervoxel = supervoxel_clusters.at (supervoxel_label);//遍历该超体素相邻超体素并以其相邻超体素中心为点集构造点云，用于后续可视化，这里的相邻超体素在多重映射容器中具有相同的键值PointCloudT adjacent_supervoxel_centers;std::multimap<uint32_t,uint32_t>::iterator adjacent_itr = supervoxel_adjacency.equal_range (supervoxel_label).first;for ( ; adjacent_itr!=supervoxel_adjacency.equal_range (supervoxel_label).second; ++adjacent_itr){pcl::Supervoxel<PointT>::Ptr neighbor_supervoxel = supervoxel_clusters.at (adjacent_itr->second);adjacent_supervoxel_centers.push_back (neighbor_supervoxel->centroid_);}//std::stringstream ss;ss << "supervoxel_" << supervoxel_label;//cout<<ss.str()<<endl;//绘制该超体素与其相邻超体素子图addSupervoxelConnectionsToViewer (supervoxel->centroid_, adjacent_supervoxel_centers, ss.str (), viewer);//使迭代器指向下一个标签。label_itr = supervoxel_adjacency.upper_bound (supervoxel_label);}while (!viewer->wasStopped ()){viewer->spinOnce();}return (0);
}voidaddSupervoxelConnectionsToViewer (PointT &supervoxel_center,PointCloudT &adjacent_supervoxel_centers,std::string supervoxel_name,boost::shared_ptr<pcl::visualization::PCLVisualizer> & viewer)
{int i=0;//Iterate through all adjacent points, and add a center point to adjacent point pairPointCloudT::iterator adjacent_itr = adjacent_supervoxel_centers.begin ();for ( ; adjacent_itr != adjacent_supervoxel_centers.end (); ++adjacent_itr){std::stringstream ss;ss<<supervoxel_name<<i;viewer->addLine(supervoxel_center,*adjacent_itr,ss.str());viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_LINE_WIDTH,3,ss.str());viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_COLOR,0,255,0,ss.str());ss<<supervoxel_name<<i;viewer->addSphere(supervoxel_center,0.008,0,0,255,ss.str());viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_SHADING,pcl::visualization::PCL_VISUALIZER_SHADING_GOURAUD,ss.str());//viewer->setShapeRenderingProperties(pcl::visualization::PCL_VISUALIZER_OPACITY,0.9,ss.str());i++;}}