anglcau.m

在点云处理中

function anglcau(varargin);
close all;
error(nargchk(0,2,nargin));
if nargin >=1
    Pstr=varargin{1};
else
    Pstr='lfront2.asc';
end
f = fopen(Pstr);
if (f == -1)
     error('File open error.');
end
fgets(f);fgets(f);
ps=fscanf(f,'%d',1);
n=0;m=0;
for j=1:ps
px=fscanf(f,'%f',1);py=fscanf(f,'%f',1);pz=fscanf(f,'%f',1);fgets(f);
if py>0 %&px*pz>0
    if px<0&pz<0
        n=n+1;
        x1(n)=px;
        y1(n)=py;z1(n)=pz;
    elseif px<0&pz>0
        m=m+1;
        x2(m)=px;y2(m)=py;z2(m)=pz;
     
    end
end
end
fclose(f);
xx=0;xy=0;xz=0;xsum=0;ysum=0;zsum=0;yy=0;zz=0;yz=0;
for i=1:n
    xx=x1(i)*x1(i)+xx;xy=x1(i)*y1(i)+xy;xz=x1(i)*z1(i)+xz;yy=y1(i)*y1(i)+yy;yz=y1(i)*z1(i)+yz;zz=z1(i)*z1(i)+zz;
    xsum=x1(i)+xsum;ysum=y1(i)+ysum;zsum=z1(i)+zsum;
end
xxt=0;xyt=0;xzt=0;xtsum=0;ytsum=0;ztsum=0;yyt=0;zzt=0;yzt=0;
for i=1:m
    xxt=x2(i)*x2(i)+xxt;xyt=x2(i)*y2(i)+xyt;xzt=x2(i)*z2(i)+xzt;yyt=y2(i)*y2(i)+yyt;yzt=y2(i)*z2(i)+yzt;zzt=z2(i)*z2(i)+zzt;
    xtsum=x2(i)+xtsum;ytsum=y2(i)+ytsum;ztsum=z2(i)+ztsum;
end
%下面用svd分解求平面的ax+by+cz+d=0,norm(a,b,c)=1;
%第一个平面
U=[xx xy xz xsum;
   xy yy yz ysum;
   xz yz zz zsum;
   xsum ysum zsum n];
[P,Q,R]=svd(U);
N1=R(:,4);
sn=N1(1:3);
sn=sn/norm(sn);
%第二个面
V=[xxt xyt xzt xtsum;
   xyt yyt yzt ytsum;
   xzt yzt zzt ztsum;
   xtsum ytsum ztsum m;];
[P1,Q1,R1]=svd(V);
N2=R1(:,4);
sn2=N2(1:3);
sn2=sn2/norm(sn2);
angl=acos(sn'*sn2);
angl=180*angl/pi;
angl=180-angl-9;
sprintf('点云文件%s 两个平面夹角拟合的角度: %f\n',Pstr,angl)