Visual Servoing Platform version 3.5.0
vpMunkres.h
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 * Class for Munkres Assignment Algorithm.
33 *
34 * Authors:
35 * Souriya Trinh
36 * Julien Dufour
37 *
38 *****************************************************************************/
39
40#pragma once
41
42#include <visp3/core/vpConfig.h>
43
44#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
45 (!defined(_MSC_VER) || ( (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911) ) )
46
47// Visual Studio: Optionals are available from Visual Studio 2017 RTW (15.0) [1910]
48// Visual Studio: Structured bindings are available from Visual Studio 2017 version 15.3 [1911]
49
50// System
51#include <optional>
52#include <tuple>
53
54// Internal
55#include "vpMath.h"
56
65class VISP_EXPORT vpMunkres
66{
67public:
68 template <typename Type>
69 static std::vector<std::pair<unsigned int, unsigned int> > run(std::vector<std::vector<Type> > costs);
70
71private:
72 enum ZERO_T : unsigned int;
73 enum STEP_T : unsigned int;
74
75 // Init
76 template <typename Type> static void padCostMatrix(std::vector<std::vector<Type> > &costs);
77
78 // Global helpers
79 template <typename Type>
80 static std::optional<std::pair<unsigned int, unsigned int> > findAZero(const std::vector<std::vector<Type> > &costs,
81 const std::vector<bool> &row_cover,
82 const std::vector<bool> &col_cover);
83 static std::optional<unsigned int> findStarInRow(const std::vector<std::vector<ZERO_T> > &mask,
84 const unsigned int &row);
85 static std::optional<unsigned int> findStarInCol(const std::vector<std::vector<ZERO_T> > &mask,
86 const unsigned int &col);
87 static std::optional<unsigned int> findPrimeInRow(const std::vector<std::vector<ZERO_T> > &mask,
88 const unsigned int &row);
89 template <typename Type>
90 static Type findSmallest(const std::vector<std::vector<Type> > &costs, const std::vector<bool> &row_cover,
91 const std::vector<bool> &col_cover);
92
93 // FSM helpers
94 static void augmentPath(std::vector<std::vector<ZERO_T> > &mask,
95 const std::vector<std::pair<unsigned int, unsigned int> > &path);
96 static void clearCovers(std::vector<bool> &row_cover, std::vector<bool> &col_cover);
97 static void erasePrimes(std::vector<std::vector<ZERO_T> > &mask);
98
99 // FSM
100 template <typename Type> static STEP_T stepOne(std::vector<std::vector<Type> > &costs);
101 template <typename Type>
102 static STEP_T stepTwo(std::vector<std::vector<Type> > &costs, std::vector<std::vector<ZERO_T> > &mask,
103 std::vector<bool> &row_cover, std::vector<bool> &col_cover);
104 static STEP_T stepThree(const std::vector<std::vector<ZERO_T> > &mask, std::vector<bool> &col_cover);
105 template <typename Type>
106 static std::tuple<STEP_T, std::optional<std::pair<unsigned int, unsigned int> > >
107 stepFour(const std::vector<std::vector<Type> > &costs, std::vector<std::vector<ZERO_T> > &mask,
108 std::vector<bool> &row_cover, std::vector<bool> &col_cover);
109 static STEP_T stepFive(std::vector<std::vector<ZERO_T> > &mask, const std::pair<unsigned int, unsigned int> &path_0,
110 std::vector<bool> &row_cover, std::vector<bool> &col_cover);
111 template <typename Type>
112 static STEP_T stepSix(std::vector<std::vector<Type> > &costs, const std::vector<bool> &row_cover,
113 const std::vector<bool> &col_cover);
114
115private:
116 static constexpr auto ZeroEpsilon{1e-6};
117};
118
119enum vpMunkres::ZERO_T : unsigned int { NA = 0, STARRED = 1, PRIMED = 2 };
120
121enum vpMunkres::STEP_T : unsigned int { ENTRY = 0, ONE = 1, TWO = 2, THREE = 3, FOUR = 4, FIVE = 5, SIX = 6, DONE };
122
128template <typename Type> inline void vpMunkres::padCostMatrix(std::vector<std::vector<Type> > &costs)
129{
130 const auto row_input_size = costs.size();
131 const auto col_input_size = costs.at(0).size();
132
133 if (row_input_size > col_input_size) {
134 for (auto &vec : costs)
135 vec.resize(row_input_size, 0);
136 }
137
138 while (costs.size() < col_input_size) {
139 costs.emplace_back(col_input_size, 0);
140 }
141}
142
151template <typename Type>
152inline std::optional<std::pair<unsigned int, unsigned int> >
153vpMunkres::findAZero(const std::vector<std::vector<Type> > &costs, const std::vector<bool> &row_cover,
154 const std::vector<bool> &col_cover)
155{
156 for (auto row = 0u; row < costs.size(); row++)
157 for (auto col = 0u; col < costs.size(); col++)
158 if (vpMath::equal(costs.at(row).at(col), static_cast<Type>(vpMunkres::ZeroEpsilon)) && !row_cover.at(row) &&
159 !col_cover.at(col)) {
160 return std::make_optional<std::pair<unsigned int, unsigned int> >(row, col);
161 }
162
163 return std::nullopt;
164}
165
174template <typename Type>
175inline Type vpMunkres::findSmallest(const std::vector<std::vector<Type> > &costs, const std::vector<bool> &row_cover,
176 const std::vector<bool> &col_cover)
177{
178 auto minval = std::numeric_limits<Type>::max();
179 for (auto row = 0u; row < costs.size(); row++)
180 for (auto col = 0u; col < costs.size(); col++)
181 if (minval > costs.at(row).at(col) && !row_cover.at(row) && !col_cover.at(col)) {
182 minval = costs.at(row).at(col);
183 }
184
185 return minval;
186}
187
196template <typename Type> inline vpMunkres::STEP_T vpMunkres::stepOne(std::vector<std::vector<Type> > &costs)
197{
198 // process rows
199 std::for_each(begin(costs), end(costs), [](auto &cost_row) {
200 const auto min_in_row = *std::min_element(begin(cost_row), end(cost_row));
201 std::transform(begin(cost_row), end(cost_row), begin(cost_row),
202 [&min_in_row](auto &cost) { return cost - min_in_row; });
203 });
204
205 // process cols
206 for (auto col = 0u; col < costs.size(); ++col) {
207 auto minval = std::numeric_limits<Type>::max();
208 for (const auto &cost_row : costs) {
209 minval = std::min(minval, cost_row.at(col));
210 }
211
212 for (auto &cost_row : costs) {
213 cost_row.at(col) -= minval;
214 }
215 }
216
217 return vpMunkres::STEP_T(2);
218}
219
231template <typename Type>
232inline vpMunkres::STEP_T vpMunkres::stepTwo(std::vector<std::vector<Type> > &costs,
233 std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
234 std::vector<bool> &row_cover, std::vector<bool> &col_cover)
235{
236 for (auto row = 0u; row < costs.size(); row++) {
237 for (auto col = 0u; col < costs.size(); col++) {
238 if (vpMath::equal(costs.at(row).at(col), static_cast<Type>(vpMunkres::ZeroEpsilon)) && !row_cover.at(row) &&
239 !col_cover.at(col)) {
240 mask.at(row).at(col) = vpMunkres::ZERO_T::STARRED;
241 row_cover.at(row) = true;
242 col_cover.at(col) = true;
243 break;
244 }
245 }
246 }
247
248 clearCovers(row_cover, col_cover);
249 return vpMunkres::STEP_T(3);
250}
251
264template <typename Type>
265inline std::tuple<vpMunkres::STEP_T, std::optional<std::pair<unsigned int, unsigned int> > >
266vpMunkres::stepFour(const std::vector<std::vector<Type> > &costs, std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
267 std::vector<bool> &row_cover, std::vector<bool> &col_cover)
268{
269 if (const auto zero = findAZero(costs, row_cover, col_cover)) {
270 const auto [row, col] = *zero; // convenient zero.value() is not working on iOS
271 mask.at(row).at(col) = vpMunkres::ZERO_T::PRIMED;
272
273 if (const auto star_in_row = findStarInRow(mask, row)) {
274 row_cover.at(row) = true;
275 col_cover.at(*star_in_row) = false;
276 return {vpMunkres::STEP_T(4), std::nullopt}; // Repeat
277 } else {
278 return {vpMunkres::STEP_T(5), std::make_optional<std::pair<unsigned int, unsigned int> >(row, col)};
279 }
280 } else {
281 return {vpMunkres::STEP_T(6), std::nullopt};
282 }
283}
284
294template <typename Type>
295inline vpMunkres::STEP_T vpMunkres::stepSix(std::vector<std::vector<Type> > &costs, const std::vector<bool> &row_cover,
296 const std::vector<bool> &col_cover)
297{
298 const auto minval = findSmallest(costs, row_cover, col_cover);
299 for (auto row = 0u; row < costs.size(); row++) {
300 for (auto col = 0u; col < costs.size(); col++) {
301 if (row_cover.at(row)) {
302 costs.at(row).at(col) += minval;
303 }
304
305 if (!col_cover.at(col)) {
306 costs.at(row).at(col) -= minval;
307 }
308 }
309 }
310
311 return vpMunkres::STEP_T(4);
312}
313
320template <typename Type>
321inline std::vector<std::pair<unsigned int, unsigned int> > vpMunkres::run(std::vector<std::vector<Type> > costs)
322{
323 const auto original_row_size = costs.size();
324 const auto original_col_size = costs.front().size();
325 const auto sq_size = std::max(original_row_size, original_col_size);
326
327 auto mask = std::vector<std::vector<vpMunkres::ZERO_T> >(
328 sq_size, std::vector<vpMunkres::ZERO_T>(sq_size, vpMunkres::ZERO_T::NA));
329 auto row_cover = std::vector<bool>(sq_size, false);
330 auto col_cover = std::vector<bool>(sq_size, false);
331
332 std::optional<std::pair<unsigned int, unsigned int> > path_0{std::nullopt};
333
334 auto step{vpMunkres::STEP_T::ENTRY};
335 while (step != vpMunkres::STEP_T::DONE) {
336 switch (step) {
337 case vpMunkres::STEP_T::ENTRY:
338 padCostMatrix(costs);
339 step = vpMunkres::STEP_T(1);
340 break;
341 case 1:
342 step = stepOne(costs);
343 break;
344 case 2:
345 step = stepTwo(costs, mask, row_cover, col_cover);
346 break;
347 case 3:
348 step = stepThree(mask, col_cover);
349 break;
350 case 4:
351 std::tie(step, path_0) = stepFour(costs, mask, row_cover, col_cover);
352 break;
353 case 5:
354 step = stepFive(mask, *path_0, row_cover, col_cover);
355 break;
356 case 6:
357 step = stepSix(costs, row_cover, col_cover);
358 break;
359 case vpMunkres::STEP_T::DONE:
360 default:
361 break;
362 }
363 }
364
365 // Compute the pairs
366 std::vector<std::pair<unsigned int, unsigned int> > ret{};
367 for (auto i = 0u; i < original_row_size; i++) {
368 if (const auto it = std::find(begin(mask.at(i)), end(mask.at(i)), vpMunkres::ZERO_T::STARRED);
369 it != end(mask.at(i))) {
370 if (const unsigned int j = static_cast<unsigned int>(std::distance(begin(mask.at(i)), it));
371 j < original_col_size) {
372 ret.emplace_back(i, j);
373 }
374 }
375 }
376
377 return ret;
378}
379
380#endif
static bool equal(double x, double y, double s=0.001)
Definition: vpMath.h:295
static std::vector< std::pair< unsigned int, unsigned int > > run(std::vector< std::vector< Type > > costs)
Definition: vpMunkres.h:321