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/Seeding/CompositeSpacePointLineSeeder.hpp"
0014 #include "Acts/Utilities/AlgebraHelpers.hpp"
0015 #include "Acts/Utilities/StringHelpers.hpp"
0016
0017 #include <format>
0018
0019 namespace Acts::Experimental {
0020
0021 template <CompositeSpacePointContainer Cont_t>
0022 CompositeSpacePointLineFitter::DoFcounts
0023 CompositeSpacePointLineFitter::countDoF(const Cont_t& measurements) const {
0024   return countDoF(measurements, Selector_t<SpacePoint_t<Cont_t>>{});
0025 }
0026
0027 template <CompositeSpacePointContainer Cont_t>
0028 CompositeSpacePointLineFitter::DoFcounts
0029 CompositeSpacePointLineFitter::countDoF(
0030     const Cont_t& measurements,
0031     const Selector_t<SpacePoint_t<Cont_t>>& selector) const {
0032   DoFcounts counts{};
0033   std::size_t nValid{0};
0034   for (const auto& sp : measurements) {
0035     if (selector.connected() && !selector(*sp)) {
0036       ACTS_VERBOSE(__func__ << "() " << __LINE__
0037                             << " -  Skip invalid measurement "
0038                             << toString(*sp));
0039       continue;
0040     }
0041     ++nValid;
0042     if (sp->measuresLoc0()) {
0043       ++counts.nonBending;
0044     }
0045     if (sp->measuresLoc1()) {
0046       ++counts.bending;
0047     }
0048     if (sp->isStraw()) {
0049       ++counts.straw;
0050     } else if (m_cfg.fitT0 && sp->hasTime()) {
0051       ++counts.time;
0052     }
0053   }
0054   ACTS_DEBUG(__func__ << "() " << __LINE__ << " - " << nValid << "/"
0055                       << measurements.size()
0056                       << " valid measurements passed. Found "
0057                       << counts.nonBending << "/" << counts.bending << "/"
0058                       << counts.time << " measuring loc0/loc1/time with "
0059                       << counts.straw << " straw measurements.");
0060   return counts;
0061 }
0062
0063 template <bool fitStraws, bool fitTime, CompositeSpacePointContainer Cont_t>
0064 CompositeSpacePointLineFitter::DelegateRet_t<fitTime>
0065 CompositeSpacePointLineFitter::fastPrecFit(
0066     const Cont_t& measurements, const Line_t& initialGuess,
0067     const FastFitDelegate_t<Cont_t, fitTime>& delegate) const {
0068   std::vector<int> strawSigns{};
0069   if constexpr (fitStraws) {
0070     strawSigns = detail::CompSpacePointAuxiliaries::strawSigns(initialGuess,
0071                                                                measurements);
0072   }
0073   ACTS_DEBUG(__func__ << " <fit " << (fitStraws ? "straws" : "strips") << ", "
0074                       << (fitTime ? "with" : "no") << " time>() " << __LINE__
0075                       << " - Start precision fit.");
0076   auto result = delegate(measurements, strawSigns);
0077
0078   if constexpr (!fitStraws) {
0079     ACTS_DEBUG(__func__ << " <fit " << (fitStraws ? "straws" : "strips") << ", "
0080                         << (fitTime ? "with" : "no") << " time>() " << __LINE__
0081                         << " - Precision fit "
0082                         << (result ? "succeeded" : "failed"));
0083
0084     return result;
0085   }
0086   if (result) {
0087     ACTS_DEBUG(__func__ << " <fit " << (fitStraws ? "straws" : "strips") << ", "
0088                         << (fitTime ? "with" : "no") << " time>() " << __LINE__
0089                         << " - Precision fit converged. " << (*result));
0090
0091     if (result->chi2 / static_cast<double>(result->nDoF) <
0092         m_cfg.badFastChi2SignSwap) {
0093       return result;
0094     }
0095   }
0096
0097   ACTS_DEBUG(
0098       __func__
0099       << " <fit " << (fitStraws ? "straws" : "strips") << ", "
0100       << (fitTime ? "with" : "no") << " time>() " << __LINE__
0101       << " - The fit result is of poor quality attempt with L<->R swapping");
0102   // Swap signs
0103
0104   for (int& s : strawSigns) {
0105     s = -s;
0106   }
0107   // Retry & check whether the chi2 is better
0108   auto swappedPrecResult = delegate(measurements, strawSigns);
0109   if (!swappedPrecResult) {
0110     ACTS_DEBUG(__func__ << " <fit " << (fitStraws ? "straws" : "strips") << ", "
0111                         << (fitTime ? "with" : "no") << " time>() " << __LINE__
0112                         << " - The fit did not improve");
0113
0114     return result;
0115   }
0116   if (!result || swappedPrecResult->chi2 < result->chi2) {
0117     ACTS_DEBUG(__func__ << " <fit " << (fitStraws ? "straws" : "strips") << ", "
0118                         << (fitTime ? "with" : "no") << " time>() " << __LINE__
0119                         << " - Swapped fit is of better quality "
0120                         << (*swappedPrecResult));
0121     if (result) {
0122       swappedPrecResult->nIter += result->nIter;
0123     }
0124     return swappedPrecResult;
0125   } else {
0126     result->nIter += swappedPrecResult->nIter;
0127     ACTS_DEBUG(__func__ << " <fit " << (fitStraws ? "straws" : "strips") << ", "
0128                         << (fitTime ? "with" : "no") << " time>() " << __LINE__
0129                         << " - Fit did not improve " << (*swappedPrecResult));
0130   }
0131   return result;
0132 }
0133
0134 template <bool fitStraws, bool fitTime, CompositeSpacePointContainer Cont_t>
0135 CompositeSpacePointLineFitter::FitParameters
0136 CompositeSpacePointLineFitter::fastFit(
0137     const Cont_t& measurements, const Line_t& initialGuess,
0138     const std::vector<FitParIndex>& parsToUse,
0139     const FastFitDelegate_t<Cont_t, fitTime>& precFitDelegate) const {
0140   FitParameters result{};
0141
0142   using enum FitParIndex;
0143
0144   static_assert(fitTime == false || (fitStraws == true && fitTime == true),
0145                 "Initial t0 can only be fitted with straws");
0146
0147   const bool doPrecFit = std::ranges::any_of(
0148       parsToUse,
0149       [](const FitParIndex idx) { return idx == theta || idx == y0; });
0150   const bool doNonPrecFit = std::ranges::any_of(
0151       parsToUse, [](const FitParIndex idx) { return idx == phi || idx == x0; });
0152   const Vector& preFitDir{initialGuess.direction()};
0153   double tanAlpha = preFitDir.x() / preFitDir.z();
0154   double tanBeta = preFitDir.y() / preFitDir.z();
0155
0156   using precResult_t = FastFitDelegate_t<Cont_t, fitTime>::result_type;
0157   precResult_t precResult =
0158       doPrecFit ? fastPrecFit<fitStraws, fitTime>(measurements, initialGuess,
0159                                                   precFitDelegate)
0160                 : std::nullopt;
0161
0162   if (doPrecFit && !precResult) {
0163     return result;
0164   } else if (precResult) {
0165     if constexpr (fitTime) {
0166       result.covariance(toUnderlying(t0), toUnderlying(t0)) =
0167           Acts::square(precResult->dT0);
0168       result.parameters[toUnderlying(t0)] = precResult->t0;
0169       if (!withinRange(precResult->t0, FitParIndex::t0)) {
0170         ACTS_DEBUG(__func__ << " <fit " << (fitStraws ? "straws" : "strips")
0171                             << ", " << (fitTime ? "with" : "no") << " time>() "
0172                             << __LINE__ << " - Time is out of bounds.");
0173         return result;
0174       }
0175     }
0176     if (!withinRange(precResult->y0, FitParIndex::y0) ||
0177         !withinRange(precResult->theta, FitParIndex::theta)) {
0178       ACTS_DEBUG(__func__ << " <fit " << (fitStraws ? "straws" : "strips")
0179                           << ", " << (fitTime ? "with" : "no") << " time>() "
0180                           << __LINE__
0181                           << " - Fit parameters are out of bounds.");
0182       return result;
0183     }
0184     result.parameters[toUnderlying(y0)] = precResult->y0;
0185     result.covariance(toUnderlying(y0), toUnderlying(y0)) =
0186         Acts::square(precResult->dY0);
0187     result.covariance(toUnderlying(theta), toUnderlying(theta)) =
0188         Acts::square(precResult->dTheta);
0189     result.nDoF = precResult->nDoF;
0190     result.nIter = precResult->nIter;
0191     result.chi2 = precResult->chi2;
0192     result.converged = true;
0193
0194     auto firstPrecMeas = std::ranges::find_if(measurements, [](const auto& m) {
0195       return (fitStraws && m->isStraw()) || (!fitStraws && m->measuresLoc1());
0196     });
0197     assert(firstPrecMeas != measurements.end());
0198
0199     const Vector postFitDir = CompositeSpacePointLineSeeder::makeDirection(
0200         **firstPrecMeas, precResult->theta);
0201     tanBeta = postFitDir.y() / postFitDir.z();
0202   }
0203   // Try to perform a fast fit in non-precision direction
0204   const FastFitResult nonPrecResult =
0205       doNonPrecFit ? fastNonPrecFit(measurements) : std::nullopt;
0206
0207   if (nonPrecResult) {
0208     tanAlpha = nonPrecResult->theta;
0209     result.parameters[toUnderlying(x0)] = nonPrecResult->y0;
0210     result.covariance(toUnderlying(x0), toUnderlying(x0)) =
0211         Acts::square(nonPrecResult->dY0);
0212     result.nDoF += nonPrecResult->nDoF;
0213     result.nIter += nonPrecResult->nIter;
0214   }
0215
0216   const Vector postFitDir = makeDirectionFromAxisTangents(tanAlpha, tanBeta);
0217   result.parameters[toUnderlying(theta)] = VectorHelpers::theta(postFitDir);
0218   result.parameters[toUnderlying(phi)] = VectorHelpers::phi(postFitDir);
0219
0220   return result;
0221 }
0222 template <CompositeSpacePointContainer Cont_t>
0223 CompositeSpacePointLineFitter::FastFitResult
0224 CompositeSpacePointLineFitter::fastNonPrecFit(
0225     const Cont_t& measurements) const {
0226   using enum FitParIndex;
0227
0228   auto firstNonPrecMeas = std::ranges::find_if(
0229       measurements, [](const auto& m) { return m->measuresLoc0(); });
0230   if (firstNonPrecMeas == measurements.end()) {
0231     ACTS_DEBUG(__func__ << "() " << __LINE__
0232                         << " - No measurement in non-bending direction");
0233     return std::nullopt;
0234   }
0235   ACTS_DEBUG(__func__ << "() " << __LINE__ << " Start non precision fit.");
0236
0237   FastFitResult result = m_fastFitter.fit(
0238       measurements, detail::CompSpacePointAuxiliaries::ResidualIdx::nonBending);
0239   if (!result) {
0240     ACTS_DEBUG(__func__ << "() " << __LINE__ << " Non precision fit failed.");
0241     return result;
0242   }
0243   if (!withinRange(result->y0, FitParIndex::x0)) {
0244     ACTS_DEBUG(__func__ << "() " << __LINE__ << " Intercept is out of bounds.");
0245     return std::nullopt;
0246   }
0247   const auto& refHit{**firstNonPrecMeas};
0248   const Vector& eY{!refHit.measuresLoc1() ? refHit.toNextSensor()
0249                                           : refHit.sensorDirection()};
0250   const Vector& eZ{refHit.planeNormal()};
0251   const double sinPhi = std::sin(result->theta);
0252   const auto dir = copySign<Vector, double>(
0253       sinPhi * eY + std::cos(result->theta) * eZ, sinPhi);
0254   result->theta = dir.x() / dir.z();
0255   return result;
0256 }
0257
0258 template <CompositeSpacePointContainer Cont_t,
0259           CompositeSpacePointCalibrator<Cont_t, Cont_t> Calibrator_t>
0260 CompositeSpacePointLineFitter::FitResult<Cont_t>
0261 CompositeSpacePointLineFitter::fit(
0262     FitOptions<Cont_t, Calibrator_t>&& fitOpts) const {
0263   using namespace Acts::UnitLiterals;
0264
0265   if (!fitOpts.calibrator) {
0266     throw std::invalid_argument(
0267         "CompositeSpacePointLineFitter::fit() - Please provide a valid pointer "
0268         "to a calibrator.");
0269   }
0270
0271   FitResult<Cont_t> result{};
0272   result.measurements = std::move(fitOpts.measurements);
0273   result.parameters = std::move(fitOpts.startParameters);
0274
0275   // Declare the auxiliaries object to calculate the residuals
0276   detail::CompSpacePointAuxiliaries::Config resCfg{};
0277   resCfg.localToGlobal = std::move(fitOpts.localToGlobal);
0278   resCfg.useHessian = m_cfg.useHessian;
0279   resCfg.calcAlongStraw = m_cfg.calcAlongStraw;
0280   resCfg.calcAlongStrip = m_cfg.calcAlongStrip;
0281   resCfg.includeToF = m_cfg.includeToF;
0282
0283   DoFcounts hitCounts{countDoF(result.measurements, fitOpts.selector)};
0284   const std::size_t nStraws{hitCounts.straw};
0285   resCfg.parsToUse = extractFitablePars(hitCounts);
0286   if (resCfg.parsToUse.empty()) {
0287     ACTS_WARNING(__func__ << "() " << __LINE__
0288                           << ": No valid degrees of freedom parsed. Please "
0289                           << "check your measurements");
0290     return result;
0291   }
0292
0293   Line_t line{};
0294
0295   // Fast fitter shall be used
0296   if (m_cfg.useFastFitter) {
0297     line.updateParameters(result.parameters);
0298     ACTS_DEBUG(__func__ << "() " << __LINE__
0299                         << " - Attempt a fast fit, first.");
0300
0301     constexpr bool fastCalibrator =
0302         CompositeSpacePointFastCalibrator<Calibrator_t, SpacePoint_t<Cont_t>>;
0303     const bool fitTime{resCfg.parsToUse.back() == FitParIndex::t0 &&
0304                        fastCalibrator};
0305     if constexpr (!fastCalibrator) {
0306       if (resCfg.parsToUse.back() == FitParIndex::t0) {
0307         ACTS_WARNING(__func__ << "() " << __LINE__
0308                               << ": Time cannot be fast-fitted because the "
0309                                  "calibrator does not satisfy"
0310                               << " CompositeSpacePointFastCalibrator concept.");
0311       }
0312     }
0313     FitParameters fastResult{};
0314     // Straw fit
0315     if (nStraws >= 2) {
0316       if (fitTime) {
0317         if constexpr (fastCalibrator) {
0318           FastFitDelegate_t<Cont_t, true> fitDelegate{
0319               [this, &fitOpts](const Cont_t& measurements,
0320                                const std::vector<int>& strawSigns) {
0321                 const double initialT0 =
0322                     fitOpts.startParameters[toUnderlying(FitParIndex::t0)];
0323                 return m_fastFitter.fit(fitOpts.calibContext,
0324                                         *fitOpts.calibrator, measurements,
0325                                         strawSigns, initialT0);
0326               }};
0327           fastResult = fastFit<true, true>(result.measurements, line,
0328                                            resCfg.parsToUse, fitDelegate);
0329         }
0330       } else {
0331         FastFitDelegate_t<Cont_t, false> fitDelegate{
0332             [this](const Cont_t& measurements,
0333                    const std::vector<int>& strawSigns) {
0334               return m_fastFitter.fit(measurements, strawSigns);
0335             }};
0336         fastResult = fastFit<true, false>(result.measurements, line,
0337                                           resCfg.parsToUse, fitDelegate);
0338       }
0339     }
0340     // Pure strip fit
0341     else {
0342       FastFitDelegate_t<Cont_t, false> fitDelegate{
0343           [this](const Cont_t& measurements,
0344                  const std::vector<int>& /*strawSigns*/) {
0345             return m_fastFitter.fit(
0346                 measurements,
0347                 detail::CompSpacePointAuxiliaries::ResidualIdx::bending);
0348           }};
0349       fastResult = fastFit<false, false>(result.measurements, line,
0350                                          resCfg.parsToUse, fitDelegate);
0351     }
0352     if (fastResult.converged) {
0353       if (fitTime) {
0354         fastResult.parameters[toUnderlying(FitParIndex::t0)] -=
0355             (fitOpts.localToGlobal * line.position()).norm() /
0356             PhysicalConstants::c;
0357       }
0358       static_cast<FitParameters&>(result) = std::move(fastResult);
0359       ACTS_DEBUG(__func__ << "() " << __LINE__ << " - Fast fit converged.");
0360       // Use the result from the fast fitter as final answer
0361       if (!m_cfg.fastPreFitter) {
0362         line.updateParameters(result.parameters);
0363         // Recalibrate the measurements before returning
0364         result.measurements = fitOpts.calibrator->calibrate(
0365             fitOpts.calibContext, line.position(), line.direction(),
0366             fitTime ? result.parameters[toUnderlying(FitParIndex::t0)] : 0.,
0367             result.measurements);
0368         return result;
0369       }
0370       // Set convergence flag to false because the full fit comes later.
0371       result.converged = false;
0372     } else {
0373       ACTS_DEBUG(__func__ << "() " << __LINE__ << " - Fit failed.");
0374       return result;
0375     }
0376   }
0377   // Update the drift signs if no re-calibration per iteration is scheduled
0378   if (!m_cfg.recalibrate) {
0379     line.updateParameters(result.parameters);
0380     fitOpts.calibrator->updateSigns(line.position(), line.direction(),
0381                                     result.measurements);
0382   }
0383   /// Proceed with the usual fit
0384   ChiSqCache cache{};
0385   detail::CompSpacePointAuxiliaries pullCalculator{resCfg, logger().clone()};
0386   for (; !result.converged && result.nIter < m_cfg.maxIter; ++result.nIter) {
0387     cache.reset();
0388     // Update the parameters from the last iteration
0389     line.updateParameters(result.parameters);
0390     const double t0 = result.parameters[toUnderlying(FitParIndex::t0)];
0391
0392     ACTS_VERBOSE(__func__ << "() " << __LINE__ << ": Start next iteration "
0393                           << result << " --> " << toString(line.position())
0394                           << " + " << toString(line.direction()) << ".");
0395     // Update the measurements if calibration loop is switched on
0396     if (m_cfg.recalibrate) {
0397       result.measurements = fitOpts.calibrator->calibrate(
0398           fitOpts.calibContext, line.position(), line.direction(), t0,
0399           result.measurements);
0400       // Check whether the measurements are still valid
0401       auto parsBkp = std::move(resCfg.parsToUse);
0402       resCfg.parsToUse =
0403           extractFitablePars(countDoF(result.measurements, fitOpts.selector));
0404
0405       // No valid measurement is left
0406       if (resCfg.parsToUse.empty()) {
0407         ACTS_WARNING(__func__ << "() " << __LINE__ << ":  Line parameters "
0408                               << toString(line.position()) << " + "
0409                               << toString(line.direction()) << ", t0: "
0410                               << t0 / 1._ns << " invalidated all measurements");
0411         return result;
0412       }
0413
0414       if (parsBkp != resCfg.parsToUse) {
0415         // Instantiate an updated pull calculator
0416         pullCalculator =
0417             detail::CompSpacePointAuxiliaries{resCfg, logger().clone()};
0418       }
0419       // update the drift signs
0420       fitOpts.calibrator->updateSigns(line.position(), line.direction(),
0421                                       result.measurements);
0422     }
0423     // Calculate the new chi2
0424     for (const auto& spacePoint : result.measurements) {
0425       // Skip bad measurements
0426       if (fitOpts.selector.connected() && !fitOpts.selector(*spacePoint)) {
0427         continue;
0428       }
0429       // Calculate the residual & derivatives
0430       if (pullCalculator.config().parsToUse.back() == FitParIndex::t0) {
0431         double driftV{fitOpts.calibrator->driftVelocity(fitOpts.calibContext,
0432                                                         *spacePoint)};
0433         double driftA{fitOpts.calibrator->driftAcceleration(
0434             fitOpts.calibContext, *spacePoint)};
0435         pullCalculator.updateFullResidual(line, t0, *spacePoint, driftV,
0436                                           driftA);
0437       } else {
0438         pullCalculator.updateSpatialResidual(line, *spacePoint);
0439       }
0440       // Propagte to the chi2
0441       pullCalculator.updateChiSq(cache, spacePoint->covariance());
0442     }
0443     pullCalculator.symmetrizeHessian(cache);
0444     result.chi2 = cache.chi2;
0445     // Now update the parameters
0446     UpdateStep update{UpdateStep::goodStep};
0447     switch (pullCalculator.config().parsToUse.size()) {
0448       // 2D fit (intercept + inclination angle)
0449       case 2: {
0450         update = updateParameters<2>(pullCalculator.config().parsToUse.front(),
0451                                      cache, result.parameters);
0452         break;
0453       }
0454       // 2D fit + time
0455       case 3: {
0456         update = updateParameters<3>(pullCalculator.config().parsToUse.front(),
0457                                      cache, result.parameters);
0458         break;
0459       }
0460       // 3D spatial fit (x0, y0, theta, phi)
0461       case 4: {
0462         update = updateParameters<4>(pullCalculator.config().parsToUse.front(),
0463                                      cache, result.parameters);
0464         break;
0465       }
0466       // full fit
0467       case 5: {
0468         update = updateParameters<5>(pullCalculator.config().parsToUse.front(),
0469                                      cache, result.parameters);
0470         break;
0471       }
0472       default:
0473         ACTS_WARNING(__func__ << "() " << __LINE__
0474                               << ": Invalid parameter size "
0475                               << pullCalculator.config().parsToUse.size());
0476         return result;
0477     }
0478     /// Check whether the fit parameters are within range
0479     if (!withinRange(result.parameters, pullCalculator.config().parsToUse)) {
0480       update = UpdateStep::outOfBounds;
0481     }
0482     switch (update) {
0483       using enum UpdateStep;
0484       case converged: {
0485         result.converged = true;
0486         break;
0487       }
0488       case goodStep: {
0489         break;
0490       }
0491       case outOfBounds: {
0492         return result;
0493       }
0494     }
0495   }
0496   // Parameters converged
0497   if (result.converged) {
0498     const DoFcounts doF = countDoF(result.measurements, fitOpts.selector);
0499     result.nDoF = doF.nonBending + doF.bending + doF.time -
0500                   pullCalculator.config().parsToUse.size();
0501     line.updateParameters(result.parameters);
0502     const double t0 = result.parameters[toUnderlying(FitParIndex::t0)];
0503     // Recalibrate the measurements before returning
0504     result.measurements = fitOpts.calibrator->calibrate(
0505         fitOpts.calibContext, line.position(), line.direction(), t0,
0506         result.measurements);
0507
0508     // Assign the Hessian
0509     switch (pullCalculator.config().parsToUse.size()) {
0510       // 2D fit (intercept + inclination angle)
0511       case 2: {
0512         fillCovariance<2>(pullCalculator.config().parsToUse.front(),
0513                           cache.hessian, result.covariance);
0514         break;
0515       }
0516       // 2D fit + time
0517       case 3: {
0518         fillCovariance<3>(pullCalculator.config().parsToUse.front(),
0519                           cache.hessian, result.covariance);
0520         break;
0521       }
0522       // 3D spatial fit (x0, y0, theta, phi)
0523       case 4: {
0524         fillCovariance<4>(pullCalculator.config().parsToUse.front(),
0525                           cache.hessian, result.covariance);
0526         break;
0527       }
0528       // full fit
0529       case 5: {
0530         fillCovariance<5>(pullCalculator.config().parsToUse.front(),
0531                           cache.hessian, result.covariance);
0532         break;
0533       }
0534       // No need to warn here -> captured by the fit iterations
0535       default:
0536         break;
0537     }
0538   }
0539   return result;
0540 }
0541
0542 template <unsigned N>
0543 CompositeSpacePointLineFitter::UpdateStep
0544 CompositeSpacePointLineFitter::updateParameters(const FitParIndex firstPar,
0545                                                 ChiSqCache& cache,
0546                                                 ParamVec_t& currentPars) const
0547   requires(N >= 2 && N <= s_nPars)
0548 {
0549   auto firstIdx = toUnderlying(firstPar);
0550   assert(firstIdx + N <= s_nPars);
0551   // Current parameters mapped to an Eigen interface
0552   if constexpr (N == 3) {
0553     constexpr auto t0Idx = toUnderlying(FitParIndex::t0);
0554     assert(firstIdx + N - 1 == 2);
0555     std::swap(currentPars[2], currentPars[t0Idx]);
0556     std::swap(cache.gradient[2], cache.gradient[t0Idx]);
0557     cache.hessian.row(2).swap(cache.hessian.row(t0Idx));
0558     cache.hessian.col(2).swap(cache.hessian.col(t0Idx));
0559   }
0560   Eigen::Map<ActsVector<N>> miniPars{currentPars.data() + firstIdx};
0561   ACTS_VERBOSE(__func__ << "<" << N << ">() - " << __LINE__
0562                         << ": Current parameters " << toString(miniPars)
0563                         << " with chi2: " << cache.chi2 << ",  gradient: "
0564                         << toString(cache.gradient) << ", hessian: \n"
0565                         << toString(cache.hessian));
0566
0567   // Take out the filled block from the gradient
0568   Eigen::Map<const ActsVector<N>> miniGradient{cache.gradient.data() +
0569                                                firstIdx};
0570   // The gradient is already small enough
0571   if (miniGradient.norm() < m_cfg.precCutOff) {
0572     ACTS_DEBUG(__func__ << "<" << N << ">() - " << __LINE__
0573                         << ": Gradient is small enough");
0574     if constexpr (N == 3) {
0575       std::swap(currentPars[2], currentPars[toUnderlying(FitParIndex::t0)]);
0576     }
0577     return UpdateStep::converged;
0578   }
0579   // Take out the filled block from the hessian
0580   Acts::ActsSquareMatrix<N> miniHessian{
0581       cache.hessian.block<N, N>(firstIdx, firstIdx)};
0582   ACTS_VERBOSE(__func__ << "<" << N << ">() - " << __LINE__
0583                         << ": Projected parameters: " << toString(miniPars)
0584                         << " gradient: " << toString(miniGradient)
0585                         << ", hessian: \n"
0586                         << toString(miniHessian)
0587                         << ", determinant: " << miniHessian.determinant());
0588
0589   auto inverseH = safeInverse(miniHessian);
0590   // The Hessian can safely be inverted
0591   if (inverseH) {
0592     const ActsVector<N> update{(*inverseH) * miniGradient};
0593
0594     if (update.norm() < m_cfg.precCutOff) {
0595       ACTS_DEBUG(__func__ << "<" << N << ">() - " << __LINE__ << ": Update "
0596                           << toString(update) << " is negligible small.");
0597       if constexpr (N == 3) {
0598         std::swap(currentPars[2], currentPars[toUnderlying(FitParIndex::t0)]);
0599       }
0600       return UpdateStep::converged;
0601     }
0602     ACTS_VERBOSE(__func__ << "<" << N << ">() - " << __LINE__
0603                           << ": Inverted Hessian \n"
0604                           << toString(*inverseH) << "\n-> Update parameters by "
0605                           << toString(update));
0606     miniPars -= update;
0607
0608   } else {
0609     // Fall back to gradient decent with a fixed damping factor
0610     const ActsVector<N> update{
0611         std::min(m_cfg.gradientStep, miniGradient.norm()) *
0612         miniGradient.normalized()};
0613
0614     ACTS_VERBOSE(__func__ << "<" << N << ">() - " << __LINE__
0615                           << ": Update parameters by " << toString(update));
0616     miniPars -= update;
0617   }
0618   // swap back t0 component if needed
0619   if constexpr (N == 3) {
0620     std::swap(currentPars[2], currentPars[toUnderlying(FitParIndex::t0)]);
0621   }
0622   // Check parameter ranges
0623
0624   return UpdateStep::goodStep;
0625 }
0626
0627 template <unsigned N>
0628 void CompositeSpacePointLineFitter::fillCovariance(const FitParIndex firstPar,
0629                                                    const CovMat_t& hessian,
0630                                                    CovMat_t& covariance) const
0631   requires(N >= 2 && N <= s_nPars)
0632 {
0633   auto firstIdx = toUnderlying(firstPar);
0634   assert(firstIdx + N <= s_nPars);
0635
0636   Acts::ActsSquareMatrix<N> miniHessian =
0637       hessian.block<N, N>(firstIdx, firstIdx).eval();
0638
0639   auto inverseH = safeInverse(miniHessian);
0640   // The Hessian can safely be inverted
0641   if (inverseH) {
0642     auto covBlock = covariance.block<N, N>(firstIdx, firstIdx);
0643     covBlock = *inverseH;
0644
0645     // swap back t0 component if needed
0646     if constexpr (N == 3) {
0647       constexpr auto t0Idx = toUnderlying(FitParIndex::t0);
0648       covariance(t0Idx, t0Idx) = 1.;
0649       covariance.row(2).swap(covariance.row(t0Idx));
0650       covariance.col(2).swap(covariance.col(t0Idx));
0651     }
0652   }
0653
0654   ACTS_DEBUG(__func__ << "<" << N << ">() - " << __LINE__
0655                       << ": Evaluated covariance: \n"
0656                       << toString(covariance));
0657 }
0658
0659 }  // namespace Acts::Experimental