hpstr
The Heavy Photon Search Toolkit for Reconstruction (hpstr) provides an interface to physics data from the HPS experiment saved in the LCIO format and converts it into an ROOT based format.
Loading...
Searching...
No Matches
TrackTools.cxx
Go to the documentation of this file.
1#include "TrackTools.h"
2#include <iostream>
3#include <cmath>
4
5
6float TrackTools::normalizeAngle(float angle) {
7 const float PI = M_PI;
8 while (angle > PI) angle -= 2.0 * PI;
9 while (angle < -PI) angle += 2.0 * PI;
10 return angle;
11}
12
13
15 TMatrixD cov(5, 5);
16 std::vector<float> covArray = track->getCov();
17
18 if (covArray.size() < 15) {
19 std::cerr << "WARNING: Track covariance array size = " << covArray.size()
20 << ", expected 15" << std::endl;
21 return cov;
22 }
23
24 int idx = 0;
25 for (int j = 0; j < 5; j++) {
26 for (int i = 0; i <= j; i++) {
27 cov(i, j) = covArray[idx];
28 cov(j, i) = covArray[idx];
29 idx++;
30 }
31 }
32 return cov;
33}
34
35
37 TMatrixD cov(3, 3);
38
39 // Vertex covariance matrix
40 std::vector<float> covArray = vtx->getCovariance();
41
42 if (covArray.size() < 6) {
43 std::cerr << "WARNING: Vertex covariance array size = " << covArray.size()
44 << ", expected 6" << std::endl;
45 return cov;
46 }
47
48 cov(0, 0) = covArray[0]; // Var(x)
49 cov(0, 1) = covArray[1]; // Cov(x,y)
50 cov(1, 0) = covArray[1];
51 cov(1, 1) = covArray[2]; // Var(y)
52 cov(0, 2) = covArray[3]; // Cov(x,z)
53 cov(2, 0) = covArray[3];
54 cov(1, 2) = covArray[4]; // Cov(y,z)
55 cov(2, 1) = covArray[4];
56 cov(2, 2) = covArray[5]; // Var(z)
57
58 return cov;
59}
60
61
63 double px, double py, double pz,
64 double phi0, double omega, double tanLambda,
65 const TMatrixD& covTrack) {
66
67 // Calculate pT from momentum components
68 double pT = std::sqrt(px*px + pz*pz);
69 double pT2 = pT * pT;
70 double pT3 = pT * pT2;
71
72 // We need to infer bfield*MOM_PARAM from the relationship:
73 // pt = |1/omega| * bfield * MOM_PARAM
74 double bfield_times_mom = pT * std::fabs(omega);
75
76 double sign_omega = (omega > 0) ? 1.0 : -1.0;
77
78 double sin_phi0 = std::sin(phi0);
79 double cos_phi0 = std::cos(phi0);
80
81 // Derivatives
82 double dpt_domega = -sign_omega / (omega * omega) * bfield_times_mom;
83
84 // Jacobian matrix: d[px, py, pz]/d[d0, phi0, omega, z0, tanLambda]
85 TMatrixD J(3, 5);
86
87 // Row 0: px = pT * sin(phi0)
88 J(0, 0) = 0.0; // d(px)/d(d0)
89 J(0, 1) = pT * cos_phi0; // d(px)/d(phi0)
90 J(0, 2) = dpt_domega * sin_phi0; // d(px)/d(omega)
91 J(0, 3) = 0.0; // d(px)/d(z0)
92 J(0, 4) = 0.0; // d(px)/d(tanLambda)
93
94 // Row 1: py = pT * tanLambda
95 J(1, 0) = 0.0; // d(py)/d(d0)
96 J(1, 1) = 0.0; // d(py)/d(phi0)
97 J(1, 2) = dpt_domega * tanLambda; // d(py)/d(omega)
98 J(1, 3) = 0.0; // d(py)/d(z0)
99 J(1, 4) = pT; // d(py)/d(tanLambda)
100
101 // Row 2: pz = pT * cos(phi0)
102 J(2, 0) = 0.0; // d(pz)/d(d0)
103 J(2, 1) = -pT * sin_phi0; // d(pz)/d(phi0)
104 J(2, 2) = dpt_domega * cos_phi0; // d(pz)/d(omega)
105 J(2, 3) = 0.0; // d(pz)/d(z0)
106 J(2, 4) = 0.0; // d(pz)/d(tanLambda)
107
108 // Propagate covariance: Cov_p = J * Cov_track * J^T
109 TMatrixD J_T(TMatrixD::kTransposed, J);
110 TMatrixD temp = J * covTrack;
111 TMatrixD covMomentum = temp * J_T;
112
113 return covMomentum;
114}
115
117 TMatrixD covTrack = getTrackCovariance(track);
118
120 track->getMomentum()[0],
121 track->getMomentum()[1],
122 track->getMomentum()[2],
123 track->getPhi(),
124 track->getOmega(),
125 track->getTanLambda(),
126 covTrack);
127}
128
129
131 float x_vtx, float y_vtx, float z_vtx,
132 float px, float py, float pz, float Q, float cB,
133 float targetPos) {
134
135 // Transverse momentum
136 float pT = std::sqrt(px*px + pz*pz);
137 float omega = -Q * cB / pT;
138
139 // Basic track parameters
140 float tan_lambda = py / pT;
141 float phi0 = std::atan2(px, pz);
142
143 // Transform to helix convention
144 float tan_L_helix = -tan_lambda;
145 float phi0_helix = -phi0;
146 float alpha_over_kappa = -1.0 / omega;
147
148 // Helix center in transverse (x-z) plane
149 float cos_phi0_h = std::cos(phi0_helix);
150 float sin_phi0_h = std::sin(phi0_helix);
151
152 float x_center = x_vtx + alpha_over_kappa * cos_phi0_h;
153 float z_center = z_vtx + alpha_over_kappa * sin_phi0_h;
154
155
156 // Angle to pivot point
157 float phi0_new_helix = std::atan2(targetPos - z_center, -x_center);
158 float cos_phi_new = std::cos(phi0_new_helix);
159 float sin_phi_new = std::sin(phi0_new_helix);
160
161 // Impact parameter d0
162 float d0_helix = (x_center * cos_phi_new +
163 (z_center - targetPos) * sin_phi_new -
164 alpha_over_kappa);
165
166 // z0 calculation
167 float phi_diff = phi0_new_helix - phi0_helix;
168 float dz_helix = -y_vtx - alpha_over_kappa * phi_diff * tan_L_helix;
169
170 // Transform back
171 std::vector<float> params;
172 params.push_back(d0_helix); // d0
173 params.push_back(-phi0_new_helix); // phi0
174 params.push_back(omega); // omega
175 params.push_back(-dz_helix); // z0
176 params.push_back(tan_lambda); // tan_lambda
177
178 // Normalize phi0
179 params[1] = normalizeAngle(params[1]);
180
181 return params;
182}
183
184
186 float x_vtx, float y_vtx, float z_vtx,
187 float px, float py, float pz,
188 float Q, float cB,
189 float targetPos) {
190
191 // ---- Intermediate quantities ----
192 float pT2 = px*px + pz*pz;
193 float pT = std::sqrt(pT2);
194 float pT3 = pT * pT2;
195
196 float omega = -Q * cB / pT;
197
198 float tan_lambda = py / pT;
199 float phi0 = std::atan2(px, pz);
200
201 // Helix frame
202 float tan_L_helix = -tan_lambda;
203 float phi0_helix = -phi0;
204 float alpha_over_kappa = -1.0 / omega;
205
206 float cos_phi0_h = std::cos(phi0_helix);
207 float sin_phi0_h = std::sin(phi0_helix);
208
209 // Center position
210 float x_center = x_vtx + alpha_over_kappa * cos_phi0_h;
211 float z_center = z_vtx + alpha_over_kappa * sin_phi0_h;
212
213 // To pivot
214 double dx_c = -x_center;
215 double dz_c = targetPos - z_center;
216 float r_c2 = dx_c*dx_c + dz_c*dz_c;
217
218 float phi0_new_helix = std::atan2(dz_c, dx_c);
219 float cos_phi_new = std::cos(phi0_new_helix);
220 float sin_phi_new = std::sin(phi0_new_helix);
221
222 float phi_diff = phi0_new_helix - phi0_helix;
223
224 // ---- Derivatives of basic quantities ----
225
226 // d(pT)/d(px), d(pT)/d(pz)
227 float dpT_dpx = px / pT;
228 float dpT_dpz = pz / pT;
229
230 // d(phi0)/d(px), d(phi0)/d(pz)
231 // phi0 = atan2(px, pz)
232 float dphi0_dpx = pz / pT2;
233 float dphi0_dpz = -px / pT2;
234
235 // d(phi0_helix)/d(px), d(phi0_helix)/d(pz)
236 float dphi0h_dpx = -dphi0_dpx;
237 float dphi0h_dpz = -dphi0_dpz;
238
239 // d(tan_lambda)/d(px), d(tan_lambda)/d(py), d(tan_lambda)/d(pz)
240 float dtanL_dpx = -py * px / pT3;
241 float dtanL_dpy = 1.0 / pT;
242 float dtanL_dpz = -py * pz / pT3;
243
244 //d(omega)/d(pT)
245 float domega_dpT = Q * cB / pT2;
246
247 //d(alpha_over_kappa)/d(px)
248 float dalpha_over_kappa_dpx = (1 / (omega*omega)) * domega_dpT * dpT_dpx;
249 float dalpha_over_kappa_dpy = 0;
250 float dalpha_over_kappa_dpz = (1 / (omega*omega)) * domega_dpT * dpT_dpz;
251
252
253
254 // d(x_center)/d(x), d(x_center)/d(px), etc.
255 float dxc_dx = 1.0;
256 float dxc_dpx = -alpha_over_kappa * sin_phi0_h * dphi0h_dpx + cos_phi0_h * dalpha_over_kappa_dpx;
257 float dxc_dpz = -alpha_over_kappa * sin_phi0_h * dphi0h_dpz + cos_phi0_h * dalpha_over_kappa_dpz;
258
259 // d(z_center)/d(z), d(z_center)/d(px), etc.
260 float dzc_dz = 1.0;
261 float dzc_dpx = alpha_over_kappa * cos_phi0_h * dphi0h_dpx + sin_phi0_h * dalpha_over_kappa_dpx;
262 float dzc_dpz = alpha_over_kappa * cos_phi0_h * dphi0h_dpz + sin_phi0_h * dalpha_over_kappa_dpz;
263
264 // d(phi0_new_helix)/d(x_center), d(phi0_new_helix)/d(z_center)
265 float dphi0new_dxc = dz_c / r_c2;
266 float dphi0new_dzc = -dx_c / r_c2;
267
268 // d(phi0_new_helix)/d(x), d(phi0_new_helix)/d(z), etc.
269 float dphi0new_dx = dphi0new_dxc * (dxc_dx);
270 float dphi0new_dz = dphi0new_dzc * (dzc_dz);
271 float dphi0new_dpx = dphi0new_dxc * (dxc_dpx) + dphi0new_dzc * (dzc_dpx);
272 float dphi0new_dpz = dphi0new_dxc * (dxc_dpz) + dphi0new_dzc * (dzc_dpz);
273
274 // ---- Build Jacobian ----
275 TMatrixD J(5, 6);
276
277 // ========================================
278 // Row 0: d0
279 // ========================================
280 // d0 = x_center * cos(phi0_new) + (z_center - target) * sin(phi0_new) - alpha_over_kappa
281
282
283 double phi_term = -(x_center * sin_phi_new) + (z_center - targetPos) * cos_phi_new;
284
285 double dd0_dxc = cos_phi_new + phi_term * dphi0new_dxc;
286 double dd0_dzc = sin_phi_new + phi_term * dphi0new_dzc;
287
288 J(0, 0) = dd0_dxc * dxc_dx; // d(d0)/d(x)
289 J(0, 1) = 0.0; // d(d0)/d(y)
290 J(0, 2) = dd0_dzc * dzc_dz; // d(d0)/d(z)
291 J(0, 3) = dd0_dxc * dxc_dpx + dd0_dzc * dzc_dpx - dalpha_over_kappa_dpx; // d(d0)/d(px)
292 J(0, 4) = 0.0; // d(d0)/d(py)
293 J(0, 5) = dd0_dxc * dxc_dpz + dd0_dzc * dzc_dpz - dalpha_over_kappa_dpz; // d(d0)/d(pz)
294
295 // ========================================
296 // Row 1: phi0_new = -phi0_new_helix
297 // ========================================
298
299 J(1, 0) = -dphi0new_dx; // d(phi0)/d(x)
300 J(1, 1) = 0.0; // d(phi0)/d(y)
301 J(1, 2) = -dphi0new_dz; // d(phi0)/d(z)
302 J(1, 3) = -dphi0new_dpx; // d(phi0)/d(px)
303 J(1, 4) = 0.0; // d(phi0)/d(py)
304 J(1, 5) = -dphi0new_dpz; // d(phi0)/d(pz)
305
306 // ========================================
307 // Row 2: omega (constant - given from original track)
308 // ========================================
309
310 J(2, 0) = 0.0; // d(omega)/d(x)
311 J(2, 1) = 0.0; // d(omega)/d(y)
312 J(2, 2) = 0.0; // d(omega)/d(z)
313 J(2, 3) = domega_dpT * dpT_dpx; // d(omega)/d(px)
314 J(2, 4) = 0.0; // d(omega)/d(py)
315 J(2, 5) = domega_dpT * dpT_dpz; // d(omega)/d(pz)
316
317 // ========================================
318 // Row 3: z0 = -dz_helix = y + alpha * phi_diff * tan_lambda
319 // ========================================
320
321 // d(phi_diff)/d(phi0_new_helix) = 1
322 // d(phi_diff)/d(phi0_helix) = -1
323
324 J(3, 0) = alpha_over_kappa * tan_L_helix * dphi0new_dx; // d(z0)/d(x)
325 J(3, 1) = 1.0; // d(z0)/d(y)
326 J(3, 2) = alpha_over_kappa * tan_L_helix * dphi0new_dz; // d(z0)/d(z)
327
328 // d(z0)/d(px) = d(y + alpha*phi_diff*tan_L)/d(px)
329 // = alpha * (dphi_diff/dpx) * tan_L + alpha * phi_diff * (dtan_L/dpx) +
330 // phi_diff*tan_L*(dalpha_dpx)
331 // where dphi_diff/dpx = dphi0new/dpx - dphi0h/dpx
332 double dphidiff_dpx = dphi0new_dpx - dphi0h_dpx;
333 double dphidiff_dpz = dphi0new_dpz - dphi0h_dpz;
334
335 J(3, 3) = alpha_over_kappa * (dphidiff_dpx * tan_L_helix - phi_diff * dtanL_dpx) + phi_diff * tan_L_helix * dalpha_over_kappa_dpx;
336 J(3, 4) = -alpha_over_kappa * phi_diff * dtanL_dpy;
337 J(3, 5) = alpha_over_kappa * (dphidiff_dpz * tan_L_helix - phi_diff * dtanL_dpz) + phi_diff * tan_L_helix * dalpha_over_kappa_dpz;
338
339 // ========================================
340 // Row 4: tan_lambda (depends only on momentum)
341 // ========================================
342
343 J(4, 0) = 0.0; // d(tanL)/d(x)
344 J(4, 1) = 0.0; // d(tanL)/d(y)
345 J(4, 2) = 0.0; // d(tanL)/d(z)
346 J(4, 3) = dtanL_dpx; // d(tanL)/d(px)
347 J(4, 4) = dtanL_dpy; // d(tanL)/d(py)
348 J(4, 5) = dtanL_dpz; // d(tanL)/d(pz)
349
350 return J;
351}
352
353
354
356 const TMatrixD& covVertex,
357 const TMatrixD& covMomentum,
358 const TMatrixD* covCross) {
359
360 TMatrixD cov(6, 6);
361
362 // Upper-left 3x3: position covariance
363 for (int i = 0; i < 3; i++) {
364 for (int j = 0; j < 3; j++) {
365 cov(i, j) = covVertex(i, j);
366 }
367 }
368
369 // Lower-right 3x3: momentum covariance
370 for (int i = 0; i < 3; i++) {
371 for (int j = 0; j < 3; j++) {
372 cov(i+3, j+3) = covMomentum(i, j);
373 }
374 }
375
376 // Off-diagonal blocks: position-momentum correlation
377 if (covCross != nullptr) {
378 for (int i = 0; i < 3; i++) {
379 for (int j = 0; j < 3; j++) {
380 cov(i, j+3) = (*covCross)(i, j);
381 cov(j+3, i) = (*covCross)(i, j);
382 }
383 }
384 } else {
385 // No correlation between position and momentum
386 for (int i = 0; i < 3; i++) {
387 for (int j = 0; j < 3; j++) {
388 cov(i, j+3) = 0.0;
389 cov(j+3, i) = 0.0;
390 }
391 }
392 }
393
394 return cov;
395}
396
397
399 float x_vtx, float y_vtx, float z_vtx,
400 float px, float py, float pz,
401 const TMatrixD& covVertex,
402 const TMatrixD& covMomentum,
403 float Q, float cB, float targetPos) {
404
405 // Get Jacobian: d[track params]/d[x,y,z,px,py,pz]
406 TMatrixD J = trackParametersJacobian(
407 x_vtx, y_vtx, z_vtx,
408 px, py, pz, Q, cB, targetPos);
409
410 // Build 6x6 combined covariance matrix
411 TMatrixD cov6x6 = buildCombinedCovariance(covVertex, covMomentum);
412
413 // Propagate covariance: Cov_track = J * Cov_6x6 * J^T
414 TMatrixD J_T(TMatrixD::kTransposed, J);
415 TMatrixD temp = J * cov6x6;
416 TMatrixD covTrack = temp * J_T;
417
418 return covTrack;
419}
420
421
423 float x_vtx, float y_vtx, float z_vtx,
424 float px, float py, float pz,
425 float Q, float cB, float targetPos) {
426
427 const double epsilon = 1e-4;
428 TMatrixD J_num(5, 6);
429
430 // Get nominal track parameters
431 std::vector<float> params_nominal = TrackTools::calculateTrackParameters(
432 x_vtx, y_vtx, z_vtx, px, py, pz, Q, cB, targetPos);
433
434 // Vary each input parameter
435 std::vector<double> inputs = {x_vtx, y_vtx, z_vtx, px, py, pz};
436
437 for (int j = 0; j < 6; j++) {
438 std::vector<double> inputs_plus = inputs;
439 inputs_plus[j] += epsilon;
440
441 std::vector<float> params_plus = TrackTools::calculateTrackParameters(
442 inputs_plus[0], inputs_plus[1], inputs_plus[2],
443 inputs_plus[3], inputs_plus[4], inputs_plus[5],
444 Q, cB, targetPos);
445
446 // Compute derivatives
447 for (int i = 0; i < 5; i++) {
448 J_num(i, j) = (params_plus[i] - params_nominal[i]) / epsilon;
449
450 // Handle angle wrapping for phi0
451 if (i == 1) {
452 double diff = params_plus[i] - params_nominal[i];
453 if (diff > M_PI) diff -= 2*M_PI;
454 if (diff < -M_PI) diff += 2*M_PI;
455 J_num(i, j) = diff / epsilon;
456 }
457 }
458 }
459
460 return J_num;
461}
462
463
464
465
Definition Track.h:32
TMatrixD numericalJacobian(float x_vtx, float y_vtx, float z_vtx, float px, float py, float pz, float Q, float cB, float targetPos)
float normalizeAngle(float angle)
Normalize angle to [-π, π].
Definition TrackTools.cxx:6
std::vector< float > calculateTrackParameters(float x_vtx, float y_vtx, float z_vtx, float px, float py, float pz, float Q, float cB, float targetPos)
Calculate track parameters at target from position and momentum.
TMatrixD getVertexCovariance(Vertex *vtx)
Extract vertex position covariance from Vertex object.
TMatrixD calculateTrackCovariance(float x_vtx, float y_vtx, float z_vtx, float px, float py, float pz, const TMatrixD &covVertex, const TMatrixD &covMomentum, float Q, float cB, float targetPos)
Calculate track parameter covariance from momentum, position.
TMatrixD getTrackCovariance(Track *track)
Extract track parameter covariance matrix from Track object.
TMatrixD momentumCovarianceFromTrack(double px, double py, double pz, double phi0, double omega, double tanLambda, const TMatrixD &covTrack)
Calculate momentum covariance from track parameter covariance.
TMatrixD buildCombinedCovariance(const TMatrixD &covVertex, const TMatrixD &covMomentum, const TMatrixD *covCross=nullptr)
Build combined 6x6 covariance matrix [x,y,z,px,py,pz].
TMatrixD trackParametersJacobian(float x_vtx, float y_vtx, float z_vtx, float px, float py, float pz, float Q, float cB, float targetPos)
Analytical Jacobian: d[d0,phi0,omega,z0,tanLambda]/d[x,y,z,px,py,pz].