The first draft of simplex algorithm, simple tests.

What we have now corresponds to "formal simplex algorithm", described in
Cormen's "Intro to Algorithms". It will work *only* if the initial
problem has (0,0,0,...,0) as feasible solution (consequently, it will
work unpredictably if problem was unfeasible or did not have zero-vector as
feasible solution). Moreover, it might cycle.

TODO (first priority)
1. Implement initialize_simplex() procedure, that shall check for
feasibility and generate initial feasible solution. (in particular, code
should pass all 4 tests implemented at the moment)
2. Implement Bland's rule to avoid cycling.
3. Make the code more clear.
4. Implement several non-trivial tests (??) and check algorithm against
them. Debug if necessary.

TODO (second priority)
1. Concentrate on stability and speed (make difficult tests)

modules/optim/include/opencv2/optim.hpp

@@ -82,12 +82,12 @@ class CV_EXPORTS LPSolver : public Solver
 public:
   class CV_EXPORTS LPFunction:public Solver::Function
   {
-      cv::Mat z;
+      Mat z;
   public:
       //! Note, that this class is supposed to be immutable, so it's ok to make only a shallow copy of z_in.*/
-      LPFunction(cv::Mat z_in):z(z_in){}
+      LPFunction(Mat z_in):z(z_in){}
       ~LPFunction(){};
-      const cv::Mat& getz()const{return z;}
+      const Mat& getz()const{return z;}
       double calc(InputArray args)const;
   };
 
@@ -96,18 +96,19 @@ public:
   //!this form and **we shall create various constructors for this class that will perform these conversions**.
   class CV_EXPORTS LPConstraints:public Solver::Constraints
   {
-      cv::Mat A,b;
+      Mat A,b;
   public:
       ~LPConstraints(){};
       //! Note, that this class is supposed to be immutable, so it's ok to make only a shallow copy of A_in and b_in.*/
-      LPConstraints(cv::Mat A_in, cv::Mat b_in):A(A_in),b(b_in){}
-      const cv::Mat& getA()const{return A;}
-      const cv::Mat& getb()const{return b;}
+      LPConstraints(Mat A_in, Mat b_in):A(A_in),b(b_in){}
+      const Mat& getA()const{return A;}
+      const Mat& getb()const{return b;}
   };
 
   LPSolver(){}
   double solve(const Function& F,const Constraints& C, OutputArray result)const;
 };
+CV_EXPORTS_W int solveLP(const Mat& Func, const Mat& Constr, Mat& z);
 }}// cv
 
 #endif

modules/optim/src/lpsolver.cpp

+#include "opencv2/ts.hpp"
 #include "precomp.hpp"
