Visual Servoing Platform  version 3.5.1 under development (2022-08-16)
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 
65 class VISP_EXPORT vpMunkres
66 {
67 public:
68  template <typename Type>
69  static std::vector<std::pair<unsigned int, unsigned int> > run(std::vector<std::vector<Type> > costs);
70 
71 private:
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 
115 private:
116  static constexpr auto ZeroEpsilon{1e-6};
117 };
118 
119 enum vpMunkres::ZERO_T : unsigned int { NA = 0, STARRED = 1, PRIMED = 2 };
120 
121 enum vpMunkres::STEP_T : unsigned int { ENTRY = 0, ONE = 1, TWO = 2, THREE = 3, FOUR = 4, FIVE = 5, SIX = 6, DONE };
122 
128 template <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 
151 template <typename Type>
152 inline std::optional<std::pair<unsigned int, unsigned int> >
153 vpMunkres::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 
174 template <typename Type>
175 inline 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 
196 template <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 
231 template <typename Type>
232 inline 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 
264 template <typename Type>
265 inline std::tuple<vpMunkres::STEP_T, std::optional<std::pair<unsigned int, unsigned int> > >
266 vpMunkres::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 
294 template <typename Type>
295 inline 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 
320 template <typename Type>
321 inline 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:364
static std::vector< std::pair< unsigned int, unsigned int > > run(std::vector< std::vector< Type > > costs)
Definition: vpMunkres.h:321