Branch data Line data Source code
1 : : // Copyright 2018 the Autoware Foundation
2 : : //
3 : : // Licensed under the Apache License, Version 2.0 (the "License");
4 : : // you may not use this file except in compliance with the License.
5 : : // You may obtain a copy of the License at
6 : : //
7 : : // http://www.apache.org/licenses/LICENSE-2.0
8 : : //
9 : : // Unless required by applicable law or agreed to in writing, software
10 : : // distributed under the License is distributed on an "AS IS" BASIS,
11 : : // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 : : // See the License for the specific language governing permissions and
13 : : // limitations under the License.
14 : : //
15 : : // Co-developed by Tier IV, Inc. and Apex.AI, Inc.
16 : :
17 : : /// \copyright Copyright 2018 the Autoware Foundation
18 : : /// \file
19 : : /// \brief Header for hungarian algorithm for optimal linear assignment
20 : : #ifndef HUNGARIAN_ASSIGNER__HUNGARIAN_ASSIGNER_HPP_
21 : : #define HUNGARIAN_ASSIGNER__HUNGARIAN_ASSIGNER_HPP_
22 : :
23 : :
24 : : /// \brief Ensure Eigen does not allocate memory dynamically
25 : : #define EIGEN_NO_MALLOC
26 : : /// \brief Prevent Eigen from yelling at you for larger fix-sized matrices
27 : : #define EIGEN_STACK_ALLOCATION_LIMIT 0
28 : :
29 : : #include <Eigen/Core>
30 : : #include <hungarian_assigner/visibility_control.hpp>
31 : : #include <utility>
32 : : #include <limits>
33 : : #include <array>
34 : : #include "common/types.hpp"
35 : :
36 : : using autoware::common::types::bool8_t;
37 : : using autoware::common::types::float32_t;
38 : :
39 : : namespace autoware
40 : : {
41 : : namespace fusion
42 : : {
43 : : /// \brief this namespace is for all functions, structs, classes and constants in the
44 : : /// hungarian_assigner package
45 : : namespace hungarian_assigner
46 : : {
47 : :
48 : : /// \brief indexing matches what matrices use
49 : : using index_t = Eigen::Index;
50 : :
51 : : /// \brief implementation of the hungarian/kuhn-munkres/jacobi algorithm for
52 : : /// minimum weight assignment problem in O(N^3 ) time
53 : : /// \tparam Capacity maximum number of things that can be matched, sets matrix size
54 : : template<uint16_t Capacity>
55 : : class HUNGARIAN_ASSIGNER_PUBLIC hungarian_assigner_c
56 : : {
57 : : static_assert(Capacity > 0, "Capacity must be positive");
58 : : /// \brief markers for each matrix slot, not strongly typed so it can decay to numeric type
59 : : enum mark_e : int8_t
60 : : {
61 : : NO_LINK = 0, ///< no cost has been assigned, so assignment is assumed to be impossible
62 : : UNMARKED = 1, ///< this location is just a normal weight
63 : : ZERO = 2, ///< zero, but unmarked
64 : : PRIMED = 3, ///< primed zero
65 : : STARRED = 4 ///< starred zero, corresponds to an assignment
66 : : };
67 : :
68 : : /// \brief this definition is for internal book-keeping
69 : : using index2_t = std::pair<index_t, index_t>;
70 : :
71 : : /// \brief This exception is for a bad edge case when the assigner tries to add a new zero
72 : : /// but the only uncovered entries in the matrix are not valid links/weights
73 : : class HUNGARIAN_ASSIGNER_LOCAL no_uncovered_values_c : public std::runtime_error
74 : : {
75 : : public:
76 : : no_uncovered_values_c();
77 : : }; // class no_uncovered_values_c
78 : :
79 : : public:
80 [ # # ]: 0 : EIGEN_MAKE_ALIGNED_OPERATOR_NEW
81 : : /// \brief This index denotes a worker for which no job assignment was possible
82 : : static constexpr index_t UNASSIGNED = std::numeric_limits<index_t>::max();
83 : : static constexpr float MAX_WEIGHT = 10000.F;
84 : :
85 : : /// \brief constructor
86 : : hungarian_assigner_c();
87 : :
88 : : /// \brief constructor, equivalent of construct(); set_size(num_rows, num_cols)
89 : : /// \param[in] num_rows number of rows/jobs
90 : : /// \param[in] num_cols number of columns/workers
91 : : hungarian_assigner_c(const index_t num_rows, const index_t num_cols);
92 : :
93 : : /// \brief set the size of the matrix. Must be less than capacity. This should be done before
94 : : /// set_weight() calls
95 : : /// \param[in] num_rows number of rows/jobs
96 : : /// \param[in] num_cols number of columns/workers
97 : : /// \throw std::length_error if num_rows or num_cols is bigger than capacity
98 : : /// \throw std::domain_error if matrix shape is skinny
99 : : void set_size(const index_t num_rows, const index_t num_cols);
100 : :
101 : : /// \brief set weight, update book-keeping for min in row
102 : : /// This function is meant to be called asynchronously over jdx, so
103 : : /// a thread should have fixed ownership over a given idx. Note that
104 : : /// you can call this function again for a same index pair, but you
105 : : /// will break assumptions if you set it to a higher weight.
106 : : /// \param[in] weight the weight for assignment of job idx to worker jdx
107 : : /// \param[in] idx the index of the job
108 : : /// \param[in] jdx the index of the worker
109 : : /// \throw std::out_of_range if idx or jdx are outside of range specified by set_size()
110 : : void set_weight(const float32_t weight, const index_t idx, const index_t jdx);
111 : :
112 : : /// \brief reset book-keeping and weight matrix, must be called after
113 : : /// assign(), and before set_weight()
114 : : void reset();
115 : :
116 : : /// \brief reset and set_size, equivalent to reset(); set_size(num_rows, num_cols);
117 : : /// \param[in] num_rows number of rows/jobs
118 : : /// \param[in] num_cols number of columns/workers
119 : : void reset(const index_t num_rows, const index_t num_cols);
120 : :
121 : : /// \brief compute minimum cost assignment
122 : : /// \return whether or not it was successful. If assignment was unsuccessful (for example, the
123 : : /// case when a row has no valid assignments), then get_assignment() may return
124 : : /// UNASSIGNED
125 : : /// \throw std::exception if some unspecified error happens internally, should never happen
126 : : bool8_t assign();
127 : :
128 : : // every row/job is guaranteed to have a worker
129 : : /// \brief dictate what the assignment for a given row/task is, should be called after assign().
130 : : /// If assign() returned true, then every job/row is guaranteed to have a worker/column.
131 : : /// If assign() returned false, then a row may not have a worker/column
132 : : /// \param[in] idx the index for the task, starting at 0
133 : : /// \return the index for the assigned job, starting at 0
134 : : /// UNASSIGNED if assign() returned false and the idx job has no possible assignments
135 : : /// \throw std::range_error if idx is out of bounds
136 : : index_t get_assignment(const index_t idx) const;
137 : :
138 : : // get unassigned workers/columns. User should know how many unassigned there will be apriori
139 : : /// \brief says what jobs have been unassigned
140 : : /// \param[in] idx the i'th unassigned job, starts from 0 to num_jobs - num_workers
141 : : /// \return the index of the i'th unassigned job
142 : : /// \throw std::range_error if idx is out of bounds (i.e. there are no unassigned rows)
143 : : index_t get_unassigned(const index_t idx) const;
144 : :
145 : : private:
146 : : // TODO(gowtham.ranganathan): Workaround. Should calculate minimum while the weight is being
147 : : // set. Fix after #979.
148 : : /// \brief find the minimum value and its index in each row of the weight matrix
149 : : HUNGARIAN_ASSIGNER_LOCAL void find_minimums();
150 : : /// \brief do steps 1-3, return true if perfect assignment found
151 : : HUNGARIAN_ASSIGNER_LOCAL bool8_t reduce_rows_and_init_zeros_and_check_result();
152 : :
153 : : /// \brief do step 5 and 3, return true if perfect assignment found
154 : : HUNGARIAN_ASSIGNER_LOCAL bool8_t increment_starred_zeroes_and_check_result(index2_t loc);
155 : :
156 : : /// \brief step 4 and 6: find an uncovered zero, add it if necessary, throws exception if it can't
157 : : HUNGARIAN_ASSIGNER_LOCAL index2_t prime_uncovered_zero();
158 : :
159 : : /// \brief step 6: reduce matrix by smallest uncovered value, return false if no uncovered value
160 : : HUNGARIAN_ASSIGNER_LOCAL bool8_t add_new_zero(index2_t & loc);
161 : :
162 : : /// \brief check if assignment is complete
163 : : HUNGARIAN_ASSIGNER_LOCAL bool8_t are_all_columns_covered() const;
164 : :
165 : : /// \brief find a zero that is uncovered by rows or columns
166 : : HUNGARIAN_ASSIGNER_LOCAL bool8_t find_uncovered_zero(index2_t & loc) const;
167 : :
168 : : /// \brief find minimal value that is not covered, false if none found
169 : : HUNGARIAN_ASSIGNER_LOCAL bool8_t find_minimum_uncovered_value(
170 : : index2_t & loc,
171 : : float32_t & min_val) const;
172 : :
173 : : /// \brief update internal bookkeeping for which rows and columns are uncovered
174 : : HUNGARIAN_ASSIGNER_LOCAL void update_uncovered_rows_and_cols();
175 : :
176 : : // TODO(gowtham.ranganathan): Workaround. There should be no need for m_is_max_matrix after #979
177 : : // Matrix to track if a weight is unset (set to max means unset)
178 : : Eigen::Matrix<bool8_t, Capacity, Capacity> m_is_max_matrix;
179 : : Eigen::Matrix<float32_t, Capacity, Capacity> m_weight_matrix;
180 : : Eigen::Matrix<int8_t, Capacity, Capacity> m_mark_matrix;
181 : : Eigen::Matrix<index_t, Capacity, 1> m_row_min_idx;
182 : : index_t m_num_rows;
183 : : index_t m_num_cols;
184 : : Eigen::Matrix<float32_t, Capacity, 1> m_row_min_weights;
185 : : Eigen::Matrix<index_t, Capacity, 1> m_assignments;
186 : : Eigen::Matrix<bool8_t, Capacity, 1> m_is_col_covered;
187 : : Eigen::Matrix<bool8_t, Capacity, 1> m_is_row_covered;
188 : : index_t m_num_uncovered_rows;
189 : : index_t m_num_uncovered_cols;
190 : : Eigen::Matrix<index_t, Capacity, 1> m_uncovered_rows;
191 : : Eigen::Matrix<index_t, Capacity, 1> m_uncovered_cols;
192 : : Eigen::Matrix<index2_t, 2 * Capacity, 1> m_augment_path;
193 : : Eigen::Matrix<index2_t, Capacity, 1> m_primed_zero_locs;
194 : : index_t m_num_primed_zeros;
195 : : }; // class hungarian_assigner_c
196 : :
197 : : } // namespace hungarian_assigner
198 : : } // namespace fusion
199 : : } // namespace autoware
200 : : #endif // HUNGARIAN_ASSIGNER__HUNGARIAN_ASSIGNER_HPP_
|
