点云区域生长分割

原理介绍：
（1）首先计算出来各点的曲率值，将曲率值按照从小到大的顺序进行排序。
（2）设置一空的种子点序列和一个空的聚类数组。
（3）选取曲率最小的点放入上述种子点序列中。
（4）从序列中拿出来一个种子点搜索其邻域。
（5）搜索到邻域后，这些点先过法线夹角阈值，通过的保留到聚类数据，然后再从这些通过法线夹角阈值的点中，检查是否通过曲率阈值，通过的加入种子点序列。即比较邻域点的法线与当前种子点法线之间的夹角，如果小于阈值将该邻域点加入聚类数组。再从这些小于法线阈值的点中判断是否小于曲率阈值，如果小于则将该邻域点加入种子序列。
（6）结束后删除用过的种子点，用下一个种子点重复第（4）（5）步，直至种子点序列清空为止。
（7）从曲率排序的点中，即从小到大的排序，取聚类数组中没有的点作为种子点，重复第（2）（3）（4）（5）（6）步骤。
可以理解为，聚类出的同一个平面上的点，曲率小于某一阈值(设置的曲率阈值)，才可以当作种子点，继续生长。
代码实现：

/**
 * @description:				区域生长分割算法
 * @param cloud					输入点云
 * @param clusters				分割结果
 * @param smoothness_threshold	平滑度阈值
 * @param curvature_threshold	曲率阈值
 * @param min_cluster_size		最小聚类点数	
 * @param max_cluster_size		最大聚类点数
 * @param nr_k					k邻近点数
 */
void regiongrow(pcl::PointCloud<pcl::PointXYZ>::Ptr& cloud, std::vector<pcl::PointCloud<pcl::PointXYZ>::Ptr>& clusters,
	float smoothness_threshold, float curvature_threshold, int min_cluster_size, int max_cluster_size, int nr_k)
{
	pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);//创建Kd搜索树
	tree->setInputCloud(cloud);//输入点云

	//计算点云的法线和曲率
	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(nr_k);// 设置用于法向量估计的k近邻数目
	normal_estimator.compute(*normals);//计算并输出法向量

	pcl::PointCloud<pcl::PointXYZINormal>::Ptr cloud_normals(new pcl::PointCloud<pcl::PointXYZINormal>);
	pcl::concatenateFields(*cloud, *normals, *cloud_normals);
	//pcl::io::savePCDFile("cloud_normals.pcd", *cloud_normals);

	//将曲率值按照从小到大的顺序对点云中的点进行排序
	sort(cloud_normals->begin(), cloud_normals->end(),
		[](pcl::PointXYZINormal pt1, pcl::PointXYZINormal pt2) {return pt1.curvature < pt2.curvature; });
	//pcl::io::savePCDFile("cloud_normals_sorted.pcd", *cloud_normals);

	//设置一空的种子点序列和一空的聚类数组
	std::vector<int> seed_queue;
	std::vector<bool> cluster_queue(cloud_normals->points.size(), false); 
	std::vector<bool> cluster_queue_old(cloud_normals->points.size(), false);

	pcl::KdTreeFLANN<pcl::PointXYZINormal> kdtree;
	kdtree.setInputCloud(cloud_normals);
	std::vector<int> pointsIdx(nr_k);          //索引
	std::vector<float> pointsDistance(nr_k);   //距离

	int cluster_size = 0; //类的数目

	//从曲率排序的点中，即从小到大的排序，取聚类数组中没有的点作为种子点
	for (int i = 0; i < cloud_normals->size(); ++i)
	{
		if (cluster_queue[i]) continue;
			
		//选取曲率最小的点放入上述种子点序列中
		seed_queue.push_back(i);

		++cluster_size;

		//直至种子点序列清空为止
		while (!seed_queue.empty())
		{
			//从序列中拿出来一个种子点搜索其邻域
			int seed_index = seed_queue.back();
			seed_queue.pop_back();

			kdtree.nearestKSearch(cloud_normals->points[seed_index], nr_k, pointsIdx, pointsDistance);    //k近邻搜索

			//搜索到邻域后，这些点先过法线夹角阈值，通过的保留到聚类数据，从这些通过法线夹角阈值的点中，检查是否通过曲率阈值，通过的加入种子点序列。
			//即比较邻域点的法线与当前种子点法线之间的夹角，如果小于阈值将该邻域点加入聚类数组。
			//再从这些小于法线阈值的点中判断是否小于曲率阈值，如果小于则将该邻域点加入种子序列。
			Eigen::Vector3f v1(cloud_normals->points[seed_index].normal_x, cloud_normals->points[seed_index].normal_y, cloud_normals->points[seed_index].normal_z);
			for (int j = 0; j < pointsIdx.size(); ++j)
			{
				Eigen::Vector3f	v2(cloud_normals->points[pointsIdx[j]].normal_x, cloud_normals->points[pointsIdx[j]].normal_y, cloud_normals->points[pointsIdx[j]].normal_z);
				float angle = atan2(v1.cross(v2).norm(), v1.transpose() * v2) * 180 / M_PI;
				if (fabs(angle) <= smoothness_threshold && !cluster_queue[pointsIdx[j]])
				{
					cluster_queue[pointsIdx[j]] = true;
					if (cloud_normals->points[pointsIdx[j]].curvature <= curvature_threshold && find(seed_queue.begin(), seed_queue.end(), pointsIdx[j]) == seed_queue.end())
						seed_queue.push_back(pointsIdx[j]);
				}
			}
		}

		//计算当前类中包含点的索引
		std::vector<int> indexs;
		for (size_t i = 0; i < cluster_queue.size(); ++i)
		{
			if (cluster_queue[i] && !cluster_queue_old[i])
				indexs.push_back(i);
		}
		cluster_queue_old = cluster_queue;
	
		//过滤满足最大点数和最小点数的类
		if (indexs.size() >= min_cluster_size && indexs.size() <= max_cluster_size)
		{
			pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_flag(new pcl::PointCloud<pcl::PointXYZ>);
			pcl::copyPointCloud(*cloud_normals, indexs, *cloud_flag);
			clusters.push_back(cloud_flag);
			//std::cout << indexs.size() << std::endl;
			//pcl::io::savePCDFile("cloud_flag" + std::to_string(cluster_size) + ".pcd", *cloud_flag);
		}
	}
}

代码传送门：https://github.com/taifyang/PCL-algorithm