www.pudn.com > calibr8.zip > decCDLT.m

function [T, R, f] = decCDLT(DLT, u, v, b1, b2)
%DEVELOPMENT PHASE
%

V = [1, 0, - u; 0, 1, - v; 0, 0, 1]
B = [1 + b1, b2, 0; b2, 1 - b1, 0; 0 0 1]
% V * DLT
M = B * V * DLT

a = M(1, 1);
b = M(2, 1);
c = M(3, 1);
d = M(1, 2);
e = M(2, 2);
f = M(3, 2);
g = M(1, 3);
h = M(2, 3);
i = M(3, 3);

mL = [1 - c^2; 1 - f^2; - c * f];
mR = [a^2 + b^2; d^2 + e^2; a * d + b * e];
lf = sqrt(mL \ mR)

R1 = [a / lf; b / lf; c]
R2 = [d / lf; e / lf; f]
vsize( R1' )
vsize( R2' )
lambda = 1 / ((vsize( R1' ) + vsize( R2' )) / 2)
R1 = R1 / vsize(R1');
R2 = R2 / vsize(R2');
R3 = cross(R1, R2)
vsize( R3' )
% R3 = R3/ vsize( R3' );

R = [R1, R2, R3]

RT = R;

R1a = cross(R2, R3);
R2a = cross(R3, R1);
R1 = (R1 + R1a) / 2;
R2 = (R2 + R2a) / 2;
R1 = R1 / vsize(R1');
R2 = R2 / vsize(R2');

R = [R1, R2, R3]

F = [lf, 0, 0; 0, lf, 0; 0, 0, 1]

T = (inv(R))*(inv(F))*M
T(1,3) = T(1,3) / T(1,1)
T(2,3) = T(2,3) / T(2,2)
     
% RT = R;
% R = R';

% R = RT; 

t1 = (-1/lf)*(DLT(1,3)*((1+b1)*R(1,1) + b2*R(2,1)) + DLT(2,3)*(b2*R(1,1) + (1-b1)*R(2,1)) - DLT(3,3)*((1+b1)*R(1,1)*u + b2*R(2,1)*u + b2*R(1,1)*v + (1-b1)*R(2,1)*v - R(3,1)*lf))

t2 = (-1/lf)*(DLT(1,3)*((1+b1)*R(1,2) + b2*R(2,2)) + DLT(2,3)*(b2*R(1,2) + (1-b1)*R(2,2)) - DLT(3,3)*((1+b1)*R(1,2)*u + b2*R(2,2)*u + b2*R(1,2)*v + (1-b1)*R(2,2)*v - R(3,2)*lf) )

t3 = (-1/lf)*(DLT(1,3)*((1+b1)*R(1,3) + b2*R(2,3)) + DLT(2,3)*(b2*R(1,3) + (1-b1)*R(2,3)) - DLT(3,3)*((1+b1)*R(1,3)*u + b2*R(2,3)*u + b2*R(1,3)*v + (1-b1)*R(2,3)*v - R(3,3)*lf) )

% R = RT;

MM = M * inv(R)
M1 = MM(:, 1);
M2 = MM(:, 2);
M3 = MM(:, 3);


R1 = M1 / sqrt(dot(M1, M1));
R2 = M2 / sqrt(dot(M2, M2));
R3 = cross(R1, R2);
RI = [R1, R2, R3]


lam = (dot(M1, M1) + dot(M2, M2)) / 2;
lam = sqrt(lam)

T = [1 0; 0 1; 0 0];
T = [T, - M3 / lambda]
residua = (lambda * R * T - M) ./ M
err = sum(abs(residua(:,:)))

f = lf;

T( : , 3 ) = [-t1; -t2; -t3];