[725]  1  function [Y,dYdom,dYdT] = rigid_motion(X,om,T);


 2 


 3  %rigid_motion.m


 4  %


 5  %[Y,dYdom,dYdT] = rigid_motion(X,om,T)


 6  %


 7  %Computes the rigid motion transformation Y = R*X+T, where R = rodrigues(om).


 8  %


 9  %INPUT: X: 3D structure in the world coordinate frame (3xN matrix for N points)


 10  % (om,T): Rigid motion parameters between world coordinate frame and camera reference frame


 11  % om: rotation vector (3x1 vector); T: translation vector (3x1 vector)


 12  %


 13  %OUTPUT: Y: 3D coordinates of the structure points in the camera reference frame (3xN matrix for N points)


 14  % dYdom: Derivative of Y with respect to om ((3N)x3 matrix)


 15  % dYdT: Derivative of Y with respect to T ((3N)x3 matrix)


 16  %


 17  %Definitions:


 18  %Let P be a point in 3D of coordinates X in the world reference frame (stored in the matrix X)


 19  %The coordinate vector of P in the camera reference frame is: Y = R*X + T


 20  %where R is the rotation matrix corresponding to the rotation vector om: R = rodrigues(om);


 21  %


 22  %Important function called within that program:


 23  %


 24  %rodrigues.m: Computes the rotation matrix corresponding to a rotation vector


 25 


 26 


 27 


 28  if nargin < 3,


 29  T = zeros(3,1);


 30  if nargin < 2,


 31  om = zeros(3,1);


 32  if nargin < 1,


 33  error('Need at least a 3D structure as input (in rigid_motion.m)');


 34  return;


 35  end;


 36  end;


 37  end;


 38 


 39 


 40  [R,dRdom] = rodrigues(om);


 41 


 42  [m,n] = size(X);


 43 


 44  Y = R*X + repmat(T,[1 n]);


 45 


 46  if nargout > 1,


 47 


 48 


 49  dYdR = zeros(3*n,9);


 50  dYdT = zeros(3*n,3);


 51 


 52  dYdR(1:3:end,1:3:end) = X';


 53  dYdR(2:3:end,2:3:end) = X';


 54  dYdR(3:3:end,3:3:end) = X';


 55 


 56  dYdT(1:3:end,1) = ones(n,1);


 57  dYdT(2:3:end,2) = ones(n,1);


 58  dYdT(3:3:end,3) = ones(n,1);


 59 


 60  dYdom = dYdR * dRdom;


 61 


 62  end;


 63 


 64 


 65 


 66 

