diff --git a/Core/include/Acts/Fitter/GainMatrixSmoother.hpp b/Core/include/Acts/Fitter/GainMatrixSmoother.hpp index 4995580faec..c50447dd42e 100644 --- a/Core/include/Acts/Fitter/GainMatrixSmoother.hpp +++ b/Core/include/Acts/Fitter/GainMatrixSmoother.hpp @@ -36,6 +36,17 @@ class GainMatrixSmoother { getDefaultLogger("GainMatrixSmoother", Logging::INFO).release())) : m_logger(std::move(logger)) {} + /// Operater for Kalman smoothing + /// + /// @tparam source_link_t The type of source link + /// + /// @param gctx The geometry context for the smoothing + /// @param trajectory The trajectory to be smoothed + /// @param entryIndex The index of state to start the smoothing + /// @param globalTrackParamsCovPtr The pointer to global track parameters + /// covariance matrix + /// + /// @return The smoothed track parameters at the first measurement state template Result operator()(const GeometryContext& gctx, MultiTrajectory& trajectory, @@ -44,8 +55,8 @@ class GainMatrixSmoother { using namespace boost::adaptors; // using ParVector_t = typename parameters_t::ParVector_t; - using CovMatrix_t = typename parameters_t::CovMatrix_t; - using gain_matrix_t = CovMatrix_t; + using CovMatrix = typename parameters_t::CovMatrix_t; + using GainMatrix = CovMatrix; // For the last state: smoothed is filtered - also: switch to next ACTS_VERBOSE("Getting previous track state"); @@ -55,7 +66,7 @@ class GainMatrixSmoother { prev_ts.smoothedCovariance() = prev_ts.filteredCovariance(); // Smoothing gain matrix - gain_matrix_t G; + GainMatrix G; // make sure there is more than one track state std::optional error{std::nullopt}; // assume ok @@ -124,8 +135,8 @@ class GainMatrixSmoother { // If not, make one (could do more) attempt to replace it with the // nearest semi-positive def matrix, // but it could still be non semi-positive - CovMatrix_t smoothedCov = ts.smoothedCovariance(); - if (not detail::covariance_helper::validate(smoothedCov)) { + CovMatrix smoothedCov = ts.smoothedCovariance(); + if (not detail::covariance_helper::validate(smoothedCov)) { ACTS_DEBUG( "Smoothed covariance is not positive definite. Could result in " "negative covariance!"); diff --git a/Core/include/Acts/Fitter/KalmanFitter.hpp b/Core/include/Acts/Fitter/KalmanFitter.hpp index 0b942c3cd78..69303cbbbc6 100644 --- a/Core/include/Acts/Fitter/KalmanFitter.hpp +++ b/Core/include/Acts/Fitter/KalmanFitter.hpp @@ -99,7 +99,7 @@ struct KalmanFitterOptions { /// Whether to consider energy loss bool energyLoss = true; - /// Whether to run backward filtering. + /// Whether to run backward filtering bool backwardFiltering = false; }; @@ -870,10 +870,10 @@ class KalmanFitter { ACTS_VERBOSE("Apply smoothing on " << nStates << " filtered track states."); } + // Smooth the track states auto smoothRes = m_smoother(state.geoContext, result.fittedStates, measurementIndices.front()); - if (!smoothRes.ok()) { ACTS_ERROR("Smoothing step failed: " << smoothRes.error()); return smoothRes.error(); diff --git a/Core/include/Acts/Fitter/detail/KalmanGlobalCovariance.hpp b/Core/include/Acts/Fitter/detail/KalmanGlobalCovariance.hpp new file mode 100644 index 00000000000..6c94ce6ae3c --- /dev/null +++ b/Core/include/Acts/Fitter/detail/KalmanGlobalCovariance.hpp @@ -0,0 +1,108 @@ +// This file is part of the Acts project. +// +// Copyright (C) 2020 CERN for the benefit of the Acts project +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#pragma once + +#include "Acts/EventData/MultiTrajectory.hpp" +#include "Acts/EventData/TrackParameters.hpp" +#include "Acts/Geometry/GeometryContext.hpp" +#include "Acts/Surfaces/Surface.hpp" +#include "Acts/Utilities/Definitions.hpp" +#include "Acts/Utilities/ParameterDefinitions.hpp" + +#include + +namespace Acts { +namespace detail { +/// Calculate the global track parameters covariance for a smoothed trajectory +/// stored in MultiTrajecty based on formulas at Journal of Physics: Conference +/// Series 219 (2010) 032028. +/// +/// @tparam source_link_t The source link type of the trajectory +/// @tparam parameters_t The track parameters type +/// +/// @param multiTraj The MultiTrajectory containing the trajectory to be +/// investigated +/// @param entryIndex The trajectory entry index +/// +/// @return The global track parameters covariance matrix and the starting +/// row/column for smoothed states +template +std::pair, + std::unordered_map> +globalTrackParametersCovariance( + const Acts::MultiTrajectory& multiTraj, + const size_t& entryIndex) { + using CovMatrix = typename parameters_t::CovMatrix_t; + using GainMatrix = CovMatrix; + + // The last smoothed state index + size_t lastSmoothedIndex = SIZE_MAX; + // The total number of smoothed states + size_t nSmoothedStates = 0; + // Visit all the states + multiTraj.visitBackwards(entryIndex, [&](const auto& ts) { + if (ts.hasSmoothed()) { + if (lastSmoothedIndex == SIZE_MAX) { + lastSmoothedIndex = ts.index(); + } + nSmoothedStates++; + } + }); + + // Set the size of global track parameters covariance for all smoothed states + ActsMatrixX fullGlobalTrackParamsCov = + ActsMatrixX::Zero( + nSmoothedStates * eBoundParametersSize, + nSmoothedStates * eBoundParametersSize); + // The index of state within the trajectory and the starting row/column for + // this state in the global covariance matrix + std::unordered_map stateRowIndices; + // Visit the smoothed states to calculate the full global track parameters + // covariance + size_t nProcessed = 0; + auto prev_ts = multiTraj.getTrackState(lastSmoothedIndex); + multiTraj.visitBackwards(lastSmoothedIndex, [&](const auto& ts) { + const size_t iRow = fullGlobalTrackParamsCov.rows() - + eBoundParametersSize * (nProcessed + 1); + // Fill the covariance of this state + fullGlobalTrackParamsCov.block( + iRow, iRow) = ts.smoothedCovariance(); + // Fill the correlation between this state (indexed by i-1) and + // beforehand smoothed states (indexed by j): C^n_{i-1, j}= G_{i-1} * + // C^n_{i, j} for i <= j + if (nProcessed > 0) { + // Calculate the gain matrix + GainMatrix G = ts.filteredCovariance() * prev_ts.jacobian().transpose() * + prev_ts.predictedCovariance().inverse(); + // Loop over the beforehand smoothed states + for (size_t iProcessed = 1; iProcessed <= nProcessed; iProcessed++) { + const size_t iCol = iRow + eBoundParametersSize * iProcessed; + CovMatrix prev_correlation = + fullGlobalTrackParamsCov + .block( + iRow + eBoundParametersSize, iCol); + CovMatrix correlation = G * prev_correlation; + fullGlobalTrackParamsCov + .block(iRow, iCol) = + correlation; + fullGlobalTrackParamsCov + .block(iCol, iRow) = + correlation.transpose(); + } + } + stateRowIndices.emplace(ts.index(), iRow); + nProcessed++; + prev_ts = ts; + }); + + return std::make_pair(fullGlobalTrackParamsCov, stateRowIndices); +} + +} // namespace detail +} // namespace Acts diff --git a/Core/include/Acts/Surfaces/ConeSurface.hpp b/Core/include/Acts/Surfaces/ConeSurface.hpp index cade8bbe7ef..592e3c7e0ea 100644 --- a/Core/include/Acts/Surfaces/ConeSurface.hpp +++ b/Core/include/Acts/Surfaces/ConeSurface.hpp @@ -212,6 +212,17 @@ class ConeSurface : public Surface { /// Return properly formatted class name for screen output std::string name() const override; + /// Calculate the derivative of bound track parameters local position w.r.t. + /// position in local 3D Cartesian coordinates + /// + /// @param gctx The current geometry context object, e.g. alignment + /// @param position The position of the paramters in global + /// + /// @return Derivative of bound local position w.r.t. position in local 3D + /// cartesian coordinates + const LocalCartesianToBoundLocalMatrix localCartesianToBoundLocalDerivative( + const GeometryContext& gctx, const Vector3D& position) const final; + protected: std::shared_ptr m_bounds; ///< bounds (shared) diff --git a/Core/include/Acts/Surfaces/CylinderSurface.hpp b/Core/include/Acts/Surfaces/CylinderSurface.hpp index 065545a0260..454869d713a 100644 --- a/Core/include/Acts/Surfaces/CylinderSurface.hpp +++ b/Core/include/Acts/Surfaces/CylinderSurface.hpp @@ -219,6 +219,17 @@ class CylinderSurface : public Surface { Polyhedron polyhedronRepresentation(const GeometryContext& gctx, size_t lseg) const override; + /// Calculate the derivative of bound track parameters local position w.r.t. + /// position in local 3D Cartesian coordinates + /// + /// @param gctx The current geometry context object, e.g. alignment + /// @param position The position of the paramters in global + /// + /// @return Derivative of bound local position w.r.t. position in local 3D + /// cartesian coordinates + const LocalCartesianToBoundLocalMatrix localCartesianToBoundLocalDerivative( + const GeometryContext& gctx, const Vector3D& position) const final; + protected: std::shared_ptr m_bounds; //!< bounds (shared) diff --git a/Core/include/Acts/Surfaces/DiscSurface.hpp b/Core/include/Acts/Surfaces/DiscSurface.hpp index 78e3679bcb1..a113a4fba74 100644 --- a/Core/include/Acts/Surfaces/DiscSurface.hpp +++ b/Core/include/Acts/Surfaces/DiscSurface.hpp @@ -310,6 +310,17 @@ class DiscSurface : public Surface { Polyhedron polyhedronRepresentation(const GeometryContext& gctx, size_t lseg) const override; + /// Calculate the derivative of bound track parameters local position w.r.t. + /// position in local 3D Cartesian coordinates + /// + /// @param gctx The current geometry context object, e.g. alignment + /// @param position The position of the paramters in global + /// + /// @return Derivative of bound local position w.r.t. position in local 3D + /// cartesian coordinates + const LocalCartesianToBoundLocalMatrix localCartesianToBoundLocalDerivative( + const GeometryContext& gctx, const Vector3D& position) const final; + protected: std::shared_ptr m_bounds; ///< bounds (shared) }; diff --git a/Core/include/Acts/Surfaces/LineSurface.hpp b/Core/include/Acts/Surfaces/LineSurface.hpp index 514d98ac052..1438133d2b2 100644 --- a/Core/include/Acts/Surfaces/LineSurface.hpp +++ b/Core/include/Acts/Surfaces/LineSurface.hpp @@ -257,6 +257,32 @@ class LineSurface : public Surface { /// Return properly formatted class name for screen output */ std::string name() const override; + /// Calculate the derivative of path length w.r.t. alignment parameters of the + /// surface (i.e. local frame origin in global 3D Cartesian coordinates and + /// its rotation represented with extrinsic Euler angles) + /// + /// @param gctx The current geometry context object, e.g. alignment + /// @param rotToLocalZAxis The derivative of local frame z axis vector w.r.t. + /// its rotation + /// @param position The position of the paramters in global + /// @param direction The direction of the track + /// + /// @return Derivative of path length w.r.t. the alignment parameters + const AlignmentRowVector alignmentToPathDerivative( + const GeometryContext& gctx, const RotationMatrix3D& rotToLocalZAxis, + const Vector3D& position, const Vector3D& direction) const final; + + /// Calculate the derivative of bound track parameters local position w.r.t. + /// position in local 3D Cartesian coordinates + /// + /// @param gctx The current geometry context object, e.g. alignment + /// @param position The position of the paramters in global + /// + /// @return Derivative of bound local position w.r.t. position in local 3D + /// cartesian coordinates + const LocalCartesianToBoundLocalMatrix localCartesianToBoundLocalDerivative( + const GeometryContext& gctx, const Vector3D& position) const final; + protected: std::shared_ptr m_bounds; ///< bounds (shared) diff --git a/Core/include/Acts/Surfaces/PlaneSurface.hpp b/Core/include/Acts/Surfaces/PlaneSurface.hpp index c7bd2c903aa..623b70679db 100644 --- a/Core/include/Acts/Surfaces/PlaneSurface.hpp +++ b/Core/include/Acts/Surfaces/PlaneSurface.hpp @@ -195,6 +195,17 @@ class PlaneSurface : public Surface { /// Return properly formatted class name for screen output std::string name() const override; + /// Calculate the derivative of bound track parameters local position w.r.t. + /// position in local 3D Cartesian coordinates + /// + /// @param gctx The current geometry context object, e.g. alignment + /// @param position The position of the paramters in global + /// + /// @return Derivative of bound local position w.r.t. position in local 3D + /// cartesian coordinates + const LocalCartesianToBoundLocalMatrix localCartesianToBoundLocalDerivative( + const GeometryContext& gctx, const Vector3D& position) const final; + protected: /// the bounds of this surface std::shared_ptr m_bounds; diff --git a/Core/include/Acts/Surfaces/Surface.hpp b/Core/include/Acts/Surfaces/Surface.hpp index 0cb2e5a9a91..0eb3dbfe0be 100644 --- a/Core/include/Acts/Surfaces/Surface.hpp +++ b/Core/include/Acts/Surfaces/Surface.hpp @@ -15,6 +15,8 @@ #include "Acts/Geometry/Polyhedron.hpp" #include "Acts/Surfaces/BoundaryCheck.hpp" #include "Acts/Surfaces/SurfaceBounds.hpp" +#include "Acts/Surfaces/detail/AlignmentHelper.hpp" +#include "Acts/Utilities/AlignmentDefinitions.hpp" #include "Acts/Utilities/BinnedArray.hpp" #include "Acts/Utilities/BinningType.hpp" #include "Acts/Utilities/Definitions.hpp" @@ -258,6 +260,24 @@ class Surface : public virtual GeometryObject, const Vector3D& momentum, const BoundaryCheck& bcheck = true) const; + /// The derivative of bound track parameters w.r.t. alignment + /// parameters of its reference surface (i.e. local frame origin in + /// global 3D Cartesian coordinates and its rotation represented with + /// extrinsic Euler angles) + /// + /// @param gctx The current geometry context object, e.g. alignment + /// @param derivatives Path length derivatives of the free, nominal + /// parameters to help evaluate change of free track parameters caused by + /// change of alignment parameters + /// @param position The position of the paramters in global + /// @param direction The direction of the track + /// + /// @return Derivative of bound track parameters w.r.t. local frame + /// alignment parameters + const AlignmentToBoundMatrix alignmentToBoundDerivative( + const GeometryContext& gctx, const FreeVector& derivatives, + const Vector3D& position, const Vector3D& direction) const; + /// The insideBounds method for local positions /// /// @param lposition The local position to check @@ -446,6 +466,36 @@ class Surface : public virtual GeometryObject, virtual Polyhedron polyhedronRepresentation(const GeometryContext& gctx, size_t lseg) const = 0; + /// Calculate the derivative of path length w.r.t. alignment parameters of the + /// surface (i.e. local frame origin in global 3D Cartesian coordinates and + /// its rotation represented with extrinsic Euler angles) + /// + /// Re-implementation is needed for surface whose intersection with track is + /// not its local xy plane, e.g. LineSurface, CylinderSurface and ConeSurface + /// + /// @param gctx The current geometry context object, e.g. alignment + /// @param rotToLocalZAxis The derivative of local frame z axis vector w.r.t. + /// its rotation + /// @param position The position of the paramters in global + /// @param direction The direction of the track + /// + /// @return Derivative of path length w.r.t. the alignment parameters + virtual const AlignmentRowVector alignmentToPathDerivative( + const GeometryContext& gctx, const RotationMatrix3D& rotToLocalZAxis, + const Vector3D& position, const Vector3D& direction) const; + + /// Calculate the derivative of bound track parameters local position w.r.t. + /// position in local 3D Cartesian coordinates + /// + /// @param gctx The current geometry context object, e.g. alignment + /// @param position The position of the paramters in global + /// + /// @return Derivative of bound local position w.r.t. position in local 3D + /// cartesian coordinates + virtual const LocalCartesianToBoundLocalMatrix + localCartesianToBoundLocalDerivative(const GeometryContext& gctx, + const Vector3D& position) const = 0; + protected: /// Transform3D definition that positions /// (translation, rotation) the surface in global space diff --git a/Core/include/Acts/Surfaces/detail/AlignmentHelper.hpp b/Core/include/Acts/Surfaces/detail/AlignmentHelper.hpp new file mode 100644 index 00000000000..c280da2921b --- /dev/null +++ b/Core/include/Acts/Surfaces/detail/AlignmentHelper.hpp @@ -0,0 +1,37 @@ +// This file is part of the Acts project. +// +// Copyright (C) 2020 CERN for the benefit of the Acts project +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#pragma once + +#include +#include +#include "Acts/Utilities/Definitions.hpp" + +namespace Acts { + +namespace detail { + +// The container for derivative of local frame axis w.r.t. its +// rotation parameters. The first element is for x axis, second for y axis and +// last for z axis +using RotationToAxes = + std::tuple; + +/// @brief Evaluate the derivative of local frame axes vector w.r.t. +/// its rotation around global x/y/z axis +/// @Todo: add parameter for rotation axis order +/// +/// @param rotation The rotation that help place the surface +/// +/// @return Derivative of local frame x/y/z axis vector w.r.t. its +/// rotation angles (extrinsic Euler angles) around global x/y/z axis +RotationToAxes rotationToLocalAxesDerivative(const RotationMatrix3D& rotation); + +} // namespace detail + +} // namespace Acts diff --git a/Core/include/Acts/Surfaces/detail/ConeSurface.ipp b/Core/include/Acts/Surfaces/detail/ConeSurface.ipp index 61c1a24fe25..b722ccde7dc 100644 --- a/Core/include/Acts/Surfaces/detail/ConeSurface.ipp +++ b/Core/include/Acts/Surfaces/detail/ConeSurface.ipp @@ -115,3 +115,26 @@ inline SurfaceIntersection ConeSurface::intersect( } return cIntersection; } + +inline const LocalCartesianToBoundLocalMatrix +ConeSurface::localCartesianToBoundLocalDerivative( + const GeometryContext& gctx, const Vector3D& position) const { + using VectorHelpers::perp; + using VectorHelpers::phi; + // The local frame transform + const auto& sTransform = transform(gctx); + // calculate the transformation to local coorinates + const Vector3D localPos = sTransform.inverse() * position; + const double lr = perp(localPos); + const double lphi = phi(localPos); + const double lcphi = std::cos(lphi); + const double lsphi = std::sin(lphi); + // Solve for radius R + const double R = localPos.z() * bounds().tanAlpha(); + LocalCartesianToBoundLocalMatrix loc3DToLocBound = + LocalCartesianToBoundLocalMatrix::Zero(); + loc3DToLocBound << -R * lsphi / lr, R * lcphi / lr, + lphi * bounds().tanAlpha(), 0, 0, 1; + + return loc3DToLocBound; +} diff --git a/Core/include/Acts/Surfaces/detail/CylinderSurface.ipp b/Core/include/Acts/Surfaces/detail/CylinderSurface.ipp index 96de47c491b..3e30a76cc3a 100644 --- a/Core/include/Acts/Surfaces/detail/CylinderSurface.ipp +++ b/Core/include/Acts/Surfaces/detail/CylinderSurface.ipp @@ -141,3 +141,25 @@ inline SurfaceIntersection CylinderSurface::intersect( } return cIntersection; } + +inline const LocalCartesianToBoundLocalMatrix +CylinderSurface::localCartesianToBoundLocalDerivative( + const GeometryContext& gctx, const Vector3D& position) const { + using VectorHelpers::perp; + using VectorHelpers::phi; + // The local frame transform + const auto& sTransform = transform(gctx); + // calculate the transformation to local coorinates + const Vector3D localPos = sTransform.inverse() * position; + const double lr = perp(localPos); + const double lphi = phi(localPos); + const double lcphi = std::cos(lphi); + const double lsphi = std::sin(lphi); + // Solve for radius R + double R = bounds().get(CylinderBounds::eR); + LocalCartesianToBoundLocalMatrix loc3DToLocBound = + LocalCartesianToBoundLocalMatrix::Zero(); + loc3DToLocBound << -R * lsphi / lr, R * lcphi / lr, 0, 0, 0, 1; + + return loc3DToLocBound; +} diff --git a/Core/include/Acts/Surfaces/detail/DiscSurface.ipp b/Core/include/Acts/Surfaces/detail/DiscSurface.ipp index 89de4acee8a..fb7235b1723 100644 --- a/Core/include/Acts/Surfaces/detail/DiscSurface.ipp +++ b/Core/include/Acts/Surfaces/detail/DiscSurface.ipp @@ -137,6 +137,26 @@ inline Intersection DiscSurface::intersectionEstimate( return intersection; } +inline const LocalCartesianToBoundLocalMatrix +DiscSurface::localCartesianToBoundLocalDerivative( + const GeometryContext& gctx, const Vector3D& position) const { + using VectorHelpers::perp; + using VectorHelpers::phi; + // The local frame transform + const auto& sTransform = transform(gctx); + // calculate the transformation to local coorinates + const Vector3D localPos = sTransform.inverse() * position; + const double lr = perp(localPos); + const double lphi = phi(localPos); + const double lcphi = std::cos(lphi); + const double lsphi = std::sin(lphi); + LocalCartesianToBoundLocalMatrix loc3DToLocBound = + LocalCartesianToBoundLocalMatrix::Zero(); + loc3DToLocBound << lcphi, lsphi, 0, -lsphi / lr, lcphi / lr, 0; + + return loc3DToLocBound; +} + inline const Vector3D DiscSurface::normal(const GeometryContext& gctx, const Vector2D& /*unused*/) const { // fast access via tranform matrix (and not rotation()) @@ -168,4 +188,4 @@ inline double DiscSurface::pathCorrection(const GeometryContext& gctx, const Vector3D& direction) const { /// we can ignore the global position here return 1. / std::abs(Surface::normal(gctx, position).dot(direction)); -} \ No newline at end of file +} diff --git a/Core/include/Acts/Surfaces/detail/LineSurface.ipp b/Core/include/Acts/Surfaces/detail/LineSurface.ipp index 1f5b0247b8e..03da4079fc9 100644 --- a/Core/include/Acts/Surfaces/detail/LineSurface.ipp +++ b/Core/include/Acts/Surfaces/detail/LineSurface.ipp @@ -210,3 +210,50 @@ inline const BoundRowVector LineSurface::derivativeFactors( (s_vec - pc * (long_mat * d_vec.asDiagonal() - jacobian.block<3, eBoundParametersSize>(4, 0)))); } + +inline const AlignmentRowVector LineSurface::alignmentToPathDerivative( + const GeometryContext& gctx, const RotationMatrix3D& rotToLocalZAxis, + const Vector3D& position, const Vector3D& direction) const { + // The vector between position and center + const ActsRowVector pcRowVec = + (position - center(gctx)).transpose(); + // The local frame transform + const auto& sTransform = transform(gctx); + const auto& rotation = sTransform.rotation(); + // The local frame z axis + const Vector3D localZAxis = rotation.col(2); + // The local z coordinate + const double localZ = pcRowVec * localZAxis; + + // Cosine of angle between momentum direction and local frame z axis + const double dirZ = localZAxis.dot(direction); + const double norm = 1. / (1. - dirZ * dirZ); + // Initialize the derivative of propagation path w.r.t. local frame + // translation (origin) and rotation + AlignmentRowVector alignToPath = AlignmentRowVector::Zero(); + alignToPath.segment<3>(eAlignmentCenter0) = + norm * (direction.transpose() - dirZ * localZAxis.transpose()); + alignToPath.segment<3>(eAlignmentRotation0) = + norm * (dirZ * pcRowVec + localZ * direction.transpose()) * + rotToLocalZAxis; + + return alignToPath; +} + +inline const LocalCartesianToBoundLocalMatrix +LineSurface::localCartesianToBoundLocalDerivative( + const GeometryContext& gctx, const Vector3D& position) const { + using VectorHelpers::phi; + // The local frame transform + const auto& sTransform = transform(gctx); + // calculate the transformation to local coorinates + const Vector3D localPos = sTransform.inverse() * position; + const double lphi = phi(localPos); + const double lcphi = std::cos(lphi); + const double lsphi = std::sin(lphi); + LocalCartesianToBoundLocalMatrix loc3DToLocBound = + LocalCartesianToBoundLocalMatrix::Zero(); + loc3DToLocBound << lcphi, lsphi, 0, 0, 0, 1; + + return loc3DToLocBound; +} diff --git a/Core/include/Acts/Surfaces/detail/PlaneSurface.ipp b/Core/include/Acts/Surfaces/detail/PlaneSurface.ipp index 05fd2aa3b4b..49ec1bd3ff0 100644 --- a/Core/include/Acts/Surfaces/detail/PlaneSurface.ipp +++ b/Core/include/Acts/Surfaces/detail/PlaneSurface.ipp @@ -46,3 +46,11 @@ inline Intersection PlaneSurface::intersectionEstimate( } return intersection; } + +inline const LocalCartesianToBoundLocalMatrix +PlaneSurface::localCartesianToBoundLocalDerivative( + const GeometryContext& /*unused*/, const Vector3D& /*unused*/) const { + const LocalCartesianToBoundLocalMatrix loc3DToLocBound = + LocalCartesianToBoundLocalMatrix::Identity(); + return loc3DToLocBound; +} diff --git a/Core/include/Acts/Surfaces/detail/Surface.ipp b/Core/include/Acts/Surfaces/detail/Surface.ipp index 2703fa423d7..b3478e90327 100644 --- a/Core/include/Acts/Surfaces/detail/Surface.ipp +++ b/Core/include/Acts/Surfaces/detail/Surface.ipp @@ -141,4 +141,4 @@ inline void Surface::assignSurfaceMaterial( inline void Surface::associateLayer(const Layer& lay) { m_associatedLayer = (&lay); -} \ No newline at end of file +} diff --git a/Core/include/Acts/Utilities/AlignmentDefinitions.hpp b/Core/include/Acts/Utilities/AlignmentDefinitions.hpp new file mode 100644 index 00000000000..1ff04061cb3 --- /dev/null +++ b/Core/include/Acts/Utilities/AlignmentDefinitions.hpp @@ -0,0 +1,53 @@ +// This file is part of the Acts project. +// +// Copyright (C) 2016-2020 CERN for the benefit of the Acts project +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#pragma once + +#include +#include + +#include "Acts/Utilities/ParameterDefinitions.hpp" + +namespace Acts { + +/// Components of alignment parameters vector. +/// +/// To be used to access components by named indices instead of just numbers. +/// This must be a regular `enum` and not a scoped `enum class` to allow +/// implicit conversion to an integer. The enum value are thus visible directly +/// in `namespace Acts` and are prefixed to avoid naming collisions. +enum AlignmentParametersIndices : unsigned int { + // Center of geometry object in global 3D cartesian coordinates + eAlignmentCenter0 = 0u, + eAlignmentCenter1 = eAlignmentCenter0 + 1u, + eAlignmentCenter2 = eAlignmentCenter0 + 2u, + // Rotation angle around global x/y/z axis of geometry object + eAlignmentRotation0 = 3u, + eAlignmentRotation1 = eAlignmentRotation0 + 1u, + eAlignmentRotation2 = eAlignmentRotation0 + 2u, + // Last uninitialized value contains the total number of components + eAlignmentParametersSize, +}; + +/// Underlying fundamental scalar type for alignment parameters. +using AlignmentParametersScalar = double; + +// Matrix and vector types related to alignment parameters. +using AlignmentVector = + ActsVector; +using AlignmentRowVector = + ActsRowVector; +using AlingmentMatrix = + ActsMatrix; +using AlignmentToLocalCartesianMatrix = + ActsMatrix; +using AlignmentToBoundMatrix = + ActsMatrix; +} // namespace Acts diff --git a/Core/include/Acts/Utilities/ParameterDefinitions.hpp b/Core/include/Acts/Utilities/ParameterDefinitions.hpp index 66e78cea93e..309be64c080 100644 --- a/Core/include/Acts/Utilities/ParameterDefinitions.hpp +++ b/Core/include/Acts/Utilities/ParameterDefinitions.hpp @@ -256,6 +256,9 @@ using BoundMatrix = ActsMatrix; +using LocalCartesianToBoundLocalMatrix = + ActsMatrix; + // Matrix and vector types related to free track parameters. using FreeVector = ActsVector; diff --git a/Core/src/Surfaces/CMakeLists.txt b/Core/src/Surfaces/CMakeLists.txt index c5f8771d889..475d6d27cd3 100644 --- a/Core/src/Surfaces/CMakeLists.txt +++ b/Core/src/Surfaces/CMakeLists.txt @@ -21,4 +21,5 @@ target_sources_local( Surface.cpp SurfaceArray.cpp TrapezoidBounds.cpp + detail/AlignmentHelper.cpp ) diff --git a/Core/src/Surfaces/Surface.cpp b/Core/src/Surfaces/Surface.cpp index c35580c6ae9..88e70f5b751 100644 --- a/Core/src/Surfaces/Surface.cpp +++ b/Core/src/Surfaces/Surface.cpp @@ -7,6 +7,7 @@ // file, You can obtain one at http://mozilla.org/MPL/2.0/. #include "Acts/Surfaces/Surface.hpp" +#include "Acts/Surfaces/detail/AlignmentHelper.hpp" #include #include @@ -51,6 +52,88 @@ bool Acts::Surface::isOnSurface(const GeometryContext& gctx, return false; } +const Acts::AlignmentToBoundMatrix Acts::Surface::alignmentToBoundDerivative( + const GeometryContext& gctx, const FreeVector& derivatives, + const Vector3D& position, const Vector3D& direction) const { + // The vector between position and center + const ActsRowVector pcRowVec = + (position - center(gctx)).transpose(); + // The local frame rotation + const auto& rotation = transform(gctx).rotation(); + // The axes of local frame + const Vector3D localXAxis = rotation.col(0); + const Vector3D localYAxis = rotation.col(1); + const Vector3D localZAxis = rotation.col(2); + + // 1) Calcuate the derivative of local frame axes w.r.t its rotation + const auto& [rotToLocalXAxis, rotToLocalYAxis, rotToLocalZAxis] = + detail::rotationToLocalAxesDerivative(rotation); + // 2) Calculate the derivative of local 3D Cartesian coordinates w.r.t. + // alignment parameters (without path correction) + AlignmentToLocalCartesianMatrix alignToLoc3D = + AlignmentToLocalCartesianMatrix::Zero(); + alignToLoc3D.block<1, 3>(eX, eAlignmentCenter0) = -localXAxis.transpose(); + alignToLoc3D.block<1, 3>(eY, eAlignmentCenter0) = -localYAxis.transpose(); + alignToLoc3D.block<1, 3>(eZ, eAlignmentCenter0) = -localZAxis.transpose(); + alignToLoc3D.block<1, 3>(eX, eAlignmentRotation0) = + pcRowVec * rotToLocalXAxis; + alignToLoc3D.block<1, 3>(eY, eAlignmentRotation0) = + pcRowVec * rotToLocalYAxis; + alignToLoc3D.block<1, 3>(eZ, eAlignmentRotation0) = + pcRowVec * rotToLocalZAxis; + // 3) Calculate the derivative of track position represented in + // (local) bound track parameters (could be in non-Cartesian coordinates) + // w.r.t. track position represented in local 3D Cartesian coordinates. + const auto& loc3DToLocBound = + localCartesianToBoundLocalDerivative(gctx, position); + // 4) Calculate the derivative of path length w.r.t. alignment parameters + const auto& alignToPath = + alignmentToPathDerivative(gctx, rotToLocalZAxis, position, direction); + // 5) Calculate the jacobian from free parameters to bound parameters + FreeToBoundMatrix jacToLocal = FreeToBoundMatrix::Zero(); + initJacobianToLocal(gctx, jacToLocal, position, direction); + // 6) Initialize the derivative of bound parameters w.r.t. alignment + // parameters + AlignmentToBoundMatrix alignToBound = AlignmentToBoundMatrix::Zero(); + // -> For bound track parameters eLOC_0, eLOC_1, it's + // loc3DToLocBound*alignToLoc3D + + // jacToLocal*derivatives*alignToPath + alignToBound.block<2, eAlignmentParametersSize>(eLOC_0, eAlignmentCenter0) = + loc3DToLocBound * alignToLoc3D + + jacToLocal.block<2, eFreeParametersSize>(eLOC_0, eFreePos0) * + derivatives * alignToPath; + // -> For bound track parameters ePHI, eTHETA, eQOP, eT, it's + // jacToLocal*derivatives*alignToPath + alignToBound.block<4, eAlignmentParametersSize>(ePHI, eAlignmentCenter0) = + jacToLocal.block<4, eFreeParametersSize>(ePHI, eFreePos0) * derivatives * + alignToPath; + + return alignToBound; +} + +const Acts::AlignmentRowVector Acts::Surface::alignmentToPathDerivative( + const GeometryContext& gctx, const RotationMatrix3D& rotToLocalZAxis, + const Vector3D& position, const Vector3D& direction) const { + // The vector between position and center + const ActsRowVector pcRowVec = + (position - center(gctx)).transpose(); + // The local frame rotation + const auto& rotation = transform(gctx).rotation(); + // The local frame z axis + const Vector3D localZAxis = rotation.col(2); + + // Cosine of angle between momentum direction and local frame z axis + const double dirZ = localZAxis.dot(direction); + // Initialize the derivative of propagation path w.r.t. local frame + // translation (origin) and rotation + AlignmentRowVector alignToPath = AlignmentRowVector::Zero(); + alignToPath.segment<3>(eAlignmentCenter0) = localZAxis.transpose() / dirZ; + alignToPath.segment<3>(eAlignmentRotation0) = + -pcRowVec * rotToLocalZAxis / dirZ; + + return alignToPath; +} + std::shared_ptr Acts::Surface::getSharedPtr() { return shared_from_this(); } diff --git a/Core/src/Surfaces/detail/AlignmentHelper.cpp b/Core/src/Surfaces/detail/AlignmentHelper.cpp new file mode 100644 index 00000000000..733dc7aa7a6 --- /dev/null +++ b/Core/src/Surfaces/detail/AlignmentHelper.cpp @@ -0,0 +1,50 @@ +// This file is part of the Acts project. +// +// Copyright (C) 2020 CERN for the benefit of the Acts project +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include "Acts/Surfaces/detail/AlignmentHelper.hpp" + +Acts::detail::RotationToAxes Acts::detail::rotationToLocalAxesDerivative( + const RotationMatrix3D& rotation) { + // Get Euler angles for rotation representated by rotZ * rotY * rotX, i.e. + // first rotation around x axis, then y axis, last z axis + // The elements stored in rotAngles is (rotZ, rotY, rotX) + const Vector3D rotAngles = rotation.eulerAngles(2, 1, 0); + double sx = std::sin(rotAngles(2)); + double cx = std::cos(rotAngles(2)); + double sy = std::sin(rotAngles(1)); + double cy = std::cos(rotAngles(1)); + double sz = std::sin(rotAngles(0)); + double cz = std::cos(rotAngles(0)); + // rotZ * rotY * rotX = + // [ cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*sy ] + // [ cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx ] + // [ -sy cy*sx cy*cx ] + + // Derivative of local x axis w.r.t. (rotX, rotY, rotZ) + RotationMatrix3D rotToLocalXAxis = RotationMatrix3D::Zero(); + rotToLocalXAxis.col(0) = Vector3D(0, 0, 0); + rotToLocalXAxis.col(1) = Vector3D(-cz * sy, -sz * sy, -cy); + rotToLocalXAxis.col(2) = Vector3D(-sz * cy, cz * cy, 0); + // Derivative of local y axis w.r.t. (rotX, rotY, rotZ) + RotationMatrix3D rotToLocalYAxis = RotationMatrix3D::Zero(); + rotToLocalYAxis.col(0) = + Vector3D(cz * sy * cx + sz * sx, sz * sy * cx - cz * sx, cy * cx); + rotToLocalYAxis.col(1) = Vector3D(cz * cy * sx, sz * cy * sx, -sy * sx); + rotToLocalYAxis.col(2) = + Vector3D(-sz * sy * sx - cz * cx, cz * sy * sx - sz * cx, 0); + // Derivative of local z axis w.r.t. (rotX, rotY, rotZ) + RotationMatrix3D rotToLocalZAxis = RotationMatrix3D::Zero(); + rotToLocalZAxis.col(0) = + Vector3D(sz * cx - cz * sy * sx, -sz * sy * sx - cz * cx, -cy * sx); + rotToLocalZAxis.col(1) = Vector3D(cz * cy * cx, sz * cy * cx, -sy * cx); + rotToLocalZAxis.col(2) = + Vector3D(cz * sx - sz * sy * cx, cz * sy * cx + sz * sx, 0); + + return std::make_tuple(std::move(rotToLocalXAxis), std::move(rotToLocalYAxis), + std::move(rotToLocalZAxis)); +} diff --git a/Tests/UnitTests/Core/Fitter/KalmanFitterTests.cpp b/Tests/UnitTests/Core/Fitter/KalmanFitterTests.cpp index b3d10acb361..5d7bae343a8 100644 --- a/Tests/UnitTests/Core/Fitter/KalmanFitterTests.cpp +++ b/Tests/UnitTests/Core/Fitter/KalmanFitterTests.cpp @@ -21,6 +21,7 @@ #include "Acts/Fitter/GainMatrixSmoother.hpp" #include "Acts/Fitter/GainMatrixUpdater.hpp" #include "Acts/Fitter/KalmanFitter.hpp" +#include "Acts/Fitter/detail/KalmanGlobalCovariance.hpp" #include "Acts/Geometry/GeometryContext.hpp" #include "Acts/Geometry/GeometryID.hpp" #include "Acts/Geometry/TrackingGeometry.hpp" @@ -380,6 +381,16 @@ BOOST_AUTO_TEST_CASE(kalman_fitter_zero_field) { auto& fittedTrack = *fitRes; auto fittedParameters = fittedTrack.fittedParameters.value(); + // Calculate global track parameters covariance matrix + const auto& [trackParamsCov, stateRowIndices] = + detail::globalTrackParametersCovariance(fittedTrack.fittedStates, + fittedTrack.trackTip); + + // Check the size of the global track parameters size + BOOST_CHECK_EQUAL(stateRowIndices.size(), 6); + BOOST_CHECK_EQUAL(stateRowIndices.at(fittedTrack.trackTip), 30); + BOOST_CHECK_EQUAL(trackParamsCov.rows(), 6 * eBoundParametersSize); + // Make sure it is deterministic fitRes = kFitter.fit(sourcelinks, rStart, kfOptions); BOOST_CHECK(fitRes.ok()); @@ -420,6 +431,17 @@ BOOST_AUTO_TEST_CASE(kalman_fitter_zero_field) { auto& fittedWithHoleTrack = *fitRes; auto fittedWithHoleParameters = fittedWithHoleTrack.fittedParameters.value(); + // Calculate global track parameters covariance matrix + const auto& [holeTrackTrackParamsCov, holeTrackStateRowIndices] = + detail::globalTrackParametersCovariance(fittedWithHoleTrack.fittedStates, + fittedWithHoleTrack.trackTip); + + // Check the size of the global track parameters size + BOOST_CHECK_EQUAL(holeTrackStateRowIndices.size(), 6); + BOOST_CHECK_EQUAL(holeTrackStateRowIndices.at(fittedWithHoleTrack.trackTip), + 30); + BOOST_CHECK_EQUAL(holeTrackTrackParamsCov.rows(), 6 * eBoundParametersSize); + // Count one hole BOOST_CHECK_EQUAL(fittedWithHoleTrack.missedActiveSurfaces.size(), 1u); // And the parameters should be different diff --git a/Tests/UnitTests/Core/Surfaces/AlignmentHelperTests.cpp b/Tests/UnitTests/Core/Surfaces/AlignmentHelperTests.cpp new file mode 100644 index 00000000000..163496cc146 --- /dev/null +++ b/Tests/UnitTests/Core/Surfaces/AlignmentHelperTests.cpp @@ -0,0 +1,124 @@ +// This file is part of the Acts project. +// +// Copyright (C) 2020 CERN for the benefit of the Acts project +// +// This Source Code Form is subject to the terms of the Mozilla Public +// License, v. 2.0. If a copy of the MPL was not distributed with this +// file, You can obtain one at http://mozilla.org/MPL/2.0/. + +#include +#include + +#include "Acts/Surfaces/detail/AlignmentHelper.hpp" +#include "Acts/Tests/CommonHelpers/FloatComparisons.hpp" +#include "Acts/Utilities/Definitions.hpp" + +namespace Acts { +namespace Test { + +/// Test for rotation matrix and calculation of derivative of rotated x/y/z axis +/// w.r.t. rotation parameters +BOOST_AUTO_TEST_CASE(alignment_helper_test) { + // (a) Test with non-identity rotation matrix + // Rotation angle parameters + const double alpha = M_PI; + const double beta = 0; + const double gamma = M_PI / 2; + // rotation around x axis + AngleAxis3D rotX(alpha, Vector3D(1., 0., 0.)); + // rotation around y axis + AngleAxis3D rotY(beta, Vector3D(0., 1., 0.)); + // rotation around z axis + AngleAxis3D rotZ(gamma, Vector3D(0., 0., 1.)); + double sz = std::sin(gamma); + double cz = std::cos(gamma); + double sy = std::sin(beta); + double cy = std::cos(beta); + double sx = std::sin(alpha); + double cx = std::cos(alpha); + + // Calculate the expected rotation matrix for rotZ * rotY * rotX, + // (i.e. first rotation around x axis, then y axis, last z axis): + // [ cz*cy cz*sy*sx-cx*sz sz*sx+cz*cx*sy ] + // [ cy*sz cz*cx+sz*sy*sx cx*sz*sy-cz*sx ] + // [ -sy cy*sx cy*cx ] + RotationMatrix3D expRot = RotationMatrix3D::Zero(); + expRot.col(0) = Vector3D(cz * cy, cy * sz, -sy); + expRot.col(1) = + Vector3D(cz * sy * sx - cx * sz, cz * cx + sz * sy * sx, cy * sx); + expRot.col(2) = + Vector3D(sz * sx + cz * cx * sy, cx * sz * sy - cz * sx, cy * cx); + + // Calculate the expected derivative of local x axis to its rotation + RotationMatrix3D expRotToXAxis = RotationMatrix3D::Zero(); + expRotToXAxis.col(0) = Vector3D(0, 0, 0); + expRotToXAxis.col(1) = Vector3D(-cz * sy, -sz * sy, -cy); + expRotToXAxis.col(2) = Vector3D(-sz * cy, cz * cy, 0); + + // Calculate the expected derivative of local y axis to its rotation + RotationMatrix3D expRotToYAxis = RotationMatrix3D::Zero(); + expRotToYAxis.col(0) = + Vector3D(cz * sy * cx + sz * sx, sz * sy * cx - cz * sx, cy * cx); + expRotToYAxis.col(1) = Vector3D(cz * cy * sx, sz * cy * sx, -sy * sx); + expRotToYAxis.col(2) = + Vector3D(-sz * sy * sx - cz * cx, cz * sy * sx - sz * cx, 0); + + // Calculate the expected derivative of local z axis to its rotation + RotationMatrix3D expRotToZAxis = RotationMatrix3D::Zero(); + expRotToZAxis.col(0) = + Vector3D(sz * cx - cz * sy * sx, -sz * sy * sx - cz * cx, -cy * sx); + expRotToZAxis.col(1) = Vector3D(cz * cy * cx, sz * cy * cx, -sy * cx); + expRotToZAxis.col(2) = + Vector3D(cz * sx - sz * sy * cx, cz * sy * cx + sz * sx, 0); + + // Construct a transform + Vector3D translation(0., 0., 0.); + auto transform = std::make_shared(Translation3D(translation)); + // Rotation with rotZ * rotY * rotX + (*transform) *= rotZ; + (*transform) *= rotY; + (*transform) *= rotX; + // Get the rotation of the transform + const auto rotation = transform->rotation(); + + // Check if the rotation matrix is as expected + CHECK_CLOSE_ABS(rotation, expRot, 1e-15); + + // Call the alignment helper to calculate the derivative of local frame axes + // w.r.t its rotation + const auto& [rotToLocalXAxis, rotToLocalYAxis, rotToLocalZAxis] = + detail::rotationToLocalAxesDerivative(rotation); + + // Check if the derivative for local x axis is as expected + CHECK_CLOSE_ABS(rotToLocalXAxis, expRotToXAxis, 1e-15); + + // Check if the derivative for local y axis is as expected + CHECK_CLOSE_ABS(rotToLocalYAxis, expRotToYAxis, 1e-15); + + // Check if the derivative for local z axis is as expected + CHECK_CLOSE_ABS(rotToLocalZAxis, expRotToZAxis, 1e-15); + + // (b) Test with identity rotation matrix + RotationMatrix3D iRotation = RotationMatrix3D::Identity(); + + // Call the alignment helper to calculate the derivative of local frame axes + // w.r.t its rotation + const auto& [irotToLocalXAxis, irotToLocalYAxis, irotToLocalZAxis] = + detail::rotationToLocalAxesDerivative(iRotation); + + // The expected derivatives + expRotToXAxis << 0, 0, 0, 0, 0, 1, 0, -1, 0; + expRotToYAxis << 0, 0, -1, 0, 0, 0, 1, 0, 0; + expRotToZAxis << 0, 1, 0, -1, 0, 0, 0, 0, 0; + + // Check if the derivative for local x axis is as expected + CHECK_CLOSE_ABS(irotToLocalXAxis, expRotToXAxis, 1e-15); + + // Check if the derivative for local y axis is as expected + CHECK_CLOSE_ABS(irotToLocalYAxis, expRotToYAxis, 1e-15); + + // Check if the derivative for local z axis is as expected + CHECK_CLOSE_ABS(irotToLocalZAxis, expRotToZAxis, 1e-15); +} +} // namespace Test +} // namespace Acts diff --git a/Tests/UnitTests/Core/Surfaces/CMakeLists.txt b/Tests/UnitTests/Core/Surfaces/CMakeLists.txt index 324b2b9bdca..7c8da0d9c05 100644 --- a/Tests/UnitTests/Core/Surfaces/CMakeLists.txt +++ b/Tests/UnitTests/Core/Surfaces/CMakeLists.txt @@ -23,3 +23,4 @@ add_unittest(SurfaceIntersectionTests SurfaceIntersectionTests.cpp) add_unittest(SurfaceTests SurfaceTests.cpp) add_unittest(TrapezoidBoundsTests TrapezoidBoundsTests.cpp) add_unittest(VerticesHelperTests VerticesHelperTests.cpp) +add_unittest(AlignmentHelperTests AlignmentHelperTests.cpp) diff --git a/Tests/UnitTests/Core/Surfaces/ConeSurfaceTests.cpp b/Tests/UnitTests/Core/Surfaces/ConeSurfaceTests.cpp index 9916e63cb7a..f8d24b9a94a 100644 --- a/Tests/UnitTests/Core/Surfaces/ConeSurfaceTests.cpp +++ b/Tests/UnitTests/Core/Surfaces/ConeSurfaceTests.cpp @@ -238,6 +238,36 @@ BOOST_AUTO_TEST_CASE(ConeSurfaceExtent) { CHECK_CLOSE_ABS(rMax, pConeExtent.max(binR), s_onSurfaceTolerance); } +/// Unit test for testing ConeSurface alignment derivatives +BOOST_AUTO_TEST_CASE(ConeSurfaceAlignment) { + double alpha{M_PI / 8.}; + bool symmetric(false); + Translation3D translation{0., 1., 2.}; + auto pTransform = std::make_shared(translation); + auto coneSurfaceObject = + Surface::makeShared(pTransform, alpha, symmetric); + + const auto& rotation = pTransform->rotation(); + // The local frame z axis + const Vector3D localZAxis = rotation.col(2); + // Check the local z axis is aligned to global z axis + CHECK_CLOSE_ABS(localZAxis, Vector3D(0., 0., 1.), 1e-15); + + /// Define the track (global) position and direction + Vector3D globalPosition{0, 1. + std::tan(alpha), 3}; + + // Test the derivative of bound track parameters local position w.r.t. + // position in local 3D Cartesian coordinates + const auto& loc3DToLocBound = + coneSurfaceObject->localCartesianToBoundLocalDerivative(tgContext, + globalPosition); + // Check if the result is as expected + LocalCartesianToBoundLocalMatrix expLoc3DToLocBound = + LocalCartesianToBoundLocalMatrix::Zero(); + expLoc3DToLocBound << -1, 0, M_PI / 2. * std::tan(alpha), 0, 0, 1; + CHECK_CLOSE_ABS(loc3DToLocBound, expLoc3DToLocBound, 1e-10); +} + BOOST_AUTO_TEST_SUITE_END() } // namespace Test diff --git a/Tests/UnitTests/Core/Surfaces/CylinderSurfaceTests.cpp b/Tests/UnitTests/Core/Surfaces/CylinderSurfaceTests.cpp index 7ac01b1117e..8e6a7d994e8 100644 --- a/Tests/UnitTests/Core/Surfaces/CylinderSurfaceTests.cpp +++ b/Tests/UnitTests/Core/Surfaces/CylinderSurfaceTests.cpp @@ -255,6 +255,35 @@ BOOST_AUTO_TEST_CASE(CylinderSurfaceExtent) { CHECK_CLOSE_ABS(radius, cylinderExtent.max(binY), s_onSurfaceTolerance); } +/// Unit test for testing CylinderSurface alignment derivatives +BOOST_AUTO_TEST_CASE(CylinderSurfaceAlignment) { + double radius(1.0), halfZ(10.); + Translation3D translation{0., 1., 2.}; + auto pTransform = std::make_shared(translation); + auto cylinderSurfaceObject = + Surface::makeShared(pTransform, radius, halfZ); + + const auto& rotation = pTransform->rotation(); + // The local frame z axis + const Vector3D localZAxis = rotation.col(2); + // Check the local z axis is aligned to global z axis + CHECK_CLOSE_ABS(localZAxis, Vector3D(0., 0., 1.), 1e-15); + + /// Define the track (global) position and direction + Vector3D globalPosition{0, 2, 2}; + + // Test the derivative of bound track parameters local position w.r.t. + // position in local 3D Cartesian coordinates + const auto& loc3DToLocBound = + cylinderSurfaceObject->localCartesianToBoundLocalDerivative( + testContext, globalPosition); + // Check if the result is as expected + LocalCartesianToBoundLocalMatrix expLoc3DToLocBound = + LocalCartesianToBoundLocalMatrix::Zero(); + expLoc3DToLocBound << -1, 0, 0, 0, 0, 1; + CHECK_CLOSE_ABS(loc3DToLocBound, expLoc3DToLocBound, 1e-10); +} + BOOST_AUTO_TEST_SUITE_END() } // namespace Test diff --git a/Tests/UnitTests/Core/Surfaces/DiscSurfaceTests.cpp b/Tests/UnitTests/Core/Surfaces/DiscSurfaceTests.cpp index b7f2f2935c1..66e53d5229b 100644 --- a/Tests/UnitTests/Core/Surfaces/DiscSurfaceTests.cpp +++ b/Tests/UnitTests/Core/Surfaces/DiscSurfaceTests.cpp @@ -238,6 +238,53 @@ BOOST_AUTO_TEST_CASE(DiscSurfaceExtent) { CHECK_CLOSE_ABS(rMax, pRingExtent.max(binY), s_onSurfaceTolerance); } +/// Unit test for testing DiscSurface alignment derivatives +BOOST_AUTO_TEST_CASE(DiscSurfaceAlignment) { + Translation3D translation{0., 1., 2.}; + Transform3D transform(translation); + auto pTransform = std::make_shared(translation); + double rMin(1.0), rMax(5.0), halfPhiSector(M_PI / 8.); + auto discSurfaceObject = + Surface::makeShared(pTransform, rMin, rMax, halfPhiSector); + + const auto& rotation = pTransform->rotation(); + // The local frame z axis + const Vector3D localZAxis = rotation.col(2); + // Check the local z axis is aligned to global z axis + CHECK_CLOSE_ABS(localZAxis, Vector3D(0., 0., 1.), 1e-15); + + /// Define the track (global) position and direction + Vector3D globalPosition{0, 4, 2}; + Vector3D momentum{0, 0, 1}; + Vector3D direction = momentum.normalized(); + + // Call the function to calculate the derivative of local frame axes w.r.t its + // rotation + const auto& [rotToLocalXAxis, rotToLocalYAxis, rotToLocalZAxis] = + detail::rotationToLocalAxesDerivative(rotation); + + // (a) Test the derivative of path length w.r.t. alignment parameters + const AlignmentRowVector& alignToPath = + discSurfaceObject->alignmentToPathDerivative(tgContext, rotToLocalZAxis, + globalPosition, direction); + // The expected results + AlignmentRowVector expAlignToPath = AlignmentRowVector::Zero(); + expAlignToPath << 0, 0, 1, 3, 0, 0; + // Check if the calculated derivative is as expected + CHECK_CLOSE_ABS(alignToPath, expAlignToPath, 1e-10); + + // (b) Test the derivative of bound track parameters local position w.r.t. + // position in local 3D Cartesian coordinates + const auto& loc3DToLocBound = + discSurfaceObject->localCartesianToBoundLocalDerivative(tgContext, + globalPosition); + // Check if the result is as expected + LocalCartesianToBoundLocalMatrix expLoc3DToLocBound = + LocalCartesianToBoundLocalMatrix::Zero(); + expLoc3DToLocBound << 0, 1, 0, -1.0 / 3, 0, 0; + CHECK_CLOSE_ABS(loc3DToLocBound, expLoc3DToLocBound, 1e-10); +} + BOOST_AUTO_TEST_SUITE_END() } // namespace Test diff --git a/Tests/UnitTests/Core/Surfaces/LineSurfaceTests.cpp b/Tests/UnitTests/Core/Surfaces/LineSurfaceTests.cpp index b70d4763a83..f1bdd548b4f 100644 --- a/Tests/UnitTests/Core/Surfaces/LineSurfaceTests.cpp +++ b/Tests/UnitTests/Core/Surfaces/LineSurfaceTests.cpp @@ -158,6 +158,50 @@ BOOST_AUTO_TEST_CASE(LineSurface_assignment_test) { BOOST_CHECK(assignedLine == originalLine); // operator == from base } +/// Unit test for testing LineSurface alignment derivatives +BOOST_AUTO_TEST_CASE(LineSurfaceAlignment) { + Translation3D translation{0., 1., 2.}; + Transform3D transform(translation); + auto pTransform = std::make_shared(translation); + LineSurfaceStub line(pTransform, 2.0, 20.); + + const auto& rotation = pTransform->rotation(); + // The local frame z axis + const Vector3D localZAxis = rotation.col(2); + // Check the local z axis is aligned to global z axis + CHECK_CLOSE_ABS(localZAxis, Vector3D(0., 0., 1.), 1e-15); + + /// Define the track (global) position and direction + Vector3D globalPosition{1, 2, 4}; + Vector3D momentum{-1, 1, 1}; + Vector3D direction = momentum.normalized(); + + // Call the function to calculate the derivative of local frame axes w.r.t its + // rotation + const auto& [rotToLocalXAxis, rotToLocalYAxis, rotToLocalZAxis] = + detail::rotationToLocalAxesDerivative(rotation); + + // (a) Test the derivative of path length w.r.t. alignment parameters + const AlignmentRowVector& alignToPath = line.alignmentToPathDerivative( + tgContext, rotToLocalZAxis, globalPosition, direction); + // The expected results + AlignmentRowVector expAlignToPath = AlignmentRowVector::Zero(); + const double value = std::sqrt(3) / 2; + expAlignToPath << -value, value, 0, -3 * value, -value, 0; + // Check if the calculated derivative is as expected + CHECK_CLOSE_ABS(alignToPath, expAlignToPath, 1e-10); + + // (b) Test the derivative of bound track parameters local position w.r.t. + // position in local 3D Cartesian coordinates + const auto& loc3DToLocBound = + line.localCartesianToBoundLocalDerivative(tgContext, globalPosition); + // Check if the result is as expected + LocalCartesianToBoundLocalMatrix expLoc3DToLocBound = + LocalCartesianToBoundLocalMatrix::Zero(); + expLoc3DToLocBound << 1 / std::sqrt(2), 1 / std::sqrt(2), 0, 0, 0, 1; + CHECK_CLOSE_ABS(loc3DToLocBound, expLoc3DToLocBound, 1e-10); +} + BOOST_AUTO_TEST_SUITE_END() } // namespace Test diff --git a/Tests/UnitTests/Core/Surfaces/PlaneSurfaceTests.cpp b/Tests/UnitTests/Core/Surfaces/PlaneSurfaceTests.cpp index 7ff669b5005..19924e6da07 100644 --- a/Tests/UnitTests/Core/Surfaces/PlaneSurfaceTests.cpp +++ b/Tests/UnitTests/Core/Surfaces/PlaneSurfaceTests.cpp @@ -245,6 +245,73 @@ BOOST_AUTO_TEST_CASE(PlaneSurfaceExtent) { s_onSurfaceTolerance); } +/// Unit test for testing PlaneSurface alignment derivatives +BOOST_AUTO_TEST_CASE(PlaneSurfaceAlignment) { + // bounds object, rectangle type + auto rBounds = std::make_shared(3., 4.); + /// Test clone method + Translation3D translation{0., 1., 2.}; + auto pTransform = std::make_shared(translation); + auto planeSurfaceObject = + Surface::makeShared(pTransform, rBounds); + const auto& rotation = pTransform->rotation(); + // The local frame z axis + const Vector3D localZAxis = rotation.col(2); + // Check the local z axis is aligned to global z axis + CHECK_CLOSE_ABS(localZAxis, Vector3D(0., 0., 1.), 1e-15); + + /// Define the track (local) position and direction + Vector2D localPosition{1, 2}; + Vector3D momentum{0, 0, 1}; + Vector3D direction = momentum.normalized(); + /// Get the global position + Vector3D globalPosition{0, 0, 0}; + planeSurfaceObject->localToGlobal(tgContext, localPosition, momentum, + globalPosition); + + // Call the function to calculate the derivative of local frame axes w.r.t its + // rotation + const auto& [rotToLocalXAxis, rotToLocalYAxis, rotToLocalZAxis] = + detail::rotationToLocalAxesDerivative(rotation); + + // (a) Test the derivative of path length w.r.t. alignment parameters + const AlignmentRowVector& alignToPath = + planeSurfaceObject->alignmentToPathDerivative(tgContext, rotToLocalZAxis, + globalPosition, direction); + // The expected results + AlignmentRowVector expAlignToPath = AlignmentRowVector::Zero(); + expAlignToPath << 0, 0, 1, 2, -1, 0; + // Check if the calculated derivative is as expected + CHECK_CLOSE_ABS(alignToPath, expAlignToPath, 1e-10); + + // (b) Test the derivative of bound track parameters local position w.r.t. + // position in local 3D Cartesian coordinates + const auto& loc3DToLocBound = + planeSurfaceObject->localCartesianToBoundLocalDerivative(tgContext, + globalPosition); + // For plane surface, this should be identity matrix + CHECK_CLOSE_ABS(loc3DToLocBound, LocalCartesianToBoundLocalMatrix::Identity(), + 1e-10); + + // (c) Test the derivative of bound parameters (only test loc0, loc1 here) + // w.r.t. alignment parameters + FreeVector derivatives = FreeVector::Zero(); + derivatives.segment<3>(0) = momentum; + const AlignmentToBoundMatrix& alignToBound = + planeSurfaceObject->alignmentToBoundDerivative(tgContext, derivatives, + globalPosition, direction); + const AlignmentRowVector& alignToloc0 = alignToBound.block<1, 6>(0, 0); + const AlignmentRowVector& alignToloc1 = alignToBound.block<1, 6>(1, 0); + // The expected results + AlignmentRowVector expAlignToloc0; + expAlignToloc0 << -1, 0, 0, 0, 0, 2; + AlignmentRowVector expAlignToloc1; + expAlignToloc1 << 0, -1, 0, 0, 0, -1; + // Check if the calculated derivatives are as expected + CHECK_CLOSE_ABS(alignToloc0, expAlignToloc0, 1e-10); + CHECK_CLOSE_ABS(alignToloc1, expAlignToloc1, 1e-10); +} + BOOST_AUTO_TEST_SUITE_END() } // namespace Test diff --git a/Tests/UnitTests/Core/Surfaces/SurfaceStub.hpp b/Tests/UnitTests/Core/Surfaces/SurfaceStub.hpp index 971491c4415..8e708de4d83 100644 --- a/Tests/UnitTests/Core/Surfaces/SurfaceStub.hpp +++ b/Tests/UnitTests/Core/Surfaces/SurfaceStub.hpp @@ -104,6 +104,13 @@ class SurfaceStub : public Surface { return Polyhedron(vertices, faces, triangularMesh); } + // Cartesian 3D to local bound derivative + const LocalCartesianToBoundLocalMatrix localCartesianToBoundLocalDerivative( + const GeometryContext& /*unused*/, + const Vector3D& /*unused*/) const final { + return LocalCartesianToBoundLocalMatrix::Identity(); + }; + private: /// the bounds of this surface std::shared_ptr m_bounds; diff --git a/Tests/UnitTests/Core/Surfaces/SurfaceTests.cpp b/Tests/UnitTests/Core/Surfaces/SurfaceTests.cpp index 5f7bf4a3fab..89a8fc4af10 100644 --- a/Tests/UnitTests/Core/Surfaces/SurfaceTests.cpp +++ b/Tests/UnitTests/Core/Surfaces/SurfaceTests.cpp @@ -146,6 +146,9 @@ BOOST_AUTO_TEST_CASE(EqualityOperators) { Translation3D translation2{1., 1., 2.}; auto pTransform1 = std::make_shared(translation1); auto pTransform2 = std::make_shared(translation2); + // build a planeSurface to be compared + auto planeSurface = + Surface::makeShared(pTransform1, pPlanarBound); auto pLayer = PlaneLayer::create(pTransform1, pPlanarBound); MaterialProperties properties{1., 1., 1., 20., 10, 5.}; auto pMaterial = @@ -158,6 +161,8 @@ BOOST_AUTO_TEST_CASE(EqualityOperators) { SurfaceStub surface2(detElement1); // 1 and 2 are the same SurfaceStub surface3(detElement2); // 3 differs in thickness SurfaceStub surface4(detElement3); // 4 has a different transform and id + SurfaceStub surface5(detElement1); + surface5.assignSurfaceMaterial(pMaterial); // 5 has non-null surface matrial // BOOST_CHECK(surface1 == surface2); // @@ -168,6 +173,14 @@ BOOST_AUTO_TEST_CASE(EqualityOperators) { // BOOST_CHECK_NE(surface1, surface3); // will fail // BOOST_CHECK(surface1 != surface4); + // + BOOST_CHECK(surface1 != surface5); + // + BOOST_CHECK(surface1 != *planeSurface); + // Test the getSharedPtr + const auto surfacePtr = Surface::makeShared(detElement1); + const auto sharedSurfacePtr = surfacePtr->getSharedPtr(); + BOOST_CHECK(*surfacePtr == *sharedSurfacePtr); } BOOST_AUTO_TEST_SUITE_END()