Fork me on GitHub

Context Navigation

source: git/external/TrackCovariance/VertexFit.cc@ 5c257a5

Visit:

Last change on this file since 5c257a5 was 53f4746, checked in by Franco BEDESCHI <bed@…>, 4 years ago
Cluster counting example and VertexFit update
Property mode set to `100644`
File size: 17.2 KB

Rev	Line
[537d24f]	1	#include <TMath.h>
	2	#include <TVectorD.h>
	3	#include <TVector3.h>
	4	#include <TMatrixD.h>
	5	#include <TMatrixDSym.h>
	6	#include "VertexFit.h"
	7	//
	8	// Constructors
	9	//
[82db145]	10	//
	11	// Empty construction (to be used when adding tracks later with AddTrk() )
[537d24f]	12	VertexFit::VertexFit()
	13	{
	14	fNtr = 0;
[82db145]	15	fRold = -1.0;
[537d24f]	16	fVtxDone = kFALSE;
	17	fVtxCst = kFALSE;
	18	fxCst.ResizeTo(3);
[91ef0b8]	19	fCovCst.ResizeTo(3, 3);
[53f4746]	20	fCovCstInv.ResizeTo(3, 3);
[537d24f]	21	fXv.ResizeTo(3);
[91ef0b8]	22	fcovXv.ResizeTo(3, 3);
[537d24f]	23	}
[82db145]	24	//
	25	// Build from list of parameters and covariances
[537d24f]	26	VertexFit::VertexFit(Int_t Ntr, TVectorD trkPar, TMatrixDSym trkCov)
	27	{
	28	fNtr = Ntr;
[82db145]	29	fRold = -1.0;
[537d24f]	30	fVtxDone = kFALSE;
	31	fVtxCst = kFALSE;
	32	fxCst.ResizeTo(3);
[91ef0b8]	33	fCovCst.ResizeTo(3, 3);
[53f4746]	34	fCovCstInv.ResizeTo(3, 3);
[537d24f]	35	fXv.ResizeTo(3);
[91ef0b8]	36	fcovXv.ResizeTo(3, 3);
[537d24f]	37	//
[82db145]	38	for (Int_t i = 0; i < fNtr; i++)
	39	{
	40	TVectorD pr = *trkPar[i];
	41	fPar.push_back(new TVectorD(pr));
[53f4746]	42	fParNew.push_back(new TVectorD(pr));
[82db145]	43	TMatrixDSym cv = *trkCov[i];
	44	fCov.push_back(new TMatrixDSym(cv));
[53f4746]	45	fCovNew.push_back(new TMatrixDSym(cv));
[82db145]	46	}
	47	fChi2List.ResizeTo(fNtr);
	48	//
[537d24f]	49	}
[82db145]	50	//
	51	// Build from ObsTrk list of tracks
[537d24f]	52	VertexFit::VertexFit(Int_t Ntr, ObsTrk** track)
	53	{
	54	fNtr = Ntr;
[82db145]	55	fRold = -1.0;
[537d24f]	56	fVtxDone = kFALSE;
	57	fVtxCst = kFALSE;
	58	fxCst.ResizeTo(3);
[91ef0b8]	59	fCovCst.ResizeTo(3, 3);
[53f4746]	60	fCovCstInv.ResizeTo(3, 3);
[537d24f]	61	fXv.ResizeTo(3);
[91ef0b8]	62	fcovXv.ResizeTo(3, 3);
[537d24f]	63	//
[82db145]	64	fChi2List.ResizeTo(fNtr);
	65	for (Int_t i = 0; i < fNtr; i++)
[537d24f]	66	{
[82db145]	67	fPar.push_back(new TVectorD(track[i]->GetObsPar()));
[53f4746]	68	fParNew.push_back(new TVectorD(track[i]->GetObsPar()));
[82db145]	69	fCov.push_back(new TMatrixDSym(track[i]->GetCov()));
[53f4746]	70	fCovNew.push_back(new TMatrixDSym(track[i]->GetCov()));
[537d24f]	71	}
	72	}
	73	//
	74	// Destructor
