// ----------------------------------------------------------------------------
// $Id: LCDVToplVRT.cxx,v 1.3 2001/06/19 03:49:12 toshi Exp $
// ----------------------------------------------------------------------------
//
// $Log: LCDVToplVRT.cxx,v $
// Revision 1.3 2001/06/19 03:49:12 toshi
// Changes accoring to the changes of LCDVToplTRK.
//
// Revision 1.2 2001/05/14 17:27:15 toshi
// Vertex fitting sometimes diverges and it may kill a running program.
// If vertex fitting goes divergence, use previous fitting parameter values
// in the iteration.
//
// Revision 1.1 2001/05/10 19:32:11 toshi
// C++ file name convention has been changed from .C to .cxx .
//
//
#include "LCDVToplVRT.h"
#include <iostream.h>
///////////////////////////////////////////////////////////
//
// LCDVToplVRT
//
ClassImp(LCDVToplVRT)
// constructors & deconstructors
LCDVToplVRT::LCDVToplVRT() {
Init(TVector3(0.0,0.0,0.0));
}
LCDVToplVRT::LCDVToplVRT(TVector3 xinit) {
Init(xinit);
}
LCDVToplVRT::LCDVToplVRT(TObjArray* tracks) {
Init(tracks,TVector3(0.0,0.0,0.0));
}
LCDVToplVRT::LCDVToplVRT(TObjArray* tracks,TVector3 xinit) {
Init(tracks,xinit);
}
LCDVToplVRT::LCDVToplVRT(const LCDVToplVRT& vrt) {
m_fitparm.ResizeTo(3,1);
m_error.ResizeTo(3,3);
maxtry = vrt.maxtry;
epscut = vrt.epscut;
m_ntrk = vrt.m_ntrk;
for (Int_t i=0 ; i < 100 ; i++) {
track_list[i]=vrt.track_list[i];
}
m_fitparm = vrt.m_fitparm;
m_error = vrt.m_error;
a4vec = vrt.a4vec;
qvtx = vrt.qvtx;
chisquared= vrt.chisquared;
vsig = vrt.vsig;
}
LCDVToplVRT & LCDVToplVRT::operator = (const LCDVToplVRT & p) {
m_fitparm.ResizeTo(3,1);
m_error.ResizeTo(3,3);
maxtry = p.maxtry;
epscut = p.epscut;
m_ntrk = p.m_ntrk;
for (Int_t i=0 ; i < 100 ; i++) {
track_list[i]=p.track_list[i];
}
m_fitparm = p.m_fitparm;
m_error = p.m_error;
a4vec = p.a4vec;
qvtx = p.qvtx;
chisquared= p.chisquared;
vsig = p.vsig;
return *this;
}
//Initialize LCDVToplVRT
void LCDVToplVRT::Init(TVector3 xinit){
Int_t i=0;
m_ntrk = 0;
m_fitparm.ResizeTo(3,1);
m_error.ResizeTo(3,3);
maxtry = 50;
epscut = 1E-3;
for (i=0 ; i < 3 ; i++) { m_fitparm(i,0) = xinit[i]; }
for (i=0 ; i < 3 ; i++) { m_error(i,i) = 1.0; }
a4vec = TLorentzVector(0.0,0.0,0.0,0.0);
chisquared = 0;
qvtx = 0;
vsig =-1;
}
void LCDVToplVRT::Init(TObjArray* tracks, TVector3 xinit){
Init(xinit);
Int_t i = 0;
Int_t ntrks = tracks->GetEntries();
LCDVToplTRK* trk = 0;
for (i=0 ; i < ntrks ; i++) {
trk=(LCDVToplTRK*)(tracks->At(i));
AddTrack(trk);
}
if (GetChi2()>0) {
// CalcFourVector();
vsig =0;
}
}
//
void LCDVToplVRT::Clean() {
Init(TVector3(0,0,0));
}
//Get chi2 contribution of the given track
Double_t LCDVToplVRT::GetPchi2(Int_t itrk) {
LCDVToplTRK* trk=GetTrackAt(itrk);
Int_t i=0;
Double_t det_tmp=0;
// setup vertex position.
TMatrixD x0(m_fitparm);
// setup track parameter and its error
TMatrixD p0(5,1);
for (i=0 ; i < 5 ; i++) { p0(i,0)=trk->GetTrackParameter(i); }
TMatrixD g(5,5);
trk->GetErrorMatrix(g);
g.Invert(&det_tmp);
if (det_tmp == 0) {
cout<<"g inv. fault!! in LCDVToplVRT::GetPchi2"<<endl;
return 1e20;
}
// setup vertex momentum.
TMatrixD q0(3,1); SmoothingParameters(trk,q0);
// loop over till chi**2 is converged.
TMatrixD a(5,3);
TMatrixD b(5,3);
TMatrixD h0(5,1);
linearizeChargedTrack(a,b,h0,x0,q0);
TMatrixD r(p0) ; r -= h0;
TMatrixD ax0(a,TMatrixD::kMult,x0); r -= ax0;
TMatrixD bq0(b,TMatrixD::kMult,q0); r -= bq0;
TMatrixD gr(g,TMatrixD::kMult,r);
TMatrixD rgr(r,TMatrixD::kTransposeMult,gr);
// return rgr(0,0);
// setup inverse of covariance error matrix
//TMatrixD cinv(TMatrixD::kInverted,m_error);
TMatrixD cinv(m_error);
cinv.Invert(&det_tmp);
if (det_tmp == 0) {
cout<<"cinv inv. fault!! in LCDVToplVRT::GetPchi2"<<endl;
return 1e20;
}
TMatrixD x(x0);
TMatrixD q(q0);
Double_t dcsold=1e20;
Double_t chisq=0.0;
Int_t iteration=0;
for (iteration=0; iteration < maxtry ; iteration++) {
linearizeChargedTrack(a,b,h0,x,q);
TMatrixD g_b(g,TMatrixD::kMult,b);
TMatrixD w(b,TMatrixD::kTransposeMult,g_b);
w.Invert(&det_tmp);
if (det_tmp == 0) {
cout<<"w inv. fault!! in LCDVToplVRT::GetPchi2"<<endl;
return 1e20;
}
TMatrixD bTg(b,TMatrixD::kTransposeMult, g);
TMatrixD wbTg(w,TMatrixD::kMult , bTg);
TMatrixD bwbTg(b,TMatrixD::kMult , wbTg);
TMatrixD gbwbTg(g,TMatrixD::kMult ,bwbTg);
TMatrixD gb(g); gb -= gbwbTg;
TMatrixD gba(gb,TMatrixD::kMult,a);
TMatrixD cov(cinv);
TMatrixD aTgba(a,TMatrixD::kTransposeMult,gba); cov -= aTgba;
cov.Invert(&det_tmp);
if (det_tmp == 0) {
cout<<"cov inv. fault!! in LCDVToplVRT::GetPchi2"<<endl;
return 1e20;
}
TMatrixD p0h0(p0); p0h0 -= h0;
TMatrixD gbp0h0(gb,TMatrixD::kMult,p0h0);
TMatrixD aTgbp0h0(a,TMatrixD::kTransposeMult,gbp0h0);
TMatrixD mtmp(cinv,TMatrixD::kMult,x0); mtmp -= aTgbp0h0;
x.Mult(cov,mtmp);
TMatrixD ax(a,TMatrixD::kMult,x);
TMatrixD p0h0ax(p0h0); p0h0ax -= ax;
TMatrixD gp0h0ax(g,TMatrixD::kMult,p0h0ax);
TMatrixD bTgp0h0ax(b,TMatrixD::kTransposeMult,gp0h0ax);
q.Mult(w,bTgp0h0ax);
//TMatrixD covinv(TMatrixD::kInverted,cov);
TMatrixD covinv(cov);
covinv.Invert(&det_tmp);
if (det_tmp == 0) {
cout<<"covinv inv. fault!! in LCDVToplVRT::GetPchi2"<<endl;
return 1e20;
}
TMatrixD xx0(x); xx0 -= x0;
TMatrixD covinvxx0(covinv,TMatrixD::kMult,xx0);
TMatrixD mdeltachi2(xx0,TMatrixD::kTransposeMult,covinvxx0);
chisq=mdeltachi2(0,0);
if (iteration > 0 && TMath::Abs(chisq-dcsold) < epscut) break;
// Iteration step is diverged...return the best fit...
if (chisq > dcsold) {
chisq = dcsold;
break;
}
dcsold=chisq;
}
return rgr(0,0)+chisq;
}
//Get Vertex Position(Vector)
TVector3 LCDVToplVRT::GetVertexVector() {
return TVector3(m_fitparm(0,0),m_fitparm(1,0),m_fitparm(2,0));
}
//Get Error Matrix(position)
void LCDVToplVRT::GetErrorMatrixVertex(TMatrixD& emvrt) {
emvrt(0,0)=m_error(0,0);
emvrt(1,0)=m_error(1,0);
emvrt(2,0)=m_error(2,0);
emvrt(0,1)=m_error(0,1);
emvrt(1,1)=m_error(1,1);
emvrt(2,1)=m_error(2,1);
emvrt(0,2)=m_error(0,2);
emvrt(1,2)=m_error(1,2);
emvrt(2,2)=m_error(2,2);
}
//Get decay length
Double_t LCDVToplVRT::GetDecayLength(TVector3 xpt) {
return (GetVertexVector() - xpt).Mag();
}
//Get error of decay length
Double_t LCDVToplVRT::GetErrorDecayLength(TVector3 xpt, TMatrixD& em_xpt) {
TVector3 dx=GetVertexVector() - xpt;
Double_t d=dx.Mag();
TMatrixD dddxT(3,1);
dddxT(0,0) = dx[0]/d;
dddxT(1,0) = dx[1]/d;
dddxT(2,0) = dx[2]/d;
TMatrixD emvrt(3,3); GetErrorMatrixVertex(emvrt);
emvrt += em_xpt;
TMatrixD tmp0(emvrt,TMatrixD::kMult,dddxT);
TMatrixD tmp1(dddxT,TMatrixD::kTransposeMult,tmp0);
return tmp1(0,0);
}
//Get # of significance track
/*
Int_t LCDVToplVRT::GetNsig(Double_t nsigma, TVector3 axis, TVector3 ipv,
TMatrixD& eipv) {
LCDVToplTRK* trk=0;
Int_t itrk=0;
Int_t ntrk=GetTrackEntries();
Int_t nsig=0;
Double_t l=0;
for (itrk=0 ; itrk < ntrk ; itrk++) {
trk=GetTrackAt(itrk);
l=trk->CalcPOCA3(ipv);
if (trk->GetSignedDistanceNormByError(l,ipv,axis,eipv) > nsigma) nsig++;
}
return nsig;
}
*/
//Get track momentum vector
TVector3 LCDVToplVRT::GetTrackMomentumVector(Int_t itrk) {
LCDVToplTRK* trk=GetTrackAt(itrk);
TMatrixD q(3,1);
SmoothingParameters(trk,q);
Double_t pt=trk->GetMagneticField()/333.567/TMath::Abs(q(0,0));
return TVector3(pt*TMath::Cos(q(1,0)),pt*TMath::Sin(q(1,0)),pt*q(2,0));
}
TLorentzVector LCDVToplVRT::GetTrack4MomentumVector(Int_t itrk) {
LCDVToplTRK* trk=GetTrackAt(itrk);
TMatrixD q(3,1);
SmoothingParameters(trk,q);
Double_t pt=trk->GetMagneticField()/333.567/TMath::Abs(q(0,0));
Double_t mpi=0.13956995;
return TLorentzVector(pt*TMath::Cos(q(1,0)),pt*TMath::Sin(q(1,0)),pt*q(2,0),
TMath::Sqrt(pt*(1.0+q(2,0)*q(2,0))*pt + mpi*mpi));
}
//Get track position vector
/*
TVector3 LCDVToplVRT::GetTrackPositionVector(Int_t itrk) {
Double_t l=GetLTrackAt(itrk);
LCDVToplTRK* trk=GetTrackAt(itrk);
return (trk->GetPositionVector(l));
}
*/
//Set Vertex
void LCDVToplVRT::SetVertex(Double_t* a) {
SetVertex(TVector3(a[0],a[1],a[2]));
}
void LCDVToplVRT::SetVertex(TVector3 a) {
m_fitparm(0,0)=a[0];
m_fitparm(1,0)=a[1];
m_fitparm(2,0)=a[2];
}
//Add a track
Int_t LCDVToplVRT::AddTrack(LCDVToplTRK* trk) {
return AddTrack(trk,1);
}
Int_t LCDVToplVRT::AddTrack(LCDVToplTRK* trk, Int_t f_fit) {
Int_t ntrks_prev=GetTrackEntries();
Bool_t f_same_track=kFALSE;
Int_t itrk=0;
for (itrk=0 ; itrk < ntrks_prev ; itrk++) {
if (trk == GetTrackAt(itrk)) {
f_same_track = kTRUE;
break;
}
}
if (f_same_track) { return 0; }
if (m_ntrk < 99) {
track_list[m_ntrk++]=trk;
if (trk->GetTrackID() >= 0 && f_fit > 0) {
if (FilteringToAddTrack(trk) < 0) {
cout<<"Something wrong FilteringToAddTrack trk="<<trk<<endl;
return -1;
}
}
return 0;
} else {
fprintf(stderr,"LCDVToplVRT::AddTrack # of tracks is over 100!n");
return -2;
}
}
//Add tracks
void LCDVToplVRT::AddTracks(TObjArray* addtracks) {
AddTracks(addtracks,1);
}
void LCDVToplVRT::AddTracks(TObjArray* addtracks, Int_t f_fit) {
Int_t ntrks=addtracks->GetEntries();
LCDVToplTRK* trk =0;
for (Int_t i=0 ; i < ntrks ; i++) {
trk=(LCDVToplTRK*)(addtracks->At(i));
AddTrack(trk,f_fit);
}
}
//Remove a track
Int_t LCDVToplVRT::RemoveTrackAt(Int_t itrk) {
return RemoveTrackAt(itrk,1);
}
Int_t LCDVToplVRT::RemoveTrackAt(Int_t itrk, Int_t f_fit) {
return RemoveTrack(GetTrackAt(itrk),f_fit);
}
Int_t LCDVToplVRT::RemoveTrack(LCDVToplTRK* trk) {
return RemoveTrack(trk,1);
}
Int_t LCDVToplVRT::RemoveTrack(LCDVToplTRK* trk,Int_t f_fit) {
Int_t ntrks = GetTrackEntries();
Bool_t f_no_track = kTRUE;
Int_t itrk = 0;
Bool_t f_ip_track = kFALSE;
LCDVToplTRK* trkj;
for (itrk=0 ; itrk < ntrks ; itrk++) {
trkj=GetTrackAt(itrk);
if (trk == trkj) {
f_no_track = kFALSE;
}
if (trkj->GetTrackID() < 0) {
f_ip_track = kTRUE;
}
}
if (f_no_track) { return 0; }
if (f_fit > 0) {
if (ntrks < 3 || (ntrks <= 3 && f_ip_track)) {
chisquared = 1E10;
qvtx -= trk->GetCharge();
} else if (trk->GetTrackID() >= 0) {
if (FilteringToRemoveTrack(trk) < 0) {
cout<<"Something wrong FilteringToRemoveTrack"<<" "
<<"trk="<<trk
<<endl;
return -1;
}
}
}
for (Int_t i=0 ; i < m_ntrk ; i++) {
if(track_list[i] == trk) {
track_list[i] = 0;
}
}
CompressTracksList();
return 0;
}
//Compress track list
void LCDVToplVRT::CompressTracksList() {
Int_t nzero=0;
for (Int_t i=0 ; i < m_ntrk ; i++) {
if(track_list[i] == 0) {
nzero++;
continue;
}
if (nzero == 0) continue;
track_list[i-nzero]=track_list[i];
}
m_ntrk -= nzero;
}
//Index of Track
Int_t LCDVToplVRT::IndexOfTrack(LCDVToplTRK* trk) {
Int_t itrk;
for (itrk=0 ; itrk < m_ntrk ; itrk++) {
if(track_list[itrk] == trk) break;
}
if (itrk >= m_ntrk) itrk=-1;
return itrk;
}
//Calcurate four vector @ vertex
void LCDVToplVRT::CalcFourVector() {
a4vec.SetXYZT(0.0,0.0,0.0,0.0);
for (Int_t i=0 ; i < m_ntrk ; i++) {
a4vec += GetTrack4MomentumVector(i);
}
}
Int_t LCDVToplVRT::FilteringToAddTrack(LCDVToplTRK* trk) {
//Kalman Filter -- add track to vertex
Int_t i=0;
Double_t det_tmp=0;
// setup vertex position.
TMatrixD x0(m_fitparm);
// setup track parameter and its error
TMatrixD p0(5,1);
for (i=0 ; i < 5 ; i++) { p0(i,0)=trk->GetTrackParameter(i); }
TMatrixD g(5,5);
trk->GetErrorMatrix(g);
g.Invert(&det_tmp);
if (det_tmp == 0) {
cout<<"g inv. fault!! in LCDVToplVRT::FilteringToAddTrack"<<endl;
return -1;
}
// setup inverse of covariance error matrix
// TMatrixD cinv(TMatrixD::kInverted,m_error);
TMatrixD cinv(m_error);
cinv.Invert(&det_tmp);
if (det_tmp == 0) {
cout<<"cinv inv. fault!! in LCDVToplVRT::FilteringToAddTrack"<<endl;
return -1;
}
// setup vertex momentum.
TMatrixD q0(3,1);
q0(0,0) = p0(2,0); q0(1,0) = p0(1,0); q0(2,0) = p0(4,0);
Double_t xc =-(p0(0,0) + 1.0/p0(2,0))*TMath::Sin(p0(1,0));
Double_t yc = (p0(0,0) + 1.0/p0(2,0))*TMath::Cos(p0(1,0));
Double_t ac = TMath::Sqrt(xc*xc + yc*yc);
Double_t xxc= x0(0,0) - xc;
Double_t yyc= x0(1,0) - yc;
Double_t axc= TMath::Sqrt(xxc*xxc + yyc*yyc);
Double_t dfi= TMath::ASin(-xc/ac*yyc/axc+yc/ac*xxc/axc);
q0(1,0) = q0(1,0) + dfi;
// loop over till chi**2 is converged.
TMatrixD a(5,3);
TMatrixD b(5,3);
TMatrixD h0(5,1);
TMatrixD x(x0);
TMatrixD q(q0);
TMatrixD mcov(3,3);
Double_t dcsold=1e20;
Double_t chisq=0;
Int_t iteration=0;
TMatrixD x_save(x);
TMatrixD q_save(q);
TMatrixD mcov_save(mcov);
Double_t chisq_save=1e20;
for (iteration=0; iteration < maxtry ; iteration++) {
linearizeChargedTrack(a,b,h0,x,q);
TMatrixD g_b(g,TMatrixD::kMult,b);
TMatrixD w(b,TMatrixD::kTransposeMult,g_b);
w.Invert(&det_tmp);
if (det_tmp == 0) {
cout<<"w inv. fault!! in LCDVToplVRT::FilteringToAddTrack"<<endl;
}
TMatrixD bTg(b,TMatrixD::kTransposeMult, g);
TMatrixD wbTg(w,TMatrixD::kMult , bTg);
TMatrixD bwbTg(b,TMatrixD::kMult , wbTg);
TMatrixD gbwbTg(g,TMatrixD::kMult ,bwbTg);
TMatrixD gb(g); gb -= gbwbTg;
TMatrixD gba(gb,TMatrixD::kMult,a);
TMatrixD cov(a,TMatrixD::kTransposeMult,gba); cov += cinv;
cov.Invert(&det_tmp);
if (det_tmp == 0) {
cout<<"cov inv. fault!! in LCDVToplVRT::FilteringToAddTrack"<<endl;
}
mcov=cov;
TMatrixD p0h0(p0); p0h0 -= h0;
TMatrixD gbp0h0(gb,TMatrixD::kMult,p0h0);
TMatrixD aTgbp0h0(a,TMatrixD::kTransposeMult,gbp0h0);
TMatrixD mtmp(cinv,TMatrixD::kMult,x0); mtmp += aTgbp0h0;
x.Mult(cov,mtmp);
TMatrixD ax(a,TMatrixD::kMult,x);
TMatrixD p0h0ax(p0h0); p0h0ax -= ax;
TMatrixD gp0h0ax(g,TMatrixD::kMult,p0h0ax);
TMatrixD bTgp0h0ax(b,TMatrixD::kTransposeMult,gp0h0ax);
q.Mult(w,bTgp0h0ax);
TMatrixD bq(b,TMatrixD::kMult,q);
TMatrixD r(p0h0ax); r -= bq;
TMatrixD gr(g,TMatrixD::kMult,r);
TMatrixD rgr(r,TMatrixD::kTransposeMult,gr);
TMatrixD xx0(x); xx0 -= x0;
TMatrixD cinvxx0(cinv,TMatrixD::kMult,xx0);
TMatrixD mdeltachi2(xx0,TMatrixD::kTransposeMult,cinvxx0);
mdeltachi2 += rgr;
chisq=mdeltachi2(0,0);
if (iteration > 0 && TMath::Abs(chisq-dcsold) < epscut) break;
// Iteration step is diverged...return the best fit...
if (chisq > 1.3*dcsold) {
// cout<<"chisq > dcsold in LCDVToplVRT::FilteringToAddTrack"<<endl;
// cout<<" "
// <<"iteration="<<iteration<<" "
// <<"chisq="<<chisq<<" "
// <<"dcsold="<<dcsold<<" "
// <<endl;
x = x_save;
q = q_save;
mcov = mcov_save;
chisq = chisq_save;
break;
}
if (iteration == 0 || chisq < chisq_save) {
x_save=x;
q_save=q;
mcov_save=mcov;
chisq_save=chisq;
}
dcsold=chisq;
}
// save new vertex infomation.
m_fitparm = x;
m_error = mcov;
chisquared += chisq;
qvtx += trk->GetCharge();
return iteration;
}
Int_t LCDVToplVRT::FilteringToRemoveTrack(LCDVToplTRK* trk) {
//Kalman Filter -- remove track from vertex
Int_t i=0;
Double_t det_tmp=0;
// setup vertex position.
TMatrixD x0(m_fitparm);
// setup track parameter and its error
TMatrixD p0(5,1);
for (i=0 ; i < 5 ; i++) { p0(i,0)=trk->GetTrackParameter(i); }
TMatrixD g(5,5);
trk->GetErrorMatrix(g);
g.Invert(&det_tmp);
if (det_tmp == 0) {
cout<<"g inv. fault!! in LCDVToplVRT::FilteringToRemoveTrack"
<<" N of tracks="<<GetTrackEntries()<<endl;
return -1;
}
// setup inverse of covariance error matrix
//TMatrixD cinv(TMatrixD::kInverted,m_error);
TMatrixD cinv(m_error);
cinv.Invert(&det_tmp);
if (det_tmp == 0) {
cout<<"cinv inv. fault!! in LCDVToplVRT::FilteringToRemoveTrack"<<endl;
return -1;
}
// setup vertex momentum.
TMatrixD q0(3,1); SmoothingParameters(trk,q0);
// loop over till chi**2 is converged.
TMatrixD a(5,3);
TMatrixD b(5,3);
TMatrixD h0(5,1);
TMatrixD x(x0);
TMatrixD q(q0);
TMatrixD mcov(3,3);
Double_t dcsold=1e20;
linearizeChargedTrack(a,b,h0,x,q);
TMatrixD r(p0); r -= h0;
TMatrixD ax0(a,TMatrixD::kMult,x0); r -= ax0;
TMatrixD bq0(b,TMatrixD::kMult,q0); r -= bq0;
TMatrixD gr(g,TMatrixD::kMult,r);
TMatrixD rgr(r,TMatrixD::kTransposeMult,gr);
Double_t chisq=rgr(0,0);
chisquared -= chisq;
Int_t iteration=0;
TMatrixD x_save(x);
TMatrixD q_save(q);
TMatrixD mcov_save(mcov);
Double_t chisq_save=1e20;
for (iteration=0; iteration < maxtry ; iteration++) {
linearizeChargedTrack(a,b,h0,x,q);
TMatrixD g_b(g,TMatrixD::kMult,b);
TMatrixD w(b,TMatrixD::kTransposeMult,g_b);
w.Invert(&det_tmp);
if (det_tmp == 0) {
cout<<"w inv. fault!! in LCDVToplVRT::FilteringToRemoveTrack"<<endl;
return -1;
}
TMatrixD bTg(b,TMatrixD::kTransposeMult, g);
TMatrixD wbTg(w,TMatrixD::kMult , bTg);
TMatrixD bwbTg(b,TMatrixD::kMult , wbTg);
TMatrixD gbwbTg(g,TMatrixD::kMult ,bwbTg);
TMatrixD gb(g); gb -= gbwbTg;
TMatrixD gba(gb,TMatrixD::kMult,a);
TMatrixD cov(cinv);
TMatrixD aTgba(a,TMatrixD::kTransposeMult,gba); cov -= aTgba;
cov.Invert(&det_tmp);
if (det_tmp == 0) {
cout<<"cov inv. fault!! in LCDVToplVRT::FilteringToRemoveTrack"<<endl;
return -1;
}
mcov=cov;
TMatrixD p0h0(p0); p0h0 -= h0;
TMatrixD gbp0h0(gb,TMatrixD::kMult,p0h0);
TMatrixD aTgbp0h0(a,TMatrixD::kTransposeMult,gbp0h0);
TMatrixD mtmp(cinv,TMatrixD::kMult,x0); mtmp -= aTgbp0h0;
x.Mult(cov,mtmp);
TMatrixD ax(a,TMatrixD::kMult,x);
TMatrixD p0h0ax(p0h0); p0h0ax -= ax;
TMatrixD gp0h0ax(g,TMatrixD::kMult,p0h0ax);
TMatrixD bTgp0h0ax(b,TMatrixD::kTransposeMult,gp0h0ax);
q.Mult(w,bTgp0h0ax);
//TMatrixD covinv(TMatrixD::kInverted,cov);
TMatrixD covinv(cov);
covinv.Invert(&det_tmp);
if (det_tmp == 0) {
cout<<"covinv inv. fault!! in LCDVToplVRT::FilteringToRemoveTrack"<<endl;
return -1;
}
TMatrixD xx0(x); xx0 -= x0;
TMatrixD covinvxx0(covinv,TMatrixD::kMult,xx0);
TMatrixD mdeltachi2(xx0,TMatrixD::kTransposeMult,covinvxx0);
chisq=mdeltachi2(0,0);
if (iteration > 0 && TMath::Abs(chisq-dcsold) < epscut) break;
// Iteration step is diverged...return the best fit...
if (chisq > 1.3*dcsold) {
x = x_save;
q = q_save;
mcov = mcov_save;
chisq = chisq_save;
break;
}
if (iteration == 0 || chisq < chisq_save) {
x_save=x;
q_save=q;
mcov_save=mcov;
chisq_save=chisq;
}
dcsold=chisq;
}
// save new vertex infomation.
m_fitparm = x;
m_error = mcov;
chisquared -= chisq;
if (chisquared < 0) chisquared=0;
qvtx -= trk->GetCharge();
return iteration;
}
void LCDVToplVRT::linearizeChargedTrack(TMatrixD& a, TMatrixD& b, TMatrixD& h0,
TMatrixD& v0, TMatrixD& q0) {
/*------------------------------------------------------------------- */
/* */
/* LinearizeTrack: a c/c++ version of Lothar Bauerdicks fortran */
/* */
/* calculate and return A, B, h0 from v0, q0: */
/* */
/* calculate coefficients of taylor expansion for estimated */
/* track parameters h around point (v0,q0): */
/* */
/* h = h(v,q) + eps */
/* approx h(v0,q0) + A.(v-v0) + B.(q-q0) + eps */
/* = h0 + A.v + B.q + eps */
/* where */
/* A = dh/dv(v0,q0), */
/* B = dh/dq(v0,q0), and */
/* h0 = h(v0,q0) - A.v0 - B.q0 are the */
/* derivatives of track parameters w/r to vertex position */
/* and track 3-momentum at space point v0 and estimated momentum q0 */
/* */
/* track model: */
/* h = {d0,phi0,w0,z0,tl0} */
/* v = {x,y,z} */
/* q = {w,tl,psi} */
/* */
/* for a charged particle (helix parameters in BaBar conventions) */
/* -------------------------------------------------------------- */
// d0 = 1/w - (1/w + y*cos(phi) - x*sin(phi))/cos(gamma)
// phi0 = phi + gamma
// w0 = w
// z0 = z + gamma/w*tl
// tl0 = tl
// where
// phi = atan2(Py/Px)
// gamma = atan((x*cos(phi) + y*sin(phi))/
// (x*sin(phi) - y0*cos(phi) - 1/w))
/* */
/* for a neutral particle (w == 0) */
/* ------------------------------- */
/* d0 = */
/* w0 = w */
/* tl0 = tl */
/* psi0 = psi */
/* z0 = z - r*cos(xi)*tl */
/* */
/*--------------------------------------------------------------------*/
Double_t w = q0(0,0);
Double_t phi = q0(1,0);
Double_t tl = q0(2,0);
Double_t x = v0(0,0);
Double_t y = v0(1,0);
Double_t z = v0(2,0);
Double_t cp = TMath::Cos(phi);
Double_t sp = TMath::Sin(phi);
Double_t oow = 1.0/w;
Double_t bb = x*cp + y*sp;
Double_t cc = oow + y*cp - x*sp;
Double_t gma = TMath::ATan(bb/(-cc));
Double_t cg = TMath::Cos(gma);
Double_t sg = TMath::Sin(gma);
//Calc. transformed quantities
Double_t phi0= phi + gma;
//Double_t d0 = oow - cc/cg;
Double_t d0 =-oow + cc/cg;
Double_t z0 = z + gma/w*tl;
// h(v0,q0)
h0(0,0) = d0;
h0(1,0) = phi0;
h0(2,0) = w;
h0(3,0) = z0;
h0(4,0) = tl;
//calc. Jacobians
Double_t mx=x - oow*sp;
Double_t my=y + oow*cp;
Double_t m2=mx*mx + my*my;
TMatrixD dgdv(3,1);
dgdv(0,0)=-my/m2;
dgdv(1,0)= mx/m2;
dgdv(2,0)= 0.0;
TMatrixD dgdq(3,1);
dgdq(0,0)=-oow*oow*bb/m2;
dgdq(1,0)= y*dgdv(0,0) - x*dgdv(1,0);
dgdq(2,0)= 0.0;
// d d0 / d x, d y, d z
//a(0,0) = (cg*sp - sg*dgdv(0,0)*(oow - x*sp + y*cp))/cg/cg;
//a(0,1) =-(cg*cp + sg*dgdv(1,0)*(oow - x*sp + y*cp))/cg/cg;
//a(0,2) = 0.0;
a(0,0) =-(cg*sp - sg*dgdv(0,0)*(oow - x*sp + y*cp))/cg/cg;
a(0,1) =+(cg*cp + sg*dgdv(1,0)*(oow - x*sp + y*cp))/cg/cg;
a(0,2) = 0.0;
// d phi0/ d x, d y, d z
a(1,0) = dgdv(0,0);
a(1,1) = dgdv(1,0);
a(1,2) = 0.0;
// d w / d x, d y, d z
a(2,0) = 0.0;
a(2,1) = 0.0;
a(2,2) = 0.0;
// d z0 / d x, d y, d z
a(3,0) = tl/w*dgdv(0,0);
a(3,1) = tl/w*dgdv(1,0);
a(3,2) = 1.0;
// d tl / d x, d y, d z
a(4,0) = 0.0;
a(4,1) = 0.0;
a(4,2) = 0.0;
// d d0 / d w0, d phi00, d tl0
//b(0,0) =-oow*oow + (oow*oow*cg - sg*dgdq(0,0)*cc)/cg/cg;
//b(0,1) = (bb*cg - sg*dgdq(1,0)*cc)/cg/cg;
//b(0,2) = 0.0;
b(0,0) =-(-oow*oow + (oow*oow*cg - sg*dgdq(0,0)*cc)/cg/cg);
b(0,1) =-((bb*cg - sg*dgdq(1,0)*cc)/cg/cg);
b(0,2) = 0.0;
// d psi0 / d w0, d phi00, d tl0
b(1,0) = dgdq(0,0);
b(1,1) = 1.0 + dgdq(1,0);
b(1,2) = 0.0;
// d w / d w0, d phi00, d tl0
b(2,0) = 1.0;
b(2,1) = 0.0;
b(2,2) = 0.0;
// d z0 / d w0, d phi00, d tl0
b(3,0) = tl/w*(dgdq(0,0) - gma/w);
b(3,1) = tl/w* dgdq(1,0);
b(3,2) = gma/w;
// d tl / d w0, d phi00, d tl0
b(4,0) = 0.0;
b(4,1) = 0.0;
b(4,2) = 1.0;
// h0(v0,q0) = h(v0,q0) - a*v0 - b*q0
TMatrixD av0(a,TMatrixD::kMult,v0);
TMatrixD bq0(b,TMatrixD::kMult,q0);
TMatrixD av0bq0(av0); av0bq0 += bq0;
h0 -= av0bq0;
}
void LCDVToplVRT::SmoothingParameters(LCDVToplTRK* trk, TMatrixD& q) {
Int_t i=0;
Double_t det_tmp=0;
// setup track parameter and its error
TMatrixD p0(5,1);
for (i=0 ; i < 5 ; i++) { p0(i,0)=trk->GetTrackParameter(i); }
TMatrixD g(5,5);
trk->GetErrorMatrix(g);
g.Invert(&det_tmp);
if (det_tmp == 0) {
cout<<"g inv. fault!! in LCDVToplVRT::SmoothingParameters"<<endl;
}
// setup vertex momentum.
TMatrixD q0(3,1);
q0(0,0) = p0(2,0); q0(1,0) = p0(1,0); q0(2,0) = p0(4,0);
TMatrixD a(5,3);
TMatrixD b(5,3);
TMatrixD h0(5,1);
TMatrixD x0(m_fitparm);
linearizeChargedTrack(a,b,h0,x0,q0);
TMatrixD g_b(g,TMatrixD::kMult,b);
TMatrixD w(b,TMatrixD::kTransposeMult,g_b);
w.Invert(&det_tmp);
if (det_tmp == 0) {
cout<<"w inv. fault!! in LCDVToplVRT::SmoothingParameters"<<endl;
}
TMatrixD p0h0(p0); p0h0 -= h0;
TMatrixD ax(a,TMatrixD::kMult,x0);
TMatrixD p0h0ax(p0h0); p0h0ax -= ax;
TMatrixD gp0h0ax(g,TMatrixD::kMult,p0h0ax);
TMatrixD bTgp0h0ax(b,TMatrixD::kTransposeMult,gp0h0ax);
q.Mult(w,bTgp0h0ax);
}
//Vertex Fitter
Int_t LCDVToplVRT::VertexFit() {
Int_t i=0;
for (i=0 ; i < 3 ; i++) { m_error(i,i) = 1.0; }
chisquared = 0;
qvtx = 0;
Int_t ntrks=GetTrackEntries();
LCDVToplTRK* trk =0;
for (i=0 ; i < ntrks ; i++) {
trk=GetTrackAt(i);
if (trk->GetTrackID() >= 0) {
if (FilteringToAddTrack(trk) < 0) {
cout<<"Something wrong FilteringToAddTrack trk="<<trk<<endl;
return 0;
}
}
}
return 1;
}
Int_t LCDVToplVRT::VertexFit(TVector3 xvinit) {
Int_t i=0;
for (i=0 ; i < 3 ; i++) { m_fitparm(i,0) = xvinit[i]; }
return VertexFit();
}
ROOT page - Class index - Top of the page
This page has been automatically generated. If you have any comments or suggestions about the page layout send a mail to ROOT support, or contact the developers with any questions or problems regarding ROOT.