Fork me on GitHub

Context Navigation

source: git/external/TrackCovariance/VertexFit.cc@ 08d82fe

Visit:

Last change on this file since 08d82fe was 91ef0b8, checked in by Franco BEDESCHI <bed@…>, 4 years ago
Fixed conflicts and Reginv3
Property mode set to `100644`
File size: 17.0 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	//
	10	// Empty
	11	VertexFit::VertexFit()
	12	{
	13	fNtr = 0;
	14	fVtxDone = kFALSE;
	15	fVtxCst = kFALSE;
	16	fxCst.ResizeTo(3);
[91ef0b8]	17	fCovCst.ResizeTo(3, 3);
[537d24f]	18	fXv.ResizeTo(3);
[91ef0b8]	19	fcovXv.ResizeTo(3, 3);
[537d24f]	20	}
	21	// Parameters and covariances
	22	VertexFit::VertexFit(Int_t Ntr, TVectorD trkPar, TMatrixDSym trkCov)
	23	{
	24	fNtr = Ntr;
	25	fVtxDone = kFALSE;
	26	fVtxCst = kFALSE;
	27	fxCst.ResizeTo(3);
[91ef0b8]	28	fCovCst.ResizeTo(3, 3);
[537d24f]	29	fXv.ResizeTo(3);
[91ef0b8]	30	fcovXv.ResizeTo(3, 3);
[537d24f]	31	//
	32	fPar = trkPar;
	33	fCov = trkCov;
	34	fChi2List.ResizeTo(Ntr);
	35	//
	36	ffi = new Double_t[Ntr]; // Fit phases
[91ef0b8]	37	fx0i = new TVectorD * [Ntr]; // Track expansion points
[537d24f]	38	for (Int_t i = 0; i < Ntr; i++) fx0i[i] = new TVectorD(3);
	39	fai = new TVectorD * [Ntr]; // dx/dphi
	40	for (Int_t i = 0; i < Ntr; i++) fai[i] = new TVectorD(3);
	41	fa2i = new Double_t[Ntr]; // a'Wa
[91ef0b8]	42	fDi = new TMatrixDSym * [Ntr]; // W-WBW
[537d24f]	43	for (Int_t i = 0; i < Ntr; i++) fDi[i] = new TMatrixDSym(3);
	44	fWi = new TMatrixDSym * [Ntr]; // (ACA')^-1
	45	for (Int_t i = 0; i < Ntr; i++) fWi[i] = new TMatrixDSym(3);
	46	fWinvi = new TMatrixDSym * [Ntr]; // ACA'
	47	for (Int_t i = 0; i < Ntr; i++) fWinvi[i] = new TMatrixDSym(3);
	48	}
	49	// ObsTrk list
	50	VertexFit::VertexFit(Int_t Ntr, ObsTrk** track)
	51	{
	52	fNtr = Ntr;
	53	fVtxDone = kFALSE;
	54	fVtxCst = kFALSE;
	55	fxCst.ResizeTo(3);
[91ef0b8]	56	fCovCst.ResizeTo(3, 3);
[537d24f]	57	fXv.ResizeTo(3);
[91ef0b8]	58	fcovXv.ResizeTo(3, 3);
[537d24f]	59	//
[91ef0b8]	60	fPar = new TVectorD * [Ntr];
	61	fCov = new TMatrixDSym * [Ntr];
[537d24f]	62	fChi2List.ResizeTo(Ntr);
	63	for (Int_t i = 0; i < Ntr; i++)
	64	{
	65	fPar[i] = new TVectorD(track[i]->GetObsPar());
	66	fCov[i] = new TMatrixDSym(track[i]->GetCov());
	67	}
	68	//
	69	ffi = new Double_t[Ntr]; // Fit phases
	70	fx0i = new TVectorD * [Ntr]; // Track expansion points
	71	for (Int_t i = 0; i < Ntr; i++) fx0i[i] = new TVectorD(3);
	72	fai = new TVectorD * [Ntr]; // dx/dphi
	73	for (Int_t i = 0; i < Ntr; i++) fai[i] = new TVectorD(3);
	74	fa2i = new Double_t[Ntr]; // a'Wa
	75	fDi = new TMatrixDSym * [Ntr]; // W-WBW
	76	for (Int_t i = 0; i < Ntr; i++) fDi[i] = new TMatrixDSym(3);
	77	fWi = new TMatrixDSym * [Ntr]; // (ACA')^-1
	78	for (Int_t i = 0; i < Ntr; i++) fWi[i] = new TMatrixDSym(3);
	79	fWinvi = new TMatrixDSym * [Ntr]; // ACA'
	80	for (Int_t i = 0; i < Ntr; i++) fWinvi[i] = new TMatrixDSym(3);
	81	}
	82	//
	83	// Destructor
	84	VertexFit::~VertexFit()
	85	{
[127644a]	86	fxCst.Clear();
[537d24f]	87	fCovCst.Clear();
[127644a]	88	fXv.Clear();
	89	fcovXv.Clear();
[537d24f]	90	fChi2List.Clear();
	91	//
[91ef0b8]	92	for (Int_t i = 0; i < fNtr; i++)
[537d24f]	93	{
[127644a]	94	fPar[i]->Clear();
[537d24f]	95	fCov[i]->Clear();
	96	//
	97	fx0i[i]->Clear(); delete fx0i[i];
	98	fai[i]->Clear(); delete fai[i];
	99	fDi[i]->Clear(); delete fDi[i];
	100	fWi[i]->Clear(); delete fWi[i];
	101	fWinvi[i]->Clear(); delete fWinvi[i];
	102	}
	103	fNtr = 0;
	104	delete[] fPar;
	105	delete[] fCov;
	106	delete[] ffi;
	107	delete[] fa2i;
	108	delete[] fx0i;
	109	delete[] fai;
	110	delete[] fDi;
	111	delete[] fWi;
	112	delete[] fWinvi;
	113	}
	114	//
	115	Double_t VertexFit::FastRv1(TVectorD p1, TVectorD p2)
	116	{
	117	//
	118	// Find radius of intersection between two tracks in the transverse plane
	119	//
	120	// p = (D,phi, C, z0, ct)
	121	//
	122	// Define arrays
	123	//
	124	Double_t r1 = 1.0 / p1(2);
	125	Double_t r2 = 1.0 / p2(2);
	126	TVectorD x0 = Fill_x0(p1);
	127	TVectorD y0 = Fill_x0(p2);
	128	TVectorD n = Fill_a(p1, 0.0);
	129	n *= r1;
	130	TVectorD k = Fill_a(p2, 0.0);
	131	k *= r2;
	132	//
	133	// Setup and solve linear system
	134	//
	135	Double_t nn = 0; for (Int_t i = 0; i < 3; i++)nn += n(i) * n(i);
	136	Double_t nk = 0; for (Int_t i = 0; i < 3; i++)nk += n(i) * k(i);
	137	Double_t kk = 0; for (Int_t i = 0; i < 3; i++)kk += k(i) * k(i);
	138	Double_t discr = nn * kk - nk * nk;
	139	TMatrixDSym H(2);
	140	H(0, 0) = kk;
	141	H(0, 1) = nk;
	142	H(1, 0) = H(0, 1);
	143	H(1, 1) = nn;
	144	TVectorD c(2);
[91ef0b8]	145	c(0) = 0; for (Int_t i = 0; i < 3; i++)c(0) += n(i) * (y0(i) - x0(i));
[537d24f]	146	c(1) = 0; for (Int_t i = 0; i < 3; i++)c(1) += -k(i) * (y0(i) - x0(i));
	147	TVectorD smin = (H * c);
	148	smin *= 1.0 / discr;
	149	//
	150	TVectorD X = x0 + smin(0) * n;
	151	TVectorD Y = y0 + smin(1) * k;
	152	Double_t R1 = TMath::Sqrt(X(0) * X(0) + X(1) * X(1));
	153	Double_t R2 = TMath::Sqrt(Y(0) * Y(0) + Y(1) * Y(1));
	154	//
[91ef0b8]	155	return 0.5 * (R1 + R2);
[537d24f]	156	}
	157	Double_t VertexFit::FastRv(TVectorD p1, TVectorD p2)
	158	{
	159	//
	160	// Find radius of minimum distance
	161	//
	162	// p = (D,phi, C)
	163	//
	164	// Solving matrix
	165	TMatrixDSym H(2);
	166	H(0, 0) = -TMath::Cos(p2(1));
[91ef0b8]	167	H(0, 1) = TMath::Cos(p1(1));
[537d24f]	168	H(1, 0) = -TMath::Sin(p2(1));
[91ef0b8]	169	H(1, 1) = TMath::Sin(p1(1));
[537d24f]	170	Double_t Det = TMath::Sin(p2(1) - p1(1));
	171	H *= 1.0 / Det;
	172	//
	173	// Convergence parameters
	174	Int_t Ntry = 0;
	175	Int_t NtryMax = 100;
	176	Double_t eps = 1000.;
	177	Double_t epsMin = 1.0e-6;
	178	//
	179	// Vertex finding loop
	180	//
	181	TVectorD cterm(2);
	182	cterm(0) = p1(0);
	183	cterm(1) = p2(0);
	184	TVectorD xv(2);
	185	Double_t R = 1000.;
	186	while (eps > epsMin)
	187	{
	188	xv = H * cterm;
	189	Ntry++;
	190	if (Ntry > NtryMax)
	191	{
[127644a]	192	std::cout << "FastRv: maximum number of iteration reached" << std::endl;
[537d24f]	193	break;
	194	}
	195	Double_t Rnew = TMath::Sqrt(xv(0) * xv(0) + xv(1) * xv(1));
	196	eps = Rnew - R;
	197	R = Rnew;
	198	cterm(0) = p1(2) * R * R;
	199	cterm(1) = p2(2) * R * R;
	200	}
	201	//
	202	return R;
	203	}
	204
