1 | %=======================================================================
|
---|
2 | % Copyright 2008-2015, LEGI UMR 5519 / CNRS UJF G-INP, Grenoble, France
|
---|
3 | % http://www.legi.grenoble-inp.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 3-vector 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 JEAN-YVES 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 | %% Jean-Yves Bouguet
|
---|
40 |
|
---|
41 | %% Add projection of the 3x3 matrix onto the set of special ortogonal matrices SO(3) by SVD -- February 7th, 2003;
|
---|
42 | %% Jean-Yves 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 = 1-cos(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) >= 1e-4,
|
---|
139 |
|
---|
140 | dthetadtr = -1/sqrt(1-tr^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 > 1e-4) - (mvec < -1e-4)); % 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)
|
---|