camcalp.m

来自澳大利亚Qeensland大学的计算机视觉Matlab工具箱。 This Toolbox provides a number of functions that are useful in co

%CAMCALP	Camera calibration matrix from parameters
%
%	C = CAMCALP(cp)
%	C = CAMCALP(cp, Tcam)
%	C = CAMCALP(cp, pC, x, z)
%
%	Compute a 3x4 camera calibration matrix from given camera intrinsic
%	and extrinsic parameters.
%       CP is a camera parameter vector comprising:
%		cp(1)   f, the focal length of the lens (m)
%        	cp(2:3) alpha is a 2-element vector of horizontal and 
%			vertical pixel pitch of the sensor (pixels/m)
%	 	cp(4:5) p0 is a 2-element vector of principal point (u0, v0)
%			in pixels,
%			If length(cp) == 3, then p0 defaults to (0,0)
%
%        Tcam is the pose of the camera wrt the world frame, defaults to
%		identity matrix if not given (optical axis along Z-axis).
%
%	Alternatively the camera pose can be given by specifying the coordinates
%	of the center, pC, and unit vectors for the camera's x-axis and 
%	z-axis (optical axis).
%
%
%
%  	f, alphax and alphay are commonly known as the intrinsic camera 
%	parameters.  Tcam is commonly known as the extrinsic camera parameters.
%
% NOTE:	 that this calibration matrix includes the lens image inversion, so
%	that the camera coordinate system is:
%
%		0------------------> X
%		|
%		|
%		|	+ (principal point)
%		|
%		|
%		v
% 
% SEE ALSO:  camera, pulnix
%
%	Copyright (c) Peter Corke, 1999  Machine Vision Toolbox for Matlab

function C = camcalp(cp, A, B, C)
	f = cp(1);
	alpha = cp(2:3);
	if length(cp) <=3,
		p0 = [ 0 0];
	else
		p0 = cp(4:5);
	end
	if nargin == 1,
		Tcam = eye(4);
	elseif nargin == 2,
		Tcam = A;
	elseif nargin == 4,
		pC = A(:);
		x = unit(B(:));
		z = unit(C(:));
		if abs(dot(x,z)) > 1e-10,
			error('x and z vectors should be orthogonal');
		end
		R=[x unit(cross(z,x)) z];

		Tcam = transl(pC) * [R zeros(3,1); 0 0 0 1];
	end

 	C = [	alpha(1) 0 p0(1) 0; 
		0 alpha(2) p0(2) 0;
		0 0 1 0
	    ] * [ 1 0 0 0;
		  0 1 0 0;
		  0 0 -1/f 1;
		  0 0 0 1] * inv(Tcam);