-
Notifications
You must be signed in to change notification settings - Fork 16
/
Copy pathrun_test_aux.m
147 lines (128 loc) · 4.86 KB
/
run_test_aux.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
close all;
clear all;
disp('== Unit-test of Auxiliary Functions for Triangulation Toolbox ==');
% The given landmark map and true pose
map = ...
[ ...
% x, y, z, r_x, r_y, r_z ...
0, 0, 0, 0, 0, 0; ...
5, 0, 5, 0, 0, tran_deg2rad( +90); ...
5, 5, 0, 0, 0, tran_deg2rad(-180); ...
0, 5, 5, 0, 0, tran_deg2rad( -90); ...
];
pose = [5, 5, 5, 0, 0, pi / 2];
% test_is_equal
disp('==== test_is_true ====');
test_is_true(82 < 84);
test_is_true([10, 18] == [10, 18]);
test_is_true(~isequal([3, 2, 9], [1, 0, 1, 8]));
% test_is_near
disp('==== test_is_near ====');
test_is_near(82, 84, 5);
test_is_near(4.17, 4.17 + eps);
% error_position
disp('==== error_position====');
test_is_true(error_position([1, 2], [3, 4]) == 2 * sqrt(2));
% error_orientation
disp('==== error_orientation ====');
test_is_near(error_orientation([ 0, 0, 0], [ 0, 0, pi]), pi);
test_is_near(error_orientation([ 0, 0, 0], [ 0, pi, 0]), pi);
test_is_near(error_orientation([ 0, 0, 0], [pi, 0, 0]), 0);
test_is_near(error_orientation([ 0, 0, 0], [pi, pi, 0]), pi);
test_is_near(error_orientation([ 0, 0, 0], [ 0, pi, pi]), 0);
% trim_rad
disp('==== trim_rad ====');
test_is_true(trim_rad(pi) == -pi);
test_is_true(trim_rad(-pi) == -pi);
test_is_true(trim_rad(2 * pi) == 0);
test_is_true(trim_rad(-2 * pi) == 0);
in = (-4 * pi):(pi/100):(+4 * pi);
out = trim_rad(in);
figure('Color', [1, 1, 1]);
hold on;
plot(in, out);
title('trim\_rad: Correctness on [-4 * pi, +4 * pi]');
xlabel('in [rad]');
ylabel('out [rad]');
axis equal;
box on;
grid on;
hold off;
% tran_rad2deg
disp('==== tran_rad2deg ====');
test_is_near(tran_rad2deg(pi / 6), 30);
test_is_near(tran_rad2deg(pi / 3), 60);
% tran_deg2rad
disp('==== tran_deg2rad ====');
test_is_near(tran_deg2rad(30), pi / 6);
test_is_near(tran_deg2rad(60), pi / 3);
% tran_rad2rot
disp('==== tran_rad2rot ====');
test_is_near(tran_rad2rot([0, 0, pi / 2]), [0, -1, 0; 1, 0, 0; 0, 0, 1]);
R = tran_rad2rot([pi / 6, pi / 6, pi / 6]);
test_is_near(R * R', eye(3,3));
% tran_rot2rad
disp('==== tran_rot2rad ====');
in = [pi / 6, pi / 4, pi / 3];
R = tran_rad2rot(in);
out = tran_rot2rad(R);
test_is_near(in, out);
% observe_distance
disp('==== observe_distance ====');
[obsData, obsMap] = observe_distance(map, pose);
test_is_true(obsMap == map);
test_is_near(obsData(1), norm([5, 5, 5]));
test_is_near(obsData(2), norm([5, 0, 0]));
test_is_near(obsData(3), norm([5, 0, 0]));
test_is_near(obsData(4), norm([5, 0, 0]));
test_is_true(isempty(observe_distance([], [0, 0, 0, 0, 0, 0]))); % In case of no landmark
test_is_true(isempty(observe_distance([0, 0, 0, 0, 0, 0], [0, 0, 0, 0, 0, 0], 0))); % In case of zero visibility
% observe_distance_relative
disp('==== observe_distance_relative ====');
[obsData, obsMap] = observe_distance_relative(map, pose);
test_is_true(obsMap == map);
test_is_near(obsData(1), norm([5, 5, 5]));
test_is_near(obsData(2), norm([5, 0, 0]) - norm([5, 5, 5]));
test_is_near(obsData(3), norm([5, 0, 0]) - norm([5, 5, 5]));
test_is_near(obsData(4), norm([5, 0, 0]) - norm([5, 5, 5]));
% observe_displacement
disp('==== observe_displacement ====');
[obsData, obsMap] = observe_displacement(map, pose);
test_is_near(obsData(1,:), [-5, +5, -5]);
test_is_near(obsData(2,:), [-5, 0, 0]);
test_is_near(obsData(3,:), [ 0, 0, -5]);
test_is_near(obsData(4,:), [ 0, 5, 0]);
% observe_bearing
disp('==== observe_bearing ====');
[obsData, obsMap] = observe_bearing(map, pose);
test_is_near(obsData(1,:), [tran_deg2rad( 135), -acos(sqrt(2/3))]);
test_is_near(obsData(2,:), [tran_deg2rad(-180), 0]);
test_is_near(obsData(3,:), [tran_deg2rad( 0), -pi / 2]);
test_is_near(obsData(4,:), [tran_deg2rad( 90), 0]);
% observe_pose
disp('==== observe_pose ====');
[obsData, obsMap] = observe_pose(map, pose);
test_is_near(obsData(1,:), [-5, +5, -5, 0, 0, -pi / 2]);
test_is_near(obsData(2,:), [-5, 0, 0, 0, 0, 0 ]);
test_is_near(obsData(3,:), [ 0, 0, -5, 0, 0, +pi / 2]);
test_is_near(obsData(4,:), [ 0, 5, 0, 0, 0, -pi ]);
% apply_noise_gauss
disp('==== apply_noise_gauss ====');
in = zeros(2000,2);
out1 = apply_noise_gauss(in, 3);
out2 = apply_noise_gauss(in, [1, 2]);
out3 = apply_noise_gauss(in, [2, 1; 1, 2]);
figure('Color', [1, 1, 1]);
hold on;
plot(out1(:,1), out1(:,2), 'r.');
plot(out2(:,1), out2(:,2), 'b.');
plot(out3(:,1), out3(:,2), 'g.');
title('apply\_noise\_gauss: Noise Distributions');
xlabel('Data X');
ylabel('Data Y');
legend({'\sigma = [3, 0; 0, 3]', '\sigma = [1, 0; 0, 2]', '\sigma = [2, 1; 1, 2]'});
grid on;
axis equal;
axis([-10, 10, -10, 10]);
box on;
hold off;