/Home/aooliver/hepsoft/lcdroot/LCDRootApps/PhUtil/src/LCDVToplVRT.cxx

Go to the documentation of this file.
00001 // ----------------------------------------------------------------------------
00002 // $Id: LCDVToplVRT.cxx,v 1.5 2001/10/08 17:54:17 toshi Exp $
00003 // ----------------------------------------------------------------------------
00004 
00005 #include "LCDVToplVRT.h"
00006 #include <iostream.h>
00007 
00008 ///////////////////////////////////////////////////////////
00009 //
00010 // LCDVToplVRT
00011 //
00012 
00013 ClassImp(LCDVToplVRT)
00014 
00015 // constructors & deconstructors
00016 
00017 LCDVToplVRT::LCDVToplVRT() {
00018   Init(TVector3(0.0,0.0,0.0));
00019 }
00020 
00021 LCDVToplVRT::LCDVToplVRT(TVector3 xinit) {
00022   Init(xinit);
00023 }
00024 
00025 LCDVToplVRT::LCDVToplVRT(TObjArray* tracks) {
00026   Init(tracks,TVector3(0.0,0.0,0.0));
00027 }
00028 
00029 LCDVToplVRT::LCDVToplVRT(TObjArray* tracks,TVector3 xinit) {
00030   Init(tracks,xinit);
00031 }
00032 
00033 LCDVToplVRT::LCDVToplVRT(const LCDVToplVRT& vrt) {
00034   m_maxtry    = vrt.m_maxtry;
00035   m_epscut    = vrt.m_epscut;
00036   m_ntrk      = vrt.m_ntrk;
00037   Int_t i;
00038   for (i=0 ; i < 100 ; i++) {
00039     m_track_list[i]=vrt.m_track_list[i];
00040   }
00041   for (i=0 ; i < 3 ; i++) {
00042     m_fitparm[i] = vrt.m_fitparm[i];
00043   }
00044   for (i=0 ; i < 6 ; i++) {
00045     m_error[i]   = vrt.m_error[i];
00046   }
00047   m_a4vec     = vrt.m_a4vec;
00048   m_qvtx      = vrt.m_qvtx;
00049   m_chisquared= vrt.m_chisquared;
00050   m_vsig      = vrt.m_vsig;
00051   m_sortval   = vrt.m_sortval;
00052   m_mcvrt     = vrt.m_mcvrt;
00053 }
00054 
00055 LCDVToplVRT::~LCDVToplVRT(){
00056   //  if (m_fitparm) delete m_fitparm;
00057   //  m_fitparm=0;
00058   //  if (m_error)   delete m_error;
00059   //  m_error  =0;
00060 }
00061 
00062 LCDVToplVRT & LCDVToplVRT::operator = (const LCDVToplVRT & p) {
00063   m_maxtry    = p.m_maxtry;
00064   m_epscut    = p.m_epscut;
00065   m_ntrk      = p.m_ntrk;
00066   Int_t       i=0;
00067   for (i=0 ; i < 100 ; i++) {
00068     m_track_list[i]=p.m_track_list[i];
00069   }
00070   for (i=0 ; i < 3 ; i++) {
00071     m_fitparm[i] = p.m_fitparm[i];
00072   }
00073   for (i=0 ; i < 6 ; i++) {
00074     m_error[i]   = p.m_error[i];
00075   }
00076   m_a4vec     = p.m_a4vec;
00077   m_qvtx      = p.m_qvtx;
00078   m_chisquared= p.m_chisquared;
00079   m_vsig      = p.m_vsig;
00080   m_sortval   = p.m_sortval;
00081   m_mcvrt     = p.m_mcvrt;
00082   return *this;
00083 }
00084 
00085 LCDVToplVRT & LCDVToplVRT::operator += (const LCDVToplVRT & p) {
00086   Int_t ntrk=p.m_ntrk;
00087   Int_t itrk=0;
00088   LCDVToplTRK* trk=0;
00089   for (itrk=0 ; itrk < ntrk ; itrk++) {
00090     trk=p.m_track_list[itrk];
00091     AddTrack(trk,1);
00092   }
00093   return *this;
00094 }
00095 
00096 //Initialize LCDVToplVRT
00097 void LCDVToplVRT::Init(TVector3 xinit){
00098   Int_t i=0;
00099 
00100   m_ntrk       = 0;
00101   m_maxtry     = 50;
00102   m_epscut     = 1E-3;
00103   for (i=0 ; i < 3 ; i++) { m_fitparm[i] = xinit[i]; }
00104   m_error[0]=1;
00105   m_error[1]=0;
00106   m_error[2]=1;
00107   m_error[3]=0;
00108   m_error[4]=0;
00109   m_error[5]=1;
00110   for (i=0 ; i < 100 ; i++) { m_track_list[i]=0;}
00111   m_a4vec      = TLorentzVector(0.0,0.0,0.0,0.0);
00112   m_chisquared = 0;
00113   m_qvtx       = 0;
00114   m_vsig       =-1;
00115   m_sortval    = 0;
00116   m_mcvrt      = 0;
00117 }
00118 
00119 void LCDVToplVRT::Init(TObjArray* tracks, TVector3 xinit){
00120   Init(xinit);
00121 
00122   Int_t        i     = 0;
00123   Int_t        ntrks = tracks->GetEntries();
00124   LCDVToplTRK* trk   = 0;
00125   for (i=0 ; i < ntrks ; i++) {
00126     trk=(LCDVToplTRK*)(tracks->At(i));
00127     AddTrack(trk);
00128   }
00129   if (GetChi2()>0) {
00130   //  CalcFourVector();
00131     m_vsig  =0;
00132   }
00133 }
00134 
00135 //
00136 void LCDVToplVRT::Clean() {
00137   Init(TVector3(0,0,0));
00138 }
00139 
00140 //Get chi2 contribution of the given track
00141 Double_t LCDVToplVRT::GetPchi2(Int_t itrk) {
00142   LCDVToplTRK* trk=GetTrackAt(itrk);
00143 
00144   Int_t i=0;
00145   Double_t det_tmp=0;
00146 
00147   //   setup vertex position.
00148   TMatrixD x0(3,1); GetVertex(x0);
00149 
00150   //   setup track parameter and its error
00151   TMatrixD p0(5,1);
00152   for (i=0 ; i < 5 ; i++) { p0(i,0)=trk->GetTrackParameter(i); }
00153   TMatrixD g(5,5);
00154   trk->GetErrorMatrix(g);
00155   g.Invert(&det_tmp);
00156   if (det_tmp == 0) {
00157     cout<<"g inv. fault!! in LCDVToplVRT::GetPchi2"<<endl;
00158     return 1e20;
00159   }
00160 
00161   //   setup vertex momentum.
00162   TMatrixD q0(3,1); SmoothingParameters(trk,q0);  
00163 
00164   //   loop over till chi**2 is converged.
00165   TMatrixD a(5,3);
00166   TMatrixD b(5,3);
00167   TMatrixD h0(5,1);
00168   linearizeChargedTrack(a,b,h0,x0,q0);
00169   TMatrixD r(p0)                    ; r -= h0;
00170   TMatrixD ax0(a,TMatrixD::kMult,x0); r -= ax0;
00171   TMatrixD bq0(b,TMatrixD::kMult,q0); r -= bq0;
00172   TMatrixD gr(g,TMatrixD::kMult,r);
00173   TMatrixD rgr(r,TMatrixD::kTransposeMult,gr);
00174 
00175   //  return rgr(0,0);
00176 
00177   //   setup inverse of covariance error matrix
00178   //TMatrixD cinv(TMatrixD::kInverted,m_error);
00179   TMatrixD cinv(3,3); GetErrorMatrixVertex(cinv);
00180   cinv.Invert(&det_tmp);
00181   if (det_tmp == 0) {
00182     cout<<"cinv inv. fault!! in LCDVToplVRT::GetPchi2"<<endl;
00183     return 1e20;
00184   }
00185 
00186   TMatrixD x(x0);
00187   TMatrixD q(q0);
00188   Double_t dcsold=1e20;
00189   Double_t chisq=0.0;
00190   Int_t iteration=0;
00191   for (iteration=0; iteration < m_maxtry ; iteration++) {
00192     linearizeChargedTrack(a,b,h0,x,q);
00193     TMatrixD g_b(g,TMatrixD::kMult,b);
00194     TMatrixD w(b,TMatrixD::kTransposeMult,g_b);
00195     w.Invert(&det_tmp);
00196     if (det_tmp == 0) {
00197       cout<<"w inv. fault!! in LCDVToplVRT::GetPchi2"<<endl;
00198       return 1e20;
00199     }
00200     TMatrixD    bTg(b,TMatrixD::kTransposeMult,    g);
00201     TMatrixD   wbTg(w,TMatrixD::kMult         ,  bTg);
00202     TMatrixD  bwbTg(b,TMatrixD::kMult         , wbTg);
00203     TMatrixD gbwbTg(g,TMatrixD::kMult         ,bwbTg);
00204 
00205     TMatrixD gb(g); gb -= gbwbTg;
00206     TMatrixD gba(gb,TMatrixD::kMult,a);
00207     TMatrixD cov(cinv); 
00208     TMatrixD aTgba(a,TMatrixD::kTransposeMult,gba); cov -= aTgba;
00209     cov.Invert(&det_tmp);
00210     if (det_tmp == 0) {
00211       cout<<"cov inv. fault!! in LCDVToplVRT::GetPchi2"<<endl;
00212       return 1e20;
00213     }
00214 
00215     TMatrixD p0h0(p0); p0h0 -= h0;
00216     TMatrixD gbp0h0(gb,TMatrixD::kMult,p0h0);
00217     TMatrixD aTgbp0h0(a,TMatrixD::kTransposeMult,gbp0h0);
00218     TMatrixD mtmp(cinv,TMatrixD::kMult,x0); mtmp -= aTgbp0h0;
00219     x.Mult(cov,mtmp);
00220 
00221     TMatrixD ax(a,TMatrixD::kMult,x);
00222     TMatrixD p0h0ax(p0h0); p0h0ax -= ax;
00223     TMatrixD gp0h0ax(g,TMatrixD::kMult,p0h0ax);
00224     TMatrixD bTgp0h0ax(b,TMatrixD::kTransposeMult,gp0h0ax);
00225     q.Mult(w,bTgp0h0ax);
00226 
00227     //TMatrixD covinv(TMatrixD::kInverted,cov);
00228     TMatrixD covinv(cov);
00229     covinv.Invert(&det_tmp);
00230     if (det_tmp == 0) {
00231       cout<<"covinv inv. fault!! in LCDVToplVRT::GetPchi2"<<endl;
00232       return 1e20;
00233     }
00234 
00235     TMatrixD xx0(x); xx0 -= x0;
00236     TMatrixD covinvxx0(covinv,TMatrixD::kMult,xx0);
00237     TMatrixD mdeltachi2(xx0,TMatrixD::kTransposeMult,covinvxx0);
00238 
00239     chisq=mdeltachi2(0,0);
00240     if (iteration > 0 && TMath::Abs(chisq-dcsold) < m_epscut) break;
00241     
00242     // Iteration step is diverged...return the best fit...
00243     if (chisq > dcsold) {
00244       chisq = dcsold;
00245       break;
00246     }
00247 
00248     dcsold=chisq;
00249   }
00250 
00251   return rgr(0,0)+chisq;
00252 }
00253 
00254 //Get Vertex Position(Vector)
00255 void LCDVToplVRT::GetVertex(TMatrixD& a) {
00256   a(0,0) = m_fitparm[0];
00257   a(1,0) = m_fitparm[1];
00258   a(2,0) = m_fitparm[2];
00259 }
00260 
00261 TVector3 LCDVToplVRT::GetVertexVector() { 
00262   return TVector3(m_fitparm[0],m_fitparm[1],m_fitparm[2]); 
00263 }
00264 
00265 //Get Error Matrix(position)
00266 void LCDVToplVRT::GetErrorMatrixVertex(TMatrixD& emvrt) {
00267   emvrt(0,0) = m_error[0];
00268   emvrt(1,0) = m_error[1];
00269   emvrt(1,1) = m_error[2];
00270   emvrt(2,0) = m_error[3];
00271   emvrt(2,1) = m_error[4];
00272   emvrt(2,2) = m_error[5];
00273   emvrt(0,1) = emvrt(1,0);
00274   emvrt(0,2) = emvrt(2,0);
00275   emvrt(1,2) = emvrt(2,1);
00276 }
00277 
00278 //Get decay length
00279 Double_t LCDVToplVRT::GetDecayLength(TVector3 xpt) {
00280   return (GetVertexVector() - xpt).Mag();
00281 }
00282 
00283 //Get error of decay length
00284 Double_t LCDVToplVRT::GetErrorDecayLength(TVector3 xpt, TMatrixD& em_xpt) {
00285   TVector3 dx=GetVertexVector() - xpt;
00286   Double_t d=dx.Mag();
00287 
00288   TMatrixD dddxT(3,1);
00289   dddxT(0,0) = dx[0]/d;
00290   dddxT(1,0) = dx[1]/d;
00291   dddxT(2,0) = dx[2]/d;
00292   TMatrixD emvrt(3,3); GetErrorMatrixVertex(emvrt);
00293   emvrt += em_xpt;
00294   TMatrixD tmp0(emvrt,TMatrixD::kMult,dddxT);
00295   TMatrixD tmp1(dddxT,TMatrixD::kTransposeMult,tmp0);
00296 
00297   return tmp1(0,0);
00298 }
00299 
00300 //Get # of significance track
00301 /*
00302 Int_t LCDVToplVRT::GetNsig(Double_t nsigma, TVector3 axis, TVector3 ipv, 
00303                            TMatrixD& eipv) {
00304   LCDVToplTRK* trk=0;
00305   Int_t itrk=0;
00306   Int_t ntrk=GetTrackEntries();
00307   Int_t nsig=0;
00308   Double_t l=0;
00309   for (itrk=0 ; itrk < ntrk ; itrk++) {
00310     trk=GetTrackAt(itrk);
00311     l=trk->CalcPOCA3(ipv);
00312     if (trk->GetSignedDistanceNormByError(l,ipv,axis,eipv) > nsigma) nsig++;
00313   }
00314   return nsig;
00315 }
00316 */
00317 //Get track momentum vector
00318 TVector3 LCDVToplVRT::GetTrackMomentumVector(Int_t itrk) {
00319   LCDVToplTRK* trk=GetTrackAt(itrk);
00320   TMatrixD q(3,1);
00321   SmoothingParameters(trk,q);
00322   Double_t pt=trk->GetMagneticField()/333.567/TMath::Abs(q(0,0));
00323   return TVector3(pt*TMath::Cos(q(1,0)),pt*TMath::Sin(q(1,0)),pt*q(2,0));
00324 }
00325 
00326 TLorentzVector LCDVToplVRT::GetTrack4MomentumVector(Int_t itrk) {
00327   LCDVToplTRK* trk=GetTrackAt(itrk);
00328   TMatrixD q(3,1);
00329   SmoothingParameters(trk,q);
00330   Double_t pt=trk->GetMagneticField()/333.567/TMath::Abs(q(0,0));
00331   Double_t mpi=0.13956995;
00332   return TLorentzVector(pt*TMath::Cos(q(1,0)),pt*TMath::Sin(q(1,0)),pt*q(2,0),
00333                         TMath::Sqrt(pt*(1.0+q(2,0)*q(2,0))*pt + mpi*mpi));
00334 }
00335 //Get track position vector
00336 /*
00337 TVector3 LCDVToplVRT::GetTrackPositionVector(Int_t itrk) {
00338   Double_t l=GetLTrackAt(itrk);
00339   LCDVToplTRK* trk=GetTrackAt(itrk);
00340   return (trk->GetPositionVector(l));
00341 }
00342 */
00343 
00344 //Set Vertex
00345 void LCDVToplVRT::SetVertex(Double_t* a) {
00346   m_fitparm[0]=a[0];
00347   m_fitparm[1]=a[1];
00348   m_fitparm[2]=a[2];
00349 }
00350 
00351 void LCDVToplVRT::SetVertex(TVector3 a) {
00352   m_fitparm[0]=a[0];
00353   m_fitparm[1]=a[1];
00354   m_fitparm[2]=a[2];
00355 }
00356 
00357 void LCDVToplVRT::SetVertex(TMatrixD& a) {
00358   m_fitparm[0]=a(0,0);
00359   m_fitparm[1]=a(1,0);
00360   m_fitparm[2]=a(2,0);
00361 }
00362 
00363 // Set Vertex Error
00364 void LCDVToplVRT::SetErrorMatrixVertex(TMatrixD& err) {
00365   m_error[0]=err(0,0);
00366   m_error[1]=err(1,0);
00367   m_error[2]=err(1,1);
00368   m_error[3]=err(2,0);
00369   m_error[4]=err(2,1);
00370   m_error[5]=err(2,2);
00371 }
00372 
00373 //Add a track
00374 Int_t LCDVToplVRT::AddTrack(LCDVToplTRK* trk, Int_t f_fit) {
00375   Int_t ntrks_prev=GetTrackEntries();
00376 
00377   Bool_t f_same_track=kFALSE;
00378   Int_t itrk=0;
00379   for (itrk=0 ; itrk < ntrks_prev ; itrk++) {
00380     if (trk == GetTrackAt(itrk)) {
00381       f_same_track = kTRUE;
00382       break;
00383     }
00384   }
00385   if (f_same_track) { return 0; }
00386 
00387   m_qvtx += trk->GetCharge();
00388 
00389   if (m_ntrk < 99) {
00390     m_track_list[m_ntrk++]=trk;
00391 
00392     if (trk->GetTrackID() >= 0 && f_fit > 0) {
00393       if (FilteringToAddTrack(trk) < 0) {
00394         cout<<"Something wrong FilteringToAddTrack vrt="<<this<<" "
00395             <<"trk="<<trk<<" "
00396             <<endl;
00397         return -1;
00398       }
00399     }
00400     return 0;
00401   } else {
00402     fprintf(stderr,"LCDVToplVRT::AddTrack  # of tracks is over 100!\n");
00403     return -2;
00404   }
00405 }
00406 
00407 //Add tracks
00408 void LCDVToplVRT::AddTracks(TObjArray* addtracks) {
00409   AddTracks(addtracks,1);
00410 }
00411 
00412 void LCDVToplVRT::AddTracks(TObjArray* addtracks, Int_t f_fit) {
00413   Int_t        ntrks=addtracks->GetEntries();
00414   LCDVToplTRK* trk  =0;
00415   for (Int_t i=0 ; i < ntrks ; i++) {
00416     trk=(LCDVToplTRK*)(addtracks->At(i));
00417     AddTrack(trk,f_fit);
00418   }
00419 }
00420 
00421 //Remove a track
00422 Int_t LCDVToplVRT::RemoveTrackAt(Int_t itrk) {
00423   return RemoveTrackAt(itrk,1);
00424 }
00425 
00426 Int_t LCDVToplVRT::RemoveTrackAt(Int_t itrk, Int_t f_fit) {
00427   return RemoveTrack(GetTrackAt(itrk),f_fit);
00428 }
00429 
00430 Int_t LCDVToplVRT::RemoveTrack(LCDVToplTRK* trk) {
00431   return RemoveTrack(trk,1);
00432 }
00433 
00434 Int_t LCDVToplVRT::RemoveTrack(LCDVToplTRK* trk,Int_t f_fit) {
00435   Int_t  ntrks      = GetTrackEntries();
00436   Bool_t f_no_track = kTRUE;
00437   Int_t  itrk       = 0;
00438   Bool_t f_ip_track = kFALSE;
00439   LCDVToplTRK* trkj;
00440   for (itrk=0 ; itrk < ntrks ; itrk++) {
00441     trkj=GetTrackAt(itrk);
00442     if (trk == trkj) {
00443       f_no_track = kFALSE;
00444     }
00445     if (trkj->GetTrackID() < 0) {
00446       f_ip_track = kTRUE;
00447     }
00448   }
00449   if (f_no_track) { return 0; }
00450 
00451   m_qvtx -= trk->GetCharge();
00452 
00453   if (f_fit > 0) {
00454     if (ntrks < 3 || (ntrks <= 3 && f_ip_track)) {
00455       m_chisquared  = 1E10;
00456     } else if (trk->GetTrackID() >= 0) {
00457       if (FilteringToRemoveTrack(trk) < 0) {
00458         cout<<"Something wrong FilteringToRemoveTrack vrt="<<this<<" "
00459             <<"trk="<<trk<<" "
00460             <<endl;
00461         return -1;
00462       }
00463     }
00464   }
00465 
00466   for (Int_t i=0 ; i < m_ntrk ; i++) {
00467     if(m_track_list[i] == trk) {
00468       m_track_list[i] = 0;
00469     }
00470   }
00471   CompressTracksList();
00472   return 0;
00473 }
00474 
00475 //Compress track list
00476 void LCDVToplVRT::CompressTracksList() {
00477   Int_t nzero=0;
00478   for (Int_t i=0 ; i < m_ntrk ; i++) {
00479     if(m_track_list[i] == 0) {
00480       nzero++;
00481       continue;
00482     }
00483     if (nzero == 0) continue;
00484     m_track_list[i-nzero]=m_track_list[i];
00485   }
00486   m_ntrk -= nzero;
00487 }
00488 
00489 //Index of Track
00490 Int_t LCDVToplVRT::IndexOfTrack(LCDVToplTRK* trk) {
00491   Int_t itrk;
00492   for (itrk=0 ; itrk < m_ntrk ; itrk++) {
00493     if(m_track_list[itrk] == trk) break;
00494   }
00495   if (itrk >= m_ntrk) itrk=-1;
00496   return itrk;
00497 }
00498 
00499 //Calcurate four vector @ vertex
00500 void LCDVToplVRT::CalcFourVector() {
00501   m_a4vec.SetXYZT(0.0,0.0,0.0,0.0);
00502   for (Int_t i=0 ; i < m_ntrk ; i++) {
00503     m_a4vec += GetTrack4MomentumVector(i);
00504   }
00505 }
00506 
00507 Int_t LCDVToplVRT::FilteringToAddTrack(LCDVToplTRK* trk) {
00508   //Kalman Filter -- add track to vertex
00509 
00510   Int_t i=0;
00511   Double_t det_tmp=0;
00512 
00513   //   setup vertex position.
00514   TMatrixD x0(3,1); GetVertex(x0);
00515 
00516   //   setup track parameter and its error
00517   TMatrixD p0(5,1);
00518   for (i=0 ; i < 5 ; i++) { p0(i,0)=trk->GetTrackParameter(i); }
00519   TMatrixD g(5,5);
00520   trk->GetErrorMatrix(g);
00521   g.Invert(&det_tmp);
00522   if (det_tmp == 0) {
00523     cout<<"g inv. fault!! in LCDVToplVRT::FilteringToAddTrack"<<endl;
00524     g.Print();
00525     return -1;
00526   }
00527   //   setup inverse of covariance error matrix
00528   // TMatrixD cinv(TMatrixD::kInverted,m_error);
00529   TMatrixD cinv(3,3);GetErrorMatrixVertex(cinv);
00530   cinv.Invert(&det_tmp);
00531   if (det_tmp == 0) {
00532     cout<<"cinv inv. fault!! in LCDVToplVRT::FilteringToAddTrack"<<endl;
00533     cinv.Print();
00534     return -1;
00535   }
00536 
00537   //   setup vertex momentum.
00538   TMatrixD q0(3,1);
00539   q0(0,0) = p0(2,0); q0(1,0) = p0(1,0); q0(2,0) = p0(4,0);
00540 
00541   Double_t xc =-(p0(0,0) + 1.0/p0(2,0))*TMath::Sin(p0(1,0));
00542   Double_t yc = (p0(0,0) + 1.0/p0(2,0))*TMath::Cos(p0(1,0));
00543   Double_t ac = TMath::Sqrt(xc*xc + yc*yc);
00544   Double_t xxc= x0(0,0) - xc;
00545   Double_t yyc= x0(1,0) - yc;
00546   Double_t axc= TMath::Sqrt(xxc*xxc + yyc*yyc);
00547   Double_t dfi= TMath::ASin(-xc/ac*yyc/axc+yc/ac*xxc/axc);
00548   q0(1,0) = q0(1,0) + dfi;
00549 
00550   //   loop over till chi**2 is converged.
00551   TMatrixD a(5,3);
00552   TMatrixD b(5,3);
00553   TMatrixD h0(5,1);
00554   TMatrixD x(x0);
00555   TMatrixD q(q0);
00556   TMatrixD mcov(3,3);
00557   Double_t dcsold=1e20;
00558   Double_t chisq=0;
00559   Int_t iteration=0;
00560   TMatrixD x_save(x);
00561   TMatrixD q_save(q);
00562   TMatrixD mcov_save(mcov);
00563   Double_t chisq_save=1e20;
00564   for (iteration=0; iteration < m_maxtry ; iteration++) {
00565     linearizeChargedTrack(a,b,h0,x,q);
00566 
00567     TMatrixD g_b(g,TMatrixD::kMult,b);
00568     TMatrixD w(b,TMatrixD::kTransposeMult,g_b);
00569     w.Invert(&det_tmp);
00570     if (det_tmp == 0) {
00571       cout<<"w inv. fault!! in LCDVToplVRT::FilteringToAddTrack"<<" "
00572           <<"iteration="<<iteration<<" "
00573           <<endl;
00574       w.Print();
00575       if (iteration == 0) {
00576         g.Print();
00577         b.Print();
00578         a.Print();
00579         h0.Print();
00580         x.Print();
00581         q.Print();
00582       }
00583       // Iteration step is diverged...return the best fit...
00584       x     = x_save;
00585       q     = q_save;
00586       mcov  = mcov_save;
00587       chisq = chisq_save;
00588       break;
00589     }
00590     TMatrixD    bTg(b,TMatrixD::kTransposeMult,   g);
00591     TMatrixD   wbTg(w,TMatrixD::kMult        ,  bTg);
00592     TMatrixD  bwbTg(b,TMatrixD::kMult        , wbTg);
00593     TMatrixD gbwbTg(g,TMatrixD::kMult        ,bwbTg);
00594     TMatrixD gb(g); gb -= gbwbTg;
00595     TMatrixD gba(gb,TMatrixD::kMult,a);
00596     TMatrixD cov(a,TMatrixD::kTransposeMult,gba); cov += cinv;
00597     cov.Invert(&det_tmp);
00598     if (det_tmp == 0) {
00599       cout<<"cov inv. fault!! in LCDVToplVRT::FilteringToAddTrack"<<" "
00600           <<"iteration="<<iteration<<" "
00601           <<endl;
00602       cov.Print();
00603       // Iteration step is diverged...return the best fit...
00604       x     = x_save;
00605       q     = q_save;
00606       mcov  = mcov_save;
00607       chisq = chisq_save;
00608       break;
00609     }
00610     mcov=cov;
00611 
00612     TMatrixD p0h0(p0); p0h0 -= h0;
00613     TMatrixD gbp0h0(gb,TMatrixD::kMult,p0h0);
00614     TMatrixD aTgbp0h0(a,TMatrixD::kTransposeMult,gbp0h0);
00615     TMatrixD mtmp(cinv,TMatrixD::kMult,x0); mtmp += aTgbp0h0;
00616     x.Mult(cov,mtmp);
00617 
00618     TMatrixD ax(a,TMatrixD::kMult,x);
00619     TMatrixD p0h0ax(p0h0); p0h0ax -= ax;
00620     TMatrixD gp0h0ax(g,TMatrixD::kMult,p0h0ax);
00621     TMatrixD bTgp0h0ax(b,TMatrixD::kTransposeMult,gp0h0ax);
00622     q.Mult(w,bTgp0h0ax);
00623 
00624     TMatrixD bq(b,TMatrixD::kMult,q);
00625     TMatrixD r(p0h0ax); r -= bq;
00626     TMatrixD gr(g,TMatrixD::kMult,r);
00627     TMatrixD rgr(r,TMatrixD::kTransposeMult,gr);
00628     TMatrixD xx0(x); xx0 -= x0;
00629     TMatrixD cinvxx0(cinv,TMatrixD::kMult,xx0);
00630     TMatrixD mdeltachi2(xx0,TMatrixD::kTransposeMult,cinvxx0);
00631     mdeltachi2 += rgr;
00632     chisq=mdeltachi2(0,0);
00633 
00634     if (iteration > 0 && TMath::Abs(chisq-dcsold) < m_epscut) break;
00635 
00636     // Iteration step is diverged...return the best fit...
00637     if (chisq > 1.3*dcsold) {
00638       // cout<<"chisq > dcsold in LCDVToplVRT::FilteringToAddTrack"<<endl;
00639       // cout<<"     "
00640       // <<"iteration="<<iteration<<" "
00641       // <<"chisq="<<chisq<<" "
00642       // <<"dcsold="<<dcsold<<" "
00643       // <<endl;
00644       x     = x_save;
00645       q     = q_save;
00646       mcov  = mcov_save;
00647       chisq = chisq_save;
00648       break;
00649     }
00650 
00651     if (iteration == 0 || chisq < chisq_save) {
00652       x_save=x;
00653       q_save=q;
00654       mcov_save=mcov;
00655       chisq_save=chisq;
00656     }
00657 
00658     dcsold=chisq;
00659   }
00660 
00661   //   save new vertex infomation.
00662   SetVertex(x);
00663   SetErrorMatrixVertex(mcov);
00664   m_chisquared  += chisq;
00665   // m_qvtx        += trk->GetCharge();
00666 
00667   return iteration;
00668 
00669 }
00670 
00671 Int_t LCDVToplVRT::FilteringToRemoveTrack(LCDVToplTRK* trk) {
00672   //Kalman Filter -- remove track from vertex
00673 
00674   Int_t i=0;
00675   Double_t det_tmp=0;
00676 
00677   //   setup vertex position.
00678   TMatrixD x0(3,1); GetVertex(x0);
00679 
00680   //   setup track parameter and its error
00681   TMatrixD p0(5,1);
00682   for (i=0 ; i < 5 ; i++) { p0(i,0)=trk->GetTrackParameter(i); }
00683   TMatrixD g(5,5);
00684   trk->GetErrorMatrix(g);
00685   g.Invert(&det_tmp);
00686   if (det_tmp == 0) {
00687     cout<<"g inv. fault!! in LCDVToplVRT::FilteringToRemoveTrack"
00688         <<" N of tracks="<<GetTrackEntries()<<endl;
00689     g.Print();
00690     return -1;
00691   }
00692 
00693   //   setup inverse of covariance error matrix
00694   //TMatrixD cinv(TMatrixD::kInverted,m_error);
00695   TMatrixD cinv(3,3); GetErrorMatrixVertex(cinv);
00696   cinv.Invert(&det_tmp);
00697   if (det_tmp == 0) {
00698     cout<<"cinv inv. fault!! in LCDVToplVRT::FilteringToRemoveTrack"<<endl;
00699     cinv.Print();
00700     return -1;
00701   }
00702 
00703   //   setup vertex momentum.
00704   TMatrixD q0(3,1); SmoothingParameters(trk,q0);  
00705 
00706   //   loop over till chi**2 is converged.
00707   TMatrixD a(5,3);
00708   TMatrixD b(5,3);
00709   TMatrixD h0(5,1);
00710   TMatrixD x(x0);
00711   TMatrixD q(q0);
00712   TMatrixD mcov(3,3);
00713   Double_t dcsold=1e20;
00714   linearizeChargedTrack(a,b,h0,x,q);
00715   TMatrixD r(p0); r -= h0;
00716   TMatrixD ax0(a,TMatrixD::kMult,x0); r -= ax0;
00717   TMatrixD bq0(b,TMatrixD::kMult,q0); r -= bq0;
00718   TMatrixD gr(g,TMatrixD::kMult,r);
00719   TMatrixD rgr(r,TMatrixD::kTransposeMult,gr);
00720   Double_t chisq=rgr(0,0);
00721   m_chisquared  -= chisq;
00722 
00723   Int_t iteration=0;
00724   TMatrixD x_save(x);
00725   TMatrixD q_save(q);
00726   TMatrixD mcov_save(mcov);
00727   Double_t chisq_save=1e20;
00728   for (iteration=0; iteration < m_maxtry ; iteration++) {
00729     linearizeChargedTrack(a,b,h0,x,q);
00730     TMatrixD g_b(g,TMatrixD::kMult,b);
00731     TMatrixD w(b,TMatrixD::kTransposeMult,g_b);
00732     w.Invert(&det_tmp);
00733     if (det_tmp == 0) {
00734       cout<<"w inv. fault!! in LCDVToplVRT::FilteringToRemoveTrack"<<" "
00735           <<"iteration="<<iteration<<" "
00736           <<endl;
00737       // Iteration step is diverged...return the best fit...
00738       x     = x_save;
00739       q     = q_save;
00740       mcov  = mcov_save;
00741       chisq = chisq_save;
00742       break;
00743     }
00744     TMatrixD    bTg(b,TMatrixD::kTransposeMult,    g);
00745     TMatrixD   wbTg(w,TMatrixD::kMult         ,  bTg);
00746     TMatrixD  bwbTg(b,TMatrixD::kMult         , wbTg);
00747     TMatrixD gbwbTg(g,TMatrixD::kMult         ,bwbTg);
00748 
00749     TMatrixD gb(g); gb -= gbwbTg;
00750     TMatrixD gba(gb,TMatrixD::kMult,a);
00751     TMatrixD cov(cinv); 
00752     TMatrixD aTgba(a,TMatrixD::kTransposeMult,gba); cov -= aTgba;
00753     cov.Invert(&det_tmp);
00754     if (det_tmp == 0) {
00755       cout<<"cov inv. fault!! in LCDVToplVRT::FilteringToRemoveTrack"<<" "
00756           <<"iteration="<<iteration<<" "
00757           <<endl;
00758       // Iteration step is diverged...return the best fit...
00759       x     = x_save;
00760       q     = q_save;
00761       mcov  = mcov_save;
00762       chisq = chisq_save;
00763       break;
00764     }
00765     mcov=cov;
00766 
00767     TMatrixD p0h0(p0); p0h0 -= h0;
00768     TMatrixD gbp0h0(gb,TMatrixD::kMult,p0h0);
00769     TMatrixD aTgbp0h0(a,TMatrixD::kTransposeMult,gbp0h0);
00770     TMatrixD mtmp(cinv,TMatrixD::kMult,x0); mtmp -= aTgbp0h0;
00771     x.Mult(cov,mtmp);
00772 
00773     TMatrixD ax(a,TMatrixD::kMult,x);
00774     TMatrixD p0h0ax(p0h0); p0h0ax -= ax;
00775     TMatrixD gp0h0ax(g,TMatrixD::kMult,p0h0ax);
00776     TMatrixD bTgp0h0ax(b,TMatrixD::kTransposeMult,gp0h0ax);
00777     q.Mult(w,bTgp0h0ax);
00778 
00779     //TMatrixD covinv(TMatrixD::kInverted,cov);
00780     TMatrixD covinv(cov);
00781     covinv.Invert(&det_tmp);
00782     if (det_tmp == 0) {
00783       cout<<"covinv inv. fault!! in LCDVToplVRT::FilteringToRemoveTrack"<<" "
00784           <<"iteration="<<iteration<<" "
00785           <<endl;
00786       // Iteration step is diverged...return the best fit...
00787       x     = x_save;
00788       q     = q_save;
00789       mcov  = mcov_save;
00790       chisq = chisq_save;
00791       break;
00792       return -1;
00793     }
00794 
00795     TMatrixD xx0(x); xx0 -= x0;
00796     TMatrixD covinvxx0(covinv,TMatrixD::kMult,xx0);
00797     TMatrixD mdeltachi2(xx0,TMatrixD::kTransposeMult,covinvxx0);
00798 
00799     chisq=mdeltachi2(0,0);
00800     if (iteration > 0 && TMath::Abs(chisq-dcsold) < m_epscut) break;
00801 
00802     // Iteration step is diverged...return the best fit...
00803     if (chisq > 1.3*dcsold) {
00804       x     = x_save;
00805       q     = q_save;
00806       mcov  = mcov_save;
00807       chisq = chisq_save;
00808       break;
00809     }
00810 
00811     if (iteration == 0 || chisq < chisq_save) {
00812       x_save=x;
00813       q_save=q;
00814       mcov_save=mcov;
00815       chisq_save=chisq;
00816     }
00817 
00818     dcsold=chisq;
00819   }
00820 
00821   //   save new vertex infomation.
00822   SetVertex(x);
00823   SetErrorMatrixVertex(mcov);
00824   m_chisquared  -= chisq;
00825   if (m_chisquared < 0) m_chisquared=0;
00826   // m_qvtx        -= trk->GetCharge();
00827 
00828   return iteration;
00829 
00830 }
00831 
00832 void LCDVToplVRT::linearizeChargedTrack(TMatrixD& a, TMatrixD& b, TMatrixD& h0,
00833                                         TMatrixD& v0, TMatrixD& q0) {
00834   /*------------------------------------------------------------------- */
00835   /*                                                                    */
00836   /*   LinearizeTrack:  a c/c++ version of Lothar Bauerdicks fortran    */
00837   /*                                                                    */
00838   /*   calculate and return A, B, h0 from v0, q0:                       */
00839   /*                                                                    */
00840   /*   calculate coefficients of taylor expansion for estimated         */
00841   /*   track parameters h around point (v0,q0):                         */
00842   /*                                                                    */
00843   /*     h    =    h(v,q) + eps                                         */
00844   /*       \approx h(v0,q0) + A.(v-v0) + B.(q-q0) + eps                 */
00845   /*          =    h0 + A.v + B.q + eps                                 */
00846   /*   where                                                            */
00847   /*     A  =  dh/dv(v0,q0),                                            */
00848   /*     B  =  dh/dq(v0,q0), and                                        */
00849   /*     h0 =  h(v0,q0) - A.v0 - B.q0 are the                           */
00850   /*   derivatives of track parameters w/r to vertex position           */
00851   /*   and track 3-momentum at space point v0 and estimated momentum q0 */
00852   /*                                                                    */
00853   /*   track model:                                                     */
00854   /*   h = {d0,phi0,w0,z0,tl0}                                            */
00855   /*   v = {x,y,z}                                                      */
00856   /*   q = {w,tl,psi}                                                   */
00857   /*                                                                    */
00858   /*   for a charged particle (helix parameters in BaBar conventions)   */
00859   /*   --------------------------------------------------------------   */
00860   //   d0   = 1/w - (1/w + y*cos(phi) - x*sin(phi))/cos(gamma)
00861   //   phi0 = phi + gamma
00862   //   w0   = w
00863   //   z0   = z + gamma/w*tl
00864   //   tl0  = tl
00865   //   where
00866   //   phi = atan2(Py/Px)
00867   //   gamma = atan((x*cos(phi) + y*sin(phi))/
00868   //                (x*sin(phi) - y0*cos(phi) - 1/w))
00869   /*                                                                    */
00870   /*   for a neutral particle (w == 0)                                  */
00871   /*   -------------------------------                                  */
00872   /*   d0 =                                                             */
00873   /*   w0 = w                                                           */
00874   /*   tl0 = tl                                                         */
00875   /*   psi0 = psi                                                       */
00876   /*   z0 = z - r*cos(xi)*tl                                            */
00877   /*                                                                    */
00878   /*--------------------------------------------------------------------*/
00879 
00880   Double_t w   = q0(0,0);
00881   Double_t phi = q0(1,0);
00882   Double_t tl  = q0(2,0);
00883 
00884   Double_t x   = v0(0,0);
00885   Double_t y   = v0(1,0);
00886   Double_t z   = v0(2,0);
00887 
00888   Double_t cp  = TMath::Cos(phi);
00889   Double_t sp  = TMath::Sin(phi);
00890   Double_t oow = 1.0/w;
00891 
00892   Double_t bb  = x*cp + y*sp;
00893   Double_t cc  = oow + y*cp - x*sp;
00894   Double_t gma = TMath::ATan(bb/(-cc));
00895 
00896   Double_t cg  = TMath::Cos(gma);
00897   Double_t sg  = TMath::Sin(gma);
00898 
00899   //Calc. transformed quantities
00900   Double_t phi0= phi + gma;
00901   //Double_t d0  = oow - cc/cg;
00902   Double_t d0  =-oow + cc/cg;
00903   Double_t z0  = z + gma/w*tl;
00904 
00905   //  h(v0,q0)
00906   h0(0,0) = d0;
00907   h0(1,0) = phi0;
00908   h0(2,0) = w;
00909   h0(3,0) = z0;
00910   h0(4,0) = tl;
00911 
00912   //calc. Jacobians
00913   Double_t mx=x - oow*sp;
00914   Double_t my=y + oow*cp;
00915   Double_t m2=mx*mx + my*my;
00916   TMatrixD dgdv(3,1);
00917   dgdv(0,0)=-my/m2;
00918   dgdv(1,0)= mx/m2;
00919   dgdv(2,0)= 0.0;
00920   TMatrixD dgdq(3,1);
00921   dgdq(0,0)=-oow*oow*bb/m2;
00922   dgdq(1,0)= y*dgdv(0,0) - x*dgdv(1,0);
00923   dgdq(2,0)= 0.0;
00924 
00925   //   d d0 / d x, d y, d z
00926   //a(0,0) = (cg*sp - sg*dgdv(0,0)*(oow - x*sp + y*cp))/cg/cg;
00927   //a(0,1) =-(cg*cp + sg*dgdv(1,0)*(oow - x*sp + y*cp))/cg/cg;
00928   //a(0,2) = 0.0;
00929   a(0,0) =-(cg*sp - sg*dgdv(0,0)*(oow - x*sp + y*cp))/cg/cg;
00930   a(0,1) =+(cg*cp + sg*dgdv(1,0)*(oow - x*sp + y*cp))/cg/cg;
00931   a(0,2) = 0.0;
00932   //   d phi0/ d x, d y, d z
00933   a(1,0) = dgdv(0,0);
00934   a(1,1) = dgdv(1,0);
00935   a(1,2) = 0.0;
00936   //   d w / d x, d y, d z
00937   a(2,0) = 0.0;
00938   a(2,1) = 0.0;
00939   a(2,2) = 0.0;
00940   //   d z0 / d x, d y, d z
00941   a(3,0) = tl/w*dgdv(0,0);
00942   a(3,1) = tl/w*dgdv(1,0);
00943   a(3,2) = 1.0;
00944   //   d tl / d x, d y, d z
00945   a(4,0) = 0.0;
00946   a(4,1) = 0.0;
00947   a(4,2) = 0.0;
00948 
00949   //   d d0 / d w0, d phi00, d tl0
00950   //b(0,0) =-oow*oow + (oow*oow*cg - sg*dgdq(0,0)*cc)/cg/cg;
00951   //b(0,1) = (bb*cg - sg*dgdq(1,0)*cc)/cg/cg;
00952   //b(0,2) = 0.0;
00953   b(0,0) =-(-oow*oow + (oow*oow*cg - sg*dgdq(0,0)*cc)/cg/cg);
00954   b(0,1) =-((bb*cg - sg*dgdq(1,0)*cc)/cg/cg);
00955   b(0,2) = 0.0;
00956   //   d psi0 / d w0, d phi00, d tl0
00957   b(1,0) = dgdq(0,0);
00958   b(1,1) = 1.0 + dgdq(1,0);
00959   b(1,2) = 0.0;
00960   //   d w / d w0, d phi00, d tl0
00961   b(2,0) = 1.0;
00962   b(2,1) = 0.0;
00963   b(2,2) = 0.0;
00964   //   d z0 / d w0, d phi00, d tl0
00965   b(3,0) = tl/w*(dgdq(0,0) - gma/w);
00966   b(3,1) = tl/w* dgdq(1,0);
00967   b(3,2) = gma/w;
00968   //   d tl / d w0, d phi00, d tl0
00969   b(4,0) = 0.0;
00970   b(4,1) = 0.0;
00971   b(4,2) = 1.0;
00972 
00973   //  h0(v0,q0) = h(v0,q0) - a*v0 - b*q0
00974   TMatrixD av0(a,TMatrixD::kMult,v0);
00975   TMatrixD bq0(b,TMatrixD::kMult,q0);
00976   TMatrixD av0bq0(av0); av0bq0 += bq0;
00977   h0 -= av0bq0;
00978 
00979 }
00980 
00981 void LCDVToplVRT::SmoothingParameters(LCDVToplTRK* trk, TMatrixD& q) {
00982   Int_t i=0;
00983   Double_t det_tmp=0;
00984 
00985   //   setup track parameter and its error
00986   TMatrixD p0(5,1);
00987   for (i=0 ; i < 5 ; i++) { p0(i,0)=trk->GetTrackParameter(i); }
00988   TMatrixD g(5,5);
00989   trk->GetErrorMatrix(g);
00990   g.Invert(&det_tmp);
00991   if (det_tmp == 0) {
00992     cout<<"g inv. fault!! in LCDVToplVRT::SmoothingParameters"<<endl;
00993   }
00994 
00995   //   setup vertex momentum.
00996   TMatrixD q0(3,1);
00997   q0(0,0) = p0(2,0); q0(1,0) = p0(1,0); q0(2,0) = p0(4,0);
00998 
00999   TMatrixD a(5,3);
01000   TMatrixD b(5,3);
01001   TMatrixD h0(5,1);
01002   TMatrixD x0(3,1); GetVertex(x0);
01003   linearizeChargedTrack(a,b,h0,x0,q0);
01004 
01005   TMatrixD g_b(g,TMatrixD::kMult,b);
01006   TMatrixD w(b,TMatrixD::kTransposeMult,g_b);
01007   w.Invert(&det_tmp);
01008   if (det_tmp == 0) {
01009     cout<<"w inv. fault!! in LCDVToplVRT::SmoothingParameters"<<endl;
01010   }
01011   TMatrixD p0h0(p0); p0h0 -= h0;
01012   TMatrixD ax(a,TMatrixD::kMult,x0);
01013   TMatrixD p0h0ax(p0h0); p0h0ax -= ax;
01014   TMatrixD gp0h0ax(g,TMatrixD::kMult,p0h0ax);
01015   TMatrixD bTgp0h0ax(b,TMatrixD::kTransposeMult,gp0h0ax);
01016   q.Mult(w,bTgp0h0ax);
01017   
01018 }
01019 
01020 //Vertex Fitter
01021 Int_t LCDVToplVRT::VertexFit() {
01022   Int_t i=0;
01023   m_error[0]=1;
01024   m_error[1]=0;
01025   m_error[2]=1;
01026   m_error[3]=0;
01027   m_error[4]=0;
01028   m_error[5]=1;
01029   m_chisquared = 0;
01030   // m_qvtx       = 0;
01031 
01032   Int_t ntrks=GetTrackEntries();
01033   LCDVToplTRK* trk  =0;
01034   for (i=0 ; i < ntrks ; i++) {
01035     trk=GetTrackAt(i);
01036     if (trk->GetTrackID() >= 0) {
01037       if (FilteringToAddTrack(trk) < 0) {
01038         cout<<"Something wrong FilteringToAddTrack trk="<<trk<<endl;
01039         return 0;
01040       }
01041     }
01042   }
01043 
01044   return 1;
01045 }
01046 
01047 Int_t LCDVToplVRT::VertexFit(TVector3 xvinit) {
01048   Int_t i=0;
01049   for (i=0 ; i < 3 ; i++) { m_fitparm[i] = xvinit[i]; }
01050   return VertexFit();
01051 }

Generated on Tue Jul 18 18:33:59 2006 for LCDROOT by  doxygen 1.4.6