PCL：超详细的基于法向量和曲率的区域生长算法原理以及源码解读

---------原理介绍：

（1）首先计算出来各点的曲率值，将曲率值按照从小到大的顺序进行排序。

（2）设置一空的种子点序列和一个空的聚类数组。

（3）选取曲率最小的点放入上述种子点序列中。

（4）从序列中拿出来一个种子点搜索其邻域。

（5）搜索到邻域后，这些点先过法线夹角阈值，通过的保留到聚类数据，然后再从这些通过法线夹角阈值的点中，检查是否通过曲率阈值，通过的加入种子点序列。即比较邻域点的法线与当前种子点法线之间的夹角，如果小于阈值将该邻域点加入聚类数组。再从这些小于法线阈值的点中判断是否小于曲率阈值，如果小于则将该邻域点加入种子序列。

（6）结束后删除用过的种子点，用下一个种子点重复第（4）（5）步，直至种子点序列清空为止。

（7）从曲率排序的点中，即从小到大的排序，取聚类数组中没有的点作为种子点，重复第（2）（3）（4）（5）（6）步骤。

可以理解为，聚类出的同一个平面上的点，曲率小于某一阈值(设置的曲率阈值)，才可以当作种子点，继续生长。

曲率阈值和法相量夹角阈值都是需要提前设置的。首先将曲率从小到大排序，选取最小的加入种子序列，这时候种子序列里只有一个点，然后拿出种子序列中的一个点(这时候也就是这个刚放进去的唯一的点)，和设置了指定范围的临域中的点比较法相量夹角，如果小于法相量夹角阈值，则这个临域的点和种子点是同一个平面，另外，如果这个点的曲率小于曲率阈值，则把这个点当作种子点，放入种子序列。反复上面的过程，直到种子序列为空，此时长出了一个平面。

其实在实际源代码中你会看到有一个做标记的过程还有一个记录聚类个数的过程。（分割其实就是赋标签的过程，种子点开始生长，满足条件就一直长，直到不满足条件了就不长了，把这些点标记一个标签，再从剩下的点里面重新选种子点，再次生长，标签累加，一直迭代到没有点为止。）（-1代表还没有标记的点云如果！=-1说明该点已经标记过标签了已经分割过的点云不参与后续的分割。）

以下是分步代码解读：

1.就是读取数据（略）

2.基于kd-tree搜索求法线，求的的法线保存在normal_里边（略）

3.邻域

//3.邻域************************************************************************************int point_num = cloud->points.size();//cloud点云个数,赋值给point_numvector<int>nebor_index;//vectot存放近邻搜索的索引vector<float>nebor_dis;//vectot存放近邻搜索的距离int nebor_size(10);//类似于 int nebor_size=10 近邻搜索的个数vector<vector<int>>point_nebor;//point_nebor是个vector，该vector的元素还是vectorpoint_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);//容器的swap()交换函数不是一般的swap(a,b)交换函数}

下面是关于在C++中vector<vector<int>>的用法

vector<vector<int> > A;//正确的定义方式
vector<vector<int>> A;//c++11之前这样定义是错误的，c++11之后支持这种定义方式

codeblocks设置支持c++11：Settings->Compiler->Compiler Flags

插入元素

若想定义A = [[0,1,2],[3,4]]，有两种方法。

（1）定义vector B分别为[0,1,2]和[3,4]，然后放入vector A。

//第一种方式
vector<vector<int> > A;vector<int> B;
B.push_back(0);
B.push_back(1);
B.push_back(2);A.push_back(B);//把B中的元素放入A中B.clear();
B.push_back(3);
B.push_back(4);A.push_back(B);

//第二种方式
vector<vector<int> > A;
for(int i = 0; i < 2; ++i)  A.push_back(vector<int>());   
A[0].push_back(0);
A[0].push_back(1);
A[0].push_back(2);//注意下边A的索引值由0变为1
A[1].push_back(3);
A[1].push_back(4);

长度

//vector<vector<int> >A中的vector元素的个数
len = A.size();
//vector<vector<int> >A中第i个vector元素的长度
len = A[i].size();

访问某元素（访问某元素时，方法和二维数组相同，例如：）

//根据前面的插入，可知输出1。
printf("%d\n", A[0][1]);

vector之resize()函数

c++中序列式容器的一个共性函数，
vv.resize(int n,element)表示调整容器vv的大小为n，扩容后的每个元素的值为element，默认为0
resize()会改变容器的容量和当前元素个数
定义 vector<type> vv；
头文件#include<vector>//标准模版库的内容

std::vector::resize

void resize (size_type n, value_type val = value_type());

resize()的作用是改变vector中元素的数目。
如果n比当前的vector元素数目要小，vector的容量要缩减到resize的第一个参数大小，既n。并移除那些超出n的元素同时销毁他们。
如果n比当前vector元素数目要大，在vector的末尾扩展需要的元素数目，如果第二个参数val指定了，扩展的新元素初始化为val的副本，否则按类型默认初始化。
注意：如果n大于当前的vector的容量(是容量，并非vector的size)，将会引起自动内存分配。所以现有的pointer,references,iterators将会失效。

