www.gusucode.com > 三维建模源码程序 > 三维建模/code/multi-intersect/mvintersect.m
%_____________________________________________ MULTI PHOTO INTERSECTION ________________________________________ % the mathematical model is based on the collinearity equations in Photogrammetry and machine vision topics. % Input: 1-Orienation of the cameras "three angles [rad] and three coordinates[m] for each camera" % 2- focal length f [mm] - assume xo=yo=lens distortion=0 % 3- measured photo coordinates in [mm] in both photos % Note: for pixel coordinates: transform to p.p. system by knowing the pixel size and image dimensions % Output: % 3D metric coordinates of the image points by least square adjustment % code prepared by [Bashar S. Alsadik] 2012. %____________________________________________________________________________________________________________ function[XYZ,sx,sy,sz]=mvintersect(wpk,f,xp,yp) format short g XYZ=[0 0 0 ]; no=size(wpk,1); % no. of images match ext=wpk ; for i=1:no omega(i,1)=ext(i,1);phi(i,1)=ext(i,2);kappa(i,1)=ext(i,3);xo(i,1)=ext(i,4);yo(i,1)=ext(i,5);zo(i,1)=ext(i,6); %%%%%%%%%%%%%%%%%%%%%%%%%%%%%% Rotation Matrix M %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%% mw=[1 0 0;0 cos(omega(i,1)) sin(omega(i,1)) ;0 -sin(omega(i,1)) cos(omega(i,1))]; mp=[cos(phi(i,1)) 0 -sin(phi(i,1));0 1 0;sin(phi(i,1)) 0 cos(phi(i,1))]; mk=[cos(kappa(i,1)) sin(kappa(i,1)) 0;-sin(kappa(i,1)) cos(kappa(i,1)) 0;0 0 1]; m=mk*mp*mw; m11(i,1)= m(1,1); m12(i,1)= m(1,2); m13(i,1)= m(1,3); m21(i,1)= m(2,1); m22(i,1)= m(2,2); m23(i,1)= m(2,3); m31(i,1)= m(3,1); m32(i,1)= m(3,2); m33(i,1)= m(3,3); end xl=xp;yl=yp;B=[0 0 0];F=0; [xp,yp]; for i=1:size(xp,1) b11(i,1)=(xl(i,1)*m31(i,1)+(f*m11(i,1))); b12(i,1)=(xl(i,1)*m32(i,1)+(f*m12(i,1))); b13(i,1)=(xl(i,1)*m33(i,1)+(f*m13(i,1))); b21(i,1)=(yl(i,1)*m31(i,1)+(f*m21(i,1))); b22(i,1)=(yl(i,1)*m32(i,1)+(f*m22(i,1))); b23(i,1)=(yl(i,1)*m33(i,1)+(f*m23(i,1))); fx1(i,1)=-((-f*m11(i,1)-xl(i,1)*m31(i,1))*xo(i,1)-(f*m13(i,1)+xl(i,1)*m33(i,1))*zo(i,1)-(f*m12(i,1)+xl(i,1)*m32(i,1))*yo(i,1)) ; fy1(i,1)=-((-f*m21(i,1)-yl(i,1)*m31(i,1))*xo(i,1)-(f*m23(i,1)+yl(i,1)*m33(i,1))*zo(i,1)-(f*m22(i,1)+yl(i,1)*m32(i,1))*yo(i,1)) ; B=[B;[b11(i,1) b12(i,1) b13(i,1);b21(i,1) b22(i,1) b23(i,1)]]; F=[F;fx1(i,1);fy1(i,1)]; end F(1,:)=[];B(1,:)=[]; ff=F;b=B; % [B F]; % %%%%%%%%%%%%%%%%%%%%% least square %%%%%%%%%%%%%%%%%%%%%%%%%%%% btb=inv(b'*b); btf=b'* ff; delta=btb*btf ; res=b*delta-ff; sigm=(res'*res)/(size(b,1)-size(b,2)); vcov= (btb); sx=sqrt( (vcov(1,1))); sy=sqrt( (vcov(2,2))); sz=sqrt( (vcov(3,3))); j=1; xx = delta(j,1); yy = delta(j+1,1); zz = delta(j+2,1); XYZ=[XYZ;xx,yy,zz];b=[];ff=[];ql=[];sl=[];rl=[]; XYZ(1,:)=[];