--- /dev/null
+#include <iostream>
+#include <algorithm>
+#include "hungarian.h"
+
+using namespace std;
+using namespace Eigen;
+
+class Hungary;
+
+int step_one(Hungary& state);
+int step_two(Hungary& state);
+int step_three(Hungary& state);
+int step_four(Hungary& state);
+int step_five(Hungary& state);
+int step_six(Hungary& state);
+
+class Hungary
+{
+public:
+ Hungary(const MatrixXi& cost){
+ this->cost = cost;
+ this->row_uncovered = VectorXi::Ones(cost.rows());
+ this->col_uncovered = VectorXi::Ones(cost.cols());
+ Z0_r = 0;
+ Z0_c = 0;
+ this->mark = MatrixXi::Zero(cost.rows(), cost.cols());
+ };
+ ~Hungary(){};
+
+ void print(){
+ cout <<"Cost:\n" << cost << "\nMark:\n" << mark << endl;
+ cout <<"Row uncovered :[" << row_uncovered.transpose() << "], col uncovered: [" << col_uncovered.transpose() << "]" << endl;
+ }
+
+public:
+ void clearCovers(){
+ this->row_uncovered.setOnes();
+ this->col_uncovered.setOnes();
+ }
+
+public :
+ // The cost matrix, cols() should > rows(). If not, transpose it first.
+ MatrixXi cost;
+ // The mark matrix. the value of the element is 0 (None), 1 (means the starred), 2 (primed)
+ MatrixXi mark;
+ // the covered state of the rows (0: covered; 1: nocovered)
+ VectorXi row_uncovered;
+ // the covered state of the column (0: covered; 1: nocovered)
+ VectorXi col_uncovered;
+ // the position of Z0, used in step 5
+ int Z0_r;
+ int Z0_c;
+ int min_value;
+};
+
+int linear_sum_assignment(const MatrixXi& cost_matrix, VectorXi& row_ind, VectorXi& col_ind)
+{
+ // The algorithm expects more columns than rows in the cost matrix.
+ MatrixXi correct_matrix = cost_matrix;
+ bool is_transposed = false;
+ if (cost_matrix.cols() < cost_matrix.rows()){
+ cout << "cols < rows, transpose." << endl;
+ correct_matrix = cost_matrix.transpose();
+ is_transposed = true;
+ }
+ Hungary state(correct_matrix);
+ cout << "Cost Matrix: \n" << correct_matrix << endl;;
+
+ bool done = false;
+ int next = 1;
+ int pre_step = 1;
+ while (!done){
+ pre_step = next;
+ switch(next)
+ {
+ case 1:
+ next = step_one(state);
+ break;
+ case 2:
+ next = step_two(state);
+ break;
+ case 3:
+ next = step_three(state);
+ break;
+ case 4:
+ next = step_four(state);
+ break;
+ case 5:
+ next = step_five(state);
+ break;
+ case 6:
+ next = step_six(state);
+ break;
+ case 7:
+ done = true;
+ break;
+ }
+ cout << "After step: " << pre_step << endl;
+ state.print();
+ }
+ MatrixXi mark = state.mark;
+ if (is_transposed){
+ mark = state.mark.transpose();
+ }
+ cout << "Done" << endl << mark << endl;
+ int sum = (mark.array() * cost_matrix.array()).sum();
+ cout << "Sum :" << sum << endl;
+
+
+ // return the array of the positions
+ row_ind = VectorXi::Zero(mark.cols());
+ col_ind = VectorXi::Zero(mark.cols());
+ VectorXi::Index max_index;
+ int point = 0;
+ for (int i = 0; i < mark.rows(); i++){
+ if (mark.row(i).maxCoeff(&max_index)){
+ row_ind(point) = i;
+ col_ind(point) = max_index;
+ point++;
+ }
+ }
+ return sum;
+}
+
+
+// Step 1:
+// For each row of the matrix, find the smallest element and subtract
+// it from every element in its row. Go to Step 2.
+int step_one(Hungary& state)
+{
+ state.cost.colwise() -= state.cost.rowwise().minCoeff();
+ return 2;
+}
+
+// Step 2:
+// Find a zero (Z) in the resulting matrix. If there is no starred zero in its row or column,
+// star Z. Repeat for each elements in the matrix. Go to Step 3.
+int step_two(Hungary& state)
+{
+ for (int i = 0; i < state.cost.rows(); i++){
+ for (int j = 0; j < state.cost.cols(); j++)
+ if (state.row_uncovered(i) == 1 && state.col_uncovered(j) == 1 && state.cost(i, j) == 0){
+ state.mark(i, j) = 1;
+ state.col_uncovered(j) = 0;
+ state.row_uncovered(i) = 0;
+ }
+ }
+ state.clearCovers();
+ return 3;
+}
+
+// Step 3:
+// Cover each column containing a starred zero. If K columns are covered,
+// the starred zeros describe a complete set of unique assignments. In this case,
+// Go to DONE, otherwise, Go to Step 4.
+int step_three(Hungary& state)
+{
+ MatrixXi m1 = (state.mark.array() == 1).select(state.mark, 0);
+ state.col_uncovered = m1.colwise().any();
+ //for (int i = 0; i < state.col_uncovered.size(); i++)
+ // if (m1.col(i).any())
+ // state.col_uncovered(i) = 0;
+ //state.col_uncovered(i) = (state.col_uncovered(i) == 1) ? 0 : 1;
+ state.col_uncovered = (state.col_uncovered.array() == 1).select(0, VectorXi::Ones(state.col_uncovered.size()));
+ int next = 4;
+ if (m1.sum() == m1.rows())
+ next = 7;
+ return next;
+}
+
+// Step 4 :
+// Find a noncovered zero and prime it. If there is no starred zero in the row
+// constaining this primed zero, Go to Step 5. Otherwise, cover this row and uncover the column
+// containing the starred zero. Continue in this manner until there are no uncovered zeros left.
+// Save the smallest uncovered value and Go to Step 6.
+//
+//
+// find_uncovered_zero
+// - find zero or minimal value in the uncovered elements.
+// Return 0 if zero found, or the minimal element.
+int find_uncovered_zero(Hungary& state, int& row, int& col)
+{
+ MatrixXi cover = state.row_uncovered * state.col_uncovered.transpose();
+ int min_value = 0;
+ for (int i = 0; i < cover.rows(); i++)
+ for (int j = 0; j < cover.cols(); j++){
+ if (cover(i, j) != 0){
+ if (state.cost(i, j) == 0){
+ row = i;
+ col = j;
+ return 0;
+ } else {
+ min_value = (min_value > 0) ? min(min_value, state.cost(i, j)) : state.cost(i, j);
+ }
+ }
+ }
+ return min_value;
+}
+
+int step_four(Hungary& state)
+{
+ while(true)
+ {
+ int rr = 0;
+ int cc = 0;
+ // find a nocovered zero
+ int min = find_uncovered_zero(state, rr, cc);
+ if(min == 0){
+ // prime it
+ state.mark(rr, cc) = 2;
+ // If no starred zero in its row
+ if ((state.mark.row(rr).array() == 1).any() == 0){
+ state.Z0_r = rr;
+ state.Z0_c = cc;
+ return 5;
+ } else {
+ // Otherwise, cover this row
+ state.row_uncovered(rr) = 0;
+ // uncover the column
+ for (int j = 0; j < state.mark.cols(); j++){
+ if (state.mark(rr, j) == 1){
+ state.col_uncovered(j) = 1;
+ }
+ }
+ }
+ } else {
+ state.min_value = min;
+ return 6;
+ }
+ }
+}
+
+// Step 5:
+// Construct a seriee of alternating primed and starred zeros as follows,
+// Let Z0 represent the uncovered primed zero found in Step 4.
+// Let Z1 denote the starred zero in the column of Z0 (if any).
+// Let Z2 denote the primed zero in the row of Z1 (there will always be one).
+// Continue until the series terminates at a primed zero that has no starred
+// zero in its column. Unstar each starred zero of the series, star each primed
+// zero of the series, erase all primes and uncover every line in the matrix. Return to Step 3
+int step_five(Hungary& state)
+{
+ MatrixXi path = MatrixXi::Zero(state.cost.rows() + state.cost.cols(), 2);
+ int count = 0;
+ path(count, 0) = state.Z0_r;
+ path(count, 1) = state.Z0_c;
+
+ MatrixXi m1 = (state.mark.array() == 1).select(state.mark, 0);
+ MatrixXi m2 = (state.mark.array() == 2).select(state.mark, 0);
+ MatrixXi::Index index;
+ while(true){
+ // Z1, find the starred zero in the column of Z0
+ int max = m1.col(path(count, 1)).maxCoeff(&index);
+ if (max != 1)
+ break;
+ else {
+ count++;
+ path(count, 0) = index;
+ path(count, 1) = path(count - 1, 1);
+ }
+
+ // Z2, find the primed zero in the row of Z1;
+ max = m2.row(path(count, 0)).maxCoeff(&index);
+ if (max != 2){
+ cout << "Error, should never reach here" << endl;
+ } else {
+ count++;
+ path(count, 0) = path(count - 1, 0);
+ path(count, 1) = index;
+ }
+ }
+
+ // terminates
+ for (int i = 0; i < count + 1; i++){
+ if (state.mark(path(i, 0), path(i, 1)) == 1)
+ state.mark(path(i, 0), path(i, 1)) = 0;
+ else
+ state.mark(path(i, 0), path(i, 1)) = 1;
+ }
+
+ state.clearCovers();
+
+ // erase all prime markings
+ state.mark = (state.mark.array() == 2).select(0, state.mark);
+ return 3;
+}
+
+// Step 6:
+// Add the value found in Step 4 to every element of each covered row, and substract it
+// from every element of each uncovered column.
+// Return to Step 4 without altering any stars, primes, or covered lines.
+//
+int step_six(Hungary& state)
+{
+ for (int i = 0; i < state.mark.rows(); i++)
+ for (int j = 0; j < state.mark.cols(); j++){
+ if (state.row_uncovered(i) == 0)
+ state.cost(i, j) += state.min_value;
+ if (state.col_uncovered(j) == 1)
+ state.cost(i, j) -= state.min_value;
+ }
+ return 4;
+}
+
+/*
+int main()
+{
+ Matrix3i C;
+// MatrixXi C2(4, 3);
+//
+ C << 1, 2, 3,
+ 2, 4, 2,
+ 3, 6, 9;
+
+ Matrix3i M;
+ M << 0, 1, 2,
+ 0, 0, 0,
+ 1, 0, 0;
+
+// C2 << 4, 1, 3,
+// 2, 4, 2,
+// 3, 6, 9,
+// 2, 6, 3;
+
+ Vector3i vv;
+ //Matrix3i m1 = (M.array() == 1).select(0, MatrixXi::Ones(M.cols(), M.rows()));
+ //cout << m1.colwise().sum().transpose() << endl;
+ //vv = vv.rowwise()
+
+ VectorXi row_ind, col_ind;
+
+ //MatrixXi RR = MatrixXi::Random(10, 10);
+ linear_sum_assignment(C, row_ind, col_ind);
+ cout << "row: [" << row_ind.transpose() << "], col: [" << col_ind.transpose() << "]" << endl;
+}
+*/
projects / trackerpp.git / commitdiff