Description of rodrigues

0001 function    [out,dout]=rodrigues(in)
0002 
0003 % RODRIGUES    Transform rotation matrix into rotation vector and viceversa.
0004 %
0005 %        Sintax:  [OUT]=RODRIGUES(IN)
0006 %         If IN is a 3x3 rotation matrix then OUT is the
0007 %        corresponding 3x1 rotation vector
0008 %         if IN is a rotation 3-vector then OUT is the
0009 %        corresponding 3x3 rotation matrix
0010 %
0011 
0012 %%
0013 %%        Copyright (c) March 1993 -- Pietro Perona
0014 %%        California Institute of Technology
0015 %%
0016 
0017 %% ALL CHECKED BY JEAN-YVES BOUGUET, October 1995.
0018 %% FOR ALL JACOBIAN MATRICES !!! LOOK AT THE TEST AT THE END !!
0019 
0020 %% BUG when norm(om)=pi fixed -- April 6th, 1997;
0021 %% Jean-Yves Bouguet
0022 
0023 %% Add projection of the 3x3 matrix onto the set of special ortogonal matrices SO(3) by SVD -- February 7th, 2003;
0024 %% Jean-Yves Bouguet
0025 
0026 % BUG FOR THE CASE norm(om)=pi fixed by Mike Burl on Feb 27, 2007
0027 
0028 
0029 [m,n] = size(in);
0030 %bigeps = 10e+4*eps;
0031 bigeps = 10e+20*eps;
0032 
0033 if ((m==1) & (n==3)) | ((m==3) & (n==1)) %% it is a rotation vector
0034     theta = norm(in);
0035     if theta < eps
0036         R = eye(3);
0037 
0038         %if nargout > 1,
0039 
0040         dRdin = [0 0 0;
0041             0 0 1;
0042             0 -1 0;
0043             0 0 -1;
0044             0 0 0;
0045             1 0 0;
0046             0 1 0;
0047             -1 0 0;
0048             0 0 0];
0049 
0050         %end;
0051 
0052     else
0053         if n==length(in)  in=in'; end;     %% make it a column vec. if necess.
0054 
0055         %m3 = [in,theta]
0056 
0057         dm3din = [eye(3);in'/theta];
0058 
0059         omega = in/theta;
0060 
0061         %m2 = [omega;theta]
0062 
0063         dm2dm3 = [eye(3)/theta -in/theta^2; zeros(1,3) 1];
0064 
0065         alpha = cos(theta);
0066         beta = sin(theta);
0067         gamma = 1-cos(theta);
0068         omegav=[[0 -omega(3) omega(2)];[omega(3) 0 -omega(1)];[-omega(2) omega(1) 0 ]];
0069         A = omega*omega';
0070 
0071         %m1 = [alpha;beta;gamma;omegav;A];
0072 
0073         dm1dm2 = zeros(21,4);
0074         dm1dm2(1,4) = -sin(theta);
0075         dm1dm2(2,4) = cos(theta);
0076         dm1dm2(3,4) = sin(theta);
0077         dm1dm2(4:12,1:3) = [0 0 0 0 0 1 0 -1 0;
0078             0 0 -1 0 0 0 1 0 0;
0079             0 1 0 -1 0 0 0 0 0]';
0080 
0081         w1 = omega(1);
0082         w2 = omega(2);
0083         w3 = omega(3);
0084 
0085         dm1dm2(13:21,1) = [2*w1;w2;w3;w2;0;0;w3;0;0];
0086         dm1dm2(13: 21,2) = [0;w1;0;w1;2*w2;w3;0;w3;0];
0087         dm1dm2(13:21,3) = [0;0;w1;0;0;w2;w1;w2;2*w3];
0088 
0089         R = eye(3)*alpha + omegav*beta + A*gamma;
0090 
0091         dRdm1 = zeros(9,21);
0092 
0093         dRdm1([1 5 9],1) = ones(3,1);
0094         dRdm1(:,2) = omegav(:);
0095         dRdm1(:,4:12) = beta*eye(9);
0096         dRdm1(:,3) = A(:);
0097         dRdm1(:,13:21) = gamma*eye(9);
0098 
0099         dRdin = dRdm1 * dm1dm2 * dm2dm3 * dm3din;
0100 
0101 
0102     end;
0103     out = R;
0104     dout = dRdin;
0105 
0106     %% it is prob. a rot matr.
0107 elseif ((m==n) & (m==3) & (norm(in' * in - eye(3)) < bigeps)...
0108         & (abs(det(in)-1) < bigeps))
0109     R = in;
0110 
0111     % project the rotation matrix to SO(3);
0112     [U,S,V] = svd(R);
0113     R = U*V';
0114 
0115     tr = (trace(R)-1)/2;
0116     dtrdR = [1 0 0 0 1 0 0 0 1]/2;
0117     theta = real(acos(tr));
0118 
0119 
0120     if sin(theta) >= 1e-4,
0121 
0122         dthetadtr = -1/sqrt(1-tr^2);
0123 
0124         dthetadR = dthetadtr * dtrdR;
0125         % var1 = [vth;theta];
0126         vth = 1/(2*sin(theta));
0127         dvthdtheta = -vth*cos(theta)/sin(theta);
0128         dvar1dtheta = [dvthdtheta;1];
0129 
0130         dvar1dR =  dvar1dtheta * dthetadR;
0131 
0132 
0133         om1 = [R(3,2)-R(2,3), R(1,3)-R(3,1), R(2,1)-R(1,2)]';
0134 
0135         dom1dR = [0 0 0 0 0 1 0 -1 0;
0136             0 0 -1 0 0 0 1 0 0;
0137             0 1 0 -1 0 0 0 0 0];
0138 
0139         % var = [om1;vth;theta];
0140         dvardR = [dom1dR;dvar1dR];
0141 
0142         % var2 = [om;theta];
0143         om = vth*om1;
0144         domdvar = [vth*eye(3) om1 zeros(3,1)];
0145         dthetadvar = [0 0 0 0 1];
0146         dvar2dvar = [domdvar;dthetadvar];
0147 
0148 
0149         out = om*theta;
0150         domegadvar2 = [theta*eye(3) om];
0151 
0152         dout = domegadvar2 * dvar2dvar * dvardR;
0153 
0154 
0155     else
0156         if tr > 0;             % case norm(om)=0;
0157 
0158             out = [0 0 0]';
0159 
0160             dout = [0 0 0 0 0 1/2 0 -1/2 0;
0161                 0 0 -1/2 0 0 0 1/2 0 0;
0162                 0 1/2 0 -1/2 0 0 0 0 0];
0163         else
0164 
0165             % case norm(om)=pi;
0166             if(0)
0167 
0168                 %% fixed April 6th by Bouguet -- not working in all cases!
0169                 out = theta * (sqrt((diag(R)+1)/2).*[1;2*(R(1,2:3)>=0)'-1]);
0170                 %keyboard;
0171 
0172             else
0173 
0174                 % Solution by Mike Burl on Feb 27, 2007
0175                 % This is a better way to determine the signs of the
0176                 % entries of the rotation vector using a hash table on all
0177                 % the combinations of signs of a pairs of products (in the
0178                 % rotation matrix)
0179 
0180                 % Define hashvec and Smat
0181                 hashvec = [0; -1; -3; -9; 9; 3; 1; 13; 5; -7; -11];
0182                 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;
0183                     1,-1,-1; 1,-1,1];
0184 
0185                 M = (R+eye(3,3))/2;
0186                 uabs = sqrt(M(1,1));
0187                 vabs = sqrt(M(2,2));
0188                 wabs = sqrt(M(3,3));
0189 
0190                 mvec = [M(1,2), M(2,3), M(1,3)];
0191                 syn  = ((mvec > 1e-4) - (mvec < -1e-4)); % robust sign() function
0192                 hash = syn * [9; 3; 1];
0193                 idx = find(hash == hashvec);
0194                 svec = Smat(idx,:)';
0195 
0196                 out = theta * [uabs; vabs; wabs] .* svec;
0197 
0198             end;
0199 
0200             if nargout > 1,
0201                 fprintf(1,'WARNING!!!! Jacobian domdR undefined!!!\n');
0202                 dout = NaN*ones(3,9);
0203             end;
0204         end;
0205     end;
0206 
0207 else
0208     error('Neither a rotation matrix nor a rotation vector were provided');
0209 end;
0210 
0211 return;
0212 
0213 %% test of the Jacobians:
0214 
0215 %%%% TEST OF dRdom:
0216 om = randn(3,1);
0217 dom = randn(3,1)/1000000;
0218 
0219 [R1,dR1] = rodrigues(om);
0220 R2 = rodrigues(om+dom);
0221 
0222 R2a = R1 + reshape(dR1 * dom,3,3);
0223 
0224 gain = norm(R2 - R1)/norm(R2 - R2a)
0225 
0226 %%% TEST OF dOmdR:
0227 om = randn(3,1);
0228 R = rodrigues(om);
0229 dom = randn(3,1)/10000;
0230 dR = rodrigues(om+dom) - R;
0231 
0232 [omc,domdR] = rodrigues(R);
0233 [om2] = rodrigues(R+dR);
0234 
0235 om_app = omc + domdR*dR(:);
0236 
0237 gain = norm(om2 - omc)/norm(om2 - om_app)
0238 
0239 
0240 %%% OTHER BUG: (FIXED NOW!!!)
0241 
0242 omu = randn(3,1);   
0243 omu = omu/norm(omu)
0244 om = pi*omu;        
0245 [R,dR]= rodrigues(om);
0246 [om2] = rodrigues(R);
0247 [om om2]
0248 
0249 %%% NORMAL OPERATION
0250 
0251 om = randn(3,1);         
0252 [R,dR]= rodrigues(om);
0253 [om2] = rodrigues(R);
0254 [om om2]
0255 
0256 return
0257 
0258 % Test: norm(om) = pi
0259 
0260 u = randn(3,1);
0261 u = u / sqrt(sum(u.^2));
0262 om = pi*u;
0263 R = rodrigues(om);
0264 
0265 R2 = rodrigues(rodrigues(R));
0266 
0267 norm(R - R2)
rodrigues

PURPOSE

SYNOPSIS

DESCRIPTION

CROSS-REFERENCE INFORMATION

SOURCE CODE
