1 | % By Philip Torr 2002
|
---|
2 | % copyright Microsoft Corp.
|
---|
3 | %script to test 1st order approx to the rotation matrix:
|
---|
4 | %it also checks that we are going between rotation matrices in the correct way
|
---|
5 |
|
---|
6 | rotation_multplier = 20;
|
---|
7 |
|
---|
8 |
|
---|
9 |
|
---|
10 | %Euler angles
|
---|
11 | theta = 1/360 * 2 * pi * rand * rotation_multplier;
|
---|
12 | n = 1/360 * 2 * pi * rand * rotation_multplier;
|
---|
13 | p = 1/360 * 2 * pi * rand * rotation_multplier;
|
---|
14 |
|
---|
15 | R(1,1) = (1 - cos(p)) * cos(n)* ( cos(n) * cos(theta) + sin(theta) * sin(n) ) + cos(p)* cos(theta);
|
---|
16 | R(1,2) = (1 - cos(p))* cos(n) * ( sin(n) *cos(theta) - sin(theta) *cos(n) ) - cos(p) *sin(theta);
|
---|
17 | R(1,3) = sin(n) *sin(p);
|
---|
18 | R(2,1) = (1 - cos(p)) *sin(n) *( cos(n) *cos(theta) + sin(theta)* sin(n) ) + cos(p)* sin(theta);
|
---|
19 | R(2,2) = (1 - cos(p)) *sin(n) * ( sin(n) *cos(theta) - sin(theta) *cos(n) ) + cos(p)* cos(theta);
|
---|
20 | R(2,3) = -cos(n) * sin(p);
|
---|
21 | R(3,1) = -sin(p) * ( sin(n) * cos(theta) - sin(theta) * cos(n));
|
---|
22 | R(3,2) = sin(p) * ( cos(n) * cos(theta) + sin(theta) * sin(n));
|
---|
23 | R(3,3) = cos(p);
|
---|
24 |
|
---|
25 |
|
---|
26 | %angles axis
|
---|
27 | rot_axis = [R(3,2)-R(2,3), R(1,3) - R(3,1), R(2,1) - R(1,2)];
|
---|
28 | rot_axis = rot_axis' /norm(rot_axis);
|
---|
29 | rot_angle = acos( (trace(R)-1)/2);
|
---|
30 |
|
---|
31 | disp('rotation angle in degree')
|
---|
32 | rot_angle * 360 /(2 * pi)
|
---|
33 |
|
---|
34 | %Rogregues
|
---|
35 | II = [1 0 0; 0 1 0; 0 0 1];
|
---|
36 | AX = torr_skew_sym(rot_axis);
|
---|
37 | RR = cos(rot_angle) * II + sin(rot_angle) * AX + (1 - cos(rot_angle)) * rot_axis * rot_axis';
|
---|
38 |
|
---|
39 | %now switch to first order (fo) approx
|
---|
40 |
|
---|
41 | sfo = 2 * tan ( 0.5 * rot_angle);
|
---|
42 | lfo = sfo * rot_axis;
|
---|
43 |
|
---|
44 | Rfo = torr_skew_sym(lfo) + II;
|
---|
45 | Rfo = Rfo/norm(Rfo);
|
---|
46 | R
|
---|
47 | Rfo
|
---|
48 |
|
---|
49 | disp('>> R * Rfo')
|
---|
50 |
|
---|
51 | R * Rfo'
|
---|
52 |
|
---|
53 | II - R * Rfo'
|
---|
54 |
|
---|
55 |
|
---|
56 | rot_axisfo = [Rfo(3,2)-Rfo(2,3), Rfo(1,3) - Rfo(3,1), Rfo(2,1) - Rfo(1,2)];
|
---|
57 | rot_axisfo = rot_axisfo/norm(rot_axisfo);
|
---|
58 | rot_anglefo = acos( (trace(Rfo)-1)/2);
|
---|
59 |
|
---|
60 | %the axis is the same in the first order approx, but the angle is lower.
|
---|
61 |
|
---|
62 | rot_axisfo
|
---|
63 | rot_anglefo * 360 /(2 * pi)
|
---|
64 | rot_axis
|
---|
65 | rot_angle * 360 /(2 * pi) |
---|