Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && \
43 (!defined(_MSC_VER) || ((VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17) && (_MSC_VER >= 1911)))
44
52std::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
67std::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
82std::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
96void 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
111void 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
122void 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
141vpMunkres::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
171vpMunkres::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 } else {
181 augmentPath(mask, path);
182 erasePrimes(mask);
183 clearCovers(row_cover, col_cover);
184
185 return vpMunkres::STEP_T(3);
186 }
187
188 if (const auto prime_in_row = findPrimeInRow(mask, path.back().first)) {
189 path.emplace_back(path.back().first, *prime_in_row); // Z2
190 }
191 }
192}
193
194#endif // (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_17)