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

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


global GDLT;
global GIp;
global GSp;
global GInt;
global Gerrs;

Gerrs = [-1];
GDLT= DLT;
GIp = Ip;
GSp = Sp;
GInt = inv([1,0,-u0;0,1,-v0;0,0,1]) * inv([1+b1,b2,0;b2,1-b1,0;0,0,1]) * [Fl,0,0;0,Fl,0;0,0,1];

[Te, Re] = getTR( DLT, Fl, u0, v0, b1, b2, Sp(:,1:2) );

% prevod normaly na sfericke souradnice

Vnorm = cross( Re(:,1)', Re(:,2)' );
[a1, b1] = dirangles( Vnorm );

% vypocet uhlu natoceni kolmych vektoru okolo normaly

NR = dir2rot( Vnorm );
v1 = NR * Re(:,1);

v1 = v1 / vsize(v1);
% vezmu uhel prvniho - druhy dostanu prictenim pi/2
Fi = acos( v1( 1,1 ) );
if( v1(2,1) < 0 )
	Fi = 2 * pi - Fi;
end

X0 = [(a1 / 180) * pi; (b1 / 180) * pi; Fi; Te( 3, 3 )];

%options = 1;

% getTR_fun( X0 )

options(2) = 0.001;
options(3) = 0.0001;
X0 = solvopt( X0, 'getTRZ_fun', '', options );

[r c] = size( Gerrs );


[n11, n12, n13] = sph2cart( X0(1,1), X0(2,1), 1 );
Fi1 = X0(3,1);

Fi2 = Fi1 + pi/2;

NR = dir2rot( [n11, n12, n13] );
v11 = cos( Fi1 );
v12 = sin( Fi1 );
v21 = cos( Fi2 );
v22 = sin( Fi2 );

ne1 = (NR') * [v11;v12;0];
ne2 = (NR') * [v21;v22;0];

ne1 = ne1 / vsize( ne1 );
ne2 = ne2 / vsize( ne2 );
ne3 = cross( ne1, ne2 );
ne3 = ne3 / vsize( ne3 );

z0 = X0(4,1);
[n m] = size( Sp );

Q = GInt * [ne1, ne2, ne3];

imgP = inv(Q) * hext(GIp)';
imgP = hnorm( imgP' );

x0 = sum( (imgP(:,1) .* z0) - Sp(:,1) ) / n;
y0 = sum( (imgP(:,2) .* z0) - Sp(:,2) ) / n;

R = [ne1, ne2, ne3];
T = [1,0,x0;0,1,y0;0,0,z0];

[Al, Be, Ga] = deeuler( R );
disp( sprintf( '%.1f, %.1f, %.1f;  %f, %f, %f\n', x0, y0, z0, Al, Be, Ga ) );

newP = (GInt * R * T) * hext(Sp(:,1:2))';

newP = hnorm( newP' );

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