lpsolver.cpp

00001 /*M///////////////////////////////////////////////////////////////////////////////////////
00002 //
00003 //  IMPORTANT: READ BEFORE DOWNLOADING, COPYING, INSTALLING OR USING.
00004 //
00005 //  By downloading, copying, installing or using the software you agree to this license.
00006 //  If you do not agree to this license, do not download, install,
00007 //  copy or use the software.
00008 //
00009 //
00010 //                           License Agreement
00011 //                For Open Source Computer Vision Library
00012 //
00013 // Copyright (C) 2013, OpenCV Foundation, all rights reserved.
00014 // Third party copyrights are property of their respective owners.
00015 //
00016 // Redistribution and use in source and binary forms, with or without modification,
00017 // are permitted provided that the following conditions are met:
00018 //
00019 //   * Redistribution's of source code must retain the above copyright notice,
00020 //     this list of conditions and the following disclaimer.
00021 //
00022 //   * Redistribution's in binary form must reproduce the above copyright notice,
00023 //     this list of conditions and the following disclaimer in the documentation
00024 //     and/or other materials provided with the distribution.
00025 //
00026 //   * The name of the copyright holders may not be used to endorse or promote products
00027 //     derived from this software without specific prior written permission.
00028 //
00029 // This software is provided by the copyright holders and contributors "as is" and
00030 // any express or implied warranties, including, but not limited to, the implied
00031 // warranties of merchantability and fitness for a particular purpose are disclaimed.
00032 // In no event shall the OpenCV Foundation or contributors be liable for any direct,
00033 // indirect, incidental, special, exemplary, or consequential damages
00034 // (including, but not limited to, procurement of substitute goods or services;
00035 // loss of use, data, or profits; or business interruption) however caused
00036 // and on any theory of liability, whether in contract, strict liability,
00037 // or tort (including negligence or otherwise) arising in any way out of
00038 // the use of this software, even if advised of the possibility of such damage.
00039 //
00040 //M*/
00041 #include "precomp.hpp"
00042 #include <climits>
00043 #include <algorithm>
00044 #include <cstdarg>
00045 
00046 #define dprintf(x)
00047 #define print_matrix(x)
00048 
00049 namespace cv
00050 {
00051 
00052 using std::vector;
00053 
00054 #ifdef ALEX_DEBUG
00055 static void print_simplex_state(const Mat& c,const Mat& b,double v,const std::vector<int> N,const std::vector<int> B){
00056     printf("\tprint simplex state\n");
00057 
00058     printf("v=%g\n",v);
00059 
00060     printf("here c goes\n");
00061     print_matrix(c);
00062 
00063     printf("non-basic: ");
00064     print(Mat(N));
00065     printf("\n");
00066 
00067     printf("here b goes\n");
00068     print_matrix(b);
00069     printf("basic: ");
00070 
00071     print(Mat(B));
00072     printf("\n");
00073 }
00074 #else
00075 #define print_simplex_state(c,b,v,N,B)
00076 #endif
00077 
00078 /**Due to technical considerations, the format of input b and c is somewhat special:
00079  *both b and c should be one column bigger than corresponding b and c of linear problem and the leftmost column will be used internally
00080  by this procedure - it should not be cleaned before the call to procedure and may contain mess after
00081  it also initializes N and B and does not make any assumptions about their init values
00082  * @return SOLVELP_UNFEASIBLE if problem is unfeasible, 0 if feasible.
00083 */
00084 static int initialize_simplex(Mat_<double>& c, Mat_<double>& b,double& v,vector<int>& N,vector<int>& B,vector<unsigned int>& indexToRow);
00085 static inline void pivot(Mat_<double>& c,Mat_<double>& b,double& v,vector<int>& N,vector<int>& B,int leaving_index,
00086         int entering_index,vector<unsigned int>& indexToRow);
00087 /**@return SOLVELP_UNBOUNDED means the problem is unbdd, SOLVELP_MULTI means multiple solutions, SOLVELP_SINGLE means one solution.
00088  */
00089 static int inner_simplex (Mat_<double>& c, Mat_<double>& b,double& v,vector<int>& N,vector<int>& B,vector<unsigned int>& indexToRow);
00090 static void swap_columns(Mat_<double>& A,int col1,int col2);
00091 #define SWAP(type,a,b) {type tmp=(a);(a)=(b);(b)=tmp;}
00092 
00093 //return codes:-2 (no_sol - unbdd),-1(no_sol - unfsbl), 0(single_sol), 1(multiple_sol=>least_l2_norm)
00094 int solveLP(const Mat& Func, const Mat& Constr, Mat& z){
00095     dprintf(("call to solveLP\n"));
00096 
00097     //sanity check (size, type, no. of channels)
00098     CV_Assert(Func.type()==CV_64FC1 || Func.type()==CV_32FC1);
00099     CV_Assert(Constr.type()==CV_64FC1 || Constr.type()==CV_32FC1);
00100     CV_Assert((Func.rows==1 && (Constr.cols-Func.cols==1))||
00101             (Func.cols==1 && (Constr.cols-Func.rows==1)));
00102 
00103     //copy arguments for we will shall modify them
00104     Mat_<double>  bigC=Mat_<double> (1,(Func.rows==1?Func.cols:Func.rows)+1),
00105         bigB=Mat_<double> (Constr.rows,Constr.cols+1);
00106     if(Func.rows==1){
00107         Func.convertTo(bigC.colRange(1,bigC.cols),CV_64FC1);
00108     }else{
00109         Mat FuncT=Func.t();
00110         FuncT.convertTo(bigC.colRange(1,bigC.cols),CV_64FC1);
00111     }
00112     Constr.convertTo(bigB.colRange(1,bigB.cols),CV_64FC1);
00113     double v=0;
00114     vector<int> N,B;
00115     vector<unsigned int> indexToRow;
00116 
00117     if(initialize_simplex(bigC,bigB,v,N,B,indexToRow)==SOLVELP_UNFEASIBLE){
00118         return SOLVELP_UNFEASIBLE;
00119     }
00120     Mat_<double>  c=bigC.colRange(1,bigC.cols),
00121         b=bigB.colRange(1,bigB.cols);
00122 
00123     int res=0;
00124     if((res=inner_simplex (c,b,v,N,B,indexToRow))==SOLVELP_UNBOUNDED){
00125         return SOLVELP_UNBOUNDED;
00126     }
00127 
00128     //return the optimal solution
00129     z.create(c.cols,1,CV_64FC1);
00130     MatIterator_<double> it=z.begin<double>();
00131     unsigned int nsize = (unsigned int)N.size();
00132     for(int i=1;i<=c.cols;i++,it++){
00133         if(indexToRow[i]<nsize){
00134             *it=0;
00135         }else{
00136             *it=b.at<double>(indexToRow[i]-nsize,b.cols-1);
00137         }
00138     }
00139 
00140     return res;
00141 }
00142 
00143 static int initialize_simplex(Mat_<double> & c, Mat_<double> & b,double& v,vector<int>& N,vector<int>& B,vector<unsigned int>& indexToRow){
00144     N.resize(c.cols);
00145     N[0]=0;
00146     for (std::vector<int>::iterator it = N.begin()+1 ; it != N.end(); ++it){
00147         *it=it[-1]+1;
00148     }
00149     B.resize(b.rows);
00150     B[0]=(int)N.size();
00151     for (std::vector<int>::iterator it = B.begin()+1 ; it != B.end(); ++it){
00152         *it=it[-1]+1;
00153     }
00154     indexToRow.resize(c.cols+b.rows);
00155     indexToRow[0]=0;
00156     for (std::vector<unsigned int>::iterator it = indexToRow.begin()+1 ; it != indexToRow.end(); ++it){
00157         *it=it[-1]+1;
00158     }
00159     v=0;
00160 
00161     int k=0;
00162     {
00163         double min=DBL_MAX;
00164         for(int i=0;i<b.rows;i++){
00165             if(b(i,b.cols-1)<min){
00166                 min=b(i,b.cols-1);
00167                 k=i;
00168             }
00169         }
00170     }
00171 
00172     if(b(k,b.cols-1)>=0){
00173         N.erase(N.begin());
00174         for (std::vector<unsigned int>::iterator it = indexToRow.begin()+1 ; it != indexToRow.end(); ++it){
00175             --(*it);
00176         }
00177         return 0;
00178     }
00179 
00180     Mat_<double>  old_c=c.clone();
00181     c=0;
00182     c(0,0)=-1;
00183     for(int i=0;i<b.rows;i++){
00184         b(i,0)=-1;
00185     }
00186 
00187     print_simplex_state(c,b,v,N,B);
00188 
00189     dprintf(("\tWE MAKE PIVOT\n"));
00190     pivot(c,b,v,N,B,k,0,indexToRow);
00191 
00192     print_simplex_state(c,b,v,N,B);
00193 
00194     inner_simplex (c,b,v,N,B,indexToRow);
00195 
00196     dprintf(("\tAFTER INNER_SIMPLEX\n"));
00197     print_simplex_state(c,b,v,N,B);
00198 
00199     unsigned int nsize = (unsigned int)N.size();
00200     if(indexToRow[0]>=nsize){
00201         int iterator_offset=indexToRow[0]-nsize;
00202         if(b(iterator_offset,b.cols-1)>0){
00203             return SOLVELP_UNFEASIBLE;
00204         }
00205         pivot(c,b,v,N,B,iterator_offset,0,indexToRow);
00206     }
00207 
00208     vector<int>::iterator iterator;
00209     {
00210         int iterator_offset=indexToRow[0];
00211         iterator=N.begin()+iterator_offset;
00212         std::iter_swap(iterator,N.begin());
00213         SWAP(int,indexToRow[*iterator],indexToRow[0]);
00214         swap_columns(c,iterator_offset,0);
00215         swap_columns(b,iterator_offset,0);
00216     }
00217 
00218     dprintf(("after swaps\n"));
00219     print_simplex_state(c,b,v,N,B);
00220 
00221     //start from 1, because we ignore x_0
00222     c=0;
00223     v=0;
00224     for(int I=1;I<old_c.cols;I++){
00225         if(indexToRow[I]<nsize){
00226             dprintf(("I=%d from nonbasic\n",I));
00227             int iterator_offset=indexToRow[I];
00228             c(0,iterator_offset)+=old_c(0,I);
00229             print_matrix(c);
00230         }else{
00231             dprintf(("I=%d from basic\n",I));
00232             int iterator_offset=indexToRow[I]-nsize;
00233             c-=old_c(0,I)*b.row(iterator_offset).colRange(0,b.cols-1);
00234             v+=old_c(0,I)*b(iterator_offset,b.cols-1);
00235             print_matrix(c);
00236         }
00237     }
00238 
00239     dprintf(("after restore\n"));
00240     print_simplex_state(c,b,v,N,B);
00241 
00242     N.erase(N.begin());
00243     for (std::vector<unsigned int>::iterator it = indexToRow.begin()+1 ; it != indexToRow.end(); ++it){
00244         --(*it);
00245     }
00246     return 0;
00247 }
00248 
00249 static int inner_simplex (Mat_<double> & c, Mat_<double> & b,double& v,vector<int>& N,vector<int>& B,vector<unsigned int>& indexToRow){
00250     int count=0;
00251     for(;;){
00252         dprintf(("iteration #%d\n",count));
00253         count++;
00254 
00255         static MatIterator_<double> pos_ptr;
00256         int e=-1,pos_ctr=0,min_var=INT_MAX;
00257         bool all_nonzero=true;
00258         for(pos_ptr=c.begin();pos_ptr!=c.end();pos_ptr++,pos_ctr++){
00259             if(*pos_ptr==0){
00260                 all_nonzero=false;
00261             }
00262             if(*pos_ptr>0){
00263                 if(N[pos_ctr]<min_var){
00264                     e=pos_ctr;
00265                     min_var=N[pos_ctr];
00266                 }
00267             }
00268         }
00269         if(e==-1){
00270             dprintf(("hello from e==-1\n"));
00271             print_matrix(c);
00272             if(all_nonzero==true){
00273                 return SOLVELP_SINGLE;
00274             }else{
00275                 return SOLVELP_MULTI;
00276             }
00277         }
00278 
00279         int l=-1;
00280         min_var=INT_MAX;
00281         double min=DBL_MAX;
00282         int row_it=0;
00283         MatIterator_<double> min_row_ptr=b.begin();
00284         for(MatIterator_<double> it=b.begin();it!=b.end();it+=b.cols,row_it++){
00285             double myite=0;
00286             //check constraints, select the tightest one, reinforcing Bland's rule
00287             if((myite=it[e])>0){
00288                 double val=it[b.cols-1]/myite;
00289                 if(val<min || (val==min && B[row_it]<min_var)){
00290                     min_var=B[row_it];
00291                     min_row_ptr=it;
00292                     min=val;
00293                     l=row_it;
00294                 }
00295             }
00296         }
00297         if(l==-1){
00298             return SOLVELP_UNBOUNDED;
00299         }
00300         dprintf(("the tightest constraint is in row %d with %g\n",l,min));
00301 
00302         pivot(c,b,v,N,B,l,e,indexToRow);
00303 
00304         dprintf(("objective, v=%g\n",v));
00305         print_matrix(c);
00306         dprintf(("constraints\n"));
00307         print_matrix(b);
00308         dprintf(("non-basic: "));
00309         print_matrix(Mat(N));
00310         dprintf(("basic: "));
00311         print_matrix(Mat(B));
00312     }
00313 }
00314 
00315 static inline void pivot(Mat_<double>& c,Mat_<double>& b,double& v,vector<int>& N,vector<int>& B,
00316         int leaving_index,int entering_index,vector<unsigned int>& indexToRow){
00317     double Coef=b(leaving_index,entering_index);
00318     for(int i=0;i<b.cols;i++){
00319         if(i==entering_index){
00320             b(leaving_index,i)=1/Coef;
00321         }else{
00322             b(leaving_index,i)/=Coef;
00323         }
00324     }
00325 
00326     for(int i=0;i<b.rows;i++){
00327         if(i!=leaving_index){
00328             double coef=b(i,entering_index);
00329             for(int j=0;j<b.cols;j++){
00330                 if(j==entering_index){
00331                     b(i,j)=-coef*b(leaving_index,j);
00332                 }else{
00333                     b(i,j)-=(coef*b(leaving_index,j));
00334                 }
00335             }
00336         }
00337     }
00338 
00339     //objective function
00340     Coef=c(0,entering_index);
00341     for(int i=0;i<(b.cols-1);i++){
00342         if(i==entering_index){
00343             c(0,i)=-Coef*b(leaving_index,i);
00344         }else{
00345             c(0,i)-=Coef*b(leaving_index,i);
00346         }
00347     }
00348     dprintf(("v was %g\n",v));
00349     v+=Coef*b(leaving_index,b.cols-1);
00350 
00351     SWAP(int,N[entering_index],B[leaving_index]);
00352     SWAP(int,indexToRow[N[entering_index]],indexToRow[B[leaving_index]]);
00353 }
00354 
00355 static inline void swap_columns(Mat_<double>& A,int col1,int col2){
00356     for(int i=0;i<A.rows;i++){
00357         double tmp=A(i,col1);
00358         A(i,col1)=A(i,col2);
00359         A(i,col2)=tmp;
00360     }
00361 }
00362 }
00363