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
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
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
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
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
00179 Mat Q_curVtt1=S2;
00180
00181
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
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 }
00231
00232
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
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 }
00287
00288
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);
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);
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);
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);
00328 tmpN3.copyTo(tmpM3);
00329
00330 tmpM=dtjsa_Q_logdet.col(j);
00331 Q_logdet.copyTo(tmpM);
00332
00333 }
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
00350
00351 Ell=Mat::zeros(Kb,Kr,OPT_MAT_TYPE);
00352
00353
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
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
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
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 }
00449
00450
00451 Mat P_Vtt1=S1.clone();
00452
00453
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
00463 Mat ell = Mat::zeros(1,Kr,OPT_MAT_TYPE);
00464
00465
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
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
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
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
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
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 }
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
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 }
00692
00693 }
00694
00695 }
00696
00697
00698
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
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
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
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
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 }
00833
00834
00835 Mat P_Vtt1=S1.clone();
00836
00837
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
00847 Mat ell = Mat::zeros(1,Kr,OPT_MAT_TYPE);
00848
00849
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
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
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
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
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 }
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
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 }
01075
01076 }
01077
01078 }
01079
01080
01081 tmpM=Ell.row(i);
01082 ell.copyTo(tmpM);
01083
01084
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
01096
01097 for(int j=0;j<Kr;j++)
01098 {
01099 if(!dtjblank[j])
01100 {
01101
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;
01125 gammaj = ut_inits;
01126 }
01127
01128 double Lambdaj = Ut_init + trace(udiff.t()*udiff)[0];
01129
01130
01131
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
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
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
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 }
01225
01226
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 }
01239
01240 }
01241
01242 }
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 }