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 |
|
---|