Acts/Seeding/CompositeSpacePointLineFitter.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 "Acts/Seeding/CompositeSpacePointLineFitter.hpp"
0012
0013 #include "Acts/Utilities/AlgebraHelpers.hpp"
0014 #include "Acts/Utilities/StringHelpers.hpp"
0015
0016 #include <format>
0017 namespace Acts::Experimental {
0018
0019 template <CompositeSpacePointContainer Cont_t>
0020 std::array<std::size_t, 3> CompositeSpacePointLineFitter::countDoF(
0021     const Cont_t& measurements) const {
0022   return countDoF(measurements, Selector_t<SpacePoint_t<Cont_t>>{});
0023 }
0024
0025 template <CompositeSpacePointContainer Cont_t>
0026 std::array<std::size_t, 3> CompositeSpacePointLineFitter::countDoF(
0027     const Cont_t& measurements,
0028     const Selector_t<SpacePoint_t<Cont_t>>& selector) const {
0029   using enum detail::CompSpacePointAuxiliaries::ResidualIdx;
0030   auto counts = filledArray<std::size_t, 3>(0u);
0031   std::size_t nValid{0};
0032   for (const auto& sp : measurements) {
0033     if (selector.connected() && !selector(*sp)) {
0034       continue;
0035     }
0036     ++nValid;
0037     if (sp->measuresLoc0()) {
0038       ++counts[toUnderlying(nonBending)];
0039     }
0040     if (sp->measuresLoc1()) {
0041       ++counts[toUnderlying(bending)];
0042     }
0043     /// Count the straws as implicit time measurements
0044     if (m_cfg.fitT0 && (sp->hasTime() || sp->isStraw())) {
0045       ++counts[toUnderlying(time)];
0046     }
0047   }
0048   ACTS_DEBUG(
0049       __func__ << "() " << __LINE__ << " - " << nValid << "/"
0050                << measurements.size() << " valid measurements passed. Found "
0051                << counts[toUnderlying(nonBending)] << "/"
0052                << counts[toUnderlying(bending)] << "/"
0053                << counts[toUnderlying(time)] << " measuring loc0/loc1/time.");
0054   return counts;
0055 }
0056
0057 template <CompositeSpacePointContainer Cont_t>
0058 CompositeSpacePointLineFitter::FastFitResult
0059 CompositeSpacePointLineFitter::fastPrecFit(
0060     const Cont_t& measurements, const Line_t& initialGuess,
0061     const std::vector<FitParIndex>& parsToUse) const {
0062   if (std::ranges::none_of(parsToUse, [](const FitParIndex idx) {
0063         using enum FitParIndex;
0064         return idx == theta || idx == y0;
0065       })) {
0066     return std::nullopt;
0067   }
0068
0069   std::size_t nStraw = std::ranges::count_if(
0070       measurements, [](const auto& sp) { return sp->isStraw(); });
0071   ACTS_DEBUG(__func__ << "() " << __LINE__ << " - Fetched " << nStraw
0072                       << " measurements for fast fit.");
0073   if (nStraw > 2) {
0074     std::vector<int> signs = detail::CompSpacePointAuxiliaries::strawSigns(
0075         initialGuess, measurements);
0076     FastFitResult precResult = m_fastFitter.fit(measurements, signs);
0077     // Fast fit gave a bad chi2 -> Maybe L/R solution is swapped?
0078     if (precResult && precResult->chi2 / static_cast<double>(precResult->nDoF) >
0079                           m_cfg.badFastChi2SignSwap) {
0080       // Swap signs
0081       ACTS_DEBUG(__func__ << "() " << __LINE__
0082                           << " - The fit result is of poor quality "
0083                           << (*precResult) << " attempt with L<->R swapping");
0084       for (auto& s : signs) {
0085         s = -s;
0086       }
0087       // Retry & check whether the chi2 is better
0088       FastFitResult swappedPrecResult = m_fastFitter.fit(measurements, signs);
0089       if (swappedPrecResult && swappedPrecResult->chi2 < precResult->chi2) {
0090         ACTS_DEBUG(__func__ << "() " << __LINE__
0091                             << " - Swapped fit is of better quality "
0092                             << (*swappedPrecResult));
0093         swappedPrecResult->nIter += precResult->nIter;
0094         return swappedPrecResult;
0095       } else {
0096         precResult->nIter += swappedPrecResult->nIter;
0097         ACTS_DEBUG(__func__ << "() " << __LINE__ << " - Fit did not improve "
0098                             << (*swappedPrecResult));
0099         return precResult;
0100       }
0101     }
0102   }
0103   using ResidualIdx = detail::CompSpacePointAuxiliaries::ResidualIdx;
0104   return m_fastFitter.fit(measurements, ResidualIdx::bending);
0105 }
0106
0107 template <CompositeSpacePointContainer Cont_t>
0108 CompositeSpacePointLineFitter::FitParameters
0109 CompositeSpacePointLineFitter::fastFit(
0110     const Cont_t& measurements, const Line_t& initialGuess,
0111     const std::vector<FitParIndex>& parsToUse) const {
0112   using namespace Acts::UnitLiterals;
0113   using enum FitParIndex;
0114
0115   FitParameters result{};
0116
0117   const FastFitResult precResult{
0118       fastPrecFit(measurements, initialGuess, parsToUse)};
0119   if (!precResult) {
0120     ACTS_DEBUG(__func__ << "() " << __LINE__ << " - Fast fit failed.");
0121     return result;
0122   }
0123
0124   // Copy the parameters & covariance
0125   result.parameters[toUnderlying(y0)] = precResult->y0;
0126   result.parameters[toUnderlying(theta)] = precResult->theta;
0127   result.covariance(toUnderlying(y0), toUnderlying(y0)) =
0128       Acts::square(precResult->dY0);
0129   result.covariance(toUnderlying(theta), toUnderlying(theta)) =
0130       Acts::square(precResult->dTheta);
0131   result.nDoF = static_cast<unsigned>(precResult->nDoF);
0132   result.nIter = static_cast<unsigned>(precResult->nIter);
0133   result.chi2 = precResult->chi2;
0134   result.converged = true;
0135
0136   // Check whether a non-bending fit is required
0137   if (std::ranges::none_of(parsToUse, [](const FitParIndex idx) {
0138         return idx == phi || idx == x0;
0139       })) {
0140     result.parameters[toUnderlying(phi)] = 90._degree;
0141     ACTS_DEBUG(
0142         __func__ << "() " << __LINE__
0143                  << " - No measurements in non precision direction parsed.");
0144     return result;
0145   }
0146
0147   ACTS_DEBUG(__func__ << "() " << __LINE__
0148                       << " - Start fast non-precision fit.");
0149   using ResidualIdx = detail::CompSpacePointAuxiliaries::ResidualIdx;
0150   const FastFitResult nonPrecResult =
0151       m_fastFitter.fit(measurements, ResidualIdx::nonBending);
0152   if (!nonPrecResult) {
0153     result.parameters[toUnderlying(phi)] = 90._degree;
0154     ACTS_DEBUG(__func__ << "() " << __LINE__
0155                         << " - Fast non-precision fit failed.");
0156     return result;
0157   }
0158   result.parameters[toUnderlying(x0)] = nonPrecResult->y0;
0159   result.covariance(toUnderlying(x0), toUnderlying(x0)) =
0160       Acts::square(nonPrecResult->dY0);
0161   result.nDoF += nonPrecResult->nDoF;
0162   result.nIter += nonPrecResult->nIter;
0163   // Combine the two results into a single direction
0164
0165   double tanTheta = std::tan(precResult->theta);
0166   double tanPhi = std::tan(nonPrecResult->theta);
0167   auto dir = makeDirectionFromAxisTangents(tanPhi, tanTheta);
0168   result.parameters[toUnderlying(phi)] = VectorHelpers::phi(dir);
0169   result.parameters[toUnderlying(theta)] = VectorHelpers::theta(dir);
0170
0171   Line_t recoLine{result.parameters};
0172   result.chi2 = 0.;
0173   std::ranges::for_each(measurements, [&recoLine, &result](const auto& m) {
0174     result.chi2 += detail::CompSpacePointAuxiliaries::chi2Term(recoLine, *m);
0175   });
0176
0177   ACTS_DEBUG(__func__ << "() " << __LINE__ << ": Fast fit done. Obtained result"
0178                       << result);
0179   return result;
0180 }
0181 template <CompositeSpacePointContainer Cont_t,
0182           CompositeSpacePointCalibrator<Cont_t, Cont_t> Calibrator_t>
0183 CompositeSpacePointLineFitter::FitResult<Cont_t>
0184 CompositeSpacePointLineFitter::fit(
0185     FitOptions<Cont_t, Calibrator_t>&& fitOpts) const {
0186   using namespace Acts::UnitLiterals;
0187
0188   if (!fitOpts.calibrator) {
0189     throw std::invalid_argument(
0190         "CompositeSpacePointLineFitter::fit() - Please provide a valid pointer "
0191         "to a calibrator.");
0192   }
0193
0194   FitResult<Cont_t> result{};
0195   result.measurements = std::move(fitOpts.measurements);
0196   result.parameters = std::move(fitOpts.startParameters);
0197
0198   // Declare the auxiliaries object to calculate the residuals
0199   detail::CompSpacePointAuxiliaries::Config resCfg{};
0200   resCfg.localToGlobal = std::move(fitOpts.localToGlobal);
0201   resCfg.useHessian = m_cfg.useHessian;
0202   resCfg.calcAlongStraw = m_cfg.calcAlongStraw;
0203   resCfg.calcAlongStrip = m_cfg.calcAlongStrip;
0204   resCfg.includeToF = m_cfg.includeToF;
0205
0206   resCfg.parsToUse =
0207       extractFitablePars(countDoF(result.measurements, fitOpts.selector));
0208   if (resCfg.parsToUse.empty()) {
0209     ACTS_WARNING(__func__ << "() " << __LINE__
0210                           << ": No valid degrees of freedom parsed. Please "
0211                           << "check your measurements");
0212     return result;
0213   }
0214
0215   Line_t line{};
0216
0217   // Fast fitter shall be used
0218   if (m_cfg.useFastFitter) {
0219     line.updateParameters(result.parameters);
0220     if (resCfg.parsToUse.back() == FitParIndex::t0) {
0221       ACTS_WARNING(__func__ << "() " << __LINE__
0222                             << " - Fit with t0 to be implemented");
0223     } else {
0224       ACTS_DEBUG(__func__ << "() " << __LINE__
0225                           << " - Attempt a fast fit, first.");
0226       FitParameters fastResult =
0227           fastFit(result.measurements, line, resCfg.parsToUse);
0228       if (fastResult.converged) {
0229         static_cast<FitParameters&>(result) = std::move(fastResult);
0230         ACTS_DEBUG(__func__ << "() " << __LINE__ << " - Fit converged.");
0231         // Use the result from the fast fitter as final answer
0232         if (!m_cfg.fastPreFitter) {
0233           line.updateParameters(result.parameters);
0234           // Recalibrate the measurements before returning
0235           result.measurements = fitOpts.calibrator->calibrate(
0236               fitOpts.calibContext, line.position(), line.direction(), 0.,
0237               result.measurements);
0238           return result;
0239         }
0240         // Set convergence flag to false because the full fit comes later.
0241         result.converged = false;
0242       } else {
0243         ACTS_DEBUG(__func__ << "() " << __LINE__ << " - Fit failed.");
0244         return result;
0245       }
0246     }
0247   }
0248   // Update the drift signs if no re-calibration per iteration is scheduled
0249   if (!m_cfg.recalibrate) {
0250     line.updateParameters(result.parameters);
0251     fitOpts.calibrator->updateSigns(line.position(), line.direction(),
0252                                     result.measurements);
0253   }
0254   /// Proceed with the usual fit
0255   ChiSqCache cache{};
0256   detail::CompSpacePointAuxiliaries pullCalculator{resCfg, logger().clone()};
0257   for (; !result.converged && result.nIter < m_cfg.maxIter; ++result.nIter) {
0258     cache.reset();
0259     // Update the parameters from the last iteration
0260     line.updateParameters(result.parameters);
0261     const double t0 = result.parameters[toUnderlying(FitParIndex::t0)];
0262
0263     ACTS_VERBOSE(__func__ << "() " << __LINE__ << ": Start next iteration "
0264                           << result << " --> " << toString(line.position())
0265                           << " + " << toString(line.direction()) << ".");
0266     // Update the measurements if calibration loop is switched on
0267     if (m_cfg.recalibrate) {
0268       result.measurements = fitOpts.calibrator->calibrate(
0269           fitOpts.calibContext, line.position(), line.direction(), t0,
0270           result.measurements);
0271       // Check whether the measurements are still valid
0272       auto parsBkp = std::move(resCfg.parsToUse);
0273       resCfg.parsToUse =
0274           extractFitablePars(countDoF(result.measurements, fitOpts.selector));
0275
0276       // No valid measurement is left
0277       if (resCfg.parsToUse.empty()) {
0278         ACTS_WARNING(__func__ << "() " << __LINE__ << ":  Line parameters "
0279                               << toString(line.position()) << " + "
0280                               << toString(line.direction()) << ", t0: " << t0
0281                               << " invalidated all measurements");
0282         return result;
0283       }
0284
0285       if (parsBkp != resCfg.parsToUse) {
0286         // Instantiate an updated pull calculator
0287         pullCalculator =
0288             detail::CompSpacePointAuxiliaries{resCfg, logger().clone()};
0289       }
0290       // update the drift signs
0291       fitOpts.calibrator->updateSigns(line.position(), line.direction(),
0292                                       result.measurements);
0293     }
0294     // Calculate the new chi2
0295     for (const auto& spacePoint : result.measurements) {
0296       // Skip bad measurements
0297       if (fitOpts.selector.connected() && !fitOpts.selector(*spacePoint)) {
0298         continue;
0299       }
0300       double driftV{0.};
0301       double driftA{0.};
0302       // Calculate the residual & derivatives
0303       if (resCfg.parsToUse.back() == FitParIndex::t0) {
0304         pullCalculator.updateFullResidual(line, t0, *spacePoint, driftV,
0305                                           driftA);
0306       } else {
0307         pullCalculator.updateSpatialResidual(line, *spacePoint);
0308       }
0309       // Propagte to the chi2
0310       pullCalculator.updateChiSq(cache, spacePoint->covariance());
0311     }
0312     pullCalculator.symmetrizeHessian(cache);
0313     result.chi2 = cache.chi2;
0314     // Now update the parameters
0315     UpdateStep update{UpdateStep::goodStep};
0316     switch (resCfg.parsToUse.size()) {
0317       // 2D fit (intercept + inclination angle)
0318       case 2: {
0319         update = updateParameters<2>(resCfg.parsToUse.front(), cache,
0320                                      result.parameters);
0321         break;
0322       }
0323       // 2D fit + time
0324       case 3: {
0325         update = updateParameters<3>(resCfg.parsToUse.front(), cache,
0326                                      result.parameters);
0327         break;
0328       }
0329       // 3D spatial fit (x0, y0, theta, phi)
0330       case 4: {
0331         update = updateParameters<4>(resCfg.parsToUse.front(), cache,
0332                                      result.parameters);
0333         break;
0334       }
0335       // full fit
0336       case 5: {
0337         update = updateParameters<5>(resCfg.parsToUse.front(), cache,
0338                                      result.parameters);
0339         break;
0340       }
0341       default:
0342         ACTS_WARNING(__func__ << "() " << __LINE__
0343                               << ": Invalid parameter size "
0344                               << resCfg.parsToUse.size());
0345         return result;
0346     }
0347     switch (update) {
0348       using enum UpdateStep;
0349       case converged: {
0350         result.converged = true;
0351         break;
0352       }
0353       case goodStep: {
0354         break;
0355       }
0356       case outOfBounds: {
0357         return result;
0358       }
0359     }
0360   }
0361   // Parameters converged
0362   if (result.converged) {
0363     const auto doF = countDoF(result.measurements, fitOpts.selector);
0364     result.nDoF = (doF[0] + doF[1] + doF[2]) - resCfg.parsToUse.size();
0365     line.updateParameters(result.parameters);
0366     const double t0 = result.parameters[toUnderlying(FitParIndex::t0)];
0367     // Recalibrate the measurements before returning
0368     result.measurements = fitOpts.calibrator->calibrate(
0369         fitOpts.calibContext, line.position(), line.direction(), t0,
0370         result.measurements);
0371
0372     // Assign the Hessian
0373     switch (resCfg.parsToUse.size()) {
0374       // 2D fit (intercept + inclination angle)
0375       case 2: {
0376         fillCovariance<2>(resCfg.parsToUse.front(), cache.hessian,
0377                           result.covariance);
0378         break;
0379       }
0380       // 2D fit + time
0381       case 3: {
0382         fillCovariance<3>(resCfg.parsToUse.front(), cache.hessian,
0383                           result.covariance);
0384         break;
0385       }
0386       // 3D spatial fit (x0, y0, theta, phi)
0387       case 4: {
0388         fillCovariance<4>(resCfg.parsToUse.front(), cache.hessian,
0389                           result.covariance);
0390         break;
0391       }
0392       // full fit
0393       case 5: {
0394         fillCovariance<5>(resCfg.parsToUse.front(), cache.hessian,
0395                           result.covariance);
0396         break;
0397       }
0398       // No need to warn here -> captured by the fit iterations
0399       default:
0400         break;
0401     }
0402   }
0403   return result;
0404 }
0405
0406 template <unsigned N>
0407 CompositeSpacePointLineFitter::UpdateStep
0408 CompositeSpacePointLineFitter::updateParameters(const FitParIndex firstPar,
0409                                                 const ChiSqCache& cache,
0410                                                 ParamVec_t& currentPars) const
0411   requires(N >= 2 && N <= s_nPars)
0412 {
0413   auto firstIdx = toUnderlying(firstPar);
0414   assert(firstIdx + N < s_nPars);
0415   // Current parameters mapped to an Eigen interface
0416   Eigen::Map<ActsVector<N>> miniPars{currentPars.data() + firstIdx};
0417   ACTS_VERBOSE(__func__ << "<" << N << ">() - " << __LINE__
0418                         << ": Current parameters " << toString(miniPars)
0419                         << " with chi2: " << cache.chi2 << ",  gradient: "
0420                         << toString(cache.gradient) << ", hessian: \n"
0421                         << cache.hessian);
0422
0423   // Take out the filled block from the gradient
0424   Eigen::Map<const ActsVector<N>> miniGradient{cache.gradient.data() +
0425                                                firstIdx};
0426   // The gradient is already small enough
0427   if (miniGradient.norm() < m_cfg.precCutOff) {
0428     ACTS_DEBUG(__func__ << "<" << N << ">() - " << __LINE__
0429                         << ": Gradient is small enough");
0430     return UpdateStep::converged;
0431   }
0432   // Take out the filled block from the hessian
0433   Acts::ActsSquareMatrix<N> miniHessian{
0434       cache.hessian.block<N, N>(firstIdx, firstIdx)};
0435   ACTS_VERBOSE(__func__ << "<" << N << ">() - " << __LINE__
0436                         << ": Projected parameters: " << toString(miniPars)
0437                         << " gradient: " << toString(miniGradient)
0438                         << ", hessian: \n"
0439                         << miniHessian
0440                         << ", determinant: " << miniHessian.determinant());
0441
0442   auto inverseH = safeInverse(miniHessian);
0443   // The Hessian can safely be inverted
0444   if (inverseH) {
0445     const ActsVector<N> update{(*inverseH) * miniGradient};
0446
0447     if (update.norm() < m_cfg.precCutOff) {
0448       ACTS_DEBUG(__func__ << "<" << N << ">() - " << __LINE__ << ": Update "
0449                           << toString(update) << " is negligible small.");
0450       return UpdateStep::converged;
0451     }
0452     ACTS_VERBOSE(__func__ << "<" << N << ">() - " << __LINE__
0453                           << ": Inverted Hessian \n"
0454                           << (*inverseH) << "\n-> Update parameters by "
0455                           << toString(update));
0456     miniPars -= update;
0457
0458   } else {
0459     // Fall back to gradient decent with a fixed damping factor
0460     const ActsVector<N> update{
0461         std::min(m_cfg.gradientStep, miniGradient.norm()) *
0462         miniGradient.normalized()};
0463
0464     ACTS_VERBOSE(__func__ << "<" << N << ">() - " << __LINE__
0465                           << ": Update parameters by " << toString(update));
0466     miniPars -= update;
0467   }
0468   // Check parameter ranges
0469
0470   return UpdateStep::goodStep;
0471 }
0472
0473 template <unsigned N>
0474 void CompositeSpacePointLineFitter::fillCovariance(const FitParIndex firstPar,
0475                                                    const CovMat_t& hessian,
0476                                                    CovMat_t& covariance) const
0477   requires(N >= 2 && N <= s_nPars)
0478 {
0479   auto firstIdx = toUnderlying(firstPar);
0480   assert(firstIdx + N < s_nPars);
0481
0482   Acts::ActsSquareMatrix<N> miniHessian{
0483       hessian.block<N, N>(firstIdx, firstIdx)};
0484
0485   auto inverseH = safeInverse(miniHessian);
0486   // The Hessian can safely be inverted
0487   if (inverseH) {
0488     auto covBlock = covariance.block<N, N>(firstIdx, firstIdx);
0489     covBlock = (*inverseH);
0490     ACTS_DEBUG(__func__ << "<" << N << ">() - " << __LINE__
0491                         << ": Evaluated covariance: \n"
0492                         << covariance);
0493   }
0494 }
0495
0496 }  // namespace Acts::Experimental