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


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