/home/laradmin/lar/perception/planarobstacles/mtt/src/munkres.cpp

Go to the documentation of this file.

/*
 *   Copyright (c) 2007 John Weaver
 *
 *   This program is free software; you can redistribute it and/or modify
 *   it under the terms of the GNU General Public License as published by
 *   the Free Software Foundation; either version 2 of the License, or
 *   (at your option) any later version.
 *
 *   This program is distributed in the hope that it will be useful,
 *   but WITHOUT ANY WARRANTY; without even the implied warranty of
 *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *   GNU General Public License for more details.
 *
 *   You should have received a copy of the GNU General Public License
 *   along with this program; if not, write to the Free Software
 *   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307 USA
 */

#include <mtt/munkres.h>

#include <iostream>
#include <cmath>

bool 
Munkres::find_uncovered_in_matrix(double item, int &row, int &col) {
  for ( row = 0 ; row < matrix.rows() ; row++ )
    if ( !row_mask[row] )
      for ( col = 0 ; col < matrix.columns() ; col++ )
        if ( !col_mask[col] )
          if ( matrix(row,col) == item )
            return true;

  return false;
}

bool 
Munkres::pair_in_list(const std::pair<int,int> &needle, const std::list<std::pair<int,int> > &haystack) {
  for ( std::list<std::pair<int,int> >::const_iterator i = haystack.begin() ; i != haystack.end() ; i++ ) {
    if ( needle == *i )
      return true;
  }
  
  return false;
}

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::step2(void) {
  int rows = matrix.rows();
  int cols = matrix.columns();
  int covercount = 0;
  for ( int row = 0 ; row < rows ; row++ )
    for ( int col = 0 ; col < cols ; col++ )
      if ( mask_matrix(row,col) == 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 < rows ; row++ ) {
    for ( int col = 0 ; col < cols ; col++ ) {
      std::cout.width(8);
      std::cout << matrix(row,col) << ",";
    }
    std::cout << std::endl;
  }
  std::cout << std::endl;
#endif


  return 3;
}

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 ( int ncol = 0 ; ncol < matrix.columns() ; 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::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;
}

int 
Munkres::step5(void) {
  int rows = matrix.rows();
  int cols = matrix.columns();
  /*
  New Zero Manufactures

   1. Let h be the smallest uncovered entry in the (modified) distance matrix.
   2. Add h to all covered rows.
   3. Subtract h from all uncovered columns
   4. Return to Step 3, without altering stars, primes, or covers. 
  */
  double h = 0;
  for ( int row = 0 ; row < rows ; row++ ) {
    if ( !row_mask[row] ) {
      for ( int col = 0 ; col < cols ; col++ ) {
        if ( !col_mask[col] ) {
          if ( (h > matrix(row,col) && matrix(row,col) != 0) || h == 0 ) {
            h = matrix(row,col);
          }
        }
      }
    }
  }

  for ( int row = 0 ; row < rows ; row++ )
    if ( row_mask[row] )
      for ( int col = 0 ; col < cols ; col++ )
        matrix(row,col) += h;
  
  for ( int col = 0 ; col < cols ; col++ )
    if ( !col_mask[col] )
      for ( int row = 0 ; row < rows ; row++ )
        matrix(row,col) -= h;

  return 3;
}

double Munkres::solve(MatrixXd& m_in,vector<orderedPairPtr>& results)
{
  // 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)

        Matrix<double> m(m_in.rows(),m_in.cols());
        
        double highValue = 0;
        
        for(int row=0;row<m_in.rows();++row)
                for(int col=0;col<m_in.cols();++col)
                {
                        if(!isinf(m_in(row,col)))
                        {
                                highValue = m_in(row,col);
                                m(row,col) = m_in(row,col);
                        }
                }
  
  highValue++;
  
  for(int row=0;row<m_in.rows();++row)
                for(int col=0;col<m_in.cols();++col)
                {
                        if(isinf(m_in(row,col)))
                        {
                                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;
    }
  }

        double cost=0;
  // Store results
  for ( int row = 0 ; row < matrix.rows() ; row++ )
    for ( int col = 0 ; col < matrix.columns() ; col++ )
      if ( mask_matrix(row,col) == STAR )
          {
                        orderedPairPtr op(new orderedPair);
                        
                        op->row=row;
                        op->col=col;
                        
                        results.push_back(op);
                        
                        cost+=m(row,col);
          }

  delete [] row_mask;
  delete [] col_mask;
  
  return cost;
}

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)

  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;
}

---

mtt
Author(s): Jorge Almeida
autogenerated on Wed Jul 23 04:34:57 2014