Visual Servoing Platform  version 3.6.1 under development (2024-03-29)
vpMunkres.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 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 https://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 #include <visp3/core/vpMunkres.h>
41 
42 // Check if std:c++17 or higher
43 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
44 
52 std::optional<unsigned int> vpMunkres::findStarInRow(const std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
53  const unsigned int &row)
54 {
55  const auto it = std::find(begin(mask.at(row)), end(mask.at(row)), vpMunkres::ZERO_T::STARRED);
56  return it != end(mask.at(row)) ? std::make_optional<unsigned int>(std::distance(begin(mask.at(row)), it))
57  : std::nullopt;
58 }
59 
67 std::optional<unsigned int> vpMunkres::findStarInCol(const std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
68  const unsigned int &col)
69 {
70  const auto it = std::find_if(begin(mask), end(mask),
71  [&col](const auto &row) { return row.at(col) == vpMunkres::ZERO_T::STARRED; });
72  return it != end(mask) ? std::make_optional<unsigned int>(std::distance(begin(mask), it)) : std::nullopt;
73 }
74 
82 std::optional<unsigned int> vpMunkres::findPrimeInRow(const std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
83  const unsigned int &row)
84 {
85  const auto it = std::find(begin(mask.at(row)), end(mask.at(row)), vpMunkres::ZERO_T::PRIMED);
86  return it != end(mask.at(row)) ? std::make_optional<unsigned int>(std::distance(begin(mask.at(row)), it))
87  : std::nullopt;
88 }
89 
96 void vpMunkres::augmentPath(std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
97  const std::vector<std::pair<unsigned int, unsigned int> > &path)
98 {
99  for (const auto &[row, col] : path) {
100  mask.at(row).at(col) =
101  mask.at(row).at(col) == vpMunkres::ZERO_T::STARRED ? vpMunkres::ZERO_T::NA : vpMunkres::ZERO_T::STARRED;
102  }
103 }
104 
111 void vpMunkres::clearCovers(std::vector<bool> &row_cover, std::vector<bool> &col_cover)
112 {
113  row_cover = std::vector<bool>(row_cover.size(), false);
114  col_cover = std::vector<bool>(col_cover.size(), false);
115 }
116 
122 void vpMunkres::erasePrimes(std::vector<std::vector<vpMunkres::ZERO_T> > &mask)
123 {
124  std::for_each(begin(mask), end(mask), [](auto &mask_row) {
125  std::for_each(begin(mask_row), end(mask_row), [](auto &val) {
126  if (val == vpMunkres::ZERO_T::PRIMED)
127  val = vpMunkres::ZERO_T::NA;
128  });
129  });
130 }
131 
141 vpMunkres::STEP_T vpMunkres::stepThree(const std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
142  std::vector<bool> &col_cover)
143 {
144  for (const auto &mask_row : mask) {
145  for (auto col = 0u; col < mask_row.size(); col++) {
146  if (mask_row.at(col) == vpMunkres::ZERO_T::STARRED) {
147  col_cover.at(col) = true;
148  }
149  }
150  }
151 
152  const unsigned int col_count = static_cast<unsigned int>(std::count(begin(col_cover), end(col_cover), true));
153  return col_count >= mask.size() ? vpMunkres::STEP_T::DONE : vpMunkres::STEP_T(4);
154 }
155 
171 vpMunkres::STEP_T vpMunkres::stepFive(std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
172  const std::pair<unsigned int, unsigned int> &path_0, std::vector<bool> &row_cover,
173  std::vector<bool> &col_cover)
174 {
175  std::vector<std::pair<unsigned int, unsigned int> > path { path_0 }; // Z0
176 
177  while (true) {
178  if (const auto star_in_col = findStarInCol(mask, path.back().second)) {
179  path.emplace_back(*star_in_col, path.back().second); // Z1
180  }
181  else {
182  augmentPath(mask, path);
183  erasePrimes(mask);
184  clearCovers(row_cover, col_cover);
185 
186  return vpMunkres::STEP_T(3);
187  }
188 
189  if (const auto prime_in_row = findPrimeInRow(mask, path.back().first)) {
190  path.emplace_back(path.back().first, *prime_in_row); // Z2
191  }
192  }
193 }
194 
195 #endif