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]) ; |
---|