[37] | 1 | function test_rodr |
---|
| 2 | % TEST_RODR Regression test of RODR() and IRODR() |
---|
| 3 | % TEST_RODR() performs a regression tests of the RODR() and IRODR() |
---|
| 4 | % functions. It compares RODR(w) with EXMP(HAT(w)) and IRODR(R) with |
---|
| 5 | % IHAT(LOGM(R)). It also checks the derivatives numerically. |
---|
| 6 | % |
---|
| 7 | % The function might isse a buch of warning message. This is |
---|
| 8 | % normal as some singular values are tested for completeness. |
---|
| 9 | % |
---|
| 10 | % See also RODR(), IRODR(). |
---|
| 11 | |
---|
| 12 | % Special cases |
---|
| 13 | fprintf('Testing [0 0 0]\n') ; |
---|
| 14 | [er, edr, eir, edir] = test([0 0 0]') ; |
---|
| 15 | report(er, edr, eir, edir) ; |
---|
| 16 | |
---|
| 17 | % General cases |
---|
| 18 | T=100; |
---|
| 19 | er=[] ; |
---|
| 20 | edr=[] ; |
---|
| 21 | eir=[] ; |
---|
| 22 | edir=[] ; |
---|
| 23 | for t=1:T |
---|
| 24 | fprintf('Testing random vector %d of %d\r',t,T) ; |
---|
| 25 | v=rand(3,1) ; |
---|
| 26 | [er_, edr_, eir_, edir_] = test(v) ; |
---|
| 27 | er = [er er_ ] ; |
---|
| 28 | edr = [edr edr_ ] ; |
---|
| 29 | eir = [eir eir_ ] ; |
---|
| 30 | edir = [edir edir_ ] ; |
---|
| 31 | end |
---|
| 32 | fprintf('\n') ; |
---|
| 33 | report(er,edr,eir,edir) ; |
---|
| 34 | |
---|
| 35 | % Around the clock |
---|
| 36 | % Note that -pi and +pi are singular. Since they give |
---|
| 37 | % the same rotation R, the inverse Rodrigues formula will map both |
---|
| 38 | % cases back to +pi (by convention), while |
---|
| 39 | % the logm function (used for the regression test here) |
---|
| 40 | % is not defined for such cases, for which the error is big. |
---|
| 41 | % Also, the derivatives of the inverse formula are not computed |
---|
| 42 | % there (even if it might be possible to do so on the local branch) |
---|
| 43 | T=100; |
---|
| 44 | er=[] ; |
---|
| 45 | edr=[] ; |
---|
| 46 | eir=[] ; |
---|
| 47 | edir=[] ; |
---|
| 48 | th_range=linspace(-pi,pi,T) ; |
---|
| 49 | for t=1:T |
---|
| 50 | fprintf('Testing angle %f [rad]\r',th_range(t)); |
---|
| 51 | v = th_range(t)*[1;0;0] ; |
---|
| 52 | [er_, edr_, eir_, edir_] = test(v) ; |
---|
| 53 | er = [er er_ ] ; |
---|
| 54 | edr = [edr edr_ ] ; |
---|
| 55 | eir = [eir eir_ ] ; |
---|
| 56 | edir = [edir edir_ ] ; |
---|
| 57 | end |
---|
| 58 | report(er,edr,eir,edir) ; |
---|
| 59 | fprintf('n') ; |
---|
| 60 | |
---|
| 61 | |
---|
| 62 | % -------------------------------------------------------------------- |
---|
| 63 | % Helper functions |
---|
| 64 | % -------------------------------------------------------------------- |
---|
| 65 | % Computes the rodrigues function and its inverse and compare the |
---|
| 66 | % result with `numerical ground truth'. Compares the derivatives as well. |
---|
| 67 | function [er, edr, eir, edir] = test(v) |
---|
| 68 | |
---|
| 69 | rodrigues = @rodr ; |
---|
| 70 | irodrigues = @irodr ; |
---|
| 71 | |
---|
| 72 | % Numerical derivations |
---|
| 73 | e1 = [1;0;0] ; |
---|
| 74 | e2 = [0;1;0] ; |
---|
| 75 | e3 = [0;0;1] ; |
---|
| 76 | E{1} = [0 0 0 ; 0 0 -1 ; 0 1 0] ; |
---|
| 77 | E{2} = [0 0 1 ; 0 0 0 ; -1 0 0] ; |
---|
| 78 | E{3} = [0 -1 0 ; 1 0 0 ; 0 0 0] ; |
---|
| 79 | |
---|
| 80 | step=1e-5; |
---|
| 81 | RR = expm(hat(v)) ; |
---|
| 82 | dR1 = (expm(hat( v + step * e1)) - RR)/step ; |
---|
| 83 | dR2 = (expm(hat( v + step * e2)) - RR)/step ; |
---|
| 84 | dR3 = (expm(hat( v + step * e3)) - RR)/step ; |
---|
| 85 | dRR = [ dR1(:), dR2(:), dR3(:) ] ; |
---|
| 86 | |
---|
| 87 | E{1} = RR*E{1} ; |
---|
| 88 | E{2} = RR*E{2} ; |
---|
| 89 | E{3} = RR*E{3} ; |
---|
| 90 | |
---|
| 91 | duu=zeros(3,3) ; |
---|
| 92 | step=1e-5; |
---|
| 93 | for k=1:3 |
---|
| 94 | duu(:,k) = ... |
---|
| 95 | (ihat(logm( RR + step * E{k})) - v)/step ; |
---|
| 96 | end |
---|
| 97 | |
---|
| 98 | % Rodrigues and inverse formulas |
---|
| 99 | [R,dR] = rodrigues( v ) ; |
---|
| 100 | [u,du] = irodrigues( R ) ; |
---|
| 101 | |
---|
| 102 | du = du * reshape([ E{:} ],9,3) ; |
---|
| 103 | |
---|
| 104 | % Errors |
---|
| 105 | er = max(abs( R(:)- RR(:)))/max(abs( R(:))+eps) ; |
---|
| 106 | edr = max(abs(dR(:)-dRR(:)))/max(abs(dRR(:))+eps) ; |
---|
| 107 | eir = max(abs( v - u ))/max(abs(v(:))+eps) ; |
---|
| 108 | edir = max(abs(du(:)-duu(:)))/max(abs(duu(:))+eps) ; |
---|
| 109 | |
---|
| 110 | % Print statistics |
---|
| 111 | function report(er, edr, eir, edir) |
---|
| 112 | %keyboard |
---|
| 113 | rng=1:length(er) ; |
---|
| 114 | fprintf('rel(x,y) = max|x-y|/(max|y|+eps)\n') ; |
---|
| 115 | fprintf('er = rel( rodrigues(u), expm(hat(u)))\n') ; |
---|
| 116 | fprintf('edr = rel( rodrigues(u), dexpm(hat(u)))\n') ; |
---|
| 117 | fprintf('eir = rel( irodrigues(R), ihat(Log(R))\n') ; |
---|
| 118 | fprintf('edir = rel( irodrigues(R), dihat(Log(R))\n') ; |
---|
| 119 | |
---|
| 120 | fprintf('t | er |edr |eir |edir \n') ; |
---|
| 121 | fprintf('%5d | %10.3e | %10.3e | %10.3e | %10.3e \n', [rng;er;edr;eir;edir]) ; |
---|