10 int step_one(Hungary& state);
11 int step_two(Hungary& state);
12 int step_three(Hungary& state);
13 int step_four(Hungary& state);
14 int step_five(Hungary& state);
15 int step_six(Hungary& state);
20 Hungary(const MatrixXi& cost){
22 this->row_uncovered = VectorXi::Ones(cost.rows());
23 this->col_uncovered = VectorXi::Ones(cost.cols());
26 this->mark = MatrixXi::Zero(cost.rows(), cost.cols());
31 cout <<"Cost:\n" << cost << "\nMark:\n" << mark << endl;
32 cout <<"Row uncovered :[" << row_uncovered.transpose() << "], col uncovered: [" << col_uncovered.transpose() << "]" << endl;
37 this->row_uncovered.setOnes();
38 this->col_uncovered.setOnes();
42 // The cost matrix, cols() should > rows(). If not, transpose it first.
44 // The mark matrix. the value of the element is 0 (None), 1 (means the starred), 2 (primed)
46 // the covered state of the rows (0: covered; 1: nocovered)
47 VectorXi row_uncovered;
48 // the covered state of the column (0: covered; 1: nocovered)
49 VectorXi col_uncovered;
50 // the position of Z0, used in step 5
56 int linear_sum_assignment(const MatrixXi& cost_matrix, VectorXi& row_ind, VectorXi& col_ind)
58 // The algorithm expects more columns than rows in the cost matrix.
59 MatrixXi correct_matrix = cost_matrix;
60 bool is_transposed = false;
61 if (cost_matrix.cols() < cost_matrix.rows()){
62 cout << "cols < rows, transpose." << endl;
63 correct_matrix = cost_matrix.transpose();
66 Hungary state(correct_matrix);
67 cout << "Cost Matrix: \n" << correct_matrix << endl;;
77 next = step_one(state);
80 next = step_two(state);
83 next = step_three(state);
86 next = step_four(state);
89 next = step_five(state);
92 next = step_six(state);
98 cout << "After step: " << pre_step << endl;
101 MatrixXi mark = state.mark;
103 mark = state.mark.transpose();
105 cout << "Done" << endl << mark << endl;
106 int sum = (mark.array() * cost_matrix.array()).sum();
107 cout << "Sum :" << sum << endl;
110 // return the array of the positions
111 row_ind = VectorXi::Zero(mark.cols());
112 col_ind = VectorXi::Zero(mark.cols());
113 VectorXi::Index max_index;
115 for (int i = 0; i < mark.rows(); i++){
116 if (mark.row(i).maxCoeff(&max_index)){
118 col_ind(point) = max_index;
127 // For each row of the matrix, find the smallest element and subtract
128 // it from every element in its row. Go to Step 2.
129 int step_one(Hungary& state)
131 state.cost.colwise() -= state.cost.rowwise().minCoeff();
136 // Find a zero (Z) in the resulting matrix. If there is no starred zero in its row or column,
137 // star Z. Repeat for each elements in the matrix. Go to Step 3.
138 int step_two(Hungary& state)
140 for (int i = 0; i < state.cost.rows(); i++){
141 for (int j = 0; j < state.cost.cols(); j++)
142 if (state.row_uncovered(i) == 1 && state.col_uncovered(j) == 1 && state.cost(i, j) == 0){
143 state.mark(i, j) = 1;
144 state.col_uncovered(j) = 0;
145 state.row_uncovered(i) = 0;
153 // Cover each column containing a starred zero. If K columns are covered,
154 // the starred zeros describe a complete set of unique assignments. In this case,
155 // Go to DONE, otherwise, Go to Step 4.
156 int step_three(Hungary& state)
158 MatrixXi m1 = (state.mark.array() == 1).select(state.mark, 0);
159 state.col_uncovered = m1.colwise().any();
160 //for (int i = 0; i < state.col_uncovered.size(); i++)
161 // if (m1.col(i).any())
162 // state.col_uncovered(i) = 0;
163 //state.col_uncovered(i) = (state.col_uncovered(i) == 1) ? 0 : 1;
164 state.col_uncovered = (state.col_uncovered.array() == 1).select(0, VectorXi::Ones(state.col_uncovered.size()));
166 if (m1.sum() == m1.rows())
172 // Find a noncovered zero and prime it. If there is no starred zero in the row
173 // constaining this primed zero, Go to Step 5. Otherwise, cover this row and uncover the column
174 // containing the starred zero. Continue in this manner until there are no uncovered zeros left.
175 // Save the smallest uncovered value and Go to Step 6.
178 // find_uncovered_zero
179 // - find zero or minimal value in the uncovered elements.
180 // Return 0 if zero found, or the minimal element.
181 int find_uncovered_zero(Hungary& state, int& row, int& col)
183 MatrixXi cover = state.row_uncovered * state.col_uncovered.transpose();
185 for (int i = 0; i < cover.rows(); i++)
186 for (int j = 0; j < cover.cols(); j++){
187 if (cover(i, j) != 0){
188 if (state.cost(i, j) == 0){
193 min_value = (min_value > 0) ? min(min_value, state.cost(i, j)) : state.cost(i, j);
200 int step_four(Hungary& state)
206 // find a nocovered zero
207 int min = find_uncovered_zero(state, rr, cc);
210 state.mark(rr, cc) = 2;
211 // If no starred zero in its row
212 if ((state.mark.row(rr).array() == 1).any() == 0){
217 // Otherwise, cover this row
218 state.row_uncovered(rr) = 0;
219 // uncover the column
220 for (int j = 0; j < state.mark.cols(); j++){
221 if (state.mark(rr, j) == 1){
222 state.col_uncovered(j) = 1;
227 state.min_value = min;
234 // Construct a seriee of alternating primed and starred zeros as follows,
235 // Let Z0 represent the uncovered primed zero found in Step 4.
236 // Let Z1 denote the starred zero in the column of Z0 (if any).
237 // Let Z2 denote the primed zero in the row of Z1 (there will always be one).
238 // Continue until the series terminates at a primed zero that has no starred
239 // zero in its column. Unstar each starred zero of the series, star each primed
240 // zero of the series, erase all primes and uncover every line in the matrix. Return to Step 3
241 int step_five(Hungary& state)
243 MatrixXi path = MatrixXi::Zero(state.cost.rows() + state.cost.cols(), 2);
245 path(count, 0) = state.Z0_r;
246 path(count, 1) = state.Z0_c;
248 MatrixXi m1 = (state.mark.array() == 1).select(state.mark, 0);
249 MatrixXi m2 = (state.mark.array() == 2).select(state.mark, 0);
250 MatrixXi::Index index;
252 // Z1, find the starred zero in the column of Z0
253 int max = m1.col(path(count, 1)).maxCoeff(&index);
258 path(count, 0) = index;
259 path(count, 1) = path(count - 1, 1);
262 // Z2, find the primed zero in the row of Z1;
263 max = m2.row(path(count, 0)).maxCoeff(&index);
265 cout << "Error, should never reach here" << endl;
268 path(count, 0) = path(count - 1, 0);
269 path(count, 1) = index;
274 for (int i = 0; i < count + 1; i++){
275 if (state.mark(path(i, 0), path(i, 1)) == 1)
276 state.mark(path(i, 0), path(i, 1)) = 0;
278 state.mark(path(i, 0), path(i, 1)) = 1;
283 // erase all prime markings
284 state.mark = (state.mark.array() == 2).select(0, state.mark);
289 // Add the value found in Step 4 to every element of each covered row, and substract it
290 // from every element of each uncovered column.
291 // Return to Step 4 without altering any stars, primes, or covered lines.
293 int step_six(Hungary& state)
295 for (int i = 0; i < state.mark.rows(); i++)
296 for (int j = 0; j < state.mark.cols(); j++){
297 if (state.row_uncovered(i) == 0)
298 state.cost(i, j) += state.min_value;
299 if (state.col_uncovered(j) == 1)
300 state.cost(i, j) -= state.min_value;
305 ////////////////////////////////////////////////////////////////////////////////
306 double distance_cosine(const VectorXd& u, const VectorXd& v)
308 return (1 - u.dot(v) / std::sqrt(u.dot(u) * v.dot(v)));
311 double distance_euclidean(const VectorXd& u, const VectorXd& v)
314 return std::sqrt(d.dot(d));