DytexKalmanFilter.cpp

00001 /**************************************************
00002 
00003   Dytex Kalman Filter class
00004 
00005   Antoni Chan,
00006   City University of Hong Kong, Copyright 2011
00007   libdt - OpenCV library for Dynamic Textures - version 1.0
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 /* TODO:
00042 
00043  */
00044 
00045 
00046 // note this requires that you keep dt around!
00047 DytexKalmanFilter::DytexKalmanFilter(Dytex &dt, int tau)
00048 {
00049   // initialize the Kalman cache
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   // create cache
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 // compute log-likelihood of videos under dt
00077 // result stored in lik (1 x size(videos))
00078 // mahal=true will compute Mahalanobis distance instead LL
00079 // this follows dytex_kalmanff_ll
00080 // note: it computes the inverse directly, so not for big videos.
00081 //       this is faster when the videos are small
00082 void DytexKalmanFilter::loglike_old(const Dytex &dt, const vector<Mat> &videos, Mat &lik, bool mahal) {
00083   
00084   // process videos (vectorize and subtract mean)
00085   int YN = videos.size();
00086 
00087   Mat* Y = new Mat[YN];
00088   //Mat Y[YN];
00089   int tau=0;
00090   for (int i=0; i<YN; i++) {
00091     dt.processVideoTesting(videos[i], Y[i]);
00092 
00093     // check that all tau are equal
00094     // [TODO: make it so we can handle different tau]
00095     if (i==0)
00096       tau = Y[i].cols;
00097     else
00098       CV_Assert(tau == Y[i].cols);
00099   }  
00100 
00101   // cache matrices for inversion lemma
00102   CovMatrix iR = dt.R.inv();   // inv(R)
00103   Mat iRf = iR.toFullMatrix(); // full inv(R)
00104   Mat RC;
00105   iR.postmultiply(dt.C, RC);   // inv(R)*C
00106   Mat CRC = dt.C.t() * RC;     // C'*inv(R)*C
00107 
00108   // initialize
00109   Mat Vt1t = dt.S0.toFullMatrix();  // Vt1t = S0
00110   Mat Vtt(dt.dtopt.n, dt.dtopt.n, OPT_MAT_TYPE);
00111   Mat xt1t;  // xt1t = mu0
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   // run forward pass
00126   for (int t=0; t<tau; t++) {
00127     // update step
00128     if (t>0) {
00129       // Vt1t = A*Vtt*A' + Q;
00130       Vt1t = dt.A*Vtt*dt.A.t() + dt.Q.mtx;
00131       
00132       // xt1t = A*xtt;
00133       xt1t = dt.A*xtt;
00134     }
00135 
00136     /*
00137     cout << "--- t=" << t << " ---\n";
00138     cout << "xt1t = " << xt1t << "\n";
00139     cout << "Vt1t = " << Vt1t << "\n";
00140     waitKey(1000);
00141     */
00142 
00143     // compute inverse
00144     switch(dt.dtopt.Ropt) {
00145     case CovMatrix::COV_IID:
00146     case CovMatrix::COV_DIAG:
00147       // all zeros?
00148       if (countNonZero(Vt1t) == 0) {
00149         //icv = iRf;
00150         iRf.copyTo(icv); // [TODO: make more efficient]
00151       } else {
00152         // tmpS = inv(inv(Vt1t) + CRC);
00153         Mat tmpS = (Vt1t.inv()+CRC).inv();
00154         //icv  = iRf - RC*tmpS*RC';
00155         icv = iRf - RC*tmpS*RC.t();
00156       }
00157       break;
00158     case CovMatrix::COV_FULL:
00159       // icv = inv(C*Vt1t*C' + dtkf.Rhat);
00160       icv = (dt.C*Vt1t*dt.C.t() + dt.R.mtx).inv();
00161       break;
00162     }
00163 
00164     // Kt  = Vt1t*C'*icv;
00165     Kt = Vt1t*dt.C.t()*icv;
00166 
00167     // Vtt = Vt1t - Kt*C*Vt1t;
00168     Vtt = Vt1t - Kt*dt.C*Vt1t;
00169   
00170     // TODO: can be sped up using Yit format?
00171     for (int i=0; i<YN; i++) {
00172       // YCx = Y(:,:,t) - C*xt1t;      
00173       YCx = Y[i].col(t) - dt.C*xt1t.col(i);
00174     
00175       // xtt = xt1t + Kt*YCx;
00176       xtt.col(i) = xt1t.col(i) + Kt*YCx;
00177 
00178       // mahal dist
00179       // term2 = sum((icv*YCx).*YCx, 1);
00180       term2.col(i) = YCx.t()*icv*YCx;
00181     }
00182     
00183     // compute Mahalanobis
00184     if (mahal) {
00185       lik += term2;
00186 
00187     } else {
00188       // compute Log-likelihood
00189 
00190       // term1 = 0.5*logdet(icv);             % the logdet term
00191       double term1 = 0.5*log(determinant(icv));
00192       // TODO: custom logdet function
00193       
00194       // term1 = term1 - 0.5*(dy*log(2*pi));  % the Gaussian constant
00195       term1 -= 0.5*(dt.dtopt.m*log(2*M_PI));
00196 
00197       // L     = L - 0.5*term2 + term1;
00198       lik += -0.5*term2 + term1;
00199     }
00200 
00201   } // end for t
00202 
00203   // clean up
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 // do one of the following:
00220 //   1) compute log-likelihood of videos under dt 
00221 //   2) cache kalman filter matrices for a dt
00222 //   3) compute log-likelihood using kalman filter cache
00223 //
00224 // result stored in lik (1 x size(videos))
00225 // mahal=true will compute Mahalanobis distance instead LL
00226 // this follows dytex_kalmanff_ll
00227 // note: it computes the inverse directly, so not for big videos.
00228 //       this is faster when the videos are small
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   // process videos (vectorize and subtract mean)
00234   int YN = (kfmode == KF_CACHE ? 0 : videos->size());
00235 
00236   Mat* Y = new Mat[YN];
00237   //Mat Y[YN];
00238   int tau = 0;
00239   
00240   if (kfmode == KF_CACHE)
00241     tau = cache_tau;
00242 
00243   else {
00244     // process videos
00245     for (int i=0; i<YN; i++) {
00246       dt.processVideoTesting((*videos)[i], Y[i]);
00247       
00248       // check that all tau are equal
00249       // [TODO: make it so we can handle different tau]
00250       if (i==0)
00251         tau = Y[i].cols;
00252       else
00253         CV_Assert(tau == Y[i].cols);
00254     }  
00255 
00256     // make sure there is enough cache
00257     if (kfmode == KF_COMPUTE_WITH_CACHE)
00258       CV_Assert(tau <= (*cache_Kt).size[0]);
00259   }
00260 
00261   // setup matrices for inversion lemma (when computing Kalman filter matrices)
00262   Mat iRf, CRC, RC;
00263   if (kfmode == KF_CACHE || kfmode == KF_COMPUTE_WITHOUT_CACHE) {
00264     CovMatrix iR = dt.R.inv();   // inv(R)
00265     iRf = iR.toFullMatrix();     // full inv(R)
00266     iR.postmultiply(dt.C, RC);   // inv(R)*C
00267     CRC = dt.C.t() * RC;         // C'*inv(R)*C
00268   }
00269 
00270   // initialize covariance matrices
00271   Mat Vt1t = dt.S0.toFullMatrix();  // Vt1t = S0
00272   Mat Vtt(dt.dtopt.n, dt.dtopt.n, OPT_MAT_TYPE);
00273 
00274 
00275   // initialize hidden states (if computing LL)
00276   Mat xt1t, xtt, term2, YCx, Kt, icv;
00277   if (kfmode == KF_COMPUTE_WITH_CACHE || kfmode == KF_COMPUTE_WITHOUT_CACHE) {
00278     // xt1t = mu0
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   // initialize Kalman gain (if not cached)
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   // run forward pass
00300   for (int t=0; t<tau; t++) {  
00301 
00302     /*
00303     cout << "--- t=" << t << " ---\n";
00304     cout << "xt1t = " << xt1t << "\n";
00305     cout << "Vt1t = " << Vt1t << "\n";
00306     waitKey(1000);
00307     */
00308 
00309     if (kfmode == KF_COMPUTE_WITH_CACHE) {
00310       // recall the cache
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       // compute kalman gain, etc
00317       
00318       if (t>0) {
00319         // update step:  Vt1t = A*Vtt*A' + Q;
00320         Vt1t = dt.A*Vtt*dt.A.t() + dt.Q.mtx;
00321       }
00322 
00323       // compute inverse
00324       switch(dt.dtopt.Ropt) {
00325       case CovMatrix::COV_IID:
00326       case CovMatrix::COV_DIAG:
00327         // all zeros?
00328         if (countNonZero(Vt1t) == 0) {
00329           //icv = iRf;
00330           iRf.copyTo(icv); // [TODO: make more efficient]
00331         } else {
00332           // tmpS = inv(inv(Vt1t) + CRC);
00333           Mat tmpS = (Vt1t.inv()+CRC).inv();
00334           //icv  = iRf - RC*tmpS*RC';
00335           icv = iRf - RC*tmpS*RC.t();
00336         }
00337         break;
00338       case CovMatrix::COV_FULL:
00339         // icv = inv(C*Vt1t*C' + dtkf.Rhat);
00340         icv = (dt.C*Vt1t*dt.C.t() + dt.R.mtx).inv();
00341         break;
00342       }
00343 
00344       // Kt  = Vt1t*C'*icv;
00345       Kt = Vt1t*dt.C.t()*icv;
00346 
00347       // Vtt = Vt1t - Kt*C*Vt1t;
00348       Vtt = Vt1t - Kt*dt.C*Vt1t;
00349       
00350       if (kfmode == KF_CACHE || !mahal) {
00351         // term1 = 0.5*logdet(icv);             % the logdet term
00352         detterm = 0.5*log(determinant(icv));
00353         // TODO: custom logdet function
00354         
00355         // term1 = term1 - 0.5*(dy*log(2*pi));  % the Gaussian constant
00356         detterm -= 0.5*(dt.dtopt.m*log(2*M_PI));
00357       }
00358 
00359       // store cache
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         // update step: xt1t = A*xtt;
00372         xt1t = dt.A*xtt;
00373       }
00374 
00375       // TODO: can be sped up using Yit format?
00376       for (int i=0; i<YN; i++) {
00377         // YCx = Y(:,:,t) - C*xt1t;      
00378         YCx = Y[i].col(t) - dt.C*xt1t.col(i);
00379         
00380         // xtt = xt1t + Kt*YCx;
00381         xtt.col(i) = xt1t.col(i) + Kt*YCx;
00382         
00383         // mahal dist
00384         // term2 = sum((icv*YCx).*YCx, 1);
00385         term2.col(i) = YCx.t()*icv*YCx;
00386       }
00387     
00388       // compute Mahalanobis
00389       if (mahal) {
00390         *lik += term2;
00391         
00392       } else {
00393         // compute Log-likelihood
00394 
00395         // L     = L - 0.5*term2 + term1;
00396         *lik += -0.5*term2 + detterm;
00397       }
00398     }
00399 
00400   } // end for t
00401 
00402   
00403   // clean up
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         //Set output modes
00448         bool doL=true; //compute logliklihood
00449         bool doX=true; //also compute stats
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                                 //TO DO
00496                         break;
00497 
00498                         case CovMatrix::COV_FULL:
00499                                 //TO DO
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         // already in Y(:,i,t) format (mean subtracted too)
00525         int YN=Y.size[2];
00526         int tau=Y.size[0];
00527 
00528         // explicitly subtract mean
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         // special case: n=0
00540         if  (dt.Q.n==0)  
00541         {
00542                 //TO DO
00543         }
00544 
00545         //OpenCV implementation of Kalman Filter
00546 
00547         //%%% initialize storage %%%%%%%%%%%%%%%%%%%%%%%%%
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          //%%% initialize forward pass %%%%%%%%%%%%%%%%%%%%
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         //RUN Forward Pass
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                         //Using Matrix iNversion lemma
00599                         Mat tmpM=MatVid::frame(Vt1t,t);
00600                         
00601                         //check if any elemenet is zero 
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                         // expansion using matrix inversion lemma...
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                         // expansion using matrix inversion lemma...
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                         //% compute the log-likelihood efficiently: p(y_t | y_1,...,y_{t-1})
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));  //% the Gaussian constant                       
00668 
00669                         }
00670                 }
00671                 else //for Ropt=full, we need to compute normally
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                 //Backward pass
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                 //initialize            
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                 //%%% convert xthat into cell array %%%
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 }

Generated on 18 Dec 2014 for libdt-OpenCV by  doxygen 1.6.1