resize()，设置大小（size）;size指指当前容器所存储的元素个数。

reserve()，设置容量（capacity）;capacity指容器在分配新的存储空间之前能存储的元素总数。

size()是分配容器的内存大小，而capacity()只是设置容器容量大小，但并没有真正分配内存。

打个比方：正在建造的一辆公交车，车里面可以设置40个座椅（reserve(40);），这是它的容量，但并不是说它里面就有了40个座椅，只能说明这部车内部空间大小可以放得下40张座椅而已。而车里面安装了40个座椅(resize(40);)，这个时候车里面才真正有了40个座椅，这些座椅就可以使用了。

1).reserve表示容器预留空间，但并不是真正的创建对象，需要通过insert（）或push_back（）等创建对象。

resize既分配了空间，也创建了对象。

2).reserve只修改capacity大小，不修改size大小，resize既修改capacity大小，也修改size大小。

3).两者的形参个数不一样。

resize带两个参数，一个表示容器大小，一个表示初始值（默认为0）

reserve只带一个参数，表示容器预留的大小。

#include <iostream>
#include <vector>
using namespace std;
int main(int argc, char *argv[])
{      vector <int> wgw;cout << "initilize size is: " << wgw.size() << endl;    //输出0cout << "initilize capacity is :" << wgw.capacity() <<endl;       //输出0wgw.reserve(100);cout << "wgw size is: " << wgw.size() << endl;    //输出0cout << "wgw capacity is :" << wgw.capacity() <<endl;    //输出100vector <int>wgw1;wgw1.resize(200);cout << "wgw1 size is :" << wgw1.size()<<endl;     //输出200cout << "wgw1 capacity is:" << wgw1.capacity() <<endl;     //输出200return 0; 
}

vector利用swap()函数进行内存的释放

swap可以有效降低当前 vector占用的内存总量。还有一个将 vector内存全部释放的技巧：

vector<int> vecEmpty;

v.swap(vecEmpty); //将要操作的 vector v与空 vector置换，可以将当前的 vector清空

首先，vector与deque不同，其内存占用空间只会增长，不会减小。比如你首先分配了10,000个字节，
然后erase掉后面9,999个，则虽然有效元素只有一个，但是内存占用仍为10,000个。所有空间在vector析构时回收。

empty()是用来检测容器是否为空的，clear()可以清空所有元素。
但是即使clear()，所占用的内存空间依然如故。如果你需要空间动态缩小，可以考虑使用deque。
如果非要用vector，这里有一个办法：
使用这种方法的前提是vector从前存储了大量数据，比如10000000，经过各种处理后，
现在只有100条，那么向清空原来数据所占有的空间，
就可以通过这种交换技术swap技法
就是通过交换函数swap（），使得vector离开其自身的作用域，
从而强制释放vector所占的内存空间。

vector<int>a,b;
for(int i=1;i<=10;++i) a.push_back(i);
a.swap(b);//这样a的空间大小就被释放了，现在是b存到了a里边，应该是这样的？？

4.曲率

vector<pair<float, int>>point_curvature_index;//在vector中插入pair对的用法for (int i_point = 0; i_point < point_num; i_point++){//使用make_pair(float,int)先给pair赋值，之后存入vectorpoint_curvature_index.push_back(make_pair(normal_->points[i_point].curvature, i_point));}sort(point_curvature_index.begin(), point_curvature_index.end());//将计算出来的曲率按照从小到大排序

C++中pair用法：

//定义一种新类型，它是一种vector类型，且每个元素都是pair类型。

typedef vector<pair<int, int> > vCoordinate; //>和>之间要有一个空格 C++11新标准可以不用

pair是C++中一种模板类型。每个pair对象可以存储两个值，这两个值可以是不同的数据类型。存储的值可以是基本数据类型也可以是自定义数据类型。

using namespace std; 或 using std::pair; pair<int, int> pdata; 或使用全名std::pair<int, int> pdata;

（1）定义和初始化

pair<int, int> p1(1, 2);//同种类型int,int
pair<int, int> p2(p1); //使用已有的p1对象初始化
pair<int, float> p3(1, 1.2);//不同类型int,float
pair<int, int> p4; //没有显示初始化，自动执行默认初始化操作。p4为(0,0)

（2）赋值操作

pair<int, int> p1(1, 2);
pair<int, int> p2;
p2 = pair<int, int> (1, 4);//赋值操作，需要用强制转换p2 = p1; //赋值操作

使用make_pair()函数

pair<int, int> p3;
p3 = make_pair(1, 4); //无需指明类型，可自动生成pair对象

（3）访问和修改操作

pair有两个属性：first和second。