[91ef0b8]	205	TMatrixDSym VertexFit::RegInv3(TMatrixDSym& Smat0)
[537d24f]	206	{
	207	//
	208	// Regularized inversion of symmetric 3x3 matrix with positive diagonal elements
	209	//
	210	TMatrixDSym Smat = Smat0;
	211	Int_t N = Smat.GetNrows();
	212	if (N != 3)
	213	{
[127644a]	214	std::cout << "RegInv3 called with matrix size != 3. Abort & return standard inversion." << std::endl;
[537d24f]	215	return Smat.Invert();
	216	}
	217	TMatrixDSym D(N); D.Zero();
	218	Bool_t dZero = kTRUE; // No elements less or equal 0 on the diagonal
	219	for (Int_t i = 0; i < N; i++) if (Smat(i, i) <= 0.0)dZero = kFALSE;
	220	if (dZero)
	221	{
	222	for (Int_t i = 0; i < N; i++) D(i, i) = 1.0 / TMath::Sqrt(Smat(i, i));
	223	TMatrixDSym RegMat = Smat.Similarity(D);
	224	TMatrixDSym Q(2);
	225	for (Int_t i = 0; i < 2; i++)
	226	{
	227	for (Int_t j = 0; j < 2; j++)Q(i, j) = RegMat(i, j);
	228	}
[91ef0b8]	229	Double_t Det = 1 - Q(0, 1) * Q(1, 0);
[537d24f]	230	TMatrixDSym H(2);
	231	H = Q;
	232	H(0, 1) = -Q(0, 1);
	233	H(1, 0) = -Q(1, 0);
	234	TVectorD p(2);
	235	p(0) = RegMat(0, 2);
	236	p(1) = RegMat(1, 2);
	237	Double_t pHp = H.Similarity(p);
[91ef0b8]	238	Double_t h = pHp - Det;
[537d24f]	239	//
	240	TMatrixDSym pp(2); pp.Rank1Update(p);
[91ef0b8]	241	TMatrixDSym F = (h * H) - pp.Similarity(H);
[537d24f]	242	F *= 1.0 / Det;
[91ef0b8]	243	TVectorD b = H * p;
[537d24f]	244	TMatrixDSym InvReg(3);
	245	for (Int_t i = 0; i < 2; i++)
	246	{
	247	InvReg(i, 2) = b(i);
	248	InvReg(2, i) = b(i);
	249	for (Int_t j = 0; j < 2; j++) InvReg(i, j) = F(i, j);
	250	}
	251	InvReg(2, 2) = -Det;
	252	//
	253	InvReg *= 1.0 / h;
	254	//
	255	//
	256	return InvReg.Similarity(D);
	257	}
	258	else
	259	{
[91ef0b8]	260	D.Zero();
	261	for (Int_t i = 0; i < N; i++) D(i, i) = 1.0 / TMath::Sqrt(TMath::Abs(Smat(i, i)));
	262	TMatrixDSym RegMat = Smat.Similarity(D);
	263	RegMat.Invert();
	264	return RegMat.Similarity(D);
[537d24f]	265	}
	266	}
	267	//
	268	//
	269	//
	270	TMatrixD VertexFit::Fill_A(TVectorD par, Double_t phi)
	271	{
	272	//
	273	// Derivative of track 3D position vector with respect to track parameters at constant phase
	274	//
	275	// par = vector of track parameters
	276	// phi = phase
	277	//
	278	TMatrixD A(3, 5);
	279	//
	280	// Decode input arrays
	281	//
	282	Double_t D = par(0);
	283	Double_t p0 = par(1);
	284	Double_t C = par(2);
	285	Double_t z0 = par(3);
	286	Double_t ct = par(4);
	287	//
	288	// Fill derivative matrix dx/d alpha
	289	// D
	290	A(0, 0) = -TMath::Sin(p0);
	291	A(1, 0) = TMath::Cos(p0);
	292	A(2, 0) = 0.0;
	293	// phi0
[91ef0b8]	294	A(0, 1) = -D * TMath::Cos(p0) + (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C);
	295	A(1, 1) = -D * TMath::Sin(p0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
[537d24f]	296	A(2, 1) = 0.0;
	297	// C
[91ef0b8]	298	A(0, 2) = -(TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C * C);
	299	A(1, 2) = (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C * C);
	300	A(2, 2) = -ct * phi / (2 * C * C);
[537d24f]	301	// z0
	302	A(0, 3) = 0.0;
	303	A(1, 3) = 0.0;
	304	A(2, 3) = 1.0;
	305	// ct = lambda
	306	A(0, 4) = 0.0;
	307	A(1, 4) = 0.0;
	308	A(2, 4) = phi / (2 * C);
	309	//
	310	return A;
	311	}
	312	//
	313	TVectorD VertexFit::Fill_a(TVectorD par, Double_t phi)
	314	{
	315	//
	316	// Derivative of track 3D position vector with respect to phase at constant track parameters
	317	//
	318	// par = vector of track parameters
	319	// phi = phase
	320	//
	321	TVectorD a(3);
	322	//
	323	// Decode input arrays
	324	//
	325	Double_t D = par(0);
	326	Double_t p0 = par(1);
	327	Double_t C = par(2);
	328	Double_t z0 = par(3);
	329	Double_t ct = par(4);
	330	//
	331	a(0) = TMath::Cos(phi + p0) / (2 * C);
	332	a(1) = TMath::Sin(phi + p0) / (2 * C);
	333	a(2) = ct / (2 * C);
	334	//
	335	return a;
	336	}
	337	//
	338	TVectorD VertexFit::Fill_x0(TVectorD par)
	339	{
	340	//
	341	// Calculate track 3D position at R = \|D\| (minimum approach to z-axis)
	342	//
	343	TVectorD x0(3);
	344	//
	345	// Decode input arrays
	346	//
	347	Double_t D = par(0);
	348	Double_t p0 = par(1);
	349	Double_t C = par(2);
	350	Double_t z0 = par(3);
	351	Double_t ct = par(4);
	352	//
[91ef0b8]	353	x0(0) = -D * TMath::Sin(p0);
	354	x0(1) = D * TMath::Cos(p0);
[537d24f]	355	x0(2) = z0;
	356	//
	357	return x0;
	358	}
	359	//
	360	TVectorD VertexFit::Fill_x(TVectorD par, Double_t phi)
	361	{
	362	//
	363	// Calculate track 3D position for a given phase, phi
	364	//
	365	TVectorD x(3);
	366	//
	367	// Decode input arrays
	368	//
	369	Double_t D = par(0);
	370	Double_t p0 = par(1);
	371	Double_t C = par(2);
	372	Double_t z0 = par(3);
	373	Double_t ct = par(4);
	374	//
	375	TVectorD x0 = Fill_x0(par);
	376	x(0) = x0(0) + (TMath::Sin(phi + p0) - TMath::Sin(p0)) / (2 * C);
	377	x(1) = x0(1) - (TMath::Cos(phi + p0) - TMath::Cos(p0)) / (2 * C);
[91ef0b8]	378	x(2) = x0(2) + ct * phi / (2 * C);
[537d24f]	379	//
	380	return x;
	381	}
	382	//
	383	void VertexFit::VertexFinder()
	384	{
	385	//
	386	// Vertex fit (units are meters)
	387	//
	388	// Initial variable definitions
	389	TVectorD x(3);
	390	TMatrixDSym covX(3);
	391	TVectorD x0(3); for (Int_t v = 0; v < 3; v++)x0(v) = 100.; // set to large value
	392	Double_t Chi2 = 0;
	393	//
	394	// Stored quantities
[91ef0b8]	395	Double_t* fi = new Double_t[fNtr]; // Phases
	396	TVectorD** x0i = new TVectorD * [fNtr]; // Track expansion point
	397	TVectorD** ai = new TVectorD * [fNtr]; // dx/dphi
	398	Double_t* a2i = new Double_t[fNtr]; // a'Wa
	399	TMatrixDSym** Di = new TMatrixDSym * [fNtr]; // W-WBW
	400	TMatrixDSym** Wi = new TMatrixDSym * [fNtr]; // (ACA')^-1
	401	TMatrixDSym** Winvi = new TMatrixDSym * [fNtr]; // ACA'
[537d24f]	402	//
	403	// vertex radius approximation
	404	// Maximum impact parameter
	405	Double_t Rd = 0;
	406	for (Int_t i = 0; i < fNtr; i++)
	407	{
	408	//ObsTrk* t = tracks[i];
	409	TVectorD par = *fPar[i];
	410	Double_t Dabs = TMath::Abs(par(0));
	411	if (Dabs > Rd)Rd = Dabs;
	412	}
	413	//
	414	// Find track pair with largest phi difference
[91ef0b8]	415	Int_t isel = 0; Int_t jsel = 0; // selected track indices
[127644a]	416	Double_t dphiMax = 0.0; // Max phi difference
[91ef0b8]	417
	418	for (Int_t i = 0; i < fNtr - 1; i++)
[537d24f]	419	{
	420	//ObsTrk* ti = tracks[i];
	421	TVectorD pari = *fPar[i];
	422	Double_t phi1 = pari(1);
	423
[91ef0b8]	424	for (Int_t j = i + 1; j < fNtr; j++)
[537d24f]	425	{
	426	//ObsTrk* tj = tracks[j];
	427	TVectorD parj = *fPar[j];
	428	Double_t phi2 = parj(1);
	429	Double_t dphi = TMath::Abs(phi2 - phi1);
	430	if (dphi > TMath::Pi())dphi = TMath::TwoPi() - dphi;
	431	if (dphi > dphiMax)
	432	{
	433	isel = i; jsel = j;
	434	dphiMax = dphi;
	435	}
	436	}
	437	}
	438	//
	439	TVectorD p1 = *fPar[isel];
	440	TVectorD p2 = *fPar[jsel];
	441	Double_t R = FastRv1(p1, p2);
[91ef0b8]	442	if (R > 1000.0) R = Rd;
	443	R = 0.9 * R + 0.1 * Rd;
[537d24f]	444	//
	445	// Iteration properties
	446	//
	447	Int_t Ntry = 0;
	448	Int_t TryMax = 100;
	449	Double_t eps = 1.0e-9; // vertex stability
	450	Double_t epsi = 1000.;
	451	//
	452	while (epsi > eps && Ntry < TryMax) // Iterate until found vertex is stable
	453	{
	454	x.Zero();
	455	TVectorD cterm(3); TMatrixDSym H(3); TMatrixDSym DW1D(3);
	456	covX.Zero(); // Reset vertex covariance
	457	cterm.Zero(); // Reset constant term
	458	H.Zero(); // Reset H matrix
	459	DW1D.Zero();
[127644a]	460	//
	461	//std::cout << "VertexFinder: start loop on tracks" << std::endl;
[537d24f]	462	for (Int_t i = 0; i < fNtr; i++)
	463	{
[127644a]	464	// Get track helix parameters and their covariance matrix
[537d24f]	465	TVectorD par = *fPar[i];
[127644a]	466	TMatrixDSym Cov = *fCov[i];
[91ef0b8]	467
[537d24f]	468	Double_t fs;
	469	if (Ntry <= 0) // Initialize all phases on first pass
	470	{
	471	Double_t D = par(0);
	472	Double_t C = par(2);
[91ef0b8]	473	Double_t arg = TMath::Max(1.0e-6, (R * R - D * D) / (1 + 2 * C * D));
	474	fs = 2 * TMath::ASin(C * TMath::Sqrt(arg));
[537d24f]	475	fi[i] = fs;
	476	}
	477	//
	478	// Starting values
	479	//
[91ef0b8]	480	fs = fi[i]; // Get phase
[127644a]	481	//std::cout << "VertexFinder: phase fs set" << std::endl;
[537d24f]	482	TVectorD xs = Fill_x(par, fs);
[127644a]	483	//std::cout << "VertexFinder: position xs set" << std::endl;
[537d24f]	484	x0i[i] = new TVectorD(xs); // Start helix position
[127644a]	485	//std::cout << "VertexFinder: position x0i stored" << std::endl;
[537d24f]	486	// W matrix = (ACA')^-1; W^-1 = ACA'
	487	TMatrixD A = Fill_A(par, fs); // A = dx/da = derivatives wrt track parameters
[127644a]	488	//std::cout << "VertexFinder: derivatives A set" << std::endl;
[537d24f]	489	TMatrixDSym Winv = Cov.Similarity(A); // W^-1 = ACA'
	490	Winvi[i] = new TMatrixDSym(Winv); // Store W^-1 matrix
[127644a]	491	//std::cout << "VertexFinder: Winvi stored" << std::endl;
[537d24f]	492	TMatrixDSym W = RegInv3(Winv); // W = (ACA')^-1
	493	Wi[i] = new TMatrixDSym(W); // Store W matrix
[127644a]	494	//std::cout << "VertexFinder: Wi stored" << std::endl;
[537d24f]	495	TVectorD a = Fill_a(par, fs); // a = dx/ds = derivatives wrt phase
[127644a]	496	//std::cout << "VertexFinder: derivatives a set" << std::endl;
[537d24f]	497	ai[i] = new TVectorD(a); // Store a
[127644a]	498	//std::cout << "VertexFinder: derivatives a stored" << std::endl;
[537d24f]	499	Double_t a2 = W.Similarity(a);
	500	a2i[i] = a2; // Store a2
	501	// Build D matrix
[127644a]	502	TMatrixDSym B(3);
[91ef0b8]	503
[537d24f]	504	B.Rank1Update(a, 1.0);
	505	B *= -1. / a2;
	506	B.Similarity(W);
[91ef0b8]	507	TMatrixDSym Ds = W + B; // D matrix
[537d24f]	508	Di[i] = new TMatrixDSym(Ds); // Store D matrix
[127644a]	509	//std::cout << "VertexFinder: matrix Di stored" << std::endl;
[537d24f]	510	TMatrixDSym DsW1Ds = Winv.Similarity(Ds); // Service matrix to calculate covX
[127644a]	511	DW1D += DsW1Ds;
[537d24f]	512	// Update hessian
	513	H += Ds;
	514	// update constant term
	515	cterm += Ds * xs;
	516	} // End loop on tracks
	517	//
	518	// update vertex position
	519	TMatrixDSym H1 = RegInv3(H);
[91ef0b8]	520	x = H1 * cterm;
[127644a]	521	//std::cout << "VertexFinder: x vertex set" << std::endl;
[537d24f]	522	// Update vertex covariance
	523	covX = DW1D.Similarity(H1);
[127644a]	524	//std::cout << "VertexFinder: cov vertex set" << std::endl;
[537d24f]	525	// Update phases and chi^2
	526	Chi2 = 0.0;
	527	for (Int_t i = 0; i < fNtr; i++)
	528	{
[91ef0b8]	529	TVectorD lambda = (Di[i]) (*x0i[i] - x);
[537d24f]	530	TMatrixDSym Wm1 = *Winvi[i];
	531	fChi2List(i) = Wm1.Similarity(lambda);
	532	Chi2 += fChi2List(i);
	533	TVectorD a = *ai[i];
[91ef0b8]	534	TVectorD b = (Wi[i]) (x - *x0i[i]);
	535	for (Int_t j = 0; j < 3; j++)fi[i] += a(j) * b(j) / a2i[i];
[537d24f]	536	}
	537
	538	//
	539	TVectorD dx = x - x0;
	540	x0 = x;
	541	// update vertex stability
	542	TMatrixDSym Hess = RegInv3(covX);
	543	epsi = Hess.Similarity(dx);
	544	Ntry++;
	545	//
	546	// Store result
	547	//
	548	fXv = x; // Vertex position
	549	fcovXv = covX; // Vertex covariance
	550	fChi2 = Chi2; // Vertex fit Chi2
	551	//
	552	// Store intermediate data
	553	//
	554
[127644a]	555	//std::cout << "VertexFinder: before store intermediate data" << std::endl;
[537d24f]	556	for (Int_t i = 0; i < fNtr; i++)
	557	{
[127644a]	558	//std::cout << "VertexFinder: inside store intermediate data" << std::endl;
	559	//std::cout << "i = " << i << ", fi[i] = " << fi[i] << std::endl;
	560	//std::cout << "i = " << i << ", ffi[i] = " << ffi[i] << std::endl;
[537d24f]	561	ffi[i] = fi[i]; // Fit phases
[127644a]	562	//std::cout << "VertexFinder: fi stored" << std::endl;
[537d24f]	563	fx0i[i] = x0i[i]; // Track expansion points
[127644a]	564	//std::cout << "VertexFinder: x0i stored" << std::endl;
[537d24f]	565	fai[i] = ai[i]; // dx/dphi
[127644a]	566	//std::cout << "VertexFinder: ai stored" << std::endl;
[537d24f]	567	fa2i[i] = a2i[i]; // a'Wa
[127644a]	568	//std::cout << "VertexFinder: a2i stored" << std::endl;
[537d24f]	569	fDi[i] = Di[i]; // W-WBW
[127644a]	570	//std::cout << "VertexFinder: Di stored" << std::endl;
[537d24f]	571	fWi[i] = Wi[i]; // (ACA')^-1
[127644a]	572	//std::cout << "VertexFinder: Wi stored" << std::endl;
[537d24f]	573	fWinvi[i] = Winvi[i]; // ACA'
[127644a]	574	//std::cout << "VertexFinder: Winvi stored" << std::endl;
[537d24f]	575	}
[127644a]	576	//std::cout << "Iteration " << Ntry << " completed - Before cleanup" << std::endl;
[537d24f]	577	//
	578	// Cleanup
	579	//
	580	for (Int_t i = 0; i < fNtr; i++)
	581	{
	582	x0i[i]->Clear();
	583	Winvi[i]->Clear();
	584	Wi[i]->Clear();
	585	ai[i]->Clear();
	586	Di[i]->Clear();
	587
	588	delete x0i[i];
	589	delete Winvi[i];
	590	delete Wi[i];
	591	delete ai[i];
	592	delete Di[i];
	593	}
	594
[127644a]	595	//std::cout << "Iteration " << Ntry << " completed - After cleanup" << std::endl;
[537d24f]	596	}
	597	//
	598	fVtxDone = kTRUE; // Set fitting completion flag
	599	//
[127644a]	600	delete[] fi; // Phases
[537d24f]	601	delete[] x0i; // Track expansion point
	602	delete[] ai; // dx/dphi
	603	delete[] a2i; // a'Wa
	604	delete[] Di; // W-WBW
	605	delete[] Wi; // (ACA')^-1
	606	delete[] Winvi; // ACA'
	607	}
	608	//
	609	TVectorD VertexFit::GetVtx()
	610	{
[127644a]	611	//std::cout << "GetVtx: flag set to " << fVtxDone << std::endl;
[537d24f]	612	if (!fVtxDone)VertexFinder();
	613	return fXv;
	614	}
	615
	616	TMatrixDSym VertexFit::GetVtxCov()
	617	{
	618	if (!fVtxDone)VertexFinder();
	619	return fcovXv;
	620	}
	621
	622	Double_t VertexFit::GetVtxChi2()
	623	{
	624	if (!fVtxDone)VertexFinder();
	625	return fChi2;
	626	}
	627
	628	TVectorD VertexFit::GetVtxChi2List()
	629	{
	630	if (!fVtxDone)VertexFinder();
	631	return fChi2List;
	632	}
	633	//
	634	// Handle tracks/constraints
	635	void VertexFit::AddVtxConstraint(TVectorD xv, TMatrixDSym cov) // Add gaussian vertex constraint
	636	{
[127644a]	637	std::cout << "VertexFit::AddVtxConstraint: Not implemented yet" << std::endl;
[537d24f]	638	}
	639	//
	640	void VertexFit::AddTrk(TVectorD par, TMatrixDSym Cov) // Add track to input list
	641	{
[127644a]	642	std::cout << "VertexFit::AddTrk: Not implemented yet" << std::endl;
[537d24f]	643	}
	644	void VertexFit::RemoveTrk(Int_t iTrk) // Remove iTrk track
	645	{
[127644a]	646	std::cout << "VertexFit::RemoveTrk: Not implemented yet" << std::endl;
[537d24f]	647	}

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

Download in other formats: