基于matlab的相机标定程序

% VPSOL % FAUCCAL supporting script that performs a % least squares solution for estimating % vanish points coordinates with simultaneous % estimation of radial distortion parameters % Least squares solution with vanish line sigc=inf; SYN=0; lenxv=length(xv(isfinite(xv))); if (lenxv>2 && isfinite(xv(1)) && isfinite(xv(2)) && rs>3 && cs>3) xv2=xv; inlp2=inlp; ina2=ina; inc2=inc; yv2=yv; k2=kiv; spost=inf; term=0; if ~isfinite(xv(3)) obs=obsnh+obsnv+obsnd1; % Observation number un=5+lnh+lnv+lnd1; % Unknown number elseif ~isfinite(xv(4)) obs=obsnh+obsnv+obsnd2; un=5+lnh+lnv+lnd2; else obs=obsnh+obsnv+obsnd1+obsnd2; un=6+lnh+lnv+lnd1+lnd2; end A=zeros(obs,un); dl=zeros(obs,1); v=0; r=obs-un; % Least squares solution while term==0 clear hia hic i=1; pon=1; mtr=1; lnum=1; % Fill A matrix and dl vector while pon~=0 mtr2=0; if isfinite(xv2(i)) for j=1:(lena(i)+lenc(i)) switch (i) case 1 while length(horl(j+mtr2,(horl(j+mtr2,:)>0)))<3 mtr2=mtr2+1; end for k=1:size(horl,2) if horl(j+mtr2,k)~=0 pntx=impoint00(impoint00(:,4)==horl(j+mtr2,k),5); pnty=impoint00(impoint00(:,4)==horl(j+mtr2,k),6); fcv=fcvprd(xv2(i),yv2(i),inlp2,k2,ina2(i,j+mtr2),inc2(i,j+mtr2),pntx,pnty); A(mtr,1)=fcv(1); if lenxv==4 A(mtr,5:8)=fcv(2:5); A(mtr,8+lnum)=fcv(6); else A(mtr,4:7)=fcv(2:5); A(mtr,7+lnum)=fcv(6); end dl(mtr,1)=-fcv(7); mtr=mtr+1; end end lnum=lnum+1; case 2 while length(verl(j+mtr2,(verl(j+mtr2,:)>0)))<=2 mtr2=mtr2+1; end for k=1:size(verl,2) if verl(j+mtr2,k)~=0 pntx=impoint00(impoint00(:,4)==verl(j+mtr2,k),5); pnty=impoint00(impoint00(:,4)==verl(j+mtr2,k),6); fcv=fcvprd(xv2(i),yv2(i),inlp2,k2,ina2(i,j+mtr2),inc2(i,j+mtr2),pntx,pnty); A(mtr,2)=fcv(1); if lenxv==4 A(mtr,5:8)=fcv(2:5); A(mtr,8+lnum)=fcv(6); else A(mtr,4:7)=fcv(2:5); A(mtr,7+lnum)=fcv(6); end dl(mtr,1)=-fcv(7); mtr=mtr+1; end end lnum=lnum+1; case 3 while length(diag2(j+mtr2,(diag2(j+mtr2,:)>0)))<=2 mtr2=mtr2+1; end for k=1:size(diag2,2) if diag2(j+mtr2,k)~=0 pntx=impoint00(impoint00(:,4)==diag2(j+mtr2,k),5); pnty=impoint00(impoint00(:,4)==diag2(j+mtr2,k),6); fcv=fcvprd(xv2(i),yv2(i),inlp2,k2,ina2(i,j+mtr2),inc2(i,j+mtr2),pntx,pnty); A(mtr,3)=fcv(1); if lenxv==4 A(mtr,5:8)=fcv(2:5); A(mtr,8+lnum)=fcv(6); else A(mtr,4:7)=fcv(2:5); A(mtr,7+lnum)=fcv(6); end dl(mtr,1)=-fcv(7); mtr=mtr+1; end end lnum=lnum+1; case 4 while length(diag1(j+mtr2,(diag1(j+mtr2,:)>0)))<=2 mtr2=mtr2+1; end for k=1:size(diag1,2) if diag1(j+mtr2,k)~=0 pntx=impoint00(impoint00(:,4)==diag1(j+mtr2,k),5); pnty=impoint00(impoint00(:,4)==diag1(j+mtr2,k),6); fcv=fcvprd(xv2(i),yv2(i),inlp2,k2,ina2(i,j+mtr2),inc2(i,j+mtr2),pntx,pnty); if lenxv==4 A(mtr,4)=fcv(1); A(mtr,5:8)=fcv(2:5); A(mtr,8+lnum)=fcv(6); else A(mtr,3)=fcv(1); A(mtr,4:7)=fcv(2:5); A(mtr,7+lnum)=fcv(6); end dl(mtr,1)=-fcv(7); mtr=mtr+1; end end lnum=lnum+1; end end i=i+1; if i==5 pon=0; end else i=i+1; if i==5 pon=0; end end end clear fcv lnum mtr pon pntx pnty Nmat=A'*A; n=A'*dl; Na=inv(Nmat); dx=Na*n; vn=A*dx-dl; so2postn=(vn'*vn)/r; spostn=sqrt(so2postn); if spostn>spost term=1; clear spostn so2postn vn else spost=spostn; end if term~=1 v=vn; xv=xv2; kiv=k2; so2post=so2postn; inlp=inlp2; ina=ina2; inc=inc2; if lenxv==4 if inlp(1)==1 xv2(1:4)=xv(1:4)+dx(1:4)'; inlp2(2)=inlp(2)+dx(5); inlp2(3)=inlp(3)+dx(6); k2(1:2)=kiv(1:2)+dx(7:8)'; yv2(1:4)=yv(1:4)+inlp(2)*xv2(1:4)+inlp(3); else yv2(1:4)=yv(1:4)+dx(1:4)'; inlp2(2)=inlp(2)+dx(5); inlp2(3)=inlp(3)+dx(6); k2(1:2)=kiv(1:2)+dx(7:8)'; xv2(1:4)=xv(1:4)+inlp(2)*yv2(1:4)+inlp(3); end else if inlp(1)==1 if isfinite(xv(3)) xv2(1:3)=xv(1:3)+dx(1:3)'; inlp2(2)=inlp(2)+dx(4); inlp2(3)=inlp(3)+dx(5); k2(1:2)=kiv(1:2)+dx(6:7)'; else xv2(1:2)=xv(1:2)+dx(1:2)';