draft code of SOCP based on .Mat

Posted 2020-07-20

#include <opencv2/highgui/highgui.hpp> #include "LoadInfo.h" #include "GroundPlaneEstimation.h" #include <fstream> #include <iomanip> #include "config.h"

 

using namespace cv; using namespace std;

#include "LP_Interface.h" std::string getFileName(std::string path, int idx); void TestLP(std::string path);

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

// ///////////////////SOCP workspace starting here // ///////////////////2016.5.10

#include<math.h>

// //////Dimension Size int n=5;  // size of X,  ---n dimension vector input int m=3; // condition number int* k=new int[m];//k[i] denote ki, size of k[] is m.

// /////input data Mat x=(Mat_ <double>(n,1)); double* f=new double[n]; // f is a column vector of size n Mat f_Mat=(Mat_ <double>(n,1)); Mat b=(Mat_<double>(n,m)); // notice column and row are different written, they are exchanged. Big Notice here. Mat c=(Mat_ <double>(n,m)); double* d=new double[m]; vector<Mat> A;

// A[i] is a ki*n matrix. // Mat A[i]=(Mat_<double>(ki,n)) //(s,t) of A[i] is A[i].at<double>(s-1,t-1);

//define dual parametres

Mat y=Mat_<double>(m,1); //column vector Mat Y=Mat_<double>(m,m); // diagonal matrix of y Mat w=Mat_<double>(m,1);//column vector Mat W=Mat_<double>(m,m);//diagonal matrix of w

//define sequence u static double u=0.1; Mat Imm=Mat_<double>(m,m); Mat Inn=Mat_<double>(n,n);

Mat Znm=Mat_<double>(n,m); Mat Zmn=Mat_<double>(m,n); Mat Zmm=Mat_<double>(m,m);

//def e Mat e=Mat_<double>(m,1); void sete() {  int i=0;  for(i=0;i<m;i++)   e.at<double>(i,0)=1; }

//tested correctly void setImm() {  int i=0;  int j=0;  for(i=0;i<m;i++)   for(j=0;j<m;j++)    Imm.at<double>(i,j)=0;  for(i=0;i<m;i++)   Imm.at<double>(i,i)=1;

} //tested correctly void setInn() {  int i=0;  int j=0;  for(i=0;i<n;i++)   for(j=0;j<n;j++)    Inn.at<double>(i,j)=0;  for(i=0;i<n;i++)   Inn.at<double>(i,i)=1;

}

//tested correctly void setZmm() {  int i=0;  int j=0;  for(i=0;i<m;i++)   for(j=0;j<m;j++)    Zmm.at<double>(i,j)=0;

} //tested correctly void setZmn() {  int i=0;  int j=0;  for(i=0;i<m;i++)   for(j=0;j<n;j++)    Zmn.at<double>(i,j)=0;

} //tested correctly void setZnm() {  int i=0;  int j=0;  for(i=0;i<n;i++)   for(j=0;j<m;j++)    Znm.at<double>(i,j)=0;

}

// test case: n=5, m=3. /* A0 is : k0=2;  So it is a 2*5 matrix as follows:   [1.5 2 3 4 5;   -6 -5 -4 -3 -2;] */ /* A1 is : k1=3;  So it is a 3*5 matrix.         [3.14 2.75 -1 0 6;         10 24 -6 -4.25 -7 ;         -102 2 3.7 8.41 3]  */ /* A2is: k2=1; So it is a 1*5 matrix.  *      [100 200 300 400 500]  */ // k[]=[2,3,1] // f[]=[1 2 3 4 5] // d[]=[10 100 100]

// Initialize Phase void setx() {  Mat temp=(Mat_<double>(n,1)<<1,2,3,4,5);  x=temp.clone(); }

void setk() {  k[0]=2;  k[1]=3;  k[2]=1; }

/* A0 is : k0=2;  So it is a 2*5 matrix as follows:   [1.5 2 3 4 5;   -6 -5 -4 -3 -2;] */

//setA() is OK //$$test correctly void setA() {  setk(); // important code line

 Mat A0=(Mat_<double>(k[0],n)<<1.5, 2, 3, 4, 5, -6, -5, -4, -3, -2);  A.push_back(A0);  Mat A1=(Mat_<double>(k[1],n)<<3.14, 2.75, -1, 0, 6, 10, 24, -6, -4.25, -7, -102, 2, 3.7, 8.41, 3);  A.push_back(A1);  Mat A2=(Mat_<double>(k[2],n)<<100,200,300,400,500);  A.push_back(A2); }

