Estats.cpp

00001 
00007 #include <iostream>
00008 #include <string>
00009 #include<fstream>
00010 #include <math.h>
00011 
00012 #ifdef WIN32
00013         #define _USE_MATH_DEFINES       
00014 #endif
00015 
00016 #include "opencv/cv.h"
00017 #include "opencv/cxcore.h"
00018 #include "opencv/highgui.h"
00019 
00020 
00021 #include "options.h"
00022 #include "utils/libutils.h"
00023 #include "video/libvideo.h"
00024 #include "stats/libstats.h"
00025 #include "dytex/libdytex.h"
00026 
00027 
00028 using namespace std;
00029 using namespace cv;
00030 
00031 
00051 Estats::Estats(std::vector<Dytex> &dti,std::vector<Dytex> &dtj,int tau,bool useYmean)
00052 {
00053         this->dti=dti;
00054         this->dtj=dtj;
00055         this->tau=tau;
00056         this->useYmean=useYmean;
00057         //build SENS ANALYSIS Cache
00058         saveCache();
00059 }
00060 
00065 void Estats::saveCache()
00066 {
00067         int Kb=dti.size();
00068         int Kr=dtj.size();
00069         int len=tau;
00070         int dx=dtj[0].C.cols;
00071         int dy=dtj[0].C.rows;
00072 
00073         //initialize cache              
00074         for(int i=0;i<Kr;i++)
00075         {
00076                 Mat tmp=MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00077                 tmp.setTo(0);
00078                 dtjsa_Q_Ft.push_back(tmp);
00079 
00080                 tmp=MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00081                 tmp.setTo(0);
00082                 dtjsa_Q_GtC2.push_back(tmp);
00083 
00084                 tmp=MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00085                 tmp.setTo(0);
00086                 dtjsa_Q_Wt.push_back(tmp);
00087 
00088                 tmp=MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00089                 tmp.setTo(0);
00090                 dtjsa_Q_foo.push_back(tmp);
00091 
00092                 tmp=MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00093                 tmp.setTo(0);
00094                 dtjsa_Q_Jt.push_back(tmp);
00095 
00096                 tmp=MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00097                 tmp.setTo(0);
00098                 dtjsa_Q_Ht.push_back(tmp);
00099 
00100                 tmp=MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00101                 tmp.setTo(0);
00102                 dtjsa_Lt.push_back(tmp);
00103 
00104                 tmp=MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00105                 tmp.setTo(0);
00106                 dtjsa_LF.push_back(tmp);
00107 
00108                 tmp=MatVid::create(len,dx,dy,OPT_MAT_TYPE);
00109                 tmp.setTo(0);
00110                 dtjsa_LtGt.push_back(tmp);
00111         }
00112         dtjsa_iA2= MatVid::create(Kr,dx,dx,OPT_MAT_TYPE);
00113         dtjsa_iA2.setTo(0);
00114         dtjsa_Szero.create(1,Kr,OPT_MAT_TYPE);
00115         dtjsa_Szero.setTo(0);
00116         dtjsa_Psij_init= MatVid::create(Kr,dx,dx,OPT_MAT_TYPE);
00117         dtjsa_Psij_init.setTo(0);
00118         dtjsa_etaj_init= MatVid::create(Kr,dx,dx,OPT_MAT_TYPE);
00119         dtjsa_etaj_init.setTo(0);
00120         dtjsa_varphij_init= MatVid::create(Kr,dx,dx,OPT_MAT_TYPE);
00121         dtjsa_varphij_init.setTo(0);
00122         dtjsa_Phij_init= MatVid::create(Kr,dx,dx,OPT_MAT_TYPE);
00123         dtjsa_Phij_init.setTo(0);
00124         dtjsa_phij_init= MatVid::create(Kr,dx,dx,OPT_MAT_TYPE);
00125         dtjsa_phij_init.setTo(0);
00126         dtjsa_Q_logdet.create(len,Kr,OPT_MAT_TYPE);
00127         dtjsa_Q_logdet.setTo(0);
00128 
00129         //RUN AND CACHE THE SENS ANALYSIS FOR DTJ       
00130         dtjblank.resize(Kr,false);
00131         for(int j=0;j<Kr;j++)
00132         {
00133                 Dytex dt2=dtj[j];
00134 
00135                 if(dt2.isempty)
00136                 {
00137                         dtjblank[j]=true;
00138                         continue;
00139                 }
00140 
00141                 Mat S2;
00142                 Mat tmpM;
00143                 switch(dt2.dtopt.Sopt)
00144                 {
00145                 case CovMatrix::COV_DIAG:
00146                         S2.create(dt2.S0.mtx.rows,dt2.S0.mtx.rows,OPT_MAT_TYPE);
00147                         S2.setTo(0);
00148                         tmpM=S2.diag();
00149                         dt2.S0.mtx.copyTo(tmpM);
00150                         break;
00151 
00152                 default:
00153                         CV_Error(-1,"Cov type Not supported");
00154                 }
00155 
00156                 bool Szero=true;
00157                 for(int m=0;m<S2.rows;m++)
00158                         for(int n=0;n<S2.cols;n++)
00159                         {
00160                                 if(S2.at<OPT_F_TYPE>(m,n)!=0)
00161                                         Szero=false;
00162                         }
00163 
00164                         //special n=0 case
00165                         if (dx==0)
00166                         {
00167                                 CV_Error(-1,"n=0 case not supported");
00168                         }
00169 
00170                         const Mat &A2=dt2.A;
00171                         Mat iA2=A2.inv();
00172                         const Mat &Q2=dt2.Q.mtx;
00173                         const Mat &C2=dt2.C;
00174                         const Mat &C2vs=dt2.Cvs;
00175                         double r2=dt2.R.mtx.at<OPT_F_TYPE>(0,0);
00176                         Mat C2R2C2=((C2.t())*C2)/r2;
00177 
00178                         // initialize KALMAN (t=1)
00179                         Mat Q_curVtt1=S2;
00180 
00181                         // Kalman cache
00182                         Mat Q_Vtt1 = MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00183                         Q_Vtt1.setTo(0);
00184                         Mat Q_Vtt  = MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00185                         Q_Vtt.setTo(0);
00186                         Mat Q_Ft   = MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00187                         Q_Ft.setTo(0);
00188                         Mat Q_GtC2 = MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00189                         Q_GtC2.setTo(0);
00190                         Mat Q_Gt   = MatVid::create(len,dx,dy,OPT_MAT_TYPE);
00191                         Q_Gt.setTo(0);
00192                         Mat Q_Wt   = MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00193                         Q_Wt.setTo(0);
00194                         Mat Q_foo  = MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00195                         Q_foo.setTo(0);
00196                         Mat Q_logdet(len,1,OPT_MAT_TYPE);
00197 
00198                         //KALMAN filter on Q
00199                         Mat Q_curVtt,Q_curfoo,Q_curWt,Q_KtC2;
00200                         for(int t=0;t<len;t++)
00201                         {
00202                                 if (t>0)
00203                                 {
00204                                         Q_curVtt1   = A2*Q_curVtt*(A2.t())+Q2;
00205                                 }
00206                                 if((t==0) && (Szero==true))
00207                                 {
00208                                         Q_curfoo= Mat::zeros(dx,dx,OPT_MAT_TYPE);
00209                                 }
00210                                 else
00211                                 {
00212                                         Q_curWt     = (Q_curVtt1.inv()+C2R2C2).inv();
00213                                         Q_curfoo    = Q_curVtt1*(Mat::eye(dx,dx,OPT_MAT_TYPE)-C2R2C2*Q_curWt);
00214                                 }
00215                                 Q_KtC2        = Q_curfoo*C2R2C2;
00216                                 MatVid::frame(Q_GtC2,t) = A2*Q_KtC2;
00217                                 MatVid::frame(Q_Ft,t)   = A2-MatVid::frame(Q_GtC2,t);
00218                                 MatVid::frame(Q_Gt,t)   = (A2*Q_curfoo*(C2.t()))/r2;
00219                                 Q_curVtt    = Q_curVtt1 - Q_KtC2*Q_curVtt1;
00220 
00221                                 Mat tmpM=MatVid::frame(Q_Vtt1,t);
00222                                 Q_curVtt1.copyTo(tmpM);
00223                                 tmpM=MatVid::frame(Q_Vtt,t);
00224                                 Q_curVtt.copyTo(tmpM);
00225                                 tmpM=MatVid::frame(Q_Wt,t);
00226                                 Q_curWt.copyTo(tmpM);
00227                                 tmpM=MatVid::frame(Q_foo,t);
00228                                 Q_curfoo.copyTo(tmpM);
00229                                 Q_logdet.at<OPT_F_TYPE>(t,0)   = logdetiid((C2vs.t())*Q_curVtt1*C2vs, r2, dy);
00230                         }//142
00231 
00232                         // KALMAN smoothing filter on Q, and sensitivity analysis
00233                         Mat Q_Jt=MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00234                         Q_Jt.setTo(0);
00235                         Mat Q_Ht=MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00236                         Q_Ht.setTo(0);
00237                         Mat Q_Vtt1tau=MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00238                         Q_Vtt1tau.setTo(0);
00239                         MatVid::frame(Q_Vtt1tau,0).setTo(INF);
00240                         Mat Q_Vttau=MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00241                         Q_Vttau.setTo(0);
00242                         Mat Lt=MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00243                         Lt.setTo(0);
00244                         Mat LF=MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00245                         LF.setTo(0);
00246                         Mat LtGt=MatVid::create(len,dx,dy,OPT_MAT_TYPE);
00247                         LtGt.setTo(0);
00248 
00249                         Mat Q_curVttau,curLt,Q_curVtt1tau;
00250                         for(int t=len-1;t>=0;t--)
00251                         {
00252                                 if (t==(len-1))
00253                                 {
00254                                         Q_curVttau   = MatVid::frame(Q_Vtt,len-1);
00255                                         iA2.copyTo(curLt);
00256                                 }      
00257                                 else
00258                                 {
00259                                         MatVid::frame(Q_Jt,t) = MatVid::frame(Q_Vtt,t)*(A2.t())*(MatVid::frame(Q_Vtt1,t+1).inv());
00260                                         MatVid::frame(Q_Ht,t) = iA2 - MatVid::frame(Q_Jt,t);
00261                                         Q_curVttau  = MatVid::frame(Q_Vtt,t) + MatVid::frame(Q_Jt,t)*(Q_curVttau-MatVid::frame(Q_Vtt1,t+1))*(MatVid::frame(Q_Jt,t).t());
00262                                         curLt = MatVid::frame(Q_Ht,t) + MatVid::frame(Q_Jt,t)*curLt*MatVid::frame(Q_Ft,t+1);
00263                                 }
00264                                 Mat tmpM=MatVid::frame(Q_Vttau,t);
00265                                 Q_curVttau.copyTo(tmpM);
00266 
00267                                 if (t<(len-1))
00268                                 {
00269                                         if (t==len-2)
00270                                         {                                       
00271                                                 Q_curVtt1tau = (Mat::eye(dx,dx,OPT_MAT_TYPE) - iA2*MatVid::frame(Q_GtC2,len-1))*A2*MatVid::frame(Q_Vtt,len-2);
00272                                         }
00273                                         else      
00274                                         {
00275                                                 Q_curVtt1tau = MatVid::frame(Q_Vtt,t+1)*(MatVid::frame(Q_Jt,t).t())+MatVid::frame(Q_Jt,t+1)*(Q_curVtt1tau-A2*MatVid::frame(Q_Vtt,t+1))*(MatVid::frame(Q_Jt,t).t());
00276                                         }
00277                                         Mat tmpM=MatVid::frame(Q_Vtt1tau,t+1);
00278                                         Q_curVtt1tau.copyTo(tmpM);
00279                                 }    
00280 
00281                                 // sensitivity analysis cache
00282                                 MatVid::frame(LF,t)   = curLt*MatVid::frame(Q_Ft,t);
00283                                 MatVid::frame(LtGt,t) = curLt*MatVid::frame(Q_Gt,t);
00284                                 tmpM=MatVid::frame(Lt,t);
00285                                 curLt.copyTo(tmpM);    
00286                         }//182
00287 
00288                         //save things
00289                         dtjsa_Q_Ft[j]   = Q_Ft;
00290                         dtjsa_Q_GtC2[j] = Q_GtC2;
00291                         dtjsa_Q_Wt[j]   = Q_Wt;
00292                         dtjsa_Q_foo[j]  = Q_foo;
00293                         dtjsa_Q_Jt[j]   = Q_Jt;
00294                         dtjsa_Q_Ht[j]   = Q_Ht;
00295                         dtjsa_Lt[j]        = Lt;
00296                         dtjsa_LF[j]        = LF;
00297                         dtjsa_LtGt[j]      = LtGt;
00298                         tmpM=MatVid::frame(dtjsa_iA2,j);
00299                         iA2.copyTo(tmpM);
00300                         dtjsa_Szero.at<OPT_F_TYPE>(0,j)     = (double)Szero;
00301 
00302                         tmpM=MatVid::subvid(Q_Vtt1tau,Range(1,len),Range::all(),Range::all());
00303                         Mat tmpM2;
00304                         MatVid::reduce(tmpM,tmpM2,CV_REDUCE_SUM);
00305                         Mat tmpM3=MatVid::frame(dtjsa_Psij_init,j);//    = sum(Q_Vtt1tau(:,:,2:len),3);
00306                         tmpM2.copyTo(tmpM3);
00307 
00308                         tmpM=MatVid::frame(dtjsa_etaj_init,j);
00309                         tmpM2= MatVid::frame(Q_Vttau,0);
00310                         tmpM2.copyTo(tmpM);
00311 
00312 
00313                         tmpM=MatVid::subvid(Q_Vttau,Range(1,len),Range::all(),Range::all());
00314                         Mat tmpN1;
00315                         MatVid::reduce(tmpM,tmpN1,CV_REDUCE_SUM);
00316                         tmpM3=MatVid::frame(dtjsa_varphij_init,j);//    = sum(Q_Vtt1tau(:,:,2:len),3);
00317                         tmpN1.copyTo(tmpM3);
00318 
00319                         Mat tmpN2;
00320                         MatVid::reduce(Q_Vttau,tmpN2,CV_REDUCE_SUM);
00321                         tmpM3=MatVid::frame(dtjsa_Phij_init,j);//    = sum(Q_Vtt1tau(:,:,2:len),3);
00322                         tmpN2.copyTo(tmpM3);
00323 
00324                         tmpM=MatVid::subvid(Q_Vttau,Range(0,len-1),Range::all(),Range::all());
00325                         Mat tmpN3;
00326                         MatVid::reduce(tmpM,tmpN3,CV_REDUCE_SUM);
00327                         tmpM3=MatVid::frame(dtjsa_phij_init,j);//    = sum(Q_Vtt1tau(:,:,2:len),3);
00328                         tmpN3.copyTo(tmpM3);
00329 
00330                         tmpM=dtjsa_Q_logdet.col(j);
00331                         Q_logdet.copyTo(tmpM);
00332 
00333         }//202
00334 }
00335 
00340 void Estats::computeEll()
00341 {
00342         int Kb=dti.size();
00343         int Kr=dtj.size();
00344         int len=tau;
00345         int dx=dtj[0].C.cols;
00346         int dy=dtj[0].C.rows;
00347 
00348         
00349         //2. Loop THROUGH EACH DTI
00350 
00351         Ell=Mat::zeros(Kb,Kr,OPT_MAT_TYPE);
00352 
00353         //constants
00354         double ell_const  = dy*log(2*M_PI);
00355 
00356         for(int i=0;i<Kb;i++)
00357         {
00358                 const Dytex &dt1=dti[i];
00359 
00360                 Mat S1;
00361                 Mat tmpM;
00362                 switch(dt1.dtopt.Sopt)
00363                 {
00364                 case CovMatrix::COV_DIAG:
00365                         S1.create(dt1.S0.mtx.rows,dt1.S0.mtx.rows,OPT_MAT_TYPE);
00366                         S1.setTo(0);
00367                         tmpM=S1.diag();
00368                         dt1.S0.mtx.copyTo(tmpM);
00369                         break;
00370 
00371                 default:
00372                         CV_Error(-1,"Cov type Not supported");
00373                 }
00374 
00375                 bool S1zero=true;
00376                 for(int m=0;m<S1.rows;m++)
00377                         for(int n=0;n<S1.cols;n++)
00378                         {
00379                                 if(S1.at<OPT_F_TYPE>(m,n)!=0)
00380                                         S1zero=false;
00381                         }
00382 
00383                         //special n=0 case
00384                         if (dx==0)
00385                         {
00386                                 CV_Error(-1,"n=0 case not supported");
00387                         }
00388 
00389                         const Mat &A1 = dt1.A;
00390                         const Mat &Q1 = dt1.Q.mtx;
00391                         const Mat &C1 = dt1.C;
00392                         const Mat &mu01 = dt1.mu0;
00393                         double r1 = dt1.R.mtx.at<OPT_F_TYPE>(0,0);
00394                         const Mat &Ymean1 = dt1.Ymean;
00395 
00396                         //some constants
00397                         Mat Ydiff=Mat::zeros(dy,Kr,OPT_MAT_TYPE);
00398                         Mat C1C1   = C1.t()*C1;
00399                         Mat C1R1C1 = (C1C1)/r1;
00400                         Mat C1R2C1 = MatVid::create(Kr,dx,dx,OPT_MAT_TYPE);
00401                         C1R2C1.setTo(0);
00402                         Mat C2R2C1 = MatVid::create(Kr,dx,dx,OPT_MAT_TYPE);
00403                         C1R2C1.setTo(0);
00404                         Mat C1R2C2 = MatVid::create(Kr,dx,dx,OPT_MAT_TYPE);
00405                         C1R2C1.setTo(0);
00406                         Mat C2R2R1R2C2 = MatVid::create(Kr,dx,dx,OPT_MAT_TYPE);
00407                         C1R2C1.setTo(0);
00408                         Mat C1R2Ydiff  = Mat::zeros(dx,Kr,OPT_MAT_TYPE);
00409                         Mat C2R2Ydiff  = Mat::zeros(dx,Kr,OPT_MAT_TYPE);
00410                         Mat C2R2C2 = MatVid::create(Kr,dx,dx,OPT_MAT_TYPE);
00411                         C1R2C1.setTo(0);
00412                         double Szero;
00413                         //cache constants
00414                         for(int j=0;j<Kr;j++)
00415                         {
00416                                 if(!dtjblank[j])
00417                                 {
00418                                         if (useYmean)
00419                                         {
00420                                                 Ydiff.col(j) = dt1.Ymean-dtj[j].Ymean;
00421                                         }
00422                                         if (S1zero && dtjsa_Szero.at<OPT_F_TYPE>(0,j))
00423                                         {
00424                                                 Szero = 1;
00425                                         }
00426                                         else
00427                                         {
00428                                                 if (S1zero || dtjsa_Szero.at<OPT_F_TYPE>(0,j))
00429                                                 {
00430                                                         CV_Error(-1,"both must have S=0");
00431                                                 }
00432                                                 Szero = 0;
00433                                         }
00434 
00435                                         const Mat &C2 = dtj[j].C;
00436                                         double r2 = dtj[j].R.mtx.at<OPT_F_TYPE>(0,0);
00437                                         MatVid::frame(C1R2C1,j) = (C1.t()*C1)/r2;
00438                                         MatVid::frame(C2R2C2,j) = (C2.t()*C2)/r2;
00439                                         MatVid::frame(C2R2C1,j) = (C2.t()*C1)/r2;
00440                                         MatVid::frame(C1R2C2,j) = MatVid::frame(C2R2C1,j).t();
00441                                         MatVid::frame(C2R2R1R2C2,j) = (C2.t()*C2)*r1/(pow(r2,2));
00442                                         if (useYmean)
00443                                         {
00444                                                 C1R2Ydiff.col(j) = (C1.t()*Ydiff.col(j))/r2;
00445                                                 C2R2Ydiff.col(j) = (C2.t()*Ydiff.col(j))/r2;
00446                                         }
00447                                 }
00448                         } // end cache constants 287
00449 
00450                         // initialize KALMAN (t=1)
00451                         Mat P_Vtt1=S1.clone();
00452 
00453                         // storage for Kalman smoother
00454                         std::vector<Mat> Q_GtC1;
00455                         for(int m=0;m<Kr;m++)
00456                         {
00457                                 Mat tmpM=MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00458                                 tmpM.setTo(0);
00459                                 Q_GtC1.push_back(tmpM);
00460                         }
00461 
00462                         // storage for ELL
00463                         Mat ell = Mat::zeros(1,Kr,OPT_MAT_TYPE);
00464 
00465                         //initialize sensitivity analysis
00466                         Mat bxt1=Mat::zeros(dx,len+1,OPT_MAT_TYPE);
00467                         tmpM=bxt1.col(0);
00468                         mu01.copyTo(tmpM);
00469 
00470                         Mat bxt2=Mat::zeros(dx,len+1,OPT_MAT_TYPE);
00471                         tmpM=bxt2.col(0);
00472                         mu01.copyTo(tmpM);
00473 
00474                         Mat bxt3  = MatVid::create(Kr,dx,len+1,OPT_MAT_TYPE);
00475                         bxt3.setTo(0);
00476 
00477                         for(int j=0;j<Kr;j++)
00478                         {
00479                                 if(!dtjblank[j])
00480                                 {
00481                                         Mat tmpM=MatVid::frame(bxt3,j);
00482                                         Mat tmp2=tmpM.col(0);
00483                                         dtj[j].mu0.copyTo(tmp2);                                
00484                                 }
00485                         }
00486 
00487                         Mat bVt11 = MatVid::create(len+1,dx,dx,OPT_MAT_TYPE);
00488                         bVt11.setTo(0);
00489 
00490                         tmpM=MatVid::frame(bVt11,0);
00491                         S1.copyTo(tmpM);
00492 
00493                         Mat bVt12 = MatVid::create(len+1,dx,dx,OPT_MAT_TYPE);
00494                         bVt12.setTo(0);
00495 
00496                         std::vector<Mat> bVt13;
00497                         for(int j=0;j<Kr;j++)
00498                         {
00499                                 Mat tmpM=MatVid::create(len+1,dx,dx,OPT_MAT_TYPE);
00500                                 tmpM.setTo(0);
00501                                 bVt13.push_back(tmpM);
00502                         }
00503 
00504                         Mat bVt22 = MatVid::create(len+1,dx,dx,OPT_MAT_TYPE);
00505                         bVt22.setTo(0);
00506 
00507                         std::vector<Mat> bVt23;
00508                         for(int j=0;j<Kr;j++)
00509                         {
00510                                 Mat tmpM=MatVid::create(len+1,dx,dx,OPT_MAT_TYPE);
00511                                 tmpM.setTo(0);
00512                                 bVt23.push_back(tmpM);
00513                         }
00514 
00515                         std::vector<Mat> bVt33;
00516                         for(int j=0;j<Kr;j++)
00517                         {
00518                                 Mat tmpM=MatVid::create(len+1,dx,dx,OPT_MAT_TYPE);
00519                                 tmpM.setTo(0);
00520                                 bVt33.push_back(tmpM);
00521                         }
00522 
00523                         // iterate from t=1 to len              
00524                         Mat P_Vtt,P_foo,P_Wt,P_KtC1,P_GtC1,P_GtR1Gt,P_Ft,tmp_GbFb;
00525                         for(int t=0;t<len;t++)
00526                         {                       
00527                                 //KALMAN filter on P at time t
00528                                 if (t>0)
00529                                 {
00530                                         P_Vtt1   = A1*P_Vtt*A1.t()+Q1;
00531                                 }       
00532                                 if((t==0) && Szero)
00533                                 {
00534                                         P_foo  = Mat::zeros(dx,dx,OPT_MAT_TYPE);
00535                                 }
00536                                 else
00537                                 {
00538                                         P_Wt     = (P_Vtt1.inv()+C1R1C1).inv(); 
00539                                         P_foo    = P_Vtt1*(Mat::eye(dx,dx,OPT_MAT_TYPE)-C1R1C1*P_Wt);
00540                                 }
00541 
00542                                 P_KtC1    = P_foo*C1R1C1;
00543                                 P_GtC1    = A1*P_KtC1;
00544                                 P_Vtt     = P_Vtt1 - P_KtC1*P_Vtt1;
00545                                 P_GtR1Gt  = P_GtC1*P_foo.t()*A1.t();
00546                                 P_Ft      = A1-P_GtC1;
00547 
00548                                 // update sensitivity analysis for P
00549                                 tmp_GbFb.create(P_GtC1.rows,P_GtC1.cols+P_Ft.cols,OPT_MAT_TYPE);
00550                                 tmpM=tmp_GbFb.colRange(Range(0,P_GtC1.cols));
00551                                 P_GtC1.copyTo(tmpM);
00552                                 tmpM=tmp_GbFb.colRange(Range(P_GtC1.cols,P_GtC1.cols+P_Ft.cols));
00553                                 P_Ft.copyTo(tmpM);
00554 
00555                                 bxt2.col(t+1)    = P_GtC1*bxt1.col(t) + P_Ft*bxt2.col(t);
00556                                 bxt1.col(t+1)    = A1*bxt1.col(t);
00557 
00558 
00559                                 Mat tmp1=MatVid::frame(bVt11,t);
00560                                 Mat tmp2=MatVid::frame(bVt12,t);
00561                                 Mat tmp3=MatVid::frame(bVt12,t).t(); 
00562                                 Mat tmp4=MatVid::frame(bVt22,t);                
00563                                 tmpM.create(tmp1.rows+tmp3.rows,tmp1.cols+tmp2.cols,OPT_MAT_TYPE);
00564                                 Mat tmpM2=tmpM(Range(0,tmp1.rows),Range(0,tmp1.cols));
00565                                 tmp1.copyTo(tmpM2);
00566                                 tmpM2=tmpM(Range(0,tmp1.rows),Range(tmp1.cols,tmp1.cols+tmp2.cols));
00567                                 tmp2.copyTo(tmpM2);
00568                                 tmpM2=tmpM(Range(tmp1.rows,tmp1.rows+tmp3.rows),Range(0,tmp1.cols));
00569                                 tmp3.copyTo(tmpM2);
00570                                 tmpM2=tmpM(Range(tmp1.rows,tmp1.rows+tmp3.rows),Range(tmp1.cols,tmp1.cols+tmp2.cols));
00571                                 tmp4.copyTo(tmpM2);
00572 
00573                                 MatVid::frame(bVt22,t+1) = tmp_GbFb * tmpM * tmp_GbFb.t()  + P_GtR1Gt;
00574 
00575                                 tmp1=MatVid::frame(bVt11,t);
00576                                 tmp2=MatVid::frame(bVt12,t);
00577                                 tmpM.create(tmp1.rows,tmp1.cols+tmp2.cols,OPT_MAT_TYPE);
00578                                 tmpM2=tmpM.colRange(0,tmp1.cols);
00579                                 tmp1.copyTo(tmpM2);
00580                                 tmpM2=tmpM.colRange(tmp1.cols,tmp1.cols+tmp2.cols);
00581                                 tmp2.copyTo(tmpM2);
00582 
00583                                 MatVid::frame(bVt12,t+1) = A1*(tmpM*tmp_GbFb.t());
00584                                 MatVid::frame(bVt11,t+1) = A1*MatVid::frame(bVt11,t)*A1.t() + Q1;
00585 
00586                                 //compute cross-covariance
00587                                 for(int j=0;j<Kr;j++)
00588                                 {
00589                                         if(dtjblank[j])
00590                                         {
00591                                                 ell.at<OPT_F_TYPE>(0,j)=-1e300;
00592                                         }
00593                                         else
00594                                         {
00595                                                 Mat tmp = dtj[j].A*MatVid::frame(dtjsa_Q_foo[j],t);
00596                                                 MatVid::frame(Q_GtC1[j],t) = tmp*MatVid::frame(C2R2C1,j);
00597                                                 Mat Q_GtR1Gt  = tmp*MatVid::frame(C2R2R1R2C2,j)*tmp.t();
00598                                                 Mat PQ_GtR1Gt = A1*P_foo*MatVid::frame(C1R2C2,j)*tmp.t();
00599 
00600                                                 double ell_mahal;
00601                                                 //compute expected log-likelihood
00602                                                 if((t==0) && Szero)
00603                                                 {
00604                                                         CV_Error(-1,"not supported yet");
00605                                                 }
00606                                                 else
00607                                                 {
00608                                                         Mat tmp_QWtC2R2C1 = MatVid::frame(dtjsa_Q_Wt[j],t)*MatVid::frame(C2R2C1,j);
00609                                                         double ell_mahal1 = trace((MatVid::frame(bVt11,t)+bxt1.col(t)*bxt1.col(t).t())*(MatVid::frame(C1R2C1,j)-MatVid::frame(C1R2C2,j)*tmp_QWtC2R2C1))[0]+ dy*r1/dtj[j].R.mtx.at<OPT_F_TYPE>(0,0) - trace(MatVid::frame(dtjsa_Q_Wt[j],t)*MatVid::frame(C2R2R1R2C2,j))[0];
00610                                                         double ell_mahal2 = trace((MatVid::frame(bVt23[j],t)+bxt2.col(t)*(MatVid::frame(bxt3,j).col(t)).t())*(MatVid::frame(C2R2C1,j)-MatVid::frame(C2R2C2,j)*tmp_QWtC2R2C1))[0];
00611                                                         double ell_mahal3 = trace((MatVid::frame(bVt33[j],t)+(MatVid::frame(bxt3,j).col(t))*(MatVid::frame(bxt3,j).col(t)).t())*(MatVid::frame(C2R2C2,j)-MatVid::frame(C2R2C2,j)*MatVid::frame(dtjsa_Q_Wt[j],t)*MatVid::frame(C2R2C2,j)))[0];
00612 
00613                                                         if (useYmean)
00614                                                         {
00615                                                                 Mat tmp_QWtC2R2Ydiff = MatVid::frame(dtjsa_Q_Wt[j],t)*C2R2Ydiff.col(j);
00616                                                                 Mat tmpM=(2*(bxt1.col(t).t())*(C1R2Ydiff.col(j) - MatVid::frame(C1R2C2,j)*tmp_QWtC2R2Ydiff)+((Ydiff.col(j).t())*Ydiff.col(j))/dtj[j].R.mtx.at<OPT_F_TYPE>(0,0)-(C2R2Ydiff.col(j).t())*tmp_QWtC2R2Ydiff);
00617                                                                 ell_mahal1 = ell_mahal1 + tmpM.at<OPT_F_TYPE>(0,0);
00618 
00619                                                                 tmpM=(MatVid::frame(bxt3,j).col(t)).t()*(C2R2Ydiff.col(j) - MatVid::frame(C2R2C2,j)*tmp_QWtC2R2Ydiff);
00620                                                                 ell_mahal2 = ell_mahal2 + tmpM.at<OPT_F_TYPE>(0,0);
00621                                                         }
00622                                                         ell_mahal = ell_mahal1 - 2*ell_mahal2 + ell_mahal3;
00623                                                 }//376
00624 
00625                                                 ell.at<OPT_F_TYPE>(0,j) = ell.at<OPT_F_TYPE>(0,j) - 0.5*(ell_mahal + dtjsa_Q_logdet.at<OPT_F_TYPE>(t,j) + ell_const);
00626 
00627                                                 //sensitivity analysis (for t+1)
00628                                                 Mat tmp1=MatVid::frame(Q_GtC1[j],t);
00629                                                 Mat tmp2=MatVid::frame(dtjsa_Q_Ft[j],t);
00630                                                 Mat tmp_GrFr(tmp1.rows,tmp1.cols+tmp2.cols,OPT_MAT_TYPE);                                       
00631                                                 Mat tmpM=tmp_GrFr.colRange(0,tmp1.cols);
00632                                                 tmp1.copyTo(tmpM);
00633                                                 tmpM=tmp_GrFr.colRange(tmp1.cols,tmp1.cols+tmp2.cols);
00634                                                 tmp2.copyTo(tmpM);
00635 
00636                                                 tmp1=bxt1.col(t);
00637                                                 tmp2=MatVid::frame(bxt3,j).col(t);
00638 
00639                                                 Mat tmpM2(tmp1.rows+tmp2.rows,tmp1.cols,OPT_MAT_TYPE);
00640                                                 tmpM=tmpM2.rowRange(0,tmp1.rows);
00641                                                 tmp1.copyTo(tmpM);
00642                                                 tmpM=tmpM2.rowRange(tmp1.rows,tmp1.rows+tmp2.rows);
00643                                                 tmp2.copyTo(tmpM);
00644                                                 MatVid::frame(bxt3,j).col(t+1) = tmp_GrFr * tmpM2;
00645 
00646                                                 if (useYmean)
00647                                                 {
00648                                                         MatVid::frame(bxt3,j).col(t+1) = MatVid::frame(bxt3,j).col(t+1) + tmp*C2R2Ydiff.col(j);
00649                                                 }
00650 
00651                                                 tmp1=MatVid::frame(bVt11,t);
00652                                                 tmp2=MatVid::frame(bVt13[j],t);
00653                                                 Mat tmp3=MatVid::frame(bVt13[j],t).t();
00654                                                 Mat tmp4=MatVid::frame(bVt33[j],t);
00655                                                 tmpM.create(tmp1.rows+tmp3.rows,tmp1.cols+tmp2.cols,OPT_MAT_TYPE);
00656                                                 tmpM2=tmpM(Range(0,tmp1.rows),Range(0,tmp1.cols));
00657                                                 tmp1.copyTo(tmpM2);
00658                                                 tmpM2=tmpM(Range(0,tmp1.rows),Range(tmp1.cols,tmp1.cols+tmp2.cols));
00659                                                 tmp2.copyTo(tmpM2);
00660                                                 tmpM2=tmpM(Range(tmp1.rows,tmp1.rows+tmp3.rows),Range(0,tmp1.cols));
00661                                                 tmp3.copyTo(tmpM2);
00662                                                 tmpM2=tmpM(Range(tmp1.rows,tmp1.rows+tmp3.rows),Range(tmp1.cols,tmp1.cols+tmp2.cols));
00663                                                 tmp4.copyTo(tmpM2);
00664                                                 MatVid::frame(bVt33[j],t+1) = tmp_GrFr * tmpM * tmp_GrFr.t()  + Q_GtR1Gt;
00665 
00666 
00667                                                 tmp1=MatVid::frame(bVt11,t);
00668                                                 tmp2=MatVid::frame(bVt13[j],t);
00669                                                 tmp3=MatVid::frame(bVt12,t).t();
00670                                                 tmp4=MatVid::frame(bVt23[j],t);
00671                                                 tmpM.create(tmp1.rows+tmp3.rows,tmp1.cols+tmp2.cols,OPT_MAT_TYPE);
00672                                                 tmpM2=tmpM(Range(0,tmp1.rows),Range(0,tmp1.cols));
00673                                                 tmp1.copyTo(tmpM2);
00674                                                 tmpM2=tmpM(Range(0,tmp1.rows),Range(tmp1.cols,tmp1.cols+tmp2.cols));
00675                                                 tmp2.copyTo(tmpM2);
00676                                                 tmpM2=tmpM(Range(tmp1.rows,tmp1.rows+tmp3.rows),Range(0,tmp1.cols));
00677                                                 tmp3.copyTo(tmpM2);
00678                                                 tmpM2=tmpM(Range(tmp1.rows,tmp1.rows+tmp3.rows),Range(tmp1.cols,tmp1.cols+tmp2.cols));
00679                                                 tmp4.copyTo(tmpM2);
00680                                                 MatVid::frame(bVt23[j],t+1) = tmp_GbFb * tmpM * tmp_GrFr.t()  + PQ_GtR1Gt;
00681 
00682                                                 tmp1=MatVid::frame(bVt11,t);
00683                                                 tmp2=MatVid::frame(bVt13[j],t);
00684                                                 tmpM.create(tmp1.rows,tmp1.cols+tmp2.cols,OPT_MAT_TYPE);                                        
00685                                                 tmpM2=tmpM.colRange(0,tmp1.cols);
00686                                                 tmp1.copyTo(tmpM2);
00687                                                 tmpM2=tmpM.colRange(tmp1.cols,tmp1.cols+tmp2.cols);
00688                                                 tmp2.copyTo(tmpM2);
00689 
00690                                                 MatVid::frame(bVt13[j],t+1) = A1*(tmpM*tmp_GrFr.t());
00691                                         }//388
00692 
00693                                 }//389
00694 
00695                         }//390
00696                         //end Kalman forward
00697 
00698                         // store expected log-likelihood
00699                         tmpM=Ell.row(i);
00700                         ell.copyTo(tmpM);
00701         }       
00702 }
00703 
00704 
00715 void Estats::computeAggregateStats(Mat W)
00716 {
00717         int Kb=dti.size();
00718         int Kr=dtj.size();
00719         int len=tau;
00720         int dx=dtj[0].C.cols;
00721         int dy=dtj[0].C.rows;
00722 
00723         //2. Loop THROUGH EACH DTI
00724         xij.resize(Kr);
00725         etaj.resize(Kr);;
00726         gammaj.resize(Kr);
00727         Phij.resize(Kr);
00728         varphij.resize(Kr);
00729         phij.resize(Kr);
00730         betaj.resize(Kr);
00731         Psij.resize(Kr);
00732         Lambdaj.resize(Kr);
00733         Gammaj.resize(Kr);
00734         
00735 
00736         Ell=Mat::zeros(Kb,Kr,OPT_MAT_TYPE);
00737 
00738         //constants
00739         double ell_const  = dy*log(2*M_PI);
00740 
00741         for(int i=0;i<Kb;i++)
00742         {
00743                 const Dytex &dt1=dti[i];
00744 
00745                 Mat S1;
00746                 Mat tmpM;
00747                 switch(dt1.dtopt.Sopt)
00748                 {
00749                 case CovMatrix::COV_DIAG:
00750                         S1.create(dt1.S0.mtx.rows,dt1.S0.mtx.rows,OPT_MAT_TYPE);
00751                         S1.setTo(0);
00752                         tmpM=S1.diag();
00753                         dt1.S0.mtx.copyTo(tmpM);
00754                         break;
00755 
00756                 default:
00757                         CV_Error(-1,"Cov type Not supported");
00758                 }
00759 
00760                 bool S1zero=true;
00761                 for(int m=0;m<S1.rows;m++)
00762                         for(int n=0;n<S1.cols;n++)
00763                         {
00764                                 if(S1.at<OPT_F_TYPE>(m,n)!=0)
00765                                         S1zero=false;
00766                         }
00767 
00768                         //special n=0 case
00769                         if (dx==0)
00770                         {
00771                                 CV_Error(-1,"n=0 case not supported");
00772                         }
00773 
00774                         const Mat &A1 = dt1.A;
00775                         const Mat &Q1 = dt1.Q.mtx;
00776                         const Mat &C1 = dt1.C;
00777                         const Mat &mu01 = dt1.mu0;
00778                         double r1 = dt1.R.mtx.at<OPT_F_TYPE>(0,0);
00779                         const Mat &Ymean1 = dt1.Ymean;
00780 
00781                         //some constants
00782                         Mat Ydiff=Mat::zeros(dy,Kr,OPT_MAT_TYPE);
00783                         Mat C1C1   = C1.t()*C1;
00784                         Mat C1R1C1 = (C1C1)/r1;
00785                         Mat C1R2C1 = MatVid::create(Kr,dx,dx,OPT_MAT_TYPE);
00786                         C1R2C1.setTo(0);
00787                         Mat C2R2C1 = MatVid::create(Kr,dx,dx,OPT_MAT_TYPE);
00788                         C1R2C1.setTo(0);
00789                         Mat C1R2C2 = MatVid::create(Kr,dx,dx,OPT_MAT_TYPE);
00790                         C1R2C1.setTo(0);
00791                         Mat C2R2R1R2C2 = MatVid::create(Kr,dx,dx,OPT_MAT_TYPE);
00792                         C1R2C1.setTo(0);
00793                         Mat C1R2Ydiff  = Mat::zeros(dx,Kr,OPT_MAT_TYPE);
00794                         Mat C2R2Ydiff  = Mat::zeros(dx,Kr,OPT_MAT_TYPE);
00795                         Mat C2R2C2 = MatVid::create(Kr,dx,dx,OPT_MAT_TYPE);
00796                         C1R2C1.setTo(0);
00797                         double Szero;
00798                         for(int j=0;j<Kr;j++)
00799                         {
00800                                 if(!dtjblank[j])
00801                                 {
00802                                         if (useYmean)
00803                                         {
00804                                                 Ydiff.col(j) = dt1.Ymean-dtj[j].Ymean;
00805                                         }
00806                                         if (S1zero && dtjsa_Szero.at<OPT_F_TYPE>(0,j))
00807                                         {
00808                                                 Szero = 1;
00809                                         }
00810                                         else
00811                                         {
00812                                                 if (S1zero || dtjsa_Szero.at<OPT_F_TYPE>(0,j))
00813                                                 {
00814                                                         CV_Error(-1,"both must have S=0");
00815                                                 }
00816                                                 Szero = 0;
00817                                         }
00818 
00819                                         const Mat &C2 = dtj[j].C;
00820                                         double r2 = dtj[j].R.mtx.at<OPT_F_TYPE>(0,0);
00821                                         MatVid::frame(C1R2C1,j) = (C1.t()*C1)/r2;
00822                                         MatVid::frame(C2R2C2,j) = (C2.t()*C2)/r2;
00823                                         MatVid::frame(C2R2C1,j) = (C2.t()*C1)/r2;
00824                                         MatVid::frame(C1R2C2,j) = MatVid::frame(C2R2C1,j).t();
00825                                         MatVid::frame(C2R2R1R2C2,j) = (C2.t()*C2)*r1/(pow(r2,2));
00826                                         if (useYmean)
00827                                         {
00828                                                 C1R2Ydiff.col(j) = (C1.t()*Ydiff.col(j))/r2;
00829                                                 C2R2Ydiff.col(j) = (C2.t()*Ydiff.col(j))/r2;
00830                                         }
00831                                 }
00832                         } // end cache constants 287
00833 
00834                         // initialize KALMAN (t=1)
00835                         Mat P_Vtt1=S1.clone();
00836 
00837                         // storage for Kalman smoother
00838                         std::vector<Mat> Q_GtC1;
00839                         for(int m=0;m<Kr;m++)
00840                         {
00841                                 Mat tmpM=MatVid::create(len,dx,dx,OPT_MAT_TYPE);
00842                                 tmpM.setTo(0);
00843                                 Q_GtC1.push_back(tmpM);
00844                         }
00845 
00846                         // storage for ELL
00847                         Mat ell = Mat::zeros(1,Kr,OPT_MAT_TYPE);
00848 
00849                         //initialize sensitivity analysis
00850                         Mat bxt1=Mat::zeros(dx,len+1,OPT_MAT_TYPE);
00851                         tmpM=bxt1.col(0);
00852                         mu01.copyTo(tmpM);
00853 
00854                         Mat bxt2=Mat::zeros(dx,len+1,OPT_MAT_TYPE);
00855                         tmpM=bxt2.col(0);
00856                         mu01.copyTo(tmpM);
00857 
00858                         Mat bxt3  = MatVid::create(Kr,dx,len+1,OPT_MAT_TYPE);
00859                         bxt3.setTo(0);
00860 
00861                         for(int j=0;j<Kr;j++)
00862                         {
00863                                 if(!dtjblank[j])
00864                                 {
00865                                         Mat tmpM=MatVid::frame(bxt3,j);
00866                                         Mat tmp2=tmpM.col(0);
00867                                         dtj[j].mu0.copyTo(tmp2);                                
00868                                 }
00869                         }
00870 
00871                         Mat bVt11 = MatVid::create(len+1,dx,dx,OPT_MAT_TYPE);
00872                         bVt11.setTo(0);
00873 
00874                         tmpM=MatVid::frame(bVt11,0);
00875                         S1.copyTo(tmpM);
00876 
00877                         Mat bVt12 = MatVid::create(len+1,dx,dx,OPT_MAT_TYPE);
00878                         bVt12.setTo(0);
00879 
00880                         std::vector<Mat> bVt13;
00881                         for(int j=0;j<Kr;j++)
00882                         {
00883                                 Mat tmpM=MatVid::create(len+1,dx,dx,OPT_MAT_TYPE);
00884                                 tmpM.setTo(0);
00885                                 bVt13.push_back(tmpM);
00886                         }
00887 
00888                         Mat bVt22 = MatVid::create(len+1,dx,dx,OPT_MAT_TYPE);
00889                         bVt22.setTo(0);
00890 
00891                         std::vector<Mat> bVt23;
00892                         for(int j=0;j<Kr;j++)
00893                         {
00894                                 Mat tmpM=MatVid::create(len+1,dx,dx,OPT_MAT_TYPE);
00895                                 tmpM.setTo(0);
00896                                 bVt23.push_back(tmpM);
00897                         }
00898 
00899                         std::vector<Mat> bVt33;
00900                         for(int j=0;j<Kr;j++)
00901                         {
00902                                 Mat tmpM=MatVid::create(len+1,dx,dx,OPT_MAT_TYPE);
00903                                 tmpM.setTo(0);
00904                                 bVt33.push_back(tmpM);
00905                         }
00906 
00907                         // iterate from t=1 to len              
00908                         Mat P_Vtt,P_foo,P_Wt,P_KtC1,P_GtC1,P_GtR1Gt,P_Ft,tmp_GbFb;
00909                         for(int t=0;t<len;t++)
00910                         {                       
00911                                 //KALMAN filter on P at time t
00912                                 if (t>0)
00913                                 {
00914                                         P_Vtt1   = A1*P_Vtt*A1.t()+Q1;
00915                                 }       
00916                                 if((t==0) && Szero)
00917                                 {
00918                                         P_foo  = Mat::zeros(dx,dx,OPT_MAT_TYPE);
00919                                 }
00920                                 else
00921                                 {
00922                                         P_Wt     = (P_Vtt1.inv()+C1R1C1).inv(); 
00923                                         P_foo    = P_Vtt1*(Mat::eye(dx,dx,OPT_MAT_TYPE)-C1R1C1*P_Wt);
00924                                 }
00925 
00926                                 P_KtC1    = P_foo*C1R1C1;
00927                                 P_GtC1    = A1*P_KtC1;
00928                                 P_Vtt     = P_Vtt1 - P_KtC1*P_Vtt1;
00929                                 P_GtR1Gt  = P_GtC1*P_foo.t()*A1.t();
00930                                 P_Ft      = A1-P_GtC1;
00931 
00932                                 // update sensitivity analysis for P
00933                                 tmp_GbFb.create(P_GtC1.rows,P_GtC1.cols+P_Ft.cols,OPT_MAT_TYPE);
00934                                 tmpM=tmp_GbFb.colRange(Range(0,P_GtC1.cols));
00935                                 P_GtC1.copyTo(tmpM);
00936                                 tmpM=tmp_GbFb.colRange(Range(P_GtC1.cols,P_GtC1.cols+P_Ft.cols));
00937                                 P_Ft.copyTo(tmpM);
00938 
00939                                 bxt2.col(t+1)    = P_GtC1*bxt1.col(t) + P_Ft*bxt2.col(t);
00940                                 bxt1.col(t+1)    = A1*bxt1.col(t);
00941 
00942 
00943                                 Mat tmp1=MatVid::frame(bVt11,t);
00944                                 Mat tmp2=MatVid::frame(bVt12,t);
00945                                 Mat tmp3=MatVid::frame(bVt12,t).t(); 
00946                                 Mat tmp4=MatVid::frame(bVt22,t);                
00947                                 tmpM.create(tmp1.rows+tmp3.rows,tmp1.cols+tmp2.cols,OPT_MAT_TYPE);
00948                                 Mat tmpM2=tmpM(Range(0,tmp1.rows),Range(0,tmp1.cols));
00949                                 tmp1.copyTo(tmpM2);
00950                                 tmpM2=tmpM(Range(0,tmp1.rows),Range(tmp1.cols,tmp1.cols+tmp2.cols));
00951                                 tmp2.copyTo(tmpM2);
00952                                 tmpM2=tmpM(Range(tmp1.rows,tmp1.rows+tmp3.rows),Range(0,tmp1.cols));
00953                                 tmp3.copyTo(tmpM2);
00954                                 tmpM2=tmpM(Range(tmp1.rows,tmp1.rows+tmp3.rows),Range(tmp1.cols,tmp1.cols+tmp2.cols));
00955                                 tmp4.copyTo(tmpM2);
00956 
00957                                 MatVid::frame(bVt22,t+1) = tmp_GbFb * tmpM * tmp_GbFb.t()  + P_GtR1Gt;
00958 
00959                                 tmp1=MatVid::frame(bVt11,t);
00960                                 tmp2=MatVid::frame(bVt12,t);
00961                                 tmpM.create(tmp1.rows,tmp1.cols+tmp2.cols,OPT_MAT_TYPE);
00962                                 tmpM2=tmpM.colRange(0,tmp1.cols);
00963                                 tmp1.copyTo(tmpM2);
00964                                 tmpM2=tmpM.colRange(tmp1.cols,tmp1.cols+tmp2.cols);
00965                                 tmp2.copyTo(tmpM2);
00966 
00967                                 MatVid::frame(bVt12,t+1) = A1*(tmpM*tmp_GbFb.t());
00968                                 MatVid::frame(bVt11,t+1) = A1*MatVid::frame(bVt11,t)*A1.t() + Q1;
00969 
00970                                 //compute cross-covariance
00971                                 for(int j=0;j<Kr;j++)
00972                                 {
00973                                         if(dtjblank[j])
00974                                         {
00975                                                 ell.at<OPT_F_TYPE>(0,j)=-1e300;
00976                                         }
00977                                         else
00978                                         {
00979                                                 Mat tmp = dtj[j].A*MatVid::frame(dtjsa_Q_foo[j],t);
00980                                                 MatVid::frame(Q_GtC1[j],t) = tmp*MatVid::frame(C2R2C1,j);
00981                                                 Mat Q_GtR1Gt  = tmp*MatVid::frame(C2R2R1R2C2,j)*tmp.t();
00982                                                 Mat PQ_GtR1Gt = A1*P_foo*MatVid::frame(C1R2C2,j)*tmp.t();
00983 
00984                                                 double ell_mahal;
00985                                                 if((t==0) && Szero)
00986                                                 {
00987                                                         CV_Error(-1,"not supported yet");
00988                                                 }
00989                                                 else
00990                                                 {
00991                                                         Mat tmp_QWtC2R2C1 = MatVid::frame(dtjsa_Q_Wt[j],t)*MatVid::frame(C2R2C1,j);
00992                                                         double ell_mahal1 = trace((MatVid::frame(bVt11,t)+bxt1.col(t)*bxt1.col(t).t())*(MatVid::frame(C1R2C1,j)-MatVid::frame(C1R2C2,j)*tmp_QWtC2R2C1))[0]+ dy*r1/dtj[j].R.mtx.at<OPT_F_TYPE>(0,0) - trace(MatVid::frame(dtjsa_Q_Wt[j],t)*MatVid::frame(C2R2R1R2C2,j))[0];
00993                                                         double ell_mahal2 = trace((MatVid::frame(bVt23[j],t)+bxt2.col(t)*(MatVid::frame(bxt3,j).col(t)).t())*(MatVid::frame(C2R2C1,j)-MatVid::frame(C2R2C2,j)*tmp_QWtC2R2C1))[0];
00994                                                         double ell_mahal3 = trace((MatVid::frame(bVt33[j],t)+(MatVid::frame(bxt3,j).col(t))*(MatVid::frame(bxt3,j).col(t)).t())*(MatVid::frame(C2R2C2,j)-MatVid::frame(C2R2C2,j)*MatVid::frame(dtjsa_Q_Wt[j],t)*MatVid::frame(C2R2C2,j)))[0];
00995 
00996                                                         if (useYmean)
00997                                                         {
00998                                                                 Mat tmp_QWtC2R2Ydiff = MatVid::frame(dtjsa_Q_Wt[j],t)*C2R2Ydiff.col(j);
00999                                                                 Mat tmpM=(2*(bxt1.col(t).t())*(C1R2Ydiff.col(j) - MatVid::frame(C1R2C2,j)*tmp_QWtC2R2Ydiff)+((Ydiff.col(j).t())*Ydiff.col(j))/dtj[j].R.mtx.at<OPT_F_TYPE>(0,0)-(C2R2Ydiff.col(j).t())*tmp_QWtC2R2Ydiff);
01000                                                                 ell_mahal1 = ell_mahal1 + tmpM.at<OPT_F_TYPE>(0,0);
01001 
01002                                                                 tmpM=(MatVid::frame(bxt3,j).col(t)).t()*(C2R2Ydiff.col(j) - MatVid::frame(C2R2C2,j)*tmp_QWtC2R2Ydiff);
01003                                                                 ell_mahal2 = ell_mahal2 + tmpM.at<OPT_F_TYPE>(0,0);
01004                                                         }
01005                                                         ell_mahal = ell_mahal1 - 2*ell_mahal2 + ell_mahal3;
01006                                                 }//376
01007 
01008                                                 ell.at<OPT_F_TYPE>(0,j) = ell.at<OPT_F_TYPE>(0,j) - 0.5*(ell_mahal + dtjsa_Q_logdet.at<OPT_F_TYPE>(t,j) + ell_const);
01009 
01010                                                 //sensitivity analysis (for t+1)
01011                                                 Mat tmp1=MatVid::frame(Q_GtC1[j],t);
01012                                                 Mat tmp2=MatVid::frame(dtjsa_Q_Ft[j],t);
01013                                                 Mat tmp_GrFr(tmp1.rows,tmp1.cols+tmp2.cols,OPT_MAT_TYPE);                                       
01014                                                 Mat tmpM=tmp_GrFr.colRange(0,tmp1.cols);
01015                                                 tmp1.copyTo(tmpM);
01016                                                 tmpM=tmp_GrFr.colRange(tmp1.cols,tmp1.cols+tmp2.cols);
01017                                                 tmp2.copyTo(tmpM);
01018 
01019                                                 tmp1=bxt1.col(t);
01020                                                 tmp2=MatVid::frame(bxt3,j).col(t);
01021 
01022                                                 Mat tmpM2(tmp1.rows+tmp2.rows,tmp1.cols,OPT_MAT_TYPE);
01023                                                 tmpM=tmpM2.rowRange(0,tmp1.rows);
01024                                                 tmp1.copyTo(tmpM);
01025                                                 tmpM=tmpM2.rowRange(tmp1.rows,tmp1.rows+tmp2.rows);
01026                                                 tmp2.copyTo(tmpM);
01027                                                 MatVid::frame(bxt3,j).col(t+1) = tmp_GrFr * tmpM2;
01028 
01029                                                 if (useYmean)
01030                                                 {
01031                                                         MatVid::frame(bxt3,j).col(t+1) = MatVid::frame(bxt3,j).col(t+1) + tmp*C2R2Ydiff.col(j);
01032                                                 }
01033 
01034                                                 tmp1=MatVid::frame(bVt11,t);
01035                                                 tmp2=MatVid::frame(bVt13[j],t);
01036                                                 Mat tmp3=MatVid::frame(bVt13[j],t).t();
01037                                                 Mat tmp4=MatVid::frame(bVt33[j],t);
01038                                                 tmpM.create(tmp1.rows+tmp3.rows,tmp1.cols+tmp2.cols,OPT_MAT_TYPE);
01039                                                 tmpM2=tmpM(Range(0,tmp1.rows),Range(0,tmp1.cols));
01040                                                 tmp1.copyTo(tmpM2);
01041                                                 tmpM2=tmpM(Range(0,tmp1.rows),Range(tmp1.cols,tmp1.cols+tmp2.cols));
01042                                                 tmp2.copyTo(tmpM2);
01043                                                 tmpM2=tmpM(Range(tmp1.rows,tmp1.rows+tmp3.rows),Range(0,tmp1.cols));
01044                                                 tmp3.copyTo(tmpM2);
01045                                                 tmpM2=tmpM(Range(tmp1.rows,tmp1.rows+tmp3.rows),Range(tmp1.cols,tmp1.cols+tmp2.cols));
01046                                                 tmp4.copyTo(tmpM2);
01047                                                 MatVid::frame(bVt33[j],t+1) = tmp_GrFr * tmpM * tmp_GrFr.t()  + Q_GtR1Gt;
01048 
01049 
01050                                                 tmp1=MatVid::frame(bVt11,t);
01051                                                 tmp2=MatVid::frame(bVt13[j],t);
01052                                                 tmp3=MatVid::frame(bVt12,t).t();
01053                                                 tmp4=MatVid::frame(bVt23[j],t);
01054                                                 tmpM.create(tmp1.rows+tmp3.rows,tmp1.cols+tmp2.cols,OPT_MAT_TYPE);
01055                                                 tmpM2=tmpM(Range(0,tmp1.rows),Range(0,tmp1.cols));
01056                                                 tmp1.copyTo(tmpM2);
01057                                                 tmpM2=tmpM(Range(0,tmp1.rows),Range(tmp1.cols,tmp1.cols+tmp2.cols));
01058                                                 tmp2.copyTo(tmpM2);
01059                                                 tmpM2=tmpM(Range(tmp1.rows,tmp1.rows+tmp3.rows),Range(0,tmp1.cols));
01060                                                 tmp3.copyTo(tmpM2);
01061                                                 tmpM2=tmpM(Range(tmp1.rows,tmp1.rows+tmp3.rows),Range(tmp1.cols,tmp1.cols+tmp2.cols));
01062                                                 tmp4.copyTo(tmpM2);
01063                                                 MatVid::frame(bVt23[j],t+1) = tmp_GbFb * tmpM * tmp_GrFr.t()  + PQ_GtR1Gt;
01064 
01065                                                 tmp1=MatVid::frame(bVt11,t);
01066                                                 tmp2=MatVid::frame(bVt13[j],t);
01067                                                 tmpM.create(tmp1.rows,tmp1.cols+tmp2.cols,OPT_MAT_TYPE);                                        
01068                                                 tmpM2=tmpM.colRange(0,tmp1.cols);
01069                                                 tmp1.copyTo(tmpM2);
01070                                                 tmpM2=tmpM.colRange(tmp1.cols,tmp1.cols+tmp2.cols);
01071                                                 tmp2.copyTo(tmpM2);
01072 
01073                                                 MatVid::frame(bVt13[j],t+1) = A1*(tmpM*tmp_GrFr.t());
01074                                         }//388
01075 
01076                                 }//389
01077 
01078                         }//390
01079 
01080                         // store expected log-likelihood
01081                         tmpM=Ell.row(i);
01082                         ell.copyTo(tmpM);
01083 
01084                         //constants
01085                         Mat ut_init = C1*bxt1.colRange(0,tau);
01086                         Mat ut_inits;
01087                         reduce(ut_init,ut_inits,1,CV_REDUCE_SUM);
01088 
01089                         double Ut_init = 0;
01090                         for(int t=0;t<len;t++)
01091                         {
01092                                 Ut_init = Ut_init + trace(MatVid::frame(bVt11,t)*(C1C1))[0] + dy*r1;
01093                         }
01094 
01095                         //Sensitivity Analysis for Kalman Smoothing Filter %%%
01096 
01097                         for(int j=0;j<Kr;j++)
01098                         {
01099                                 if(!dtjblank[j])
01100                                 {
01101                                         //initialize aggregate statistics
01102                                         Mat xij= Mat::zeros(dx,1,OPT_MAT_TYPE);
01103                                         Mat etaj=MatVid::frame(dtjsa_etaj_init,j).clone();
01104                                         Mat Phij=MatVid::frame(dtjsa_Phij_init,j).clone();
01105                                         Mat varphij=MatVid::frame(dtjsa_varphij_init,j).clone();
01106                                         Mat phij=MatVid::frame(dtjsa_phij_init,j).clone();
01107                                         Mat betaj=Mat::zeros(dx,1,OPT_MAT_TYPE);
01108                                         Mat Psij=MatVid::frame(dtjsa_Psij_init,j).clone();
01109                                         Mat Gammaj=Mat::zeros(dy,dx,OPT_MAT_TYPE);
01110 
01111                                         Mat d_ut,udiff;
01112                                         Mat gammaj; 
01113                                         if(useYmean)
01114                                         {
01115                                                 d_ut   = Ymean1;
01116                                                 Mat tmpM;
01117                                                 repeat(Ydiff.col(j),1,len,tmpM);
01118                                                 udiff  = ut_init + tmpM;
01119                                                 gammaj    = ut_inits  + d_ut*len;
01120                                         }
01121                                         else
01122                                         {
01123                                                 d_ut   = Mat::zeros(1,1,OPT_MAT_TYPE);
01124                                                 udiff  = ut_init; //reference NO change
01125                                                 gammaj    = ut_inits;
01126                                         }
01127 
01128                                         double Lambdaj   = Ut_init + trace(udiff.t()*udiff)[0];
01129 
01130 
01131                                         // initialize
01132                                         Mat xtb = MatVid::frame(dtjsa_iA2,j)*(MatVid::frame(bxt3,j).col(len));
01133                                         Mat Xit = MatVid::frame(dtjsa_iA2,j)*MatVid::frame(bVt33[j],len)*(MatVid::frame(dtjsa_iA2,j).t());
01134                                         Mat Mt  = Mat::zeros(dx,dx,OPT_MAT_TYPE);
01135                                         Mat omt = Mat::zeros(dx,dx,OPT_MAT_TYPE);
01136 
01137                                         Mat Xit_t1,xtb_t1;
01138                                         for(int t=len-1;t>=0;t--)
01139                                         {
01140                                                 Mat LGCM = MatVid::frame(dtjsa_Lt[j],t)*MatVid::frame(Q_GtC1[j],t) + Mt;
01141 
01142                                                 Mat omt_t1 = omt.clone();                                       
01143                                                 Mat Omt = LGCM*MatVid::frame(bVt11,t)+ MatVid::frame(dtjsa_LF[j],t)*(MatVid::frame(bVt23[j],t).t());
01144                                                 omt = LGCM*MatVid::frame(bVt23[j],t) + MatVid::frame(dtjsa_LF[j],t)*MatVid::frame(bVt33[j],t);
01145 
01146                                                 Mat kappat = C1*(Omt.t()) + (MatVid::frame(dtjsa_LtGt[j],t)*r1).t();
01147 
01148 
01149                                                 //%%% compute statistics %%%
01150                                                 Mat d_Pt     = Xit + xtb*xtb.t();
01151                                                 Mat d_Ptt1;
01152                                                 if (t<(len-1))
01153                                                 {
01154                                                         Mat Xitt1 = omt_t1*(MatVid::frame(dtjsa_Q_Ht[j],t).t()) + Xit_t1*(MatVid::frame(dtjsa_Q_Jt[j],t).t());
01155                                                         d_Ptt1  = Xitt1 + xtb_t1*xtb.t();
01156                                                 }
01157                                                 Mat Wt     = kappat + udiff.col(t)*xtb.t();
01158 
01159                                                 //%%% aggregate statistics
01160                                                 if (t==0)
01161                                                 {
01162                                                         xij     = xtb.clone();
01163                                                         etaj    = etaj + d_Pt;
01164                                                 }
01165                                                 else
01166                                                 {
01167                                                         varphij = varphij + d_Pt;
01168                                                 }
01169                                                 Phij      = Phij    + d_Pt;
01170                                                 betaj     = betaj   + xtb;
01171                                                 Gammaj    = Gammaj  + Wt;
01172                                                 if (t<(tau-1))
01173                                                 {
01174                                                         phij    = phij    + d_Pt;
01175                                                         Psij    = Psij    + d_Ptt1;
01176                                                 }
01177 
01178                                                 //%%% update sensitivity analysis
01179                                                 if(t>0)
01180                                                 {
01181                                                         xtb_t1 = xtb.clone();
01182                                                         Xit_t1 = Xit.clone();                                                                           
01183 
01184                                                         Mat tmp1=MatVid::frame(dtjsa_Q_Ht[j],t-1);
01185                                                         Mat tmp2=MatVid::frame(dtjsa_Q_Jt[j],t-1);
01186                                                         Mat QHJ(tmp1.rows,tmp1.cols+tmp2.cols,OPT_MAT_TYPE);                                    
01187                                                         Mat tmpM2=QHJ.colRange(0,tmp1.cols);
01188                                                         tmp1.copyTo(tmpM2);
01189                                                         tmpM2=QHJ.colRange(tmp1.cols,tmp1.cols+tmp2.cols);
01190                                                         tmp2.copyTo(tmpM2);
01191 
01192 
01193 
01194                                                         Mat tmp3=MatVid::frame(bxt3,j).col(t);
01195                                                         Mat tmp4=xtb;
01196                                                         Mat tmpM(tmp3.rows+tmp4.rows,tmp3.cols,OPT_MAT_TYPE);                                   
01197                                                         tmpM2=tmpM.rowRange(0,tmp3.rows);
01198                                                         tmp3.copyTo(tmpM2);
01199                                                         tmpM2=tmpM.rowRange(tmp3.rows,tmp3.rows+tmp4.rows);
01200                                                         tmp4.copyTo(tmpM2);
01201                                                         xtb   = QHJ * tmpM;
01202 
01203 
01204                                                         tmp1=MatVid::frame(bVt33[j],t);
01205                                                         Mat tmp5=omt.t();
01206                                                         tmp2=tmp5;
01207                                                         tmp3=omt;
01208                                                         tmp4=Xit;
01209                                                         tmpM.create(tmp1.rows+tmp3.rows,tmp1.cols+tmp2.cols,OPT_MAT_TYPE);
01210                                                         tmpM2=tmpM(Range(0,tmp1.rows),Range(0,tmp1.cols));
01211                                                         tmp1.copyTo(tmpM2);
01212                                                         tmpM2=tmpM(Range(0,tmp1.rows),Range(tmp1.cols,tmp1.cols+tmp2.cols));
01213                                                         tmp2.copyTo(tmpM2);
01214                                                         tmpM2=tmpM(Range(tmp1.rows,tmp1.rows+tmp3.rows),Range(0,tmp1.cols));
01215                                                         tmp3.copyTo(tmpM2);
01216                                                         tmpM2=tmpM(Range(tmp1.rows,tmp1.rows+tmp3.rows),Range(tmp1.cols,tmp1.cols+tmp2.cols));
01217                                                         tmp4.copyTo(tmpM2);
01218 
01219                                                         Xit   = QHJ * tmpM * QHJ.t();
01220 
01221                                                         Mt = MatVid::frame(dtjsa_Q_Jt[j],t-1)*LGCM*A1;
01222                                                 }
01223 
01224                                         }//501
01225 
01226                                         //save and aggregate statistics
01227                                         this->xij[j]     = this->xij[j]+W.at<OPT_F_TYPE>(i,j)*xij;
01228                                         this->etaj[j]    = this->etaj[j]+W.at<OPT_F_TYPE>(i,j)*etaj;
01229                                         this->gammaj[j]  = this->gammaj[j]+W.at<OPT_F_TYPE>(i,j)*(gammaj  / tau);
01230                                         this->Phij[j]    = this->Phij[j]+W.at<OPT_F_TYPE>(i,j)*(Phij    / tau);
01231                                         this->varphij[j] = this->varphij[j]+W.at<OPT_F_TYPE>(i,j)*(varphij / (tau-1));
01232                                         this->phij[j]    = this->phij[j]+W.at<OPT_F_TYPE>(i,j)*(phij    / (tau-1));
01233                                         this->betaj[j]   = this->betaj[j]+W.at<OPT_F_TYPE>(i,j)*(betaj   / tau);
01234                                         this->Psij[j]    = this->Psij[j]+W.at<OPT_F_TYPE>(i,j)*(Psij    / (tau-1));
01235                                         this->Lambdaj[j] = this->Lambdaj[j]+W.at<OPT_F_TYPE>(i,j)*(Lambdaj / tau);
01236                                         this->Gammaj[j]  = this->Gammaj[j]+W.at<OPT_F_TYPE>(i,j)*(Gammaj  / tau);                                       
01237 
01238                                 }//522
01239 
01240                         }//523
01241 
01242         }//525
01243 
01244 }
01245 
01246 
01266 double Estats::logdetiid(Mat Q, double r, int C)
01267 {
01268         double ld;
01269         Mat tmpM,tmpM2;
01270         eigen(Q,tmpM);
01271         log(tmpM/r+1,tmpM2);
01272         Scalar ts=sum(tmpM2);
01273         ld=ts[0]+C*log(r);
01274         return ld;
01275 }

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