camcalt.m

machine vision 工具箱

%CAMCALT	Camera calibration using Tsai's two-stage method.
%
% This method works when the calibration target comprises coplanar points.
%
%	[Tcam, f, k1]] = CAMCALT(D, PAR)
%
%	Compute a 3x4 camera calibration matrix from calibration data
%	using the method of Tsai.
%
%  	D is camera calibration data with rows of the form [x y z X Y] where 
%	(x,y,z) is the world coordinate,  and (X,Y) is the image coordinate
%
%	PAR is a vector of apriori knowledge:
%		 Ncx
%		 Nfx
%		 dx
%		 dy
%		 Cx	principal point, framestore coordinate of optical
%		 Cy	axis.
%
%	The output is an estimate of the camera's pose, the focal length, and 
%	a lens radial distortion coefficient k1.
%
% REF:	"A versatile camera calibration technique for high-accuracy 3D machine
%	vision metrology using off-the-shelf TV cameras and lens"
%	R.Y. Tsai, IEEE Trans R&A RA-3, No.4, Aug 1987, pp 323-344.
%
% SEE ALSO: CAMCALP, CAMCALD, INVCAMCAL, CAMERA
%
%	Copyright (c) Peter Corke, 1999  Machine Vision Toolbox for Matlab

% Peter Corke 2/92

function [Tcam f k1] = camcalt(camcal, par)
%
% manifest constants of camera sensor geometry and digitizer
%
	Ncx = par(1);
	Nfx = par(2);
	dx = par(3);
	dy = par(4);
	Cx = par(5);
	Cy = par(6);
%
% derived constants
%
	sx = Ncx/Nfx;
	dxp = sx * dx;

	X = Xf - Cx;
	Y = Yf - Cy;
	Xd = dx * X;
	Yd = dy * Y;

	z = [Yd.*xw Yd.*yw Yd -Xd.*xw -Xd.*yw] \ Xd;
	r1p = z(1);
	r2p = z(2);
	r4p = z(4);
	r5p = z(5);
	Txp = z(3);
	C = [r1p r2p; r4p r5p];
	if rank(C) == 2,
		Sr = r1p^2 + r2p^2 + r4p^2 + r5p^2;
		Ty2 = (Sr - sqrt(Sr^2 - 4*(r1p*r5p - r4p*r2p)^2)) / (2*(r1p*r5p - r4p*r2p)^2);
	else
		disp('unusual case')
		z = C(abs(C) > 0);
		Ty2 = 1.0 / (z(1)^2 + z(2)^2);
	end

	Ty = sqrt(Ty2);

%
% determine the sign of Ty
%

	%
	% find the calib point furthest from the center
	%
	[ymax i] = max(Xd.^2 + Yd.^2);

	r1 = r1p*Ty;
	r2 = r2p*Ty;
	r4 = r4p*Ty;
	r5 = r5p*Ty;
	Tx = Txp*Ty;
	x = r1*xw(i) + r2*yw(i) + Tx;
	y = r4*xw(i) + r5*yw(i) + Ty;

	if (sign(x) == sign(Xf(i))) & (sign(y) == sign(Yf(i))),
		Ty = Ty;
	else
		disp('sign of Ty reversed');
		Ty = -Ty;
	end

%
% determine the 3D rotation matrix R
%
	r1 = r1p*Ty;
	r2 = r2p*Ty;
	r4 = r4p*Ty;
	r5 = r5p*Ty;
	Tx = Txp*Ty;
	s = -sign(r1*r4 + r2*r5);
	R = [r1 r2 sqrt(1-r1^2-r2^2); r4 r5 s*sqrt(1-r4^2-r5^2)];
	R = [R(1:2,:); cross(R(1,:)', R(2,:)')];
	r7 = R(3,1);
	r8 = R(3,2);
	r9 = R(3,3);

	y = r4*xw+r5*yw+Ty;
	w = r7*xw+r8*yw;
	z = [y -dy*Y] \ [dy*(w.*Y)];
	f = z(1);

	if f < 0,
		disp('f is negative');
		R(1,3) = -R(1,3);
		R(2,3) = -R(2,3);
		R(3,1) = -R(3,1);
		R(3,2) = -R(3,2);
	end
	r6 = R(2,3);

%
% solve non-linear equation (8b) by minimization to find f, k1, Tz
%
	Tz = z(2);
	params = [r4 r5 r6 r7 r8 r9 dx dy sx Ty];
	z0 = [z; 0];		% add initial guess for k1
	z = fmins('camcalt8', z0, 0,[],params, xw, yw, zw, Xf-Cx, Yf-Cy);
	f = z(1);
	Tz = z(2);
	k1 = z(3);
	Tcam = [R [Tx Ty Tz]'; 0 0 0 1];	% the camera transform