Visual Servoing Platform  version 3.6.1 under development (2024-02-13)
testMunkres.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Test Munkres assignment algorithm.
33  *
34 *****************************************************************************/
41 #include <visp3/core/vpMunkres.h>
42 
43 // Check if std:c++17 or higher
44 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
45 
46 // System
47 #include <iostream>
48 
49 #ifndef DOXYGEN_SHOULD_SKIP_THIS
50 namespace std
51 {
52 
53 // Helper to output a Munkres pair
54 ostream &operator<<(ostream &os, const pair<unsigned int, unsigned int> &val)
55 {
56  os << "[" << val.first << "," << val.second << "]";
57  return os;
58 }
59 } // namespace std
60 #endif // DOXYGEN_SHOULD_SKIP_THIS
61 
62 #ifdef VISP_HAVE_CATCH2
63 #define CATCH_CONFIG_RUNNER
64 #include <catch.hpp>
65 
66 TEST_CASE("Check Munkres-based assignment", "[visp_munkres]")
67 {
68  auto testMunkres = [](const std::vector<std::vector<double> > &cost_matrix,
69  const std::vector<std::pair<unsigned int, unsigned int> > &expected_pairs) {
70  const auto munkres_pairs = vpMunkres::run(cost_matrix);
71  REQUIRE(expected_pairs.size() == munkres_pairs.size());
72  for (auto i = 0u; i < munkres_pairs.size(); i++) {
73  REQUIRE(expected_pairs.at(i) == munkres_pairs.at(i));
74  }
75  };
76 
77  SECTION("Square cost matrix")
78  {
79  std::vector<std::vector<double> > costs {};
80  costs.push_back(std::vector<double>{3, 1, 2});
81  costs.push_back(std::vector<double>{2, 3, 1});
82  costs.push_back(std::vector<double>{1, 2, 3});
83 
84  std::vector<std::pair<unsigned int, unsigned int> > expected_pairs {};
85  expected_pairs.emplace_back(0, 1);
86  expected_pairs.emplace_back(1, 2);
87  expected_pairs.emplace_back(2, 0);
88 
89  testMunkres(costs, expected_pairs);
90  }
91 
92  SECTION("Horizontal cost matrix")
93  {
94  std::vector<std::vector<double> > costs {};
95  costs.push_back(std::vector<double>{4, 1, 2, 3});
96  costs.push_back(std::vector<double>{3, 4, 1, 2});
97  costs.push_back(std::vector<double>{2, 3, 4, 1});
98 
99  std::vector<std::pair<unsigned int, unsigned int> > expected_pairs {};
100  expected_pairs.emplace_back(0, 1);
101  expected_pairs.emplace_back(1, 2);
102  expected_pairs.emplace_back(2, 3);
103 
104  testMunkres(costs, expected_pairs);
105  }
106 
107  SECTION("Vertical cost matrix")
108  {
109  std::vector<std::vector<double> > costs {};
110  costs.push_back(std::vector<double>{4, 1, 2});
111  costs.push_back(std::vector<double>{3, 4, 1});
112  costs.push_back(std::vector<double>{2, 3, 4});
113  costs.push_back(std::vector<double>{1, 2, 3});
114 
115  std::vector<std::pair<unsigned int, unsigned int> > expected_pairs {};
116  expected_pairs.emplace_back(0, 1);
117  expected_pairs.emplace_back(1, 2);
118  expected_pairs.emplace_back(3, 0);
119 
120  testMunkres(costs, expected_pairs);
121  }
122 }
123 
124 int main(int argc, char *argv[])
125 {
126  Catch::Session session;
127  session.applyCommandLine(argc, argv);
128 
129  return session.run();
130 }
131 #else
132 // Fallback to classic tests
133 
134 bool testMunkres(const std::vector<std::vector<double> > &costs_matrix,
135  const std::vector<std::pair<unsigned int, unsigned int> > &expected_pairs)
136 {
137  const auto pairs = vpMunkres::run(costs_matrix);
138 
139  if (pairs.size() != expected_pairs.size()) {
140  // clang-format off
141  std::cerr << "Expected nb of association | Munkres nb of association: "
142  << expected_pairs.size() << " | " << pairs.size()
143  << std::endl;
144 // clang-format on
145  return false;
146  }
147 
148  for (auto i = 0u; i < pairs.size(); i++) {
149  if (expected_pairs.at(i) != pairs.at(i)) {
150 
151  // Output the cost matrix
152  std::cout << "Cost matrix:" << std::endl;
153  for (const auto &cost_row : costs_matrix) {
154  std::cout << "| ";
155  for (const auto &cost : cost_row) {
156  std::cout << cost << " | ";
157  }
158  std::cout << std::endl;
159  }
160  std::cout << std::endl;
161 
162  // Output the pair which fails
163  std::cerr << "FAIL: "
164  << "Expected association | Munkres association: " << expected_pairs.at(i) << " | " << pairs.at(i)
165  << std::endl;
166 
167  return false;
168  }
169  }
170 
171  return true;
172 }
173 
174 bool testSquareMat()
175 {
176  std::vector<std::vector<double> > costs {};
177  costs.push_back(std::vector<double>{3, 4, 1, 2});
178  costs.push_back(std::vector<double>{3, 4, 2, 1});
179  costs.push_back(std::vector<double>{1, 2, 3, 4});
180  costs.push_back(std::vector<double>{2, 1, 4, 3});
181 
182  std::vector<std::pair<unsigned int, unsigned int> > pairs {};
183  pairs.emplace_back(0, 2);
184  pairs.emplace_back(1, 3);
185  pairs.emplace_back(2, 0);
186  pairs.emplace_back(3, 1);
187 
188  return testMunkres(costs, pairs);
189 }
190 
191 bool testVertMat()
192 {
193  std::vector<std::vector<double> > costs {};
194  costs.push_back(std::vector<double>{3, 2, 1});
195  costs.push_back(std::vector<double>{4, 3, 2});
196  costs.push_back(std::vector<double>{1, 4, 3});
197  costs.push_back(std::vector<double>{2, 1, 4});
198 
199  std::vector<std::pair<unsigned int, unsigned int> > pairs {};
200  pairs.emplace_back(0, 2);
201  pairs.emplace_back(2, 0);
202  pairs.emplace_back(3, 1);
203 
204  return testMunkres(costs, pairs);
205 }
206 
207 bool testHorMat()
208 {
209  std::vector<std::vector<double> > costs {};
210  costs.push_back(std::vector<double>{2, 3, 4, 1});
211  costs.push_back(std::vector<double>{4, 1, 2, 3});
212  costs.push_back(std::vector<double>{1, 2, 3, 4});
213 
214  std::vector<std::pair<unsigned int, unsigned int> > pairs {};
215  pairs.emplace_back(0, 3);
216  pairs.emplace_back(1, 1);
217  pairs.emplace_back(2, 0);
218 
219  return testMunkres(costs, pairs);
220 }
221 
222 int main()
223 {
224  if (not testSquareMat()) {
225  return EXIT_FAILURE;
226  }
227 
228  if (not testVertMat()) {
229  return EXIT_FAILURE;
230  }
231 
232  if (not testHorMat()) {
233  return EXIT_FAILURE;
234  }
235 
236  return EXIT_SUCCESS;
237 }
238 #endif
239 
240 #else
241 int main() { return EXIT_SUCCESS; }
242 #endif