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