draft code of SOCP based on .Mat

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
#include <opencv2/highgui/highgui.hpp> <br>#include "LoadInfo.h"<br> #include "GroundPlaneEstimation.h" <br>#include <fstream> <br>#include <iomanip><br> #include "config.h"
 
  
 
using namespace cv; using namespace std;
 
#include "LP_Interface.h" <br>std::string getFileName(std::string path, int idx); <br>void TestLP(std::string path);
 
  
 
  
 
  
 
  
 
  
 
  
 
  
 
  
 
  
 
  
 
  
 
  
 
  
 
  
 
  
 
// ///////////////////SOCP workspace starting here // ///////////////////2016.5.10
 
#include<math.h>
 
// //////Dimension Size int n=5; <br> // size of X,  ---n dimension vector input int m=3; <br>// condition number <br><br>int* k=new int[m];<br>//k[i] denote ki, size of k[] is m.
 
// /////input data <br>Mat x=(Mat_ <double>(n,1)); <br>double* f=new double[n]; // f is a column vector of size n <br>Mat f_Mat=(Mat_ <double>(n,1)); <br>Mat b=(Mat_<double>(n,m)); // notice column and row are different written, they are exchanged. Big Notice here.<br> Mat c=(Mat_ <double>(n,m)); <br>double* d=new double[m]; <br>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 <br>Mat Y=Mat_<double>(m,m); // diagonal matrix of y <br>Mat w=Mat_<double>(m,1);//column vector<br> 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);<br> Mat Zmn=Mat_<double>(m,n); <br>Mat Zmm=Mat_<double>(m,m);
 
//def e <br>Mat e=Mat_<double>(m,1);<br> void sete() {  int i=0;  for(i=0;i<m;i++)   e.at<double>(i,0)=1; }
 
//tested correctly <br>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;
 
}<br> //tested correctly <br>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<br> 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;
 
} <br>//tested correctly <br>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;
 
} <br><br>//tested correctly <br>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 @   steven_xiu  阅读(135)  评论(0编辑  收藏  举报
努力加载评论中...
点击右上角即可分享
微信分享提示