[82db145]	75	//
	76	void VertexFit::ResetWrkArrays()
	77	{
	78	Int_t N = (Int_t)ffi.size();
	79	for (Int_t i = 0; i < N; i++)
	80	{
	81	if (fx0i[i]) { fx0i[i]->Clear(); delete fx0i[i]; }
[53f4746]	82	if (fai[i]) { fai[i]->Clear(); delete fai[i]; }
	83	if (fdi[i]) { fdi[i]->Clear(); delete fdi[i]; }
	84	if (fAti[i]) { fAti[i]->Clear(); delete fAti[i]; }
	85	if (fDi[i]) { fDi[i]->Clear(); delete fDi[i]; }
	86	if (fWi[i]) { fWi[i]->Clear(); delete fWi[i]; }
[82db145]	87	if (fWinvi[i]){ fWinvi[i]->Clear(); delete fWinvi[i]; }
	88	}
	89	fa2i.clear();
	90	fx0i.clear();
	91	fai.clear();
[53f4746]	92	fdi.clear();
	93	fAti.clear();
[82db145]	94	fDi.clear();
	95	fWi.clear();
	96	fWinvi.clear();
	97	}
[537d24f]	98	VertexFit::~VertexFit()
	99	{
[82db145]	100	fxCst.Clear();
[53f4746]	101	fCovCst.Clear();
	102	fCovCstInv.Clear();
[82db145]	103	fXv.Clear();
	104	fcovXv.Clear();
	105	fChi2List.Clear();
[537d24f]	106	//
[91ef0b8]	107	for (Int_t i = 0; i < fNtr; i++)
[537d24f]	108	{
[53f4746]	109	fPar[i]->Clear(); delete fPar[i];
	110	fParNew[i]->Clear(); delete fParNew[i];
	111	fCov[i]->Clear(); delete fCov[i];
	112	fCovNew[i]->Clear(); delete fCovNew[i];
[537d24f]	113	}
[82db145]	114	fPar.clear();
[53f4746]	115	fParNew.clear();
	116	fCov.clear();
	117	fCovNew.clear();
[82db145]	118	//
	119	ResetWrkArrays();
	120	ffi.clear();
[537d24f]	121	fNtr = 0;
	122	}
	123	//
[82db145]	124	Double_t VertexFit::FastRv(TVectorD p1, TVectorD p2)
[537d24f]	125	{
	126	//
[82db145]	127	// Find radius of minimum distance between two tracks
[537d24f]	128	//
	129	// p = (D,phi, C, z0, ct)
	130	//
	131	// Define arrays
	132	//
[82db145]	133	Double_t C1 = p1(2);
	134	Double_t C2 = p2(2);
	135	Double_t ph1 = p1(1);
	136	Double_t ph2 = p2(1);
[537d24f]	137	TVectorD x0 = Fill_x0(p1);
	138	TVectorD y0 = Fill_x0(p2);
	139	TVectorD n = Fill_a(p1, 0.0);
[82db145]	140	n = (2C1);
[537d24f]	141	TVectorD k = Fill_a(p2, 0.0);
[82db145]	142	k = (2C2);
[537d24f]	143	//
	144	// Setup and solve linear system
	145	//
	146	Double_t nn = 0; for (Int_t i = 0; i < 3; i++)nn += n(i) * n(i);
	147	Double_t nk = 0; for (Int_t i = 0; i < 3; i++)nk += n(i) * k(i);
	148	Double_t kk = 0; for (Int_t i = 0; i < 3; i++)kk += k(i) * k(i);
	149	Double_t discr = nn * kk - nk * nk;
	150	TMatrixDSym H(2);
	151	H(0, 0) = kk;
	152	H(0, 1) = nk;
	153	H(1, 0) = H(0, 1);
	154	H(1, 1) = nn;
	155	TVectorD c(2);
[91ef0b8]	156	c(0) = 0; for (Int_t i = 0; i < 3; i++)c(0) += n(i) * (y0(i) - x0(i));
[537d24f]	157	c(1) = 0; for (Int_t i = 0; i < 3; i++)c(1) += -k(i) * (y0(i) - x0(i));
	158	TVectorD smin = (H * c);
	159	smin *= 1.0 / discr;
	160	//
	161	TVectorD X = x0 + smin(0) * n;
	162	TVectorD Y = y0 + smin(1) * k;
	163	//
[82db145]	164	// Higher order corrections
	165	X(0) += -C1 * smin(0) * smin(0) * TMath::Sin(ph1);
	166	X(1) += C1 * smin(0) * smin(0) * TMath::Cos(ph1);
	167	Y(0) += -C2 * smin(1) * smin(1) * TMath::Sin(ph2);
	168	Y(1) += C2 * smin(1) * smin(1) * TMath::Cos(ph2);
	169	//
	170	TVectorD Xavg = 0.5 * (X + Y);
	171	//
	172	//
	173	return TMath::Sqrt(Xavg(0)Xavg(0)+Xavg(1)Xavg(1));
[537d24f]	174	}
[82db145]	175	//
	176	// Starting radius determination
	177	Double_t VertexFit::StartRadius()
