estcdlt.m

来自「一个很好用的摄像机标定程序」· M 代码 · 共 34 行

function A=estCDLT(data)
%DLT2D Direct linear transform for coplanar control points, which
%calculates the initial camera position for nonlinear optimization.
%
%Usage:
%   A=estCDLT(data)
%
%where
%   data - n x 5 cordinates of controlpoints and their images.

%Reference:
%   Trond Melen: Geometrical modelling and calibration of video
%   cameras for underwater navigation, Ph.D.-thesis,
%   ITK-rapport 1994:103-W, NTH, Norway.
%
%   Version 2.0  15.5.-97
%   Janne Heikkila, University of Oulu, Finland

NDX=768; NDY=576; Sx=6.2031; Sy=4.6515; f0=16;
wx=data(:,1); wy=data(:,2); wz=data(:,3);
num=size(data,1);
u0=Sx/2; v0=Sy/2;

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

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

a=pinv(L)*l;
a(9)=1;
A=reshape(a,3,3)';