void setf() {  f[0]=1;  f[1]=2;  f[2]=3;  f[3]=4;  f[4]=5;  int i=0;  for(i=0;i<n;i++)   f_Mat.at<double>(i,0)=f[i]; }

// setb is ok //$$tested correctly void setb() {  int rr,cc;  for(rr=0;rr<n;rr++)   for(cc=0;cc<m;cc++)    b.at<double>(rr,cc)=rr+cc+1+rr*cc; }

//setc() is ok //$$tested correctly void setc() {  int rr,cc;   for(rr=0;rr<n;rr++)    for(cc=0;cc<m;cc++)     c.at<double>(rr,cc)=-3*(cc+1)*(rr-1)+1-rr*cc;

 // for (int i=0;i<n;i++)   //c.at<double>(i,2)=0; }

// setd() is ok // $$tested correctly void setd() {  d[0]=10;  d[1]=200;  d[2]=0; }

//$$test correctly void sety() {    y=(Mat_<double>(m,1)<<1,1,1); }

//$$tested correctly void setY() {  int i=0;  int j=0;  for(i=0;i<m;i++)   for(j=0;j<m;j++)    Y.at<double>(i,j)=0;  for(i=0;i<m;i++)   Y.at<double>(i,i)=y.at<double>(i,0); }

//$$tested correctly

void setw() {  w=(Mat_<double>(m,1)<<1,1,1); }

//$$ test correctly void setW() {  int i=0;   int j=0;   for(i=0;i<m;i++)    for(j=0;j<m;j++)     W.at<double>(i,j)=0;   for(i=0;i<m;i++)    W.at<double>(i,i)=w.at<double>(i,0);

} //$$tested correctly void initialize() {  setk();  setf();  setA();  setb();  setc();  setd();  setx();  sety();  setY();  setw();  setW();  setInn();  setImm();  sete(); } ////////////////////////////end of initialize phase

////////////////////////////begin of def norm // def ||x|| // $$tested correctly double EuclideanNorm (Mat& xx, int nn)     //  xx is :  Mat x=(Mat_ <double>(n,1));          // nn is size of vector xx. {  double sum=0;  for(int i=0;i<nn;i++)  sum=sum+xx.at<double>(i,0)*xx.at<double>(i,0);  double sqrtSum=sqrt(sum);  return sqrtSum; }

// def ||Aix+bi|| // $$tested correctly double normOfAxPlusb (Mat& xx,    // n*1 vector. Mat xx=(Mat_ <double>(n,1));  Mat& AA,   // ki*n matrix.  Mat& bb,  int ki)       // temp dimesion is ki. {  Mat temp;  temp=AA*xx+bb;  double result=0;  result=EuclideanNorm(temp,ki);  return result; } /////////////////////////end of def norm /// /// /////////////////////begin of define h_i // define a single hi(x) , index is i, means the ith condition // hi(x)=||Aix+b||-ci.t()*x-di // $$tested correctly double hSingle (  Mat& xx,     // xx is :  Mat x=(Mat_ <double>(n,1));   Mat& AA,   // ki*n matrix.   Mat& bb,   Mat& cc,  // as default. cc is a n*1 vector, with size n   double& dd,   int ki) {  double result=0;  Mat temp=cc.t()*xx;  double temp1=temp.at<double>(0,0);  result=normOfAxPlusb(xx,AA,bb,ki)-temp1-dd;  return result;

}

 

// define h_i function // index_i starts from 0,1,2,... // $$tested correctly double h_i    (Mat& xx, int index_i)     // xx is :  Mat x=(Mat_ <double>(n,1)); {  //cout<<"index_i="<<index_i<<endl;

   int  ki=k[index_i];    Mat AA=A[index_i].clone();    Mat tempbbCol=b.col(index_i).clone();    Mat bb=tempbbCol.rowRange(0,ki).clone(); // get the first ki numbers in the column index_i, to match the dimension.                      // From "Row 0" to "Row ki-1", not "Row ki". !!    Mat cc=c.col(index_i).clone();    double dd=d[index_i];

   //cout<<"AA"<<AA<<endl;   // cout<<"bb"<<bb<<endl;    //cout<<"cc"<<cc<<endl;   // cout<<"dd"<<dd<<endl;   // cout<<"ki"<<ki<<endl;

   double result=0;    result=hSingle(xx,AA,bb,cc,dd,ki);

   return result;

} // // now we have h_0, h_1,h_2 //   calling: h_0=h_i(xx,0); h_1=hi(xx,1);h_2=(xx,2); ///////////////////////end of define h_i

