project.m

用ＭＡＴＬＡＢ实现ＬＭ算法库

function y = project( x, M )
% PROJECT a 3D point using the image projection model
% This function projects a point in three space onto an
% image plane, given by the camera translation and rotation,
% as well as its focal length.   J. Watlington, 11/18/95

%  The format of the trajectory data is :
%  tx ty tz wx wy wz B   per row output

arbitrary_scaling_constant = 1;

%  Allocate the rotation matrices

R  = zeros([3,3]);
Rx = zeros([3,3]);
Ry = zeros([3,3]);
Rz = zeros([3,3]);

%  Generate rotation matrix from angular rotation parms

even = cos( M( 4 ) );
odd = sin( M( 4 ) );
Rx( 1, 1 ) = 1;
Rx( 2, 2 ) = even;
Rx( 2, 3 ) = -odd;
Rx( 3, 2 ) = odd;
Rx( 3, 3 ) = even;

even = cos( M( 5 ) );
odd = sin( M( 5 ) );
Ry( 2, 2 ) = 1;
Ry( 1, 1 ) = even;
Ry( 1, 3 ) = odd;
Ry( 3, 1 ) = -odd;
Ry( 3, 3 ) = even;

even = cos( M( 6 ) );
odd = sin( M( 6 ) );
Rz( 3, 3 ) = 1;
Rz( 1, 1 ) = even;
Rz( 1, 2 ) = -odd;
Rz( 2, 1 ) = odd;
Rz( 2, 2 ) = even;

R = Rx * Ry * Rz;

% Camera Transformation 

Xc =  M( 1:3 )' + (R * x);

%  Perspective projection

y = zeros( [1, 2] );	   % preallocate results

denom = arbitrary_scaling_constant / (1 + (M( 7 ) * Xc( 3 )));
y(1) = Xc(1) * denom;
y(2) = Xc(2) * denom;