参考
【C++】pcl中的Region Growing(区域生长)算法
区域生长的原理
1、排序,基于曲率对点云进行排序,曲率最小的点叫做初始种子点,区域生长算法从曲率最小的种子点开始生长,初始种子点所在区域为最平滑区域,从初始种子点所在的区域开始生长可减小分割片段的总数,从而提高算法的效率。
2、设置一个种子集合Q,聚类区域C,对种子点的邻域点进行分析,如果和种子点的法向量差值小于阈值,就把这个点放到C中;再计算曲率,若小于曲率阈值,就把这个点放入到Q中。对邻域计算完后再种子点的集合Q中剔除原来的种子点。
3、若Q非空,则重复2(找种子点的邻域点,进行判断)。直到Q为空集合。
论文
PCL封装的效果
matlab实现代码
------------------------------------------------------------
更新:2020-1-15
思路比较简单,pcl的代码实在看不下去,便参考了二维图像的区域生长分割,采用最简单的一种方法:手工指定一个初始种子点,在matlab中给出了实现
-------------------------------------------------------------
主函数building_Example2_Region_Grow_Segment.m
% 读取txt文件,将txt文件保存为pcd文件并以点云的格式显示
clear;clear all;close all;
pt=pcread('region_growing_tutorial.pcd');
pcData=pt.Location;
% pcData=load('building_Example2.txt');
% pcData=load('Building1.txt');
color=zeros(size(pcData,1),3);
pcData=[pcData color];
ptCloud = pointCloud(pcData(:,1:1:3),'Color',pcData(:,4:1:6));%读取txt文件的坐标信息以及颜色信息
% ptCloud = pointCloud(pcData(:,1:1:3));%读取txt文件的坐标信息
pcwrite(ptCloud, 'building_Example2_color.pcd', 'Encoding', 'ascii'); %将程序中的xyz数据写入pcd文件中
% pc = pcread('building_Example2_color.pcd');
pcshow(ptCloud); %显示点云
view(-37.5,30);
% 计算法向量
normals=pcnormals(ptCloud,10);
%2020-1-9
%使用PCA计算法向量和曲率pw
k=8;%近邻点数量为8
[pn,pw] = pca(pcData(:,1:1:3)', k); %输入为一个3*n的点云矩阵
pn=pn';
pw=pw';
% 法向量写入
ptCloud = pointCloud(pcData(:,1:1:3),'Color',pcData(:,4:1:6),'Normal' ,normals(:,1:3));
%显示法向量
u = pn(1:end,1);
v = pn(1:end,2);
w = pn(1:end,3);
x=pcData(:,1);
y=pcData(:,2);
z=pcData(:,3);
title('Adjusted Normals of Point Cloud')
hold on
quiver3(x, y, z, u, v, w);
hold off
% 创建一个新的格式 x,y,z,rgb,normal_x,normay_y,normal_z,curvature
PointCloud=struct('x',pcData(:,1),'y',pcData(:,2),'z',pcData(:,3),...
'rgb',pcData(:,4:1:6),...
'normal_x',pn(1,:),'normal_y',pn(2,:),'normal_z',pn(3,:),...
'curvature',pw);
%使用sort对曲率进行排序
[pw_,indice_pw]=sort(pw);%pw_为按照小到大顺序排好的数组,indice_pw为对应的点的索引
% %找出曲率最大的几个点,验证
% pw_first=indice_pw(size(indice_pw,1)-2500:1:size(indice_pw,1),:);%曲率最大值排前1000的点
%
% %找出前1000个点,放在pw_first1000里面
% for i=1:size(pw_first)
% Point_pw(i,1)=pcData(pw_first(i),1);
% Point_pw(i,2)=pcData(pw_first(i),2);
% Point_pw(i,3)=pcData(pw_first(i),3);
% end
%
% figure(2)
%
%
% plot3(pcData(:,1),pcData(:,2),pcData(:,3),'g.');
% hold on;
% plot3(Point_pw(:,1),Point_pw(:,2),Point_pw(:,3),'r.');
%
% hold off;
%
%
% title('曲率最大的几个点用红色标出');
%2020-1-14
% result=computeAngle(pn(1,:),pn(2,:))
% result=computeAngle(pn(1,:),pn(2,:))%计算出第一个点和第二个点的角度差
% %找到第一个点和第二个点的图上显示
% figure(2)
% plot3(pcData(:,1),pcData(:,2),pcData(:,3),'g.');
% hold on;
% plot3(pcData(1,1),pcData(1,2),pcData(1,3),'r.');
% plot3(pcData(2,1),pcData(2,2),pcData(2,3),'ro');
%2020-1-14
[pose,pose_indice]=getpointsXYZ(pcData(:,1:1:3),1);%pose为鼠标选取点的坐标,pose_indice为选取点的索引
%2020-1-15
%手动选择一个初始种子点,进行区域生长分割
k=8;
neighbors = transpose(knnsearch(pcData(:,1:3), pcData(:,1:3), 'k', k+1));%neighbor为一个索引矩阵,第一行代表第几个点,后8行代表K近邻的点。记录每个点及其周围的8个点
seeds=[];%种子点序列 ,第一列元素代表种子点的索引
cluster=[];%聚类后的点云簇
pcData(:,7)=0;%pcData的第7列为是否生长过的标志位,0代表未生长过,1代表生长过
current_neighbors8=[];%该种子点的近邻点,第一列为索引,后三列为x,y,z
Curvature_th=0.05 ;%设置曲率阈值
Angle_th=10;%设置角度阈值
seeds(1,:)=[pose_indice pose];%将种子点的栈拼接一下,由索引和三维坐标组成
cluster=seeds(1,2:end);%默认该种子点是属于某一类的
while(size(seeds)>0) %种子点为空时停止循环
current_seed=seeds(1,:);%将种子点栈的第一个种子点给current_seed,current_seed带索引
pcData(seeds(1,1),7)=1;%将种子点标记为已经生长过的点
seeds(1,:)=[];%第一个种子点为置为空
%根据current_seed在neighbors中找到8个邻域点
current_neighbors8_indice=neighbors(2:end,current_seed(1,1));%当前种子点的8个邻域点的索引数组,一个8x1的向量
%检查每一个邻域点 共8个
for seed_k=1:k%seed_k是1~8的数
current_neighbor=pcData(current_neighbors8_indice(seed_k),: );%current_neighbor是一个1x7的向量,存放8个近邻点中每一个近邻点的信息:x,y,z,r,g,b,flag
%判断当前邻域点是否生长过
%如果当前邻域点已经生长过就换下一个邻域点
if current_neighbor(1,7)==1
continue;
end
%如果当前邻域点没有生长过,就进行生长
%先计算角度是否满足阈值
current_neighbor_D_angle=computeAngle( pn(current_neighbors8_indice(seed_k),:),pn(current_seed(1,1),:) );%计算选取的邻域点和当前种子点的角度差
if(current_neighbor_D_angle<Angle_th)%邻域与当前种子点之间的夹角小于阈值°
cluster=[cluster;current_neighbor(1,1:3)];%将这个点加入到cluster中
pcData( current_neighbors8_indice(seed_k),7 )=1;%将这个近邻点的第7位置为1,代表已经进行生长过了
%判断当前点和初始点的曲率的差值是否小于阈值,可以作为新的种子点
if( abs(pw( pose_indice(1,1),1)-pw(current_neighbors8_indice(seed_k) ) )<Curvature_th)
seeds=[seeds;current_neighbors8_indice(seed_k) pcData(current_neighbors8_indice(seed_k),1:3)];
end
end
end
end
% figure(3)
% plot3(pcData(:,1),pcData(:,2),pcData(:,3),'g.');
% hold on;
plot3(cluster(:,1),cluster(:,2),cluster(:,3),'r.');
plot3(pose(1),pose(2),pose(3),'bo');
legend('原数据','分割结果','第一个种子点');
view(37.5,-45);
xlabel('x');
ylabel('y');
zlabel('z');
title('分割结果');
hold off;
figure(4)
plot3(cluster(:,1),cluster(:,2),cluster(:,3),'r.');
hold on;
xlabel('x');
ylabel('y');
zlabel('z');
title('分割结果');
hold off;
% 保存点云
% pcwrite(ptCloud,'building_Example2_color_normals.pcd','Encoding', 'ascii');%保存带有法向量的点云
getpointsXYZ.m
% 2020-1-14
% 抓取三维点云中的点的信息(包括曲率,法线)
%使用:选取点,然后按回车
function [pos,pos_indice] = getpointsXYZ(data,n)
hFigure= figure;
% scatter3(data(:,1),data(:,2),data(:,3),10,data(:,3),'filled')
plot3(data(:,1),data(:,2),data(:,3),'g.')
hold on;
datacursormode on
dcm_obj = datacursormode(hFigure);
pos = zeros(n,3);
for i = 1:n
disp('Click line to display a data tip, then press Return.')
% Wait while the user does this.
pause
c_info = getCursorInfo(dcm_obj);
pos(i,:) = c_info.Position;
%找出这个点
for j=1:size(data,1)
if( data(j,:)==pos(i,:) )
pos_indice(i)=j;%保存选取点的坐标
end
end
% pos_indice=find(data==repmat(pos(i,:),[size(data,1) 1]))
fprintf('\n索引点:%d\n',pos_indice)
end
% ————————————————
% 版权声明:本文为CSDN博主「阿阿阿昆」的原创文章,遵循 CC 4.0 BY-SA 版权协议,转载请附上原文出处链接及本声明。
% 原文链接:https://blog.csdn.net/qq_26447137/article/details/95502478
pca.m
% PCA主元分析法求法向量
% 输入:
% p:3*n的数值矩阵
% k:k近邻参数
% neighbors = transpose(knnsearch(transpose(p), transpose(p), 'k', k+1));
% neighbors一般可缺省。若之前做过k邻域求取操作也可直接调用,提高运算效率
% 输出
% n:法矢,已规定方向由邻域拟合出的平面指向查询点
% w:用于评估曲率的参数,详见:Mark P,et al. Multi-scale Feature Extraction on Point-Sampled Surfaces[J]. Computer Graphics Forum, 2010, 22(3): 281-289.
function [n,w] = pca(p, k, neighbors)%p为输入的点云
if nargin < 2
error('no bandwidth specified')
end
if nargin < 3
neighbors = transpose(knnsearch(transpose(p), transpose(p), 'k', k+1));%neighbor为一个索引矩阵,第一行代表第几个点,后8行代表K近邻的点。记录每个点及其周围的8个点
end
m = size(p,2);%返回第2维的维度
n = zeros(3,m); %存放法线的矩阵
w = zeros(1,m);
for i = 1:m
x = p(:,neighbors(2:end, i));%x为8个邻域点 ,3x8的矩阵
p_bar=mean(x,2);%每一行求均值(一共三行)
% P = (x - repmat(p_bar,1,k)) * transpose(x - repmat(p_bar,1,k));%中心化样本矩阵,再计算协方差矩阵
% P = 1/(k) * (x - repmat(p_bar,1,k)) * transpose(x - repmat(p_bar,1,k)); %邻域协方差矩阵P
P=(x - repmat(p_bar,1,k)) * transpose(x - repmat(p_bar,1,k))./(size(x,2)-1);
[V,D] = eig(P);%求P的特征值、特征向量。 D是对应的特征值对角矩阵,V是特征向量(因为协方差矩阵为实对称矩阵,故特征向量为单位正交向量)
[d0, idx] = min(diag(D)); %d0为最小特征值 idx为特征值的列数索引。diag():创建对角矩阵或获取矩阵的对角元素
n(:,i) = V(:,idx); % 最小特征值对应的特征向量为法矢,即法向量
%规定法矢方向指向
flag = p(:,i) - p_bar;%由近邻点的平均点指向对应点的向量
if dot(n(:,i),flag)<0%如果这个向量与法向量的数量积为负数(反向)
n(:,i)=-n(:,i);%法向量取反向
end
if nargout > 1
w(1,i)=abs(d0)./sum(abs(diag(D)));%最小特征值的绝对值在协方差矩阵特征值绝对值的总和中占的比重
end
end
computeAngle.m
%2020-1-14
%用于计算两个向量的夹角
%a,b为两个向量
function d_degree=computeAngle(a,b)
c=dot(a,b); %求内积
d=dot(a,a); %求a的长度
e=sqrt(d);
f=dot(b,b); %求b的长度
g=sqrt(f);
h=c/(e*g);
z=acos(h); %两个向量的夹角
% pi2angle
% d_degree=z/pi*180;
d_degree=min(z/pi*180,180-z/pi*180);
end
matlab分割效果