estidlt.m

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

function [DLT, errstat] = estiDLT( data )

p = data(:, 1 : 3);
q = data(:, 4 : 5);
[n m] = size(p);

NDX=768; NDY=576; Sx=6.2031; Sy=4.6515; f0=16;
%NDX=sys(1); NDY=sys(2); Sx=sys(3); Sy=sys(4); f0=sys(5);
wx=data(:,1); wy=data(:,2); wz=data(:,3);
num=size(data,1);

u = Sx*data(:,4)/NDX;
v = Sy*data(:,5)/NDY;

Lu=[wx wy wz 0*u+1 0*u 0*u 0*u 0*u -wx.*u -wy.*u -wz.*u];
Lv=[0*v 0*v 0*v 0*v wx wy wz 0*v+1 -wx.*v -wy.*v -wz.*v];
L=reshape([Lu';Lv'],11,2*num)';
l=reshape([u';v'],2*num,1);

a=pinv(L)*l;
a(12)=1;
DLT=reshape(a,4,3)'; 

newq = applyDLT(DLT, p);
%newq = newq ./ 123.809
% errors = newp - p;
errors = newq - (q./123.809);
xyzmeans = sum(abs(errors)) ./ n;
xyzoffsets = sum(errors) ./ n;
err = vsize(errors);
errstat = [mean(err), max(err), std(err)];

% display results
    disp(sprintf('%d points, estimated DLT:', n));
    disp(DLT);
    disp('Mean and shift for each axis:');
    disp(sprintf('  X: %f, %f', xyzmeans(1), xyzoffsets(1)));
    disp(sprintf('  Y: %f, %f', xyzmeans(2), xyzoffsets(2)));
    %disp(sprintf('  Z: %f, %f', xyzmeans(3), xyzoffsets(3)));
    disp(sprintf('Errors in the object space: mean = %f, max = %f, std = %f', ...
         errstat(1), errstat(2), errstat(3)));