pair<int, int> p1(1, 2);
p1.first = 11; //修改第一个数值
p1.second = 22; //修改第二个数值
cout << p1.first << "," << p1.second << endl;

STL中map通过键－值的形式保证一一对应关系，而multimap则可以出现一对多的关系，这两种数据类型在存储数据时，会根据pair<>的first成员进行排序，不同的是前者将不会插入对first成员重复的结构，而后者可以。

而当我们我们只想存储pair对，不需要对其排序时，就可以用到vector，将pair对插入其中即可。下面就使用做一些简单说明：
（1）声明vector：
vector<pair<int,int>>vec
（2）往vector中插入数据，需要用到make_pair:
vec.push_back(make_pair<int,int>(10,50));
vec.push_back(make_pair(20,30));
（3）定义迭代器：
vector<pair<int,int> > ::iterator iter;
for(iter=vec.begin();iter!=vec.end();iter++);
（4）数据读取：
第一个数据:(*iter).first
第二个数据:(*iter).second

//使用vector+pair来实现直角坐标数据的存放
#include<iostream>
#include<vector>
using namespace std;		
//定义一种新类型，它是一种vector类型，且每个元素都是pair类型。		
typedef vector<pair<int, int> > vCoordinate;// >和>之间要有一个空格	 	
int main()	
{		vCoordinate vCoor;		for (int i = 0; i < 10; ++i)		{	vCoor.push_back(vCoordinate::value_type(i, i));//往vector中插入元素//相当于存储坐标数据（x,y）	}	for (int i = 0; i < vCoor.size(); ++i)		{	//输出坐标数据		cout << vCoor[i].first << ", " << vCoor[i].second << endl;//循环输出坐标（x,y）	}		 	getchar();	return 0;	
}

value_type可用于获取vector中元素的类型，则vCoordinate::value_type会获取到vCoordinate元素的类型pair<int, int>。所以vCoordinate::value_type(i, i)就等价于pair<int, int>(i, i)。
sort()函数用法

（1）Sort函数包含在头文件为#include<algorithm>的c++标准库中，调用标准库里的排序方法可以不必知道其内部是如何实现的，只要出现我们想要的结果即可！

（2）Sort函数有三个参数：

（2.1）第一个是要排序的数组的起始地址。

（2.2）第二个是结束的地址（最后一位要排序的地址）

（ 2.3）第三个参数是排序的方法，可以是从大到小也可是从小到大，还可以不写第三个参数，此时默认的排序方法是从小到大排序。

Sort函数使用模板: Sort(start,end,排序方法)

//指定一个从大到小的排序方法
#include<iostream>
#include<algorithm>
using namespace std;
bool complare(int a,int b)
{return a>b;
}
int main()
{int a[10]={9,6,3,8,5,2,7,4,1,0};for(int i=0;i<10;i++)cout<<a[i]<<endl;sort(a,a+10,complare);//在这里就不需要对complare函数传入参数了，
//这是规则for(int i=0;i<10;i++)cout<<a[i]<<endl;return 0;
}

5.区域生长

float normal_threshold = cosf(10.0 / 180.0 * M_PI);//余弦 表示法线阈值float curvature_threshold = 0.4;//法向量阈值,曲率阈值int counter_(0), segment_label(0);//赋值为0int seed_index_orginal = point_curvature_index[0].second;//选取第一个最小的曲率点作为初始种子点的索引，对应前边两个值中的第二个vector<int>point_label;vector<int>seg_ave_num;point_label.resize(point_num, -1);//（大小，初始值）就是point_label里边都是-1while (counter_<point_num)//point_num整个点云的点数{queue<int>seed;//队列创建：queue <int> queue1;这里的int可以是各种类型，包括结构体类型seed.push(seed_index_orginal);//加入队列：queue1.push(x);将x添加到队列的末尾int counter_1(1);point_label[seed_index_orginal] = segment_label;//此处的为0while (!seed.empty())//判断是否为空，不为空执行操作{int curr_seed = seed.front();//访问队首元素，如例：q.front()，即最早被压入队列的元素。seed.pop();//出队，如例：q.pop(); 弹出队列的第一个元素，注意，并不会返回被弹出元素的值。int K_nebor(0);while (K_nebor<nebor_size)//相当于小于10{//point_nebor[队列中的第一个元素就是第一个曲率值最小的那个点][从0到9]int index_nebor = point_nebor[curr_seed][K_nebor];if (point_label[index_nebor] != -1)//这里怎么判断是否等于-1？？?如果不等于-1就K_nebor叠加{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;}}}

queue

C++队列是一种容器适配器，提供了一种先进先出的数据结构。
queue 模板类的定义在<queue>头文件中。
与stack 模板类很相似，queue 模板类也需要两个模板参数，一个是元素类型，一个容器类
型，元素类型是必要的，容器类型是可选的，默认为deque 类型。
定义queue 对象的示例代码如下：
queue<int> q1;
queue<double> q2;

queue 的基本操作有：
（1）入队，如例：q.push(x); 将x 接到队列的末端。
（2）出队，如例：q.pop(); 弹出队列的第一个元素，注意，并不会返回被弹出元素的值。
（3）访问队首元素，如例：q.front()，即最早被压入队列的元素。
（4）访问队尾元素，如例：q.back()，即最后被压入队列的元素。
（5）判断队列空，如例：q.empty()，当队列空时，返回true。即若为空队列则返回1，否则返回0
（6）访问队列中的元素个数，如例：q.size()

注意queue中无法使用迭代器，无法对队列进行遍历，所以如果有遍历要求的话就不能使用队列。

//一部分源码
//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() 
{//1.读取数据PointCloud<PointXYZ>::Ptr cloud(new PointCloud<PointXYZ>);io::loadPCDFile("F://coutsaved//test//boxes//pcd//6.pcd", *cloud);//2.基于kd-tree搜索求法线，求的的法线保存在normal_里边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_);//3.邻域************************************************************************************int point_num = cloud->points.size();//点云个数vector<int>nebor_index;//vectot存放近邻索引vector<float>nebor_dis;//vectot存放近邻距离int nebor_size(10);//类似于 int nebor_size=10vector<vector<int>>point_nebor;//point_nebor是个vector，该vector的元素还是vectorpoint_nebor.resize(point_num, nebor_index);//重新修改point_nebor大小，将point_num即cloud大小给point_neborfor (int i_point = 0; i_point < point_num; i_point++){tree->nearestKSearch(cloud->points[i_point], nebor_size, nebor_index, nebor_dis);//原来point_nebor被resize成了cloud点云个数的大小，现在point_nebor[i_point]就是一个点即i_point表示一个点，存储10个索引point_nebor[i_point].swap(nebor_index);//swap()交换函数//替换完之后事实上这个point_nebor仍然是cloud点云个数的大小，只是里边每个点都存储了10个索引，并未改变point_nebor的大小}//4.曲率*************************************************************************************vector<pair<float, int>>point_curvature_index;for (int i_point = 0; i_point < point_num; i_point++){//point_curvature_index里边存放的是（曲率，该曲率对应的点号）是两个值point_curvature_index.push_back(make_pair(normal_->points[i_point].curvature, i_point));}sort(point_curvature_index.begin(), point_curvature_index.end());//每点的曲率排序//5.区域生长*******************************************************************************float normal_threshold = cosf(10.0 / 180.0 * M_PI);//余弦 表示法线阈值float curvature_threshold = 0.4;//法向量阈值,曲率阈值int counter_(0), segment_label(0);//赋值为0int seed_index_orginal = point_curvature_index[0].second;//选取第一个最小的曲率点作为初始种子点的索引，对应前边两个值中的第二个vector<int>point_label;vector<int>seg_ave_num;point_label.resize(point_num, -1);//（大小，初始值）就是point_label里边都是-1while (counter_<point_num)//point_num整个点云的点数{queue<int>seed;//队列创建：queue <int> queue1;这里的int可以是各种类型，包括结构体类型seed.push(seed_index_orginal);//加入队列：queue1.push(x);将x添加到队列的末尾int counter_1(1);point_label[seed_index_orginal] = segment_label;//此处的为0while (!seed.empty())//判断是否为空，不为空执行操作{int curr_seed = seed.front();//访问队首元素，如例：q.front()，即最早被压入队列的元素。seed.pop();//出队，如例：q.pop(); 弹出队列的第一个元素，注意，并不会返回被弹出元素的值。int K_nebor(0);while (K_nebor<nebor_size)//相当于小于10{//point_nebor[队列中的第一个元素就是第一个曲率值最小的那个点][从0到9]int index_nebor = point_nebor[curr_seed][K_nebor];if (point_label[index_nebor] != -1)//这里怎么判断是否等于-1？？?如果不等于-1就K_nebor叠加{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 = 6000, 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;
}

下面是调用pcl::RegionGrowing类中调用区域增长算法。首先注意一点，这里是region growing segmentation，不是color-based region growing segmentation.

算法核心：该算法是基于点法线之间角度的比较，企图将满足平滑约束的相邻点合并在一起，以一簇点集的形式输出。每簇点集被认为是属于相同平面。

工作原理：首先需要明白，区域增长是从有最小曲率值(curvature value)的点开始的。因此，我们必须计算出所有曲率值，并对它们进行排序。这是因为曲率最小的点位于平坦区域，而从最平坦的区域增长可以减少区域的总数。现在我们来具体描述这个过程：

1.点云中有未标记点，按照点的曲率值对点进行排序，找到最小曲率值点，并把它添加到种子点集；

2.对于每个种子点，算法都会发现周边的所有近邻点。1）计算每个近邻点与当前种子点的法线角度差(reg.setSmoothnessThreshold)，如果差值小于设置的阈值，则该近邻点被重点考虑，进行第二步测试；2）该近邻点通过了法线角度差检验，如果它的曲率小于我们设定的阈值(reg.setCurvatureThreshold)，这个点就被添加到种子点集，即属于当前平面。

3.通过两次检验的点，被从原始点云去除。

4.设置最小点簇的点数min（reg.setMinClusterSize），最大点簇为max（reg.setMaxClusterSize）。

4.重复1-3步，算法会生成点数在min和max的所有平面，并对不同平面标记不同颜色加以区分。

5.直到算法在剩余点中生成的点簇不能满足min，算法停止工作。

//PCL里边的Region growing调用格式非源码
#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/cloud_viewer.h>
#include <pcl/features/integral_image_normal.h>
#include <pcl/segmentation/region_growing.h>
#include <pcl/filters/voxel_grid.h>int main(int argc, char** argv)
{pcl::PointCloud<pcl::PointXYZRGBA>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZRGBA>); // PointXYZRGBA型点云pcl::PCDReader reader;reader.read("pig2.pcd", *cloud); pcl::visualization::CloudViewer viewer("Cloud Viewer"); // 创建一个可视化窗口viewer.showCloud(cloud); // 在窗口中显示点云while (!viewer.wasStopped ()){}// 体素网格滤波pcl::VoxelGrid<pcl::PointXYZRGBA> sor;sor.setInputCloud(cloud);sor.setLeafSize(0.005f, 0.005f, 0.005f);sor.filter(*cloud);pcl::visualization::CloudViewer viewer2("Cloud Viewer"); // 创建一个可视化窗口viewer2.showCloud(cloud); // 在窗口中显示点云while (!viewer2.wasStopped ()){}// 创建一个空的kd-treepcl::search::KdTree<pcl::PointXYZRGBA>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZRGBA> ());// 创建一个normal点云pcl::PointCloud <pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud <pcl::Normal>);pcl::NormalEstimation<pcl::PointXYZRGBA, pcl::Normal> normal_estimator; // 创建法线估计对象normal_estimator.setSearchMethod (tree); // 设置搜索方法normal_estimator.setInputCloud (cloud); // 设置法线估计对象输入点集normal_estimator.setRadiusSearch (0.02); // 使用半径在查询点周围2厘米范围内的所有邻元素normal_estimator.compute (*cloud_normals); // 计算并输出法向量pcl::RegionGrowing<pcl::PointXYZRGBA, pcl::Normal> reg; // 创造区域生长分割对象reg.setMinClusterSize (50); // 设置一个聚类需要的最小点数，聚类小于阈值的结果将被舍弃reg.setMaxClusterSize (1000000); //设置一个聚类需要的最大点数，聚类大于阈值的结果将被舍弃reg.setSearchMethod (tree); // 设置搜索方法reg.setResidualThreshold (0.03); // 设置搜索的近邻点数目reg.setInputCloud (cloud); // 设置输入点云 reg.setInputNormals (cloud_normals); // 设置输入点云reg.setSmoothnessThreshold (5 / 180.0 * M_PI); //设置平滑阀值reg.setCurvatureThreshold (1); //设置曲率阀值// 以下两行用于启动分割算法，并返回聚类向量std::vector <pcl::PointIndices> clusters;reg.extract (clusters); // 获取聚类的结果，分割结果保存在点云索引的向量中//区域生长结果可视化pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud ();pcl::visualization::CloudViewer viewer3("Cloud Viewer"); // 创建一个可视化窗口viewer3.showCloud(colored_cloud);while (!viewer3.wasStopped ()){}
}

