00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013 #include <iostream>
00014 #include <string>
00015 #include<fstream>
00016 #include<math.h>
00017 #ifdef WIN32
00018
00019 #define _USE_MATH_DEFINES
00020
00021 #include <cmath>
00022
00023 #endif
00024
00025 #include "opencv/cv.h"
00026 #include "opencv/cxcore.h"
00027 #include "opencv/highgui.h"
00028
00029 #include "options.h"
00030 #include "utils/libutils.h"
00031 #include "video/libvideo.h"
00032 #include "stats/libstats.h"
00033 #include "dytex/DytexOptions.hpp"
00034 #include "dytex/DytexRegOptions.hpp"
00035 #include "dytex/Dytex.hpp"
00036 #include "dytex/DytexKalmanFilter.hpp"
00037
00038 using namespace cv;
00039 using namespace std;
00040
00041
00042
00043
00044
00045
00046
00047 DytexKalmanFilter::DytexKalmanFilter(Dytex &dt, int tau)
00048 {
00049
00050 cache_Kt = MatVid::create(tau, dt.dtopt.n, dt.dtopt.m, OPT_MAT_TYPE);
00051 cache_detMt = Mat(1, tau, OPT_MAT_TYPE);
00052 cache_invMt = MatVid::create(tau, dt.dtopt.m, dt.dtopt.m, OPT_MAT_TYPE);
00053 cache_dt = &dt;
00054
00055
00056 loglike_internal(dt, NULL, NULL, tau, &cache_Kt, &cache_detMt, &cache_invMt, false, KF_CACHE);
00057 }
00058
00059
00060 DytexKalmanFilter::~DytexKalmanFilter() {
00061 }
00062
00063 void DytexKalmanFilter::loglike(const vector<Mat> &videos, Mat &lik, bool mahal) {
00064 loglike_internal(*cache_dt, &videos, &lik, 0, &cache_Kt, &cache_detMt, &cache_invMt, mahal, KF_COMPUTE_WITH_CACHE);
00065 }
00066
00067 double DytexKalmanFilter::loglike(const Mat &video, bool mahal) {
00068 vector<Mat> vs(1, video);
00069 Mat lik(1, 1, OPT_MAT_TYPE);
00070 loglike(vs, lik, mahal);
00071 return lik.at<OPT_F_TYPE>(0);
00072 }
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082 void DytexKalmanFilter::loglike_old(const Dytex &dt, const vector<Mat> &videos, Mat &lik, bool mahal) {
00083
00084
00085 int YN = videos.size();
00086
00087 Mat* Y = new Mat[YN];
00088
00089 int tau=0;
00090 for (int i=0; i<YN; i++) {
00091 dt.processVideoTesting(videos[i], Y[i]);
00092
00093
00094
00095 if (i==0)
00096 tau = Y[i].cols;
00097 else
00098 CV_Assert(tau == Y[i].cols);
00099 }
00100
00101
00102 CovMatrix iR = dt.R.inv();
00103 Mat iRf = iR.toFullMatrix();
00104 Mat RC;
00105 iR.postmultiply(dt.C, RC);
00106 Mat CRC = dt.C.t() * RC;
00107
00108
00109 Mat Vt1t = dt.S0.toFullMatrix();
00110 Mat Vtt(dt.dtopt.n, dt.dtopt.n, OPT_MAT_TYPE);
00111 Mat xt1t;
00112 if (YN == 1)
00113 dt.mu0.copyTo(xt1t);
00114 else
00115 repeat(dt.mu0, 1, YN, xt1t);
00116 Mat xtt(dt.dtopt.n, YN, OPT_MAT_TYPE);
00117 Mat Kt(dt.dtopt.n, dt.dtopt.m, OPT_MAT_TYPE);
00118 lik.create(1, YN, OPT_MAT_TYPE);
00119 lik = 0.0;
00120 Mat term2(1, YN, OPT_MAT_TYPE);
00121 Mat YCx(dt.dtopt.m, 1, OPT_MAT_TYPE);
00122
00123 Mat icv(dt.dtopt.m, dt.dtopt.m, OPT_MAT_TYPE);
00124
00125
00126 for (int t=0; t<tau; t++) {
00127
00128 if (t>0) {
00129
00130 Vt1t = dt.A*Vtt*dt.A.t() + dt.Q.mtx;
00131
00132
00133 xt1t = dt.A*xtt;
00134 }
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144 switch(dt.dtopt.Ropt) {
00145 case CovMatrix::COV_IID:
00146 case CovMatrix::COV_DIAG:
00147
00148 if (countNonZero(Vt1t) == 0) {
00149
00150 iRf.copyTo(icv);
00151 } else {
00152
00153 Mat tmpS = (Vt1t.inv()+CRC).inv();
00154
00155 icv = iRf - RC*tmpS*RC.t();
00156 }
00157 break;
00158 case CovMatrix::COV_FULL:
00159
00160 icv = (dt.C*Vt1t*dt.C.t() + dt.R.mtx).inv();
00161 break;
00162 }
00163
00164
00165 Kt = Vt1t*dt.C.t()*icv;
00166
00167
00168 Vtt = Vt1t - Kt*dt.C*Vt1t;
00169
00170
00171 for (int i=0; i<YN; i++) {
00172
00173 YCx = Y[i].col(t) - dt.C*xt1t.col(i);
00174
00175
00176 xtt.col(i) = xt1t.col(i) + Kt*YCx;
00177
00178
00179
00180 term2.col(i) = YCx.t()*icv*YCx;
00181 }
00182
00183
00184 if (mahal) {
00185 lik += term2;
00186
00187 } else {
00188
00189
00190
00191 double term1 = 0.5*log(determinant(icv));
00192
00193
00194
00195 term1 -= 0.5*(dt.dtopt.m*log(2*M_PI));
00196
00197
00198 lik += -0.5*term2 + term1;
00199 }
00200
00201 }
00202
00203
00204 if( Y != NULL ) delete [] Y;
00205 }
00206
00207 double DytexKalmanFilter::loglike(const Dytex &dt, const Mat &video, bool mahal) {
00208 vector<Mat> vs(1, video);
00209 Mat lik(1, 1, OPT_MAT_TYPE);
00210 loglike(dt, vs, lik, mahal);
00211 return lik.at<OPT_F_TYPE>(0);
00212 }
00213
00214 void DytexKalmanFilter::loglike(const Dytex &dt, const vector<Mat> &videos, Mat &lik, bool mahal) {
00215 loglike_internal(dt, &videos, &lik, 0, NULL, NULL, NULL, mahal, KF_COMPUTE_WITHOUT_CACHE);
00216 }
00217
00218
00219
00220
00221
00222
00223
00224
00225
00226
00227
00228
00229 void DytexKalmanFilter::loglike_internal(const Dytex &dt, const vector<Mat> *videos, Mat *lik,
00230 int cache_tau, Mat *cache_Kt, Mat *cache_detMt, Mat *cache_invMt,
00231 bool mahal, kf_mode kfmode) {
00232
00233
00234 int YN = (kfmode == KF_CACHE ? 0 : videos->size());
00235
00236 Mat* Y = new Mat[YN];
00237
00238 int tau = 0;
00239
00240 if (kfmode == KF_CACHE)
00241 tau = cache_tau;
00242
00243 else {
00244
00245 for (int i=0; i<YN; i++) {
00246 dt.processVideoTesting((*videos)[i], Y[i]);
00247
00248
00249
00250 if (i==0)
00251 tau = Y[i].cols;
00252 else
00253 CV_Assert(tau == Y[i].cols);
00254 }
00255
00256
00257 if (kfmode == KF_COMPUTE_WITH_CACHE)
00258 CV_Assert(tau <= (*cache_Kt).size[0]);
00259 }
00260
00261
00262 Mat iRf, CRC, RC;
00263 if (kfmode == KF_CACHE || kfmode == KF_COMPUTE_WITHOUT_CACHE) {
00264 CovMatrix iR = dt.R.inv();
00265 iRf = iR.toFullMatrix();
00266 iR.postmultiply(dt.C, RC);
00267 CRC = dt.C.t() * RC;
00268 }
00269
00270
00271 Mat Vt1t = dt.S0.toFullMatrix();
00272 Mat Vtt(dt.dtopt.n, dt.dtopt.n, OPT_MAT_TYPE);
00273
00274
00275
00276 Mat xt1t, xtt, term2, YCx, Kt, icv;
00277 if (kfmode == KF_COMPUTE_WITH_CACHE || kfmode == KF_COMPUTE_WITHOUT_CACHE) {
00278
00279 if (YN == 1)
00280 dt.mu0.copyTo(xt1t);
00281 else
00282 repeat(dt.mu0, 1, YN, xt1t);
00283
00284 xtt = Mat(dt.dtopt.n, YN, OPT_MAT_TYPE);
00285 lik->create(1, YN, OPT_MAT_TYPE);
00286 (*lik) = 0.0;
00287 term2 = Mat(1, YN, OPT_MAT_TYPE);
00288 YCx = Mat(dt.dtopt.m, 1, OPT_MAT_TYPE);
00289 }
00290
00291
00292 if (kfmode == KF_CACHE || KF_COMPUTE_WITHOUT_CACHE) {
00293 Kt = Mat(dt.dtopt.n, dt.dtopt.m, OPT_MAT_TYPE);
00294 icv = Mat(dt.dtopt.m, dt.dtopt.m, OPT_MAT_TYPE);
00295 }
00296
00297 double detterm = 0.0;
00298
00299
00300 for (int t=0; t<tau; t++) {
00301
00302
00303
00304
00305
00306
00307
00308
00309 if (kfmode == KF_COMPUTE_WITH_CACHE) {
00310
00311 Kt = MatVid::frame(*cache_Kt, t);
00312 icv = MatVid::frame(*cache_invMt, t);
00313 detterm = cache_detMt->at<OPT_F_TYPE>(t);
00314
00315 } else {
00316
00317
00318 if (t>0) {
00319
00320 Vt1t = dt.A*Vtt*dt.A.t() + dt.Q.mtx;
00321 }
00322
00323
00324 switch(dt.dtopt.Ropt) {
00325 case CovMatrix::COV_IID:
00326 case CovMatrix::COV_DIAG:
00327
00328 if (countNonZero(Vt1t) == 0) {
00329
00330 iRf.copyTo(icv);
00331 } else {
00332
00333 Mat tmpS = (Vt1t.inv()+CRC).inv();
00334
00335 icv = iRf - RC*tmpS*RC.t();
00336 }
00337 break;
00338 case CovMatrix::COV_FULL:
00339
00340 icv = (dt.C*Vt1t*dt.C.t() + dt.R.mtx).inv();
00341 break;
00342 }
00343
00344
00345 Kt = Vt1t*dt.C.t()*icv;
00346
00347
00348 Vtt = Vt1t - Kt*dt.C*Vt1t;
00349
00350 if (kfmode == KF_CACHE || !mahal) {
00351
00352 detterm = 0.5*log(determinant(icv));
00353
00354
00355
00356 detterm -= 0.5*(dt.dtopt.m*log(2*M_PI));
00357 }
00358
00359
00360 if (kfmode == KF_CACHE) {
00361 Mat tmp1 = MatVid::frame(*cache_Kt, t);
00362 Kt.copyTo(tmp1);
00363 Mat tmp2 = MatVid::frame(*cache_invMt, t);
00364 icv.copyTo(tmp2);
00365 cache_detMt->at<OPT_F_TYPE>(t) = detterm;
00366 }
00367 }
00368
00369 if (kfmode == KF_COMPUTE_WITH_CACHE || kfmode == KF_COMPUTE_WITHOUT_CACHE) {
00370 if (t>0) {
00371
00372 xt1t = dt.A*xtt;
00373 }
00374
00375
00376 for (int i=0; i<YN; i++) {
00377
00378 YCx = Y[i].col(t) - dt.C*xt1t.col(i);
00379
00380
00381 xtt.col(i) = xt1t.col(i) + Kt*YCx;
00382
00383
00384
00385 term2.col(i) = YCx.t()*icv*YCx;
00386 }
00387
00388
00389 if (mahal) {
00390 *lik += term2;
00391
00392 } else {
00393
00394
00395
00396 *lik += -0.5*term2 + detterm;
00397 }
00398 }
00399
00400 }
00401
00402
00403
00404 if( Y != NULL ) delete [] Y;
00405 }
00406
00407
00408
00409
00442 void DytexKalmanFilter::dytex_kalman(Mat Y2, Dytex dt, string opt,std::vector<Mat> &xt,Mat &Vt,Mat &Vt1,Mat &L)
00443 {
00444 Mat Y;
00445 Y2.copyTo(Y);
00446
00447
00448 bool doL=true;
00449 bool doX=true;
00450
00451 Mat A=dt.A;
00452 Mat C=dt.C;
00453 Mat Q=dt.Q.mtx;
00454 int dx=C.cols;
00455 int dy=C.rows;
00456
00457 Mat Cu,Cs,Cv;
00458 Mat R,iR,CR,CRC;
00459 CovMatrix::cov_type Ropt;
00460 Mat mu0;
00461 Mat V0;
00462 if(dt.Q.n!=0)
00463 {
00464 Ropt=dt.R.covopt;
00465 switch(Ropt)
00466 {
00467 case CovMatrix::COV_IID:
00468 R=dt.R.mtx;
00469 iR=1/R;
00470 CR=C.t()*iR.at<OPT_F_TYPE>(0,0);
00471 CRC=CR*C;
00472 if(doL)
00473 {
00474 Mat tmpM;
00475 sqrt(iR,tmpM);
00476 Mat tmpM1=tmpM.at<OPT_F_TYPE>(0,0)*C;
00477
00478 SVD svd(tmpM1);
00479 svd.u.copyTo(Cu);
00480 svd.vt.copyTo(Cv);
00481 Cv=Cv.t();
00482 Cs=Mat::zeros(svd.w.rows,svd.w.rows,svd.w.type());
00483 tmpM=Cs.diag();
00484 svd.w.copyTo(tmpM);
00485
00486 }
00487 else
00488 {
00489 Cv=1;
00490 Cs=1;
00491 }
00492 break;
00493
00494 case CovMatrix::COV_DIAG:
00495
00496 break;
00497
00498 case CovMatrix::COV_FULL:
00499
00500 break;
00501 }
00502
00503 mu0=dt.mu0;
00504 Mat tmpM;
00505 switch(dt.S0.covopt)
00506 {
00507 case CovMatrix::COV_DIAG:
00508 V0.create(dt.S0.mtx.rows,dt.S0.mtx.rows,dt.S0.mtx.type());
00509 V0=Scalar(0);
00510 tmpM=V0.diag();
00511 dt.S0.mtx.copyTo(tmpM);
00512 break;
00513 case CovMatrix::COV_FULL:
00514 V0=dt.S0.mtx;
00515 break;
00516 default:
00517 CV_Error(-1,"bad Sopt");
00518 break;
00519 }
00520 }
00521
00522 Mat Ymean=dt.Ymean;
00523
00524
00525 int YN=Y.size[2];
00526 int tau=Y.size[0];
00527
00528
00529 if(checkOptionInStr(opt,"y"))
00530 {
00531 for(int t=0;t<tau;t++)
00532 {
00533 Mat tmpM=repeat(Ymean,1,YN);
00534 MatVid::frame(Y,t)=MatVid::frame(Y,t)-tmpM;
00535 }
00536
00537 }
00538
00539
00540 if (dt.Q.n==0)
00541 {
00542
00543 }
00544
00545
00546
00547
00548 int sz[]={tau,dx,YN};
00549 Mat xt1t;
00550 xt1t.create(3,sz,OPT_MAT_TYPE);
00551 xt1t=Scalar(0);
00552 Mat xtt;
00553 xt1t.copyTo(xtt);
00554 int sz2[]={tau,dx,dx};
00555 Mat Vt1t;
00556 Vt1t.create(3,sz2,OPT_MAT_TYPE);
00557 Vt1t=Scalar(0);
00558 Mat Vtt;
00559 Vt1t.copyTo(Vtt);
00560
00561 Mat Kt;
00562 if( (Ropt!=CovMatrix::COV_IID) && (Ropt!=CovMatrix::COV_DIAG) )
00563 {
00564 Kt=Mat::zeros(dx,dy,OPT_MAT_TYPE);
00565 }
00566 Mat L1=Mat::zeros(1,YN,OPT_MAT_TYPE);
00567
00568
00569 Mat tmpM=MatVid::frame(xt1t,0);
00570 Mat tmpM2;
00571 for(int i=0;i<YN;i++)
00572 {
00573 tmpM2=tmpM(Range::all(),Range(i,i+1));
00574 mu0.copyTo(tmpM2);
00575 }
00576 tmpM=MatVid::frame(Vt1t,0);
00577 V0.copyTo(tmpM);
00578
00579
00580 Mat Phi;
00581
00582 for(int t=0;t<tau;t++)
00583 {
00584 if(t>0)
00585 {
00586 Mat tmpM=MatVid::frame(xt1t,t);
00587 Mat tmpM2=A*MatVid::frame(xtt,t-1);
00588 tmpM2.copyTo(tmpM);
00589
00590 tmpM=MatVid::frame(Vt1t,t);
00591 tmpM2=A*MatVid::frame(Vtt,t-1)*A.t()+Q;
00592 tmpM2.copyTo(tmpM);
00593 }
00594
00595
00596 if( (Ropt==CovMatrix::COV_IID) || (Ropt==CovMatrix::COV_DIAG) )
00597 {
00598
00599 Mat tmpM=MatVid::frame(Vt1t,t);
00600
00601
00602 bool flag=true;
00603 for(int i=0;i<tmpM.rows;i++)
00604 for(int j=0;j<tmpM.rows;j++)
00605 {
00606 if(tmpM.at<OPT_F_TYPE>(i,j)!=0)
00607 flag=false;
00608 }
00609 if(flag==true)
00610 {
00611 tmpM.copyTo(Phi);
00612 }
00613 else
00614 {
00615 Mat tmpM2=tmpM.inv()+CRC;
00616 Mat tmpM3=tmpM2.inv();
00617 tmpM3.copyTo(Phi);
00618 }
00619
00620
00621 Mat CYCx= (CR*MatVid::frame(Y,t))-(CRC*MatVid::frame(xt1t,t));
00622 MatVid::frame(xtt,t)=MatVid::frame(xt1t,t)+(MatVid::frame(Vt1t,t)*CYCx)-(MatVid::frame(Vt1t,t)*CRC*Phi*CYCx);
00623
00624
00625 tmpM=MatVid::frame(Vt1t,t);
00626 MatVid::frame(Vtt,t)=tmpM- (tmpM*CRC*tmpM)+(tmpM*CRC*Phi*CRC*tmpM);
00627 Mat tmpM2=MatVid::frame(Vtt,t);
00628
00629
00630 if(doL)
00631 {
00632 Mat E;
00633 eigen(Cs*Cv.t()*MatVid::frame(Vt1t,t)*Cv*Cs,E);
00634 log(E+1,E);
00635 Scalar tmpSc=sum(E);
00636 double term1=tmpSc[0];
00637
00638
00639
00640 Mat term2;
00641 Mat YCx=MatVid::frame(Y,t)-C*MatVid::frame(xt1t,t);
00642 if(Ropt==CovMatrix::COV_IID)
00643 {
00644 term1=term1-dy*log(iR.at<OPT_F_TYPE>(0,0));
00645 Mat tmpM;
00646 pow(YCx,2,tmpM);
00647
00648 Mat abc2;
00649 reduce(tmpM,abc2,0,CV_REDUCE_SUM);
00650 tmpM=abc2;
00651
00652 term2=iR.at<OPT_F_TYPE>(0,0)*tmpM;
00653 }
00654 else
00655 {
00656 CV_Error(-1,"TO DO");
00657 }
00658
00659
00660 multiply((Phi*CYCx),CYCx,tmpM);
00661 Mat abc2;
00662 reduce(tmpM,abc2,0,CV_REDUCE_SUM);
00663 tmpM=abc2;
00664
00665 term2=term2-tmpM;
00666 L1=L1-((double)0.5)*(term1+term2);
00667 L1 = L1 - ((double)0.5)*(dy*log( (double(2.0))*M_PI));
00668
00669 }
00670 }
00671 else
00672 {
00673 CV_Error(-1,"TO DO");
00674 }
00675
00676 }
00677
00678 if(!doX)
00679 {
00680 L=L1;
00681 return;
00682 }
00683 else
00684 {
00685
00686 Mat xthat=MatVid::create(tau,dx,YN,OPT_MAT_TYPE);
00687 xthat=Scalar(0);
00688 Mat Vthat=MatVid::create(tau,dx,dx,OPT_MAT_TYPE);
00689 Vthat=Scalar(0);
00690 Mat Vtt1hat=MatVid::create(tau-1,dx,dx,OPT_MAT_TYPE);
00691 Vtt1hat=Scalar(0);
00692 Mat Jt=MatVid::create(tau,dx,dx,OPT_MAT_TYPE);
00693 Jt=Scalar(0);
00694
00695
00696 Mat tmpM;
00697 tmpM=MatVid::frame(xthat,tau-1);
00698 (MatVid::frame(xtt,tau-1)).copyTo(tmpM);
00699
00700 tmpM=MatVid::frame(Vthat,tau-1);
00701 (MatVid::frame(Vtt,tau-1)).copyTo(tmpM);
00702 if( (Ropt==CovMatrix::COV_IID) || (Ropt==CovMatrix::COV_DIAG) )
00703 {
00704 MatVid::frame(Vtt1hat,tau-2)=(Mat::eye(dx,dx,OPT_MAT_TYPE)-MatVid::frame(Vt1t,tau-1)*CRC+MatVid::frame(Vt1t,tau-1)*CRC*Phi*CRC)*A*MatVid::frame(Vtt,tau-2);
00705 Mat tmpM=MatVid::frame(Vtt1hat,tau-2);
00706 }
00707 else
00708 {
00709 CV_Error(-1,"TO DO");
00710 }
00711
00712 for(int t=tau-1;t>0;t--)
00713 {
00714 Mat tmpM=(MatVid::frame(Vt1t,t)).inv();
00715 MatVid::frame(Jt,t-1)=MatVid::frame(Vtt,t-1)*A.t()*tmpM;
00716 MatVid::frame(xthat,t-1)= MatVid::frame(xtt,t-1) + MatVid::frame(Jt,t-1)*(MatVid::frame(xthat,t)-A*MatVid::frame(xtt,t-1));
00717 MatVid::frame(Vthat,t-1) = MatVid::frame(Vtt,t-1)+MatVid::frame(Jt,t-1)*(MatVid::frame(Vthat,t)-MatVid::frame(Vt1t,t))*(MatVid::frame(Jt,t-1)).t();
00718
00719 if(t<tau-1)
00720 {
00721 MatVid::frame(Vtt1hat,t-1) = MatVid::frame(Vtt,t)*(MatVid::frame(Jt,t-1)).t()+ MatVid::frame(Jt,t)*(MatVid::frame(Vtt1hat,t)-A*MatVid::frame(Vtt,t))*(MatVid::frame(Jt,t-1)).t();
00722 }
00723
00724 }
00725
00726 std::vector<Mat> xthatout;
00727 for(int i=0;i<YN;i++)
00728 {
00729 Mat tmpM;
00730 tmpM.create(dx,tau,OPT_MAT_TYPE);
00731 for(int j=0;j<dx;j++)
00732 for(int k=0;k<tau;k++)
00733 tmpM.at<OPT_F_TYPE>(j,k)=xthat.at<OPT_F_TYPE>(k,j,i);
00734
00735 xthatout.push_back(tmpM);
00736 }
00737
00738 xt=xthatout;
00739 Vt=Vthat;
00740 Vt1=Vtt1hat;
00741 if(doL)
00742 L=L1;
00743
00744 }
00745
00746
00747 }