fautosc.m

Faugeras Toscani Camera Calibration

function [K,MT] = FauTosc(uvXYZ,u0,v0)
[M,N]=size(uvXYZ);

% Boucle pour cr閑r A et B
j=1;
for(i=1:M)  
    A(j,1)=uvXYZ(i,3);
    A(j,2)=uvXYZ(i,4);
    A(j,3)=uvXYZ(i,5);
    A(j,4)=1;
    A(j,5:8)=0;
    A(j,9)=-uvXYZ(i,1);
    A(j+1,1:4)=0;
    A(j+1,5)=uvXYZ(i,3);
    A(j+1,6)=uvXYZ(i,4);
    A(j+1,7)=uvXYZ(i,5);
    A(j+1,8)=1;
    A(j+1,9)=-uvXYZ(i,2);

    B(j,1)=-uvXYZ(i,1)*uvXYZ(i,3);
    B(j,2)=-uvXYZ(i,1)*uvXYZ(i,4);
    B(j,3)=-uvXYZ(i,1)*uvXYZ(i,5);
    B(j+1,1)=-uvXYZ(i,2)*uvXYZ(i,3);
    B(j+1,2)=-uvXYZ(i,2)*uvXYZ(i,4);
    B(j+1,3)=-uvXYZ(i,2)*uvXYZ(i,5);
    j=j+2;
end


% Cr閍tion de D
D=B'*B-B'*A*inv(A'*A)*A'*B;
[U2,W2,V2]=svd(D);

% Extraction de p et q
V2
eig(V2)

q=V2(:,3)
p=-inv(A'*A)*A'*B*q;

% Cr閍tion de M
M(1,1)=p(1,1);
M(1,2)=p(2,1);
M(1,3)=p(3,1);
M(1,4)=p(4,1);
M(2,1)=p(5,1);
M(2,2)=p(6,1);
M(2,3)=p(7,1);
M(2,4)=p(8,1);
M(3,4)=p(9,1);
M(3,1)=q(1,1);
M(3,2)=q(2,1);
M(3,3)=q(3,1);

u0 = dot(M(1,1:3),M(3,1:3));
v0 = dot(M(2,1:3),M(3,1:3));
alpha_u = - norm( cross(M(1,1:3),M(3,1:3)) );
alpha_v =   norm( cross(M(2,1:3),M(3,1:3)) );

K=[alpha_u 0 u0
    0 alpha_v v0
    0 0 1];


tx=(M(1,4)-u0*M(3,4))/alpha_u;
ty=(M(2,4)-v0*M(3,4))/alpha_v;
tz=M(3,4);

R(1,:)=(M(:,1)-u0*M(:,3))/alpha_u;
R(2,:)=M(:,2)/alpha_v;
R(3,:)=M(:,3);

T=[tx ; ty ; tz];
MT=horzcat(R,T);
MT=[MT;0 0 0 1];