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