iteratethem2.m

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

function [T, R, fl, u0, v0, b1, b2, errstat] = iterateThem2()
%DEVELOPMENT PHASE
%
% [T, R, fl, u0, v0, b1, b2] = iterateThem2()
%
% performs one iteration 

global GA1;
global GA2;
global GA3;
global Gfl;
global Gu0;
global Gv0;
global Gb1;
global Gb2;
global Gpoints1;
global Gpoints2;
global Gpoints3;

[Te1, Re1, Errs] = getTR( GA1, Gfl, Gu0, Gv0, Gb1, Gb2 );
[Te2, Re2, Errs] = getTR( GA2, Gfl, Gu0, Gv0, Gb1, Gb2 );
[Te3, Re3, Errs] = getTR( GA3, Gfl, Gu0, Gv0, Gb1, Gb2 );

HTe1 = eye( 4 );
HTe2 = eye( 4 );
HTe3 = eye( 4 );

HRe1 = eye( 4 );
HRe2 = eye( 4 );
HRe3 = eye( 4 );

HTe1(1:3,4) = Te1(:,3);
HTe2(1:3,4) = Te2(:,3);
HTe3(1:3,4) = Te3(:,3);

HRe1(1:3,1:3) = Re1;
HRe2(1:3,1:3) = Re2;
HRe3(1:3,1:3) = Re3;

HSp1 = hext( Gpoints1(:,1:3) );
HSp2 = hext( Gpoints2(:,1:3) );
HSp3 = hext( Gpoints3(:,1:3) );

dKe1 = inv( HTe1 ) * inv( HRe1 ) * HRe2 * HTe2;
dKe2 = inv( HTe1 ) * inv( HRe1 ) * HRe3 * HTe3;

HSpoints2e = dKe1 * HSp2';
HSpoints3e = dKe2 * HSp3';

HSpoints2e = HSpoints2e';
HSpoints3e = HSpoints3e';

PC = makePCoords( HSp1(:,1:3), Gpoints1(:,4:5), HSpoints2e(:,1:3), Gpoints2(:,4:5), HSpoints3e(:,1:3), Gpoints3(:,4:5) );

[AE, errstat] = estDLT( PC );

[Te, Re, Ine, Lae] = decDLT( AE );

T = eye( 3 );
T(:,3) = -Te';

R = Re;

u0 = Ine(1,1);
v0 = Ine(1,2);
b1 = Ine(1,3);
b2 = Ine(1,4);
fl = Ine(1,5);