[37] | 1 | function [out,dout]=rodrigues(in)
|
---|
| 2 |
|
---|
| 3 | % RODRIGUES Transform rotation matrix into rotation vector and viceversa.
|
---|
| 4 | %
|
---|
| 5 | % Sintax: [OUT]=RODRIGUES(IN)
|
---|
| 6 | % If IN is a 3x3 rotation matrix then OUT is the
|
---|
| 7 | % corresponding 3x1 rotation vector
|
---|
| 8 | % if IN is a rotation 3-vector then OUT is the
|
---|
| 9 | % corresponding 3x3 rotation matrix
|
---|
| 10 | %
|
---|
| 11 |
|
---|
| 12 | %%
|
---|
| 13 | %% Copyright (c) March 1993 -- Pietro Perona
|
---|
| 14 | %% California Institute of Technology
|
---|
| 15 | %%
|
---|
| 16 |
|
---|
| 17 | %% ALL CHECKED BY JEAN-YVES BOUGUET, October 1995.
|
---|
| 18 | %% FOR ALL JACOBIAN MATRICES !!! LOOK AT THE TEST AT THE END !!
|
---|
| 19 |
|
---|
| 20 | %% BUG when norm(om)=pi fixed -- April 6th, 1997;
|
---|
| 21 | %% Jean-Yves Bouguet
|
---|
| 22 |
|
---|
| 23 | %% Add projection of the 3x3 matrix onto the set of special ortogonal matrices SO(3) by SVD -- February 7th, 2003;
|
---|
| 24 | %% Jean-Yves Bouguet
|
---|
| 25 |
|
---|
| 26 | [m,n] = size(in);
|
---|
| 27 | %bigeps = 10e+4*eps;
|
---|
| 28 | bigeps = 10e+20*eps;
|
---|
| 29 |
|
---|
| 30 | if ((m==1) & (n==3)) | ((m==3) & (n==1)) %% it is a rotation vector
|
---|
| 31 | theta = norm(in);
|
---|
| 32 | if theta < eps
|
---|
| 33 | R = eye(3);
|
---|
| 34 |
|
---|
| 35 | %if nargout > 1,
|
---|
| 36 |
|
---|
| 37 | dRdin = [0 0 0;
|
---|
| 38 | 0 0 1;
|
---|
| 39 | 0 -1 0;
|
---|
| 40 | 0 0 -1;
|
---|
| 41 | 0 0 0;
|
---|
| 42 | 1 0 0;
|
---|
| 43 | 0 1 0;
|
---|
| 44 | -1 0 0;
|
---|
| 45 | 0 0 0];
|
---|
| 46 |
|
---|
| 47 | %end;
|
---|
| 48 |
|
---|
| 49 | else
|
---|
| 50 | if n==length(in) in=in'; end; %% make it a column vec. if necess.
|
---|
| 51 |
|
---|
| 52 | %m3 = [in,theta]
|
---|
| 53 |
|
---|
| 54 | dm3din = [eye(3);in'/theta];
|
---|
| 55 |
|
---|
| 56 | omega = in/theta;
|
---|
| 57 |
|
---|
| 58 | %m2 = [omega;theta]
|
---|
| 59 |
|
---|
| 60 | dm2dm3 = [eye(3)/theta -in/theta^2; zeros(1,3) 1];
|
---|
| 61 |
|
---|
| 62 | alpha = cos(theta);
|
---|
| 63 | beta = sin(theta);
|
---|
| 64 | gamma = 1-cos(theta);
|
---|
| 65 | omegav=[[0 -omega(3) omega(2)];[omega(3) 0 -omega(1)];[-omega(2) omega(1) 0 ]];
|
---|
| 66 | A = omega*omega';
|
---|
| 67 |
|
---|
| 68 | %m1 = [alpha;beta;gamma;omegav;A];
|
---|
| 69 |
|
---|
| 70 | dm1dm2 = zeros(21,4);
|
---|
| 71 | dm1dm2(1,4) = -sin(theta);
|
---|
| 72 | dm1dm2(2,4) = cos(theta);
|
---|
| 73 | dm1dm2(3,4) = sin(theta);
|
---|
| 74 | dm1dm2(4:12,1:3) = [0 0 0 0 0 1 0 -1 0;
|
---|
| 75 | 0 0 -1 0 0 0 1 0 0;
|
---|
| 76 | 0 1 0 -1 0 0 0 0 0]';
|
---|
| 77 |
|
---|
| 78 | w1 = omega(1);
|
---|
| 79 | w2 = omega(2);
|
---|
| 80 | w3 = omega(3);
|
---|
| 81 |
|
---|
| 82 | dm1dm2(13:21,1) = [2*w1;w2;w3;w2;0;0;w3;0;0];
|
---|
| 83 | dm1dm2(13: 21,2) = [0;w1;0;w1;2*w2;w3;0;w3;0];
|
---|
| 84 | dm1dm2(13:21,3) = [0;0;w1;0;0;w2;w1;w2;2*w3];
|
---|
| 85 |
|
---|
| 86 | R = eye(3)*alpha + omegav*beta + A*gamma;
|
---|
| 87 |
|
---|
| 88 | dRdm1 = zeros(9,21);
|
---|
| 89 |
|
---|
| 90 | dRdm1([1 5 9],1) = ones(3,1);
|
---|
| 91 | dRdm1(:,2) = omegav(:);
|
---|
| 92 | dRdm1(:,4:12) = beta*eye(9);
|
---|
| 93 | dRdm1(:,3) = A(:);
|
---|
| 94 | dRdm1(:,13:21) = gamma*eye(9);
|
---|
| 95 |
|
---|
| 96 | dRdin = dRdm1 * dm1dm2 * dm2dm3 * dm3din;
|
---|
| 97 |
|
---|
| 98 |
|
---|
| 99 | end;
|
---|
| 100 | out = R;
|
---|
| 101 | dout = dRdin;
|
---|
| 102 |
|
---|
| 103 | %% it is prob. a rot matr.
|
---|
| 104 | elseif ((m==n) & (m==3) & (norm(in' * in - eye(3)) < bigeps)...
|
---|
| 105 | & (abs(det(in)-1) < bigeps))
|
---|
| 106 | R = in;
|
---|
| 107 |
|
---|
| 108 | % project the rotation matrix to SO(3);
|
---|
| 109 | [U,S,V] = svd(R);
|
---|
| 110 | R = U*V';
|
---|
| 111 |
|
---|
| 112 | tr = (trace(R)-1)/2;
|
---|
| 113 | dtrdR = [1 0 0 0 1 0 0 0 1]/2;
|
---|
| 114 | theta = real(acos(tr));
|
---|
| 115 |
|
---|
| 116 |
|
---|
| 117 | if sin(theta) >= 1e-5,
|
---|
| 118 |
|
---|
| 119 | dthetadtr = -1/sqrt(1-tr^2);
|
---|
| 120 |
|
---|
| 121 | dthetadR = dthetadtr * dtrdR;
|
---|
| 122 | % var1 = [vth;theta];
|
---|
| 123 | vth = 1/(2*sin(theta));
|
---|
| 124 | dvthdtheta = -vth*cos(theta)/sin(theta);
|
---|
| 125 | dvar1dtheta = [dvthdtheta;1];
|
---|
| 126 |
|
---|
| 127 | dvar1dR = dvar1dtheta * dthetadR;
|
---|
| 128 |
|
---|
| 129 |
|
---|
| 130 | om1 = [R(3,2)-R(2,3), R(1,3)-R(3,1), R(2,1)-R(1,2)]';
|
---|
| 131 |
|
---|
| 132 | dom1dR = [0 0 0 0 0 1 0 -1 0;
|
---|
| 133 | 0 0 -1 0 0 0 1 0 0;
|
---|
| 134 | 0 1 0 -1 0 0 0 0 0];
|
---|
| 135 |
|
---|
| 136 | % var = [om1;vth;theta];
|
---|
| 137 | dvardR = [dom1dR;dvar1dR];
|
---|
| 138 |
|
---|
| 139 | % var2 = [om;theta];
|
---|
| 140 | om = vth*om1;
|
---|
| 141 | domdvar = [vth*eye(3) om1 zeros(3,1)];
|
---|
| 142 | dthetadvar = [0 0 0 0 1];
|
---|
| 143 | dvar2dvar = [domdvar;dthetadvar];
|
---|
| 144 |
|
---|
| 145 |
|
---|
| 146 | out = om*theta;
|
---|
| 147 | domegadvar2 = [theta*eye(3) om];
|
---|
| 148 |
|
---|
| 149 | dout = domegadvar2 * dvar2dvar * dvardR;
|
---|
| 150 |
|
---|
| 151 |
|
---|
| 152 | else
|
---|
| 153 | if tr > 0; % case norm(om)=0;
|
---|
| 154 |
|
---|
| 155 | out = [0 0 0]';
|
---|
| 156 |
|
---|
| 157 | dout = [0 0 0 0 0 1/2 0 -1/2 0;
|
---|
| 158 | 0 0 -1/2 0 0 0 1/2 0 0;
|
---|
| 159 | 0 1/2 0 -1/2 0 0 0 0 0];
|
---|
| 160 | else % case norm(om)=pi; %% fixed April 6th
|
---|
| 161 |
|
---|
| 162 |
|
---|
| 163 | out = theta * (sqrt((diag(R)+1)/2).*[1;2*(R(1,2:3)>=0)'-1]);
|
---|
| 164 | %keyboard;
|
---|
| 165 |
|
---|
| 166 | if nargout > 1,
|
---|
| 167 | fprintf(1,'WARNING!!!! Jacobian domdR undefined!!!\n');
|
---|
| 168 | dout = NaN*ones(3,9);
|
---|
| 169 | end;
|
---|
| 170 | end;
|
---|
| 171 | end;
|
---|
| 172 |
|
---|
| 173 | else
|
---|
| 174 | error('Neither a rotation matrix nor a rotation vector were provided');
|
---|
| 175 | end;
|
---|
| 176 |
|
---|
| 177 | return;
|
---|
| 178 |
|
---|
| 179 | %% test of the Jacobians:
|
---|
| 180 |
|
---|
| 181 | %%%% TEST OF dRdom:
|
---|
| 182 | om = randn(3,1);
|
---|
| 183 | dom = randn(3,1)/1000000;
|
---|
| 184 |
|
---|
| 185 | [R1,dR1] = rodrigues(om);
|
---|
| 186 | R2 = rodrigues(om+dom);
|
---|
| 187 |
|
---|
| 188 | R2a = R1 + reshape(dR1 * dom,3,3);
|
---|
| 189 |
|
---|
| 190 | gain = norm(R2 - R1)/norm(R2 - R2a)
|
---|
| 191 |
|
---|
| 192 | %%% TEST OF dOmdR:
|
---|
| 193 | om = randn(3,1);
|
---|
| 194 | R = rodrigues(om);
|
---|
| 195 | dom = randn(3,1)/10000;
|
---|
| 196 | dR = rodrigues(om+dom) - R;
|
---|
| 197 |
|
---|
| 198 | [omc,domdR] = rodrigues(R);
|
---|
| 199 | [om2] = rodrigues(R+dR);
|
---|
| 200 |
|
---|
| 201 | om_app = omc + domdR*dR(:);
|
---|
| 202 |
|
---|
| 203 | gain = norm(om2 - omc)/norm(om2 - om_app)
|
---|
| 204 |
|
---|
| 205 |
|
---|
| 206 | %%% OTHER BUG: (FIXED NOW!!!)
|
---|
| 207 |
|
---|
| 208 | omu = randn(3,1);
|
---|
| 209 | omu = omu/norm(omu)
|
---|
| 210 | om = pi*omu;
|
---|
| 211 | [R,dR]= rodrigues(om);
|
---|
| 212 | [om2] = rodrigues(R);
|
---|
| 213 | [om om2]
|
---|
| 214 |
|
---|
| 215 | %%% NORMAL OPERATION
|
---|
| 216 |
|
---|
| 217 | om = randn(3,1);
|
---|
| 218 | [R,dR]= rodrigues(om);
|
---|
| 219 | [om2] = rodrigues(R);
|
---|
| 220 | [om om2]
|
---|
| 221 |
|
---|