From a4a5e98cc06a9217d0ac6950ab0f11d465d04f2a Mon Sep 17 00:00:00 2001
From: Alex Leontiev <alozz1991@gmail.com>
Date: Wed, 3 Jul 2013 13:54:23 +0300
Subject: [PATCH] Non-optimized simplex algorithm.

This version is supposed to work on all problems (please, let me know if
this is not so), but is not optimized yet in terms of numerical
stability and performance. Bland's rule is implemented as well, so
algorithm is supposed to allow no cycling. Additional check for multiple
solutions is added (in case of multiple solutions algorithm returns an
appropriate return code of 1 and returns arbitrary optimal solution).
Finally, now we have 5 tests.

Before Thursday we have 4 directions that can be tackled in parallel:
*) Prepare the pull request!
*) Make the code more clear and readable (refactoring)
*) Wrap the core solveLP() procedure in OOP-style interface
*) Test solveLP on non-trivial tests (possibly test against
http://www.coin-or.org/Clp/)
---
 modules/optim/src/lpsolver.cpp       | 403 ++++++++++++++++++++---------------
 modules/optim/test/test_lpsolver.cpp |  89 +++++++-
 2 files changed, 318 insertions(+), 174 deletions(-)

diff --git a/modules/optim/src/lpsolver.cpp b/modules/optim/src/lpsolver.cpp
index b9cabc1..b56dc3e 100644
--- a/modules/optim/src/lpsolver.cpp
+++ b/modules/optim/src/lpsolver.cpp
@@ -25,68 +25,232 @@ void print_matrix(const Mat& X){
       printf("]\n");
     } 
 }
+void print_simplex_state(const Mat& c,const Mat&b,double v,const vector<int>& N,const vector<int>& B){
+    printf("\tprint simplex state\n");
+
+    printf("v=%g\n",v);
+
+    printf("here c goes\n");
+    print_matrix(c);
+
+    printf("non-basic: ");
+    for (std::vector<int>::const_iterator it = N.begin() ; it != N.end(); ++it){
+        printf("%d, ",*it);
+    }
+    printf("\n");
+
+    printf("here b goes\n");
+    print_matrix(b);
+    printf("basic: ");
+
+    for (std::vector<int>::const_iterator it = B.begin() ; it != B.end(); ++it){
+        printf("%d, ",*it);
+    }
+    printf("\n");
+}
+
 namespace solveLP_aux{
-    //return -1 if problem is unfeasible, 0 if feasible
-    //in latter case it returns feasible solution in z with homogenised b's and v
-    int initialize_simplex(const Mat& c, Mat& b, Mat& z,double& v);
+    /**Due to technical considerations, the format of input b and c is somewhat special:
+     *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
+     by this procedure - it should not be cleaned before the call to procedure and may contain mess after
+     it also initializes N and B and does not make any assumptions about their init values
+     * @return -1 if problem is unfeasible, 0 if feasible.
+    */
+    int initialize_simplex(Mat_<double>& c, Mat_<double>& b,double& v,vector<int>& N,vector<int>& B);
+    inline void pivot(Mat_<double>& c,Mat_<double>& b,double& v,vector<int>& N,vector<int>& B, int leaving_index,int entering_index);
+    /**@return -2 means the problem is unbdd, 1 means multiple solutions, 0 means successful.
+     */
+    int inner_simplex(Mat_<double>& c, Mat_<double>& b,double& v,vector<int>& N,vector<int>& B);
+    void swap_columns(Mat_<double>& A,int col1,int col2);
 }
