1  %=======================================================================


2  % Copyright 20082015, LEGI UMR 5519 / CNRS UJF GINP, Grenoble, France


3  % http://www.legi.grenobleinp.fr


4  % Joel.Sommeria  Joel.Sommeria (A) legi.cnrs.fr


5  %


6  % This file is part of the toolbox UVMAT.


7  %


8  % UVMAT is free software; you can redistribute it and/or modify


9  % it under the terms of the GNU General Public License as published


10  % by the Free Software Foundation; either version 2 of the license,


11  % or (at your option) any later version.


12  %


13  % UVMAT is distributed in the hope that it will be useful,


14  % but WITHOUT ANY WARRANTY; without even the implied warranty of


15  % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the


16  % GNU General Public License (see LICENSE.txt) for more details.


17  %=======================================================================


18 


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


20 


21  %rigid_motion.m


22  %


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


24  %


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


26  %


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


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


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


30  %


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


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


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


34  %


35  %Definitions:


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


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


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


39  %


40  %Important function called within that program:


41  %


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


43 


44 


45 


46  if nargin < 3,


47  T = zeros(3,1);


48  if nargin < 2,


49  om = zeros(3,1);


50  if nargin < 1,


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


52  return;


53  end;


54  end;


55  end;


56 


57 


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


59 


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


61 


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


63 


64  if nargout > 1,


65 


66 


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


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


69 


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


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


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


73 


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


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


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


77 


78  dYdom = dYdR * dRdom;


79 


80  end;


81 


82 


83 


84 

