[810]  1  %=======================================================================


 2  % Copyright 20082014, 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 


[725]  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 

