source: trunk/src/toolbox_calib/rodrigues.m @ 907

Last change on this file since 907 was 810, checked in by g7moreau, 10 years ago
  • Add license
File size: 7.3 KB
Line 
1%=======================================================================
2% Copyright 2008-2014, 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
19function        [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;
49bigeps = 10e+20*eps;
50
51if ((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.
125elseif ((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
225else
226    error('Neither a rotation matrix nor a rotation vector were provided');
227end;
228
229return;
230
231%% test of the Jacobians:
232
233%%%% TEST OF dRdom:
234om = randn(3,1);
235dom = randn(3,1)/1000000;
236
237[R1,dR1] = rodrigues(om);
238R2 = rodrigues(om+dom);
239
240R2a = R1 + reshape(dR1 * dom,3,3);
241
242gain = norm(R2 - R1)/norm(R2 - R2a)
243
244%%% TEST OF dOmdR:
245om = randn(3,1);
246R = rodrigues(om);
247dom = randn(3,1)/10000;
248dR = rodrigues(om+dom) - R;
249
250[omc,domdR] = rodrigues(R);
251[om2] = rodrigues(R+dR);
252
253om_app = omc + domdR*dR(:);
254
255gain = norm(om2 - omc)/norm(om2 - om_app)
256
257
258%%% OTHER BUG: (FIXED NOW!!!)
259
260omu = randn(3,1);   
261omu = omu/norm(omu)
262om = pi*omu;       
263[R,dR]= rodrigues(om);
264[om2] = rodrigues(R);
265[om om2]
266
267%%% NORMAL OPERATION
268
269om = randn(3,1);         
270[R,dR]= rodrigues(om);
271[om2] = rodrigues(R);
272[om om2]
273
274return
275
276% Test: norm(om) = pi
277
278u = randn(3,1);
279u = u / sqrt(sum(u.^2));
280om = pi*u;
281R = rodrigues(om);
282
283R2 = rodrigues(rodrigues(R));
284
285norm(R - R2)
Note: See TracBrowser for help on using the repository browser.