Tsai.rar_tsai_三维坐标转换_图像坐标_图像坐标转换

function [R, T, f, k1] = Tsai(Xf, Yf, xw, yw, zw, Ncx, Nfx, dx, dy, Cx, Cy, sx) dxp = dx * Ncx / Nfx; X = Xf - Cx; Y = Yf - Cy; Xd=sx^(-1)*dxp*(Xf-Cx); Yd=dy*(Yf-Cy); A=[Yd.*xw Yd.*yw Yd -Xd.*xw -Xd.*yw]; B=Xd; C=A\B; r1p=C(1); r2p=C(2); Txp=C(3); r4p=C(4); r5p=C(5); clear A B C; C=[r1p, r2p; r4p, r5p]; Sr=r1p^2 + r2p^2 + r4p^2 + r5p^2; if rank(C)==2 Ty2=( Sr - (Sr^2-4*(r1p*r5p-r4p*r2p)^2)^(0.5) )/(2*(r1p*r5p-r4p*r2p)^2); else z = C(abs(C) > 0); Ty2 = 1.0 / (z(1)^2 + z(2)^2); end Ty = sqrt(Ty2); clear C Sr Ty2 z [ymax i] = max(Xd.^2 + Yd.^2); r1 = r1p*Ty; r2 = r2p*Ty; r4 = r4p*Ty; r5 = r5p*Ty; Tx = Txp*Ty; x = r1*xw(i) + r2*yw(i) + Tx; y = r4*xw(i) + r5*yw(i) + Ty; if (sign(x) == sign(Xf(i))) & (sign(y) == sign(Yf(i))), Ty = Ty; else Ty = -Ty; end clear ymax i x y r1 = r1p*Ty; r2 = r2p*Ty; r4 = r4p*Ty; r5 = r5p*Ty; Tx = Txp*Ty; s = -sign(r1*r4 + r2*r5); R=[r1, r2, (1-r1^2-r2^2)^(0.5); r4, r5, s*(1-r4^2-r5^2)^(0.5)]; R = [R(1:2,:); cross(R(1,:), R(2,:))]; r7 = R(3,1); r8 = R(3,2); r9 = R(3,3); y = r4*xw+r5*yw+Ty; w = r7*xw+r8*yw; z = [y -dy*Y] \ [dy*(w.*Y)]; f = z(1); if f < 0, R(1,3) = -R(1,3); R(2,3) = -R(2,3); R(3,1) = -R(3,1); R(3,2) = -R(3,2); end r3 = R(1,3); r6 = R(2,3); r7 = R(3,1); r8 = R(3,2); clear s y w z y = r4*xw+r5*yw+Ty; w = r7*xw+r8*yw; z = [y -dy*Y] \ [dy*(w.*Y)]; f = z(1); Tz = z(2); params_const = [r4 r5 r6 r7 r8 r9 dx dy sx Ty]; params = [f, Tz, 0]; % add initial guess for k1 [x,fval,exitflag,output] = fminsearch( @Tsai_8b, params, [], params_const, xw, yw, zw, X, Y); f = x(1); Tz = x(2); k1 = x(3); T=[Tx, Ty, Tz]';