Changeset 926 for trunk/src/toolbox_calib/rodrigues.m
- Timestamp:
- Feb 18, 2016, 12:05:51 PM (8 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
trunk/src/toolbox_calib/rodrigues.m
r924 r926 1 %=======================================================================2 % Copyright 2008-2016, LEGI UMR 5519 / CNRS UGA G-INP, Grenoble, France3 % http://www.legi.grenoble-inp.fr4 % Joel.Sommeria - Joel.Sommeria (A) legi.cnrs.fr5 %6 % This file is part of the toolbox UVMAT.7 %8 % UVMAT is free software; you can redistribute it and/or modify9 % it under the terms of the GNU General Public License as published10 % 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 of15 % MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the16 % GNU General Public License (see LICENSE.txt) for more details.17 %=======================================================================18 19 1 function [out,dout]=rodrigues(in) 20 2 … … 206 188 wabs = sqrt(M(3,3)); 207 189 208 mvec = [M(1,2), M(2,3), M(1,3)];209 syn = ((mvec > 1e-4) - (mvec < -1e-4)); % robust sign() function190 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 210 192 hash = syn * [9; 3; 1]; 211 193 idx = find(hash == hashvec); … … 231 213 %% test of the Jacobians: 232 214 233 %% %%TEST OF dRdom:215 %% TEST OF dRdom: 234 216 om = randn(3,1); 235 217 dom = randn(3,1)/1000000; 236 237 218 [R1,dR1] = rodrigues(om); 238 219 R2 = rodrigues(om+dom); 239 240 220 R2a = R1 + reshape(dR1 * dom,3,3); 241 242 221 gain = norm(R2 - R1)/norm(R2 - R2a) 243 222 244 %% %TEST OF dOmdR:223 %% TEST OF dOmdR: 245 224 om = randn(3,1); 246 225 R = rodrigues(om); … … 250 229 [omc,domdR] = rodrigues(R); 251 230 [om2] = rodrigues(R+dR); 252 253 231 om_app = omc + domdR*dR(:); 254 255 232 gain = norm(om2 - omc)/norm(om2 - om_app) 256 233 257 234 258 %%% OTHER BUG: (FIXED NOW!!!) 259 235 %% OTHER BUG: (FIXED NOW!!!) 260 236 omu = randn(3,1); 261 237 omu = omu/norm(omu) … … 265 241 [om om2] 266 242 267 %%% NORMAL OPERATION 268 243 %% NORMAL OPERATION 269 244 om = randn(3,1); 270 245 [R,dR]= rodrigues(om); … … 272 247 [om om2] 273 248 274 return 275 276 % Test: norm(om) = pi 277 249 %% Test: norm(om) = pi 278 250 u = randn(3,1); 279 251 u = u / sqrt(sum(u.^2)); 280 252 om = pi*u; 281 253 R = rodrigues(om); 282 283 254 R2 = rodrigues(rodrigues(R)); 284 285 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
Note: See TracChangeset
for help on using the changeset viewer.