Visual Servoing Platform  version 3.6.1 under development (2025-01-13)
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 // Check if std:c++17 or higher.
39 // Here we cannot use (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) in the declaration of the class
40 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
41 
42 // System
43 #include <optional>
44 #include <tuple>
45 
46 // Internal
47 #include "vpMath.h"
48 
49 BEGIN_VISP_NAMESPACE
58 class VISP_EXPORT vpMunkres
59 {
60 public:
61  template <typename Type>
62  static std::vector<std::pair<unsigned int, unsigned int> > run(std::vector<std::vector<Type> > costs);
63 
64 private:
65  enum ZERO_T : unsigned int;
66  enum STEP_T : unsigned int;
67 
68  // Init
69  template <typename Type> static void padCostMatrix(std::vector<std::vector<Type> > &costs);
70 
71  // Global helpers
72  template <typename Type>
73  static std::optional<std::pair<unsigned int, unsigned int> > findAZero(const std::vector<std::vector<Type> > &costs,
74  const std::vector<bool> &row_cover,
75  const std::vector<bool> &col_cover);
76  static std::optional<unsigned int> findStarInRow(const std::vector<std::vector<ZERO_T> > &mask,
77  const unsigned int &row);
78  static std::optional<unsigned int> findStarInCol(const std::vector<std::vector<ZERO_T> > &mask,
79  const unsigned int &col);
80  static std::optional<unsigned int> findPrimeInRow(const std::vector<std::vector<ZERO_T> > &mask,
81  const unsigned int &row);
82  template <typename Type>
83  static Type findSmallest(const std::vector<std::vector<Type> > &costs, const std::vector<bool> &row_cover,
84  const std::vector<bool> &col_cover);
85 
86  // FSM helpers
87  static void augmentPath(std::vector<std::vector<ZERO_T> > &mask,
88  const std::vector<std::pair<unsigned int, unsigned int> > &path);
89  static void clearCovers(std::vector<bool> &row_cover, std::vector<bool> &col_cover);
90  static void erasePrimes(std::vector<std::vector<ZERO_T> > &mask);
91 
92  // FSM
93  template <typename Type> static STEP_T stepOne(std::vector<std::vector<Type> > &costs);
94  template <typename Type>
95  static STEP_T stepTwo(std::vector<std::vector<Type> > &costs, std::vector<std::vector<ZERO_T> > &mask,
96  std::vector<bool> &row_cover, std::vector<bool> &col_cover);
97  static STEP_T stepThree(const std::vector<std::vector<ZERO_T> > &mask, std::vector<bool> &col_cover);
98  template <typename Type>
99  static std::tuple<STEP_T, std::optional<std::pair<unsigned int, unsigned int> > >
100  stepFour(const std::vector<std::vector<Type> > &costs, std::vector<std::vector<ZERO_T> > &mask,
101  std::vector<bool> &row_cover, std::vector<bool> &col_cover);
102  static STEP_T stepFive(std::vector<std::vector<ZERO_T> > &mask, const std::pair<unsigned int, unsigned int> &path_0,
103  std::vector<bool> &row_cover, std::vector<bool> &col_cover);
104  template <typename Type>
105  static STEP_T stepSix(std::vector<std::vector<Type> > &costs, const std::vector<bool> &row_cover,
106  const std::vector<bool> &col_cover);
107 
108 private:
109  static constexpr auto ZeroEpsilon { 1e-6 };
110 };
111 
112 enum vpMunkres::ZERO_T : unsigned int { NA = 0, STARRED = 1, PRIMED = 2 };
113 
114 enum vpMunkres::STEP_T : unsigned int { ENTRY = 0, ONE = 1, TWO = 2, THREE = 3, FOUR = 4, FIVE = 5, SIX = 6, DONE };
115 
121 template <typename Type> inline void vpMunkres::padCostMatrix(std::vector<std::vector<Type> > &costs)
122 {
123  const auto row_input_size = costs.size();
124  const auto col_input_size = costs.at(0).size();
125 
126  if (row_input_size > col_input_size) {
127  for (auto &vec : costs)
128  vec.resize(row_input_size, 0);
129  }
130 
131  while (costs.size() < col_input_size) {
132  costs.emplace_back(col_input_size, 0);
133  }
134 }
135 
144 template <typename Type>
145 inline std::optional<std::pair<unsigned int, unsigned int> >
146 vpMunkres::findAZero(const std::vector<std::vector<Type> > &costs, const std::vector<bool> &row_cover,
147  const std::vector<bool> &col_cover)
148 {
149  for (auto row = 0u; row < costs.size(); row++)
150  for (auto col = 0u; col < costs.size(); col++)
151  if (vpMath::equal(costs.at(row).at(col), static_cast<Type>(vpMunkres::ZeroEpsilon)) && !row_cover.at(row) &&
152  !col_cover.at(col)) {
153  return std::make_optional<std::pair<unsigned int, unsigned int> >(row, col);
154  }
155 
156  return std::nullopt;
157 }
158 
167 template <typename Type>
168 inline Type vpMunkres::findSmallest(const std::vector<std::vector<Type> > &costs, const std::vector<bool> &row_cover,
169  const std::vector<bool> &col_cover)
170 {
171  auto minval = std::numeric_limits<Type>::max();
172  for (auto row = 0u; row < costs.size(); row++)
173  for (auto col = 0u; col < costs.size(); col++)
174  if (minval > costs.at(row).at(col) && !row_cover.at(row) && !col_cover.at(col)) {
175  minval = costs.at(row).at(col);
176  }
177 
178  return minval;
179 }
180 
189 template <typename Type> inline vpMunkres::STEP_T vpMunkres::stepOne(std::vector<std::vector<Type> > &costs)
190 {
191  // process rows
192  std::for_each(begin(costs), end(costs), [](auto &cost_row) {
193  const auto min_in_row = *std::min_element(begin(cost_row), end(cost_row));
194  std::transform(begin(cost_row), end(cost_row), begin(cost_row),
195  [&min_in_row](auto &cost) { return cost - min_in_row; });
196  });
197 
198  // process cols
199  for (auto col = 0u; col < costs.size(); ++col) {
200  auto minval = std::numeric_limits<Type>::max();
201  for (const auto &cost_row : costs) {
202  minval = std::min<Type>(minval, cost_row.at(col));
203  }
204 
205  for (auto &cost_row : costs) {
206  cost_row.at(col) -= minval;
207  }
208  }
209 
210  return vpMunkres::STEP_T(2);
211 }
212 
224 template <typename Type>
225 inline vpMunkres::STEP_T vpMunkres::stepTwo(std::vector<std::vector<Type> > &costs,
226  std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
227  std::vector<bool> &row_cover, std::vector<bool> &col_cover)
228 {
229  for (auto row = 0u; row < costs.size(); row++) {
230  for (auto col = 0u; col < costs.size(); col++) {
231  if (vpMath::equal(costs.at(row).at(col), static_cast<Type>(vpMunkres::ZeroEpsilon)) && !row_cover.at(row) &&
232  !col_cover.at(col)) {
233  mask.at(row).at(col) = vpMunkres::ZERO_T::STARRED;
234  row_cover.at(row) = true;
235  col_cover.at(col) = true;
236  break;
237  }
238  }
239  }
240 
241  clearCovers(row_cover, col_cover);
242  return vpMunkres::STEP_T(3);
243 }
244 
257 template <typename Type>
258 inline std::tuple<vpMunkres::STEP_T, std::optional<std::pair<unsigned int, unsigned int> > >
259 vpMunkres::stepFour(const std::vector<std::vector<Type> > &costs, std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
260  std::vector<bool> &row_cover, std::vector<bool> &col_cover)
261 {
262  if (const auto zero = findAZero(costs, row_cover, col_cover)) {
263  const auto [row, col] = *zero; // convenient zero.value() is not working on iOS
264  mask.at(row).at(col) = vpMunkres::ZERO_T::PRIMED;
265 
266  if (const auto star_in_row = findStarInRow(mask, row)) {
267  row_cover.at(row) = true;
268  col_cover.at(*star_in_row) = false;
269  return { vpMunkres::STEP_T(4), std::nullopt }; // Repeat
270  }
271  else {
272  return { vpMunkres::STEP_T(5), std::make_optional<std::pair<unsigned int, unsigned int> >(row, col) };
273  }
274  }
275  else {
276  return { vpMunkres::STEP_T(6), std::nullopt };
277  }
278 }
279 
289 template <typename Type>
290 inline vpMunkres::STEP_T vpMunkres::stepSix(std::vector<std::vector<Type> > &costs, const std::vector<bool> &row_cover,
291  const std::vector<bool> &col_cover)
292 {
293  const auto minval = findSmallest(costs, row_cover, col_cover);
294  for (auto row = 0u; row < costs.size(); row++) {
295  for (auto col = 0u; col < costs.size(); col++) {
296  if (row_cover.at(row)) {
297  costs.at(row).at(col) += minval;
298  }
299 
300  if (!col_cover.at(col)) {
301  costs.at(row).at(col) -= minval;
302  }
303  }
304  }
305 
306  return vpMunkres::STEP_T(4);
307 }
308 
315 template <typename Type>
316 inline std::vector<std::pair<unsigned int, unsigned int> > vpMunkres::run(std::vector<std::vector<Type> > costs)
317 {
318  const auto original_row_size = static_cast<Type>(costs.size());
319  const auto original_col_size = static_cast<Type>(costs.front().size());
320  const size_t sq_size = static_cast<size_t>(std::max<Type>(original_row_size, original_col_size));
321 
322  auto mask = std::vector<std::vector<vpMunkres::ZERO_T> >(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 END_VISP_NAMESPACE
374 #endif
vpMath::equal
static bool equal(double x, double y, double threshold=0.001)
Definition: vpMath.h:459