下图从左到右分别为原始点云降采样后的点云区域分割后的点云

#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/cloud_viewer.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/region_growing.h>int main(int argc, char** argv)
{DWORD t1, t2;t1 = GetTickCount();//以上两句和最后return 0 之上的为计时函数。//点云的类型pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);//打开点云if (pcl::io::loadPCDFile <pcl::PointXYZ>("resultFloor局部.pcd", *cloud) == -1)//改成想要输入的点云名称...*cloud就是把输入的点云记录到变量指针cloud中。{std::cout << "Cloud reading failed." << std::endl;return (-1);}//设置搜索的方式或者说是结构pcl::search::Search<pcl::PointXYZ>::Ptr tree = boost::shared_ptr<pcl::search::Search<pcl::PointXYZ> >(new pcl::search::KdTree<pcl::PointXYZ>);//求法线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(50);normal_estimator.compute(*normals);//直通滤波在Z轴的0到1米之间pcl::IndicesPtr indices(new std::vector <int>);pcl::PassThrough<pcl::PointXYZ> pass;pass.setInputCloud(cloud);pass.setFilterFieldName("z");pass.setFilterLimits(0.0, 1.0);pass.filter(*indices);//聚类对象<点，法线>pcl::RegionGrowing<pcl::PointXYZ, pcl::Normal> reg;//1首先还是先建立了一个区域增长的对象regreg.setMinClusterSize(50);  //最小的聚类的点数  //2然后设置平面包含的最少点数（这个参数非常重要，小于这个参数的平面会被忽略不计）reg.setMaxClusterSize(1000000);  //最大的   //3然后设置最大的点数，原理同上，但是一般我们希望的是无穷大，所以可以设大一点，当然如果你有特殊要求可以按自己需求来reg.setSearchMethod(tree);    //搜索方式reg.setNumberOfNeighbours(30);    //设置搜索的邻域点的个数  //4然后设置参考的邻域点数，也就是看看周边的多少个点来决定这是一个平面（这个参数至关重要，决定了你的容错率，如果设置的很大，那么从全局角度看某一个点稍微有点歪也可以接受，如果设置的很小则通常检测到的平面都会很小）reg.setInputCloud(cloud);         //输入点  //5然后输入要检测的点云cloud//reg.setIndices (indices);reg.setInputNormals(normals);     //输入的法线  //6然后输入点云的法线reg.setSmoothnessThreshold(3.0 / 180.0 * M_PI);  //设置平滑度  //7然后设置判断的阈值，大概也就是两个法线在多大的夹角内还可以当做是共面的reg.setCurvatureThreshold(1.0);     //设置曲率的阈值   //8最后也是一个弯曲的阈值，这个决定了比当前考察的点和平均的法线角度，决定是否还有继续探索下去的必要。（也就是假设每个点都是平稳弯曲的，那么normal的夹角都很小，但是时间长了偏移的就大了，这个参数就是限制这个用的）std::vector <pcl::PointIndices> clusters;//9reg.extract(clusters);
//9然后就可以把结果输出到一个簇里面，这个簇会自动把每个平面分成一个vector，可以打印下来看看std::cout << "Number of clusters is equal to " << clusters.size() << std::endl;std::cout << "First cluster has " << clusters[0].indices.size() << " points." << endl;std::cout << "These are the indices of the points of the initial" <<std::endl << "cloud that belong to the first cluster:" << std::endl;//int counter = 0;//while (counter < clusters[0].indices.size())//{//	std::cout << clusters[0].indices[counter] << ", ";//	counter++;//	if (counter % 10 == 0)//		std::cout << std::endl;//}//std::cout << std::endl;//可视化聚类的结果   //10也可以把检测到的每个平面涂上不同的颜色pcl::PointCloud <pcl::PointXYZRGB>::Ptr colored_cloud = reg.getColoredCloud();//pcl::visualization::CloudViewer viewer("Cluster viewer");//viewer.showCloud(colored_cloud);//while (!viewer.wasStopped())//{//}pcl::PCDWriter writer;//将点云写入磁盘writer.write("resultFloor局部_Cluster_viewer.pcd", *colored_cloud, false);//改成想要输出的点云名称t2 = GetTickCount();  //从这句到return 0之间的两句为计时函数printf("Use Time:%f\n", (t2 - t1)*1.0 / 1000);return (0);
}

