00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012 #include <iostream>
00013 #include <string>
00014 #include<fstream>
00015 #ifndef WIN32
00016 #include <sys/stat.h>
00017 #include <sys/types.h>
00018 #include <errno.h>
00019 #include <sys/time.h>
00020 #include <sys/resource.h>
00021 #else
00022 #include <errno.h>
00023 #include <time.h>
00024 #endif
00025
00026 #include "opencv/cv.h"
00027 #include "opencv/cxcore.h"
00028 #include "opencv/highgui.h"
00029
00030
00031 #include "options.h"
00032 #include "utils/libutils.h"
00033
00034
00035
00036 using namespace cv;
00037 using namespace std;
00038
00039 clock_t _timer_starttime;
00040 #ifndef WIN32
00041 struct rusage _timer_rusage;
00042 #endif
00043
00044
00045
00046 static int imaxarg1,imaxarg2;
00047 #define IMAX(a,b) (imaxarg1=(a),imaxarg2=(b),(imaxarg1) > (imaxarg2) ?\
00048 (imaxarg1) : (imaxarg2))
00049 #define ABCSIGN(a,b) ((b) >= 0.0 ? fabs(a) : -fabs(a))
00050
00051
00052 void dumpMatSize(const Mat &m) {
00053 cout << "dims=" << m.dims << " (" << m.rows << " x " << m.cols << ")";
00054 cout << " :: size:";
00055 for (int n=0; n<m.dims; n++) {
00056 cout << m.size[n] << " ";
00057 }
00058 cout << ":: step: ";
00059 for (int n=0; n<m.dims; n++) {
00060 cout << m.step[n] << " ";
00061 }
00062 cout << ":: addr: " << (void *)(m.data);
00063 cout << ":: type: " << m.type();
00064 cout << "\n";
00065 }
00066
00067
00068
00069 void scaleRows(Mat &m, const Mat &w) {
00070 CV_Assert(m.rows == w.rows*w.cols);
00071 for (int i=0; i<m.rows; i++)
00072 m.row(i) *= w.at<OPT_F_TYPE>(i);
00073 }
00074
00075
00076 void scaleCols(Mat &m, const Mat &w) {
00077 CV_Assert(m.cols == w.rows*w.cols);
00078 for (int j=0; j<m.cols; j++)
00079 m.col(j) *= w.at<OPT_F_TYPE>(j);
00080 }
00081
00082
00083 ostream& operator <<(ostream &output, const Range &r) {
00084 if (r == Range::all()) {
00085 output << "[:]";
00086 } else {
00087 output << "[" << r.start << ":" << r.end << "]";
00088 }
00089 return output;
00090 }
00091
00092
00093
00094 void timer_start() {
00095 _timer_starttime = clock();
00096 }
00097
00098
00099 double timer_stop() {
00100 clock_t dur = clock() - _timer_starttime;
00101 return (double)dur/CLOCKS_PER_SEC;
00102 }
00103
00104 void timer_print() {
00105 cout << "Elapsed Time: " << timer_stop() << " seconds\n";
00106 }
00107
00108
00109
00110
00111 void timerOn()
00112 {
00113 #ifdef WIN32
00114 timer_start();
00115 #else
00116 getrusage(RUSAGE_SELF, &_timer_rusage);
00117 #endif
00118 }
00119
00120
00121 int timerOff()
00122 {
00123 #ifdef WIN32
00124 return (int)(timer_stop()*1000);
00125 #else
00126 int t1, t2, u1, u2;
00127 struct rusage end;
00128 getrusage(RUSAGE_SELF, &end);
00129 t1 = end.ru_utime.tv_sec - _timer_rusage.ru_utime.tv_sec;
00130 t2 = end.ru_stime.tv_sec - _timer_rusage.ru_stime.tv_sec;
00131
00132 u1 = end.ru_utime.tv_usec - _timer_rusage.ru_utime.tv_usec;
00133 u2 = end.ru_stime.tv_usec - _timer_rusage.ru_stime.tv_usec;
00134
00135 return (t1+t2)*1000 + (u1+u2)/1000;
00136 #endif
00137 }
00138
00139 void timerPrint() {
00140 int t = timerOff();
00141 cout << "Elapsed Time: " << (double)t/1000 << " seconds\n";
00142 }
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156
00157
00158
00159
00160
00161
00162
00163
00164
00165
00166
00167
00168
00169
00170 void strtok2(string str, string del,string& key, string& valuestr)
00171 {
00172 int ind=str.find(del,0);
00173 if(ind==string::npos)
00174 {
00175 key=str;
00176 valuestr="";
00177 }
00178 else
00179 {
00180 key=str.substr(0,ind);
00181 valuestr=str.substr(ind+1,str.length());
00182 }
00183 }
00184
00185 int checkOptionInStr(string str,string opt)
00186 {
00187 int j=str.find(opt);
00188 if(j>=0)
00189 return 1;
00190 else
00191 return 0;
00192 }
00193
00194
00195 OPT_F_TYPE computeVariance3dDouble(Mat input)
00196 {
00197 OPT_F_TYPE sum=0;
00198 OPT_F_TYPE count=0;
00199 OPT_F_TYPE mean,var;
00200 for(int i=0;i<input.size[0];i++)
00201 for(int j=0;j<input.size[1];j++)
00202 for(int k=0;k<input.size[2];k++)
00203 {
00204 count=count+1;
00205 sum=sum+(OPT_F_TYPE)input.at<OPT_F_TYPE>(i,j,k);
00206 }
00207
00208 mean=sum/count;
00209
00210 sum=0;
00211 for(int i=0;i<input.size[0];i++)
00212 for(int j=0;j<input.size[1];j++)
00213 for(int k=0;k<input.size[2];k++)
00214 {
00215
00216 sum += ((OPT_F_TYPE)input.at<OPT_F_TYPE>(i,j,k) - mean) * ((OPT_F_TYPE)input.at<OPT_F_TYPE>(i,j,k) - mean) ;
00217 }
00218
00219 var=sum/(count-1);
00220 return var;
00221
00222 }
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244
00245
00246
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266
00267
00268
00269
00270
00271
00272
00273
00274
00275 char* writeMatrix(char *mptr,Mat &mtx)
00276 {
00277 char *ptr=mptr;
00278
00279 int type=mtx.type();
00280 int rows=mtx.rows;
00281 int cols=mtx.cols;
00282 int els=mtx.elemSize();
00283
00284 int tsize=sizeof(int)*4+ (rows*cols*els);
00285 memcpy(ptr,&tsize,4);
00286 ptr+=4;
00287
00288 memcpy(ptr,&type,4);
00289 ptr+=4;
00290
00291 memcpy(ptr,&rows,4);
00292 ptr+=4;
00293
00294 memcpy(ptr,&cols,4);
00295 ptr+=4;
00296
00297 memcpy(ptr,&els,4);
00298 ptr+=4;
00299
00300
00301 double tmpD;
00302 for(int i=0;i<rows;i++)
00303 {
00304 for(int j=0;j<cols;j++)
00305 {
00306 switch(type)
00307 {
00308 case CV_64F:
00309 tmpD=mtx.at<double>(i,j);
00310 memcpy(ptr,&tmpD,8);
00311 ptr+=8;
00312 break;
00313 default:
00314 CV_Error(-1,"type not handled yet");
00315 }
00316 }
00317 }
00318
00319 return ptr;
00320 }
00321
00322 char* readMatrix(char *mptr,Mat &mtx)
00323 {
00324 char *ptr=mptr;
00325
00326 int type;
00327 int rows;
00328 int cols;
00329 int els;
00330
00331 memcpy(&type,ptr,4);
00332 ptr+=4;
00333
00334 memcpy(&rows,ptr,4);
00335 ptr+=4;
00336
00337 memcpy(&cols,ptr,4);
00338 ptr+=4;
00339
00340 memcpy(&els,ptr,4);
00341 ptr+=4;
00342
00343 mtx.create(rows,cols,type);
00344
00345
00346 double tmpD;
00347 for(int i=0;i<rows;i++)
00348 {
00349 for(int j=0;j<cols;j++)
00350 {
00351 switch(type)
00352 {
00353 case CV_64F:
00354 memcpy(&tmpD,ptr,8);
00355 ptr+=8;
00356 mtx.at<double>(i,j)=tmpD;
00357 break;
00358 default:
00359 CV_Error(-1,"type not handled yet");
00360 }
00361 }
00362 }
00363
00364 return ptr;
00365 }
00366
00367
00368
00373 void elmhes(Mat &a, int n)
00374 {
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384 int m,j,i;
00385 double y,x;
00386 for (m=2;m<n;m++)
00387 {
00388 x=0.0;
00389 i=m;
00390 for (j=m;j<=n;j++)
00391 {
00392 if (fabs(a.at<OPT_F_TYPE>(j,m-1)) > fabs(x))
00393 {
00394 x=a.at<OPT_F_TYPE>(j,m-1);
00395 i=j;
00396 }
00397 }
00398 if (i != m)
00399 {
00400 for (j=m-1;j<=n;j++)
00401 {
00402 double temp=a.at<OPT_F_TYPE>(i,j);
00403 a.at<OPT_F_TYPE>(i,j)=a.at<OPT_F_TYPE>(m,j);
00404 a.at<OPT_F_TYPE>(m,j)=temp;
00405 }
00406 for (j=1;j<=n;j++)
00407 {
00408 double temp=a.at<OPT_F_TYPE>(j,i);
00409 a.at<OPT_F_TYPE>(j,i)=a.at<OPT_F_TYPE>(j,m);
00410 a.at<OPT_F_TYPE>(j,m)=temp;
00411 }
00412 }
00413 if (x)
00414 {
00415 for (i=m+1;i<=n;i++)
00416 {
00417 if ((y=a.at<OPT_F_TYPE>(i,m-1)) != 0.0)
00418 {
00419 y /= x;
00420 a.at<OPT_F_TYPE>(i,m-1)=y;
00421 for (j=m;j<=n;j++)
00422 a.at<OPT_F_TYPE>(i,j)=a.at<OPT_F_TYPE>(i,j) - y*a.at<OPT_F_TYPE>(m,j);
00423 for (j=1;j<=n;j++)
00424 a.at<OPT_F_TYPE>(j,m)=a.at<OPT_F_TYPE>(j,m) + y*a.at<OPT_F_TYPE>(j,i);
00425 }
00426 }
00427 }
00428 }
00429 }
00430
00431
00432
00433
00434
00435
00436
00442 int hqr(Mat &a, int n, std::vector<double> &wr, std::vector<double> &wi) {
00443
00444
00445
00446
00447
00448
00449
00450 int nn,m,l,k,j,its,i,mmin;
00451 double z,y,x,w,v,u,t,s,r,q,p,anorm;
00452
00453 r=0; q=0; p=0;
00454
00455 anorm=0.0;
00456 for (i=1;i<=n;i++)
00457 for (j=IMAX(i-1,1);j<=n;j++)
00458 anorm += fabs(a.at<OPT_F_TYPE>(i,j));
00459 nn=n;
00460 t=0.0;
00461 while (nn >= 1) {
00462 its=0;
00463 do {
00464 for (l=nn;l>=2;l--) {
00465 s=fabs(a.at<OPT_F_TYPE>(l-1,l-1))+fabs(a.at<OPT_F_TYPE>(l,l));
00466 if (s == 0.0) s=anorm;
00467 if ((double)(fabs(a.at<OPT_F_TYPE>(l,l-1)) + s) == s) break;
00468 }
00469 x=a.at<OPT_F_TYPE>(nn,nn);
00470 if (l == nn) {
00471 wr[nn]=x+t;
00472 wi[nn--]=0.0;
00473 } else {
00474 y=a.at<OPT_F_TYPE>(nn-1,nn-1);
00475 w=a.at<OPT_F_TYPE>(nn,nn-1)*a.at<OPT_F_TYPE>(nn-1,nn);
00476 if (l == (nn-1)) {
00477 p=0.5*(y-x);
00478 q=p*p+w;
00479 z=sqrt(fabs(q));
00480 x += t;
00481 if (q >= 0.0) {
00482 z=p+ABCSIGN(z,p);
00483 wr[nn-1]=wr[nn]=x+z;
00484 if (z) wr[nn]=x-w/z;
00485 wi[nn-1]=wi[nn]=0.0;
00486 } else {
00487 wr[nn-1]=wr[nn]=x+p;
00488 wi[nn-1]= -(wi[nn]=z);
00489 }
00490 nn -= 2;
00491 } else {
00492
00493 if (its == 1000) {
00494 fprintf(stderr, "hqr: too many iterations\n");
00495 return 1;
00496
00497 }
00498 if (its == 10 || its == 20) {
00499 t += x;
00500 for (i=1;i<=nn;i++) a.at<OPT_F_TYPE>(i,i) -= x;
00501 s=fabs(a.at<OPT_F_TYPE>(nn,nn-1))+fabs(a.at<OPT_F_TYPE>(nn-1,nn-2));
00502 y=x=0.75*s;
00503 w = -0.4375*s*s;
00504 }
00505 ++its;
00506 for (m=(nn-2);m>=l;m--) {
00507
00508 z=a.at<OPT_F_TYPE>(m,m);
00509 r=x-z;
00510 s=y-z;
00511 p=(r*s-w)/a.at<OPT_F_TYPE>(m+1,m)+a.at<OPT_F_TYPE>(m,m+1);
00512 q=a.at<OPT_F_TYPE>(m+1,m+1)-z-r-s;
00513 r=a.at<OPT_F_TYPE>(m+2,m+1);
00514 s=fabs(p)+fabs(q)+fabs(r);
00515 p /= s;
00516 q /= s;
00517 r /= s;
00518 if (m == l) break;
00519 u=fabs(a.at<OPT_F_TYPE>(m,m-1))*(fabs(q)+fabs(r));
00520 v=fabs(p)*(fabs(a.at<OPT_F_TYPE>(m-1,m-1))+fabs(z)+fabs(a.at<OPT_F_TYPE>(m+1,m+1)));
00521 if ((double)(u+v) == v) break;
00522 }
00523 for (i=m+2;i<=nn;i++) {
00524 a.at<OPT_F_TYPE>(i,i-2)=0.0;
00525 if (i != (m+2)) a.at<OPT_F_TYPE>(i,i-3)=0.0;
00526 }
00527 for (k=m;k<=nn-1;k++) {
00528
00529 if (k != m) {
00530 p=a.at<OPT_F_TYPE>(k,k-1);
00531 q=a.at<OPT_F_TYPE>(k+1,k-1);
00532 r=0.0;
00533 if (k != (nn-1)) r=a.at<OPT_F_TYPE>(k+2,k-1);
00534 if ((x=fabs(p)+fabs(q)+fabs(r)) != 0.0) {
00535 p /= x;
00536 q /= x;
00537 r /= x;
00538 }
00539 }
00540 if ((s=ABCSIGN(sqrt(p*p+q*q+r*r),p)) != 0.0) {
00541 if (k == m) {
00542 if (l != m)
00543 a.at<OPT_F_TYPE>(k,k-1) = -a.at<OPT_F_TYPE>(k,k-1);
00544 } else
00545 a.at<OPT_F_TYPE>(k,k-1) = -s*x;
00546 p += s;
00547 x=p/s;
00548 y=q/s;
00549 z=r/s;
00550 q /= p;
00551 r /= p;
00552 for (j=k;j<=nn;j++) {
00553 p=a.at<OPT_F_TYPE>(k,j)+q*a.at<OPT_F_TYPE>(k+1,j);
00554 if (k != (nn-1)) {
00555 p += r*a.at<OPT_F_TYPE>(k+2,j);
00556 a.at<OPT_F_TYPE>(k+2,j) -= p*z;
00557 }
00558 a.at<OPT_F_TYPE>(k+1,j) -= p*y;
00559 a.at<OPT_F_TYPE>(k,j) -= p*x;
00560 }
00561 mmin = nn<k+3 ? nn : k+3;
00562 for (i=l;i<=mmin;i++) {
00563 p=x*a.at<OPT_F_TYPE>(i,k)+y*a.at<OPT_F_TYPE>(i,k+1);
00564 if (k != (nn-1)) {
00565 p += z*a.at<OPT_F_TYPE>(i,k+2);
00566 a.at<OPT_F_TYPE>(i,k+2) -= p*r;
00567 }
00568 a.at<OPT_F_TYPE>(i,k+1) -= p*q;
00569 a.at<OPT_F_TYPE>(i,k) -= p;
00570 }
00571 }
00572 }
00573 }
00574 }
00575 } while (l < nn-1);
00576 }
00577 return 0;
00578 }
00579
00580
00581
00582
00583
00592 Mat eigc_abs(Mat &W) {
00593 Mat A;
00594 int i;
00595 A.create(W.rows,1,OPT_MAT_TYPE);
00596
00597 for (i=0; i<W.rows; i++)
00598 A.at<OPT_F_TYPE>(i,0) = sqrt(W.at<OPT_F_TYPE>(i,0)*W.at<OPT_F_TYPE>(i,0)+W.at<OPT_F_TYPE>(i,1)*W.at<OPT_F_TYPE>(i,1));
00599 return A;
00600 }
00601
00602
00603
00604
00614 Mat eigc(Mat &X)
00615 {
00616 int i, j, n, n1;
00617 Mat X_n;
00618 std::vector<double> wr_n;
00619 std::vector<double> wi_n;
00620 Mat w;
00621
00622 if (X.rows != X.cols)
00623 CV_Error(-1,"eigc: X not symmetric!\n");
00624
00625 n = X.rows;
00626 n1 = n+1;
00627
00628
00629
00630 X_n.create(n1,n1,OPT_MAT_TYPE);
00631 X_n.setTo(0);
00632 wr_n.resize(n1,0);
00633 wi_n.resize(n1,0);
00634
00635
00636
00637 for (i=0; i<n; i++)
00638 for (j=0; j<n; j++)
00639 X_n.at<OPT_F_TYPE>(i+1,j+1)=X.at<OPT_F_TYPE>(i,j);
00640
00641
00642 elmhes(X_n, n);
00643
00644 w.create(n,2,OPT_MAT_TYPE);
00645
00646
00647 if ( !hqr(X_n, n, wr_n, wi_n) )
00648 {
00649
00650 for (i=0; i<n; i++) {
00651 w.at<OPT_F_TYPE>(i,0) = wr_n[i+1];
00652 w.at<OPT_F_TYPE>(i,1) = wi_n[i+1];
00653 }
00654
00655 }
00656 else
00657 {
00658
00659
00660
00661
00662 CV_Error(-1,"No other methods to try");
00663 }
00664
00665
00666 return w;
00667 }
00668
00669
00670
00671 void exitWithError(const std::string &error)
00672 {
00673 std::cout << error;
00674 std::cin.ignore();
00675 std::cin.get();
00676
00677 exit(EXIT_FAILURE);
00678 }
00679
00680
00681 template <>
00682 std::string Convert::string_to_T<std::string>(std::string const &val)
00683 {
00684 return val;
00685 }
00686
00687
00688
00689
00690
00691
00692
00693