dltdecomp.m

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

function [t, r, intern, lambda] = DLTdecomp(dlt)
%DLTDECOMP	Decomposition of DLT matrix
%
% Syntax:
%  [t, r, intern, lambda] = DLTDECOMP(dlt)
%
% Input:
%  DLT = Direct Linear Transform matrix (3 x 4)
%
% Output:
%  T = [TX, TY, TZ] translation of camera
%  R = [ROLL, PITCH, YAW] rotation of camera
%  INTERN = [U0, V0, B1, B2, F] camera internal parameters (center, affine
%      distortions, focal length)
%  LAMBDA = scale factor (generally not needed)
%
% Decomposition is based on "Trond Melen: An Ambiguity Free Decomposition
% of the Direct Linear Transformation (DLT) Matrix, 1995"; e-mail:
% [e-mail]. Direct method, fast and numerically stable
% (QR decomposition).
%
% Limitations:
%  the left 3 x 3 submatrix of the DLT matrix should have full rank 3
%
% See also: DIRANGLES
%
% Radim Halir ([e-mail]), PRIP TU Wien
% 26.2.96
%

if (size(dlt) ~= [3, 4])
    error('dltdecomp: bad dlt matrix');
end

a = dlt(:, 1 : 3);
if (rank(a) ~= 3)
    error('dltdecomp: bad dlt matrix');
end

% translation
t = a \ dlt(:, 4);
t = -t';
lambda = sqrt(a(3, :) * a(3, :)');


% rq decomposition
[q r] = qr(inv(a / lambda));
q = inv(q);
r = inv(r);
j = eye(3);
if (r(1, 1) < 0.0)
    j(1, 1) = -1.0;
end;
if (r(2, 2) < 0.0)
    j(2, 2) = -1.0;
end;
if (r(3, 3) < 0.0)
    j(3, 3) = -1.0;
end;
r = r * j;

temp = - r(1, 2) / (r(1, 1) + r(2, 2));
cosphi = 1.0 / sqrt(1 + temp ^ 2);
sinphi = temp * cosphi;
s = eye(3);
s(1, 1) = cosphi;
s(1, 2) = sinphi;
s(2, 1) = -sinphi;
s(2, 2) = cosphi;

% rotation
g = r * s;
m = s' * j * q;
if (det(m) < 0.0)
    m = -m;
    lambda = -lambda;
end    
% r = dirangles(m);
% beware, r is used before in different meaning

% internal parameters
u0 = g(1, 3);
v0 = g(2, 3);
temp = g(1, 1) + g(2, 2);
b1 = (g(2, 2) - g(1, 1)) / temp;
b2 = -2 * g(1, 2) / temp;
f = 2 * (g(1, 1) * g(2, 2) - g(1, 2) ^ 2) / temp;
intern = [u0, v0, b1, b2, f];