hungarian.cpp
Go to the documentation of this file.
00001 /********************************************************************
00002  ********************************************************************
00003  ** C++ and STL class implementation of the Hungarian algorithm by David Schwarz, 2012
00004  **
00005  **
00006  ** O(n^3) implementation derived from libhungarian by Cyrill Stachniss, 2004
00007  **
00008  **
00009  ** Solving the Minimum Assignment Problem using the 
00010  ** Hungarian Method.
00011  **
00012  ** ** This file may be freely copied and distributed! **
00013  **
00014  **
00015  ** This file is distributed in the hope that it will be useful,
00016  ** but WITHOUT ANY WARRANTY; without even the implied 
00017  ** warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
00018  ** PURPOSE.  
00019  **
00020  ********************************************************************
00021  ********************************************************************/
00022 
00023 #include <algorithm>
00024 #include "hungarian.h"
00025 #include <limits>
00026 #include <cstddef>
00027 #include <iostream>
00028 #include <printf.h>
00029 #include <stdexcept>
00030 #include <stdio.h>
00031 #define verbose (0)
00032 
00033 Hungarian::Hungarian()
00034 {
00035         //much ado about nothing
00036         m_rows = 1;
00037         m_cols = 1;
00038         m_cost = 0;
00039 
00040         m_costmatrix.resize(m_rows, vector<int>(m_cols,0));
00041         m_assignment.resize(m_rows, vector<int>(m_cols,0));
00042 }
00043 
00044 Hungarian::Hungarian(const vector<vector<int> >& input_matrix, int rows, int cols, MODE mode)
00045 {
00046     
00047   int i,j, org_cols, org_rows;
00048   int max_cost;
00049   max_cost = 0;
00050   
00051   org_cols = cols;
00052   org_rows = rows;
00053 
00054   // is the matrix square? 
00055   // if no, expand with 0-cols / 0-cols
00056 
00057   if(rows!=cols)
00058   {
00059           rows = std::max(cols, rows);
00060           cols = rows;
00061   }
00062   
00063   m_rows = rows;
00064   m_cols = cols;
00065 
00066   m_costmatrix.resize(rows, vector<int>(cols,0));
00067   m_assignment.resize(rows, vector<int>(cols,0));
00068 
00069   for(i=0; i<m_rows; i++) 
00070   {
00071           for(j=0; j<m_cols; j++) 
00072           {
00073               
00074                   m_costmatrix[i][j] =  (i < org_rows && j < org_cols) ? input_matrix[i][j] : 0;
00075                   m_assignment[i][j] = 0;
00076 
00077                   if (max_cost < m_costmatrix[i][j])
00078                   {
00079                           max_cost = m_costmatrix[i][j];
00080                   }
00081           }
00082   }
00083 
00084 
00085   if (mode == HUNGARIAN_MODE_MAXIMIZE_UTIL) 
00086   {
00087           for(i=0; i<m_rows; i++) 
00088           {
00089                   for(j=0; j<m_cols; j++) 
00090                   {
00091                           m_costmatrix[i][j] =  max_cost - m_costmatrix[i][j];
00092                   }
00093           }
00094   }
00095   else if (mode == HUNGARIAN_MODE_MINIMIZE_COST) 
00096   {
00097     // nothing to do
00098   }
00099   else 
00100     fprintf(stderr,"%s: unknown mode. Mode was set to HUNGARIAN_MODE_MINIMIZE_COST !\n", __FUNCTION__);
00101 }
00102 
00103 
00104 void hungarian_print_matrix(const vector<vector<int> >& C, int rows, int cols) 
00105 {
00106         int i,j;
00107         fprintf(stderr , "\n");
00108         for(i=0; i<rows; i++) 
00109         {
00110                 fprintf(stderr, " [");
00111                 for(j=0; j<cols; j++) 
00112                 {
00113                 fprintf(stderr, "%5d ",C[i][j]);
00114                 }
00115                 fprintf(stderr, "]\n");
00116         }
00117         fprintf(stderr, "\n");
00118 }
00119 
00120 void Hungarian::print_assignment() {
00121   hungarian_print_matrix(m_assignment, m_rows, m_cols) ;
00122 }
00123 
00124 void Hungarian::print_cost() {
00125   hungarian_print_matrix(m_costmatrix, m_rows, m_cols) ;
00126 }
00127 
00128 void Hungarian::print_status() 
00129 {
00130   
00131   fprintf(stderr,"cost:\n");
00132   print_cost();
00133 
00134   fprintf(stderr,"assignment:\n");
00135   print_assignment();
00136   
00137 }
00138 
00139 int Hungarian::init(const vector<vector<int> >& input_matrix, int rows, int cols, MODE mode) 
00140 {
00141 
00142   int i,j, org_cols, org_rows;
00143   int max_cost;
00144   max_cost = 0;
00145   
00146   org_cols = cols;
00147   org_rows = rows;
00148 
00149   // is the number of cols  not equal to number of rows ? 
00150   // if yes, expand with 0-cols / 0-cols
00151   rows = std::max(cols, rows);
00152   cols = rows;
00153   
00154   m_rows = rows;
00155   m_cols = cols;
00156 
00157   m_costmatrix.resize(rows, vector<int>(cols,0));
00158   m_assignment.resize(rows, vector<int>(cols,0));
00159 
00160   for(i=0; i<m_rows; i++) 
00161   {
00162     for(j=0; j<m_cols; j++) 
00163         {
00164       m_costmatrix[i][j] =  (i < org_rows && j < org_cols) ? input_matrix[i][j] : 0;
00165       m_assignment[i][j] = 0;
00166 
00167       if (max_cost < m_costmatrix[i][j])
00168            max_cost = m_costmatrix[i][j];
00169     }
00170   }
00171 
00172 
00173   if (mode == HUNGARIAN_MODE_MAXIMIZE_UTIL) {
00174     for(i=0; i<m_rows; i++) {
00175       for(j=0; j<m_cols; j++) {
00176         m_costmatrix[i][j] =  max_cost - m_costmatrix[i][j];
00177       }
00178     }
00179   }
00180   else if (mode == HUNGARIAN_MODE_MINIMIZE_COST) {
00181     // nothing to do
00182   }
00183   else 
00184     fprintf(stderr,"%s: unknown mode. Mode was set to HUNGARIAN_MODE_MINIMIZE_COST !\n", __FUNCTION__);
00185   
00186   return rows;
00187 }
00188 
00189 bool Hungarian::check_solution(const vector<int>& row_dec, const vector<int>& col_inc, const vector<int>& col_vertex)
00190 {
00191         int k, l, m, n;
00192 
00193         m = m_rows;
00194         n = m_cols;
00195         // Begin doublecheck the solution 23
00196         for (k=0;k<m;k++)
00197                 for (l=0;l<n;l++)
00198                         if (m_costmatrix[k][l]<row_dec[k]-col_inc[l])
00199                         return false;
00200 
00201         for (k=0;k<m;k++)
00202         {
00203                 l=col_vertex[k];
00204                 if (l<0 || m_costmatrix[k][l]!=row_dec[k]-col_inc[l])
00205                         return false;
00206         }
00207         k=0;
00208         
00209         for (l=0;l<n;l++)
00210         {
00211                 if (col_inc[l])
00212                 {
00213                         k++;
00214                 }
00215         }
00216         if (k>m)
00217         {
00218                 return false;
00219         }
00220 
00221         //everything checks out, then
00222         return true;
00223   // End doublecheck the solution 23
00224 }
00225 bool Hungarian::assign_solution(const vector<int>& row_dec,const vector<int>&  col_inc, const vector<int>&  col_vertex)
00226 {
00227           // End Hungarian algorithm 18
00228         int i, j, k, l, m, n;
00229 
00230         m = m_rows;
00231         n = m_cols;
00232 
00233         for (i=0;i<m;++i)
00234         {
00235                 m_assignment[i][col_vertex[i]]=HUNGARIAN_ASSIGNED;
00236                 /*TRACE("%d - %d\n", i, col_vertex[i]);*/
00237         }
00238         for (k=0;k<m;++k)
00239         {
00240                 for (l=0;l<n;++l)
00241                 {
00242                 /*TRACE("%d ",m_costmatrix[k][l]-row_dec[k]+col_inc[l]);*/
00243                         m_costmatrix[k][l]=m_costmatrix[k][l]-row_dec[k]+col_inc[l];
00244                 }
00245                 /*TRACE("\n");*/
00246         }
00247         for (i=0;i<m;i++)
00248         {
00249                 m_cost+=row_dec[i];
00250         }
00251         for (i=0;i<n;i++)
00252         {
00253                 m_cost-=col_inc[i];
00254         }
00255         if (verbose)
00256                 fprintf(stderr, "Cost is %d\n",m_cost);
00257 
00258         return true;
00259 
00260 }
00261 
00262 bool Hungarian::solve()
00263 {
00264         int i, j, m, n, k, l, s, t, q, unmatched, cost;
00265 
00266         m = m_rows;
00267         n = m_cols;
00268 
00269         int INF = std::numeric_limits<int>::max();
00270 
00271         //vertex alternating paths,
00272         vector<int> col_vertex(m), row_vertex(n), unchosen_row(m), parent_row(n),
00273                                 row_dec(m),  col_inc(n),  slack_row(m),    slack(n);
00274 
00275         cost=0;
00276 
00277         for (i=0;i<m_rows;i++) 
00278         {
00279                 col_vertex[i]=0;
00280                 unchosen_row[i]=0;
00281                 row_dec[i]=0;
00282                 slack_row[i]=0;
00283         }
00284 
00285         for (j=0;j<m_cols;j++) 
00286         {
00287                 row_vertex[j]=0;
00288                 parent_row[j] = 0;
00289                 col_inc[j]=0;
00290                 slack[j]=0;
00291         }
00292 
00293         //Double check assignment matrix is 0
00294         m_assignment.assign(m, vector<int>(n, HUNGARIAN_NOT_ASSIGNED));
00295 
00296   // Begin subtract column minima in order to start with lots of zeroes 12
00297         if (verbose)
00298         {
00299                 fprintf(stderr, "Using heuristic\n");
00300         }
00301 
00302         for (l=0;l<n;l++)
00303         {
00304                 s = m_costmatrix[0][l];
00305 
00306                 for (k=1;k<m;k++) 
00307                 {
00308                         if (m_costmatrix[k][l] < s)
00309                         {
00310                                 s=m_costmatrix[k][l];
00311                         }
00312                         cost += s;
00313                 }
00314 
00315                 if (s!=0)
00316                 {
00317                         for (k=0;k<m;k++)
00318                         {
00319                                 m_costmatrix[k][l]-=s;
00320                         }
00321                 }
00322 
00323                 //pre-initialize state 16
00324                 row_vertex[l]= -1;
00325                 parent_row[l]= -1;
00326                 col_inc[l]=0;
00327                 slack[l]=INF;
00328         }
00329   // End subtract column minima in order to start with lots of zeroes 12
00330 
00331   // Begin initial state 16
00332         t=0;
00333 
00334         for (k=0;k<m;k++)
00335         {
00336                 bool row_done = false;
00337                 s=m_costmatrix[k][0];
00338 
00339                 for (l=0;l<n;l++)
00340                 {
00341 
00342                         if(l > 0)
00343                         {
00344                                 if (m_costmatrix[k][l] < s)
00345                                 {
00346                                         s = m_costmatrix[k][l];
00347                                 }
00348                                 row_dec[k]=s;
00349                         }
00350 
00351                         if (s == m_costmatrix[k][l] && row_vertex[l]<0)
00352                                 {
00353                                         col_vertex[k]=l;
00354                                         row_vertex[l]=k;
00355 
00356                                         if (verbose)
00357                                         {
00358                                                 fprintf(stderr, "matching col %d==row %d\n",l,k);
00359                                         }
00360                                         row_done = true;
00361                                         break;
00362                                 }
00363                 }
00364 
00365                 if(!row_done)
00366                 {
00367                         col_vertex[k]= -1;
00368 
00369                         if (verbose)
00370                         {
00371                                 fprintf(stderr, "node %d: unmatched row %d\n",t,k);
00372                         }
00373                 
00374                         unchosen_row[t++]=k;
00375                 }
00376 
00377         }
00378   // End initial state 16
00379 
00380         bool checked = false;
00381 
00382   // Begin Hungarian algorithm 18
00383 
00384         //is matching already complete?
00385         if (t == 0)
00386         {
00387                 checked = check_solution(row_dec, col_inc, col_vertex);
00388                 if (checked)
00389                 {
00390                         //finish assignment, wrap up and done.
00391                         bool assign = assign_solution(row_dec, col_inc, col_vertex);
00392                         return true;
00393                 }
00394                 else
00395                 {
00396                         if(verbose)
00397                         {
00398                                 fprintf(stderr, "Could not solve. Error.\n");
00399                         }
00400                         return false;
00401                 }
00402         }
00403 
00404         unmatched=t;
00405 
00406 
00407         while (1)
00408         {
00409                 if (verbose)
00410                 {
00411                         fprintf(stderr, "Matched %d rows.\n",m-t);
00412                 }
00413                 q=0;
00414                 bool try_matching;
00415                 while (1)
00416                 {
00417                         while (q<t)
00418                         {
00419                         // Begin explore node q of the forest 19
00420                                 
00421                                 k=unchosen_row[q];
00422                                 s=row_dec[k];
00423                                 for (l=0;l<n;l++)
00424                                 {
00425                                         if (slack[l])
00426                                         {
00427                                                 int del;
00428                                                 del=m_costmatrix[k][l]-s+col_inc[l];
00429                                                 if (del<slack[l])
00430                                                 {
00431                                                         if (del==0)
00432                                                         {
00433                                                                 if (row_vertex[l]<0)
00434                                                                 {
00435                                                                         goto breakthru;
00436                                                                 }
00437                                                                 slack[l]=0;
00438                                                                 parent_row[l]=k;
00439                                                                 if (verbose){
00440                                                                         fprintf(stderr, "node %d: row %d==col %d--row %d\n",
00441                                                                                 t,row_vertex[l],l,k);}
00442                                                                 unchosen_row[t++]=row_vertex[l];
00443                                                         }
00444                                                         else
00445                                                         {
00446                                                                 slack[l]=del;
00447                                                                 slack_row[l]=k;
00448                                                         }
00449                                                 }
00450                                         }
00451                                 }
00452                         // End explore node q of the forest 19
00453                                 q++;    
00454                         }
00455  
00456           // Begin introduce a new zero into the matrix 21
00457                 s=INF;
00458                 for (l=0;l<n;l++)
00459                 {
00460                         if (slack[l] && slack[l]<s)
00461                         {
00462                                 s=slack[l];
00463                         }
00464                 }
00465                 for (q=0;q<t;q++)
00466                 {
00467                         row_dec[unchosen_row[q]]+=s;
00468                 }
00469                 for (l=0;l<n;l++)
00470                 {
00471                         //check slack
00472                         if (slack[l])
00473                         {
00474                                 slack[l]-=s;
00475                                 if (slack[l]==0)
00476                                 {
00477                                         // Begin look at a new zero 22
00478                                         k=slack_row[l];
00479                                         if (verbose)
00480                                         {
00481                                                 fprintf(stderr, 
00482                                                 "Decreasing uncovered elements by %d produces zero at [%d,%d]\n",
00483                                                 s,k,l);
00484                                         }
00485                                         if (row_vertex[l]<0)
00486                                         {
00487                                                 for (j=l+1;j<n;j++)
00488                                                         if (slack[j]==0)
00489                                                         {
00490                                                                 col_inc[j]+=s;
00491                                                         }
00492 
00493                                                 goto breakthru;
00494                                         }
00495                                         else
00496                                         {
00497                                                 parent_row[l]=k;
00498                                                 if (verbose)
00499                                                         { fprintf(stderr, "node %d: row %d==col %d--row %d\n",t,row_vertex[l],l,k);}
00500                                                 unchosen_row[t++]=row_vertex[l];
00501                                                 
00502                                         }
00503                 // End look at a new zero 22
00504                                 }
00505                         }
00506                         else
00507                         {
00508                                 col_inc[l]+=s;
00509                         }
00510                 }
00511         // End introduce a new zero into the matrix 21
00512         }
00513 
00514     breakthru:
00515       // Begin update the matching 20
00516                 if (verbose)
00517                 {
00518                         fprintf(stderr, "Breakthrough at node %d of %d!\n",q,t);
00519                 }
00520                 while (1)
00521                 {
00522                         j=col_vertex[k];
00523                         col_vertex[k]=l;
00524                         row_vertex[l]=k;
00525                         if (verbose)
00526                         {
00527                                 fprintf(stderr, "rematching col %d==row %d\n",l,k);
00528                         }
00529                         if (j<0)
00530                         {
00531                                 break;
00532                         }
00533                         k=parent_row[j];
00534                         l=j;
00535                 }
00536                 // End update the matching 20
00537                 if (--unmatched == 0)
00538                 {
00539                         checked = check_solution(row_dec, col_inc, col_vertex);
00540                         if (checked)
00541                         {
00542                                 //finish assignment, wrap up and done.
00543                                 bool assign = assign_solution(row_dec, col_inc, col_vertex);
00544                                 return true;
00545                         }
00546                         else
00547                         {
00548                                 if(verbose)
00549                                 {
00550                                         fprintf(stderr, "Could not solve. Error.\n");
00551                                 }
00552                                 return false;
00553                         }
00554                 }
00555                 
00556                 // Begin get ready for another stage 17
00557                         t=0;
00558                         for (l=0;l<n;l++)
00559                         {
00560                                 parent_row[l]= -1;
00561                                 slack[l]=INF;
00562                         }
00563                         for (k=0;k<m;k++)
00564                         {
00565                                 if (col_vertex[k]<0)
00566                                 {
00567                                         if (verbose)
00568                                         { fprintf(stderr, "node %d: unmatched row %d\n",t,k);}
00569                                         unchosen_row[t++]=k;
00570                                 }
00571                         }
00572                 // End get ready for another stage 17
00573         }// back to while loop
00574 
00575 
00576 }
00577 
00578 //ACCESSORS
00579 
00580 int Hungarian::cost() const
00581 {
00582         return m_cost;
00583 }
00584 
00585 const vector<vector<int> >& Hungarian::assignment() const
00586 {
00587         return m_assignment;
00588 }
00589 


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