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 #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
0168     // 2. The delta transform
0169     deltaAlignmentParam = alignResult.deltaAlignmentParameters.segment(
0170         Acts::eAlignmentSize * index, Acts::eAlignmentSize);
0171     // The delta translation
0172     Acts::Vector3 deltaCenter =
0173         deltaAlignmentParam.segment<3>(Acts::eAlignmentCenter0);
0174     // The delta Euler angles
0175     Acts::Vector3 deltaEulerAngles =
0176         deltaAlignmentParam.segment<3>(Acts::eAlignmentRotation0);
0177
0178     // 3. The new transform
0179     const Acts::Vector3 newCenter = oldCenter + deltaCenter;
0180     Acts::Transform3 newTransform = oldTransform;
0181     newTransform.translation() = newCenter;
0182     // Rotation first around fixed local x, then around fixed local y, and last
0183     // around fixed local z, this is the same as first around local z, then
0184     // around new loca y, and last around new local x below
0185     newTransform *=
0186         Acts::AngleAxis3(deltaEulerAngles(2), Acts::Vector3::UnitZ());
0187     newTransform *=
0188         Acts::AngleAxis3(deltaEulerAngles(1), Acts::Vector3::UnitY());
0189     newTransform *=
0190         Acts::AngleAxis3(deltaEulerAngles(0), Acts::Vector3::UnitX());
0191
0192     // 4. Update the aligned transform
0193     //@Todo: use a better way to handle this (need dynamic cast to inherited
0194     // detector element type)
0195     ACTS_VERBOSE("Delta of alignment parameters at element "
0196                  << index << "= \n"
0197                  << deltaAlignmentParam);
0198     bool updated = alignedTransformUpdater(alignedDetElements.at(index), gctx,
0199                                            newTransform);
0200     if (!updated) {
0201       ACTS_ERROR("Update alignment parameters for detector element failed");
0202       return AlignmentError::AlignmentParametersUpdateFailure;
0203     }
0204   }
0205
0206   return Acts::Result<void>::success();
0207 }
0208
0209 template <typename fitter_t>
0210 template <typename trajectory_container_t,
0211           typename start_parameters_container_t, typename fit_options_t>
0212 Acts::Result<ActsAlignment::AlignmentResult>
0213 ActsAlignment::Alignment<fitter_t>::align(
0214     const trajectory_container_t& trajectoryCollection,
0215     const start_parameters_container_t& startParametersCollection,
0216     const ActsAlignment::AlignmentOptions<fit_options_t>& alignOptions) const {
0217   // Construct an AlignmentResult object
0218   AlignmentResult alignResult;
0219
0220   // Assign index to the alignable surface
0221   for (unsigned int iDetElement = 0;
0222        iDetElement < alignOptions.alignedDetElements.size(); iDetElement++) {
0223     alignResult.idxedAlignSurfaces.emplace(
0224         &alignOptions.alignedDetElements.at(iDetElement)->surface(),
0225         iDetElement);
0226   }
0227   ACTS_VERBOSE("There are " << alignResult.idxedAlignSurfaces.size()
0228                             << " detector elements to be aligned");
0229
0230   // Start the iteration to minimize the chi2
0231   bool converged = false;
0232   bool alignmentParametersUpdated = false;
0233   std::queue<double> recentChi2ONdf;
0234   ACTS_INFO("Max number of iterations: " << alignOptions.maxIterations);
0235   for (unsigned int iIter = 0; iIter < alignOptions.maxIterations; iIter++) {
0236     // Perform the fit to the trajectories and update alignment parameters
0237     // Initialize the alignment mask (all dof in default)
0238     AlignmentMask alignMask = AlignmentMask::All;
0239     // Set the alignment mask
0240     auto iter_it = alignOptions.iterationState.find(iIter);
0241     if (iter_it != alignOptions.iterationState.end()) {
0242       alignMask = iter_it->second;
0243     }
0244     // Calculate the alignment parameters delta etc.
0245     calculateAlignmentParameters(
0246         trajectoryCollection, startParametersCollection,
0247         alignOptions.fitOptions, alignResult, alignMask);
0248     // Screen out the information
0249     ACTS_INFO("iIter = " << iIter << ", total chi2 = " << alignResult.chi2
0250                          << ", total measurementDim = "
0251                          << alignResult.measurementDim
0252                          << " and average chi2/ndf = "
0253                          << alignResult.averageChi2ONdf);
0254     // Check if it has converged against the provided precision
0255     // 1. either the delta average chi2/ndf in the last few
0256     // iterations is within tolerance
0257     if (recentChi2ONdf.size() >=
0258         alignOptions.deltaAverageChi2ONdfCutOff.first) {
0259       if (std::abs(recentChi2ONdf.front() - alignResult.averageChi2ONdf) <=
0260           alignOptions.deltaAverageChi2ONdfCutOff.second) {
0261         ACTS_INFO(
0262             "Alignment has converged with change of chi2/ndf < "
0263             << alignOptions.deltaAverageChi2ONdfCutOff.second << " in the last "
0264             << alignOptions.deltaAverageChi2ONdfCutOff.first << " iterations"
0265             << " after " << iIter << " iteration(s)");
0266         converged = true;
0267         break;
0268       }
0269       recentChi2ONdf.pop();
0270     }
0271     // 2. or the average chi2/ndf (is this correct?)
0272     if (alignResult.averageChi2ONdf <= alignOptions.averageChi2ONdfCutOff) {
0273       ACTS_INFO("Alignment has converged with average chi2/ndf < "
0274                 << alignOptions.averageChi2ONdfCutOff << " after " << iIter
0275                 << " iteration(s)");
0276       converged = true;
0277       break;
0278     }
0279     // Remove the first element
0280     // Store the result in the queue
0281     recentChi2ONdf.push(alignResult.averageChi2ONdf);
0282
0283     ACTS_INFO("The solved delta of alignmentParameters = \n "
0284               << alignResult.deltaAlignmentParameters);
0285     // Not coveraged yet, update the detector element alignment parameters
0286     auto updateRes = updateAlignmentParameters(
0287         alignOptions.fitOptions.geoContext, alignOptions.alignedDetElements,
0288         alignOptions.alignedTransformUpdater, alignResult);
0289     if (!updateRes.ok()) {
0290       ACTS_ERROR("Update alignment parameters failed: " << updateRes.error());
0291       return updateRes.error();
0292     }
0293     alignmentParametersUpdated = true;
0294   }  // end of all iterations
0295
0296   // Alignment failure if not converged
0297   if (!converged) {
0298     ACTS_ERROR("Alignment is not converged.");
0299     alignResult.result = AlignmentError::ConvergeFailure;
0300   }
0301
0302   // Screen out the final aligned parameters
0303   // @todo
0304   if (alignmentParametersUpdated) {
0305     for (const auto& det : alignOptions.alignedDetElements) {
0306       const auto& surface = &det->surface();
0307       const auto& transform =
0308           det->transform(alignOptions.fitOptions.geoContext);
0309       // write it to the result
0310       alignResult.alignedParameters.emplace(det, transform);
0311       const auto& translation = transform.translation();
0312       const auto& rotation = transform.rotation();
0313       const Acts::Vector3 rotAngles = rotation.eulerAngles(2, 1, 0);
0314       ACTS_VERBOSE("Detector element with surface "
0315                    << surface->geometryId()
0316                    << " has aligned geometry position as below:");
0317       ACTS_VERBOSE("Center (cenX, cenY, cenZ) = " << translation.transpose());
0318       ACTS_VERBOSE(
0319           "Euler angles (rotZ, rotY, rotX) = " << rotAngles.transpose());
0320       ACTS_VERBOSE("Rotation matrix = \n" << rotation);
0321     }
0322   } else {
0323     ACTS_DEBUG("Alignment parameters is not updated.");
0324   }
0325
0326   return alignResult;
0327 }