Formula Student Autonomous Systems
The code for the main driverless system
Loading...
Searching...
No Matches
maximum_likelihood_md_test.cpp
Go to the documentation of this file.
1#include "perception_sensor_lib/data_association/maximum_likelihood_md.hpp"
2
3#include <gtest/gtest.h>
4
9TEST(MaximumLikelihoodMD, TestCase1) {
10 // Arrange
11 Eigen::VectorXd landmarks(10);
12 landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4;
13 Eigen::VectorXd observations(10);
14 observations << 4.499999722610841, 13.199999626032849, 34.19999913867148, -7.200000784733975,
15 3.0999999999972934, 5.649999999996787, 6.859999998027547, 9.200000001049675,
16 1.699999998811323, 0.49999999845647614;
17 Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5);
18 Eigen::MatrixXd covariance(10, 10);
19 covariance.setIdentity();
20 covariance *= 0.1;
21 DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1);
22 MaximumLikelihoodMD ml(params);
23 std::vector<int> expected_associations = {-1, 0, 4, 6, 8};
24 // Act
25 Eigen::VectorXi associations =
26 ml.associate(landmarks, observations, covariance, observation_confidences);
27 // Assert
28 for (int i = 0; i < static_cast<int>(associations.size()); ++i) {
29 EXPECT_EQ(associations[i], expected_associations[i]);
30 }
31}
32
37TEST(MaximumLikelihoodMD, TestCase2) {
38 // Arrange
39 Eigen::VectorXd landmarks(10);
40 landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4;
41 Eigen::VectorXd observations(10);
42 observations << 4.499999722610841, 13.199999626032849, 34.19999913867148, -7.200000784733975,
43 3.0999999999972934, 5.649999999996787, 6.859999998027547, 9.200000001049675,
44 1.699999998811323, 0.49999999845647614;
45 Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5);
46 Eigen::MatrixXd covariance(10, 10);
47 covariance.setIdentity();
48 covariance *= 0.1;
49 DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1);
50 MaximumLikelihoodMD ml(params);
51 std::vector<int> expected_associations = {-1, 0, 4, 6, 8};
52 // Act
53 Eigen::VectorXi associations =
54 ml.associate(landmarks, observations, covariance, observation_confidences);
55 // Assert
56 for (int i = 0; i < static_cast<int>(associations.size()); ++i) {
57 EXPECT_EQ(associations[i], expected_associations[i]);
58 }
59}
60
65TEST(MaximumLikelihoodMD, TestCase3) {
66 // Arrange
67 Eigen::VectorXd landmarks(10);
68 landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4;
69 Eigen::VectorXd observations(10);
70 observations << 4.499999823221495, 13.199999786278871, 34.19999419521099, -7.1999951106971025,
71 3.1000000000117893, 5.649999999968996, 6.859999985817916, 9.200000011828031,
72 1.6999999970212407, 0.49999999653619254;
73 Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5);
74 Eigen::MatrixXd covariance(10, 10);
75 covariance.setIdentity();
76 covariance *= 0.2;
77 DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1);
78 std::vector<int> expected_associations = {-1, 0, 4, 6, 8};
79 MaximumLikelihoodMD ml(params);
80 // Act
81 Eigen::VectorXi associations =
82 ml.associate(landmarks, observations, covariance, observation_confidences);
83 // Assert
84 for (int i = 0; i < static_cast<int>(associations.size()); ++i) {
85 EXPECT_EQ(associations[i], expected_associations[i]);
86 }
87}
88
93TEST(MaximumLikelihoodMD, TestCase4) {
94 // Arrange
95 Eigen::VectorXd landmarks(10);
96 landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4;
97 Eigen::VectorXd observations(8);
98 observations << -1.9010818621517096, 13.361719473329373, 12.460973577144308, -1.9640632387881682,
99 1.033044730121015, 10.00589092620211, 8.066244986285891, -4.8905987333937615;
100 Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(4);
101 Eigen::MatrixXd covariance(10, 10);
102 covariance.setIdentity();
103 covariance *= 0.1;
104 DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1);
105 MaximumLikelihoodMD ml(params);
106 std::vector<int> expected_associations = {-1, -1, -1, -1};
107 // Act
108 Eigen::VectorXi associations =
109 ml.associate(landmarks, observations, covariance, observation_confidences);
110 // Assert
111 for (int i = 0; i < static_cast<int>(associations.size()); ++i) {
112 EXPECT_EQ(associations[i], expected_associations[i]);
113 }
114}
115
120TEST(MaximumLikelihoodMD, TestCase5) {
121 // Arrange
122 Eigen::VectorXd landmarks(10);
123 landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4;
124 Eigen::VectorXd observations(8);
125 observations << -1.9010818621517096, 13.361719473329373, 12.460973577144308, -1.9640632387881682,
126 1.033044730121015, 10.00589092620211, 8.066244986285891, -4.8905987333937615;
127 Eigen::VectorXd observation_confidences = Eigen::VectorXd::Zero(4);
128 Eigen::MatrixXd covariance(10, 10);
129 covariance.setIdentity();
130 covariance *= 0.1;
131 DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1);
132 MaximumLikelihoodMD ml(params);
133 std::vector<int> expected_associations = {-2, -2, -2, -2};
134 // Act
135 Eigen::VectorXi associations =
136 ml.associate(landmarks, observations, covariance, observation_confidences);
137 // Assert
138 for (int i = 0; i < static_cast<int>(associations.size()); ++i) {
139 EXPECT_EQ(associations[i], expected_associations[i]);
140 }
141}
142
147TEST(MaximumLikelihoodMD, TestCase6) {
148 // Arrange
149 Eigen::VectorXd landmarks(0);
150 Eigen::VectorXd observations(8);
151 observations << -6.59954619, 7.988805, 14.25830, 5.5192899677, -2.193518284, 7.3123420012,
152 12.7823521, 0.449790270;
153 Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(4);
154 Eigen::MatrixXd covariance(0, 0);
155 DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1);
156 MaximumLikelihoodMD ml(params);
157 std::vector<int> expected_associations = {-1, -1, -1, -1};
158 // Act
159 Eigen::VectorXi associations =
160 ml.associate(landmarks, observations, covariance, observation_confidences);
161 // Assert
162 for (int i = 0; i < static_cast<int>(associations.size()); ++i) {
163 EXPECT_EQ(associations[i], expected_associations[i]);
164 }
165}
166
171TEST(MaximumLikelihoodMD, TestCase7) {
172 // Arrange
173 Eigen::VectorXd state(0);
174 Eigen::VectorXd observations(0);
175 Eigen::VectorXd observation_confidences(0);
176 Eigen::MatrixXd covariance(0, 0);
177 DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1);
178 MaximumLikelihoodMD ml(params);
179 // Act
180 Eigen::VectorXi associations =
181 ml.associate(state, observations, covariance, observation_confidences);
182 // Assert
183 EXPECT_EQ(associations.size(), 0);
184}
185
190TEST(MaximumLikelihoodMD, TestCase8) {
191 // Arrange
192 Eigen::VectorXd state(10);
193 state << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4;
194 Eigen::VectorXd observations(10);
195 observations << 4.499999823221495, 13.199999786278871, 34.19999419521099, -7.1999951106971025,
196 3.1000000000117893, 5.649999999968996, 6.859999985817916, 9.200000011828031,
197 1.6999999970212407, 0.49999999653619254;
198 Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5);
199 Eigen::MatrixXd covariance(10, 10);
200 covariance.setIdentity();
201 covariance *= 0.2;
202 covariance(2, 2) = 1000; // Very high uncertainty, should match even though wasn't observed
203 covariance(3, 3) = 1000;
204 DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1);
205 MaximumLikelihoodMD ml(params);
206 std::vector<int> expected_associations = {2, 0, 4, 6, 8};
207 // Act
208 Eigen::VectorXi associations =
209 ml.associate(state, observations, covariance, observation_confidences);
210 // Assert
211 for (int i = 0; i < static_cast<int>(associations.size()); ++i) {
212 EXPECT_EQ(associations[i], expected_associations[i]);
213 }
214}
215
220TEST(MaximumLikelihoodMD, TestCase9) {
221 // Arrange
222 Eigen::VectorXd state(10);
223 state << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4;
224 Eigen::VectorXd observations(10);
225 observations << 4.499999823221495, 13.199999786278871, 34.19999419521099, -7.1999951106971025,
226 3.1000000000117893, 5.649999999968996, 6.859999985817916, 9.200000011828031,
227 1.6999999970212407, 0.49999999653619254;
228 Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5);
229 Eigen::MatrixXd covariance(10, 10);
230 covariance.setIdentity();
231 covariance *= 0.01;
232 covariance(0, 0) = 0; // Very low uncertainty, should not match even though was observed
233 covariance(1, 1) = 0;
234 DataAssociationParameters params(50.0, 1.0, 0.8, 0.1, 0.1);
235 MaximumLikelihoodMD ml(params);
236 std::vector<int> expected_associations = {-1, -1, 4, 6, 8};
237 // Act
238 Eigen::VectorXi associations =
239 ml.associate(state, observations, covariance, observation_confidences);
240 // Assert
241 for (int i = 0; i < static_cast<int>(associations.size()); ++i) {
242 EXPECT_EQ(associations[i], expected_associations[i]);
243 }
244}
MaximumLikelihoodMD
Data association implementation that uses the Malhanobis Distance only as criterion to make observati...
Definition maximum_likelihood_md.hpp:13
MaximumLikelihoodMD::associate
Eigen::VectorXi associate(const Eigen::VectorXd &landmarks, const Eigen::VectorXd &observations, const Eigen::MatrixXd &covariance, const Eigen::VectorXd &observation_confidences) const override
This function associates the landmarks with the observations.
Definition maximum_likelihood_md.cpp:9
maximum_likelihood_md.hpp
TEST
TEST(MaximumLikelihoodMD, TestCase1)
Test with mostly easy matches and a new landmark.
Definition maximum_likelihood_md_test.cpp:9