///////////////////////begin of def B(x) /* // def B(x)=gradient of h(x)  *      =[ dh1/dx1, dh1/dx2, dh1/dx3, dh1/dx4, ..., dh1/dxn;]  *        [ dh2/dx1, dh2/dx2, dh2/dx3, dh2/dx4, ..., dh2/dxn;]  *        [ dh3/dx1, dh3/dx2, dh3/dx3, dh3/dx4, ..., dh3/dxn;]  *          ...  *          ...  *        [ dhm/dx1, dhm/dx2, dhm/dx3, dhm/dx4, ..., dhm/dxn;]  *  * so B(x) is a m*n matrix  * In this test, B(x) is a 3*5 matrix  * This B(x) is a value matrix. not a function pointer matrix. * B(x) is Mat_<double>(m,n) */

////////////////////////////////////////////////////////

// ///diff of 1D f(x) // $$ tested correctly double firstOrderGradientOf1DFunction (double x, double(*f)(double&)) {  double y0,y1;  double x0,x1;  x0=x;  x1=x+0.000001;  y0=(*f)(x0);  y1=(*f)(x1);  double deltaY=y1-y0;  double deltaX=x1-x0;  cout<<"deltaY"<<deltaY<<endl;  cout<<"deltaX"<<deltaX<<endl;  double result=double(deltaY/deltaX);  return result; }

//h_i(xx,i) //double h_i (Mat& xx, int index_i)

//def dhi/dxt

// dhi/dxt= firstOrderGradientOfh_iOverx_t(h_i, x,i,t) //$$tested correctly.  without every num checked. double firstOrderGradientOfh_iOverx_t ( double(*hh_i)(Mat&,int),   Mat& xx,   int i,   int t) {   double y0,y1;   Mat x0,x1;   x0=xx.clone();   x1=x0.clone();   //cout<<"here i am here"<<endl;   //cout<<"x0="<<x0<<endl;

  //cout<<"x1.at<double>(t,0)"<<x1.at<double>(t,0)<<endl;   x1.at<double>(t,0)= x1.at<double>(t,0)+0.0000001;   //cout<<"x1="<<x1<<endl;   y0=(*hh_i)(x0,i);   y1=(*hh_i)(x1,i);   //cout<<"y0="<<y0<<endl;   //cout<<"y1="<<y1<<endl;

  double deltaY=y1-y0;   double deltaX=x1.at<double>(t,0)-x0.at<double>(t,0);   //cout<<"deltaY"<<deltaY<<endl;   //cout<<"deltaX"<<deltaX<<endl;   double result=double(deltaY/deltaX);   return result;

}

// def B(x) in value matrix // dhi/dxt= firstOrderGradientOfh_iOverx_t(h_i, x,i,t) //$$ tested correctly. not check every number. void B_value   (Mat& xx, Mat& BxTemp) {  int i=0;  int t=0;  Mat xxTemp=xx.clone();

 for (i=0;i<m;i++)   for(t=0;t<n;t++)    BxTemp.at<double>(i,t) = firstOrderGradientOfh_iOverx_t(h_i, xxTemp,i,t);

 //cout<<"B ="<<endl<<BxTemp<<endl;

}   //end of defining B_value() .

////////////////////////////////////end of def B(x) /// /// /// /////////////////////////////////begin of def H(x,y) // //////////////////////////////////////////////////

// def Hessian matrix // def Hess of hi(x)= [ddhi/dx0dx0,  ddhi/dx0dx1, ddhi/dx0dx2, ...,ddhi/dx0dx(n-1);] //         [ddhi/dx1dx0,   ddhi/dx1dx1, ddhi/dx1dx2,..., ddhi/dx1dx(n-1);] //         ... ... //         ... ... //         [ddhi/dx(n-1)dx0,ddhi/dx(n-1)dx1,ddhi/dx(n-1)dx2,...,ddhi/dx(n-1)dx(n-1)];

// def ddh_i/dx_sdx_t=d/dx_s (dh_i/dx_t);

//double firstOrderGradientOfh_iOverx_t //( double(*hh_i)(Mat&,int), //  Mat& xx, //  int i, //  int t)