[537d24f]	178	{
	179	//
[82db145]	180	// Maximum impact parameter
	181	Double_t Rd = 0;
	182	for (Int_t i = 0; i < fNtr; i++)
	183	{
	184	TVectorD par = *fPar[i];
	185	Double_t Dabs = TMath::Abs(par(0));
	186	if (Dabs > Rd)Rd = Dabs;
	187	}
	188	//-----------------------------
[537d24f]	189	//
[82db145]	190	// Find track pair with phi difference closest to pi/2
	191	Int_t isel = 0; Int_t jsel = 0; // selected track indices
	192	Double_t dSinMax = 0.0; // Max phi difference
	193	for (Int_t i = 0; i < fNtr - 1; i++)
[537d24f]	194	{
[82db145]	195	TVectorD pari = *fPar[i];
	196	Double_t phi1 = pari(1);
	197
	198	for (Int_t j = i + 1; j < fNtr; j++)
[537d24f]	199	{
[82db145]	200	TVectorD parj = *fPar[j];
	201	Double_t phi2 = parj(1);
	202	Double_t Sindphi = TMath::Abs(TMath::Sin(phi2 - phi1));
	203	if (Sindphi > dSinMax)
	204	{
	205	isel = i; jsel = j;
	206	dSinMax = Sindphi;
	207	}
[537d24f]	208	}
	209	}
	210	//
[82db145]	211	//------------------------------------------
	212	//
	213	// Find radius of minimum distrance between tracks
	214	TVectorD p1 = *fPar[isel];
	215	TVectorD p2 = *fPar[jsel];
	216	Double_t R = FastRv(p1, p2);
	217	//
	218	R = 0.9 * R + 0.1 * Rd; // Protect for overshoot
	219	//
[537d24f]	220	return R;
	221	}
[82db145]	222	//
	223	// Regularized symmetric matrix inversion
	224	//
	225	TMatrixDSym VertexFit::RegInv(TMatrixDSym& Min)
