calibr8.m

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

function [T, R, Int, errs, cerr] = calibr8( IInt, maxcount, tolerance, PC1, PC2, PC3, PC4, PC5, PC6 )
% DEVELOPMENT PHASE
%
% [T, R, Int, errs] = calibr8( Int, maxcount, tolerance, PC1, PC2, PC3, PC4 )
%
% computes the camera parameters using iterative proces of 
% decompositions and recompositions of CDLT/DLT matrix
% input is taken from global variables
%
% returns:
% T - translation matrix
% R - rotation matrix
% Int - internal parameters
% IInt - initial guess of internal parameters
% fl - focal lenght
% u0, v0 - position of the centre of the image plane
% b1, b2 - linear distorion coefficients
% errs - errors in each step + for all the planes

clear G*;

global Gfl;
global Gu0;
global Gv0;
global Gb1;
global Gb2;
global GoldP;
global GSp;
global GA;
global GIpts;
global GHSp;
global Gno_views;

Gno_views = nargin - 3;

for i = 1:Gno_views
	eval( ['GIpts{' num2str(i) '} = PC' num2str(i) '(:,4:5);'] );
	eval( ['GHSp{' num2str(i) '} = PC' num2str(i) '(:,1:3);'] );
	eval( ['Pc' num2str(i) ' = PC' num2str(i) '(:,1:2);'] );
	eval( ['Pc' num2str(i) '(:,3:4) = PC' num2str(i) '(:,4:5);'] );
	eval( ['GA{' num2str(i) '} = estiCDLT( Pc' num2str(i) ');'] );
end

Gfl = IInt(1,1);
Gu0 = IInt(1,2);
Gv0 = IInt(1,3);
Gb1 = IInt(1,4);
Gb2 = IInt(1,5);

count = 1;
errs = initErrs;

while( count < maxcount )
	[T, R, fl, u0, v0, b1, b2, PC, err] = minThem;
	count = count+1
	errs(:,count) = err;
	if( fl < 0 )
		fl = 1;
	end	
	Gfl = fl;
	Gb1 = b1;
	Gb2 = b2;
	%Gb2 = 0;
	if( (Gb1^2 + Gb2^2) > 1 )
		Gb1 = 0;
		Gb2 = 0;
	end		
	disp( sprintf( 'fl: %f, u0: %f, v0: %f, b1: %f, b2: %f\n', fl, u0, v0, b1, b2 ) ); 
	if( errs(1,count) < tolerance )
		break;
	end	
end

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

T(:,3) = T(:,3) * -1;
pom = PC1(:,1:2);
AE = createCDLT( fl, b1, b2, u0, v0, T, R );
TT = AE * hext( pom )';
TT = hnorm( TT' );
VV = PC1(:,4:5) - TT;
cerr = PC1(:,4:5);
cerr(:,3:4) = VV;