//$$tested correctly double secondOrderGradientOfh_ioverx_sx_t   (double (*ptr_firstOrderGradientOfh_iOverx_t) (double(*)(Mat&,int),Mat&, int,int),      Mat& xx,      int i,      int s,      int t) {    double y0,y1;    Mat x0,x1;    x0=xx.clone();    x1=x0.clone();

   x1.at<double>(s,0)= x1.at<double>(s,0)+0.0000001;    //cout<<"x1="<<x1<<endl;    y0=(*ptr_firstOrderGradientOfh_iOverx_t)(h_i,x0,i,t);    y1=(*ptr_firstOrderGradientOfh_iOverx_t)(h_i,x1,i,t);   // cout<<"y0="<<y0<<endl;    //cout<<"y1="<<y1<<endl;

 

   double deltaY=y1-y0;    double deltaX=x1.at<double>(s,0)-x0.at<double>(s,0);   // cout<<"deltaY"<<deltaY<<endl;   // cout<<"deltaX"<<deltaX<<endl;    double result=double(deltaY/deltaX);    return result;

}

// define Hessian matrix Hess(hi)

void HessianMatrix(Mat& xx, int i, Mat& HessianMatrix_hi) {

 //cout<<"x="<<x<<endl;  //cout<<"i="<<i<<endl;  //cout<<"A["<<i<<"]="<<endl<<A[i]<<endl;

  int tt=0;   int ss=0;   Mat xxTemp=xx.clone();

  for (ss=0;ss<n;ss++)    for(tt=0;tt<n;tt++)     HessianMatrix_hi.at<double>(ss,tt) =secondOrderGradientOfh_ioverx_sx_t(firstOrderGradientOfh_iOverx_t, xxTemp,i,ss,tt);

  //cout<<"Hessian of hi ="<<endl<<HessianMatrix_hi<<endl;

}

// def H(x,y) // $$tested correctly, almost. void Hxy    (Mat& xx,     Mat& yy,     Mat& Hxy_temp) {    int i=0;    int tt=0;    int ss=0;    Mat xxTemp=xx.clone();    Mat yyTemp=yy.clone();

   //cout<<"xxTemp="<<xxTemp<<endl;    //cout<<"yyTemp="<<yyTemp<<endl;

       for(i=0;i<m;i++)        {             Mat HessianMatrix_hi_temp=Mat_<double>(n,n);             HessianMatrix( xxTemp,  i, HessianMatrix_hi_temp);             Hxy_temp=Hxy_temp+y.at<double>(i,0)*HessianMatrix_hi_temp;

       }     //cout<<"Hxy="<<Hxy_temp<<endl;

} //////////////end of define H(x,y)

/////////////////begin of define Final Matrix /// //Merge to a final Matrix Final=[ H(xy), 0, B(x).t(); //                0,       Y,   W; //              B(x),   I,     0]

 

//$$tested correctly void mergeToFinal          (Mat& Hxy_temp,          Mat& B_temp,    Mat& Y_temp,    Mat& W_temp,    Mat& Final) {  Mat Final_temp=Mat_<double>(n+2*m,n+2*m);  Hxy_temp.copyTo(Final_temp.rowRange(0,n).colRange(0,n));

 Znm.copyTo(Final_temp.rowRange(0,n).colRange(n,n+m));  Mat B_temp_t=B_temp.t();  B_temp_t.copyTo(Final_temp.rowRange(0,n).colRange(n+m,n+2*m));

 Zmn.copyTo(Final_temp.rowRange(n,n+m).colRange(0,n));  Y_temp.copyTo(Final_temp.rowRange(n,n+m).colRange(n,n+m));  W_temp.copyTo(Final_temp.rowRange(n,n+m).colRange(n+m,n+2*m));  B_temp.copyTo(Final_temp.rowRange(n+m,n+2*m).colRange(0,n));  Imm.copyTo(Final_temp.rowRange(n+m,n+2*m).colRange(n,n+m));  Zmm.copyTo(Final_temp.rowRange(n+m,n+2*m).colRange(n+m,n+2*m));

 //cout<<"Final_temp="<<endl<<Final_temp<<endl;

 Final_temp.copyTo(Final);

} // end of final

 

///////////////def RightVec /// RightVec=[- f- B(x).t()*y;---------RV_1: n*1 matrix ///       ue-W*Y*e;------------RV_2: m*1 matrix ///      -h(x)-w;]     ------------RV_3: m*1 matrix /// ///

