Visual Servoing Platform  version 3.6.1 under development (2025-03-14)
vpMunkres.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 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 #include <visp3/core/vpMunkres.h>
35 
36 // Check if std:c++17 or higher
37 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
38 
39 BEGIN_VISP_NAMESPACE
47  std::optional<unsigned int> vpMunkres::findStarInRow(const std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
48  const unsigned int &row)
49 {
50  const auto it = std::find(begin(mask.at(row)), end(mask.at(row)), vpMunkres::ZERO_T::STARRED);
51  return it != end(mask.at(row)) ? std::make_optional<unsigned int>(static_cast<unsigned int>(std::distance(begin(mask.at(row)), it)))
52  : std::nullopt;
53 }
54 
62 std::optional<unsigned int> vpMunkres::findStarInCol(const std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
63  const unsigned int &col)
64 {
65  const auto it = std::find_if(begin(mask), end(mask),
66  [&col](const auto &row) { return row.at(col) == vpMunkres::ZERO_T::STARRED; });
67  return it != end(mask) ? std::make_optional<unsigned int>(static_cast<unsigned int>(std::distance(begin(mask), it)))
68  : std::nullopt;
69 }
70 
78 std::optional<unsigned int> vpMunkres::findPrimeInRow(const std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
79  const unsigned int &row)
80 {
81  const auto it = std::find(begin(mask.at(row)), end(mask.at(row)), vpMunkres::ZERO_T::PRIMED);
82  return it != end(mask.at(row))
83  ? std::make_optional<unsigned int>(static_cast<unsigned int>(std::distance(begin(mask.at(row)), it)))
84  : std::nullopt;
85 }
86 
93 void vpMunkres::augmentPath(std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
94  const std::vector<std::pair<unsigned int, unsigned int> > &path)
95 {
96  for (const auto &[row, col] : path) {
97  mask.at(row).at(col) =
98  mask.at(row).at(col) == vpMunkres::ZERO_T::STARRED ? vpMunkres::ZERO_T::NA : vpMunkres::ZERO_T::STARRED;
99  }
100 }
101 
108 void vpMunkres::clearCovers(std::vector<bool> &row_cover, std::vector<bool> &col_cover)
109 {
110  row_cover = std::vector<bool>(row_cover.size(), false);
111  col_cover = std::vector<bool>(col_cover.size(), false);
112 }
113 
119 void vpMunkres::erasePrimes(std::vector<std::vector<vpMunkres::ZERO_T> > &mask)
120 {
121  std::for_each(begin(mask), end(mask), [](auto &mask_row) {
122  std::for_each(begin(mask_row), end(mask_row), [](auto &val) {
123  if (val == vpMunkres::ZERO_T::PRIMED)
124  val = vpMunkres::ZERO_T::NA;
125  });
126  });
127 }
128 
138 vpMunkres::STEP_T vpMunkres::stepThree(const std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
139  std::vector<bool> &col_cover)
140 {
141  for (const auto &mask_row : mask) {
142  for (auto col = 0u; col < mask_row.size(); col++) {
143  if (mask_row.at(col) == vpMunkres::ZERO_T::STARRED) {
144  col_cover.at(col) = true;
145  }
146  }
147  }
148 
149  const unsigned int col_count = static_cast<unsigned int>(std::count(begin(col_cover), end(col_cover), true));
150  return col_count >= mask.size() ? vpMunkres::STEP_T::DONE : vpMunkres::STEP_T(4);
151 }
152 
168 vpMunkres::STEP_T vpMunkres::stepFive(std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
169  const std::pair<unsigned int, unsigned int> &path_0, std::vector<bool> &row_cover,
170  std::vector<bool> &col_cover)
171 {
172  std::vector<std::pair<unsigned int, unsigned int> > path { path_0 }; // Z0
173 
174  while (true) {
175  if (const auto star_in_col = findStarInCol(mask, path.back().second)) {
176  path.emplace_back(*star_in_col, path.back().second); // Z1
177  }
178  else {
179  augmentPath(mask, path);
180  erasePrimes(mask);
181  clearCovers(row_cover, col_cover);
182 
183  return vpMunkres::STEP_T(3);
184  }
185 
186  if (const auto prime_in_row = findPrimeInRow(mask, path.back().first)) {
187  path.emplace_back(path.back().first, *prime_in_row); // Z2
188  }
189  }
190 }
191 END_VISP_NAMESPACE
192 #endif