k_best.cpp
Go to the documentation of this file.
00001 /**************************************************************************************************
00002  Software License Agreement (BSD License)
00003 
00004  Copyright (c) 2011-2013, LAR toolkit developers - University of Aveiro - http://lars.mec.ua.pt
00005  All rights reserved.
00006 
00007  Redistribution and use in source and binary forms, with or without modification, are permitted
00008  provided that the following conditions are met:
00009 
00010   *Redistributions of source code must retain the above copyright notice, this list of
00011    conditions and the following disclaimer.
00012   *Redistributions in binary form must reproduce the above copyright notice, this list of
00013    conditions and the following disclaimer in the documentation and/or other materials provided
00014    with the distribution.
00015   *Neither the name of the University of Aveiro nor the names of its contributors may be used to
00016    endorse or promote products derived from this software without specific prior written permission.
00017  
00018  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR
00019  IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
00020  FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR
00021  CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
00022  DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00023  DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER
00024  IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT
00025  OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00026 ***************************************************************************************************/
00032 #include <mtt/k_best.h>
00033 
00034 Timer t;
00035 
00036 int factorial(int x)
00037 {
00038         return (x == 1 ? x : x * factorial(x - 1));
00039 }
00040 
00041 vector<vector<int> > convertToStdVector(MatrixXd&mat)
00042 {
00043         vector<int> line;//define a working line
00044         vector<vector<int> > out_mat;//define the output matrix
00045         
00046         line.resize(mat.cols(),-1);//resize the line to have the same number of columns as the input matrix
00047         out_mat.resize(mat.rows(),line);//resize the matrix to have the same number of lines as the input matrix
00048         
00049         for(uint l=0;l<mat.rows();++l)
00050                 for(uint c=0;c<mat.cols();++c)
00051                         out_mat[l][c]=mat(l,c);
00052         
00053         return out_mat;
00054 }
00055 
00056 orderedPairPtr makeOrderedPair(int row,int col)
00057 {
00058         orderedPairPtr op(new orderedPair);
00059         op->row=row;
00060         op->col=col;
00061         
00062         return op;
00063 }
00064 
00065 double munkers_wrapper(MatrixXd&cost_mat,vector<orderedPairPtr>&assignments)
00066 {
00067         // Apply Munkres algorithm to matrix.
00068         Munkres m;
00069         double cost = m.solve(cost_mat,assignments);
00070         
00071         return cost;
00072 }
00073 
00074 ostream &operator<<(ostream &o,orderedPairPtr& p)
00075 {
00076         o<<"("<<p->row+1<<", "<<p->col+1<<") ";
00077         return o;
00078 }
00079 
00080 ostream &operator<<(ostream &o,vector<orderedPairPtr>& op)
00081 {       
00082         if(op.size()==0)
00083                 o<<"[]";
00084         
00085         for(uint i=0;i<op.size();++i)
00086         {
00087                 o<<op[i];
00088                 
00089                 if(i<op.size()-1)
00090                         o<<", ";
00091         }
00092         
00093         return o;
00094 }
00095 
00096 ostream &operator<<(ostream &o,vector<AssignmentsPtr>& assing)
00097 {
00098         if(assing.size()==0)
00099                 o<<"[]";
00100         for(uint i=0;i<assing.size();++i)
00101         {
00102                 o<<"i: "<<i<<" cost: "<<assing[i]->cost<<" ";
00103                 o<<assing[i]->pairs<<endl;
00104         }
00105 
00106         return o;
00107 }
00108 
00109 
00110 ostream &operator<<(ostream &o,NodePtr& n)
00111 {               
00112         o<<*n;
00113 //      o<<"cost: "<<n->cost<<endl;
00114 //      o<<"fixed: "<<n->fixed<<endl;
00115 //      o<<"excluded: "<<n->excluded<<endl;
00116 //      o<<"unspecific: "<<n->unspecific<<endl;
00117         
00118         return o;
00119 }
00120 
00121 ostream &operator<<(ostream &o,Node& n)
00122 {               
00123         o<<"cost: "<<n.cost<<endl;
00124         o<<"fixed: "<<n.fixed<<endl;
00125         o<<"excluded: "<<n.excluded<<endl;
00126         o<<"unspecific: "<<n.unspecific<<endl;
00127         
00128         return o;
00129 }
00130 
00131 
00132 ostream &operator<<(ostream &o,vector<NodePtr>& n)
00133 {
00134         cout<<"*****************************************"<<endl;
00135         for(uint i=0;i<n.size();++i)
00136         {
00137                 o<<"cost: "<<n[i]->cost<<endl;
00138                 o<<"fixed: "<<n[i]->fixed<<endl;
00139                 o<<"excluded: "<<n[i]->excluded<<endl;
00140                 o<<"unspecific: "<<n[i]->unspecific<<endl;
00141                 
00142                 if(i<n.size()-1)
00143                         cout<<endl;
00144         }
00145         cout<<"*****************************************"<<endl;
00146         return o;
00147 }
00148 
00149 vector<NodePtr>& operator+=(vector<NodePtr>& a,vector<NodePtr>& b)
00150 {
00151         a.insert(a.begin(),b.begin(),b.end());
00152         return a;
00153 }
00154 
00155 int test(void)
00156 {
00157         vector<AssignmentsPtr> assignments;
00158         
00159 //      MatrixXd cost_mat(10,10);
00160 //      MatrixXd cost_mat(2,2);
00161 //      MatrixXd cost_mat(4,4);
00162 //      MatrixXd cost_mat(3,2);
00163         MatrixXd cost_mat(5,2);
00164 //      MatrixXd cost_mat(2,5);
00165 //      MatrixXd cost_mat(3,3);
00166         
00167 //      cost_mat<<  8, 3, 3,
00168 //                              4, 2, 55,
00169 //                              57, 3, 5;
00170 //      
00171 //      cost_mat<<  0.1054, 1.6097, 2.3026, 0.1,
00172 //                              1.6094, 1.2040, 0.3567, 0.5,
00173 //                              0.5108, 0.3567, 1.6094, 0.3,
00174 //                              0.5   , 0.1       , 0.8   , 0.9;
00175                                 
00176 //      cost_mat<<  0.1054, 1.6097,
00177 //                              1.6094, 1.2040,
00178 //                              0.5108, 0.3567,
00179 //                              0.5   , 0.1       ;
00180                                 
00181 //      cost_mat<<  0.1, 1.6,
00182 //                              1.6, 0.2,
00183 //                              0.5, 0.3;
00184                                 
00185         cost_mat<<  0.1, 0.1,
00186                                 1.6, 0.4,
00187                                 0.5, 0.7,
00188                                 0.01, 2.,
00189                                 0.0, 0.54 ;
00190 
00191                                                                 
00192 //      cost_mat<<  0.1,
00193 //                              1.6,
00194 //                              0.5,
00195 //                              0.01,
00196 //                              0.0;
00197                                 
00198 //      cost_mat<<  1.1,        1.6,    0.5,    0.01,   0.0,
00199 //                              0.1,    0.4,    0.7,    2.0,    0.54;
00200                                 
00201 //      cost_mat<<  0.1054, 1.6097, 2.3026,
00202 //                              1.6094, 0.7040, 0.3567,
00203 //                              0.5108, 0.3567, 0.8   ;
00204 //                              
00205         
00206 //      cost_mat<<      4.170220e-01, 4.191945e-01, 8.007446e-01, 9.834683e-02, 9.888611e-01, 1.936696e-02, 1.023344e-01, 9.034019e-01, 8.833061e-01, 1.147460e-01, 
00207 // 7.203245e-01, 6.852195e-01, 9.682616e-01, 4.211076e-01, 7.481657e-01, 6.788355e-01, 4.140560e-01, 1.374747e-01, 6.236722e-01, 9.494893e-01, 
00208 // 1.143748e-04, 2.044522e-01, 3.134242e-01, 9.578895e-01, 2.804440e-01, 2.116281e-01, 6.944002e-01, 1.392763e-01, 7.509424e-01, 4.499121e-01, 
00209 // 3.023326e-01, 8.781174e-01, 6.923226e-01, 5.331653e-01, 7.892793e-01, 2.655467e-01, 4.141793e-01, 8.073913e-01, 3.488983e-01, 5.783896e-01, 
00210 // 1.467559e-01, 2.738759e-02, 8.763892e-01, 6.918771e-01, 1.032260e-01, 4.915732e-01, 4.995346e-02, 3.976768e-01, 2.699279e-01, 4.081368e-01, 
00211 // 9.233859e-02, 6.704675e-01, 8.946067e-01, 3.155156e-01, 4.478935e-01, 5.336255e-02, 5.358964e-01, 1.653542e-01, 8.958862e-01, 2.370270e-01, 
00212 // 1.862602e-01, 4.173048e-01, 8.504421e-02, 6.865009e-01, 9.085955e-01, 5.741176e-01, 6.637946e-01, 9.275086e-01, 4.280912e-01, 9.033795e-01, 
00213 // 3.455607e-01, 5.586898e-01, 3.905478e-02, 8.346257e-01, 2.936141e-01, 1.467286e-01, 5.148891e-01, 3.477659e-01, 9.648400e-01, 5.736795e-01, 
00214 // 3.967675e-01, 1.403869e-01, 1.698304e-01, 1.828828e-02, 2.877753e-01, 5.893055e-01, 9.445948e-01, 7.508121e-01, 6.634415e-01, 2.870327e-03, 
00215 // 5.388167e-01, 1.981015e-01, 8.781425e-01, 7.501443e-01, 1.300286e-01, 6.997584e-01, 5.865550e-01, 7.259980e-01, 6.216957e-01, 6.171449e-01;
00216         
00217         
00218 //      cost_mat<<      4.170220e-01, 4.191945e-01, 8.007446e-01, 9.834683e-02, 9.888611e-01, 1.936696e-02, 1.023344e-01, 9.034019e-01, 8.833061e-01, 1.147460e-01, 
00219 // 7.203245e-01, 6.852195e-01, 9.682616e-01, 4.211076e-01, 7.481657e-01, 6.788355e-01, 4.140560e-01, 1.374747e-01, 6.236722e-01, 9.494893e-01, 
00220 // 1.143748e-04, 2.044522e-01, 3.134242e-01, 9.578895e-01, 2.804440e-01, 2.116281e-01, 6.944002e-01, 1.392763e-01, 7.509424e-01, 4.499121e-01, 
00221 // 3.023326e-01, 8.781174e-01, 6.923226e-01, 5.331653e-01, 7.892793e-01, 2.655467e-01, 4.141793e-01, 8.073913e-01, 3.488983e-01, 5.783896e-01, 
00222 // 1.467559e-01, 2.738759e-02, 8.763892e-01, 6.918771e-01, 1.032260e-01, 4.915732e-01, 4.995346e-02, 3.976768e-01, 2.699279e-01, 4.081368e-01, 
00223 // 9.233859e-02, 6.704675e-01, 8.946067e-01, 3.155156e-01, 4.478935e-01, 5.336255e-02, 5.358964e-01, 1.653542e-01, 8.958862e-01, 2.370270e-01, 
00224 // 1.862602e-01, 4.173048e-01, 8.504421e-02, 6.865009e-01, 9.085955e-01, 5.741176e-01, 6.637946e-01, 9.275086e-01, 4.280912e-01, 9.033795e-01, 
00225 // 3.455607e-01, 5.586898e-01, 3.905478e-02, 8.346257e-01, 2.936141e-01, 1.467286e-01, 5.148891e-01, 3.477659e-01, 9.648400e-01, 5.736795e-01, 
00226 // 3.967675e-01, 1.403869e-01, 1.698304e-01, 1.828828e-02, 2.877753e-01, 5.893055e-01, 9.445948e-01, 7.508121e-01, 6.634415e-01, 2.870327e-03, 
00227 // 5.388167e-01, 1.981015e-01, 8.781425e-01, 7.501443e-01, 1.300286e-01, 6.997584e-01, 5.865550e-01, 7.259980e-01, 6.216957e-01, 6.171449e-01;
00228 
00229 //      int right_cols = 2;
00230         
00231 //      cost_mat.rightCols(right_cols)=MatrixXd::Constant(cost_mat.rows(),right_cols,-log(0));
00232 
00233         cout<<"Cost mat:"<<endl<<cost_mat<<endl<<endl;
00234         
00235         
00236         
00237 //      vector<orderedPairPtr> single_assignments;
00238         
00239         // Apply Munkres algorithm to matrix.
00240 //      Munkres m;
00241 //      double cost = m.solve(cost_mat,single_assignments);
00242         
00243 //      cout<<"cost: "<<cost<<endl;
00244 //      cout<<single_assignments<<endl;
00245 //      cout<<"FINAL"<<endl;
00246         
00247 //      return 0;
00248         
00249 //      for(uint c=1;c<cost_mat.cols();++c)
00250                 
00251 
00252 //      cost_mat<<      -1., 2.,
00253 //                              2., 1.2;
00254 
00255 //      uint k=1088640;
00256         uint k=2000;
00257         
00258         t.tic();
00259         
00260         assignments = k_best_assignment(cost_mat,k);
00261         
00262         t.toc();
00263         
00264 //      cout<<"print assignments:"<<endl<<endl;
00265         cout<<assignments<<endl;
00266         
00267         cout<<"Time for code was "<< t.value() <<" seconds"<<endl;
00268         
00269 //      cout<<"done"<<endl;
00270         
00271         return 0;
00272 }
00273 
00274 //assignment
00275 vector<AssignmentsPtr> k_best_assignment(MatrixXd& cost_mat,uint k)
00276 {
00277         //Output variable
00278         vector<AssignmentsPtr> assignments_list;
00279         
00280         //Declare the assignments 
00281         vector<orderedPairPtr> assignments;
00282         //Calculate the first best assignment
00283         double cost = munkers_wrapper(cost_mat,assignments);
00284         
00285 //      cout<<"here0"<<endl;
00286         
00287         //Declare an auxiliary node ptr
00288         NodePtr node_aux(new Node);
00289         
00290         //Put the best assignment in it
00291         node_aux->unspecific = assignments;
00292         node_aux->cost=cost;
00293         
00294         //Declare a node and add the calculated assignment as top layer
00295         vector<NodePtr> optimal_nodes;
00296         optimal_nodes.push_back(node_aux);
00297         
00298 //      cout<<"here2"<<endl;
00299         
00300 //      cout<<"optimal nodes:\n"<<optimal_nodes<<endl;
00301         
00302         //List of nodes to do the partitioning 
00303         vector<NodePtr> nodes_list = partitionNode(*node_aux,cost_mat);
00304         
00305 //      cout<<"node list:\n"<<nodes_list<<endl;
00306         
00307         unsigned long max;
00308         
00309         uint r = cost_mat.rows();
00310         uint c = cost_mat.cols();
00311         
00312 //      cout<<"here1"<<endl;
00313         
00314         if(r<10 && c <10)
00315         {
00316                 if(r==c)
00317                 {
00318                         max=factorial(r);
00319                         
00320                 }else
00321                 {
00322 //                      long c_max=1;
00323 //                      long r_max=1;
00324                         
00325                         max=1;
00326                         
00327                         for(uint i=r;i>0;i--)
00328                         {
00329                                 max*=i;
00330                                 if(max>k)
00331                                         break;
00332                         }
00333                         
00334                         max=r>c?factorial(r)/factorial(r-c):factorial(c)/factorial(c-r);
00335                 }
00336         }else//the maximum number of hypothesis is too big to calculate and it is also not usefull
00337                 max=100;
00338         
00339         if(k>max)
00340                 k=max;
00341         
00342 //      cout<<"k"<<k<<endl;
00343         
00344         while(optimal_nodes.size()<k)
00345         {
00346                 double min_cost=1e10;
00347                 uint ind=-1;
00348                 
00349                 for(uint i=0;i<nodes_list.size();++i)
00350                         if(nodes_list[i]->cost<min_cost)
00351                         {
00352                                 min_cost=nodes_list[i]->cost;
00353                                 ind=i;
00354                         }
00355                 
00356                 optimal_nodes.push_back(nodes_list[ind]);
00357 
00358                 vector<NodePtr> nodes_tmp = partitionNode(*nodes_list[ind],cost_mat);
00359                 
00360                 nodes_list.erase(nodes_list.begin()+ind);               
00361                 nodes_list+=nodes_tmp;
00362         }
00363         
00364 //      cout<<"stuff"<<endl;
00365         
00366         for(uint ii=0;ii<optimal_nodes.size();++ii)
00367         {
00368                 AssignmentsPtr assign(new Assignments);
00369                 
00370                 for(uint i=0;i<optimal_nodes[ii]->fixed.size();++i)
00371                 {
00372                         orderedPairPtr pair(new orderedPair);
00373                         pair->row=optimal_nodes[ii]->fixed[i]->row;
00374                         pair->col=optimal_nodes[ii]->fixed[i]->col;
00375                         
00376                         assign->pairs.push_back(pair);
00377                 }
00378                 
00379                 for(uint i=0;i<optimal_nodes[ii]->unspecific.size();++i)
00380                 {
00381                         orderedPairPtr pair(new orderedPair);
00382                         pair->row=optimal_nodes[ii]->unspecific[i]->row;
00383                         pair->col=optimal_nodes[ii]->unspecific[i]->col;
00384                         
00385                         assign->pairs.push_back(pair);
00386                 }
00387                 
00388                 assign->cost=optimal_nodes[ii]->cost;   
00389                 assignments_list.push_back(assign);
00390         }
00391                 
00392 //      cout<<"stuff out"<<endl;
00393         
00394         return assignments_list;
00395 }
00396 
00397 vector<NodePtr> partitionNode(Node&node_in,MatrixXd&cost_mat)
00398 {
00399         //Unspecific unspecific assignments     
00400         vector<NodePtr> nodes_out;
00401         
00402         //Declare a temporary node
00403         NodePtr node_out_tmp(new Node); 
00404         
00405         bool non_square_matrix=false;
00406         
00407         if(cost_mat.rows()!=cost_mat.cols())
00408                 non_square_matrix=true;
00409         
00410 //      cout<<"Node in: "<<node_in<<endl;
00411         
00412 //      cout<<"iterate under the number of unspecific assignments"<<endl;
00413         
00414         if(!non_square_matrix)
00415         {
00416                 //Iterate u times to permute help out assignments
00417                 for(uint ii=0;ii<node_in.unspecific.size()-1;++ii)
00418                 {
00419         //              cout<<"iteration: "<<ii<<endl;
00420                         //Keep fixed assignments
00421                         node_out_tmp->fixed = node_in.fixed;
00422                         
00423         //              cout<<"tmp fixed: "<<node_out_tmp->fixed<<endl;
00424                         
00425                         //Keep excluded assignments 
00426                         node_out_tmp->excluded = node_in.excluded;
00427                         
00428         //              cout<<"tmp excluded: "<<node_out_tmp->excluded<<endl;
00429                         
00430                         //If there are unspecific assignments, permute and add to list of exclusions
00431                         if(ii>0)
00432                         {
00433                                 //Add that section to the fixed assignments
00434                                 node_out_tmp->fixed.insert(node_out_tmp->fixed.begin(),node_in.unspecific.begin(),node_in.unspecific.begin()+ii);
00435                         }
00436                         
00437         //              cout<<"tmp fixed after uns: "<<node_out_tmp->fixed<<endl;
00438                         
00439                         //If there are unspecific assignment, permute and add to list of exclusions
00440 
00441                         //Get the unspecific part of the input node
00442                         //Add it has an excluded assignment
00443                         node_out_tmp->excluded.push_back(node_in.unspecific[ii]);
00444                         
00445         //              cout<<"node_out_tmp->excluded: "<<node_out_tmp->excluded<<endl;
00446                         
00447                         //Add the rest of the unspecific
00448                         node_out_tmp->unspecific.clear();
00449                         
00450                         node_out_tmp->unspecific.insert(node_out_tmp->unspecific.begin(),node_in.unspecific.begin()+ii+1,node_in.unspecific.end());
00451                         
00452         //              cout<<"node_out_tmp->unspecific: "<<node_out_tmp->unspecific<<endl;
00453                         
00454 //                      cout<<"node tmp"<<endl<<node_out_tmp<<endl;
00455                         
00456                         NodePtr node_out_tmp2 = calcNodeCost(*node_out_tmp,cost_mat,non_square_matrix);
00457                         
00458         //              cout<<"calc node cost: "<<node_out_tmp2<<endl;
00459                         
00460                         nodes_out.push_back(node_out_tmp2);
00461                 }
00462         }else
00463         {
00464                 //Iterate u times to permute help out assignments
00465                 for(uint ii=0;ii<node_in.unspecific.size();++ii)
00466                 {
00467         //              cout<<"iteration: "<<ii<<endl;
00468                         //Keep fixed assignments
00469                         node_out_tmp->fixed = node_in.fixed;
00470                         
00471         //              cout<<"tmp fixed: "<<node_out_tmp->fixed<<endl;
00472                         
00473                         //Keep excluded assignments 
00474                         node_out_tmp->excluded = node_in.excluded;
00475                         
00476         //              cout<<"tmp excluded: "<<node_out_tmp->excluded<<endl;
00477                         
00478                         //If there are unspecific assignments, permute and add to list of exclusions
00479                         if(ii>0)
00480                         {
00481                                 //Add that section to the fixed assignments
00482                                 node_out_tmp->fixed.insert(node_out_tmp->fixed.begin(),node_in.unspecific.begin(),node_in.unspecific.begin()+ii);
00483                         }
00484                         
00485         //              cout<<"tmp fixed after uns: "<<node_out_tmp->fixed<<endl;
00486                         
00487                         //If there are unspecific assignment, permute and add to list of exclusions
00488 
00489                         //Get the unspecific part of the input node
00490                         //Add it has an excluded assignment
00491                         node_out_tmp->excluded.push_back(node_in.unspecific[ii]);
00492                         
00493         //              cout<<"node_out_tmp->excluded: "<<node_out_tmp->excluded<<endl;
00494                         
00495                         //Add the rest of the unspecific
00496                         node_out_tmp->unspecific.clear();
00497                         
00498                         if(ii<node_in.unspecific.size()-1)
00499                                 node_out_tmp->unspecific.insert(node_out_tmp->unspecific.begin(),node_in.unspecific.begin()+ii+1,node_in.unspecific.end());
00500                         
00501         //              cout<<"node_out_tmp->unspecific: "<<node_out_tmp->unspecific<<endl;
00502                         
00503 //                      cout<<"node tmp"<<endl<<node_out_tmp<<endl;
00504                         
00505                         NodePtr node_out_tmp2 = calcNodeCost(*node_out_tmp,cost_mat,non_square_matrix);
00506                         
00507         //              cout<<"calc node cost: "<<node_out_tmp2<<endl;
00508                         
00509                         nodes_out.push_back(node_out_tmp2);
00510                 }
00511         }
00512                 
00513         
00514         if(nodes_out.size()==0) 
00515         {
00516                 NodePtr empty(new Node);
00517                 empty->cost=1e12;
00518                 
00519                 nodes_out.push_back(empty);
00520         }
00521         
00522 //      cout<<"nodes out:\n"<<nodes_out<<endl;
00523         
00524         return nodes_out;
00525 }
00526 
00527 NodePtr calcNodeCost(Node&node_in,MatrixXd&cost_mat,bool non_square_matrix)
00528 {
00529 //      cout<<">>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>"<<endl;
00530         
00531         vector<orderedPairPtr> fixed = node_in.fixed;
00532         vector<orderedPairPtr> excluded = node_in.excluded;
00533         
00534         MatrixXd mask(cost_mat.rows(),cost_mat.cols());
00535         mask = MatrixXd::Zero(cost_mat.rows(),cost_mat.cols());
00536         
00537         double cost_sum = 0;
00538         double mask_num = 1e12;
00539         
00540 //      cout<<"calc node cost"<<endl;
00541 //      cout<<"fixed:"<<fixed<<endl;
00542         
00543 //      cout<<"mask:\n"<<mask<<endl;
00544         
00545         for(uint ii=0;ii<fixed.size();++ii)
00546         {
00547 //              cout<<"R:"<<fixed[ii]->row<<", C:"<<fixed[ii]->col<<endl;
00548                 cost_sum += cost_mat(fixed[ii]->row,fixed[ii]->col);
00549                 
00550 //              cout<<"Cost sum:"<<cost_sum<<endl;
00551                 
00552                 //Mask out all entries in same rows/cols as fixed assignments
00553 //              cout<<"r:"<<mask.row(fixed[ii]->row)<<endl;
00554 //              cout<<"rn:"<<MatrixXd::Constant(1,mask.rows(),mask_num)<<endl;
00555                 
00556                 mask.row(fixed[ii]->row) = MatrixXd::Constant(1,mask.cols(),mask_num);
00557                 mask.col(fixed[ii]->col) = MatrixXd::Constant(mask.rows(),1,mask_num);
00558         }
00559         
00560 //      cout<<"mask after fixed:"<<mask<<endl;
00561 
00562         //Add excluded assignments to mask_num
00563         for(uint kk=0;kk<excluded.size();++kk)
00564                 mask(excluded[kk]->row,excluded[kk]->col) = mask_num;
00565         
00566 //      cout<<"mask after excluded:"<<mask<<endl;
00567         
00568         //Get list of row, col where mask is 0
00569         vector<orderedPairPtr> zeros;
00570         
00571         for(uint r=0;r<mask.rows();++r)
00572                 for(uint c=0;c<mask.cols();++c)
00573                         if(mask(r,c)==0)
00574                         {
00575                                 orderedPairPtr pair(new orderedPair);
00576                                 pair->row=r;
00577                                 pair->col=c;
00578                                 
00579                                 zeros.push_back(pair);
00580                         }
00581 
00582 //      cout<<"mask after excluded:"<<mask<<endl;
00583 
00584         //Obtain a vector of rows and cols of zeros values the mask
00585         vector<int> rows=getRows(zeros);
00586         vector<int> cols=getCols(zeros);
00587 
00588         //Sort the rows and cols
00589         sort(rows.begin(),rows.end());
00590         sort(cols.begin(),cols.end());
00591 
00592         NodePtr node_out(new Node);
00593 
00594         //Check if the number of repeating values in the cols and rows is the same
00595         if(countRepeatingValues<int>(rows)==countRepeatingValues<int>(cols) && !non_square_matrix)
00596         {
00597                 //Declare the assignments 
00598                 vector<orderedPairPtr> assignments;
00599 
00600                 //Calculate optimal assignment using masked cost matrix
00601                 mask+=cost_mat;
00602 
00603                 double cost = munkers_wrapper(mask,assignments);
00604                         
00605                 for(uint f=0;f<fixed.size();++f)
00606                         for(uint a=0;a<assignments.size();++a)
00607                                 if(assignments[a]->row==fixed[f]->row || assignments[a]->col==fixed[f]->col)
00608                                 {
00609                                         cost-=mask(assignments[a]->row,assignments[a]->col);
00610                                         assignments.erase(assignments.begin()+a);
00611                                         a=a-1;
00612                                 }
00613 
00614                 *node_out = node_in;
00615 
00616                 //Copy unspecific assignments
00617                 node_out->unspecific = assignments;
00618 
00619                 //Add the sum of the costs from fixed assignments and the rest
00620                 node_out->cost=cost_sum + cost;
00621 
00622         }else if(!non_square_matrix) //Degenerate state
00623         {
00624 //              cout<<"Degenerate case"<<endl;
00625                 node_out->cost=1e12;
00626         }else
00627         {
00628                 //This case happens in non square matrices 
00629                 
00630                 //Declare the assignments 
00631                 vector<orderedPairPtr> assignments;
00632 
00633                 //Calculate optimal assignment using masked cost matrix
00634                 mask+=cost_mat;
00635 
00636                 double cost = munkers_wrapper(mask,assignments);
00637                         
00638                 for(uint f=0;f<fixed.size();++f)
00639                         for(uint a=0;a<assignments.size();++a)
00640                                 if(assignments[a]->row==fixed[f]->row || assignments[a]->col==fixed[f]->col)
00641                                 {
00642                                         cost-=mask(assignments[a]->row,assignments[a]->col);
00643                                         assignments.erase(assignments.begin()+a);
00644                                         a=a-1;
00645                                 }
00646 
00647                 *node_out = node_in;
00648 
00649                 //Copy unspecific assignments
00650                 node_out->unspecific = assignments;
00651 
00652                 //Add the sum of the costs from fixed assignments and the rest
00653                 node_out->cost = cost_sum + cost;
00654                 
00655 //              cout<<"assignments:"<<assignments<<endl;
00656 //              cout<<"assignments.size():"<<assignments.size()<<endl;
00657 //              cout<<"cost mat:"<<cost_mat<<endl;
00658                 
00659                 uint total = assignments.size()+fixed.size();
00660                 //Strange degenerate case, where we have too few assignments
00661                 if(total < cost_mat.rows() && total <cost_mat.cols())
00662                         node_out->cost=1e12;
00663         }
00664         
00665 //      cout<<"<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<"<<endl;
00666         return node_out;
00667 }
00668 
00670 template<class type>
00671 uint countRepeatingValues(vector<type> sorted_values)
00672 {
00673         uint repetitions=0;
00674         
00675         if(sorted_values.size()>1)
00676                 for(uint i=1;i<sorted_values.size();++i)
00677                         if(sorted_values[i-1]==sorted_values[i])
00678                                 repetitions++;
00679         return repetitions;
00680 }
00681 
00682 vector<int> getRows(vector<orderedPairPtr>& pairs)
00683 {
00684         vector<int> rows;
00685         
00686         for(uint s=0;s<pairs.size();++s)
00687                 rows.push_back(pairs[s]->row);
00688         
00689         return rows;
00690 }
00691 
00692 vector<int> getCols(vector<orderedPairPtr>& pairs)
00693 {
00694         vector<int> cols;
00695         
00696         for(uint s=0;s<pairs.size();++s)
00697                 cols.push_back(pairs[s]->col);
00698         
00699         return cols;
00700 }
00701 
00702 bool compareOrdered_pair(orderedPairPtr& p1,orderedPairPtr& p2)
00703 {
00704         return true;
00705 }


mtt
Author(s): Jorge Almeida
autogenerated on Thu Nov 20 2014 11:35:45