//$$tested correctly // define   - f- B(x).t()*y; void setRightVec_1     (Mat& xx,      Mat& y_temp,      Mat& B_temp,      Mat& RightVec_1) {   Mat RightVec_1_temp=Mat_<double>(n,1);   //cout<<"B_temp="<<endl<<B_temp<<endl;   RightVec_1_temp= -f_Mat - B_temp.t()*y_temp;   RightVec_1_temp.copyTo(RightVec_1);   //cout<<"RV_1 is "<<endl<<RightVec_1<<endl; }

//$$tested correctly //ue-W*Y*e void setRightVec_2                (Mat& W_temp,                 Mat& Y_temp,        Mat& RightVec_2) {  Mat RightVec_2_temp=Mat_<double>(m,1);  RightVec_2_temp=u*e-(W_temp*Y_temp)*e;  RightVec_2_temp.copyTo(RightVec_2);    //cout<<"RV_2 is "<<endl<<RightVec_2<<endl; }

///////////////////////////////////// /// setRightVec_3 // -h(x)-w //call h_i //$$tested correctly void setRightVec_3                (Mat& xx,                 Mat& W_temp,        Mat& RightVec_3) {  Mat RightVec_3_temp=Mat_<double>(m,1);  int i=0;  for(i=0;i<m;i++)   RightVec_3_temp.at<double>(i,0)=-h_i(xx,i)-W_temp.at<double>(i,0);

 RightVec_3_temp.copyTo(RightVec_3);     //cout<<"RV_3 is "<<endl<<RightVec_3<<endl;

}

// define the whole RightVec=[RV_1; //              RV_2; //              RV_3;]

//$$tested correctly void setRightVec    (Mat& RightVec_1,      Mat& RightVec_2,      Mat& RightVec_3,      Mat& RightVec) {

  RightVec_1.copyTo(RightVec.rowRange(0,n));   RightVec_2.copyTo(RightVec.rowRange(n,n+m));   RightVec_3.copyTo(RightVec.rowRange(n+m,n+2*m));

      //cout<<"RV whole is "<<endl<<RightVec<<endl; }

 

 

 

 

 

 

// def a func of n dim vector xx

 

 

double(*h_ptr)(Mat&,int);

