X-Git-Url: http://47.100.26.94:8080/?a=blobdiff_plain;f=src%2Fhungarian.cpp;fp=src%2Fhungarian.cpp;h=9b46613d0738a9ce871dddd20f6400bfdf7daf09;hb=3811d034ba9e1f44a2e9915289438db3807217fe;hp=0000000000000000000000000000000000000000;hpb=135538c82b396ddcc3d87564b6be10083d8876f3;p=trackerpp.git

diff --git a/src/hungarian.cpp b/src/hungarian.cpp
new file mode 100644
index 0000000..9b46613
--- /dev/null
+++ b/src/hungarian.cpp
@@ -0,0 +1,336 @@
+#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;
+}
+*/