ActsAlignment/Kernel/Alignment.ipp

0001 // This file is part of the ACTS project.
0002 //
0003 // Copyright (C) 2016 CERN for the benefit of the ACTS project
0004 //
0005 // This Source Code Form is subject to the terms of the Mozilla Public
0006 // License, v. 2.0. If a copy of the MPL was not distributed with this
0007 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
0008
0009 #pragma once
0010
0011 #include "ActsAlignment/Kernel/Alignment.hpp"
0012
0013 #include "Acts/EventData/VectorMultiTrajectory.hpp"
0014 #include "Acts/EventData/VectorTrackContainer.hpp"
0015 #include "Acts/TrackFitting/detail/KalmanGlobalCovariance.hpp"
0016 #include "Acts/Utilities/detail/EigenCompat.hpp"
0017 #include "ActsAlignment/Kernel/AlignmentError.hpp"
0018
0019 #include <queue>
0020
0021 template <typename fitter_t>
0022 template <typename source_link_t, typename fit_options_t>
0023 Acts::Result<ActsAlignment::detail::TrackAlignmentState>
0024 ActsAlignment::Alignment<fitter_t>::evaluateTrackAlignmentState(
0025     const Acts::GeometryContext& gctx,
0026     const std::vector<source_link_t>& sourceLinks,
0027     const Acts::BoundTrackParameters& sParameters,
0028     const fit_options_t& fitOptions,
0029     const std::unordered_map<const Acts::Surface*, std::size_t>&
0030         idxedAlignSurfaces,
0031     const ActsAlignment::AlignmentMask& alignMask) const {
0032   Acts::TrackContainer tracks{Acts::VectorTrackContainer{},
0033                               Acts::VectorMultiTrajectory{}};
0034
0035   // Convert to Acts::SourceLink during iteration
0036   Acts::SourceLinkAdapterIterator begin{sourceLinks.begin()};
0037   Acts::SourceLinkAdapterIterator end{sourceLinks.end()};
0038
0039   // Perform the fit
0040   auto fitRes = m_fitter.fit(begin, end, sParameters, fitOptions, tracks);
0041
0042   if (!fitRes.ok()) {
0043     ACTS_WARNING("Fit failure");
0044     return fitRes.error();
0045   }
0046   // The fit results
0047   const auto& track = fitRes.value();
0048   // Calculate the global track parameters covariance with the fitted track
0049   const auto& globalTrackParamsCov =
0050       Acts::detail::globalTrackParametersCovariance(
0051           tracks.trackStateContainer(), track.tipIndex());
0052   // Calculate the alignment state
0053   const auto alignState = detail::trackAlignmentState(
0054       gctx, tracks.trackStateContainer(), track.tipIndex(),
0055       globalTrackParamsCov, idxedAlignSurfaces, alignMask);
0056   if (alignState.alignmentDof == 0) {
0057     ACTS_VERBOSE("No alignment dof on track!");
0058     return AlignmentError::NoAlignmentDofOnTrack;
0059   }
0060   return alignState;
0061 }
0062
0063 template <typename fitter_t>
0064 template <typename trajectory_container_t,
0065           typename start_parameters_container_t, typename fit_options_t>
0066 void ActsAlignment::Alignment<fitter_t>::calculateAlignmentParameters(
0067     const trajectory_container_t& trajectoryCollection,
0068     const start_parameters_container_t& startParametersCollection,
0069     const fit_options_t& fitOptions,
0070     ActsAlignment::AlignmentResult& alignResult,
0071     const ActsAlignment::AlignmentMask& alignMask) const {
0072   // The number of trajectories must be equal to the number of starting
0073   // parameters
0074   assert(trajectoryCollection.size() == startParametersCollection.size());
0075
0076   // The total alignment degree of freedom
0077   alignResult.alignmentDof =
0078       alignResult.idxedAlignSurfaces.size() * Acts::eAlignmentSize;
0079   // Initialize derivative of chi2 w.r.t. alignment parameters for all tracks
0080   Acts::DynamicVector sumChi2Derivative =
0081       Acts::DynamicVector::Zero(alignResult.alignmentDof);
0082   Acts::DynamicMatrix sumChi2SecondDerivative = Acts::DynamicMatrix::Zero(
0083       alignResult.alignmentDof, alignResult.alignmentDof);
0084   // Copy the fit options
0085   fit_options_t fitOptionsWithRefSurface = fitOptions;
0086   // Calculate contribution to chi2 derivatives from all input trajectories
0087   // @Todo: How to update the source link error iteratively?
0088   alignResult.chi2 = 0;
0089   alignResult.measurementDim = 0;
0090   alignResult.numTracks = trajectoryCollection.size();
0091   double sumChi2ONdf = 0;
0092   for (unsigned int iTraj = 0; iTraj < trajectoryCollection.size(); iTraj++) {
0093     const auto& sourceLinks = trajectoryCollection.at(iTraj);
0094     const auto& sParameters = startParametersCollection.at(iTraj);
0095     // Set the target surface
0096     fitOptionsWithRefSurface.referenceSurface = &sParameters.referenceSurface();
0097     // The result for one single track
0098     auto evaluateRes = evaluateTrackAlignmentState(
0099         fitOptions.geoContext, sourceLinks, sParameters,
0100         fitOptionsWithRefSurface, alignResult.idxedAlignSurfaces, alignMask);
0101     if (!evaluateRes.ok()) {
0102       ACTS_DEBUG("Evaluation of alignment state for track " << iTraj
0103                                                             << " failed");
0104       continue;
0105     }
0106     const auto& alignState = evaluateRes.value();
0107     for (const auto& [rowSurface, rows] : alignState.alignedSurfaces) {
0108       const auto& [dstRow, srcRow] = rows;
0109       // Fill the results into full chi2 derivative matrix
0110       sumChi2Derivative.segment<Acts::eAlignmentSize>(dstRow *
0111                                                       Acts::eAlignmentSize) +=
0112           alignState.alignmentToChi2Derivative.segment(
0113               srcRow * Acts::eAlignmentSize, Acts::eAlignmentSize);
0114
0115       for (const auto& [colSurface, cols] : alignState.alignedSurfaces) {
0116         const auto& [dstCol, srcCol] = cols;
0117         sumChi2SecondDerivative
0118             .block<Acts::eAlignmentSize, Acts::eAlignmentSize>(
0119                 dstRow * Acts::eAlignmentSize, dstCol * Acts::eAlignmentSize) +=
0120             alignState.alignmentToChi2SecondDerivative.block(
0121                 srcRow * Acts::eAlignmentSize, srcCol * Acts::eAlignmentSize,
0122                 Acts::eAlignmentSize, Acts::eAlignmentSize);
0123       }
0124     }
0125     alignResult.chi2 += alignState.chi2;
0126     alignResult.measurementDim += alignState.measurementDim;
0127     sumChi2ONdf += alignState.chi2 / alignState.measurementDim;
0128   }
0129   alignResult.averageChi2ONdf = sumChi2ONdf / alignResult.numTracks;
0130
0131   // Get the inverse of chi2 second derivative matrix (we need this to
0132   // calculate the covariance of the alignment parameters)
0133   // @TODO: use more stable method for solving the inverse
0134   std::size_t alignDof = alignResult.alignmentDof;
0135   Acts::DynamicMatrix sumChi2SecondDerivativeInverse =
0136       Acts::DynamicMatrix::Zero(alignDof, alignDof);
0137   sumChi2SecondDerivativeInverse = sumChi2SecondDerivative.inverse();
0138   if (sumChi2SecondDerivativeInverse.hasNaN()) {
0139     ACTS_DEBUG("Chi2 second derivative inverse has NaN");
0140   }
0141
0142   // Initialize the alignment results
0143   alignResult.deltaAlignmentParameters = Acts::DynamicVector::Zero(alignDof);
0144   alignResult.alignmentCovariance =
0145       Acts::DynamicMatrix::Zero(alignDof, alignDof);
0146   // Solve the linear equation to get alignment parameters change
0147   alignResult.deltaAlignmentParameters =
0148       -sumChi2SecondDerivative.fullPivLu().solve(sumChi2Derivative);
0149   ACTS_VERBOSE("sumChi2SecondDerivative = \n" << sumChi2SecondDerivative);
0150   ACTS_VERBOSE("sumChi2Derivative = \n" << sumChi2Derivative);
0151   ACTS_VERBOSE("alignResult.deltaAlignmentParameters \n");
0152
0153   // Alignment parameters covariance
0154   alignResult.alignmentCovariance = 2 * sumChi2SecondDerivativeInverse;
0155   // chi2 change
0156   alignResult.deltaChi2 = 0.5 * sumChi2Derivative.transpose() *
0157                           alignResult.deltaAlignmentParameters;
0158 }
0159
0160 template <typename fitter_t>
0161 Acts::Result<void>
0162 ActsAlignment::Alignment<fitter_t>::updateAlignmentParameters(
0163     const Acts::GeometryContext& gctx,
0164     const std::vector<Acts::SurfacePlacementBase*>& alignedDetElements,
0165     const ActsAlignment::AlignedTransformUpdaterConcept auto&
0166         alignedTransformUpdater,
0167     ActsAlignment::AlignmentResult& alignResult) const {
0168   // Update the aligned transform
0169   Acts::AlignmentVector deltaAlignmentParam = Acts::AlignmentVector::Zero();
0170   for (const auto& [surface, index] : alignResult.idxedAlignSurfaces) {
0171     // 1. The original transform
0172     const Acts::Vector3& oldCenter = surface->center(gctx);
0173     const Acts::Transform3& oldTransform =
0174         surface->localToGlobalTransform(gctx);
0175
0176     // 2. The delta transform
0177     deltaAlignmentParam = alignResult.deltaAlignmentParameters.segment(
0178         Acts::eAlignmentSize * index, Acts::eAlignmentSize);
0179     // The delta translation
0180     Acts::Vector3 deltaCenter =
0181         deltaAlignmentParam.segment<3>(Acts::eAlignmentCenter0);
0182     // The delta Euler angles
0183     Acts::Vector3 deltaEulerAngles =
0184         deltaAlignmentParam.segment<3>(Acts::eAlignmentRotation0);
0185
0186     // 3. The new transform
0187     const Acts::Vector3 newCenter = oldCenter + deltaCenter;
0188     Acts::Transform3 newTransform = oldTransform;
0189     newTransform.translation() = newCenter;
0190     // Rotation first around fixed local x, then around fixed local y, and last
0191     // around fixed local z, this is the same as first around local z, then
0192     // around new loca y, and last around new local x below
0193     newTransform *=
0194         Acts::AngleAxis3(deltaEulerAngles(2), Acts::Vector3::UnitZ());
0195     newTransform *=
0196         Acts::AngleAxis3(deltaEulerAngles(1), Acts::Vector3::UnitY());
0197     newTransform *=
0198         Acts::AngleAxis3(deltaEulerAngles(0), Acts::Vector3::UnitX());
0199
0200     // 4. Update the aligned transform
0201     //@Todo: use a better way to handle this (need dynamic cast to inherited
0202     // detector element type)
0203     ACTS_VERBOSE("Delta of alignment parameters at element "
0204                  << index << "= \n"
0205                  << deltaAlignmentParam);
0206     bool updated = alignedTransformUpdater(alignedDetElements.at(index), gctx,
0207                                            newTransform);
0208     if (!updated) {
0209       ACTS_ERROR("Update alignment parameters for detector element failed");
0210       return AlignmentError::AlignmentParametersUpdateFailure;
0211     }
0212   }
0213
0214   return Acts::Result<void>::success();
0215 }
0216
0217 template <typename fitter_t>
0218 template <typename trajectory_container_t,
0219           typename start_parameters_container_t, typename fit_options_t>
0220 Acts::Result<ActsAlignment::AlignmentResult>
0221 ActsAlignment::Alignment<fitter_t>::align(
0222     const trajectory_container_t& trajectoryCollection,
0223     const start_parameters_container_t& startParametersCollection,
0224     const ActsAlignment::AlignmentOptions<fit_options_t>& alignOptions) const {
0225   // Construct an AlignmentResult object
0226   AlignmentResult alignResult;
0227
0228   // Assign index to the alignable surface
0229   for (unsigned int iDetElement = 0;
0230        iDetElement < alignOptions.alignedDetElements.size(); iDetElement++) {
0231     alignResult.idxedAlignSurfaces.emplace(
0232         &alignOptions.alignedDetElements.at(iDetElement)->surface(),
0233         iDetElement);
0234   }
0235   ACTS_VERBOSE("There are " << alignResult.idxedAlignSurfaces.size()
0236                             << " detector elements to be aligned");
0237
0238   // Start the iteration to minimize the chi2
0239   bool converged = false;
0240   bool alignmentParametersUpdated = false;
0241   std::queue<double> recentChi2ONdf;
0242   ACTS_INFO("Max number of iterations: " << alignOptions.maxIterations);
0243   for (unsigned int iIter = 0; iIter < alignOptions.maxIterations; iIter++) {
0244     // Perform the fit to the trajectories and update alignment parameters
0245     // Initialize the alignment mask (all dof in default)
0246     AlignmentMask alignMask = AlignmentMask::All;
0247     // Set the alignment mask
0248     auto iter_it = alignOptions.iterationState.find(iIter);
0249     if (iter_it != alignOptions.iterationState.end()) {
0250       alignMask = iter_it->second;
0251     }
0252     // Calculate the alignment parameters delta etc.
0253     calculateAlignmentParameters(
0254         trajectoryCollection, startParametersCollection,
0255         alignOptions.fitOptions, alignResult, alignMask);
0256     // Screen out the information
0257     ACTS_INFO("iIter = " << iIter << ", total chi2 = " << alignResult.chi2
0258                          << ", total measurementDim = "
0259                          << alignResult.measurementDim
0260                          << " and average chi2/ndf = "
0261                          << alignResult.averageChi2ONdf);
0262     // Check if it has converged against the provided precision
0263     // 1. either the delta average chi2/ndf in the last few
0264     // iterations is within tolerance
0265     if (recentChi2ONdf.size() >=
0266         alignOptions.deltaAverageChi2ONdfCutOff.first) {
0267       if (std::abs(recentChi2ONdf.front() - alignResult.averageChi2ONdf) <=
0268           alignOptions.deltaAverageChi2ONdfCutOff.second) {
0269         ACTS_INFO(
0270             "Alignment has converged with change of chi2/ndf < "
0271             << alignOptions.deltaAverageChi2ONdfCutOff.second << " in the last "
0272             << alignOptions.deltaAverageChi2ONdfCutOff.first << " iterations"
0273             << " after " << iIter << " iteration(s)");
0274         converged = true;
0275         break;
0276       }
0277       recentChi2ONdf.pop();
0278     }
0279     // 2. or the average chi2/ndf (is this correct?)
0280     if (alignResult.averageChi2ONdf <= alignOptions.averageChi2ONdfCutOff) {
0281       ACTS_INFO("Alignment has converged with average chi2/ndf < "
0282                 << alignOptions.averageChi2ONdfCutOff << " after " << iIter
0283                 << " iteration(s)");
0284       converged = true;
0285       break;
0286     }
0287     // Remove the first element
0288     // Store the result in the queue
0289     recentChi2ONdf.push(alignResult.averageChi2ONdf);
0290
0291     ACTS_INFO("The solved delta of alignmentParameters = \n "
0292               << alignResult.deltaAlignmentParameters);
0293     // Not coveraged yet, update the detector element alignment parameters
0294     auto updateRes = updateAlignmentParameters(
0295         alignOptions.fitOptions.geoContext, alignOptions.alignedDetElements,
0296         alignOptions.alignedTransformUpdater, alignResult);
0297     if (!updateRes.ok()) {
0298       ACTS_ERROR("Update alignment parameters failed: " << updateRes.error());
0299       return updateRes.error();
0300     }
0301     alignmentParametersUpdated = true;
0302   }  // end of all iterations
0303
0304   // Alignment failure if not converged
0305   if (!converged) {
0306     ACTS_ERROR("Alignment is not converged.");
0307     alignResult.result = AlignmentError::ConvergeFailure;
0308   }
0309
0310   // Screen out the final aligned parameters
0311   // @todo
0312   if (alignmentParametersUpdated) {
0313     for (const auto& det : alignOptions.alignedDetElements) {
0314       const auto& surface = &det->surface();
0315       const auto& transform =
0316           det->localToGlobalTransform(alignOptions.fitOptions.geoContext);
0317       // write it to the result
0318       alignResult.alignedParameters.emplace(det, transform);
0319       const auto& translation = transform.translation();
0320       const auto& rotation = transform.rotation();
0321       const Acts::Vector3 rotAngles =
0322           Acts::detail::EigenCompat::canonicalEulerAngles(rotation, 2, 1, 0);
0323       ACTS_VERBOSE("Detector element with surface "
0324                    << surface->geometryId()
0325                    << " has aligned geometry position as below:");
0326       ACTS_VERBOSE("Center (cenX, cenY, cenZ) = " << translation.transpose());
0327       ACTS_VERBOSE(
0328           "Euler angles (rotZ, rotY, rotX) = " << rotAngles.transpose());
0329       ACTS_VERBOSE("Rotation matrix = \n" << rotation);
0330     }
0331   } else {
0332     ACTS_DEBUG("Alignment parameters is not updated.");
0333   }
0334
0335   return alignResult;
0336 }