estdlt.m

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

function [DLT, errstat, errors] = estDLT(coords, showres)
% ONLY FOR TESTING PURPOSES
%
% coords ... n x 4 (x, y, u, v)
% showres (default TRUE) ... TRUE -> show results, FALSE -> do not show results
% DLT ... 3 x 4 "general" DLT
% errstat = [mean, max, std] statistics of errors
% errors ... n x 3, errors in the object space
%
%
% general 3D calibration based on 3x4 DLT
% computes the 3 by 4 DLT matrix DLT from n (n >= 7) object points (x, y, z)
% and the coresponding image points (u, v)
% standard approach (i.e. constraint DLT(3, 4) = 1)
% errors are measured in object space (using pseudoinverse of DLT)

% default parameters
if (nargin < 2)
    showres = 1;
end


% world and image coordinates
p = coords(:, 1 : 3);
q = coords(:, 4 : 5);

% estimate the matrix using LSQ
[n m] = size(p);
M = [p, ones(n, 1), zeros(n, 4), -p(:, 1) .* q(:, 1), -p(:, 2) .* q(:, 1), ...
-p(:, 3) .* q(:, 1), zeros(n, 4), p, ones(n, 1), -p(:, 1) .* q(:, 2), ...
-p(:, 2) .* q(:, 2), -p(:, 3) .* q(:, 2)];
M = reshape(M', 11, 2 * n)';
temp = q';
temp1 = reshape( temp, 2*n, 1 );

% nahrada temp(:) za temp1(:)
DLT = reshape([M \ temp1(:); 1], 4, 3)';

% return

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

% display results
if (showres)
    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)));
end