int main(void) {  initialize();

 Mat Variable_All_Old=Mat_<double>(2*m+n,1);  Mat Variable_All_New=Mat_<double>(2*m+n,1);  Mat Variable_All_Delta=Mat_<double>(2*m+n,1);

  x.copyTo(Variable_All_Old.rowRange(0,n));   w.copyTo(Variable_All_Old.rowRange(n,n+m));   y.copyTo(Variable_All_Old.rowRange(n+m,n+2*m));

 //iteration begins:

 int iter=1;

 while(iter<30)  {  // Variable_All_Old=[x;  //         w;  //         y;]

 

 Variable_All_Old.rowRange(0,n).copyTo(x);  Variable_All_Old.rowRange(n,n+m).copyTo(w);  Variable_All_Old.rowRange(n+m,n+2*m).copyTo(y);

 // don‘t forget updat Y and W . Big Mistake

 for(int s=0;s<m;s++)    Y.at<double>(s,s)=y.at<double>(s,0);  for(int t=0;t<m;t++)     W.at<double>(t,t)=w.at<double>(t,0);

 // oneStep iteration

 

 //test B_value  Mat B_value_temp=Mat_<double>(m,n);  B_value(x,B_value_temp);

 //test Hxy   Mat Hxy_temp=Mat_<double>(n,n);   Hxy(x, y, Hxy_temp);

 cout<<"/////////////////////////"<<endl;  Mat Final=Mat_<double>(n+2*m,n+2*m);  mergeToFinal(Hxy_temp,B_value_temp, Y,W,Final);//Big Mistake when debugging. here use Y, W , So they need to be updated every step.

 // now we get a final matrix. // now we have:  Hxy_temp, B_value_temp

 //test setRightVec_1  Mat RightVec_1=Mat_<double>(n,1);  setRightVec_1      ( x,        y,        B_value_temp,      RightVec_1);

 //test setRightVec_2  Mat RightVec_2=Mat_<double>(m,1);  setRightVec_2(W,       Y,     RightVec_2);

 //test setRightVec_3  Mat RightVec_3=Mat_<double>(m,1);  setRightVec_3                 ( x,                  W,         RightVec_3);

 //test  Mat RightVec=Mat_<double>(n+2*m,1);  setRightVec     ( RightVec_1,        RightVec_2,        RightVec_3,        RightVec);

 // end of set RightVec

 Variable_All_Delta=Final.inv() * RightVec;  Variable_All_New=Variable_All_Old+Variable_All_Delta;

 cout<<"Variable_All_Delta="<<endl<<Variable_All_Delta<<endl;  //update for next iter  Variable_All_Old=Variable_All_New;  iter++;

 }// end of while iter

 cout<<"final iter="<<iter<<endl;  cout<<"Variable_All"<<endl<<Variable_All_Old<<endl;

 

 

 

 //test Hxy  //Mat Hxy_temp=Mat_<double>(n,n);  //Hxy(x, y, Hxy_temp);

 //cout<<"Y is"<<Y<<endl;  //cout<<"W is"<<W<<endl;  //cout<<"I is "<<I<<endl; /*  // test rowRange, whether index from 0, or from 1;  // test result, index from 0.  there is a 0 row. There has a "Row 0".  // test result, rowRange(i,j) means that from "Row i to Row j-1"  // The same with colRange(i,j)  // This is a big hole.  Mat example=(Mat_<double>(3,5)<<1,2,3,4,5,6,7,8,9,10,11,12,13,14,15);  cout<<"example="<<example<<endl;  Mat row12example=example.rowRange(1,3);  cout<<"row12example="<<row12example<<endl;  Mat rowtt=(Mat_<double>(1,5)<<-1,-2,-3,-4,-5);  cout<<"rowtt"<<rowtt<<endl;  example.rowRange(0,1)=rowtt;//pointer  cout<<"after change ex is"<<example<<endl;  example.rowRange(1,2).copyTo(rowtt);  cout<<"after copyto, rowtt is"<<rowtt<<endl;  rowtt.copyTo(example.rowRange(2,3));  cout<<"after rowtt.copyTo(example.rowRange(2,3)), example is"<<example<<endl;  */  /*  n=5;  Mat pp=(Mat_ <double>(n,1)<<1,1,1,1,1);  cout<<"pp="<<endl<<pp<<endl;  double norm=EuclideanNorm(pp,n);  cout<<"norm="<<norm<<endl;

 vector< double(*)(Mat&,int)>hh;  hh.push_back(EuclideanNorm);  double normt=(*(hh[0]))(pp,n);  cout<<"normt="<<normt<<endl;

 int ki=3;  Mat AA=(Mat_<double>(ki,n));  for (int i=0;i<ki;i++)   for(int j=0;j<n;j++)   {    AA.at<double>(i,j)=i+j;   }  cout<<"AA"<<AA<<endl;  Mat cc=(Mat_<double>(n,1)<<1,-1,2,-2,3);  cout<<"cc"<<cc<<endl;  Mat bb=(Mat_<double>(ki,1)<<1,2,3);   Mat xx=pp;  double dd=10;

 double temp_normOfAxPlusb;  cout<<"xx="<<xx<<endl;  cout<<"AA="<<AA<<endl;  cout<<"bb="<<bb<<endl;  Mat tempMul=AA*xx;  cout<<"tempMul="<<tempMul<<endl;  int nn=5;  temp_normOfAxPlusb=normOfAxPlusb(xx,AA,bb,ki);     cout<<"temp_normOfAxPlusb="<<temp_normOfAxPlusb<<endl;

    double temp_hSingle=hSingle(xx,AA,bb,cc,dd,ki);     cout<<"temp_hSingle="<<temp_hSingle<<endl;

    // ////// test     Mat ttt=(Mat_<double>(2,3)<<1,2,3,4,5,6);     cout<<"ttt="<<ttt<<endl;

 */  /*  double a,b;  a=3;  b=firstOrderGradientOf1DFunction(a,sq);  cout<<"a="<<a<<endl;  cout<<"b="<<b<<endl;  */

 /*  ff=gg;  int b=3;  (*ff)(b);  cout<<b<<endl;

 b=calFunctionGx(b,gg);  cout<<b<<endl;*/

 

 /*  vector<Mat> AA(10);  for( int i=0;i<10;i++)   AA[i]=Mat::eye(i+1,i+1,CV_64F);

 for( int i=0;i<10;i++)   {cout<<AA[i]<<endl;  cout<<endl;   }*/

}

 

