36 #include <visp3/core/vpConfig.h>
40 #if ((__cplusplus >= 201703L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201703L)))
57 class VISP_EXPORT vpMunkres
60 template <
typename Type>
61 static std::vector<std::pair<unsigned int, unsigned int> > run(std::vector<std::vector<Type> > costs);
64 enum ZERO_T :
unsigned int;
65 enum STEP_T :
unsigned int;
68 template <
typename Type>
static void padCostMatrix(std::vector<std::vector<Type> > &costs);
71 template <
typename Type>
72 static std::optional<std::pair<unsigned int, unsigned int> > findAZero(
const std::vector<std::vector<Type> > &costs,
73 const std::vector<bool> &row_cover,
74 const std::vector<bool> &col_cover);
75 static std::optional<unsigned int> findStarInRow(
const std::vector<std::vector<ZERO_T> > &mask,
76 const unsigned int &row);
77 static std::optional<unsigned int> findStarInCol(
const std::vector<std::vector<ZERO_T> > &mask,
78 const unsigned int &col);
79 static std::optional<unsigned int> findPrimeInRow(
const std::vector<std::vector<ZERO_T> > &mask,
80 const unsigned int &row);
81 template <
typename Type>
82 static Type findSmallest(
const std::vector<std::vector<Type> > &costs,
const std::vector<bool> &row_cover,
83 const std::vector<bool> &col_cover);
86 static void augmentPath(std::vector<std::vector<ZERO_T> > &mask,
87 const std::vector<std::pair<unsigned int, unsigned int> > &path);
88 static void clearCovers(std::vector<bool> &row_cover, std::vector<bool> &col_cover);
89 static void erasePrimes(std::vector<std::vector<ZERO_T> > &mask);
92 template <
typename Type>
static STEP_T stepOne(std::vector<std::vector<Type> > &costs);
93 template <
typename Type>
94 static STEP_T stepTwo(std::vector<std::vector<Type> > &costs, std::vector<std::vector<ZERO_T> > &mask,
95 std::vector<bool> &row_cover, std::vector<bool> &col_cover);
96 static STEP_T stepThree(
const std::vector<std::vector<ZERO_T> > &mask, std::vector<bool> &col_cover);
97 template <
typename Type>
98 static std::tuple<STEP_T, std::optional<std::pair<unsigned int, unsigned int> > >
99 stepFour(
const std::vector<std::vector<Type> > &costs, std::vector<std::vector<ZERO_T> > &mask,
100 std::vector<bool> &row_cover, std::vector<bool> &col_cover);
101 static STEP_T stepFive(std::vector<std::vector<ZERO_T> > &mask,
const std::pair<unsigned int, unsigned int> &path_0,
102 std::vector<bool> &row_cover, std::vector<bool> &col_cover);
103 template <
typename Type>
104 static STEP_T stepSix(std::vector<std::vector<Type> > &costs,
const std::vector<bool> &row_cover,
105 const std::vector<bool> &col_cover);
108 static constexpr
auto ZeroEpsilon { 1e-6 };
111 enum vpMunkres::ZERO_T :
unsigned int { NA = 0, STARRED = 1, PRIMED = 2 };
113 enum vpMunkres::STEP_T :
unsigned int { ENTRY = 0, ONE = 1, TWO = 2, THREE = 3, FOUR = 4, FIVE = 5, SIX = 6, DONE };
120 template <
typename Type>
inline void vpMunkres::padCostMatrix(std::vector<std::vector<Type> > &costs)
122 const auto row_input_size = costs.size();
123 const auto col_input_size = costs.at(0).size();
125 if (row_input_size > col_input_size) {
126 for (
auto &vec : costs)
127 vec.resize(row_input_size, 0);
130 while (costs.size() < col_input_size) {
131 costs.emplace_back(col_input_size, 0);
143 template <
typename Type>
144 inline std::optional<std::pair<unsigned int, unsigned int> >
145 vpMunkres::findAZero(
const std::vector<std::vector<Type> > &costs,
const std::vector<bool> &row_cover,
146 const std::vector<bool> &col_cover)
148 for (
auto row = 0u; row < costs.size(); row++)
149 for (
auto col = 0u; col < costs.size(); col++)
150 if (
vpMath::equal(costs.at(row).at(col),
static_cast<Type
>(vpMunkres::ZeroEpsilon)) && !row_cover.at(row) &&
151 !col_cover.at(col)) {
152 return std::make_optional<std::pair<unsigned int, unsigned int> >(row, col);
166 template <
typename Type>
167 inline Type vpMunkres::findSmallest(
const std::vector<std::vector<Type> > &costs,
const std::vector<bool> &row_cover,
168 const std::vector<bool> &col_cover)
170 auto minval = std::numeric_limits<Type>::max();
171 for (
auto row = 0u; row < costs.size(); row++)
172 for (
auto col = 0u; col < costs.size(); col++)
173 if (minval > costs.at(row).at(col) && !row_cover.at(row) && !col_cover.at(col)) {
174 minval = costs.at(row).at(col);
188 template <
typename Type>
inline vpMunkres::STEP_T vpMunkres::stepOne(std::vector<std::vector<Type> > &costs)
191 std::for_each(begin(costs), end(costs), [](
auto &cost_row) {
192 const auto min_in_row = *std::min_element(begin(cost_row), end(cost_row));
193 std::transform(begin(cost_row), end(cost_row), begin(cost_row),
194 [&min_in_row](
auto &cost) {
return cost - min_in_row; });
198 for (
auto col = 0u; col < costs.size(); ++col) {
199 auto minval = std::numeric_limits<Type>::max();
200 for (
const auto &cost_row : costs) {
201 minval = std::min<Type>(minval, cost_row.at(col));
204 for (
auto &cost_row : costs) {
205 cost_row.at(col) -= minval;
209 return vpMunkres::STEP_T(2);
223 template <
typename Type>
224 inline vpMunkres::STEP_T vpMunkres::stepTwo(std::vector<std::vector<Type> > &costs,
225 std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
226 std::vector<bool> &row_cover, std::vector<bool> &col_cover)
228 for (
auto row = 0u; row < costs.size(); row++) {
229 for (
auto col = 0u; col < costs.size(); col++) {
230 if (
vpMath::equal(costs.at(row).at(col),
static_cast<Type
>(vpMunkres::ZeroEpsilon)) && !row_cover.at(row) &&
231 !col_cover.at(col)) {
232 mask.at(row).at(col) = vpMunkres::ZERO_T::STARRED;
233 row_cover.at(row) =
true;
234 col_cover.at(col) =
true;
240 clearCovers(row_cover, col_cover);
241 return vpMunkres::STEP_T(3);
256 template <
typename Type>
257 inline std::tuple<vpMunkres::STEP_T, std::optional<std::pair<unsigned int, unsigned int> > >
258 vpMunkres::stepFour(
const std::vector<std::vector<Type> > &costs, std::vector<std::vector<vpMunkres::ZERO_T> > &mask,
259 std::vector<bool> &row_cover, std::vector<bool> &col_cover)
261 if (
const auto zero = findAZero(costs, row_cover, col_cover)) {
262 const auto [row, col] = *zero;
263 mask.at(row).at(col) = vpMunkres::ZERO_T::PRIMED;
265 if (
const auto star_in_row = findStarInRow(mask, row)) {
266 row_cover.at(row) =
true;
267 col_cover.at(*star_in_row) =
false;
268 return { vpMunkres::STEP_T(4), std::nullopt };
271 return { vpMunkres::STEP_T(5), std::make_optional<std::pair<unsigned int, unsigned int> >(row, col) };
275 return { vpMunkres::STEP_T(6), std::nullopt };
288 template <
typename Type>
289 inline vpMunkres::STEP_T vpMunkres::stepSix(std::vector<std::vector<Type> > &costs,
const std::vector<bool> &row_cover,
290 const std::vector<bool> &col_cover)
292 const auto minval = findSmallest(costs, row_cover, col_cover);
293 for (
auto row = 0u; row < costs.size(); row++) {
294 for (
auto col = 0u; col < costs.size(); col++) {
295 if (row_cover.at(row)) {
296 costs.at(row).at(col) += minval;
299 if (!col_cover.at(col)) {
300 costs.at(row).at(col) -= minval;
305 return vpMunkres::STEP_T(4);
314 template <
typename Type>
315 inline std::vector<std::pair<unsigned int, unsigned int> > vpMunkres::run(std::vector<std::vector<Type> > costs)
317 const auto original_row_size =
static_cast<Type
>(costs.size());
318 const auto original_col_size =
static_cast<Type
>(costs.front().size());
319 const auto sq_size = std::max<Type>(original_row_size, original_col_size);
321 auto mask = std::vector<std::vector<vpMunkres::ZERO_T> >(sq_size, std::vector<vpMunkres::ZERO_T>(sq_size, vpMunkres::ZERO_T::NA));
322 auto row_cover = std::vector<bool>(sq_size,
false);
323 auto col_cover = std::vector<bool>(sq_size,
false);
325 std::optional<std::pair<unsigned int, unsigned int> > path_0 { std::nullopt };
327 auto step { vpMunkres::STEP_T::ENTRY };
328 while (step != vpMunkres::STEP_T::DONE) {
330 case vpMunkres::STEP_T::ENTRY:
331 padCostMatrix(costs);
332 step = vpMunkres::STEP_T(1);
335 step = stepOne(costs);
338 step = stepTwo(costs, mask, row_cover, col_cover);
341 step = stepThree(mask, col_cover);
344 std::tie(step, path_0) = stepFour(costs, mask, row_cover, col_cover);
347 step = stepFive(mask, *path_0, row_cover, col_cover);
350 step = stepSix(costs, row_cover, col_cover);
352 case vpMunkres::STEP_T::DONE:
359 std::vector<std::pair<unsigned int, unsigned int> > ret {};
360 for (
auto i = 0u; i < original_row_size; i++) {
361 if (
const auto it = std::find(begin(mask.at(i)), end(mask.at(i)), vpMunkres::ZERO_T::STARRED);
362 it != end(mask.at(i))) {
363 if (
const unsigned int j =
static_cast<unsigned int>(std::distance(begin(mask.at(i)), it));
364 j < original_col_size) {
365 ret.emplace_back(i, j);
static bool equal(double x, double y, double threshold=0.001)