# draft code of SOCP based on .Mat

#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;*/

/*//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)

/*  //  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)

}



posted @ 2016-06-04 02:40  steven_xiu  阅读(127)  评论(0编辑  收藏  举报