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 


19  function [out,dout]=rodrigues(in)


20 


21  % RODRIGUES Transform rotation matrix into rotation vector and viceversa.


22  %


23  % Sintax: [OUT]=RODRIGUES(IN)


24  % If IN is a 3x3 rotation matrix then OUT is the


25  % corresponding 3x1 rotation vector


26  % if IN is a rotation 3vector then OUT is the


27  % corresponding 3x3 rotation matrix


28  %


29 


30  %%


31  %% Copyright (c) March 1993  Pietro Perona


32  %% California Institute of Technology


33  %%


34 


35  %% ALL CHECKED BY JEANYVES BOUGUET, October 1995.


36  %% FOR ALL JACOBIAN MATRICES !!! LOOK AT THE TEST AT THE END !!


37 


38  %% BUG when norm(om)=pi fixed  April 6th, 1997;


39  %% JeanYves Bouguet


40 


41  %% Add projection of the 3x3 matrix onto the set of special ortogonal matrices SO(3) by SVD  February 7th, 2003;


42  %% JeanYves Bouguet


43 


44  % BUG FOR THE CASE norm(om)=pi fixed by Mike Burl on Feb 27, 2007


45 


46 


47  [m,n] = size(in);


48  %bigeps = 10e+4*eps;


49  bigeps = 10e+20*eps;


50 


51  if ((m==1) & (n==3))  ((m==3) & (n==1)) %% it is a rotation vector


52  theta = norm(in);


53  if theta < eps


54  R = eye(3);


55 


56  %if nargout > 1,


57 


58  dRdin = [0 0 0;


59  0 0 1;


60  0 1 0;


61  0 0 1;


62  0 0 0;


63  1 0 0;


64  0 1 0;


65  1 0 0;


66  0 0 0];


67 


68  %end;


69 


70  else


71  if n==length(in) in=in'; end; %% make it a column vec. if necess.


72 


73  %m3 = [in,theta]


74 


