圆柱拟合.zip_matlab圆柱拟合_圆柱_圆柱拟合_拟合圆柱

function [r]=cylinderfitting(points) %points数据结构：行为点号n,点坐标x,y,z 列数为点数 %返回值r为计算结果参数矩阵『半径r，轴线起点x0,y0,z0，轴线法向xn,yn,zn， %循环次数你，单位权中误差s』,共9个参数 %第二种方法 %误差方程未化简，条件方程是某一坐标为众坐标的平均值 %如当方向向量（1 1 0）（0 1 0）时不行，若是（±1 ±1 ±1）则可以(仅在这里！！) %但是无法剔除粗差 % clear all % [filename,pathname]=uigetfile('*.txt','圆柱面上的坐标'); % i=find('.'==filename); % file=filename(1:i-1); % fid=fopen(strcat(pathname,filename),'rt'); % if(fid==-1) % msgbox('文件或路径错误','Warning','warn'); % return; % end % data=fscanf(fid,'%f',[4,inf]); data=points; % data=data'; name=data(:,1);X=data(:,2);Y=data(:,3);Z=data(:,4); % fclose(fid); n=size(data,1); % a=0.1220;b=-0.0380;c=0.9918;x0=5630.4;y0=2570.2;z0=2.1661; a=1;b=1;c=1;x0=mean(X);y0=mean(Y);z0=mean(Z); R=0.1756;para=ones(9,1);t=0; while max(abs(para(1:7)))>0.001 B=[];L=[]; if a<0 a=-a;b=-b;c=-c; end if a==0 if b<0 b=-b;c=-c; end if b==0 if c<0 c=-c; end end end s=sqrt(a^2+b^2+c^2)+0.0000001; %防止a b c 同时为0 a=a/s+0.0000001;b=b/s+0.0000001;c=c/s+0.0000001; for i=1:n D=a*(X(i)-x0)+b*(Y(i)-y0)+c*(Z(i)-z0); dx=x0+a*D-X(i);dy=y0+b*D-Y(i);dz=z0+c*D-Z(i); S=sqrt(dx^2+dy^2+dz^2); b1=(dx*(a*(X(i)-x0)+D)+dy*b*(X(i)-x0)+dz*c*(X(i)-x0))/S; b2=(dx*a*(Y(i)-y0)+dy*( b*(Y(i)-y0)+D)+dz*c*(Y(i)-y0))/S; b3=(dx*a*(Z(i)-z0)+dy*b*(Z(i)-z0)+dz*(c*(Z(i)-z0)+D))/S; b4=(dx*(1-a^2)-dy*a*b-dz*a*c)/S; b5=(-dx*a*b+dy*(1-b^2)-dz*b*c)/S; b6=(-dx*a*c-dy*b*c+dz*(1-c^2))/S; b7=-1; B=[B;b1 b2 b3 b4 b5 b6 b7]; l=[R-S]; L=[L;l]; end if abs(a)>=abs(b) && abs(a)>=abs(c) C=[2*a 2*b 2*c 0 0 0 0;0 0 0 1 0 0 0]; W=[1-a^2-b^2-c^2;mean(X)-x0]; end if abs(b)>=abs(a) && abs(b)>=abs(c) C=[2*a 2*b 2*c 0 0 0 0;0 0 0 0 1 0 0]; W=[1-a^2-b^2-c^2;mean(Y)-y0]; end if abs(c)>=abs(a) && abs(c)>=abs(b) C=[2*a 2*b 2*c 0 0 0 0;0 0 0 0 0 1 0]; W=[1-a^2-b^2-c^2;mean(Z)-z0]; end Nbb=B'*B;U=B'*L; N=[Nbb C';C zeros(2)]; UU=[U;W]; para=inv(N)*UU; a=a+para(1);b=b+para(2);c=c+para(3); x0=x0+para(4);y0=y0+para(5);z0=z0+para(6); R=R+para(7); V=B*para(1:7)-L; sigma=sqrt((sum(V.^2))/(n-5)); % for i=n:-1:1 % if abs(V(i))>2*sigma % name(i)=[];X(i)=[];Y(i)=[];Z(i)=[]; % end % end % n=size(X,1); t=t+1; end for i=1:n dx=x0+a^2*(X(i)-x0)+a*b*(Y(i)-y0)+a*c*(Z(i)-z0)-X(i); dy=y0+a*b*(X(i)-x0)+b^2*(Y(i)-y0)+b*c*(Z(i)-z0)-Y(i); dz=z0+a*c*(X(i)-x0)+b*c*(Y(i)-y0)+c^2*(Z(i)-z0)-Z(i); S(i)=sqrt(dx^2+dy^2+dz^2); d(i,1)=S(i)-R; end sigma=sqrt((sum(d.^2))/(n-5)); %控制精度 % a=round(a*1000000)/1000000; % b=round(b*1000000)/1000000; % c=round(c*1000000)/1000000; r=[R,x0,y0,z0,a,b,c,t,sigma*1000] % r=vpa(r,6); % r=round(r*100000000)/100000000; % [filename2,pathname2]=uiputfile('*.txt','参数及残差结果存储于...'); % fid2=fopen(strcat(pathname,file,'.bak'),'wt'); % if(fid2==-1) % msgbox('文件或路径错误','Warning','warn'); % return; % end % fprintf(fid2,'轴线的单位方向向量为(m)：\n'); % fprintf(fid2,'%10.6f%10.6f%10.6f\n',[a b c]); % fprintf(fid2,'轴线的起始点坐标为：\n'); % fprintf(fid2,' X0 Y0 Z0\n'); % fprintf(fid2,'%15.4f%15.4f%15.4f\n',[x0 y0 z0]); % fprintf(fid2,'圆柱半径为为(m)：\n'); % fprintf(fid2,'%8f\n',R); % fprintf(fid2,'循环次数为：\n'); % fprintf(fid2,'%4g\n',t); % fprintf(fid2,'剩余点数为：\n'); % fprintf(fid2,'%4g\n',n); % fprintf(fid2,'各点的圆度：\n'); % fprintf(fid2,' 点号 圆度（mm)\n'); % fprintf(fid2,'%4g%15.3f\n',[name d*1000]'); % fprintf(fid2,'单位权中误差为(mm)：\n'); % fprintf(fid2,'%8f\n',sigma*1000); % fprintf(fid2,'%8f %15.4f %15.4f %15.4f %10.6f %10.6f %10.6f %4g %8f',R,[x0 y0 z0],[a b c],t,sigma*1000); % fclose(fid2); % open(strcat(pathname,file,'.bak')); % end