PCL基于法向量和曲率的区域生长算法（源码）:

#include <pcl/io/pcd_io.h>
#include <pcl/search/kdtree.h>
#include <vector>
#include <pcl/features/normal_3d.h>
#include <Eigen/dense>
#include <cmath>
#include <ctime>
#include<pcl/visualization/pcl_visualizer.h>
using namespace pcl;
using namespace Eigen;
using namespace std;
int main(){
	PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);
	io::loadPCDFile("C:\\Users\\admin\\Desktop\\test.pcd", *cloud);
	
	search::KdTree<PointXYZ>::Ptr tree(new search::KdTree<PointXYZ>);
	PointCloud<Normal>::Ptr normal_(new PointCloud<Normal>);
	NormalEstimation<PointXYZ, Normal>ne;
	tree->setInputCloud(cloud);
	ne.setInputCloud(cloud);
	ne.setSearchMethod(tree);
	ne.setKSearch(10);
	ne.compute(*normal_);

	//邻域************************************************************************************
	int point_num = cloud->points.size();
	vector<int>nebor_index;
	vector<float>nebor_dis;
	int nebor_size(10);
	vector<vector<int>>point_nebor;
	point_nebor.resize(point_num, nebor_index);
	for (int i_point = 0; i_point < point_num;i_point++)
	{
		tree->nearestKSearch(cloud->points[i_point], nebor_size, nebor_index, nebor_dis);
		point_nebor[i_point].swap(nebor_index);
	}
	//曲率*************************************************************************************
	vector<pair<float, int>>point_curvature_index;
	for (int i_point = 0; i_point < point_num;i_point++)
	{
		point_curvature_index.push_back(make_pair(normal_->points[i_point].curvature, i_point));
	}
	sort(point_curvature_index.begin(), point_curvature_index.end());
	//区域生长*******************************************************************************
	float normal_threshold = cosf(10.0 / 180.0 * M_PI);
	float curvature_threshold = 0.1;//法向量阈值,曲率阈值
	int counter_(0),segment_label(0);
	int seed_index_orginal = point_curvature_index[0].second;//初始种子点的索引
	vector<int>point_label;
	vector<int>seg_ave_num;
	point_label.resize(point_num, -1);
	while (counter_<point_num)
	{
		queue<int>seed;
		seed.push(seed_index_orginal);
		int counter_1(1);
		point_label[seed_index_orginal] = segment_label;
		while (!seed.empty())
		{
			int curr_seed = seed.front();
			seed.pop();
			int K_nebor(0);
			while (K_nebor<nebor_size)
			{
				int index_nebor = point_nebor[curr_seed][K_nebor];
				if (point_label[index_nebor]!=-1)
				{
					K_nebor++;
					continue;
				}
				bool is_a_seed = false;
				Map<Vector3f>curr_seed(static_cast<float*>(normal_->points[curr_seed].normal));
				Map<Vector3f>seed_nebor(static_cast<float*>(normal_->points[index_nebor].normal));
				float dot_normal = fabsf(curr_seed.dot(seed_nebor));
				if (dot_normal<normal_threshold)
				{
					is_a_seed = false;
				}
				else if (normal_->points[index_nebor].curvature>curvature_threshold)
				{
					is_a_seed = false;
				}
				else
				{
					is_a_seed = true;
				}
				if (!is_a_seed)
				{
					K_nebor++;
					continue;
				}
				point_label[index_nebor] = segment_label;
				counter_1++;
				if (is_a_seed)
				{
					seed.push(index_nebor);
				}
				K_nebor++;
			}
		}
		segment_label++;
		counter_ += counter_1;
		seg_ave_num.push_back(counter_1);
		for (int i_seg = 0; i_seg < point_num;i_seg++)
		{
			int index_ = point_curvature_index[i_seg].second;
			if (point_label[index_]==-1)
			{
				seed_index_orginal = index_;
				break;
			}
		}
	}
	//分割结果汇总***************************************************
	vector<PointIndices>cluster_;
	PointIndices segments;
	int seg_num = seg_ave_num.size();
	cluster_.resize(seg_num, segments);

	for (int i_seg = 0; i_seg < seg_num;i_seg++){
		cluster_[i_seg].indices.resize(seg_ave_num[i_seg], 0);
	}
	vector<int> counter;
	counter.resize(seg_num, 0);
	for (int i_point = 0; i_point < point_num; i_point++)
	{
		int segment_index = point_label[i_point];
		int nebor_idx = counter[segment_index];
		cluster_[segment_index].indices[nebor_idx] = i_point;
		counter[segment_index] += 1;
	}
	//剔除一定数量的分割单元*****************************************
	vector<PointIndices>clusters;
	int min_number = 100, max_number = 1000000;
	for (int i_seg = 0; i_seg < seg_num;i_seg++)
	{
		if (cluster_[i_seg].indices.size()>min_number&&cluster_[i_seg].indices.size() < max_number)
		{
			clusters.push_back(cluster_[i_seg]);
		}
	}
	//可视化*********************************************************
	visualization::PCLVisualizer::Ptr viewer(new visualization::PCLVisualizer("view"));
	PointCloud<PointXYZRGB>::Ptr color_point(new PointCloud<PointXYZRGB>);
	srand(time(nullptr));
	vector<unsigned char>color;
	for (int i_segment = 0; i_segment < clusters.size(); i_segment++)
	{
		color.push_back(static_cast<unsigned char>(rand() % 256));
		color.push_back(static_cast<unsigned char>(rand() % 256));
		color.push_back(static_cast<unsigned char>(rand() % 256));
	}
	
	int color_index = 0;
	for (int i_seg = 0; i_seg < clusters.size(); i_seg++)
	{
		int clusters_size = clusters[i_seg].indices.size();
		for (int i_idx = 0; i_idx < clusters_size;i_idx++)
		{
			PointXYZRGB point;
			point.x = cloud->points[clusters[i_seg].indices[i_idx]].x;
			point.y = cloud->points[clusters[i_seg].indices[i_idx]].y;
			point.z = cloud->points[clusters[i_seg].indices[i_idx]].z;
			point.r = color[3 * color_index];
			point.g = color[3 * color_index+1];
			point.b = color[3 * color_index+2];
			color_point->push_back(point);
		}
		color_index++;
	}
	viewer->addPointCloud(color_point);
	viewer->spin();



	return 0;
}