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 

