Visual Servoing Platform version 3.5.0
testMunkres.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2022 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 http://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#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
44 (!defined(_MSC_VER) || ( (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911) ) )
45
46// System
47#include <iostream>
48
49namespace std
50{
51
52// Helper to output a Munkres pair
53ostream &operator<<(ostream &os, const pair<unsigned int, unsigned int> &val)
54{
55 os << "[" << val.first << "," << val.second << "]";
56 return os;
57}
58} // namespace std
59
60#ifdef VISP_HAVE_CATCH2
61#define CATCH_CONFIG_RUNNER
62#include <catch.hpp>
63
64TEST_CASE("Check Munkres-based assignment", "[visp_munkres]")
65{
66 auto testMunkres = [](const std::vector<std::vector<double> > &cost_matrix,
67 const std::vector<std::pair<unsigned int, unsigned int> > &expected_pairs) {
68 const auto munkres_pairs = vpMunkres::run(cost_matrix);
69 REQUIRE(expected_pairs.size() == munkres_pairs.size());
70 for (auto i = 0u; i < munkres_pairs.size(); i++) {
71 REQUIRE(expected_pairs.at(i) == munkres_pairs.at(i));
72 }
73 };
74
75 SECTION("Square cost matrix")
76 {
77 std::vector<std::vector<double> > costs{};
78 costs.push_back(std::vector<double>{3, 1, 2});
79 costs.push_back(std::vector<double>{2, 3, 1});
80 costs.push_back(std::vector<double>{1, 2, 3});
81
82 std::vector<std::pair<unsigned int, unsigned int> > expected_pairs{};
83 expected_pairs.emplace_back(0, 1);
84 expected_pairs.emplace_back(1, 2);
85 expected_pairs.emplace_back(2, 0);
86
87 testMunkres(costs, expected_pairs);
88 }
89
90 SECTION("Horizontal cost matrix")
91 {
92 std::vector<std::vector<double> > costs{};
93 costs.push_back(std::vector<double>{4, 1, 2, 3});
94 costs.push_back(std::vector<double>{3, 4, 1, 2});
95 costs.push_back(std::vector<double>{2, 3, 4, 1});
96
97 std::vector<std::pair<unsigned int, unsigned int> > expected_pairs{};
98 expected_pairs.emplace_back(0, 1);
99 expected_pairs.emplace_back(1, 2);
100 expected_pairs.emplace_back(2, 3);
101
102 testMunkres(costs, expected_pairs);
103 }
104
105 SECTION("Vertical cost matrix")
106 {
107 std::vector<std::vector<double> > costs{};
108 costs.push_back(std::vector<double>{4, 1, 2});
109 costs.push_back(std::vector<double>{3, 4, 1});
110 costs.push_back(std::vector<double>{2, 3, 4});
111 costs.push_back(std::vector<double>{1, 2, 3});
112
113 std::vector<std::pair<unsigned int, unsigned int> > expected_pairs{};
114 expected_pairs.emplace_back(0, 1);
115 expected_pairs.emplace_back(1, 2);
116 expected_pairs.emplace_back(3, 0);
117
118 testMunkres(costs, expected_pairs);
119 }
120}
121
122int main(int argc, char *argv[])
123{
124 Catch::Session session;
125 session.applyCommandLine(argc, argv);
126
127 return session.run();
128}
129#else
130// Fallback to classic tests
131
132bool testMunkres(const std::vector<std::vector<double> > &costs_matrix,
133 const std::vector<std::pair<unsigned int, unsigned int> > &expected_pairs)
134{
135 const auto pairs = vpMunkres::run(costs_matrix);
136
137 if (pairs.size() != expected_pairs.size()) {
138 // clang-format off
139 std::cerr << "Expected nb of association | Munkres nb of association: "
140 << expected_pairs.size() << " | " << pairs.size()
141 << std::endl;
142 // clang-format on
143 return false;
144 }
145
146 for (auto i = 0u; i < pairs.size(); i++) {
147 if (expected_pairs.at(i) != pairs.at(i)) {
148
149 // Output the cost matrix
150 std::cout << "Cost matrix:" << std::endl;
151 for (const auto &cost_row : costs_matrix) {
152 std::cout << "| ";
153 for (const auto &cost : cost_row) {
154 std::cout << cost << " | ";
155 }
156 std::cout << std::endl;
157 }
158 std::cout << std::endl;
159
160 // Output the pair which fails
161 std::cerr << "FAIL: "
162 << "Expected association | Munkres association: " << expected_pairs.at(i) << " | " << pairs.at(i)
163 << std::endl;
164
165 return false;
166 }
167 }
168
169 return true;
170}
171
172bool testSquareMat()
173{
174 std::vector<std::vector<double> > costs{};
175 costs.push_back(std::vector<double>{3, 4, 1, 2});
176 costs.push_back(std::vector<double>{3, 4, 2, 1});
177 costs.push_back(std::vector<double>{1, 2, 3, 4});
178 costs.push_back(std::vector<double>{2, 1, 4, 3});
179
180 std::vector<std::pair<unsigned int, unsigned int> > pairs{};
181 pairs.emplace_back(0, 2);
182 pairs.emplace_back(1, 3);
183 pairs.emplace_back(2, 0);
184 pairs.emplace_back(3, 1);
185
186 return testMunkres(costs, pairs);
187}
188
189bool testVertMat()
190{
191 std::vector<std::vector<double> > costs{};
192 costs.push_back(std::vector<double>{3, 2, 1});
193 costs.push_back(std::vector<double>{4, 3, 2});
194 costs.push_back(std::vector<double>{1, 4, 3});
195 costs.push_back(std::vector<double>{2, 1, 4});
196
197 std::vector<std::pair<unsigned int, unsigned int> > pairs{};
198 pairs.emplace_back(0, 2);
199 pairs.emplace_back(2, 0);
200 pairs.emplace_back(3, 1);
201
202 return testMunkres(costs, pairs);
203}
204
205bool testHorMat()
206{
207 std::vector<std::vector<double> > costs{};
208 costs.push_back(std::vector<double>{2, 3, 4, 1});
209 costs.push_back(std::vector<double>{4, 1, 2, 3});
210 costs.push_back(std::vector<double>{1, 2, 3, 4});
211
212 std::vector<std::pair<unsigned int, unsigned int> > pairs{};
213 pairs.emplace_back(0, 3);
214 pairs.emplace_back(1, 1);
215 pairs.emplace_back(2, 0);
216
217 return testMunkres(costs, pairs);
218}
219
220int main()
221{
222 if (not testSquareMat()) {
223 return EXIT_FAILURE;
224 }
225
226 if (not testVertMat()) {
227 return EXIT_FAILURE;
228 }
229
230 if (not testHorMat()) {
231 return EXIT_FAILURE;
232 }
233
234 return EXIT_SUCCESS;
235}
236#endif
237
238#else
239int main() { return EXIT_SUCCESS; }
240#endif
static std::vector< std::pair< unsigned int, unsigned int > > run(std::vector< std::vector< Type > > costs)
Definition: vpMunkres.h:321
ostream & operator<<(ostream &os, const pair< unsigned int, unsigned int > &val)
Definition: testMunkres.cpp:53