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 | % BUG FOR THE CASE norm(om)=pi fixed by Mike Burl on Feb 27, 2007 |
---|
27 | |
---|
28 | |
---|
29 | [m,n] = size(in); |
---|
30 | %bigeps = 10e+4*eps; |
---|
31 | bigeps = 10e+20*eps; |
---|
32 | |
---|
33 | if ((m==1) & (n==3)) | ((m==3) & (n==1)) %% it is a rotation vector |
---|
34 | theta = norm(in); |
---|
35 | if theta < eps |
---|
36 | R = eye(3); |
---|
37 | |
---|
38 | %if nargout > 1, |
---|
39 | |
---|
40 | dRdin = [0 0 0; |
---|
41 | 0 0 1; |
---|
42 | 0 -1 0; |
---|
43 | 0 0 -1; |
---|
44 | 0 0 0; |
---|
45 | 1 0 0; |
---|
46 | 0 1 0; |
---|
47 | -1 0 0; |
---|
48 | 0 0 0]; |
---|
49 | |
---|
50 | %end; |
---|
51 | |
---|
52 | else |
---|
53 | if n==length(in) in=in'; end; %% make it a column vec. if necess. |
---|
54 | |
---|
55 | %m3 = [in,theta] |
---|
56 | |
---|
57 | dm3din = [eye(3);in'/theta]; |
---|
58 | |
---|
59 | omega = in/theta; |
---|
60 | |
---|
61 | %m2 = [omega;theta] |
---|
62 | |
---|
63 | dm2dm3 = [eye(3)/theta -in/theta^2; zeros(1,3) 1]; |
---|
64 | |
---|
65 | alpha = cos(theta); |
---|
66 | beta = sin(theta); |
---|
67 | gamma = 1-cos(theta); |
---|
68 | omegav=[[0 -omega(3) omega(2)];[omega(3) 0 -omega(1)];[-omega(2) omega(1) 0 ]]; |
---|
69 | A = omega*omega'; |
---|
70 | |
---|
71 | %m1 = [alpha;beta;gamma;omegav;A]; |
---|
72 | |
---|
73 | dm1dm2 = zeros(21,4); |
---|
74 | dm1dm2(1,4) = -sin(theta); |
---|
75 | dm1dm2(2,4) = cos(theta); |
---|
76 | dm1dm2(3,4) = sin(theta); |
---|
77 | dm1dm2(4:12,1:3) = [0 0 0 0 0 1 0 -1 0; |
---|
78 | 0 0 -1 0 0 0 1 0 0; |
---|
79 | 0 1 0 -1 0 0 0 0 0]'; |
---|
80 | |
---|
81 | w1 = omega(1); |
---|
82 | w2 = omega(2); |
---|
83 | w3 = omega(3); |
---|
84 | |
---|
85 | dm1dm2(13:21,1) = [2*w1;w2;w3;w2;0;0;w3;0;0]; |
---|
86 | dm1dm2(13: 21,2) = [0;w1;0;w1;2*w2;w3;0;w3;0]; |
---|
87 | dm1dm2(13:21,3) = [0;0;w1;0;0;w2;w1;w2;2*w3]; |
---|
88 | |
---|
89 | R = eye(3)*alpha + omegav*beta + A*gamma; |
---|
90 | |
---|
91 | dRdm1 = zeros(9,21); |
---|
92 | |
---|
93 | dRdm1([1 5 9],1) = ones(3,1); |
---|
94 | dRdm1(:,2) = omegav(:); |
---|
95 | dRdm1(:,4:12) = beta*eye(9); |
---|
96 | dRdm1(:,3) = A(:); |
---|
97 | dRdm1(:,13:21) = gamma*eye(9); |
---|
98 | |
---|
99 | dRdin = dRdm1 * dm1dm2 * dm2dm3 * dm3din; |
---|
100 | |
---|
101 | |
---|
102 | end; |
---|
103 | out = R; |
---|
104 | dout = dRdin; |
---|
105 | |
---|
106 | %% it is prob. a rot matr. |
---|
107 | elseif ((m==n) & (m==3) & (norm(in' * in - eye(3)) < bigeps)... |
---|
108 | & (abs(det(in)-1) < bigeps)) |
---|
109 | R = in; |
---|
110 | |
---|
111 | % project the rotation matrix to SO(3); |
---|
112 | [U,S,V] = svd(R); |
---|
113 | R = U*V'; |
---|
114 | |
---|
115 | tr = (trace(R)-1)/2; |
---|
116 | dtrdR = [1 0 0 0 1 0 0 0 1]/2; |
---|
117 | theta = real(acos(tr)); |
---|
118 | |
---|
119 | |
---|
120 | if sin(theta) >= 1e-4, |
---|
121 | |
---|
122 | dthetadtr = -1/sqrt(1-tr^2); |
---|
123 | |
---|
124 | dthetadR = dthetadtr * dtrdR; |
---|
125 | % var1 = [vth;theta]; |
---|
126 | vth = 1/(2*sin(theta)); |
---|
127 | dvthdtheta = -vth*cos(theta)/sin(theta); |
---|
128 | dvar1dtheta = [dvthdtheta;1]; |
---|
129 | |
---|
130 | dvar1dR = dvar1dtheta * dthetadR; |
---|
131 | |
---|
132 | |
---|
133 | om1 = [R(3,2)-R(2,3), R(1,3)-R(3,1), R(2,1)-R(1,2)]'; |
---|
134 | |
---|
135 | dom1dR = [0 0 0 0 0 1 0 -1 0; |
---|
136 | 0 0 -1 0 0 0 1 0 0; |
---|
137 | 0 1 0 -1 0 0 0 0 0]; |
---|
138 | |
---|
139 | % var = [om1;vth;theta]; |
---|
140 | dvardR = [dom1dR;dvar1dR]; |
---|
141 | |
---|
142 | % var2 = [om;theta]; |
---|
143 | om = vth*om1; |
---|
144 | domdvar = [vth*eye(3) om1 zeros(3,1)]; |
---|
145 | dthetadvar = [0 0 0 0 1]; |
---|
146 | dvar2dvar = [domdvar;dthetadvar]; |
---|
147 | |
---|
148 | |
---|
149 | out = om*theta; |
---|
150 | domegadvar2 = [theta*eye(3) om]; |
---|
151 | |
---|
152 | dout = domegadvar2 * dvar2dvar * dvardR; |
---|
153 | |
---|
154 | |
---|
155 | else |
---|
156 | if tr > 0; % case norm(om)=0; |
---|
157 | |
---|
158 | out = [0 0 0]'; |
---|
159 | |
---|
160 | dout = [0 0 0 0 0 1/2 0 -1/2 0; |
---|
161 | 0 0 -1/2 0 0 0 1/2 0 0; |
---|
162 | 0 1/2 0 -1/2 0 0 0 0 0]; |
---|
163 | else |
---|
164 | |
---|
165 | % case norm(om)=pi; |
---|
166 | if(0) |
---|
167 | |
---|
168 | %% fixed April 6th by Bouguet -- not working in all cases! |
---|
169 | out = theta * (sqrt((diag(R)+1)/2).*[1;2*(R(1,2:3)>=0)'-1]); |
---|
170 | %keyboard; |
---|
171 | |
---|
172 | else |
---|
173 | |
---|
174 | % Solution by Mike Burl on Feb 27, 2007 |
---|
175 | % This is a better way to determine the signs of the |
---|
176 | % entries of the rotation vector using a hash table on all |
---|
177 | % the combinations of signs of a pairs of products (in the |
---|
178 | % rotation matrix) |
---|
179 | |
---|
180 | % Define hashvec and Smat |
---|
181 | hashvec = [0; -1; -3; -9; 9; 3; 1; 13; 5; -7; -11]; |
---|
182 | 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; |
---|
183 | 1,-1,-1; 1,-1,1]; |
---|
184 | |
---|
185 | M = (R+eye(3,3))/2; |
---|
186 | uabs = sqrt(M(1,1)); |
---|
187 | vabs = sqrt(M(2,2)); |
---|
188 | wabs = sqrt(M(3,3)); |
---|
189 | |
---|
190 | mvec = ([M(1,2), M(2,3), M(1,3)] + [M(2,1), M(3,2), M(3,1)])/2; |
---|
191 | syn = ((mvec > eps) - (mvec < -eps)); % robust sign() function |
---|
192 | hash = syn * [9; 3; 1]; |
---|
193 | idx = find(hash == hashvec); |
---|
194 | svec = Smat(idx,:)'; |
---|
195 | |
---|
196 | out = theta * [uabs; vabs; wabs] .* svec; |
---|
197 | |
---|
198 | end; |
---|
199 | |
---|
200 | if nargout > 1, |
---|
201 | fprintf(1,'WARNING!!!! Jacobian domdR undefined!!!\n'); |
---|
202 | dout = NaN*ones(3,9); |
---|
203 | end; |
---|
204 | end; |
---|
205 | end; |
---|
206 | |
---|
207 | else |
---|
208 | error('Neither a rotation matrix nor a rotation vector were provided'); |
---|
209 | end; |
---|
210 | |
---|
211 | return; |
---|
212 | |
---|
213 | %% test of the Jacobians: |
---|
214 | |
---|
215 | %% TEST OF dRdom: |
---|
216 | om = randn(3,1); |
---|
217 | dom = randn(3,1)/1000000; |
---|
218 | [R1,dR1] = rodrigues(om); |
---|
219 | R2 = rodrigues(om+dom); |
---|
220 | R2a = R1 + reshape(dR1 * dom,3,3); |
---|
221 | gain = norm(R2 - R1)/norm(R2 - R2a) |
---|
222 | |
---|
223 | %% TEST OF dOmdR: |
---|
224 | om = randn(3,1); |
---|
225 | R = rodrigues(om); |
---|
226 | dom = randn(3,1)/10000; |
---|
227 | dR = rodrigues(om+dom) - R; |
---|
228 | |
---|
229 | [omc,domdR] = rodrigues(R); |
---|
230 | [om2] = rodrigues(R+dR); |
---|
231 | om_app = omc + domdR*dR(:); |
---|
232 | gain = norm(om2 - omc)/norm(om2 - om_app) |
---|
233 | |
---|
234 | |
---|
235 | %% OTHER BUG: (FIXED NOW!!!) |
---|
236 | omu = randn(3,1); |
---|
237 | omu = omu/norm(omu) |
---|
238 | om = pi*omu; |
---|
239 | [R,dR]= rodrigues(om); |
---|
240 | [om2] = rodrigues(R); |
---|
241 | [om om2] |
---|
242 | |
---|
243 | %% NORMAL OPERATION |
---|
244 | om = randn(3,1); |
---|
245 | [R,dR]= rodrigues(om); |
---|
246 | [om2] = rodrigues(R); |
---|
247 | [om om2] |
---|
248 | |
---|
249 | %% Test: norm(om) = pi |
---|
250 | u = randn(3,1); |
---|
251 | u = u / sqrt(sum(u.^2)); |
---|
252 | om = pi*u; |
---|
253 | R = rodrigues(om); |
---|
254 | R2 = rodrigues(rodrigues(R)); |
---|
255 | norm(R - R2) |
---|
256 | |
---|
257 | %% Another test case where norm(om)=pi from Chen Feng (June 27th, 2014) |
---|
258 | R = [-0.950146567583153 -6.41765854280073e-05 0.311803617668748; ... |
---|
259 | -6.41765854277654e-05 -0.999999917385145 -0.000401386434914383; ... |
---|
260 | 0.311803617668748 -0.000401386434914345 0.950146484968298]; |
---|
261 | om = rodrigues(R) |
---|
262 | norm(om) - pi |
---|
263 | |
---|
264 | %% Another test case where norm(om)=pi from äœæä¹ (July 1st, 2014) |
---|
265 | R = [-0.999920129411407 -6.68593208347372e-05 -0.0126384464118876; ... |
---|
266 | 9.53007036072085e-05 -0.999997464662094 -0.00224979713751896; ... |
---|
267 | -0.0126382639492467 -0.00225082189773293 0.999917600647740]; |
---|
268 | om = rodrigues(R) |
---|
269 | norm(om) - pi |
---|
270 | |
---|
271 | |
---|
272 | |
---|
273 | |
---|