munkres.cpp
Go to the documentation of this file.
00001 /*
00002  *   Copyright (c) 2007 John Weaver
00003  *
00004  *   This program is free software; you can redistribute it and/or modify
00005  *   it under the terms of the GNU General Public License as published by
00006  *   the Free Software Foundation; either version 2 of the License, or
00007  *   (at your option) any later version.
00008  *
00009  *   This program is distributed in the hope that it will be useful,
00010  *   but WITHOUT ANY WARRANTY; without even the implied warranty of
00011  *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00012  *   GNU General Public License for more details.
00013  *
00014  *   You should have received a copy of the GNU General Public License
00015  *   along with this program; if not, write to the Free Software
00016  *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
00017  */
00018 
00019 #include "munkres.h"
00020 
00021 #include <iostream>
00022 #include <cmath>
00023 #include <limits>
00024 
00025 
00026 void
00027 replace_infinites(Matrix<double> &matrix) {
00028   const unsigned int rows = matrix.rows(),
00029                      columns = matrix.columns();
00030   assert( rows > 0 && columns > 0 );
00031   double max = matrix(0, 0);
00032   const double infinity = std::numeric_limits<double>::infinity();
00033 
00034   // Find the greatest value in the matrix that isn't infinity.
00035   for ( unsigned int row = 0 ; row < rows ; row++ ) {
00036     for ( unsigned int col = 0 ; col < columns ; col++ ) {
00037       if ( matrix(row, col) != infinity ) {
00038         if ( max == infinity ) {
00039           max = matrix(row, col);
00040         } else {
00041           max = std::max<double>(max, matrix(row, col));
00042         }
00043       }
00044     }
00045   }
00046 
00047   // a value higher than the maximum value present in the matrix.
00048   if ( max == infinity ) {
00049     // This case only occurs when all values are infinite.
00050     max = 0;
00051   } else {
00052     max++;
00053   }
00054   
00055   for ( unsigned int row = 0 ; row < rows ; row++ ) {
00056     for ( unsigned int col = 0 ; col < columns ; col++ ) {
00057       if ( matrix(row, col) == infinity ) {
00058         matrix(row, col) = max;
00059       }
00060     }
00061   }
00062 
00063 }
00064 
00065 void 
00066 minimize_along_direction(Matrix<double> &matrix, bool over_columns) {
00067   const unsigned int outer_size = over_columns ? matrix.columns() : matrix.rows(),
00068                      inner_size = over_columns ? matrix.rows() : matrix.columns();
00069 
00070   // Look for a minimum value to subtract from all values along
00071   // the "outer" direction.
00072   for ( unsigned int i = 0 ; i < outer_size ; i++ ) {
00073     double min = over_columns ? matrix(0, i) : matrix(i, 0);
00074 
00075     // As long as the current minimum is greater than zero,
00076     // keep looking for the minimum.
00077     // Start at one because we already have the 0th value in min.
00078     for ( unsigned int j = 1 ; j < inner_size && min > 0 ; j++ ) {
00079       min = std::min<double>(
00080         min,
00081         over_columns ? matrix(j, i) : matrix(i, j));
00082     }
00083 
00084     if ( min > 0 ) {
00085       for ( unsigned int j = 0 ; j < inner_size ; j++ ) {
00086         if ( over_columns ) {
00087           matrix(j, i) -= min;
00088         } else {
00089           matrix(i, j) -= min;
00090         }
00091       }
00092     }
00093   }
00094 }
00095 
00096 
00097 bool 
00098 Munkres::find_uncovered_in_matrix(double item, unsigned int &row, unsigned int &col) const {
00099   unsigned int rows = matrix.rows(),
00100                columns = matrix.columns();
00101 
00102   for ( row = 0 ; row < rows ; row++ ) {
00103     if ( !row_mask[row] ) {
00104       for ( col = 0 ; col < columns ; col++ ) {
00105         if ( !col_mask[col] ) {
00106           if ( matrix(row,col) == item ) {
00107             return true;
00108           }
00109         }
00110       }
00111     }
00112   }
00113 
00114   return false;
00115 }
00116 
00117 bool 
00118 Munkres::pair_in_list(const std::pair<int,int> &needle, const std::list<std::pair<int,int> > &haystack) {
00119   for ( std::list<std::pair<int,int> >::const_iterator i = haystack.begin() ; i != haystack.end() ; i++ ) {
00120     if ( needle == *i ) {
00121       return true;
00122     }
00123   }
00124   
00125   return false;
00126 }
00127 
00128 int 
00129 Munkres::step1(void) {
00130   const unsigned int rows = matrix.rows(),
00131                      columns = matrix.columns();
00132 
00133   for ( unsigned int row = 0 ; row < rows ; row++ ) {
00134     for ( unsigned int col = 0 ; col < columns ; col++ ) {
00135       if ( 0 == matrix(row, col) ) {
00136         bool isstarred = false;
00137         for ( unsigned int nrow = 0 ; nrow < rows ; nrow++ )
00138           if ( STAR == mask_matrix(nrow,col) ) {
00139             isstarred = true;
00140             break;
00141           }
00142 
00143         if ( !isstarred ) {
00144           for ( unsigned int ncol = 0 ; ncol < columns ; ncol++ )
00145             if ( STAR == mask_matrix(row, ncol) ) {
00146               isstarred = true;
00147               break;
00148             }
00149         }
00150               
00151         if ( !isstarred ) {
00152           mask_matrix(row,col) = STAR;
00153         }
00154       }
00155     }
00156   }
00157 
00158   return 2;
00159 }
00160 
00161 int 
00162 Munkres::step2(void) {
00163   const unsigned int rows = matrix.rows(),
00164                      columns = matrix.columns();
00165   unsigned int covercount = 0;
00166 
00167   for ( unsigned int row = 0 ; row < rows ; row++ )
00168     for ( unsigned int col = 0 ; col < columns ; col++ )
00169       if ( STAR == mask_matrix(row, col) ) {
00170         col_mask[col] = true;
00171         covercount++;
00172       }
00173       
00174   if ( covercount >= matrix.minsize() ) {
00175 #ifdef DEBUG
00176     std::cout << "Final cover count: " << covercount << std::endl;
00177 #endif
00178     return 0;
00179   }
00180 
00181 #ifdef DEBUG
00182   std::cout << "Munkres matrix has " << covercount << " of " << matrix.minsize() << " Columns covered:" << std::endl;
00183   for ( unsigned int row = 0 ; row < rows ; row++ ) {
00184     for ( unsigned int col = 0 ; col < columns ; col++ ) {
00185       std::cout.width(8);
00186       std::cout << matrix(row, col) << ",";
00187     }
00188     std::cout << std::endl;
00189   }
00190   std::cout << std::endl;
00191 #endif
00192 
00193 
00194   return 3;
00195 }
00196 
00197 int 
00198 Munkres::step3(void) {
00199   /*
00200   Main Zero Search
00201 
00202    1. Find an uncovered Z in the distance matrix and prime it. If no such zero exists, go to Step 5
00203    2. If No Z* exists in the row of the Z', go to Step 4.
00204    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
00205   */
00206   if ( find_uncovered_in_matrix(0, saverow, savecol) ) {
00207     mask_matrix(saverow,savecol) = PRIME; // prime it.
00208   } else {
00209     return 5;
00210   }
00211 
00212   for ( unsigned int ncol = 0 ; ncol < matrix.columns() ; ncol++ ) {
00213     if ( mask_matrix(saverow,ncol) == STAR ) {
00214       row_mask[saverow] = true; //cover this row and
00215       col_mask[ncol] = false; // uncover the column containing the starred zero
00216       return 3; // repeat
00217     }
00218   }
00219 
00220   return 4; // no starred zero in the row containing this primed zero
00221 }
00222 
00223 int 
00224 Munkres::step4(void) {
00225   const unsigned int rows = matrix.rows(),
00226                      columns = matrix.columns();
00227 
00228   // seq contains pairs of row/column values where we have found
00229   // either a star or a prime that is part of the ``alternating sequence``.
00230   std::list<std::pair<int,int> > seq;
00231   // use saverow, savecol from step 3.
00232   std::pair<int,int> z0(saverow, savecol);
00233   seq.insert(seq.end(), z0);
00234 
00235   // We have to find these two pairs:
00236   std::pair<int,int> z1(-1, -1);
00237   std::pair<int,int> z2n(-1, -1);
00238 
00239   unsigned int row, col = savecol;
00240   /*
00241   Increment Set of Starred Zeros
00242 
00243    1. Construct the ``alternating sequence'' of primed and starred zeros:
00244 
00245          Z0 : Unpaired Z' from Step 4.2 
00246          Z1 : The Z* in the column of Z0
00247          Z[2N] : The Z' in the row of Z[2N-1], if such a zero exists 
00248          Z[2N+1] : The Z* in the column of Z[2N]
00249 
00250       The sequence eventually terminates with an unpaired Z' = Z[2N] for some N.
00251   */
00252   bool madepair;
00253   do {
00254     madepair = false;
00255     for ( row = 0 ; row < rows ; row++ ) {
00256       if ( mask_matrix(row,col) == STAR ) {
00257         z1.first = row;
00258         z1.second = col;
00259         if ( pair_in_list(z1, seq) ) {
00260           continue;
00261         }
00262         
00263         madepair = true;
00264         seq.insert(seq.end(), z1);
00265         break;
00266       }
00267     }
00268 
00269     if ( !madepair )
00270       break;
00271 
00272     madepair = false;
00273 
00274     for ( col = 0 ; col < columns ; col++ ) {
00275       if ( mask_matrix(row, col) == PRIME ) {
00276         z2n.first = row;
00277         z2n.second = col;
00278         if ( pair_in_list(z2n, seq) ) {
00279           continue;
00280         }
00281         madepair = true;
00282         seq.insert(seq.end(), z2n);
00283         break;
00284       }
00285     }
00286   } while ( madepair );
00287 
00288   for ( std::list<std::pair<int,int> >::iterator i = seq.begin() ;
00289       i != seq.end() ;
00290       i++ ) {
00291     // 2. Unstar each starred zero of the sequence.
00292     if ( mask_matrix(i->first,i->second) == STAR )
00293       mask_matrix(i->first,i->second) = NORMAL;
00294 
00295     // 3. Star each primed zero of the sequence,
00296     // thus increasing the number of starred zeros by one.
00297     if ( mask_matrix(i->first,i->second) == PRIME )
00298       mask_matrix(i->first,i->second) = STAR;
00299   }
00300 
00301   // 4. Erase all primes, uncover all columns and rows, 
00302   for ( unsigned int row = 0 ; row < mask_matrix.rows() ; row++ ) {
00303     for ( unsigned int col = 0 ; col < mask_matrix.columns() ; col++ ) {
00304       if ( mask_matrix(row,col) == PRIME ) {
00305         mask_matrix(row,col) = NORMAL;
00306       }
00307     }
00308   }
00309   
00310   for ( unsigned int i = 0 ; i < rows ; i++ ) {
00311     row_mask[i] = false;
00312   }
00313 
00314   for ( unsigned int i = 0 ; i < columns ; i++ ) {
00315     col_mask[i] = false;
00316   }
00317 
00318   // and return to Step 2. 
00319   return 2;
00320 }
00321 
00322 int 
00323 Munkres::step5(void) {
00324   const unsigned int rows = matrix.rows(),
00325                      columns = matrix.columns();
00326   /*
00327   New Zero Manufactures
00328 
00329    1. Let h be the smallest uncovered entry in the (modified) distance matrix.
00330    2. Add h to all covered rows.
00331    3. Subtract h from all uncovered columns
00332    4. Return to Step 3, without altering stars, primes, or covers. 
00333   */
00334   double h = 0;
00335   for ( unsigned int row = 0 ; row < rows ; row++ ) {
00336     if ( !row_mask[row] ) {
00337       for ( unsigned int col = 0 ; col < columns ; col++ ) {
00338         if ( !col_mask[col] ) {
00339           if ( (h > matrix(row, col) && matrix(row, col) != 0) || h == 0 ) {
00340             h = matrix(row, col);
00341           }
00342         }
00343       }
00344     }
00345   }
00346 
00347   for ( unsigned int row = 0 ; row < rows ; row++ ) {
00348     if ( row_mask[row] ) {
00349       for ( unsigned int col = 0 ; col < columns ; col++ ) {
00350         matrix(row, col) += h;
00351       }
00352     }
00353   }
00354   
00355   for ( unsigned int col = 0 ; col < columns ; col++ ) {
00356     if ( !col_mask[col] ) {
00357       for ( unsigned int row = 0 ; row < rows ; row++ ) {
00358         matrix(row, col) -= h;
00359       }
00360     }
00361   }
00362 
00363   return 3;
00364 }
00365 
00366 /*
00367  *
00368  * Linear assignment problem solution
00369  * [modifies matrix in-place.]
00370  * matrix(row,col): row major format assumed.
00371  *
00372  * Assignments are remaining 0 values
00373  * (extra 0 values are replaced with -1)
00374  *
00375  */
00376 void 
00377 Munkres::solve(Matrix<double> &m) {
00378   const unsigned int rows = m.rows(),
00379                      columns = m.columns(),
00380                      size = std::max<unsigned int>(rows, columns);
00381 
00382 #ifdef DEBUG
00383   std::cout << "Munkres input matrix:" << std::endl;
00384   for ( unsigned int row = 0 ; row < rows ; row++ ) {
00385     for ( unsigned int col = 0 ; col < columns ; col++ ) {
00386       std::cout.width(8);
00387       std::cout << m(row, col) << ",";
00388     }
00389     std::cout << std::endl;
00390   }
00391   std::cout << std::endl;
00392 #endif
00393 
00394   bool notdone = true;
00395   int step = 1;
00396 
00397   // Copy input matrix
00398   this->matrix = m;
00399 
00400   if ( rows != columns ) {
00401     // If the input matrix isn't square, make it square
00402     // and fill the empty values with the largest value present
00403     // in the matrix.
00404     matrix.resize(size, size, matrix.max());
00405   }
00406 
00407 
00408   // STAR == 1 == starred, PRIME == 2 == primed
00409   mask_matrix.resize(size, size);
00410 
00411   row_mask = new bool[size];
00412   col_mask = new bool[columns];
00413   for ( unsigned int i = 0 ; i < size ; i++ ) {
00414     row_mask[i] = false;
00415   }
00416 
00417   for ( unsigned int i = 0 ; i < size ; i++ ) {
00418     col_mask[i] = false;
00419   }
00420 
00421   // Prepare the matrix values...
00422 
00423   // If there were any infinities, replace them with a value greater
00424   // than the maximum value in the matrix.
00425   replace_infinites(matrix);
00426 
00427   minimize_along_direction(matrix, false);
00428   minimize_along_direction(matrix, true);
00429 
00430   // Follow the steps
00431   while ( notdone ) {
00432     switch ( step ) {
00433       case 0:
00434         notdone = false;
00435         // end the step flow
00436         break;
00437       case 1:
00438         step = step1();
00439         // step is always 2
00440         break;
00441       case 2:
00442         step = step2();
00443         // step is always either 0 or 3
00444         break;
00445       case 3:
00446         step = step3();
00447         // step in [3, 4, 5]
00448         break;
00449       case 4:
00450         step = step4();
00451         // step is always 2
00452         break;
00453       case 5:
00454         step = step5();
00455         // step is always 3
00456         break;
00457     }
00458   }
00459 
00460   // Store results
00461   for ( unsigned int row = 0 ; row < size ; row++ ) {
00462     for ( unsigned int col = 0 ; col < size ; col++ ) {
00463       if ( mask_matrix(row, col) == STAR ) {
00464         matrix(row, col) = 1;
00465       } else {
00466         matrix(row, col) = 0;
00467       }
00468     }
00469   }
00470 
00471 #ifdef DEBUG
00472   std::cout << "Munkres output matrix:" << std::endl;
00473   for ( unsigned int row = 0 ; row < rows ; row++ ) {
00474     for ( unsigned int col = 0 ; col < columns ; col++ ) {
00475       std::cout.width(1);
00476       std::cout << matrix(row, col) << ",";
00477     }
00478     std::cout << std::endl;
00479   }
00480   std::cout << std::endl;
00481 #endif
00482 
00483 
00484   // Remove the excess rows or columns that we added to fit the
00485   // input to a square matrix.
00486   matrix.resize(rows, columns);
00487 
00488   m = matrix;
00489 
00490   delete [] row_mask;
00491   delete [] col_mask;
00492 }


explorer
Author(s): Daniel Neuhold , Torsten Andre
autogenerated on Thu Aug 27 2015 11:56:52