From a1b40bc16ba0bed854c5158f0cdac110f74ffa05 Mon Sep 17 00:00:00 2001 From: ssdetlab <113530373+ssdetlab@users.noreply.github.com> Date: Mon, 4 Mar 2024 12:29:01 +0200 Subject: [PATCH] refactor: Safety check for spherical coordinates poles (#2980) Adding a small check for zero values of the `SinTheta` inside the `evaluateTrigonomics` of the `VectorHelpers`. It was causing the NaNs in the update Jacobians inside the KF when the geometry is oriented along z-axis and the initial momentum estimate is directed as (0,0,1). --- Core/include/Acts/Utilities/VectorHelpers.hpp | 3 +++ .../PropagationDenseConstant.cpp | 11 +++++---- .../PropagationEigenConstant.cpp | 10 ++++---- .../PropagationStraightLine.cpp | 16 ++++++------- .../Core/Surfaces/PlaneSurfaceTests.cpp | 24 ++++++++++++------- 5 files changed, 37 insertions(+), 27 deletions(-) diff --git a/Core/include/Acts/Utilities/VectorHelpers.hpp b/Core/include/Acts/Utilities/VectorHelpers.hpp index 9760343f71b..cd326659511 100644 --- a/Core/include/Acts/Utilities/VectorHelpers.hpp +++ b/Core/include/Acts/Utilities/VectorHelpers.hpp @@ -139,6 +139,9 @@ inline std::array evaluateTrigonomics(const Vector3& direction) { // can be turned into cosine/sine const ActsScalar cosTheta = z; const ActsScalar sinTheta = std::sqrt(1 - z * z); + assert(sinTheta != 0 && + "VectorHelpers: Vector is parallel to the z-axis " + "which leads to division by zero"); const ActsScalar invSinTheta = 1. / sinTheta; const ActsScalar cosPhi = x * invSinTheta; const ActsScalar sinPhi = y * invSinTheta; diff --git a/Tests/IntegrationTests/PropagationDenseConstant.cpp b/Tests/IntegrationTests/PropagationDenseConstant.cpp index e9e1d5469bb..3f1efdeaa1e 100644 --- a/Tests/IntegrationTests/PropagationDenseConstant.cpp +++ b/Tests/IntegrationTests/PropagationDenseConstant.cpp @@ -166,10 +166,10 @@ BOOST_DATA_TEST_CASE(ToStrawAlongZ, } // check covariance transport using the ridders propagator for comparison - +// Covariance transport does not work for theta close to poles BOOST_DATA_TEST_CASE(CovarianceCurvilinear, - ds::phi* ds::theta* ds::absMomentum* ds::chargeNonZero* - ds::pathLength* ds::magneticField, + ds::phi* ds::thetaWithoutBeam* ds::absMomentum* + ds::chargeNonZero* ds::pathLength* ds::magneticField, phi, theta, p, q, s, bz) { runForwardComparisonTest( @@ -202,9 +202,10 @@ BOOST_DATA_TEST_CASE(CovarianceToDisc, DiscSurfaceBuilder(), epsPos, epsDir, epsMom, epsCov); } +// Covariance transport does not work for theta close to poles BOOST_DATA_TEST_CASE(CovarianceToPlane, - ds::phi* ds::theta* ds::absMomentum* ds::chargeNonZero* - ds::pathLength* ds::magneticField, + ds::phi* ds::thetaWithoutBeam* ds::absMomentum* + ds::chargeNonZero* ds::pathLength* ds::magneticField, phi, theta, p, q, s, bz) { runToSurfaceComparisonTest( diff --git a/Tests/IntegrationTests/PropagationEigenConstant.cpp b/Tests/IntegrationTests/PropagationEigenConstant.cpp index 844ead4db1e..0c76437e156 100644 --- a/Tests/IntegrationTests/PropagationEigenConstant.cpp +++ b/Tests/IntegrationTests/PropagationEigenConstant.cpp @@ -109,10 +109,10 @@ BOOST_DATA_TEST_CASE(ToStrawAlongZ, } // check covariance transport using the ridders propagator for comparison - +// Covariance transport does not work for theta close to poles BOOST_DATA_TEST_CASE(CovarianceCurvilinear, - ds::phi* ds::theta* ds::absMomentum* ds::chargeNonZero* - ds::pathLength* ds::magneticField, + ds::phi* ds::thetaWithoutBeam* ds::absMomentum* + ds::chargeNonZero* ds::pathLength* ds::magneticField, phi, theta, p, q, s, bz) { runForwardComparisonTest( makePropagator(bz), makeRiddersPropagator(bz), geoCtx, magCtx, @@ -142,8 +142,8 @@ BOOST_DATA_TEST_CASE(CovarianceToDisc, } BOOST_DATA_TEST_CASE(CovarianceToPlane, - ds::phi* ds::theta* ds::absMomentum* ds::chargeNonZero* - ds::pathLength* ds::magneticField, + ds::phi* ds::thetaWithoutBeam* ds::absMomentum* + ds::chargeNonZero* ds::pathLength* ds::magneticField, phi, theta, p, q, s, bz) { runToSurfaceComparisonTest( makePropagator(bz), makeRiddersPropagator(bz), geoCtx, magCtx, diff --git a/Tests/IntegrationTests/PropagationStraightLine.cpp b/Tests/IntegrationTests/PropagationStraightLine.cpp index 233faba43ee..9fef03c4f5a 100644 --- a/Tests/IntegrationTests/PropagationStraightLine.cpp +++ b/Tests/IntegrationTests/PropagationStraightLine.cpp @@ -99,10 +99,10 @@ BOOST_DATA_TEST_CASE(ToStrawAlongZ, // check covariance transport using the ridders propagator for comparison -BOOST_DATA_TEST_CASE( - CovarianceCurvilinear, - ds::phi* ds::theta* ds::absMomentum* ds::chargeNonZero* ds::pathLength, phi, - theta, p, q, s) { +BOOST_DATA_TEST_CASE(CovarianceCurvilinear, + ds::phi* ds::thetaWithoutBeam* ds::absMomentum* + ds::chargeNonZero* ds::pathLength, + phi, theta, p, q, s) { runForwardComparisonTest( propagator, riddersPropagator, geoCtx, magCtx, makeParametersCurvilinearWithCovariance(phi, theta, p, q), s, epsPos, @@ -129,10 +129,10 @@ BOOST_DATA_TEST_CASE(CovarianceToDisc, DiscSurfaceBuilder(), epsPos, epsDir, epsMom, epsCov); } -BOOST_DATA_TEST_CASE( - CovarianceToPlane, - ds::phi* ds::theta* ds::absMomentum* ds::chargeNonZero* ds::pathLength, phi, - theta, p, q, s) { +BOOST_DATA_TEST_CASE(CovarianceToPlane, + ds::phi* ds::thetaWithoutBeam* ds::absMomentum* + ds::chargeNonZero* ds::pathLength, + phi, theta, p, q, s) { runToSurfaceComparisonTest( propagator, riddersPropagator, geoCtx, magCtx, makeParametersCurvilinearWithCovariance(phi, theta, p, q), s, diff --git a/Tests/UnitTests/Core/Surfaces/PlaneSurfaceTests.cpp b/Tests/UnitTests/Core/Surfaces/PlaneSurfaceTests.cpp index 0bcbd33dfb9..8aacba86c77 100644 --- a/Tests/UnitTests/Core/Surfaces/PlaneSurfaceTests.cpp +++ b/Tests/UnitTests/Core/Surfaces/PlaneSurfaceTests.cpp @@ -271,22 +271,27 @@ BOOST_AUTO_TEST_CASE(PlaneSurfaceAlignment) { auto rBounds = std::make_shared(3., 4.); // Test clone method Translation3 translation{0., 1., 2.}; - auto pTransform = Transform3(translation); + double rotationAngle = M_PI_2; + AngleAxis3 rotation(rotationAngle, Vector3::UnitY()); + RotationMatrix3 rotationMat = rotation.toRotationMatrix(); + + auto pTransform = Transform3(translation * rotationMat); auto planeSurfaceObject = Surface::makeShared(pTransform, rBounds); - const auto& rotation = pTransform.rotation(); + // The local frame z axis - const Vector3 localZAxis = rotation.col(2); - // Check the local z axis is aligned to global z axis - CHECK_CLOSE_ABS(localZAxis, Vector3(0., 0., 1.), 1e-15); + const Vector3 localZAxis = rotationMat.col(2); + // Check the local z axis is aligned to global x axis + CHECK_CLOSE_ABS(localZAxis, Vector3(1., 0., 0.), 1e-15); // Define the track (local) position and direction Vector2 localPosition{1, 2}; - Vector3 momentum{0, 0, 1}; + Vector3 momentum{1, 0, 0}; Vector3 direction = momentum.normalized(); // Get the global position Vector3 globalPosition = planeSurfaceObject->localToGlobal(tgContext, localPosition, momentum); + // Construct a free parameters FreeVector parameters = FreeVector::Zero(); parameters.head<3>() = globalPosition; @@ -297,7 +302,8 @@ BOOST_AUTO_TEST_CASE(PlaneSurfaceAlignment) { planeSurfaceObject->alignmentToPathDerivative(tgContext, parameters); // The expected results AlignmentToPathMatrix expAlignToPath = AlignmentToPathMatrix::Zero(); - expAlignToPath << 0, 0, 1, 2, -1, 0; + expAlignToPath << 1, 0, 0, 2, -1, -2; + // Check if the calculated derivative is as expected CHECK_CLOSE_ABS(alignToPath, expAlignToPath, 1e-10); @@ -322,9 +328,9 @@ BOOST_AUTO_TEST_CASE(PlaneSurfaceAlignment) { alignToBound.block<1, 6>(eBoundLoc1, eAlignmentCenter0); // The expected results AlignmentToPathMatrix expAlignToloc0; - expAlignToloc0 << -1, 0, 0, 0, 0, 2; + expAlignToloc0 << 0, 0, 1, 0, 0, 0; AlignmentToPathMatrix expAlignToloc1; - expAlignToloc1 << 0, -1, 0, 0, 0, -1; + expAlignToloc1 << 0, -1, 0, 0, 0, 0; // Check if the calculated derivatives are as expected CHECK_CLOSE_ABS(alignToloc0, expAlignToloc0, 1e-10); CHECK_CLOSE_ABS(alignToloc1, expAlignToloc1, 1e-10);