gettr.m

一个很好用的摄像机标定程序

function [T, R, errs] = getTR( DLT, Fl, u0, v0, b1, b2, Sp )
%DEVELOPMENT PHASE
%
%  function [T, R] = getTR( DLT, Fl, u0, v0, b1, b2, Sp )
%
%  decomposes the 3x3 CDLT matrix 
%
%returns
%  T - translation matrix
%  R - rotation matrix  
%  errs - difference between the original CDLT matrix and matrix composed of the decomposed parameters
%
%parametres
%  DLT - 3x3 CDLT matrix
%  Fl - focal length
%  u0, v0 - coordinates of the principal point
%  b1, b2 - linear distortion coefficients 

F = [Fl, 0, 0; 0, Fl, 0; 0, 0, 1]; 
V = [1, 0, -u0; 0, 1, -v0; 0, 0, 1];
B = [1 + b1, b2, 0; b2, 1 - b1, 0; 0, 0, 1];

Q = inv( F ) * B * V * DLT;

lam1 = vsize( Q(:,1) );
lam2 = vsize( Q(:,2) );

lambda = (lam1 + lam2) / 2;

M1 = Q(:,1);
M2 = Q(:,2);

% we have the approximataion first two columns of the rotation matrix
% but they are not orthogonal

M1 = M1 / lam1;
M2 = M2 / lam2;

% so we have to make the orthogonal - we will rotate them, but they are in 3D

MN = cross( M1, M2 );

Ro = dir2rot( MN' );

% we transform them into 2D

R1 = Ro * M1;
R2 = Ro * M2;

coAng = dot( R1, R2 );


roAng = (acos( coAng ) - pi/2) / 2;

if( R1(2,1) > R2(2,1) )
	if( R1(2,1) < 0 )
		a1 = acos(dot( R1, [1,0,0]')) + roAng;	
		y1 = sin( a1 );
		y1 = y1 * -1;
	else
		a1 = acos(dot( R1, [1,0,0]')) - roAng;	
		y1 = sin( a1 );
	end
	x1 = cos( a1 );
	if( R2(2,1) < 0 )
		a2 = acos(dot( R2, [1,0,0]')) - roAng;	
		y2 = sin( a2 );
		y2 = y2 * -1;
	else
		a2 = acos(dot( R2, [1,0,0]')) + roAng;	
		y2 = sin( a2 );
	end
	x2 = cos( a2 );
	R1(1,1) = x1;
	R1(2,1) = y1;
	R2(1,1) = x2;
	R2(2,1) = y2;
else
	if( R2(2,1) < 0 )
		a1 = acos(dot( [1,0,0]', R2)) + roAng;
		y1 = sin( a1 );
		y1 = y1 * -1;
	else
		a1 = acos(dot( [1,0,0]', R2)) - roAng;
		y1 = sin( a1 );
	end
	x1 = cos( a1 );	
	if( R1(2,1) < 0 )
		a2 = acos(dot( [1,0,0]', R1)) - roAng;
		y2 = sin( a2 );
		y2 = y2 * -1;
	else
		a2 = acos(dot( [1,0,0]', R1)) + roAng;
		y2 = sin( a2 );
	end
	x2 = cos( a2 );	
	R2(1,1) = x1;
	R2(2,1) = y1;
	R1(1,1) = x2;
	R1(2,1) = y2;
end

R1 = inv(Ro) * R1;
R2 = inv(Ro) * R2;

R3 = cross( R1, R2 );

R = [R1,R2,R3];

Tv = Q(:,3)/lambda;

%T3 = ols( Tv, R );
T3 = R \ Tv;

T = [[1,0,0]',[0,1,0]',T3];

% predpoklad - vime kde je kamera....
if( T(3,3) < 0 )
    %T( :, 3 ) = T( :, 3 ) .* -1;
end

%errors
newDLT = inv(V) * inv(B) * F * R * T;
%newDLT = newDLT / newDLT(3,3);
%newDLT = newDLT .* 123.8

newP = newDLT * hext(Sp)';
newP = hnorm( newP' );

Ip = DLT * hext(Sp)';
Ip = hnorm( Ip' );

errs1 = vsize( Ip - newP );
errs = mean( errs1 );