/////////////////////////////////////////////////////////// /// /////////////////////////////////////////////////////// /// ////////////////////////////////////////////////////// // test reserve area

/*cout<<"b="<<b<<endl;

 cout<<"c="<<c<<endl;

 cout<<d[0]<<"  "<<d[1]<<"  "<<d[2]<<endl;

 cout<<A[0]<<endl;  cout<<A[1]<<endl;  cout<<A[2]<<endl;  cout<<x<<endl;

//test h_i  double temph_0=h_i(x,0);  cout<<"temph_0="<<temph_0<<endl;  double temph_1=h_i(x,1);  cout<<"temph_1="<<temph_1<<endl;  Mat xt=x.clone();

 xt.at<double>(3,0)+=0.001;   temph_0=h_i(xt,0);  cout<<"temph_0="<<temph_0<<endl;

 h_ptr=h_i;  cout<<(*h_ptr)(x,1)<<endl;  cout<<(*h_ptr)(xt,1)<<endl;*/

//test  firstOrderGradientOfh_iOverx_t  /*  double firstOrderGradientOfh_iOverx_t  ( double(*hh_i)(Mat&,int),    Mat& xx,    int i,    int t)*/

 

//test merge to a big matrix from different small matrix. /* Mat st1=(Mat_<double>(1,3)<<1,-2,3);  cout<<"st1="<<st1<<endl;  Mat st2=(Mat_<double>(2,2)<<20,21,22,23);  cout<<"st2="<<st2<<endl;  Mat st3=(Mat_<double>(3,3)<<0,0,0,0,0,0,0,0,0);  st1.copyTo(st3.rowRange(0,1));  st2.copyTo(st3.rowRange(1,3).colRange(1,3));  cout<<"after merge, st3 is"<<endl;  cout<<st3<<endl;

 Zmm.copyTo(st3);  cout<<st3<<endl;

 cout<<Zmm<<endl;  cout<<Znm<<endl;  cout<<Zmn<<endl;*/

/*double tempdh0dx2;  tempdh0dx2=firstOrderGradientOfh_iOverx_t(h_i,x,1,4);  cout<<"tempdh0dx2="<<tempdh0dx2<<endl;*/

/*//test secondOrder()

 //double secondOrderGradientOfh_ioverx_sx_t  //  (double (*ptr_firstOrderGradientOfh_iOverx_t) (double(*)(Mat&,int),Mat&, int,int),  //     Mat& xx,  //     int i,  //     int s,  //     int t)

 cout<<"sec"<<endl;  cout<<"x="<<x<<endl;  cout<<secondOrderGradientOfh_ioverx_sx_t(firstOrderGradientOfh_iOverx_t, x,1,0,3)<<endl; */

/*  //  Mat HessianMatrix_hi_temp=Mat_<double>(n,n);  cout<<"x="<<x<<endl;  cout<<"b"<<b<<endl;  cout<<"c"<<c<<endl;  cout<<"A[2]"<<A[2]<<endl;  HessianMatrix(x,2, HessianMatrix_hi_temp);

 */

/*  // test B(x)  //Mat B_temp=Mat_<double>(m,n);  //B(x,B_temp);  //cout<<"B_temp"<<endl<<B_temp<<endl;

 */

/*

void(*ff)(int&); double(*dd)(double&);

 

void gg(int& a) {  a=a+1; }

double sq (double& x) {  return x*x; }

 */ /*

double calFunctionGx (int t, void(*f)(int&)) {  int y;  y=t*t;  (*f)(y);  return y; }

 */

/* // ////////////     List of Function vector< double(*)(Mat& xx,Mat& AA, Mat&bb, Mat&cc, double dd,int ki,int index_i)>h;// condition h , m-vector, every hi is a function of vector x.               // Mat& is Mat x.               // n is size.               // index denote i.

               */

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

/////////////// version 2 of B(x), which relys on exact formula // hard to work Mat Bx=Mat_<double>(m,n);

//def B(x) // row i and column j of B(x) in math  is B(x).at(i-1,j-1)=dhi/dxj in C++ Mat. for starting from 0,1 2 3... // B(x).at(i-1,t-1)=dhi/dxt // here didn‘t call diff, here use the exact fomula.

 