------扩展到二维图片中Region Growing算法

（1）原理：

区域生长是根据预先定义的生长准则将像素或子区域组合为更大区域的过程。基本方法是从一组“种子”点开始（原点），将与种子相似的临近像素（在特定范围内的灰度或颜色）添加到种子栈中，不断迭代，生成一大片区域。严谨的数学定义可以查看冈萨雷斯的数字图像处理。

（2）算法实现

算法的步骤如下：

创建一个与原图像大小相同的空白图像
将种子点存入vector中，vector中存储待生长的种子点
依次弹出种子点并判断种子点如周围8领域的关系(生长规则)并与最大与最小阈值进行比较，符合条件则作为下次生长的种子点
vector中不存在种子点后就停止生长

（3）与三维区域生长算法不同的地方：

不需要kd-tree搜索邻域，2D图片的区域生长算法生长方向为八个且方向固定，如上图所示。
三维区域生长，是否为同一类看法线阈值，是否可以作为种子点看曲率阈值。而2D图片的区域生长这两个阈值为同一个灰度阈值。

opencv实现：

#include <iostream>
#include <opencv2/opencv.hpp>using namespace std;
using namespace cv;Mat RegionGrow(Mat src, Point2i pt, int th)
{/*src：Mat类，输入图像pt：Point2i类，初始生长点th：int型，与初始生长点的灰度差在这个范围内则进行生长*/int ptValue = 0;      /*当前生长点的灰度值*/Point2i pToGrowing;   /*待生长点位置*/int pToGrowLabel = 0; /*判断待生长点是否生长过*/int pToGrowValue = 0; /*当前生长点灰度值*/Mat growLabelImage = Mat::zeros(src.size(), CV_8UC1); /*创建和输入src等尺寸的空白区域，填充为黑色*/int DIR[8][2] = {{-1,-1}, {0,-1}, {1,-1}, {1,0}, {1,1}, {0,1}, {-1,1}, {-1,0}}; /*生长方向顺序数据*/vector<Point2i> ptVector; /*生长点栈*/ptVector.push_back(pt);   /*将初始生长点压入栈中*/growLabelImage.at<uchar>(pt.y, pt.x) = 255; /*初始生长点位置填充为白色*/ptValue = src.at<uchar>(pt.y, pt.x); /*记录初始生长点的灰度值*/while (!ptVector.empty()) /*生长栈为空则停止循环*/{pt = ptVector.back(); /*取出一个生长点*/ptVector.pop_back();  /*删除生长点栈中的当前生长点*//*分别对八个方向上的点进行生长*/for (int i=0; i<8; ++i){   pToGrowing.x = pt.x + DIR[i][0]; /*得到待生长点的位置*/pToGrowing.y = pt.y + DIR[i][1];/*检查是否是边界点，如果是，则跳出循环*/if (pToGrowing.x < 0 || pToGrowing.y < 0 || pToGrowing.x > (src.cols-1) || (pToGrowing.y > src.rows -1)){continue;}/*判断当前待生长点是否生长过*/pToGrowLabel = growLabelImage.at<uchar>(pToGrowing.y, pToGrowing.x);ptValue = src.at<uchar>(pt.y, pt.x) /*得到当前生长点的灰度值*/; /*如果当前待生长点还没生长，进行区域生长，否则跳过*/if (pToGrowLabel == 0)                    {pToGrowValue = src.at<uchar>(pToGrowing.y, pToGrowing.x); /*得到当前待生长点的灰度值*/if (abs(ptValue - pToGrowValue) < th) /*在阈值范围内则生长*/{growLabelImage.at<uchar>(pToGrowing.y, pToGrowing.x) = 255; /*生长过的点标记为白色*/ptVector.push_back(pToGrowing); /*将下一个生长点压入栈中*/}}}}return growLabelImage.clone(); /*同一类的255，不是用一类是0*/
}int main(int argc, char** argv)
{Mat srcImg = imread("test.jpg"); /*用opencv读取图片*/Mat graySrcImg;Mat srcImgClone = srcImg.clone();cvtColor(srcImgClone, graySrcImg, CV_BGR2GRAY); /*转化为灰度图像*/Mat LabelMask;/*得到区域生长算法返回的mask*/    LabelMask = RegionGrow(graySrcImg, Point2i(134, 70), 24); /*设置初始种子位置和阈值*/  Mat outputImage = srcImg.clone();for(int j=0; j<LabelMask.rows; ++j){for(int k=0; k<LabelMask.cols; ++k){if(LabelMask.at<uchar>(j,k) == 0){outputImage.at<Vec3b>(j, k) = 0;}}}imshow("src", srcImg);imshow("gray", graySrcImg);imshow("mask", LabelMask);imshow("result", outputImage);waitKey(0);return 0;
}