+
+//return codes:-2 (no_sol - unbdd),-1(no_sol - unfsbl), 0(single_sol), 1(multiple_sol=>least_l2_norm)
 int solveLP(const Mat& Func, const Mat& Constr, Mat& z){
-    printf("call to solveLP\n");//-3(incorrect),-2 (no_sol - unbdd),-1(no_sol - unfsbl), 0(single_sol), 1(multiple_sol=>least_l2_norm)
+    printf("call to solveLP\n");
 
     //sanity check (size, type, no. of channels) (and throw exception, if appropriate)
-    if(Func.type()!=CV_64FC1 || Constr.type()!=CV_64FC1){
-        printf("both Func and Constr should be one-channel matrices of double's\n");
-        return -3;
+    CV_Assert(Func.type()==CV_64FC1);
+    CV_Assert(Constr.type()==CV_64FC1);
+    CV_Assert(Func.rows==1);
+    CV_Assert(Constr.cols-Func.cols==1);
+
+    //copy arguments for we will shall modify them
+    Mat_<double> bigC=Mat_<double>(1,Func.cols+1),
+        bigB=Mat_<double>(Constr.rows,Constr.cols+1);
+    Func.copyTo(bigC.colRange(1,bigC.cols));
+    Constr.copyTo(bigB.colRange(1,bigB.cols));
+    double v=0;
+    vector<int> N,B;
+
+    if(solveLP_aux::initialize_simplex(bigC,bigB,v,N,B)==-1){
+        return -1;
     }
-    if(Func.rows!=1){
-        printf("Func should be row-vector\n");
-        return -3;
+    Mat_<double> c=bigC.colRange(1,bigC.cols),
+        b=bigB.colRange(1,bigB.cols);
+
+    int res=0;
+    if((res=solveLP_aux::inner_simplex(c,b,v,N,B))==-2){
+        return -2;
     }
-    vector<int> N(Func.cols);
-    N[0]=1;
+
+    //return the optimal solution
+    const int z_size[]={1,c.cols};
+    z.create(2,z_size,CV_64FC1);
+    MatIterator_<double> it=z.begin<double>();
+    for(int i=1;i<=c.cols;i++,it++){
+        std::vector<int>::iterator pos=B.begin();
+        if((pos=std::find(B.begin(),B.end(),i))==B.end()){
+            *it=0;
+        }else{
+            *it=b.at<double>(pos-B.begin(),b.cols-1);
+        }
+    }
+
+    return res;
+}
+
+int solveLP_aux::initialize_simplex(Mat_<double>& c, Mat_<double>& b,double& v,vector<int>& N,vector<int>& B){
+    N.resize(c.cols);
+    N[0]=0;
     for (std::vector<int>::iterator it = N.begin()+1 ; it != N.end(); ++it){
         *it=it[-1]+1;
     }
-    if((Constr.cols-1)!=Func.cols){
-        printf("Constr should have one more column when compared to Func\n");
-        return -3;
-    }
-    vector<int> B(Constr.rows);
-    B[0]=Func.cols+1;
+    B.resize(b.rows);
+    B[0]=N.size();
     for (std::vector<int>::iterator it = B.begin()+1 ; it != B.end(); ++it){
         *it=it[-1]+1;
     }
+    v=0;
 
-    //copy arguments for we will shall modify them
-    Mat c=Func.clone(),
-        b=Constr.clone();
-    double v=0;
+    int k=0;
+    {
+        double min=DBL_MAX;
+        for(int i=0;i<b.rows;i++){
+            if(b(i,b.cols-1)<min){
+                min=b(i,b.cols-1);
+                k=i;
+            }
+        }
+    }
+
+    if(b(k,b.cols-1)>=0){
+        N.erase(N.begin());
+        return 0;
+    }
+
+    Mat_<double> old_c=c.clone();
+    c=0;
+    c(0,0)=-1;
+    for(int i=0;i<b.rows;i++){
+        b(i,0)=-1;
+    }
+
+    print_simplex_state(c,b,v,N,B);
+
+    printf("\tWE MAKE PIVOT\n");
+    pivot(c,b,v,N,B,k,0);
+
+    print_simplex_state(c,b,v,N,B);
+
+    inner_simplex(c,b,v,N,B);
+
+    printf("\tAFTER INNER_SIMPLEX\n");
+    print_simplex_state(c,b,v,N,B);
+
+    vector<int>::iterator it=std::find(B.begin(),B.end(),0);
+    if(it!=B.end()){
+        int it_offset=it-B.begin();
+        if(b(it_offset,b.cols-1)>0){
+            return -1;
+        }
+        pivot(c,b,v,N,B,it_offset,0);
+    }
+
+    it=std::find(N.begin(),N.end(),0);
+    int it_offset=it-N.begin();
+    std::iter_swap(it,N.begin());
+    swap_columns(c,it_offset,0);
+    swap_columns(b,it_offset,0);
+
+    printf("after swaps\n");
+    print_simplex_state(c,b,v,N,B);
+
+    //start from 1, because we ignore x_0
+    c=0;
+    v=0;
+    for(int i=1;i<old_c.cols;i++){
+        if((it=std::find(N.begin(),N.end(),i))!=N.end()){
+            printf("i=%d from nonbasic\n",i);
+            fflush(stdout);
+            int it_offset=it-N.begin();
+            c(0,it_offset)+=old_c(0,i);     
+            print_matrix(c);
+        }else{
+            //cv::Mat_
+            printf("i=%d from basic\n",i);
+            fflush(stdout);
+            int it_offset=std::find(B.begin(),B.end(),i)-B.begin();
+            c-=old_c(0,i)*b.row(it_offset).colRange(0,b.cols-1);
+            v+=old_c(0,i)*b(it_offset,b.cols-1);
+            print_matrix(c);
+        }
+    }
 
-    solveLP_aux::initialize_simplex(c,b,z,v);
+    printf("after restore\n");
+    print_simplex_state(c,b,v,N,B);
+
+    N.erase(N.begin());
+    return 0;
+}
 
+int solveLP_aux::inner_simplex(Mat_<double>& c, Mat_<double>& b,double& v,vector<int>& N,vector<int>& B){
     int count=0;
     while(1){
         printf("iteration #%d\n",count++);
 
         MatIterator_<double> pos_ptr;
-        int e=0;
-        for(pos_ptr=c.begin<double>();(*pos_ptr<=0) && pos_ptr!=c.end<double>();pos_ptr++,e++);
-        if(pos_ptr==c.end<double>()){
-            break;
+        int e=-1,pos_ctr=0,min_var=INT_MAX;
+        bool all_nonzero=true;
+        for(pos_ptr=c.begin();pos_ptr!=c.end();pos_ptr++,pos_ctr++){
+            if(*pos_ptr==0){
+                all_nonzero=false;
+            }
+            if(*pos_ptr>0){
+                if(N[pos_ctr]<min_var){
+                    e=pos_ctr;
+                    min_var=N[pos_ctr];
+                }
+            }
+        }
+        if(e==-1){
+            printf("hello from e==-1\n");
+            print_matrix(c);
+            if(all_nonzero==true){
+                return 0;
+            }else{
+                return 1;
+            }
         }
-        printf("offset of first nonneg coef is %d\n",e);//TODO: choose the var with the smallest index
+
+
+        /*for(pos_ptr=c.begin();(*pos_ptr<=0) && pos_ptr!=c.end();pos_ptr++,e++);//TODO: select the smallest index var w/ pos coef
+        if(pos_ptr==c.end()){
+            return 0;
+        }*/
 
         int l=-1;
+        min_var=INT_MAX;
         double min=DBL_MAX;
         int row_it=0;
         double ite=0;
-        MatIterator_<double> min_row_ptr=b.begin<double>();
-        for(MatIterator_<double> it=b.begin<double>();it!=b.end<double>();it+=b.cols,row_it++){
+        MatIterator_<double> min_row_ptr=b.begin();
+        for(MatIterator_<double> it=b.begin();it!=b.end();it+=b.cols,row_it++){
             double myite=0;
-            //check constraints, select the tightest one, TODO: smallest index
+            //check constraints, select the tightest one, reinforcing Bland's rule
             if((myite=it[e])>0){
                 double val=it[b.cols-1]/myite;
-                if(val<min){
+                if(val<min || (val==min && B[row_it]<min_var)){
+                    min_var=B[row_it];
                     min_row_ptr=it;
                     ite=myite;
                     min=val;
@@ -100,52 +264,7 @@ int solveLP(const Mat& Func, const Mat& Constr, Mat& z){
         }
         printf("the tightest constraint is in row %d with %g\n",l,min);
 
-        //pivoting:
-        {
-            int col_count=0;
-            for(MatIterator_<double> it=min_row_ptr;col_count<b.cols;col_count++,it++){
-                if(col_count==e){
-                    *it=1/ite;
-                }else{
-                    *it/=ite;
-                }
-            }
-        }
-        int row_count=0;
-        for(MatIterator_<double> it=b.begin<double>();row_count<b.rows;row_count++){
-            printf("offset: %d\n",it-b.begin<double>());
-            if(row_count==l){
-                it+=b.cols;
-            }else{
-                //remaining constraints
-                double coef=it[e];
-                MatIterator_<double> shadow_it=min_row_ptr;
-                for(int col_it=0;col_it<(b.cols);col_it++,it++,shadow_it++){
-                    if(col_it==e){
-                        *it=-coef*(*shadow_it);
-                    }else{
-                        *it-=coef*(*shadow_it);
-                    }
-                }
-            }
-        }
-        //objective function
-        double coef=*pos_ptr;
-        MatIterator_<double> shadow_it=min_row_ptr;
-        MatIterator_<double> it=c.begin<double>();
-        for(int col_it=0;col_it<(b.cols-1);col_it++,it++,shadow_it++){
-            if(col_it==e){
-                *it=-coef*(*shadow_it);
-            }else{
-                *it-=coef*(*shadow_it);
-            }
-        }
-        v+=coef*(*shadow_it);
-        
-        //new basis and nonbasic sets
-        int tmp=N[e];
-        N[e]=B[l];
-        B[l]=tmp;
+        solveLP_aux::pivot(c,b,v,N,B,l,e);
 
         printf("objective, v=%g\n",v);
         print_matrix(c);
@@ -161,105 +280,57 @@ int solveLP(const Mat& Func, const Mat& Constr, Mat& z){
         }
         printf("\n");
     }
+}
 
-    //return the optimal solution
-    //z=cv::Mat_<double>(1,c.cols,0);
-    MatIterator_<double> it=z.begin<double>();
-    for(int i=1;i<=c.cols;i++,it++){
-        std::vector<int>::iterator pos=B.begin();
-        if((pos=std::find(B.begin(),B.end(),i))==B.end()){
-            *it+=0;
+inline void solveLP_aux::pivot(Mat_<double>& c,Mat_<double>& b,double& v,vector<int>& N,vector<int>& B, int leaving_index,int entering_index){
+    double coef=b(leaving_index,entering_index);
+    for(int i=0;i<b.cols;i++){
+        if(i==entering_index){
+            b(leaving_index,i)=1/coef;
         }else{
-            *it+=b.at<double>(pos-B.begin(),b.cols-1);
+            b(leaving_index,i)/=coef;
         }
     }
 
-    return 0;
-}
-int solveLP_aux::initialize_simplex(const Mat& c, Mat& b, Mat& z,double& v){//TODO
-    z=Mat_<double>(1,c.cols,0.0);
-    v=0;
-    return 0;
-
-    cv::Mat mod_b=(cv::Mat_<double>(1,b.rows));
-    bool gen_new_sol_flag=false,hom_sol_given=false;
-    if(z.type()!=CV_64FC1 || z.rows!=1 || z.cols!=c.cols || (hom_sol_given=(countNonZero(z)==0))){
-        printf("line %d\n",__LINE__);
-        if(hom_sol_given==false){
-            printf("line %d, %d\n",__LINE__,hom_sol_given);
-            z=cv::Mat_<double>(1,c.cols,(double)0);
-        }
-        //check homogeneous solution
-        printf("line %d\n",__LINE__);
-        for(MatIterator_<double> b_it=b.begin<double>()+b.cols-1,mod_b_it=mod_b.begin<double>();mod_b_it!=mod_b.end<double>();
-                b_it+=b.cols,mod_b_it++){
-            if(*b_it<0){
-                //if no - we need feasible solution
-                gen_new_sol_flag=true;
-                break;
-            }
-        }
-        printf("line %d, gen_new_sol_flag=%d - I've got here!!!\n",__LINE__,gen_new_sol_flag);
-        //if yes - we have feasible solution!
-    }else{
-        //check for feasibility
-        MatIterator_<double> it=b.begin<double>();
-        for(MatIterator_<double> mod_b_it=mod_b.begin<double>();it!=b.end<double>();mod_b_it++){
-            double sum=0;
-            for(MatIterator_<double> z_it=z.begin<double>();z_it!=z.end<double>();z_it++,it++){
-                sum+=(*it)*(*z_it);
-            }
-            if((*mod_b_it=(*it-sum))<0){
-                break;
+    for(int i=0;i<b.rows;i++){
+        if(i!=leaving_index){
+            double coef=b(i,entering_index);
+            for(int j=0;j<b.cols;j++){
+                if(j==entering_index){
+                    b(i,j)=-coef*b(leaving_index,j);
+                }else{
+                    b(i,j)-=(coef*b(leaving_index,j));
+                }
             }
-            it++;
         }
-        if(it==b.end<double>()){
-            //z contains feasible solution - just homogenise b's TODO: and v
-            gen_new_sol_flag=false;
-            for(MatIterator_<double> b_it=b.begin<double>()+b.cols-1,mod_b_it=mod_b.begin<double>();mod_b_it!=mod_b.end<double>();
-                    b_it+=b.cols,mod_b_it++){
-                *b_it=*mod_b_it;
-            }
+    }
+
+    //objective function
+    coef=c(0,entering_index);
+    for(int i=0;i<(b.cols-1);i++){
+        if(i==entering_index){
+            c(0,i)=-coef*b(leaving_index,i);
         }else{
-            //if no - we need feasible solution
-            gen_new_sol_flag=true;
+            c(0,i)-=coef*b(leaving_index,i);
         }
     }
-    if(gen_new_sol_flag==true){
-        //we should generate new solution - TODO
-        printf("we should generate new solution\n");
-        Mat new_c=Mat_<double>(1,c.cols+1,0.0),
-            new_b=Mat_<double>(b.rows,b.cols+1,-1.0),
-            new_z=Mat_<double>(1,c.cols+1,0.0);
-
-        new_c.end<double>()[-1]=-1;
-        c.copyTo(new_c.colRange(0,new_c.cols-1));
-
-        b.col(b.cols-1).copyTo(new_b.col(new_b.cols-1));
-        b.colRange(0,b.cols-1).copyTo(new_b.colRange(0,new_b.cols-2));
-
-        Mat b_slice=b.col(b.cols-1);
-        new_z.end<double>()[-1]=-*(std::min_element(b_slice.begin<double>(),b_slice.end<double>()));
-
-        /*printf("matrix new_c\n");
-        print_matrix(new_c);
-        printf("matrix new_b\n");
-        print_matrix(new_b);
-        printf("matrix new_z\n");
-        print_matrix(new_z);*/
-        
-        printf("run for the second time!\n");
-        solveLP(new_c,new_b,new_z);
-        printf("original z was\n");
-        print_matrix(z);
-        printf("that's what I've got\n");
-        print_matrix(new_z);
-        printf("for the constraints\n");
-        print_matrix(b);
-        return 0;
-    }
+    printf("v was %g\n",v);
+    v+=coef*b(leaving_index,b.cols-1);
     
+    int tmp=N[entering_index];
+    N[entering_index]=B[leaving_index];
+    B[leaving_index]=tmp;
 }
 
+void solveLP_aux::swap_columns(Mat_<double>& A,int col1,int col2){
+    for(int i=0;i<A.rows;i++){
+        double tmp=A(i,col1);
+        A(i,col1)=A(i,col2);
+        A(i,col2)=tmp;
+    }
+}
 }}
+/*FIXME (possible optimizations)
+ * use iterator-style (as in ddc0010e7... commit version of this file)
+ * remove calls to pivot inside the while-loops
+ */
diff --git a/modules/optim/test/test_lpsolver.cpp b/modules/optim/test/test_lpsolver.cpp
index c95edf5..9219f0a 100644
--- a/modules/optim/test/test_lpsolver.cpp
+++ b/modules/optim/test/test_lpsolver.cpp
@@ -1,7 +1,7 @@
 #include "test_precomp.hpp"
 #include "opencv2/optim.hpp"
 
-TEST(Optim_LpSolver, regression)
+TEST(Optim_LpSolver, regression_basic)
 {
     cv::Mat A,B,z,etalon_z;
 
@@ -36,26 +36,99 @@ TEST(Optim_LpSolver, regression)
     std::cout<<"here z goes\n"<<z<<"\n";
     etalon_z=(cv::Mat_<double>(1,2)<<1,0);
     ASSERT_EQ(cv::countNonZero(z!=etalon_z),0);
-
     }
-    if(false){
+}
+
+TEST(Optim_LpSolver, regression_init_unfeasible){
+    cv::Mat A,B,z,etalon_z;
+
+    if(true){
     //cormen's example #4 - unfeasible
     A=(cv::Mat_<double>(1,3)<<-1,-1,-1);
     B=(cv::Mat_<double>(2,4)<<-2,-7.5,-3,-10000,-20,-5,-10,-30000);
     std::cout<<"here A goes\n"<<A<<"\n";
     cv::optim::solveLP(A,B,z);
     std::cout<<"here z goes\n"<<z<<"\n";
-    etalon_z=(cv::Mat_<double>(1,2)<<1,0);
+    etalon_z=(cv::Mat_<double>(1,3)<<1250,1000,0);
     ASSERT_EQ(cv::countNonZero(z!=etalon_z),0);
     }
 }
 
+TEST(Optim_LpSolver, regression_absolutely_unfeasible){
+    cv::Mat A,B,z,etalon_z;
+
+    if(true){
+    //trivial absolutely unfeasible example
+    A=(cv::Mat_<double>(1,1)<<1);
+    B=(cv::Mat_<double>(2,2)<<1,-1);
+    std::cout<<"here A goes\n"<<A<<"\n";
+    int res=cv::optim::solveLP(A,B,z);
+    ASSERT_EQ(res,-1);
+    }
+}
+
+TEST(Optim_LpSolver, regression_multiple_solutions){
+    cv::Mat A,B,z,etalon_z;
+
+    if(true){
+    //trivial example with multiple solutions
+    A=(cv::Mat_<double>(1,2)<<1,1);
+    B=(cv::Mat_<double>(1,3)<<1,1,1);
+    std::cout<<"here A goes\n"<<A<<"\n";
+    int res=cv::optim::solveLP(A,B,z);
+    printf("res=%d\n",res);
+    printf("scalar %g\n",z.dot(A));
+    std::cout<<"here z goes\n"<<z<<"\n";
+    ASSERT_EQ(res,1);
+    ASSERT_EQ(z.dot(A),1);
+    }
+
+    if(false){
+    //cormen's example from chapter about initialize_simplex
+    //online solver told it has inf many solutions, but I'm not sure
+    A=(cv::Mat_<double>(1,2)<<2,-1);
+    B=(cv::Mat_<double>(2,3)<<2,-1,2,1,-5,-4);
+    std::cout<<"here A goes\n"<<A<<"\n";
+    int res=cv::optim::solveLP(A,B,z);
+    printf("res=%d\n",res);
+    printf("scalar %g\n",z.dot(A));
+    std::cout<<"here z goes\n"<<z<<"\n";
+    ASSERT_EQ(res,1);
+    }
+}
+
+TEST(Optim_LpSolver, regression_cycling){
+    cv::Mat A,B,z,etalon_z;
+
+    if(true){
+    //example with cycling from http://people.orie.cornell.edu/miketodd/or630/SimplexCyclingExample.pdf
+    A=(cv::Mat_<double>(1,4)<<10,-57,-9,-24);
+    B=(cv::Mat_<double>(3,5)<<0.5,-5.5,-2.5,9,0,0.5,-1.5,-0.5,1,0,1,0,0,0,1);
+    std::cout<<"here A goes\n"<<A<<"\n";
+    int res=cv::optim::solveLP(A,B,z);
+    printf("res=%d\n",res);
+    printf("scalar %g\n",z.dot(A));
+    std::cout<<"here z goes\n"<<z<<"\n";
+    ASSERT_EQ(z.dot(A),1);
+    //ASSERT_EQ(res,1);
+    }
+}
+
 //TODO
 // get optimal solution from initial (0,0,...,0) - DONE
 // milestone: pass first test (wo initial solution) - DONE
-    // learn how to get initial solution
-    // Blands_rule
-    // 1_more_test & make_more_clear
-    // -> **contact_Vadim**: min_l2_norm, init_optional_fsbl_check, error_codes, comment_style-too_many?, copyTo temp headers
+    //
+    // ??how_check_multiple_solutions & pass_test - DONE
+    // Blands_rule - DONE
+    // (&1tests on cycling)
+    //
+    // make_more_clear (assert, assign)
+    // wrap in OOP
+    //
+    // non-trivial tests
+    // pull-request
+    //
+    // study hill and other algos
+    //
 // ??how to get smallest l2 norm
 // FUTURE: compress&debug-> more_tests(Cormen) -> readNumRecipes-> fast&stable || hill_climbing
-- 
2.7.4