munkres.cpp
Go to the documentation of this file.
1 
6 /*
7  * Copyright (c) 2007 John Weaver
8  *
9  * This program is free software; you can redistribute it and/or modify
10  * it under the terms of the GNU General Public License as published by
11  * the Free Software Foundation; either version 2 of the License, or
12  * (at your option) any later version.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program; if not, write to the Free Software
21  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
22  */
23 
24 #include <mtt/munkres.h>
25 
26 #include <iostream>
27 #include <cmath>
28 
29 bool
30 Munkres::find_uncovered_in_matrix(double item, int &row, int &col) {
31  for ( row = 0 ; row < matrix.rows() ; row++ )
32  if ( !row_mask[row] )
33  for ( col = 0 ; col < matrix.columns() ; col++ )
34  if ( !col_mask[col] )
35  if ( matrix(row,col) == item )
36  return true;
37 
38  return false;
39 }
40 
41 bool
42 Munkres::pair_in_list(const std::pair<int,int> &needle, const std::list<std::pair<int,int> > &haystack) {
43  for ( std::list<std::pair<int,int> >::const_iterator i = haystack.begin() ; i != haystack.end() ; i++ ) {
44  if ( needle == *i )
45  return true;
46  }
47 
48  return false;
49 }
50 
51 int
53  for ( int row = 0 ; row < matrix.rows() ; row++ )
54  for ( int col = 0 ; col < matrix.columns() ; col++ )
55  if ( matrix(row,col) == 0 ) {
56  bool isstarred = false;
57  for ( int nrow = 0 ; nrow < matrix.rows() ; nrow++ )
58  if ( mask_matrix(nrow,col) == STAR ) {
59  isstarred = true;
60  break;
61  }
62 
63  if ( !isstarred ) {
64  for ( int ncol = 0 ; ncol < matrix.columns() ; ncol++ )
65  if ( mask_matrix(row,ncol) == STAR ) {
66  isstarred = true;
67  break;
68  }
69  }
70 
71  if ( !isstarred ) {
72  mask_matrix(row,col) = STAR;
73  }
74  }
75 
76  return 2;
77 }
78 
79 int
81  int rows = matrix.rows();
82  int cols = matrix.columns();
83  int covercount = 0;
84  for ( int row = 0 ; row < rows ; row++ )
85  for ( int col = 0 ; col < cols ; col++ )
86  if ( mask_matrix(row,col) == STAR ) {
87  col_mask[col] = true;
88  covercount++;
89  }
90 
91  int k = matrix.minsize();
92 
93  if ( covercount >= k ) {
94 #ifdef DEBUG
95  std::cout << "Final cover count: " << covercount << std::endl;
96 #endif
97  return 0;
98  }
99 
100 #ifdef DEBUG
101  std::cout << "Munkres matrix has " << covercount << " of " << k << " Columns covered:" << std::endl;
102  for ( int row = 0 ; row < rows ; row++ ) {
103  for ( int col = 0 ; col < cols ; col++ ) {
104  std::cout.width(8);
105  std::cout << matrix(row,col) << ",";
106  }
107  std::cout << std::endl;
108  }
109  std::cout << std::endl;
110 #endif
111 
112 
113  return 3;
114 }
115 
116 int
118  /*
119  Main Zero Search
120 
121  1. Find an uncovered Z in the distance matrix and prime it. If no such zero exists, go to Step 5
122  2. If No Z* exists in the row of the Z', go to Step 4.
123  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
124  */
126  mask_matrix(saverow,savecol) = PRIME; // prime it.
127  } else {
128  return 5;
129  }
130 
131  for ( int ncol = 0 ; ncol < matrix.columns() ; ncol++ )
132  if ( mask_matrix(saverow,ncol) == STAR ) {
133  row_mask[saverow] = true; //cover this row and
134  col_mask[ncol] = false; // uncover the column containing the starred zero
135  return 3; // repeat
136  }
137 
138  return 4; // no starred zero in the row containing this primed zero
139 }
140 
141 int
143  int rows = matrix.rows();
144  int cols = matrix.columns();
145 
146  std::list<std::pair<int,int> > seq;
147  // use saverow, savecol from step 3.
148  std::pair<int,int> z0(saverow, savecol);
149  std::pair<int,int> z1(-1,-1);
150  std::pair<int,int> z2n(-1,-1);
151  seq.insert(seq.end(), z0);
152  int row, col = savecol;
153  /*
154  Increment Set of Starred Zeros
155 
156  1. Construct the ``alternating sequence'' of primed and starred zeros:
157 
158  Z0 : Unpaired Z' from Step 4.2
159  Z1 : The Z* in the column of Z0
160  Z[2N] : The Z' in the row of Z[2N-1], if such a zero exists
161  Z[2N+1] : The Z* in the column of Z[2N]
162 
163  The sequence eventually terminates with an unpaired Z' = Z[2N] for some N.
164  */
165  bool madepair;
166  do {
167  madepair = false;
168  for ( row = 0 ; row < rows ; row++ )
169  if ( mask_matrix(row,col) == STAR ) {
170  z1.first = row;
171  z1.second = col;
172  if ( pair_in_list(z1, seq) )
173  continue;
174 
175  madepair = true;
176  seq.insert(seq.end(), z1);
177  break;
178  }
179 
180  if ( !madepair )
181  break;
182 
183  madepair = false;
184 
185  for ( col = 0 ; col < cols ; col++ )
186  if ( mask_matrix(row,col) == PRIME ) {
187  z2n.first = row;
188  z2n.second = col;
189  if ( pair_in_list(z2n, seq) )
190  continue;
191  madepair = true;
192  seq.insert(seq.end(), z2n);
193  break;
194  }
195  } while ( madepair );
196 
197  for ( std::list<std::pair<int,int> >::iterator i = seq.begin() ;
198  i != seq.end() ;
199  i++ ) {
200  // 2. Unstar each starred zero of the sequence.
201  if ( mask_matrix(i->first,i->second) == STAR )
202  mask_matrix(i->first,i->second) = NORMAL;
203 
204  // 3. Star each primed zero of the sequence,
205  // thus increasing the number of starred zeros by one.
206  if ( mask_matrix(i->first,i->second) == PRIME )
207  mask_matrix(i->first,i->second) = STAR;
208  }
209 
210  // 4. Erase all primes, uncover all columns and rows,
211  for ( int row = 0 ; row < mask_matrix.rows() ; row++ )
212  for ( int col = 0 ; col < mask_matrix.columns() ; col++ )
213  if ( mask_matrix(row,col) == PRIME )
214  mask_matrix(row,col) = NORMAL;
215 
216  for ( int i = 0 ; i < rows ; i++ ) {
217  row_mask[i] = false;
218  }
219 
220  for ( int i = 0 ; i < cols ; i++ ) {
221  col_mask[i] = false;
222  }
223 
224  // and return to Step 2.
225  return 2;
226 }
227 
228 int
230  int rows = matrix.rows();
231  int cols = matrix.columns();
232  /*
233  New Zero Manufactures
234 
235  1. Let h be the smallest uncovered entry in the (modified) distance matrix.
236  2. Add h to all covered rows.
237  3. Subtract h from all uncovered columns
238  4. Return to Step 3, without altering stars, primes, or covers.
239  */
240  double h = 0;
241  for ( int row = 0 ; row < rows ; row++ ) {
242  if ( !row_mask[row] ) {
243  for ( int col = 0 ; col < cols ; col++ ) {
244  if ( !col_mask[col] ) {
245  if ( (h > matrix(row,col) && matrix(row,col) != 0) || h == 0 ) {
246  h = matrix(row,col);
247  }
248  }
249  }
250  }
251  }
252 
253  for ( int row = 0 ; row < rows ; row++ )
254  if ( row_mask[row] )
255  for ( int col = 0 ; col < cols ; col++ )
256  matrix(row,col) += h;
257 
258  for ( int col = 0 ; col < cols ; col++ )
259  if ( !col_mask[col] )
260  for ( int row = 0 ; row < rows ; row++ )
261  matrix(row,col) -= h;
262 
263  return 3;
264 }
265 
266 double Munkres::solve(MatrixXd& m_in,vector<orderedPairPtr>& results)
267 {
268  // Linear assignment problem solution
269  // [modifies matrix in-place.]
270  // matrix(row,col): row major format assumed.
271 
272  // Assignments are remaining 0 values
273  // (extra 0 values are replaced with -1)
274 
275  Matrix<double> m(m_in.rows(),m_in.cols());
276 
277  double highValue = 0;
278 
279  for(int row=0;row<m_in.rows();++row)
280  for(int col=0;col<m_in.cols();++col)
281  {
282  if(!std::isinf(m_in(row,col)))
283  {
284  highValue = m_in(row,col);
285  m(row,col) = m_in(row,col);
286  }
287  }
288 
289  highValue++;
290 
291  for(int row=0;row<m_in.rows();++row)
292  for(int col=0;col<m_in.cols();++col)
293  {
294  if(std::isinf(m_in(row,col)))
295  {
296  m(row,col) = highValue ;
297  }
298  }
299 
300  bool notdone = true;
301  int step = 1;
302 
303  this->matrix = m;
304  // STAR == 1 == starred, PRIME == 2 == primed
306 
307  row_mask = new bool[matrix.rows()];
308  col_mask = new bool[matrix.columns()];
309 
310  for ( int i = 0 ; i < matrix.rows() ; i++ ) {
311  row_mask[i] = false;
312  }
313 
314  for ( int i = 0 ; i < matrix.columns() ; i++ ) {
315  col_mask[i] = false;
316  }
317 
318  while ( notdone ) {
319  switch ( step ) {
320  case 0:
321  notdone = false;
322  break;
323  case 1:
324  step = step1();
325  break;
326  case 2:
327  step = step2();
328  break;
329  case 3:
330  step = step3();
331  break;
332  case 4:
333  step = step4();
334  break;
335  case 5:
336  step = step5();
337  break;
338  }
339  }
340 
341  double cost=0;
342  // Store results
343  for ( int row = 0 ; row < matrix.rows() ; row++ )
344  for ( int col = 0 ; col < matrix.columns() ; col++ )
345  if ( mask_matrix(row,col) == STAR )
346  {
347  orderedPairPtr op(new orderedPair);
348 
349  op->row=row;
350  op->col=col;
351 
352  results.push_back(op);
353 
354  cost+=m(row,col);
355  }
356 
357  delete [] row_mask;
358  delete [] col_mask;
359 
360  return cost;
361 }
362 
363 void
365  // Linear assignment problem solution
366  // [modifies matrix in-place.]
367  // matrix(row,col): row major format assumed.
368 
369  // Assignments are remaining 0 values
370  // (extra 0 values are replaced with -1)
371 
372  double highValue = 0;
373  for ( int row = 0 ; row < m.rows() ; row++ ) {
374  for ( int col = 0 ; col < m.columns() ; col++ ) {
375  if ( m(row,col) != INFINITY && m(row,col) > highValue )
376  highValue = m(row,col);
377  }
378  }
379  highValue++;
380 
381  for ( int row = 0 ; row < m.rows() ; row++ )
382  for ( int col = 0 ; col < m.columns() ; col++ )
383  if ( m(row,col) == INFINITY )
384  m(row,col) = highValue;
385 
386  bool notdone = true;
387  int step = 1;
388 
389  this->matrix = m;
390  // STAR == 1 == starred, PRIME == 2 == primed
392 
393  row_mask = new bool[matrix.rows()];
394  col_mask = new bool[matrix.columns()];
395  for ( int i = 0 ; i < matrix.rows() ; i++ ) {
396  row_mask[i] = false;
397  }
398 
399  for ( int i = 0 ; i < matrix.columns() ; i++ ) {
400  col_mask[i] = false;
401  }
402 
403  while ( notdone ) {
404  switch ( step ) {
405  case 0:
406  notdone = false;
407  break;
408  case 1:
409  step = step1();
410  break;
411  case 2:
412  step = step2();
413  break;
414  case 3:
415  step = step3();
416  break;
417  case 4:
418  step = step4();
419  break;
420  case 5:
421  step = step5();
422  break;
423  }
424  }
425 
426  // Store results
427  for ( int row = 0 ; row < matrix.rows() ; row++ )
428  for ( int col = 0 ; col < matrix.columns() ; col++ )
429  if ( mask_matrix(row,col) == STAR )
430  matrix(row,col) = 0;
431  else
432  matrix(row,col) = -1;
433 
434 #ifdef DEBUG
435  std::cout << "Munkres output matrix:" << std::endl;
436  for ( int row = 0 ; row < matrix.rows() ; row++ ) {
437  for ( int col = 0 ; col < matrix.columns() ; col++ ) {
438  std::cout.width(1);
439  std::cout << matrix(row,col) << ",";
440  }
441  std::cout << std::endl;
442  }
443  std::cout << std::endl;
444 #endif
445 
446  m = matrix;
447 
448  delete [] row_mask;
449  delete [] col_mask;
450 }
int step1(void)
Definition: munkres.cpp:52
bool pair_in_list(const std::pair< int, int > &, const std::list< std::pair< int, int > > &)
Definition: munkres.cpp:42
bool * col_mask
Definition: munkres.h:72
void resize(int rows, int columns)
Definition: matrix.h:99
int saverow
Definition: munkres.h:73
static const int NORMAL
Definition: munkres.h:58
int step4(void)
Definition: munkres.cpp:142
void solve(Matrix< double > &m)
Definition: munkres.cpp:364
int columns(void)
Definition: matrix.h:241
int step5(void)
Definition: munkres.cpp:229
int minsize(void)
Definition: matrix.h:236
Matrix< double > matrix
Definition: munkres.h:70
bool * row_mask
Definition: munkres.h:71
Matrix< int > mask_matrix
Definition: munkres.h:69
bool find_uncovered_in_matrix(double, int &, int &)
Definition: munkres.cpp:30
External library declaration to preform the Hungarian algorithm (Munkres)
static const int STAR
Definition: munkres.h:59
int rows(void)
Definition: matrix.h:246
int step3(void)
Definition: munkres.cpp:117
int savecol
Definition: munkres.h:73
static const int PRIME
Definition: munkres.h:60
int step2(void)
Definition: munkres.cpp:80
boost::shared_ptr< orderedPair > orderedPairPtr
Definition: munkres.h:49


mtt
Author(s): Jorge Almeida
autogenerated on Mon Mar 2 2015 01:32:18