Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpMunkres.h
1/*
2 * ViSP, open source Visual Servoing Platform software.
3 * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
4 *
5 * This software is free software; you can redistribute it and/or modify
6 * it under the terms of the GNU General Public License as published by
7 * the Free Software Foundation; either version 2 of the License, or
8 * (at your option) any later version.
9 * See the file LICENSE.txt at the root directory of this source
10 * distribution for additional information about the GNU GPL.
11 *
12 * For using ViSP with software that can not be combined with the GNU
13 * GPL, please contact Inria about acquiring a ViSP Professional
14 * Edition License.
15 *
16 * See https://visp.inria.fr for more information.
17 *
18 * This software was developed at:
19 * Inria Rennes - Bretagne Atlantique
20 * Campus Universitaire de Beaulieu
21 * 35042 Rennes Cedex
22 * France
23 *
24 * If you have questions regarding the use of this file, please contact
25 * Inria at visp@inria.fr
26 *
27 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29 *
30 * Description:
31 * Class for Munkres Assignment Algorithm.
32 */
33
34#pragma once
35
36#include <visp3/core/vpConfig.h>
37
38#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
39 (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
40
41// Visual Studio: Optionals are available from Visual Studio 2017 RTW (15.0) [1910]
42// Visual Studio: Structured bindings are available from Visual Studio 2017 version 15.3 [1911]
43
44// System
45#include <optional>
46#include <tuple>
47
48// Internal
49#include "vpMath.h"
50
59class VISP_EXPORT vpMunkres
60{
61public:
62 template <typename Type>
63 static std::vector<std::pair<unsigned int, unsigned int> > run(std::vector<std::vector<Type> > costs);
64
65private:
66 enum ZERO_T : unsigned int;
67 enum STEP_T : unsigned int;
68
69 // Init
70 template <typename Type> static void padCostMatrix(std::vector<std::vector<Type> > &costs);
71
72 // Global helpers
73 template <typename Type>
74 static std::optional<std::pair<unsigned int, unsigned int> > findAZero(const std::vector<std::vector<Type> > &costs,
75 const std::vector<bool> &row_cover,
76 const std::vector<bool> &col_cover);
77 static std::optional<unsigned int> findStarInRow(const std::vector<std::vector<ZERO_T> > &mask,
78 const unsigned int &row);
79 static std::optional<unsigned int> findStarInCol(const std::vector<std::vector<ZERO_T> > &mask,
80 const unsigned int &col);
81 static std::optional<unsigned int> findPrimeInRow(const std::vector<std::vector<ZERO_T> > &mask,
82 const unsigned int &row);
83 template <typename Type>
84 static Type findSmallest(const std::vector<std::vector<Type> > &costs, const std::vector<bool> &row_cover,
85 const std::vector<bool> &col_cover);
86
87 // FSM helpers
88 static void augmentPath(std::vector<std::vector<ZERO_T> > &mask,
89 const std::vector<std::pair<unsigned int, unsigned int> > &path);
90 static void clearCovers(std::vector<bool> &row_cover, std::vector<bool> &col_cover);
91 static void erasePrimes(std::vector<std::vector<ZERO_T> > &mask);
92
93 // FSM
94 template <typename Type> static STEP_T stepOne(std::vector<std::vector<Type> > &costs);
95 template <typename Type>
96 static STEP_T stepTwo(std::vector<std::vector<Type> > &costs, std::vector<std::vector<ZERO_T> > &mask,
97 std::vector<bool> &row_cover, std::vector<bool> &col_cover);
98 static STEP_T stepThree(const std::vector<std::vector<ZERO_T> > &mask, std::vector<bool> &col_cover);
99 template <typename Type>
100 static std::tuple<STEP_T, std::optional<std::pair<unsigned int, unsigned int> > >
101 stepFour(const std::vector<std::vector<Type> > &costs, std::vector<std::vector<ZERO_T> > &mask,
102 std::vector<bool> &row_cover, std::vector<bool> &col_cover);
103 static STEP_T stepFive(std::vector<std::vector<ZERO_T> > &mask, const std::pair<unsigned int, unsigned int> &path_0,
104 std::vector<bool> &row_cover, std::vector<bool> &col_cover);
105 template <typename Type>
106 static STEP_T stepSix(std::vector<std::vector<Type> > &costs, const std::vector<bool> &row_cover,
107 const std::vector<bool> &col_cover);
108
109private:
110 static constexpr auto ZeroEpsilon{1e-6};
111};
112
113enum vpMunkres::ZERO_T : unsigned int { NA = 0, STARRED = 1, PRIMED = 2 };
114
115enum vpMunkres::STEP_T : unsigned int { ENTRY = 0, ONE = 1, TWO = 2, THREE = 3, FOUR = 4, FIVE = 5, SIX = 6, DONE };
116
122template <typename Type> inline void vpMunkres::padCostMatrix(std::vector<std::vector<Type> > &costs)
123{
124 const auto row_input_size = costs.size();
125 const auto col_input_size = costs.at(0).size();
126
127 if (row_input_size > col_input_size) {
128 for (auto &vec : costs)
129 vec.resize(row_input_size, 0);
130 }
131
132 while (costs.size() < col_input_size) {
133 costs.emplace_back(col_input_size, 0);
134 }
135}
136
145template <typename Type>
146inline std::optional<std::pair<unsigned int, unsigned int> >
147vpMunkres::findAZero(const std::vector<std::vector<Type> > &costs, const std::vector<bool> &row_cover,
148 const std::vector<bool> &col_cover)
149{
150 for (auto row = 0u; row < costs.size(); row++)
151 for (auto col = 0u; col < costs.size(); col++)
152 if (vpMath::equal(costs.at(row).at(col), static_cast<Type>(vpMunkres::ZeroEpsilon)) && !row_cover.at(row) &&
153 !col_cover.at(col)) {
154 return std::make_optional<std::pair<unsigned int, unsigned int> >(row, col);
155 }
156
157 return std::nullopt;
158}
159
168template <typename Type>
169inline Type vpMunkres::findSmallest(const std::vector<std::vector<Type> > &costs, const std::vector<bool> &row_cover,
170 const std::vector<bool> &col_cover)
171{
172 auto minval = std::numeric_limits<Type>::max();
173 for (auto row = 0u; row < costs.size(); row++)
174 for (auto col = 0u; col < costs.size(); col++)
175 if (minval > costs.at(row).at(col) && !row_cover.at(row) && !col_cover.at(col)) {
176 minval = costs.at(row).at(col);
177 }
178
179 return minval;
180}
181
190template <typename Type> inline vpMunkres::STEP_T vpMunkres::stepOne(std::vector<std::vector<Type> > &costs)
191{
192 // process rows
193 std::for_each(begin(costs), end(costs), [](auto &cost_row) {
194 const auto min_in_row = *std::min_element(begin(cost_row), end(cost_row));
195 std::transform(begin(cost_row), end(cost_row), begin(cost_row),
196 [&min_in_row](auto &cost) { return cost - min_in_row; });
197 });
198
199 // process cols
200 for (auto col = 0u; col < costs.size(); ++col) {
201 auto minval = std::numeric_limits<Type>::max();
202 for (const auto &cost_row : costs) {
203 minval = std::min(minval, cost_row.at(col));
204 }
205
206 for (auto &cost_row : costs) {
207 cost_row.at(col) -= minval;
208 }
209 }
210
211 return vpMunkres::STEP_T(2);
212}
213
225template <typename Type>
226inline vpMunkres::STEP_T vpMunkres::stepTwo(std::vector<std::vector<Type> > &costs,
227 std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
228 std::vector<bool> &row_cover, std::vector<bool> &col_cover)
229{
230 for (auto row = 0u; row < costs.size(); row++) {
231 for (auto col = 0u; col < costs.size(); col++) {
232 if (vpMath::equal(costs.at(row).at(col), static_cast<Type>(vpMunkres::ZeroEpsilon)) && !row_cover.at(row) &&
233 !col_cover.at(col)) {
234 mask.at(row).at(col) = vpMunkres::ZERO_T::STARRED;
235 row_cover.at(row) = true;
236 col_cover.at(col) = true;
237 break;
238 }
239 }
240 }
241
242 clearCovers(row_cover, col_cover);
243 return vpMunkres::STEP_T(3);
244}
245
258template <typename Type>
259inline std::tuple<vpMunkres::STEP_T, std::optional<std::pair<unsigned int, unsigned int> > >
260vpMunkres::stepFour(const std::vector<std::vector<Type> > &costs, std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
261 std::vector<bool> &row_cover, std::vector<bool> &col_cover)
262{
263 if (const auto zero = findAZero(costs, row_cover, col_cover)) {
264 const auto [row, col] = *zero; // convenient zero.value() is not working on iOS
265 mask.at(row).at(col) = vpMunkres::ZERO_T::PRIMED;
266
267 if (const auto star_in_row = findStarInRow(mask, row)) {
268 row_cover.at(row) = true;
269 col_cover.at(*star_in_row) = false;
270 return {vpMunkres::STEP_T(4), std::nullopt}; // Repeat
271 } else {
272 return {vpMunkres::STEP_T(5), std::make_optional<std::pair<unsigned int, unsigned int> >(row, col)};
273 }
274 } else {
275 return {vpMunkres::STEP_T(6), std::nullopt};
276 }
277}
278
288template <typename Type>
289inline vpMunkres::STEP_T vpMunkres::stepSix(std::vector<std::vector<Type> > &costs, const std::vector<bool> &row_cover,
290 const std::vector<bool> &col_cover)
291{
292 const auto minval = findSmallest(costs, row_cover, col_cover);
293 for (auto row = 0u; row < costs.size(); row++) {
294 for (auto col = 0u; col < costs.size(); col++) {
295 if (row_cover.at(row)) {
296 costs.at(row).at(col) += minval;
297 }
298
299 if (!col_cover.at(col)) {
300 costs.at(row).at(col) -= minval;
301 }
302 }
303 }
304
305 return vpMunkres::STEP_T(4);
306}
307
314template <typename Type>
315inline std::vector<std::pair<unsigned int, unsigned int> > vpMunkres::run(std::vector<std::vector<Type> > costs)
316{
317 const auto original_row_size = costs.size();
318 const auto original_col_size = costs.front().size();
319 const auto sq_size = std::max(original_row_size, original_col_size);
320
321 auto mask = std::vector<std::vector<vpMunkres::ZERO_T> >(
322 sq_size, std::vector<vpMunkres::ZERO_T>(sq_size, vpMunkres::ZERO_T::NA));
323 auto row_cover = std::vector<bool>(sq_size, false);
324 auto col_cover = std::vector<bool>(sq_size, false);
325
326 std::optional<std::pair<unsigned int, unsigned int> > path_0{std::nullopt};
327
328 auto step{vpMunkres::STEP_T::ENTRY};
329 while (step != vpMunkres::STEP_T::DONE) {
330 switch (step) {
331 case vpMunkres::STEP_T::ENTRY:
332 padCostMatrix(costs);
333 step = vpMunkres::STEP_T(1);
334 break;
335 case 1:
336 step = stepOne(costs);
337 break;
338 case 2:
339 step = stepTwo(costs, mask, row_cover, col_cover);
340 break;
341 case 3:
342 step = stepThree(mask, col_cover);
343 break;
344 case 4:
345 std::tie(step, path_0) = stepFour(costs, mask, row_cover, col_cover);
346 break;
347 case 5:
348 step = stepFive(mask, *path_0, row_cover, col_cover);
349 break;
350 case 6:
351 step = stepSix(costs, row_cover, col_cover);
352 break;
353 case vpMunkres::STEP_T::DONE:
354 default:
355 break;
356 }
357 }
358
359 // Compute the pairs
360 std::vector<std::pair<unsigned int, unsigned int> > ret{};
361 for (auto i = 0u; i < original_row_size; i++) {
362 if (const auto it = std::find(begin(mask.at(i)), end(mask.at(i)), vpMunkres::ZERO_T::STARRED);
363 it != end(mask.at(i))) {
364 if (const unsigned int j = static_cast<unsigned int>(std::distance(begin(mask.at(i)), it));
365 j < original_col_size) {
366 ret.emplace_back(i, j);
367 }
368 }
369 }
370
371 return ret;
372}
373
374#endif
static bool equal(double x, double y, double threshold=0.001)
Definition vpMath.h:369
static std::vector< std::pair< unsigned int, unsigned int > > run(std::vector< std::vector< Type > > costs)
Definition vpMunkres.h:315