75  dm3din = [eye(3);in'/theta];


76 


77  omega = in/theta;


78 


79  %m2 = [omega;theta]


80 


81  dm2dm3 = [eye(3)/theta in/theta^2; zeros(1,3) 1];


82 


83  alpha = cos(theta);


84  beta = sin(theta);


85  gamma = 1cos(theta);


86  omegav=[[0 omega(3) omega(2)];[omega(3) 0 omega(1)];[omega(2) omega(1) 0 ]];


87  A = omega*omega';


88 


89  %m1 = [alpha;beta;gamma;omegav;A];


90 


91  dm1dm2 = zeros(21,4);


92  dm1dm2(1,4) = sin(theta);


93  dm1dm2(2,4) = cos(theta);


94  dm1dm2(3,4) = sin(theta);


95  dm1dm2(4:12,1:3) = [0 0 0 0 0 1 0 1 0;


96  0 0 1 0 0 0 1 0 0;


97  0 1 0 1 0 0 0 0 0]';


98 


99  w1 = omega(1);


100  w2 = omega(2);


101  w3 = omega(3);


102 


103  dm1dm2(13:21,1) = [2*w1;w2;w3;w2;0;0;w3;0;0];


104  dm1dm2(13: 21,2) = [0;w1;0;w1;2*w2;w3;0;w3;0];


105  dm1dm2(13:21,3) = [0;0;w1;0;0;w2;w1;w2;2*w3];


106 


107  R = eye(3)*alpha + omegav*beta + A*gamma;


108 


109  dRdm1 = zeros(9,21);


110 


111  dRdm1([1 5 9],1) = ones(3,1);


112  dRdm1(:,2) = omegav(:);


113  dRdm1(:,4:12) = beta*eye(9);


114  dRdm1(:,3) = A(:);


115  dRdm1(:,13:21) = gamma*eye(9);


116 


117  dRdin = dRdm1 * dm1dm2 * dm2dm3 * dm3din;


118 


119 


120  end;


121  out = R;


122  dout = dRdin;


123 


124  %% it is prob. a rot matr.


125  elseif ((m==n) & (m==3) & (norm(in' * in  eye(3)) < bigeps)...


126  & (abs(det(in)1) < bigeps))


127  R = in;


128 


129  % project the rotation matrix to SO(3);


130  [U,S,V] = svd(R);


131  R = U*V';


132 


133  tr = (trace(R)1)/2;


134  dtrdR = [1 0 0 0 1 0 0 0 1]/2;


135  theta = real(acos(tr));


136 


137 


138  if sin(theta) >= 1e4,


139 


140  dthetadtr = 1/sqrt(1tr^2);


141 


142  dthetadR = dthetadtr * dtrdR;


143  % var1 = [vth;theta];


144  vth = 1/(2*sin(theta));


145  dvthdtheta = vth*cos(theta)/sin(theta);


146  dvar1dtheta = [dvthdtheta;1];


147 


148  dvar1dR = dvar1dtheta * dthetadR;


149 


150 


151  om1 = [R(3,2)R(2,3), R(1,3)R(3,1), R(2,1)R(1,2)]';


152 


153  dom1dR = [0 0 0 0 0 1 0 1 0;


154  0 0 1 0 0 0 1 0 0;


155  0 1 0 1 0 0 0 0 0];


156 


157  % var = [om1;vth;theta];


158  dvardR = [dom1dR;dvar1dR];


159 


160  % var2 = [om;theta];


161  om = vth*om1;


162  domdvar = [vth*eye(3) om1 zeros(3,1)];


163  dthetadvar = [0 0 0 0 1];


164  dvar2dvar = [domdvar;dthetadvar];


165 


166 


167  out = om*theta;


168  domegadvar2 = [theta*eye(3) om];


169 


170  dout = domegadvar2 * dvar2dvar * dvardR;


171 


172 


173  else


174  if tr > 0; % case norm(om)=0;


175 


176  out = [0 0 0]';


177 


178  dout = [0 0 0 0 0 1/2 0 1/2 0;


179  0 0 1/2 0 0 0 1/2 0 0;


180  0 1/2 0 1/2 0 0 0 0 0];


181  else


182 


183  % case norm(om)=pi;


184  if(0)


185 


186  %% fixed April 6th by Bouguet  not working in all cases!


187  out = theta * (sqrt((diag(R)+1)/2).*[1;2*(R(1,2:3)>=0)'1]);


188  %keyboard;


189 


190  else


191 


192  % Solution by Mike Burl on Feb 27, 2007


193  % This is a better way to determine the signs of the


194  % entries of the rotation vector using a hash table on all


195  % the combinations of signs of a pairs of products (in the


196  % rotation matrix)


197 


198  % Define hashvec and Smat


199  hashvec = [0; 1; 3; 9; 9; 3; 1; 13; 5; 7; 11];


200  Smat = [1,1,1; 1,0,1; 0,1,1; 1,1,0; 1,1,0; 0,1,1; 1,0,1; 1,1,1; 1,1,1;


201  1,1,1; 1,1,1];


202 


203  M = (R+eye(3,3))/2;


204  uabs = sqrt(M(1,1));


205  vabs = sqrt(M(2,2));


206  wabs = sqrt(M(3,3));


207 


208  mvec = [M(1,2), M(2,3), M(1,3)];


209  syn = ((mvec > 1e4)  (mvec < 1e4)); % robust sign() function


210  hash = syn * [9; 3; 1];


211  idx = find(hash == hashvec);


212  svec = Smat(idx,:)';


213 


214  out = theta * [uabs; vabs; wabs] .* svec;


215 


216  end;


217 


218  if nargout > 1,


219  fprintf(1,'WARNING!!!! Jacobian domdR undefined!!!\n');


220  dout = NaN*ones(3,9);


221  end;


222  end;


223  end;


224 


225  else


226  error('Neither a rotation matrix nor a rotation vector were provided');


227  end;


228 


229  return;


230 


231  %% test of the Jacobians:


232 


233  %%%% TEST OF dRdom:


234  om = randn(3,1);


235  dom = randn(3,1)/1000000;


236 


237  [R1,dR1] = rodrigues(om);


238  R2 = rodrigues(om+dom);


239 


240  R2a = R1 + reshape(dR1 * dom,3,3);


241 


242  gain = norm(R2  R1)/norm(R2  R2a)


243 


244  %%% TEST OF dOmdR:


245  om = randn(3,1);


246  R = rodrigues(om);


247  dom = randn(3,1)/10000;


248  dR = rodrigues(om+dom)  R;


249 


250  [omc,domdR] = rodrigues(R);


251  [om2] = rodrigues(R+dR);


252 


253  om_app = omc + domdR*dR(:);


254 


255  gain = norm(om2  omc)/norm(om2  om_app)


256 


257 


258  %%% OTHER BUG: (FIXED NOW!!!)


259 


260  omu = randn(3,1);


261  omu = omu/norm(omu)


262  om = pi*omu;


263  [R,dR]= rodrigues(om);


264  [om2] = rodrigues(R);


265  [om om2]


266 


267  %%% NORMAL OPERATION


268 


269  om = randn(3,1);


270  [R,dR]= rodrigues(om);


271  [om2] = rodrigues(R);


272  [om om2]


273 


274  return


275 


276  % Test: norm(om) = pi


277 


278  u = randn(3,1);


279  u = u / sqrt(sum(u.^2));


280  om = pi*u;


281  R = rodrigues(om);


282 


283  R2 = rodrigues(rodrigues(R));


284 


285  norm(R  R2)