下图从左到右分别为原图灰度图区域生长算法返回的mask 掩膜后的结果

参考：https://blog.csdn.net/Zhang_Chen_/article/details/101228569

对原本的区域生长算法多加了最大与最小值的限制，作为默认参数可以不填。

/*** @brief 区域生长算法，输入图像应为灰度图像* @param srcImage 区域生长的源图像* @param pt 区域生长点* @param ch1Thres 通道的生长限制阈值，临近像素符合±chxThres范围内才能进行生长* @param ch1LowerBind 通道的最小值阈值* @param ch1UpperBind 通道的最大值阈值，在这个范围外即使临近像素符合±chxThres也不能生长* @return 生成的区域图像（二值类型）*/
Mat RegionGrow(Mat srcImage, Point pt, int ch1Thres,int ch1LowerBind=0,int ch1UpperBind=255)
{Point pToGrowing;                       //待生长点位置int pGrowValue = 0;                             //待生长点灰度值Scalar pSrcValue = 0;                               //生长起点灰度值Scalar pCurValue = 0;                               //当前生长点灰度值Mat growImage = Mat::zeros(srcImage.size(), CV_8UC1);   //创建一个空白区域，填充为黑色//生长方向顺序数据int DIR[8][2] = {{-1,-1}, {0,-1}, {1,-1}, {1,0}, {1,1}, {0,1}, {-1,1}, {-1,0}};vector<Point> growPtVector;                     //生长点栈growPtVector.push_back(pt);                         //将生长点压入栈中growImage.at<uchar>(pt.y, pt.x) = 255;              //标记生长点pSrcValue = srcImage.at<uchar>(pt.y, pt.x);         //记录生长点的灰度值while (!growPtVector.empty())                       //生长栈不为空则生长{pt = growPtVector.back();                       //取出一个生长点growPtVector.pop_back();//分别对八个方向上的点进行生长for (int i = 0; i<8; ++i){pToGrowing.x = pt.x + DIR[i][0];pToGrowing.y = pt.y + DIR[i][1];//检查是否是边缘点if (pToGrowing.x < 0 || pToGrowing.y < 0 ||pToGrowing.x > (srcImage.cols-1) || (pToGrowing.y > srcImage.rows -1))continue;pGrowValue = growImage.at<uchar>(pToGrowing.y, pToGrowing.x);       //当前待生长点的灰度值pSrcValue = srcImage.at<uchar>(pt.y, pt.x);if (pGrowValue == 0)                    //如果标记点还没有被生长{pCurValue = srcImage.at<uchar>(pToGrowing.y, pToGrowing.x);if(pCurValue[0] <= ch1UpperBind && pCurValue[0] >= ch1LowerBind ){if (abs(pSrcValue[0] - pCurValue[0]) < ch1Thres )                   //在阈值范围内则生长{growImage.at<uchar>(pToGrowing.y, pToGrowing.x) = 255;      //标记为白色growPtVector.push_back(pToGrowing);                 //将下一个生长点压入栈中}}}}}return growImage.clone();
}

