clc
clear
%获取点云并显示
load('dianyun.txt','-ascii')%用ASCII码打开dianyun.txt点云文档
R=dianyun;%点云数据保存在P里
x=dianyun(:,1);%建立x坐标
y=dianyun(:,2);%建立y坐标
z=dianyun(:,3);%建立z坐标
scatter3(x,y,z)%以x,y,z描绘点云三维图像
m=size(x);%确定点的个数
K=30;
m1=0;
for i=1:m
m=0;
p=[];
L=[];
Q=[];
x1=R(i,1);
y1=R(i,2);
z1=R(i,3);
Q=[x1,y1,z1];
%邻域
idx=knnsearch(Q,R,K);
P=R(idx,:);
x2=P(:,1);
y2=P(:,2);
z2=P(:,3);
%拟合平面
r=[x2 y2 ones(length(x2),1)]\z2 ; %最小二乘法拟合函数
A=r(1);
B=r(2);
C=-1;
D=r(3);
N=[A,B,C];%平面法向量
N=N/norm(N);%向量单位化
%把点投影到拟合平面上并求出坐标
kk=-(A*x1+B*y1+C*z1+D)/(A^2+B^2+C^2);
%求出的投影坐标
x3=x2+kk*A;
y3=y2+kk*B;
z3=z2+kk*C;
p=[x3,y3,z3];
%待判断点投影坐标
x4=x1+kk*A;
y4=y1+kk*B;
z4=z1+kk*C;
%投影面上,其他投影点到待判断投影点的向量
for k=1:K
x5=p(k,1)-x4;
y5=p(k,2)-y4;
z5=p(k,3)-z4;
if x5~=0||y5~=0||z5~=0
LL=[x5,y5,z5];
m=m+1;
L(m,:)=LL;
end
end
%求向量的旋转角
for k1=1:m
l1=L(1,:);
l2=L(k1,:);
ff1=dot(l1,l2);
ff2=cross(l1,l2);
ff3=dot(ff2,N);
theta=atan2(ff3,ff1);
if theta<0
theta=pi-theta;
end
T(k1)=theta;
end
T=sort(T,'descend'); %从大到小排列旋转角
for k2=1:m
if k2<m
t(k2)=T(k2)-T(k2+1);
end
if k2==m
t(k2)=T(k2);
end
end
ma=max(t);
if ma>=pi/2
m1=m1+1;
M(m1,:)=[x1,y1,z1];
end
end
x0=M(:,1);
y0=M(:,2);
z0=M(:,3);
%显示边缘点结果
figure(2)
scatter3(x0,y0,z0)