esticdlt.m

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

function [CDLT] = estiCDLT(coords)
%DEVELOPMENT PHASE
%
% coords ... n x 4 (x, y, u, v)
% showres (default TRUE) ... TRUE -> show results, FALSE -> do not show results
% CDLT ... 3 x 3 "coplanar" DLT
% errstat = [mean, max, std] statistics of errors
% errors ... n x 2, errors in the object space (i.e. original plane)
%
% coplanar calibration based on 3x3 DLT
% computes the 3 by 3 "coplanar" DLT matrix CDLT from n >= 4 object points
% (x, y) and the coresponding image points (u, v)
% standard approach (i.e. constraint CDLT(3, 3) = 1)
% errors are measured in object space

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

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

CDLT = reshape([M \ temp(:); 1], 3, 3)';