ActsAlignment/Kernel/Alignment.ipp

0001 // This file is part of the Acts project.
0002 //
0003 // Copyright (C) 2020-2021 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 http://mozilla.org/MPL/2.0/.
0008
0009 #include "Acts/EventData/VectorMultiTrajectory.hpp"
0010 #include "Acts/EventData/VectorTrackContainer.hpp"
0011
0012 template <typename fitter_t>
0013 template <typename source_link_t, typename start_parameters_t,
0014           typename fit_options_t>
0015 Acts::Result<ActsAlignment::detail::TrackAlignmentState>
0016 ActsAlignment::Alignment<fitter_t>::evaluateTrackAlignmentState(
0017     const Acts::GeometryContext& gctx,
0018     const std::vector<source_link_t>& sourcelinks,
0019     const start_parameters_t& sParameters, const fit_options_t& fitOptions,
0020     const std::unordered_map<const Acts::Surface*, std::size_t>&
0021         idxedAlignSurfaces,
0022     const ActsAlignment::AlignmentMask& alignMask) const {
0023   Acts::TrackContainer tracks{Acts::VectorTrackContainer{},
0024                               Acts::VectorMultiTrajectory{}};
0025
0026   // Convert to Acts::SourceLink during iteration
0027   Acts::SourceLinkAdapterIterator begin{sourcelinks.begin()};
0028   Acts::SourceLinkAdapterIterator end{sourcelinks.end()};
0029
0030   // Perform the fit
0031   auto fitRes = m_fitter.fit(begin, end, sParameters, fitOptions, tracks);
0032
0033   if (!fitRes.ok()) {
0034     ACTS_WARNING("Fit failure");
0035     return fitRes.error();
0036   }
0037   // The fit results
0038   const auto& track = fitRes.value();
0039   // Calculate the global track parameters covariance with the fitted track
0040   const auto& globalTrackParamsCov =
0041       Acts::detail::globalTrackParametersCovariance(
0042           tracks.trackStateContainer(), track.tipIndex());
0043   // Calculate the alignment state
0044   const auto alignState = detail::trackAlignmentState(
0045       gctx, tracks.trackStateContainer(), track.tipIndex(),
0046       globalTrackParamsCov, idxedAlignSurfaces, alignMask);
0047   if (alignState.alignmentDof == 0) {
0048     ACTS_VERBOSE("No alignment dof on track!");
0049     return AlignmentError::NoAlignmentDofOnTrack;
0050   }
0051   return alignState;
0052 }
0053
0054 template <typename fitter_t>
0055 template <typename trajectory_container_t,
0056           typename start_parameters_container_t, typename fit_options_t>
0057 void ActsAlignment::Alignment<fitter_t>::calculateAlignmentParameters(
0058     const trajectory_container_t& trajectoryCollection,
0059     const start_parameters_container_t& startParametersCollection,
0060     const fit_options_t& fitOptions,
0061     ActsAlignment::AlignmentResult& alignResult,
0062     const ActsAlignment::AlignmentMask& alignMask) const {
0063   // The number of trajectories must be equal to the number of starting
0064   // parameters
0065   assert(trajectoryCollection.size() == startParametersCollection.size());
0066
0067   // The total alignment degree of freedom
0068   alignResult.alignmentDof =
0069       alignResult.idxedAlignSurfaces.size() * Acts::eAlignmentSize;
0070   // Initialize derivative of chi2 w.r.t. alignment parameters for all tracks
0071   Acts::ActsDynamicVector sumChi2Derivative =
0072       Acts::ActsDynamicVector::Zero(alignResult.alignmentDof);
0073   Acts::ActsDynamicMatrix sumChi2SecondDerivative =
0074       Acts::ActsDynamicMatrix::Zero(alignResult.alignmentDof,
0075                                     alignResult.alignmentDof);
0076   // Copy the fit options
0077   fit_options_t fitOptionsWithRefSurface = fitOptions;
0078   // Calculate contribution to chi2 derivatives from all input trajectories
0079   // @Todo: How to update the source link error iteratively?
0080   alignResult.chi2 = 0;
0081   alignResult.measurementDim = 0;
0082   alignResult.numTracks = trajectoryCollection.size();
0083   double sumChi2ONdf = 0;
0084   for (unsigned int iTraj = 0; iTraj < trajectoryCollection.size(); iTraj++) {
0085     const auto& sourcelinks = trajectoryCollection.at(iTraj);
0086     const auto& sParameters = startParametersCollection.at(iTraj);
0087     // Set the target surface
0088     fitOptionsWithRefSurface.referenceSurface = &sParameters.referenceSurface();
0089     // The result for one single track
0090     auto evaluateRes = evaluateTrackAlignmentState(
0091         fitOptions.geoContext, sourcelinks, sParameters,
0092         fitOptionsWithRefSurface, alignResult.idxedAlignSurfaces, alignMask);
0093     if (!evaluateRes.ok()) {
0094       ACTS_DEBUG("Evaluation of alignment state for track " << iTraj
0095                                                             << " failed");
0096       continue;
0097     }
0098     const auto& alignState = evaluateRes.value();
0099     for (const auto& [rowSurface, rows] : alignState.alignedSurfaces) {
0100       const auto& [dstRow, srcRow] = rows;
0101       // Fill the results into full chi2 derivative matrix
0102       sumChi2Derivative.segment<Acts::eAlignmentSize>(dstRow *
0103                                                       Acts::eAlignmentSize) +=
0104           alignState.alignmentToChi2Derivative.segment(
0105               srcRow * Acts::eAlignmentSize, Acts::eAlignmentSize);
0106
0107       for (const auto& [colSurface, cols] : alignState.alignedSurfaces) {
0108         const auto& [dstCol, srcCol] = cols;
0109         sumChi2SecondDerivative
0110             .block<Acts::eAlignmentSize, Acts::eAlignmentSize>(
0111                 dstRow * Acts::eAlignmentSize, dstCol * Acts::eAlignmentSize) +=
0112             alignState.alignmentToChi2SecondDerivative.block(
0113                 srcRow * Acts::eAlignmentSize, srcCol * Acts::eAlignmentSize,
0114                 Acts::eAlignmentSize, Acts::eAlignmentSize);
0115       }
0116     }
0117     alignResult.chi2 += alignState.chi2;
0118     alignResult.measurementDim += alignState.measurementDim;
0119     sumChi2ONdf += alignState.chi2 / alignState.measurementDim;
0120   }
0121   alignResult.averageChi2ONdf = sumChi2ONdf / alignResult.numTracks;
0122
0123   // Get the inverse of chi2 second derivative matrix (we need this to
0124   // calculate the covariance of the alignment parameters)
0125   // @Todo: use more stable method for solving the inverse
0126   std::size_t alignDof = alignResult.alignmentDof;
0127   Acts::ActsDynamicMatrix sumChi2SecondDerivativeInverse =
0128       Acts::ActsDynamicMatrix::Zero(alignDof, alignDof);
0129   sumChi2SecondDerivativeInverse = sumChi2SecondDerivative.inverse();
0130   if (sumChi2SecondDerivativeInverse.hasNaN()) {
0131     ACTS_DEBUG("Chi2 second derivative inverse has NaN");
0132     // return AlignmentError::AlignmentParametersUpdateFailure;
0133   }
0134
0135   // Initialize the alignment results
0136   alignResult.deltaAlignmentParameters =
0137       Acts::ActsDynamicVector::Zero(alignDof);
0138   alignResult.alignmentCovariance =
0139       Acts::ActsDynamicMatrix::Zero(alignDof, alignDof);
0140   // Solve the linear equation to get alignment parameters change
0141   alignResult.deltaAlignmentParameters =
0142       -sumChi2SecondDerivative.fullPivLu().solve(sumChi2Derivative);
0143   ACTS_VERBOSE("sumChi2SecondDerivative = \n" << sumChi2SecondDerivative);
0144   ACTS_VERBOSE("sumChi2Derivative = \n" << sumChi2Derivative);
0145   ACTS_VERBOSE("alignResult.deltaAlignmentParameters \n");
0146
0147   // Alignment parameters covariance
0148   alignResult.alignmentCovariance = 2 * sumChi2SecondDerivativeInverse;
0149   // chi2 change
0150   alignResult.deltaChi2 = 0.5 * sumChi2Derivative.transpose() *
0151                           alignResult.deltaAlignmentParameters;
0152 }
0153
0154 template <typename fitter_t>
0155 Acts::Result<void>
0156 ActsAlignment::Alignment<fitter_t>::updateAlignmentParameters(
0157     const Acts::GeometryContext& gctx,
0158     const std::vector<Acts::DetectorElementBase*>& alignedDetElements,
0159     const ActsAlignment::AlignedTransformUpdater& alignedTransformUpdater,
0160     ActsAlignment::AlignmentResult& alignResult) const {
0161   // Update the aligned transform
0162   Acts::AlignmentVector deltaAlignmentParam = Acts::AlignmentVector::Zero();
0163   for (const auto& [surface, index] : alignResult.idxedAlignSurfaces) {
0164     // 1. The original transform
0165     const Acts::Vector3& oldCenter = surface->center(gctx);
0166     const Acts::Transform3& oldTransform = surface->transform(gctx);
0167     const Acts::RotationMatrix3& oldRotation = oldTransform.rotation();
0168     // The elements stored below is (rotZ, rotY, rotX)
0169     const Acts::Vector3& oldEulerAngles = oldRotation.eulerAngles(2, 1, 0);
0170
0171     // 2. The delta transform
0172     deltaAlignmentParam = alignResult.deltaAlignmentParameters.segment(
0173         Acts::eAlignmentSize * index, Acts::eAlignmentSize);
0174     // The delta translation
0175     Acts::Vector3 deltaCenter =
0176         deltaAlignmentParam.segment<3>(Acts::eAlignmentCenter0);
0177     // The delta Euler angles
0178     Acts::Vector3 deltaEulerAngles =
0179         deltaAlignmentParam.segment<3>(Acts::eAlignmentRotation0);
0180
0181     // 3. The new transform
0182     const Acts::Vector3 newCenter = oldCenter + deltaCenter;
0183     // The rotation around global z axis
0184     Acts::AngleAxis3 rotZ(oldEulerAngles(0) + deltaEulerAngles(2),
0185                           Acts::Vector3::UnitZ());
0186     // The rotation around global y axis
0187     Acts::AngleAxis3 rotY(oldEulerAngles(1) + deltaEulerAngles(1),
0188                           Acts::Vector3::UnitY());
0189     // The rotation around global x axis
0190     Acts::AngleAxis3 rotX(oldEulerAngles(2) + deltaEulerAngles(0),
0191                           Acts::Vector3::UnitX());
0192     Eigen::Quaternion<Acts::ActsScalar> newRotation = rotZ * rotY * rotX;
0193     const Acts::Transform3 newTransform =
0194         Acts::Translation3(newCenter) * newRotation;
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 }