source: trunk/src/rodrigues.m @ 962

Last change on this file since 962 was 961, checked in by sommeria, 5 years ago

NomType? level introduced

File size: 6.8 KB
Line 
1function        [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;
31bigeps = 10e+20*eps;
32
33if ((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.
107elseif ((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
207else
208    error('Neither a rotation matrix nor a rotation vector were provided');
209end;
210
211return;
212
213%% test of the Jacobians:
214
215%% TEST OF dRdom:
216om = randn(3,1);
217dom = randn(3,1)/1000000;
218[R1,dR1] = rodrigues(om);
219R2 = rodrigues(om+dom);
220R2a = R1 + reshape(dR1 * dom,3,3);
221gain = norm(R2 - R1)/norm(R2 - R2a)
222
223%% TEST OF dOmdR:
224om = randn(3,1);
225R = rodrigues(om);
226dom = randn(3,1)/10000;
227dR = rodrigues(om+dom) - R;
228
229[omc,domdR] = rodrigues(R);
230[om2] = rodrigues(R+dR);
231om_app = omc + domdR*dR(:);
232gain = norm(om2 - omc)/norm(om2 - om_app)
233
234
235%% OTHER BUG: (FIXED NOW!!!)
236omu = randn(3,1);   
237omu = omu/norm(omu)
238om = pi*omu;       
239[R,dR]= rodrigues(om);
240[om2] = rodrigues(R);
241[om om2]
242
243%% NORMAL OPERATION
244om = randn(3,1);         
245[R,dR]= rodrigues(om);
246[om2] = rodrigues(R);
247[om om2]
248
249%% Test: norm(om) = pi
250u = randn(3,1);
251u = u / sqrt(sum(u.^2));
252om = pi*u;
253R = rodrigues(om);
254R2 = rodrigues(rodrigues(R));
255norm(R - R2)
256
257%% Another test case where norm(om)=pi from Chen Feng (June 27th, 2014)
258R = [-0.950146567583153 -6.41765854280073e-05 0.311803617668748; ...
259     -6.41765854277654e-05 -0.999999917385145 -0.000401386434914383; ...
260      0.311803617668748 -0.000401386434914345 0.950146484968298];
261om = rodrigues(R)
262norm(om) - pi
263
264%% Another test case where norm(om)=pi from 䜙成义 (July 1st, 2014)
265R = [-0.999920129411407 -6.68593208347372e-05   -0.0126384464118876; ...
266     9.53007036072085e-05       -0.999997464662094      -0.00224979713751896; ...
267    -0.0126382639492467 -0.00225082189773293    0.999917600647740];
268om = rodrigues(R)
269norm(om) - pi
270
271
272
273
Note: See TracBrowser for help on using the repository browser.