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