void B    (Mat& xx,        // xx is a Mat, n*1 matrix.     Mat& BxTemp) {  int i=0;  int t=0;

 int j=0;  int kk=0; // this k is not the global k, just a Psudoindex

 //cout<<"m="<<m<<endl;  //cout<<"n="<<n<<endl;  // cal dhi/dxt

 

 for (i=0;i<m;i++)   for(t=0;t<n;t++)   {

   double P;    double Q;

   double totalSumP=0;

   /* 4 steps：     * （1） T[k]=sumOver_j(Ai,kj*xj)  +bi,k ;  (b in math)                                    sum_k=sumOver_j(Ai,kj*xj);                                    so, T[k]=sum_k + bi,k;                  (2)  P= sumOver_k(T[k]*Ai,kt);                  (3)  Q=sqrt(sumOver_k(T[k]*T[k]));                  (4)  dhi/dxt= P/Q  -  ci,t

 

    */    // cal P:

   double *T=new double[k[i]];    //cout<<"k[i]"<<k[i]<<endl;    double sum_TkSquared=0;

 for (kk=0;kk<k[i];kk++)  {

    double sum_k=0;

   for(j=0;j<n;j++)    {     cout<<"j="<<j<<endl;     cout<<"A["<<i<<"].at("<<kk<<","<<j<<")="<<A[i].at<double>(kk,j)<<endl;

    sum_k=sum_k+A[i].at<double>(kk,j)*xx.at<double>(j,0);    //cout<<"j="<<j<<"   "<<"A[i].at<double>(kk,j)"<<endl<<A[i].at<double>(kk,j)<<endl;

   }// end of for j

   //cout<<"kk="<<kk<<endl;    //cout<<"sum_k="<<sum_k<<endl;    sum_k=sum_k+b.at<double>(kk,i); // notice row and column of b is different. row index is column. colum is row.

   T[kk]=sum_k;

   double sum_Tk;    sum_Tk=T[kk]*A[i].at<double>(kk,t);

   totalSumP=totalSumP+sum_Tk;

   //cal Q

 

       sum_TkSquared=sum_TkSquared+T[kk]*T[kk];        cout<<"T["<<kk<<"]="<<T[kk]<<endl;

   }// end of for kk

   P=totalSumP;    //cout<<"P="<<P<<endl;

   Q=sqrt(sum_TkSquared);    // end of cal P;

   // cal Q

 

 

 

   //cout<<"Q="<<Q<<endl;    // end of cal Q

 

   //cal dhi/dxt

   BxTemp.at<double>(i,t)=P/Q - c.at<double>(t,i);

   cout<<"i="<<i<<endl;    cout<<"t="<<t<<endl;

  }// end of for i,t  // end of  cal B(x)

}

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

void TestLP(std::string path) {  LP_Interface m_cFlow;  ST_LP_INPUTINFO stParam;  stParam.fFps = 30;  stParam.nImgH = 800;  stParam.nImgW = 1280;  stParam.nBpp  = 24;

 m_cFlow.SetConfig(&stParam);

 std::string outputdir = "resource/debugImg/";

 for (int i = 0; i < 4000; i ++)  {   if (i == 336)   {    int ss = 1;   }

  std::string filename = getFileName(path, i);   cv::Mat image = cv::imread(path + filename);   m_cFlow.TrackLP(&image, i, i / stParam.fFps, NULL, NULL);

  // use the frame result  data   int                 nFrmNum    = 0;   ST_LP_FRAME_LaneSet*   pstFrmRlt  = NULL;   m_cFlow.GetTrackResult(nFrmNum);

  for (int i = 0; i < nFrmNum; i++)   {    m_cFlow.GetTrackResult(i, &pstFrmRlt);

   cv::Mat* pbyMask = NULL;    m_cFlow.GetTrackResult(&pbyMask);

   //dilate for show    cv::Mat element = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(5, 5));    cv::Mat temp(*pbyMask);    cv::dilate(temp, *pbyMask, element, cv::Point(-1,-1), 1);

   int nOutFrmInd = pstFrmRlt->nFrameInd;    stringstream ss;    ss << nOutFrmInd;

   std::string fileNameO = ss.str()+".png";    std::string savepath = outputdir + fileNameO;    cv::imwrite(savepath, *pbyMask);   }  } }

std::string getFileName(std::string path, int idx) {  int FrameNum = idx;

 stringstream ss;  ss << FrameNum;

 std::string fileName = ss.str()+".jpg";

 return fileName; }