+#include <climits>
+#include <algorithm>
 
 namespace cv{namespace optim{
+using std::vector;
 
 double LPSolver::solve(const Function& F,const Constraints& C, OutputArray result)const{
-    printf("call to solve\n");
-
-    //TODO: sanity check and throw exception, if appropriate
-
-    //TODO: copy A,b,z
-    
-    //TODO: run simplex algo
 
     return 0.0;
 }
@@ -18,5 +15,251 @@ double LPSolver::LPFunction::calc(InputArray args)const{
     printf("call to LPFunction::calc()\n");
     return 0.0;
 }
+void print_matrix(const Mat& X){
+    printf("\ttype:%d vs %d,\tsize: %d-on-%d\n",X.type(),CV_64FC1,X.rows,X.cols);
+    for(int i=0;i<X.rows;i++){
+      printf("\t[");
+      for(int j=0;j<X.cols;j++){
+          printf("%g, ",X.at<double>(i,j));
+      }
+      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);
+}
+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)
+
+    //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;
+    }
+    if(Func.rows!=1){
+        printf("Func should be row-vector\n");
+        return -3;
+    }
+    vector<int> N(Func.cols);
+    N[0]=1;
+    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;
+    for (std::vector<int>::iterator it = B.begin()+1 ; it != B.end(); ++it){
+        *it=it[-1]+1;
+    }
+
+    //copy arguments for we will shall modify them
+    Mat c=Func.clone(),
+        b=Constr.clone();
+    double v=0;
+
+    solveLP_aux::initialize_simplex(c,b,z,v);
+
+    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;
+        }
+        printf("offset of first nonneg coef is %d\n",e);//TODO: choose the var with the smallest index
+
+        int l=-1;
+        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++){
+            double myite=0;
+            //check constraints, select the tightest one, TODO: smallest index
+            if((myite=it[e])>0){
+                double val=it[b.cols-1]/myite;
+                if(val<min){
+                    min_row_ptr=it;
+                    ite=myite;
+                    min=val;
+                    l=row_it;
+                }
+            }
+        }
+        if(l==-1){
+            //unbounded
+            return -2;
+        }
+        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;
+
+        printf("objective, v=%g\n",v);
+        print_matrix(c);
+        printf("constraints\n");
+        print_matrix(b);
+        printf("non-basic: ");
+        for (std::vector<int>::iterator it = N.begin() ; it != N.end(); ++it){
+            printf("%d, ",*it);
+        }
+        printf("\nbasic: ");
+        for (std::vector<int>::iterator it = B.begin() ; it != B.end(); ++it){
+            printf("%d, ",*it);
+        }
+        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;
+        }else{
+            *it+=b.at<double>(pos-B.begin(),b.cols-1);
+        }
+    }
+
+    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;
+            }
+            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;
+            }
+        }else{
+            //if no - we need feasible solution
+            gen_new_sol_flag=true;
+        }
+    }
+    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;
+    }
+    
+}
 
 }}

modules/optim/test/test_lpsolver.cpp

+#include "test_precomp.hpp"
+#include "opencv2/optim.hpp"
+
+TEST(Optim_LpSolver, regression)
+{
+    cv::Mat A,B,z,etalon_z;
+
+    if(true){
+    //cormen's example #1
+    A=(cv::Mat_<double>(1,3)<<3,1,2);
+    B=(cv::Mat_<double>(3,4)<<1,1,3,30,2,2,5,24,4,1,2,36);
+    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,3)<<8,4,0);
+    ASSERT_EQ(cv::countNonZero(z!=etalon_z),0);
+    }
+
+    if(true){
+    //cormen's example #2
+    A=(cv::Mat_<double>(1,2)<<18,12.5);
+    B=(cv::Mat_<double>(3,3)<<1,1,20,1,0,20,0,1,16);
+    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)<<20,0);
+    ASSERT_EQ(cv::countNonZero(z!=etalon_z),0);
+    }
+
+    if(true){
+    //cormen's example #3
+    A=(cv::Mat_<double>(1,2)<<5,-3);
+    B=(cv::Mat_<double>(2,3)<<1,-1,1,2,1,2);
+    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);
+    ASSERT_EQ(cv::countNonZero(z!=etalon_z),0);
+
+    }
+    if(false){
+    //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);
+    ASSERT_EQ(cv::countNonZero(z!=etalon_z),0);
+    }
+}
+
+//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 to get smallest l2 norm
+// FUTURE: compress&debug-> more_tests(Cormen) -> readNumRecipes-> fast&stable || hill_climbing

modules/optim/test/test_main.cpp

+#include "test_precomp.hpp"
+
+CV_TEST_MAIN("cv")

modules/optim/test/test_precomp.cpp

+#include "test_precomp.hpp"

modules/optim/test/test_precomp.hpp

+#ifdef __GNUC__
+#  pragma GCC diagnostic ignored "-Wmissing-declarations"
+#  if defined __clang__ || defined __APPLE__
+#    pragma GCC diagnostic ignored "-Wmissing-prototypes"
+#    pragma GCC diagnostic ignored "-Wextra"
+#  endif
+#endif
+
+#ifndef __OPENCV_TEST_PRECOMP_HPP__
+#define __OPENCV_TEST_PRECOMP_HPP__
+
+#include "opencv2/ts.hpp"
+#include "opencv2/optim.hpp"
+
+#endif