#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)
}