[537d24f]	226	{
[82db145]	227	TMatrixDSym M = Min; // Decouple from input
	228	Int_t N = M.GetNrows(); // Matrix size
	229	TMatrixDSym D(N); D.Zero(); // Normaliztion matrix
	230	TMatrixDSym R(N); // Normarized matrix
	231	TMatrixDSym Rinv(N); // Inverse of R
	232	TMatrixDSym Minv(N); // Inverse of M
	233	//
	234	// Check for 0's and normalize
	235	for (Int_t i = 0; i < N; i++)
[537d24f]	236	{
[82db145]	237	if (M(i, i) != 0.0) D(i, i) = 1. / TMath::Sqrt(TMath::Abs(M(i, i)));
	238	else D(i, i) = 1.0;
[537d24f]	239	}
[82db145]	240	R = M.Similarity(D);
	241	//
	242	// Recursive algorithms stops when N = 2
	243	//
	244	//****************
	245	// case N = 2 ***
	246	//****************
	247	if (N == 2)
[537d24f]	248	{
[82db145]	249	Double_t det = R(0, 0) * R(1, 1) - R(0, 1) * R(1, 0);
	250	if (det == 0)
[537d24f]	251	{
[82db145]	252	std::cout << "VertexFit::RegInv: null determinant for N = 2" << std::endl;
	253	Rinv.Zero(); // Return null matrix
[537d24f]	254	}
[82db145]	255	else
[537d24f]	256	{
[82db145]	257	// invert matrix
	258	Rinv(0, 0) = R(1, 1);
	259	Rinv(0, 1) = -R(0, 1);
	260	Rinv(1, 0) = Rinv(0, 1);
	261	Rinv(1, 1) = R(0, 0);
	262	Rinv *= 1. / det;
[537d24f]	263	}
	264	}
[82db145]	265	//****************
	266	// case N > 2 ***
	267	//****************
[537d24f]	268	else
	269	{
[82db145]	270	// Break up matrix
	271	TMatrixDSym Q = R.GetSub(0, N - 2, 0, N - 2); // Upper left
	272	TVectorD p(N - 1);
	273	for (Int_t i = 0; i < N - 1; i++)p(i) = R(N - 1, i);
	274	Double_t q = R(N - 1, N - 1);
	275	//Invert pieces and re-assemble
	276	TMatrixDSym Ainv(N - 1);
	277	TMatrixDSym A(N - 1);
	278	if (TMath::Abs(q) > 1.0e-15)
	279	{
	280	// Case \|q\| > 0
	281	Ainv.Rank1Update(p, -1.0 / q);
	282	Ainv += Q;
	283	A = RegInv(Ainv); // Recursive call
	284	TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A;
	285	//
	286	TVectorD b = (-1.0 / q) * (A * p);
	287	for (Int_t i = 0; i < N - 1; i++)
	288	{
	289	Rinv(N - 1, i) = b(i);
	290	Rinv(i, N - 1) = b(i);
	291	}
	292	//
	293	Double_t pdotb = 0.;
	294	for (Int_t i = 0; i < N - 1; i++)pdotb += p(i) * b(i);
	295	Double_t c = (1.0 - pdotb) / q;
	296	Rinv(N - 1, N - 1) = c;
	297	}
	298	else
	299	{
	300	// case q = 0
	301	TMatrixDSym Qinv = RegInv(Q); // Recursive call
	302	Double_t a = Qinv.Similarity(p);
	303	Double_t c = -1.0 / a;
	304	Rinv(N - 1, N - 1) = c;
	305	//
	306	TVectorD b = (1.0 / a) * (Qinv * p);
	307	for (Int_t i = 0; i < N - 1; i++)
	308	{
	309	Rinv(N - 1, i) = b(i);
	310	Rinv(i, N - 1) = b(i);
	311	}
	312	//
	313	A.Rank1Update(p, -1 / a);
	314	A += Q;
	315	A.Similarity(Qinv);
	316	TMatrixDSub(Rinv, 0, N - 2, 0, N - 2) = A;
	317	}
[537d24f]	318	}
[82db145]	319	Minv = Rinv.Similarity(D);
	320	return Minv;
[537d24f]	321	}
	322	//
	323	//
	324	//
	325	TMatrixD VertexFit::Fill_A(TVectorD par, Double_t phi)
	326	{
	327	//
	328	// Derivative of track 3D position vector with respect to track parameters at constant phase
	329	//
	330	// par = vector of track parameters
	331	// phi = phase
	332	//
	333	TMatrixD A(3, 5);
	334	//
	335	// Decode input arrays
	336	//
	337	Double_t D = par(0);
	338	Double_t p0 = par(1);
	339	Double_t C = par(2);
	340	Double_t z0 = par(3);
	341	Double_t ct = par(4);
	342	//
	343	// Fill derivative matrix dx/d alpha
	344	// D
	345	A(0, 0) = -TMath::Sin(p0);
	346	A(1, 0) = TMath::Cos(p0);
	347	A(2, 0) = 0.0;
	348	// phi0
[91ef0b8]	349	A(0, 1) = -D * TMath::Cos(p0) + (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C);
	350	A(1, 1) = -D * TMath::Sin(p0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
[537d24f]	351	A(2, 1) = 0.0;
	352	// C
[91ef0b8]	353	A(0, 2) = -(TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C * C);
	354	A(1, 2) = (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C * C);
	355	A(2, 2) = -ct * phi / (2 * C * C);
[537d24f]	356	// z0
	357	A(0, 3) = 0.0;
	358	A(1, 3) = 0.0;
	359	A(2, 3) = 1.0;
	360	// ct = lambda
	361	A(0, 4) = 0.0;
	362	A(1, 4) = 0.0;
	363	A(2, 4) = phi / (2 * C);
	364	//
	365	return A;
	366	}
	367	//
	368	TVectorD VertexFit::Fill_a(TVectorD par, Double_t phi)
	369	{
	370	//
	371	// Derivative of track 3D position vector with respect to phase at constant track parameters
	372	//
	373	// par = vector of track parameters
	374	// phi = phase
	375	//
	376	TVectorD a(3);
	377	//
	378	// Decode input arrays
	379	//
	380	Double_t D = par(0);
	381	Double_t p0 = par(1);
	382	Double_t C = par(2);
	383	Double_t z0 = par(3);
	384	Double_t ct = par(4);
	385	//
	386	a(0) = TMath::Cos(phi + p0) / (2 * C);
	387	a(1) = TMath::Sin(phi + p0) / (2 * C);
	388	a(2) = ct / (2 * C);
	389	//
	390	return a;
	391	}
	392	//
	393	TVectorD VertexFit::Fill_x0(TVectorD par)
	394	{
	395	//
	396	// Calculate track 3D position at R = \|D\| (minimum approach to z-axis)
	397	//
	398	TVectorD x0(3);
	399	//
	400	// Decode input arrays
	401	//
	402	Double_t D = par(0);
	403	Double_t p0 = par(1);
	404	Double_t C = par(2);
	405	Double_t z0 = par(3);
	406	Double_t ct = par(4);
	407	//
[91ef0b8]	408	x0(0) = -D * TMath::Sin(p0);
	409	x0(1) = D * TMath::Cos(p0);
[537d24f]	410	x0(2) = z0;
	411	//
	412	return x0;
	413	}
	414	//
	415	TVectorD VertexFit::Fill_x(TVectorD par, Double_t phi)
	416	{
	417	//
	418	// Calculate track 3D position for a given phase, phi
	419	//
	420	TVectorD x(3);
	421	//
	422	// Decode input arrays
	423	//
	424	Double_t D = par(0);
	425	Double_t p0 = par(1);
	426	Double_t C = par(2);
	427	Double_t z0 = par(3);
	428	Double_t ct = par(4);
	429	//
	430	TVectorD x0 = Fill_x0(par);
	431	x(0) = x0(0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
	432	x(1) = x0(1) - (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C);
[91ef0b8]	433	x(2) = x0(2) + ct * phi / (2 * C);
[537d24f]	434	//
	435	return x;
	436	}
	437	//
[82db145]	438	void VertexFit::UpdateTrkArrays(Int_t i)
[537d24f]	439	{
	440	//
[82db145]	441	// Get track parameters and their covariance
[53f4746]	442	TVectorD par = *fParNew[i];
[82db145]	443	TMatrixDSym Cov = *fCov[i];
[537d24f]	444	//
[82db145]	445	// Fill all track related work arrays arrays
	446	Double_t fs = ffi[i]; // Get phase
	447	TVectorD xs = Fill_x(par, fs);
	448	fx0i.push_back(new TVectorD(xs)); // Start helix position
[537d24f]	449	//
[82db145]	450	TMatrixD A = Fill_A(par, fs); // A = dx/da = derivatives wrt track parameters
[53f4746]	451	TMatrixD At(TMatrixD::kTransposed, A); // A transposed
	452	fAti.push_back(new TMatrixD(At)); // Store A'
	453	TVectorD di = A * (par - *fPar[i]); // x-shift
	454	fdi.push_back(new TVectorD(di)); // Store x-shift
[82db145]	455	TMatrixDSym Winv = Cov.Similarity(A); // W^-1 = ACA'
[53f4746]	456	fWinvi.push_back(new TMatrixDSym(Winv)); // Store W^-1 matrix
[537d24f]	457	//
[82db145]	458	TMatrixDSym W = RegInv(Winv); // W = (ACA')^-1
	459	fWi.push_back(new TMatrixDSym(W)); // Store W matrix
[537d24f]	460	//
[82db145]	461	TVectorD a = Fill_a(par, fs); // a = dx/ds = derivatives wrt phase
	462	fai.push_back(new TVectorD(a)); // Store a
	463	//
	464	Double_t a2 = W.Similarity(a);
	465	fa2i.push_back(a2); // Store a2
	466	//
	467	// Build D matrix
	468	TMatrixDSym B(3);
	469	B.Rank1Update(a, -1. / a2);
	470	B.Similarity(W);
	471	TMatrixDSym Ds = W + B; // D matrix
	472	fDi.push_back(new TMatrixDSym(Ds)); // Store D matrix
	473	}
	474	//
	475	void VertexFit::VertexFitter()
	476	{
	477	//std::cout << "VertexFitter: just in" << std::endl;
[53f4746]	478	if (fNtr < 2 && !fVtxCst){
[82db145]	479	std::cout << "VertexFit::VertexFitter - Method called with less than 2 tracks - Aborting " << std::endl;
	480	std::exit(1);
[537d24f]	481	}
	482	//
[82db145]	483	// Vertex fit
	484	//
	485	// Initial variable definitions
	486	TVectorD x(3);
	487	TMatrixDSym covX(3);
	488	Double_t Chi2 = 0;
	489	TVectorD x0 = fXv; // If previous fit done
[53f4746]	490	if (fRold < 0.0) {
	491	// External constraint
	492	if (fVtxCst) fRold = TMath::Sqrt(fxCst(0) * fxCst(0) + fxCst(1) * fxCst(1));
	493	// No constraint
	494	else for (Int_t i = 0; i < 3; i++)x0(i) = 1000.; // Set to arbitrary large value
	495	}
[82db145]	496	//
	497	// Starting vertex radius approximation
	498	//
	499	Double_t R = fRold; // Use previous fit if available
	500	if (R < 0.0) R = StartRadius(); // Rough vertex estimate
[53f4746]	501	//std::cout << "Start radius: " << R << std::endl;
[537d24f]	502	//
	503	// Iteration properties
	504	//
	505	Int_t Ntry = 0;
	506	Int_t TryMax = 100;
	507	Double_t eps = 1.0e-9; // vertex stability
	508	Double_t epsi = 1000.;
	509	//
[82db145]	510	// Iteration loop
[537d24f]	511	while (epsi > eps && Ntry < TryMax) // Iterate until found vertex is stable
	512	{
[82db145]	513	// Initialize arrays
[537d24f]	514	x.Zero();
	515	TVectorD cterm(3); TMatrixDSym H(3); TMatrixDSym DW1D(3);
[82db145]	516	covX.Zero(); // Reset vertex covariance
[537d24f]	517	cterm.Zero(); // Reset constant term
	518	H.Zero(); // Reset H matrix
	519	DW1D.Zero();
[127644a]	520	//
[82db145]	521	// Reset work arrays
	522	//
	523	ResetWrkArrays();
	524	//
	525	// Start loop on tracks
	526	//
[537d24f]	527	for (Int_t i = 0; i < fNtr; i++)
	528	{
[127644a]	529	// Get track helix parameters and their covariance matrix
[537d24f]	530	TVectorD par = *fPar[i];
[127644a]	531	TMatrixDSym Cov = *fCov[i];
[82db145]	532	//
	533	// For first iteration only
[537d24f]	534	Double_t fs;
	535	if (Ntry <= 0) // Initialize all phases on first pass
	536	{
	537	Double_t D = par(0);
	538	Double_t C = par(2);
[91ef0b8]	539	Double_t arg = TMath::Max(1.0e-6, (R * R - D * D) / (1 + 2 * C * D));
	540	fs = 2 * TMath::ASin(C * TMath::Sqrt(arg));
[82db145]	541	ffi.push_back(fs);
[537d24f]	542	}
	543	//
[82db145]	544	// Update track related arrays
[537d24f]	545	//
[82db145]	546	UpdateTrkArrays(i);
	547	TMatrixDSym Ds = *fDi[i];
	548	TMatrixDSym Winv = *fWinvi[i];
[537d24f]	549	TMatrixDSym DsW1Ds = Winv.Similarity(Ds); // Service matrix to calculate covX
[82db145]	550	//
	551	// Update global arrays
[127644a]	552	DW1D += DsW1Ds;
[537d24f]	553	// Update hessian
	554	H += Ds;
	555	// update constant term
[53f4746]	556	TVectorD xs = fx0i[i] - fdi[i];
	557	TVectorD xx0 = *fx0i[i];
	558	/*
	559	std::cout << "Iter. " << Ntry << ", trk " << i << ", x= "
	560	<< xx0(0) << ", " << xx0(1) << ", " << xx0(2)<<
	561	", ph0= "<<par(1)<< std::endl;
	562	*/
[537d24f]	563	cterm += Ds * xs;
	564	} // End loop on tracks
[53f4746]	565	// Some additions in case of external constraints
	566	if (fVtxCst) {
	567	H += fCovCstInv;
	568	cterm += fCovCstInv * fxCst;
	569	DW1D += fCovCstInv;
	570	}
[537d24f]	571	//
	572	// update vertex position
[82db145]	573	TMatrixDSym H1 = RegInv(H);
[91ef0b8]	574	x = H1 * cterm;
[82db145]	575	//
[537d24f]	576	// Update vertex covariance
	577	covX = DW1D.Similarity(H1);
[82db145]	578	//
[537d24f]	579	// Update phases and chi^2
	580	Chi2 = 0.0;
	581	for (Int_t i = 0; i < fNtr; i++)
	582	{
[53f4746]	583	TVectorD lambda = (fDi[i]) (fx0i[i] - x - fdi[i]);
[82db145]	584	TMatrixDSym Wm1 = *fWinvi[i];
[537d24f]	585	fChi2List(i) = Wm1.Similarity(lambda);
	586	Chi2 += fChi2List(i);
[82db145]	587	TVectorD a = *fai[i];
	588	TVectorD b = (fWi[i]) (x - (*fx0i[i]));
	589	for (Int_t j = 0; j < 3; j++)ffi[i] += a(j) * b(j) / fa2i[i];
[53f4746]	590	TVectorD newPar = fPar[i] - ((fCov[i]) * (fAti[i])) lambda;
	591	fParNew[i] = new TVectorD(newPar);
[537d24f]	592	}
[53f4746]	593	// Add external constraint to Chi2
	594	if (fVtxCst) Chi2 += fCovCstInv.Similarity(x - fxCst);
[537d24f]	595	//
	596	TVectorD dx = x - x0;
	597	x0 = x;
	598	// update vertex stability
[82db145]	599	TMatrixDSym Hess = RegInv(covX);
[537d24f]	600	epsi = Hess.Similarity(dx);
	601	Ntry++;
	602	//
	603	// Store result
	604	//
[53f4746]	605	fXv = x; // Vertex position
[537d24f]	606	fcovXv = covX; // Vertex covariance
	607	fChi2 = Chi2; // Vertex fit Chi2
[53f4746]	608	//std::cout << "Found vertex " << fXv(0) << ", " << fXv(1) << ", " << fXv(2) << std::endl;
[82db145]	609	} // end of iteration loop
[537d24f]	610	//
[82db145]	611	fVtxDone = kTRUE; // Set fit completion flag
	612	fRold = TMath::Sqrt(fXv(0)fXv(0) + fXv(1)fXv(1)); // Store fit radius
[537d24f]	613	//
	614	}
	615	//
[82db145]	616	// Return fit vertex
[537d24f]	617	TVectorD VertexFit::GetVtx()
	618	{
[82db145]	619	if (!fVtxDone) VertexFitter();
[537d24f]	620	return fXv;
	621	}
[82db145]	622	//
	623	// Return fit vertex covariance
[537d24f]	624	TMatrixDSym VertexFit::GetVtxCov()
	625	{
[82db145]	626	if (!fVtxDone) VertexFitter();
[537d24f]	627	return fcovXv;
	628	}
[82db145]	629	//
	630	// Return fit vertex chi2
[537d24f]	631	Double_t VertexFit::GetVtxChi2()
	632	{
[82db145]	633	if (!fVtxDone) VertexFitter();
[537d24f]	634	return fChi2;
	635	}
[82db145]	636	//
	637	// Return array of chi2 contributions from each track
[537d24f]	638	TVectorD VertexFit::GetVtxChi2List()
	639	{
[82db145]	640	if (!fVtxDone) VertexFitter();
[537d24f]	641	return fChi2List;
	642	}
	643	//
	644	// Handle tracks/constraints
	645	void VertexFit::AddVtxConstraint(TVectorD xv, TMatrixDSym cov) // Add gaussian vertex constraint
	646	{
[53f4746]	647	//std::cout << "VertexFit::AddVtxConstraint: Not implemented yet" << std::endl;
	648	fVtxCst = kTRUE; // Vertex constraint flag
	649	fxCst = xv; // Constraint value
	650	fCovCst = cov; // Constraint covariance
	651	fCovCstInv = cov;
	652	fCovCstInv.Invert(); // Its inverse
	653	//
	654	// Set starting vertex as external constraint
	655	fXv = fxCst;
	656	fcovXv = fCovCst;
[537d24f]	657	}
	658	//
[82db145]	659	// Adding tracks one by one
	660	void VertexFit::AddTrk(TVectorD par, TMatrixDSym Cov) // Add track to input list
[537d24f]	661	{
[82db145]	662	fNtr++;
	663	fChi2List.ResizeTo(fNtr); // Resize chi2 array
	664	fPar.push_back(par); // add new track
	665	fCov.push_back(Cov);
[53f4746]	666	fParNew.push_back(par); // add new track
	667	fCovNew.push_back(Cov);
[82db145]	668	//
	669	// Reset previous vertex temp arrays
	670	ResetWrkArrays();
	671	ffi.clear();
	672	fVtxDone = kFALSE; // Reset vertex done flag
[537d24f]	673	}
[82db145]	674	//
	675	// Removing tracks one by one
	676	void VertexFit::RemoveTrk(Int_t iTrk) // Remove iTrk track
[537d24f]	677	{
[82db145]	678	fNtr--;
	679	fChi2List.Clear();
	680	fChi2List.ResizeTo(fNtr); // Resize chi2 array
	681	fPar.erase(fPar.begin() + iTrk); // Remove track
	682	fCov.erase(fCov.begin() + iTrk);
[53f4746]	683	fParNew.erase(fParNew.begin() + iTrk); // Remove track
	684	fCovNew.erase(fCovNew.begin() + iTrk);
[82db145]	685	//
	686	// Reset previous vertex temp arrays
	687	ResetWrkArrays();
	688	ffi.clear();
	689	fVtxDone = kFALSE; // Reset vertex done flag
[537d24f]	690	}

Note: See TracBrowser for help on using the repository browser.

Download in other formats: