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