biaoding.m

Tsai标定法的matlab实现 完成图像坐标到世界坐标的转换

%    [ Xw, Yw, Zw] = biaoding (xf, yf, Ncx, Nfx, dx,dy,Cx, Cy, sx,R,T,f,k1)
%     Note:        xf, xf, Xw, Yw, Yw are all column vectors

function [ Xw, Yw] = biaoding (xf, yf, Ncx, Nfx, dx, dy, Cx, Cy, sx,R,T,f,k1)
 
 dxp = dx * Ncx / Nfx;
 X = xf - Cx;
 Y = yf - Cy;
 Xd=sx^(-1)*dxp*X;
 Yd=dy*Y;
 r= (Xd.^2 + Yd.^2).^(0.5)   
 Dx= Xd.*((r.^2)*k1) ;                  
 Xu=Xd+Dx;                                           
 Dy= Yd.*((r.^2) *k1) ;                                
 Yu=Yd+Dy ;
 
r1=R(1,1);
r2=R(1,2);
r3=R(1,3);
r4=R(2,1);
r5=R(2,2);
r6=R(2,3);
r7=R(3,1);
r8=R(3,2);
r9=R(3,3);

 Tx=T(1,1);
 Ty=T(2,1);
 Tz=T(3,1);
 
   a=length(xf);
for i=1:a
    Xu1=Xu(i,1);
    Yu1=Yu(i,1);
  
Xw(i,1)=[ (-f*r5*Tx+f*Ty*r2+Yu1*r8*Tx-Yu1*Tz*r2+r5*Xu1*Tz-Ty*Xu1*r8)/(-Xu1*r7*r5-r1*Yu1*r8+r1*f*r5+Xu1*r8*r4+r2*Yu1*r7-r2*f*r4)]
Yw(i,1)=[ -(-Xu1*r7*Ty-r1*Yu1*Tz+r1*f*Ty+Xu1*Tz*r4+Tx*Yu1*r7-Tx*f*r4)/(-Xu1*r7*r5-r1*Yu1*r8+r1*f*r5+Xu1*r8*r4+r2*Yu1*r7-r2*f*r4)];
end