campose4.m

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

%CAMPOSE4	Camera pose estimation from 4 coplanar points.
%
%	T = CAMPOSE4(D, K)
%	[T, UV] = CAMPOSE4(D, K)
%
% Input is a table of data points, D, with each row of the form [x y iX iY] where (x,y)
% is the world coordinate of the point within the plane, and (iX, iY) is the image 
% plane coordinate.  K = [k1 k2] contains the X and Y-direction scale
% factors: k1 = kx * F, k2 = ky * F, where F is the focal length.
%
% Output is a homogeneous transformation, T, of the camera's pose with respect to the
% coordinate frame of the planar points.  The optional result, UV, is the coordinate
% of the principal point (in distance units).
%
% from Ganapathy "Camera Location Determination Problem",
% Bell Labs Tech. Memo 11358-841102-20-TM, Nov 2 1984
%
% SEE ALSO: CAMPOSE4, CAMCALP, CAMERA, CAMCALT, INVCAMCAL
%
%
%	Copyright (c) Peter Corke, 1999  Machine Vision Toolbox for Matlab

%		pic 4/93
function [T, UV] = campose4(xyXY, k, uv)

	t = camcald4(xyXY);

	lam1 = t(1,1)*t(3,2) - t(1,2)*t(3,1);
	lam2 = t(2,1)*t(3,2) - t(2,2)*t(3,1);
	if abs(lam1) < 1e-8,
		lam1 = 0;
	end
	if abs(lam2) < 1e-8,
		lam2 = 0;
	end

	if (lam1 == 0) & (lam2 == 0),
		r2 = (k(1)^2 + k(2)^2) / sqrt(t(1,1)^2 + t(1,2)^2 + t(2,1)^2 + t(2,2)^2);
		r = sqrt(r2);
		disp('problem with solution: lam1 = lam2 = 0');
		i = 1;
		f = 0;
		c = 0;
		g = 0;
		h = 0;
		
		u0 = uv(1);
		v0 = uv(2);
	else
		r2 = (t(3,1)^2 + t(3,2)^2)/( (lam1/k(1))^2 + (lam2/k(2))^2 );
		r = sqrt(r2);


		i2 = 1 - r2*(t(3,1)^2 + t(3,2)^2);
		i = sqrt(i2);
		f = -lam1/k(1)*r2;
		c = lam2/k(2)*r2;

		u0 = (t(1,1)*t(3,1) + t(1,2)*t(3,2) + k(1)*c*i/r2) / (t(3,1)^2 + t(3,2)^2);
		v0 = (t(2,1)*t(3,1) + t(2,2)*t(3,2) + k(2)*f*i/r2) / (t(3,1)^2 + t(3,2)^2);

		g = t(3,1)*r;
		h = t(3,2)*r;
	end

	a = (t(1,1)*r - u0*g) / k(1);
	b = (t(1,2)*r - u0*h) / k(1);
	d = (t(2,1)*r - v0*g) / k(2);
	e = (t(2,2)*r - v0*h) / k(2);
	p = (t(1,3)*r - u0*r) / k(1);
	q = (t(2,3)*r - v0*r) / k(2);

	R = [a b c;d e f;g h i];
	P = [p q r];

	T = [R P'; 0 0 0 1];
	UV = [u0 v0];