上面是灰度图像的处理，我这里重载了三通道图像的区域生长

/*** @brief 区域生长算法，输入图像应为三通道图像（RGB、HSV、YUV等）* @param srcImage 区域生长的源图像* @param pt 区域生长点* @param ch1Thres ch2Thres ch3Thres 三个通道的生长限制阈值，临近像素符合±chxThres范围内才能进行生长* @param ch1LowerBind ch1LowerBind ch1LowerBind 三个通道的最小值阈值* @param ch1UpperBind ch2UpperBind ch3UpperBind 三个通道的最大值阈值，在这个范围外即使临近像素符合±chxThres也不能生长* @return 生成的区域图像（二值类型）*/
Mat RegionGrow(Mat srcImage, Point pt, int ch1Thres,int ch2Thres, int ch3Thres,int ch1LowerBind=0,int ch1UpperBind=255,int ch2LowerBind=0,int ch2UpperBind=255,int ch3LowerBind=0,int ch3UpperBind=255)
{Point pToGrowing;                       //待生长点位置int pGrowValue = 0;                             //待生长点灰度值Scalar pSrcValue = 0;                               //生长起点灰度值Scalar pCurValue = 0;                               //当前生长点灰度值Mat growImage = Mat::zeros(srcImage.size(), CV_8UC1);   //创建一个空白区域，填充为黑色//生长方向顺序数据int DIR[8][2] = {{-1,-1}, {0,-1}, {1,-1}, {1,0}, {1,1}, {0,1}, {-1,1}, {-1,0}};vector<Point> growPtVector;                     //生长点栈growPtVector.push_back(pt);                         //将生长点压入栈中growImage.at<uchar>(pt.y, pt.x) = 255;              //标记生长点pSrcValue = srcImage.at<Vec3b>(pt.y, pt.x);         //记录生长点的灰度值while (!growPtVector.empty())                       //生长栈不为空则生长{pt = growPtVector.back();                       //取出一个生长点growPtVector.pop_back();//分别对八个方向上的点进行生长for (int i = 0; i<8; ++i){pToGrowing.x = pt.x + DIR[i][0];pToGrowing.y = pt.y + DIR[i][1];//检查是否是边缘点if (pToGrowing.x < 0 || pToGrowing.y < 0 ||pToGrowing.x > (srcImage.cols-1) || (pToGrowing.y > srcImage.rows -1))continue;pGrowValue = growImage.at<uchar>(pToGrowing.y, pToGrowing.x);       //当前待生长点的灰度值pSrcValue = srcImage.at<Vec3b>(pt.y, pt.x);if (pGrowValue == 0)                    //如果标记点还没有被生长{pCurValue = srcImage.at<Vec3b>(pToGrowing.y, pToGrowing.x);if(pCurValue[0] <= ch1UpperBind && pCurValue[0] >= ch1LowerBind&&   //限制生长点的三通道上下界pCurValue[1] <= ch2UpperBind && pCurValue[1] >= ch2LowerBind &&pCurValue[2] <= ch3UpperBind && pCurValue[2] >= ch3LowerBind ){if (abs(pSrcValue[0] - pCurValue[0]) < ch1Thres &&abs(pSrcValue[1] - pCurValue[1]) < ch2Thres &&abs(pSrcValue[2] - pCurValue[2]) < ch3Thres)                    //在阈值范围内则生长{growImage.at<uchar>(pToGrowing.y, pToGrowing.x) = 255;      //标记为白色growPtVector.push_back(pToGrowing);                 //将下一个生长点压入栈中}}}}}return growImage.clone();
}

References：

https://www.jianshu.com/p/8e1bc28ef6fc

https://blog.csdn.net/liukunrs/article/details/80482788
https://blog.csdn.net/robin__chou/article/details/50071313
数字图像处理（第三版） ——冈萨雷斯 P493