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