Formula Student Autonomous Systems
The code for the main driverless system
Loading...
Searching...
No Matches
nearest_neighbor_test.cpp
Go to the documentation of this file.
1#include "perception_sensor_lib/data_association/nearest_neighbor.hpp"
2
3#include <gtest/gtest.h>
4
9TEST(NearestNeighbor, 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.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 DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1);
18 NearestNeighbor ml(params);
19 std::vector<int> expected_associations = {-1, 0, 4, 6, 8};
20 // Act
21 Eigen::VectorXi associations =
22 ml.associate(landmarks, observations, covariance, observation_confidences);
23 // Assert
24 for (int i = 0; i < static_cast<int>(associations.size()); ++i) {
25 EXPECT_EQ(associations[i], expected_associations[i]);
26 }
27}
28
33TEST(NearestNeighbor, TestCase2) {
34 // Arrange
35 Eigen::VectorXd landmarks(10);
36 landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4;
37 Eigen::VectorXd observations(10);
38 observations << 4.499999722610841, 13.199999626032849, 34.19999913867148, -7.200000784733975,
39 3.0999999999972934, 5.649999999996787, 6.859999998027547, 9.200000001049675,
40 1.699999998811323, 0.49999999845647614;
41 Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5);
42 Eigen::MatrixXd covariance(10, 10);
43 DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1);
44 NearestNeighbor ml(params);
45 std::vector<int> expected_associations = {-1, 0, 4, 6, 8};
46 // Act
47 Eigen::VectorXi associations =
48 ml.associate(landmarks, observations, covariance, observation_confidences);
49 // Assert
50 for (int i = 0; i < static_cast<int>(associations.size()); ++i) {
51 EXPECT_EQ(associations[i], expected_associations[i]);
52 }
53}
54
59TEST(NearestNeighbor, TestCase3) {
60 // Arrange
61 Eigen::VectorXd landmarks(10);
62 landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4;
63 Eigen::VectorXd observations(10);
64 observations << 4.499999823221495, 13.199999786278871, 34.19999419521099, -7.1999951106971025,
65 3.1000000000117893, 5.649999999968996, 6.859999985817916, 9.200000011828031,
66 1.6999999970212407, 0.49999999653619254;
67 Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(5);
68 Eigen::MatrixXd covariance(10, 10);
69 DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1);
70 std::vector<int> expected_associations = {-1, 0, 4, 6, 8};
71 NearestNeighbor ml(params);
72 // Act
73 Eigen::VectorXi associations =
74 ml.associate(landmarks, observations, covariance, observation_confidences);
75 // Assert
76 for (int i = 0; i < static_cast<int>(associations.size()); ++i) {
77 EXPECT_EQ(associations[i], expected_associations[i]);
78 }
79}
80
85TEST(NearestNeighbor, TestCase4) {
86 // Arrange
87 Eigen::VectorXd landmarks(10);
88 landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4;
89 Eigen::VectorXd observations(8);
90 observations << -1.9010818621517096, 13.361719473329373, 12.460973577144308, -1.9640632387881682,
91 1.033044730121015, 10.00589092620211, 8.066244986285891, -4.8905987333937615;
92 Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(4);
93 Eigen::MatrixXd covariance(10, 10);
94 DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1);
95 NearestNeighbor ml(params);
96 std::vector<int> expected_associations = {-1, -1, -1, -1};
97 // Act
98 Eigen::VectorXi associations =
99 ml.associate(landmarks, observations, covariance, observation_confidences);
100 // Assert
101 for (int i = 0; i < static_cast<int>(associations.size()); ++i) {
102 EXPECT_EQ(associations[i], expected_associations[i]);
103 }
104}
105
110TEST(NearestNeighbor, TestCase5) {
111 // Arrange
112 Eigen::VectorXd landmarks(10);
113 landmarks << 34.5, -7, 12.3, 4.5, 3.2, 5.6, 6.8, 9.1, 1.8, 0.4;
114 Eigen::VectorXd observations(8);
115 observations << -1.9010818621517096, 13.361719473329373, 12.460973577144308, -1.9640632387881682,
116 1.033044730121015, 10.00589092620211, 8.066244986285891, -4.8905987333937615;
117 Eigen::VectorXd observation_confidences = Eigen::VectorXd::Zero(4);
118 Eigen::MatrixXd covariance(10, 10);
119 DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1);
120 NearestNeighbor ml(params);
121 std::vector<int> expected_associations = {-2, -2, -2, -2};
122 // Act
123 Eigen::VectorXi associations =
124 ml.associate(landmarks, observations, covariance, observation_confidences);
125 // Assert
126 for (int i = 0; i < static_cast<int>(associations.size()); ++i) {
127 EXPECT_EQ(associations[i], expected_associations[i]);
128 }
129}
130
135TEST(NearestNeighbor, TestCase6) {
136 // Arrange
137 Eigen::VectorXd landmarks(0);
138 Eigen::VectorXd observations(8);
139 observations << -6.59954619, 7.988805, 14.25830, 5.5192899677, -2.193518284, 7.3123420012,
140 12.7823521, 0.449790270;
141 Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(4);
142 Eigen::MatrixXd covariance(0, 0);
143 DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1);
144 NearestNeighbor ml(params);
145 std::vector<int> expected_associations = {-1, -1, -1, -1};
146 // Act
147 Eigen::VectorXi associations =
148 ml.associate(landmarks, observations, covariance, observation_confidences);
149 // Assert
150 for (int i = 0; i < static_cast<int>(associations.size()); ++i) {
151 EXPECT_EQ(associations[i], expected_associations[i]);
152 }
153}
154
159TEST(NearestNeighbor, TestCase7) {
160 // Arrange
161 Eigen::VectorXd landmarks(0);
162 Eigen::VectorXd observations(0);
163 Eigen::VectorXd observation_confidences(0);
164 Eigen::MatrixXd covariance(0, 0);
165 DataAssociationParameters params(50.0, 1.5, 0.8, 0.1, 0.1);
166 NearestNeighbor ml(params);
167 // Act
168 Eigen::VectorXi associations =
169 ml.associate(landmarks, observations, covariance, observation_confidences);
170 // Assert
171 EXPECT_EQ(associations.size(), 0);
172}
173
174TEST(NearestNeighbor, TestCase8) {
175 // Arrange
176 Eigen::VectorXd landmarks(28);
177 landmarks << 1.395321, 1.702726, 4.554022, -1.237558, 8.210190, 4.143576, 3.203739, 4.371034,
178 6.414619, 6.287142, 10.785306, 2.572242, 7.224374, -3.087130, 13.088638, -7.904575, 13.266725,
179 2.447038, 4.708611, 1.970104, 9.944443, -1.026403, 5.617912, -8.881818, 12.886412, -0.762993,
180 7.856627, -13.420066;
181 Eigen::VectorXd observations(30);
182 observations << 1.415478, 1.678782, 4.581321, -1.253966, 3.242400, 4.364834, 8.236903, 4.121514,
183 4.720761, 1.951072, 6.461193, 6.287533, 10.826837, 2.560291, 9.981289, -1.036438, 5.655921,
184 -8.897340, 20.887318, -0.297938, 13.148567, -7.931385, 9.667393, -14.61736, 18.244286,
185 4.775482, 7.306300, -3.099958, 16.092053, -1.154598;
186 Eigen::VectorXd observation_confidences = Eigen::VectorXd::Ones(15);
187 Eigen::MatrixXd covariance(10, 10);
188 DataAssociationParameters params(50.0, 0.43, 0.8, 0.1, 0.1);
189 NearestNeighbor ml(params);
190 std::vector<int> expected_associations = {0, 2, 6, 4, 18, 8, 10, 20, 22, -1, 14, -1, -1, 12, -1};
191 // Act
192 Eigen::VectorXi associations =
193 ml.associate(landmarks, observations, covariance, observation_confidences);
194 // Assert
195 for (int i = 0; i < static_cast<int>(associations.size()); ++i) {
196 EXPECT_EQ(associations[i], expected_associations[i]);
197 }
198}
NearestNeighbor
Data association implementation that uses the Malhanobis Distance only as criterion to make observati...
Definition nearest_neighbor.hpp:13
NearestNeighbor::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 nearest_neighbor.cpp:16
nearest_neighbor.hpp
TEST
TEST(NearestNeighbor, TestCase1)
Test with mostly easy matches and a new landmark.
Definition nearest_neighbor_test.cpp:9