int
Munkres::step1(void) {
const unsigned int rows = matrix.rows,
columns = matrix.cols;
for ( unsigned int row = 0 ; row < rows ; row++ ) {
for ( unsigned int col = 0 ; col < columns ; col++ ) {
if ( 0 == matrix(row, col) ) {
bool isstarred = false;
for ( unsigned int nrow = 0 ; nrow < rows ; nrow++ )
if ( STAR == mask_matrix(nrow,col) ) {
isstarred = true;
break;
}
if ( !isstarred ) {
for ( unsigned int ncol = 0 ; ncol < columns ; ncol++ )
if ( STAR == mask_matrix(row, ncol) ) {
isstarred = true;
break;
}
}
if ( !isstarred ) {
mask_matrix(row,col) = STAR;
}
}
}
}
return 2;
}
int
Munkres::step1(void) {
for ( int row = 0 ; row < matrix.rows() ; row++ )
for ( int col = 0 ; col < matrix.columns() ; col++ )
if ( matrix(row,col) == 0 ) {
bool isstarred = false;
for ( int nrow = 0 ; nrow < matrix.rows() ; nrow++ )
if ( mask_matrix(nrow,col) == STAR ) {
isstarred = true;
break;
}
if ( !isstarred ) {
for ( int ncol = 0 ; ncol < matrix.columns() ; ncol++ )
if ( mask_matrix(row,ncol) == STAR ) {
isstarred = true;
break;
}
}
if ( !isstarred ) {
mask_matrix(row,col) = STAR;
}
}
return 2;
}
int
Munkres::step3(void) {
/*
Main Zero Search
1. Find an uncovered Z in the distance matrix and prime it. If no such zero exists, go to Step 5
2. If No Z* exists in the row of the Z', go to Step 4.
3. If a Z* exists, cover this row and uncover the column of the Z*. Return to Step 3.1 to find a new Z
*/
if ( find_uncovered_in_matrix(0, saverow, savecol) ) {
mask_matrix(saverow,savecol) = PRIME; // prime it.
} else {
return 5;
}
for ( unsigned int ncol = 0 ; ncol < matrix.cols ; ncol++ ) {
if ( mask_matrix(saverow,ncol) == STAR ) {
row_mask[saverow] = true; //cover this row and
col_mask[ncol] = false; // uncover the column containing the starred zero
return 3; // repeat
}
}
return 4; // no starred zero in the row containing this primed zero
}
int
Munkres::step2(void) {
const unsigned int rows = matrix.rows,
columns = matrix.cols;
unsigned int covercount = 0;
for ( unsigned int row = 0 ; row < rows ; row++ )
for ( unsigned int col = 0 ; col < columns ; col++ )
if ( STAR == mask_matrix(row, col) ) {
col_mask[col] = true;
covercount++;
}
if ( covercount >= minsize(matrix) ) {
if(isDiag) {
std::cout << "Final cover count: " << covercount << std::endl;
}
return 0;
}
if(isDiag) {
std::cout << "Munkres matrix has " << covercount << " of " << minsize(matrix) << " Columns covered:" << std::endl;
for ( unsigned int row = 0 ; row < rows ; row++ ) {
for ( unsigned int col = 0 ; col < columns ; col++ ) {
std::cout.width(4);
std::cout << matrix(row, col) << ",";
}
std::cout << std::endl;
}
std::cout << std::endl;
}
return 3;
}
int
Munkres::step2(void) {
int covercount = 0;
for ( int row = 0 ; row < matrix.rows() ; row++ )
for ( int col = 0 ; col < matrix.columns() ; col++ )
if ( mask_matrix(row,col) == Z_STAR ) {
col_mask[col] = true;
covercount++;
}
int k = matrix.minsize();
if ( covercount >= k ) {
#ifdef DEBUG
std::cout << "Final cover count: " << covercount << std::endl;
#endif
return 0;
}
#ifdef DEBUG
std::cout << "Munkres matrix has " << covercount << " of " << k << " Columns covered:" << std::endl;
for ( int row = 0 ; row < matrix.rows() ; row++ ) {
for ( int col = 0 ; col < matrix.columns() ; col++ ) {
std::cout.width(8);
std::cout << matrix(row,col) << ",";
}
std::cout << std::endl;
}
std::cout << std::endl;
#endif
return 3;
}
int
Munkres::step2() {
const size_t rows = matrix.rows(),
columns = matrix.columns();
size_t covercount = 0;
for ( size_t row = 0 ; row < rows ; row++ )
for ( size_t col = 0 ; col < columns ; col++ )
if ( STAR == mask_matrix(row, col) ) {
col_mask[col] = true;
covercount++;
}
if ( covercount >= matrix.minsize() ) {
#ifdef DEBUG
std::cout << "Final cover count: " << covercount << std::endl;
#endif
return 0;
}
#ifdef DEBUG
std::cout << "Munkres matrix has " << covercount << " of " << matrix.minsize() << " Columns covered:" << std::endl;
for ( size_t row = 0 ; row < rows ; row++ ) {
for ( size_t col = 0 ; col < columns ; col++ ) {
std::cout.width(8);
std::cout << matrix(row, col) << ",";
}
std::cout << std::endl;
}
std::cout << std::endl;
#endif
return 3;
}
/*!
Main Zero Search
1. Find an uncovered Z in the distance matrix and prime it. If no such zero exists, go to Step 5
2. If No Z* exists in the row of the Z', go to Step 4.
3. If a Z* exists, cover this row and uncover the column of the Z*. Return to Step 3.1 to find a new Z
*/
int Munkres::step3(void)
{
if ( find_uncovered_in_matrix(0, saverow, savecol) ) {
mask_matrix(saverow,savecol) = Z_PRIME; // prime it.
} else {
return 5;
}
for ( unsigned ncol = 0 ; ncol < matrix.columns() ; ncol++ )
if ( mask_matrix(saverow,ncol) == Z_STAR ) {
row_mask[saverow] = true; //cover this row and
col_mask[ncol] = false; // uncover the column containing the starred zero
return 3; // repeat
}
return 4; // no starred zero in the row containing this primed zero
}
int
Munkres::step1() {
const size_t rows = matrix.rows(),
columns = matrix.columns();
for ( size_t row = 0 ; row < rows ; row++ ) {
for ( size_t col = 0 ; col < columns ; col++ ) {
if ( 0 == matrix(row, col) ) {
for ( size_t nrow = 0 ; nrow < row ; nrow++ )
if ( STAR == mask_matrix(nrow,col) )
goto next_column;
mask_matrix(row,col) = STAR;
goto next_row;
}
next_column:;
}
next_row:;
}
return 2;
}
int Munkres::step1(void)
{
for ( unsigned row = 0 ; row < matrix.rows() ; row++ )
for ( unsigned col = 0 ; col < matrix.columns() ; col++ )
if ( matrix(row,col) == 0 ) {
bool isstarred = false;
for ( unsigned nrow = 0 ; nrow < matrix.rows() ; nrow++ )
if ( mask_matrix(nrow,col) == Z_STAR )
isstarred = true;
if ( !isstarred ) {
for ( unsigned ncol = 0 ; ncol < matrix.columns() ; ncol++ )
if ( mask_matrix(row,ncol) == Z_STAR )
isstarred = true;
}
if ( !isstarred ) {
mask_matrix(row,col) = Z_STAR;
}
}
return 2;
}
void
Munkres::solve(Matrix<double> &m) {
// Linear assignment problem solution
// [modifies matrix in-place.]
// matrix(row,col): row major format assumed.
// Assignments are remaining 0 values
// (extra 0 values are replaced with -1)
#ifdef DEBUG
std::cout << "Munkres input matrix:" << std::endl;
for ( int row = 0 ; row < m.rows() ; row++ ) {
for ( int col = 0 ; col < m.columns() ; col++ ) {
std::cout.width(8);
std::cout << m(row,col) << ",";
}
std::cout << std::endl;
}
std::cout << std::endl;
#endif
double highValue = 0;
for ( int row = 0 ; row < m.rows() ; row++ ) {
for ( int col = 0 ; col < m.columns() ; col++ ) {
if ( m(row,col) != INFINITY && m(row,col) > highValue )
highValue = m(row,col);
}
}
highValue++;
for ( int row = 0 ; row < m.rows() ; row++ )
for ( int col = 0 ; col < m.columns() ; col++ )
if ( m(row,col) == INFINITY )
m(row,col) = highValue;
bool notdone = true;
int step = 1;
this->matrix = m;
// STAR == 1 == starred, PRIME == 2 == primed
mask_matrix.resize(matrix.rows(), matrix.columns());
row_mask = new bool[matrix.rows()];
col_mask = new bool[matrix.columns()];
for ( int i = 0 ; i < matrix.rows() ; i++ ) {
row_mask[i] = false;
}
for ( int i = 0 ; i < matrix.columns() ; i++ ) {
col_mask[i] = false;
}
while ( notdone ) {
switch ( step ) {
case 0:
notdone = false;
break;
case 1:
step = step1();
break;
case 2:
step = step2();
break;
case 3:
step = step3();
break;
case 4:
step = step4();
break;
case 5:
step = step5();
break;
}
}
// Store results
for ( int row = 0 ; row < matrix.rows() ; row++ )
for ( int col = 0 ; col < matrix.columns() ; col++ )
if ( mask_matrix(row,col) == STAR )
matrix(row,col) = 0;
else
matrix(row,col) = -1;
#ifdef DEBUG
std::cout << "Munkres output matrix:" << std::endl;
for ( int row = 0 ; row < matrix.rows() ; row++ ) {
for ( int col = 0 ; col < matrix.columns() ; col++ ) {
std::cout.width(1);
std::cout << matrix(row,col) << ",";
}
std::cout << std::endl;
}
std::cout << std::endl;
#endif
m = matrix;
delete [] row_mask;
delete [] col_mask;
}
int
Munkres::step4(void) {
int rows = matrix.rows();
int cols = matrix.columns();
std::list<std::pair<int,int> > seq;
// use saverow, savecol from step 3.
std::pair<int,int> z0(saverow, savecol);
std::pair<int,int> z1(-1,-1);
std::pair<int,int> z2n(-1,-1);
seq.insert(seq.end(), z0);
int row, col = savecol;
/*
Increment Set of Starred Zeros
1. Construct the ``alternating sequence'' of primed and starred zeros:
Z0 : Unpaired Z' from Step 4.2
Z1 : The Z* in the column of Z0
Z[2N] : The Z' in the row of Z[2N-1], if such a zero exists
Z[2N+1] : The Z* in the column of Z[2N]
The sequence eventually terminates with an unpaired Z' = Z[2N] for some N.
*/
bool madepair;
do {
madepair = false;
for ( row = 0 ; row < rows ; row++ )
if ( mask_matrix(row,col) == STAR ) {
z1.first = row;
z1.second = col;
if ( pair_in_list(z1, seq) )
continue;
madepair = true;
seq.insert(seq.end(), z1);
break;
}
if ( !madepair )
break;
madepair = false;
for ( col = 0 ; col < cols ; col++ )
if ( mask_matrix(row,col) == PRIME ) {
z2n.first = row;
z2n.second = col;
if ( pair_in_list(z2n, seq) )
continue;
madepair = true;
seq.insert(seq.end(), z2n);
break;
}
} while ( madepair );
for ( std::list<std::pair<int,int> >::iterator i = seq.begin() ;
i != seq.end() ;
i++ ) {
// 2. Unstar each starred zero of the sequence.
if ( mask_matrix(i->first,i->second) == STAR )
mask_matrix(i->first,i->second) = NORMAL;
// 3. Star each primed zero of the sequence,
// thus increasing the number of starred zeros by one.
if ( mask_matrix(i->first,i->second) == PRIME )
mask_matrix(i->first,i->second) = STAR;
}
// 4. Erase all primes, uncover all columns and rows,
for ( int row = 0 ; row < mask_matrix.rows() ; row++ )
for ( int col = 0 ; col < mask_matrix.columns() ; col++ )
if ( mask_matrix(row,col) == PRIME )
mask_matrix(row,col) = NORMAL;
for ( int i = 0 ; i < rows ; i++ ) {
row_mask[i] = false;
}
for ( int i = 0 ; i < cols ; i++ ) {
col_mask[i] = false;
}
// and return to Step 2.
return 2;
}
/*
*
* Linear assignment problem solution
* [modifies matrix in-place.]
* matrix(row,col): row major format assumed.
*
* Assignments are remaining 0 values
* (extra 0 values are replaced with -1)
*
*/
void
Munkres::solve(cv::Mat_<int> &m) {
const unsigned int rows = m.rows,
columns = m.cols,
size = std::max<unsigned int>(rows, columns);
if(isDiag) {
std::cout << "Munkres input matrix:" << std::endl;
for ( unsigned int row = 0 ; row < rows ; row++ ) {
for ( unsigned int col = 0 ; col < columns ; col++ ) {
std::cout.width(4);
std::cout << m(row, col) << ",";
}
std::cout << std::endl;
}
std::cout << std::endl;
}
bool notdone = true;
int step = 1;
// Copy input matrix
this->matrix = m;
if ( rows != columns ) {
// If the input matrix isn't square, make it square
// and fill the empty values with the largest value present
// in the matrix.
extendMat(matrix, size, size, maxValue(matrix));
}
// STAR == 1 == starred, PRIME == 2 == primed
// mask_matrix.resize(size, size);
extendMat(mask_matrix, size, size);
row_mask = new bool[size];
col_mask = new bool[size];
for ( unsigned int i = 0 ; i < size ; i++ ) {
row_mask[i] = false;
}
for ( unsigned int i = 0 ; i < size ; i++ ) {
col_mask[i] = false;
}
// Prepare the matrix values...
// If there were any infinities, replace them with a value greater
// than the maximum value in the matrix.
replace_infinites(matrix);
minimize_along_direction(matrix, false);
minimize_along_direction(matrix, true);
// Follow the steps
while ( notdone ) {
switch ( step ) {
case 0:
notdone = false;
// end the step flow
break;
case 1:
step = step1();
// step is always 2
break;
case 2:
step = step2();
// step is always either 0 or 3
break;
case 3:
step = step3();
// step in [3, 4, 5]
break;
case 4:
step = step4();
// step is always 2
break;
case 5:
step = step5();
// step is always 3
break;
}
}
// Store results
for ( unsigned int row = 0 ; row < size ; row++ ) {
for ( unsigned int col = 0 ; col < size ; col++ ) {
if ( mask_matrix(row, col) == STAR ) {
matrix(row, col) = 0;
} else {
matrix(row, col) = -1;
}
}
}
if(isDiag) {
std::cout << "Munkres output matrix:" << std::endl;
for ( unsigned int row = 0 ; row < rows ; row++ ) {
for ( unsigned int col = 0 ; col < columns ; col++ ) {
std::cout.width(2);
std::cout << matrix(row, col) << ",";
}
std::cout << std::endl;
}
std::cout << std::endl;
}
// Remove the excess rows or columns that we added to fit the
// input to a square matrix.
// matrix.resize(rows, columns);
extendMat(matrix, rows, columns);
m = matrix;
delete [] row_mask;
delete [] col_mask;
}
/*!
Linear assignment problem solution
[modifies matrix in-place.]
matrix(row,col): row major format assumed.
Assignments are remaining 0 values on 'matrix'
(extra 0 values are replaced with -1)
The correspondence results are stored in the variables
row2colMap and col2rowMap s.t.
row2colMap(r) = c means that col c is assigned to row r
col2rowMap(c) = r means that row r is assigned to column c
@param initMaps if true the row/col maps are init to invalid values,
which help check the mapping by calling CheckMapping()
*/
double Munkres::Solve(const DoubleMatrix& m, UIntVector* row2colMap,
UIntVector* col2rowMap)
{
#ifdef MUNKRES_DEBUG
std::cout << "Munkres input matrix:" << std::endl;
for ( int row = 0 ; row < m.rows() ; row++ )
{
for ( int col = 0 ; col < m.columns() ; col++ ) {
std::cout.width(8);
std::cout << m(row,col) << ",";
}
std::cout << std::endl;
}
std::cout << std::endl;
#endif
bool notdone = true;
int step = 1;
// Copy m, because it need to be modified
matrix = m;
// Z_STAR == 1 == starred, Z_PRIME == 2 == primed
mask_matrix.set_size(matrix.rows(), matrix.columns());
mask_matrix.fill(0);
row_mask = new bool[matrix.rows()];
col_mask = new bool[matrix.columns()];
for ( unsigned i = 0 ; i < matrix.rows() ; i++ ) {
row_mask[i] = false;
}
for ( unsigned i = 0 ; i < matrix.columns() ; i++ ) {
col_mask[i] = false;
}
while ( notdone ) {
switch ( step ) {
case 0:
notdone = false;
break;
case 1:
step = step1();
break;
case 2:
step = step2();
break;
case 3:
step = step3();
break;
case 4:
step = step4();
break;
case 5:
step = step5();
break;
}
}
delete[] row_mask;
delete[] col_mask;
// Store results
/*
for ( unsigned row = 0 ; row < matrix.rows() ; row++ )
for ( unsigned col = 0 ; col < matrix.columns() ; col++ )
if ( mask_matrix(row,col) == Z_STAR )
matrix(row,col) = 0;
else
matrix(row,col) = -1;
#ifdef MUNKRES_DEBUG
std::cout << "Munkres output matrix:" << std::endl;
for ( int row = 0 ; row < matrix.rows() ; row++ ) {
for ( int col = 0 ; col < matrix.columns() ; col++ ) {
std::cout.width(1);
std::cout << matrix(row,col) << ",";
}
std::cout << std::endl;
}
std::cout << std::endl;
#endif
*/
// Store results in the row and col maps s.t.
// row2colMap(r) = c means that col c is assigned to row r
// col2rowMap(c) = r means that row r is assigned to column c
// Init vector to "known" invalid values
if (row2colMap)
{
row2colMap->set_size(matrix.rows());
row2colMap->fill(UINT_MAX);
}
// Init vector to "known" invalid values
if (col2rowMap)
{
col2rowMap->set_size(matrix.cols());
col2rowMap->fill(UINT_MAX);
}
double matchCost = 0;
for (unsigned row = 0 ; row < matrix.rows() ; ++row)
{
for (unsigned col = 0 ; col < matrix.columns() ; ++col)
{
if (mask_matrix(row,col) == Z_STAR)
{
if (row2colMap)
(*row2colMap)[row] = col;
if (col2rowMap)
(*col2rowMap)[col] = row;
matchCost += m[row][col]; // use given 'm' not 'matrix'!
}
}
}
return matchCost;
}
/*!
Increment Set of Starred Zeros
1. Construct the ``alternating sequence'' of primed and starred zeros:
Z0 : Unpaired Z' from Step 4.2
Z1 : The Z* in the column of Z0
Z[2N] : The Z' in the row of Z[2N-1], if such a zero exists
Z[2N+1] : The Z* in the column of Z[2N]
The sequence eventually terminates with an unpaired Z' = Z[2N] for some N.
*/
int Munkres::step4(void)
{
std::list<std::pair<int,int> > seq;
// use saverow, savecol from step 3.
std::pair<int,int> z0(saverow, savecol);
std::pair<int,int> z1(-1,-1);
std::pair<int,int> z2n(-1,-1);
seq.insert(seq.end(), z0);
unsigned row, col = savecol;
bool madepair;
do {
madepair = false;
for ( row = 0 ; row < matrix.rows() ; row++ )
if ( mask_matrix(row,col) == Z_STAR ) {
z1.first = row;
z1.second = col;
if ( pair_in_list(z1, seq) )
continue;
madepair = true;
seq.insert(seq.end(), z1);
break;
}
if ( !madepair )
break;
madepair = false;
for ( col = 0 ; col < matrix.columns() ; col++ )
if ( mask_matrix(row,col) == Z_PRIME ) {
z2n.first = row;
z2n.second = col;
if ( pair_in_list(z2n, seq) )
continue;
madepair = true;
seq.insert(seq.end(), z2n);
break;
}
} while ( madepair );
for ( std::list<std::pair<int,int> >::iterator i = seq.begin() ;
i != seq.end() ;
i++ ) {
// 2. Unstar each starred zero of the sequence.
if ( mask_matrix(i->first,i->second) == Z_STAR )
mask_matrix(i->first,i->second) = Z_NORMAL;
// 3. Star each primed zero of the sequence,
// thus increasing the number of starred zeros by one.
if ( mask_matrix(i->first,i->second) == Z_PRIME )
mask_matrix(i->first,i->second) = Z_STAR;
}
// 4. Erase all primes, uncover all columns and rows,
for ( unsigned row = 0 ; row < mask_matrix.rows() ; row++ )
for ( unsigned col = 0 ; col < mask_matrix.columns() ; col++ )
if ( mask_matrix(row,col) == Z_PRIME )
mask_matrix(row,col) = Z_NORMAL;
for ( unsigned i = 0 ; i < matrix.rows() ; i++ ) {
row_mask[i] = false;
}
for ( unsigned i = 0 ; i < matrix.columns() ; i++ ) {
col_mask[i] = false;
}
// and return to Step 2.
return 2;
}
/*
*
* Linear assignment problem solution
* [modifies matrix in-place.]
* matrix(row,col): row major format assumed.
*
* Assignments are remaining 0 values
* (extra 0 values are replaced with -1)
*
*/
void
Munkres::solve(Matrix<double> &m) {
const size_t rows = m.rows(),
columns = m.columns(),
size = std::max(rows, columns);
#ifdef DEBUG
std::cout << "Munkres input matrix:" << std::endl;
for ( size_t row = 0 ; row < rows ; row++ ) {
for ( size_t col = 0 ; col < columns ; col++ ) {
std::cout.width(8);
std::cout << m(row, col) << ",";
}
std::cout << std::endl;
}
std::cout << std::endl;
#endif
// Copy input matrix
this->matrix = m;
if ( rows != columns ) {
// If the input matrix isn't square, make it square
// and fill the empty values with the largest value present
// in the matrix.
matrix.resize(size, size, matrix.max());
}
// STAR == 1 == starred, PRIME == 2 == primed
mask_matrix.resize(size, size);
row_mask = new bool[size];
col_mask = new bool[size];
for ( size_t i = 0 ; i < size ; i++ ) {
row_mask[i] = false;
}
for ( size_t i = 0 ; i < size ; i++ ) {
col_mask[i] = false;
}
// Prepare the matrix values...
// If there were any infinities, replace them with a value greater
// than the maximum value in the matrix.
replace_infinites(matrix);
minimize_along_direction(matrix, false);
minimize_along_direction(matrix, true);
// Follow the steps
int step = 1;
while ( step ) {
switch ( step ) {
case 1:
step = step1();
// step is always 2
break;
case 2:
step = step2();
// step is always either 0 or 3
break;
case 3:
step = step3();
// step in [3, 4, 5]
break;
case 4:
step = step4();
// step is always 2
break;
case 5:
step = step5();
// step is always 3
break;
}
}
// Store results
for ( size_t row = 0 ; row < size ; row++ ) {
for ( size_t col = 0 ; col < size ; col++ ) {
if ( mask_matrix(row, col) == STAR ) {
matrix(row, col) = 0;
} else {
matrix(row, col) = -1;
}
}
}
#ifdef DEBUG
std::cout << "Munkres output matrix:" << std::endl;
for ( size_t row = 0 ; row < rows ; row++ ) {
for ( size_t col = 0 ; col < columns ; col++ ) {
std::cout.width(1);
std::cout << matrix(row, col) << ",";
}
std::cout << std::endl;
}
std::cout << std::endl;
#endif
// Remove the excess rows or columns that we added to fit the
// input to a square matrix.
matrix.resize(rows, columns);
m = matrix;
delete [] row_mask;
delete [] col_mask;
}