Skip to content

Commit c0d6334

Browse files
mpucciompuccio
committed
Stabilise propagation in forward direction
1 parent 72b2899 commit c0d6334

2 files changed

Lines changed: 171 additions & 146 deletions

File tree

‎Detectors/ITSMFT/common/tracking/src/Propagator.cxx‎

Lines changed: 50 additions & 146 deletions
Original file line numberDiff line numberDiff line change
@@ -389,86 +389,68 @@ bool propagateLinear(SurfaceTrackState& state, float targetZ) noexcept
389389
return true;
390390
}
391391

392-
bool propagateHelixParameters(SurfaceTrackState& state, float targetZ, float bz) noexcept
392+
// Share the same helix and Jacobian between direct and reference propagation.
393+
// The midpoint-angle form avoids subtracting O(1/curvature) coordinates;
394+
// its sinc derivative also remains well conditioned for almost straight tracks.
395+
template <typename State>
396+
bool propagateHelixWithJacobian(State& state, float targetZ, float bz, DenseMatrix5& jacobian) noexcept
393397
{
394-
const float dz = targetZ - state.referenceCoordinate;
395-
if (dz == 0.f) {
398+
identity(jacobian);
399+
const double dz = static_cast<double>(targetZ) - state.referenceCoordinate;
400+
if (dz == 0.) {
396401
return true;
397402
}
398-
const float tanl = state.parameters[3];
399-
const float inverseQPt = state.parameters[4];
400-
if (tanl == 0.f) {
401-
return false;
402-
}
403-
if (bz == 0.f || inverseQPt == 0.f) {
404-
return false;
405-
}
406-
const float inverseTanl = 1.f / tanl;
407-
const float qPt = 1.f / inverseQPt;
408-
const float phi = state.parameters[2];
409-
const float sinPhi = std::sin(phi);
410-
const float cosPhi = std::cos(phi);
411-
const float k = std::abs(o2::constants::math::B2C * bz);
412-
const float inverseK = 1.f / k;
413-
const float theta = -inverseQPt * dz * k * inverseTanl;
414-
const float sinTheta = std::sin(theta);
415-
const float cosTheta = std::cos(theta);
416-
const float fieldSign = std::copysign(1.f, bz);
417-
const float y = sinPhi * qPt * inverseK;
418-
const float x = cosPhi * qPt * inverseK;
419-
state.parameters[0] += fieldSign * (y - y * cosTheta) - x * sinTheta;
420-
state.parameters[1] += fieldSign * (-x + x * cosTheta) - y * sinTheta;
421-
state.parameters[2] += fieldSign * theta;
403+
const double tanl = state.parameters[3];
404+
const double inverseQPt = state.parameters[4];
405+
if (tanl == 0. || bz == 0.f || inverseQPt == 0.) {
406+
return false;
407+
}
408+
const double n = dz / tanl;
409+
const double curvatureScale = -std::abs(static_cast<double>(o2::constants::math::B2C)) * bz;
410+
const double halfAnglePerQPt = 0.5 * curvatureScale * n;
411+
const double halfAngle = inverseQPt * halfAnglePerQPt;
412+
double sinc, sincDerivative;
413+
if (std::abs(halfAngle) < 0.01) {
414+
// sin(h)/h and its derivative, including their limits at h = 0.
415+
const double h2 = halfAngle * halfAngle;
416+
sinc = 1. + h2 * (-1. / 6. + h2 * (1. / 120. - h2 / 5040.));
417+
sincDerivative = halfAngle * (-1. / 3. + h2 * (1. / 30. - h2 / 840.));
418+
} else {
419+
sinc = std::sin(halfAngle) / halfAngle;
420+
sincDerivative = (std::cos(halfAngle) - sinc) / halfAngle;
421+
}
422+
const double phi = state.parameters[2];
423+
const double sinMid = std::sin(phi + halfAngle);
424+
const double cosMid = std::cos(phi + halfAngle);
425+
const double endPhi = phi + 2. * halfAngle;
426+
const double dx = n * sinc * cosMid;
427+
const double dy = n * sinc * sinMid;
428+
429+
jacobian[0][2] = -dy;
430+
jacobian[1][2] = dx;
431+
jacobian[0][3] = -n / tanl * std::cos(endPhi);
432+
jacobian[1][3] = -n / tanl * std::sin(endPhi);
433+
jacobian[0][4] = n * halfAnglePerQPt * (sincDerivative * cosMid - sinc * sinMid);
434+
jacobian[1][4] = n * halfAnglePerQPt * (sincDerivative * sinMid + sinc * cosMid);
435+
jacobian[2][3] = -2. * halfAngle / tanl;
436+
jacobian[2][4] = 2. * halfAnglePerQPt;
437+
438+
state.parameters[0] += dx;
439+
state.parameters[1] += dy;
440+
state.parameters[2] = endPhi;
422441
state.referenceCoordinate = targetZ;
423442
return true;
424443
}
425444

426445
bool propagateHelix(SurfaceTrackState& state, float targetZ, float bz) noexcept
427446
{
428-
const float originalZ = state.referenceCoordinate;
429-
const float dz = targetZ - originalZ;
430-
if (dz == 0.f) {
447+
if (targetZ == state.referenceCoordinate) {
431448
return true;
432449
}
433-
const float phi = state.parameters[2];
434-
const float tanl = state.parameters[3];
435-
const float inverseQPt = state.parameters[4];
436-
if (!propagateHelixParameters(state, targetZ, bz)) {
450+
DenseMatrix5 jacobian{};
451+
if (!propagateHelixWithJacobian(state, targetZ, bz, jacobian)) {
437452
return false;
438453
}
439-
const float inverseTanl = 1.f / tanl;
440-
const float qPt = 1.f / inverseQPt;
441-
const float sinPhi = std::sin(phi);
442-
const float cosPhi = std::cos(phi);
443-
const float k = std::abs(o2::constants::math::B2C * bz);
444-
const float inverseK = 1.f / k;
445-
const float theta = -inverseQPt * dz * k * inverseTanl;
446-
const float sinTheta = std::sin(theta);
447-
const float cosTheta = std::cos(theta);
448-
const float fieldSign = std::copysign(1.f, bz);
449-
const float n = dz * inverseTanl;
450-
const float m = n * inverseTanl;
451-
const float o = sinTheta * cosPhi;
452-
const float p = sinPhi * cosTheta;
453-
const float r = sinPhi * sinTheta;
454-
const float s = cosPhi * cosTheta;
455-
const float y = sinPhi * qPt * inverseK;
456-
const float x = cosPhi * qPt * inverseK;
457-
const float t = qPt * cosTheta;
458-
const float u = qPt * sinTheta;
459-
const float v = qPt;
460-
const float nn = dz * inverseTanl * qPt;
461-
462-
DenseMatrix5 jacobian{};
463-
identity(jacobian);
464-
jacobian[0][2] = fieldSign * x - fieldSign * x * cosTheta + y * sinTheta;
465-
jacobian[0][3] = fieldSign * r * m - s * m;
466-
jacobian[0][4] = -fieldSign * nn * r + fieldSign * t * y - fieldSign * v * y + nn * s + u * x;
467-
jacobian[1][2] = fieldSign * y - fieldSign * y * cosTheta - x * sinTheta;
468-
jacobian[1][3] = -fieldSign * o * m - p * m;
469-
jacobian[1][4] = fieldSign * nn * o - fieldSign * t * x + fieldSign * v * x + nn * p + u * y;
470-
jacobian[2][3] = -fieldSign * theta * inverseTanl;
471-
jacobian[2][4] = -fieldSign * k * n;
472454
transportCovariance(state, jacobian);
473455
return true;
474456
}
@@ -512,87 +494,9 @@ bool referencePropagateLinear(SurfaceTrackParameters& ref, float targetZ, DenseM
512494
return true;
513495
}
514496

515-
// Position-only helix step, matching propagateHelixParameters.
516-
bool referencePropagateHelixParameters(SurfaceTrackParameters& ref, float targetZ, float bz) noexcept
517-
{
518-
const float dz = targetZ - ref.referenceCoordinate;
519-
if (dz == 0.f) {
520-
return true;
521-
}
522-
const float tanl = ref.parameters[3];
523-
const float inverseQPt = ref.parameters[4];
524-
if (tanl == 0.f) {
525-
return false;
526-
}
527-
if (bz == 0.f || inverseQPt == 0.f) {
528-
return false;
529-
}
530-
const float inverseTanl = 1.f / tanl;
531-
const float qPt = 1.f / inverseQPt;
532-
const float phi = ref.parameters[2];
533-
const float sinPhi = std::sin(phi);
534-
const float cosPhi = std::cos(phi);
535-
const float k = std::abs(o2::constants::math::B2C * bz);
536-
const float inverseK = 1.f / k;
537-
const float theta = -inverseQPt * dz * k * inverseTanl;
538-
const float sinTheta = std::sin(theta);
539-
const float cosTheta = std::cos(theta);
540-
const float fieldSign = std::copysign(1.f, bz);
541-
const float y = sinPhi * qPt * inverseK;
542-
const float x = cosPhi * qPt * inverseK;
543-
ref.parameters[0] += fieldSign * (y - y * cosTheta) - x * sinTheta;
544-
ref.parameters[1] += fieldSign * (-x + x * cosTheta) - y * sinTheta;
545-
ref.parameters[2] += fieldSign * theta;
546-
ref.referenceCoordinate = targetZ;
547-
return true;
548-
}
549-
550497
bool referencePropagateHelix(SurfaceTrackParameters& ref, float targetZ, float bz, DenseMatrix5& jacobian) noexcept
551498
{
552-
identity(jacobian);
553-
const float originalZ = ref.referenceCoordinate;
554-
const float dz = targetZ - originalZ;
555-
if (dz == 0.f) {
556-
return true;
557-
}
558-
const float phi = ref.parameters[2];
559-
const float tanl = ref.parameters[3];
560-
const float inverseQPt = ref.parameters[4];
561-
if (!referencePropagateHelixParameters(ref, targetZ, bz)) {
562-
return false;
563-
}
564-
const float inverseTanl = 1.f / tanl;
565-
const float qPt = 1.f / inverseQPt;
566-
const float sinPhi = std::sin(phi);
567-
const float cosPhi = std::cos(phi);
568-
const float k = std::abs(o2::constants::math::B2C * bz);
569-
const float inverseK = 1.f / k;
570-
const float theta = -inverseQPt * dz * k * inverseTanl;
571-
const float sinTheta = std::sin(theta);
572-
const float cosTheta = std::cos(theta);
573-
const float fieldSign = std::copysign(1.f, bz);
574-
const float n = dz * inverseTanl;
575-
const float m = n * inverseTanl;
576-
const float o = sinTheta * cosPhi;
577-
const float p = sinPhi * cosTheta;
578-
const float r = sinPhi * sinTheta;
579-
const float s = cosPhi * cosTheta;
580-
const float y = sinPhi * qPt * inverseK;
581-
const float x = cosPhi * qPt * inverseK;
582-
const float t = qPt * cosTheta;
583-
const float u = qPt * sinTheta;
584-
const float v = qPt;
585-
const float nn = dz * inverseTanl * qPt;
586-
587-
jacobian[0][2] = fieldSign * x - fieldSign * x * cosTheta + y * sinTheta;
588-
jacobian[0][3] = fieldSign * r * m - s * m;
589-
jacobian[0][4] = -fieldSign * nn * r + fieldSign * t * y - fieldSign * v * y + nn * s + u * x;
590-
jacobian[1][2] = fieldSign * y - fieldSign * y * cosTheta - x * sinTheta;
591-
jacobian[1][3] = -fieldSign * o * m - p * m;
592-
jacobian[1][4] = fieldSign * nn * o - fieldSign * t * x + fieldSign * v * x + nn * p + u * y;
593-
jacobian[2][3] = -fieldSign * theta * inverseTanl;
594-
jacobian[2][4] = -fieldSign * k * n;
595-
return true;
499+
return propagateHelixWithJacobian(ref, targetZ, bz, jacobian);
596500
}
597501

598502
bool propagateAccepted(SurfaceTrackState& state, SurfaceTrackParameters& linRef, float targetZ, float bz) noexcept

‎Detectors/ITSMFT/common/tracking/test/testPropagator.cxx‎

Lines changed: 121 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -240,6 +240,127 @@ void checkConversionCovariance(const SurfaceTrackState& source, float bz)
240240

241241
} // namespace
242242

243+
BOOST_AUTO_TEST_CASE(ForwardHelixSmallAngleMomentumDerivative)
244+
{
245+
// Isolate the q/pT Jacobian column with a unit momentum variance. An
246+
// independent double-precision trajectory supplies numerical derivatives;
247+
// checking only total position variances can hide this column's cancellation.
248+
for (const float bz : {-5.f, 5.f}) {
249+
for (const float qOverPt : {-20.f, -1.f, -0.1f, -0.01f, -1.e-8f, 1.e-8f, 0.01f, 0.1f, 1.f, 20.f}) {
250+
for (const float dz : {-32.f, -1.4222f, -0.01f, 0.01f, 1.4222f, 32.f}) {
251+
for (const float tanl : {-10.f, 10.f}) {
252+
BOOST_TEST_CONTEXT("bz=" << bz << " q/pT=" << qOverPt << " dz=" << dz << " tanl=" << tanl)
253+
{
254+
auto source = diskState();
255+
source.parameters[0] = source.parameters[1] = 0.f;
256+
source.parameters[2] = 0.7f;
257+
source.parameters[3] = tanl;
258+
source.parameters[4] = qOverPt;
259+
std::fill(std::begin(source.covariance), std::end(source.covariance), 0.f);
260+
source.covariance[packedCovarianceIndex(4, 4)] = 1.f;
261+
auto plane = source;
262+
plane.referenceCoordinate += dz;
263+
std::array<double, 5> parameters{};
264+
std::copy(std::begin(source.parameters), std::end(source.parameters), parameters.begin());
265+
const auto expected = intersectConversionPlane(source, parameters, plane, bz);
266+
constexpr double step = 1.e-3;
267+
auto plus = parameters, minus = parameters;
268+
plus[4] += step;
269+
minus[4] -= step;
270+
const auto high = intersectConversionPlane(source, plus, plane, bz);
271+
const auto low = intersectConversionPlane(source, minus, plane, bz);
272+
std::array<double, 5> derivative{};
273+
for (int row = 0; row < 5; ++row) {
274+
derivative[row] = (high[row] - low[row]) / (2. * step);
275+
}
276+
277+
auto direct = source;
278+
auto referenced = source;
279+
SurfaceTrackParameters reference{source};
280+
BOOST_REQUIRE(Propagator::propagateForward(direct, plane.referenceCoordinate, bz));
281+
BOOST_REQUIRE(Propagator::propagateForward(referenced, reference, plane.referenceCoordinate, bz));
282+
for (int row = 0; row < 5; ++row) {
283+
const double positionTolerance = 2.e-7 * std::abs(expected[row]) + 1.e-12;
284+
BOOST_CHECK_SMALL(double(direct.parameters[row]) - expected[row], positionTolerance);
285+
BOOST_CHECK_SMALL(double(referenced.parameters[row]) - expected[row], positionTolerance);
286+
BOOST_CHECK_SMALL(double(reference.parameters[row]) - expected[row], positionTolerance);
287+
for (int column = 0; column <= row; ++column) {
288+
const auto index = packedCovarianceIndex(row, column);
289+
const double covariance = derivative[row] * derivative[column];
290+
const double tolerance = 2.e-5 * std::abs(covariance) + 1.e-16;
291+
BOOST_CHECK_SMALL(double(direct.covariance[index]) - covariance, tolerance);
292+
BOOST_CHECK_SMALL(double(referenced.covariance[index]) - covariance, tolerance);
293+
}
294+
}
295+
}
296+
}
297+
}
298+
}
299+
}
300+
}
301+
302+
BOOST_AUTO_TEST_CASE(ForwardHelixTransportMatchesNumericalDerivatives)
303+
{
304+
for (const float bz : {-5.f, 5.f}) {
305+
for (const float qOverPt : {-20.f, -0.1f, 0.1f, 20.f}) {
306+
for (const float dz : {-32.f, 32.f}) {
307+
BOOST_TEST_CONTEXT("bz=" << bz << " q/pT=" << qOverPt << " dz=" << dz)
308+
{
309+
auto source = diskState();
310+
source.parameters[4] = qOverPt;
311+
auto plane = source;
312+
plane.referenceCoordinate += dz;
313+
std::array<double, 5> parameters{};
314+
std::copy(std::begin(source.parameters), std::end(source.parameters), parameters.begin());
315+
const auto expected = intersectConversionPlane(source, parameters, plane, bz);
316+
double jacobian[5][5]{};
317+
constexpr double step = 1.e-5;
318+
for (int column = 0; column < 5; ++column) {
319+
auto plus = parameters, minus = parameters;
320+
plus[column] += step;
321+
minus[column] -= step;
322+
const auto high = intersectConversionPlane(source, plus, plane, bz);
323+
const auto low = intersectConversionPlane(source, minus, plane, bz);
324+
for (int row = 0; row < 5; ++row) {
325+
jacobian[row][column] = (high[row] - low[row]) / (2. * step);
326+
}
327+
}
328+
auto direct = source;
329+
auto referenced = source;
330+
SurfaceTrackParameters reference{source};
331+
std::array<double, 5> difference{};
332+
for (int row = 0; row < 5; ++row) {
333+
referenced.parameters[row] += 0.001f * (row + 1);
334+
difference[row] = double(referenced.parameters[row]) - source.parameters[row];
335+
}
336+
BOOST_REQUIRE(Propagator::propagateForward(direct, plane.referenceCoordinate, bz));
337+
BOOST_REQUIRE(Propagator::propagateForward(referenced, reference, plane.referenceCoordinate, bz));
338+
for (int row = 0; row < 5; ++row) {
339+
double linearized = expected[row];
340+
for (int i = 0; i < 5; ++i) {
341+
linearized += jacobian[row][i] * difference[i];
342+
}
343+
BOOST_CHECK_SMALL(double(direct.parameters[row]) - expected[row], 2.e-6);
344+
BOOST_CHECK_SMALL(double(referenced.parameters[row]) - linearized, 3.e-6);
345+
for (int column = 0; column <= row; ++column) {
346+
double covariance = 0.;
347+
for (int i = 0; i < 5; ++i) {
348+
for (int j = 0; j < 5; ++j) {
349+
covariance += jacobian[row][i] * source.covariance[packedCovarianceIndex(i, j)] * jacobian[column][j];
350+
}
351+
}
352+
const auto index = packedCovarianceIndex(row, column);
353+
const double tolerance = 1.e-7 + 2.e-5 * std::abs(covariance);
354+
BOOST_CHECK_SMALL(double(direct.covariance[index]) - covariance, tolerance);
355+
BOOST_CHECK_SMALL(double(referenced.covariance[index]) - covariance, tolerance);
356+
}
357+
}
358+
}
359+
}
360+
}
361+
}
362+
}
363+
243364
// --- 1/2: same-family propagate-to-measurement succeeds ---------------------
244365

245366
BOOST_AUTO_TEST_CASE(CylinderToCylinderPropagateAndUpdateSucceeds)

0 commit comments

Comments
 (0)