gettrz_fun.m

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

function Res = getTRZ_fun( X0 )
%
% function that is minimized in getTR3
%

global GDLT;
global GSp;
global GIp;
global GInt;
global 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( GSp );

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

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

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

newE = [ne1, ne2, ne3] * [1,0,x0;0,1,y0;0,0,z0];

newP = (GInt * newE) * hext(GSp(:,1:2))';

newP = hnorm( newP' );

errs = vsize( GIp - newP );
Res = sum( errs .^ 2);
%Res = errs;
Gerrs = [Gerrs, mean(errs)];