src/Seeding/CompSpacePointAuxiliaries.cpp

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/Seeding/detail/CompSpacePointAuxiliaries.hpp"
0010 //
0011 #include "Acts/Definitions/Units.hpp"
0012 #include "Acts/Surfaces/detail/LineHelper.hpp"
0013 #include "Acts/Utilities/Helpers.hpp"
0014 #include "Acts/Utilities/StringHelpers.hpp"
0015
0016 namespace Acts::Experimental::detail {
0017
0018 using Vector = CompSpacePointAuxiliaries::Vector;
0019
0020 namespace {
0021 double angle(const Vector& v1, const Vector& v2) {
0022   using namespace Acts::UnitLiterals;
0023   return std::acos(std::clamp(v1.dot(v2), -1., 1.)) / 1_degree;
0024 }
0025 constexpr double s_tolerance = 1.e-12;
0026 constexpr auto bendingComp =
0027     toUnderlying(CompSpacePointAuxiliaries::ResidualIdx::bending);
0028 constexpr auto nonBendingComp =
0029     toUnderlying(CompSpacePointAuxiliaries::ResidualIdx::nonBending);
0030 constexpr auto timeComp =
0031     toUnderlying(CompSpacePointAuxiliaries::ResidualIdx::time);
0032 }  // namespace
0033
0034 std::string CompSpacePointAuxiliaries::parName(const FitParIndex idx) {
0035   switch (idx) {
0036     using enum FitParIndex;
0037     case x0:
0038       return "x0";
0039     case y0:
0040       return "y0";
0041     case theta:
0042       return "theta";
0043     case phi:
0044       return "phi";
0045     case t0:
0046       return "t0";
0047     case nPars:
0048       return "nPars";
0049   }
0050   return "unknown";
0051 }
0052
0053 void CompSpacePointAuxiliaries::ChiSqWithDerivatives::reset() {
0054   *this = ChiSqWithDerivatives();
0055 }
0056
0057 CompSpacePointAuxiliaries::CompSpacePointAuxiliaries(
0058     const Config& cfg, std::unique_ptr<const Acts::Logger> logger)
0059     : m_cfg{cfg}, m_logger{std::move(logger)} {
0060   std::ranges::sort(m_cfg.parsToUse);
0061 }
0062 void CompSpacePointAuxiliaries::updateChiSq(
0063     ChiSqWithDerivatives& chiSqObj, const std::array<double, 3>& cov) const {
0064   // Calculate the inverted covariacne
0065   std::array<double, 3> invCov{cov};
0066   for (auto& val : invCov) {
0067     val = val > s_tolerance ? 1. / val : 0.;
0068   }
0069
0070   auto contract = [&invCov, this](const Vector& v1, const Vector& v2) {
0071     const double term = v1[0] * v2[0] * invCov[0] + v1[1] * v2[1] * invCov[1] +
0072                         v1[2] * v2[2] * invCov[2];
0073     ACTS_VERBOSE("updateChiSq() - Contribution from "
0074                  << toString(v1) << " & " << toString(v2) << " is ("
0075                  << (v1[0] * v2[0] * invCov[0]) << ", "
0076                  << (v1[1] * v2[1] * invCov[1]) << ", "
0077                  << v1[2] * v2[2] * invCov[2] << ") -> overall: " << term);
0078     return term;
0079   };
0080   ACTS_VERBOSE("updateChiSq() - Update chi2 value.");
0081   chiSqObj.chi2 += contract(residual(), residual());
0082   for (const auto partial1 : m_cfg.parsToUse) {
0083     ACTS_VERBOSE("updateChiSq() - Update chi2 derivative w.r.t. "
0084                  << parName(partial1) << ".");
0085     chiSqObj.gradient[toUnderlying(partial1)] +=
0086         2. * contract(residual(), gradient(partial1));
0087     for (const auto partial2 : m_cfg.parsToUse) {
0088       if (partial2 > partial1) {
0089         break;
0090       }
0091       ACTS_VERBOSE("updateChiSq() - Update chi2 hessian w.r.t. "
0092                    << parName(partial1) << " & " << parName(partial2) << ".");
0093       chiSqObj.hessian(toUnderlying(partial1), toUnderlying(partial2)) +=
0094           2. * contract(gradient(partial1), gradient(partial2)) +
0095           (m_cfg.useHessian
0096                ? 2. * contract(residual(), hessian(partial1, partial2))
0097                : 0.0);
0098     }
0099   }
0100 }
0101 void CompSpacePointAuxiliaries::symmetrizeHessian(
0102     ChiSqWithDerivatives& chiSqObj) const {
0103   for (const auto partial1 : m_cfg.parsToUse) {
0104     for (const auto partial2 : m_cfg.parsToUse) {
0105       if (partial2 >= partial1) {
0106         break;
0107       }
0108       chiSqObj.hessian(toUnderlying(partial2), toUnderlying(partial1)) =
0109           chiSqObj.hessian(toUnderlying(partial1), toUnderlying(partial2));
0110     }
0111   }
0112 }
0113 const Vector& CompSpacePointAuxiliaries::residual() const {
0114   return m_residual;
0115 }
0116 const Vector& CompSpacePointAuxiliaries::gradient(
0117     const FitParIndex param) const {
0118   assert(toUnderlying(param) < m_gradient.size());
0119   return m_gradient[toUnderlying(param)];
0120 }
0121 const Vector& CompSpacePointAuxiliaries::hessian(
0122     const FitParIndex param1, const FitParIndex param2) const {
0123   const auto idx =
0124       vecIdxFromSymMat<s_nPars>(toUnderlying(param1), toUnderlying(param2));
0125   assert(idx < m_hessian.size());
0126   return m_hessian[idx];
0127 }
0128
0129 void CompSpacePointAuxiliaries::reset() {
0130   m_residual.setZero();
0131   for (Vector3& grad : m_gradient) {
0132     grad.setZero();
0133   }
0134   if (m_cfg.useHessian) {
0135     for (Vector3& hess : m_hessian) {
0136       hess.setZero();
0137     }
0138   }
0139 }
0140 void CompSpacePointAuxiliaries::resetTime() {
0141   m_gradient[toUnderlying(FitParIndex::t0)].setZero();
0142   if (m_cfg.useHessian) {
0143     for (const auto partial : m_cfg.parsToUse) {
0144       const auto pIdx = vecIdxFromSymMat<s_nPars>(toUnderlying(FitParIndex::t0),
0145                                                   toUnderlying(partial));
0146       m_hessian[pIdx].setZero();
0147     }
0148   }
0149 }
0150
0151 bool CompSpacePointAuxiliaries::updateStrawResidual(const Line_t& line,
0152                                                     const Vector& hitMinSeg,
0153                                                     const Vector& wireDir,
0154                                                     const double driftRadius) {
0155   // Fetch the hit position & direction
0156   if (!updateStrawAuxiliaries(line, wireDir)) {
0157     ACTS_WARNING("updateStrawResidual() - The line "
0158                  << toString(line.direction())
0159                  << " is parallel to the measurement: " << toString(wireDir));
0160     return false;
0161   }
0162   // Distance from the segment line to the tube wire
0163   const double lineDist = m_projDir.cross(wireDir).dot(hitMinSeg);
0164   const double resVal = lineDist - driftRadius;
0165   m_residual = resVal * Vector::Unit(bendingComp);
0166
0167   // Calculate the first derivative of the residual
0168   for (const auto partial : m_cfg.parsToUse) {
0169     const auto pIdx = toUnderlying(partial);
0170     if (isDirectionParam(partial)) {
0171       const double partialDist =
0172           m_gradProjDir[pIdx].cross(wireDir).dot(hitMinSeg);
0173       ACTS_VERBOSE("updateStrawResidual() - Partial "
0174                    << parName(partial) << ": " << toString(m_gradProjDir[pIdx])
0175                    << ", residual grad: " << partialDist);
0176       m_gradient[pIdx] = partialDist * Vector::Unit(bendingComp);
0177
0178     } else if (isPositionParam(partial)) {
0179       const double partialDist = -m_projDir.cross(wireDir).dot(
0180           line.gradient(static_cast<LineIndex>(partial)));
0181       ACTS_VERBOSE("updateStrawResidual() - Partial " << parName(partial)
0182                                                       << ": " << partialDist);
0183       m_gradient[pIdx] = partialDist * Vector3::Unit(bendingComp);
0184     }
0185   }
0186
0187   if (!m_cfg.useHessian) {
0188     ACTS_VERBOSE("updateStrawResidual() - Skip Hessian calculation");
0189     return true;
0190   }
0191   // Loop to include the second order derivatvies
0192   for (const auto partial1 : m_cfg.parsToUse) {
0193     if (partial1 == FitParIndex::t0) {
0194       continue;
0195     }
0196     for (const auto partial2 : m_cfg.parsToUse) {
0197       if (partial2 == FitParIndex::t0) {
0198         continue;
0199       } else if (partial2 > partial1) {
0200         break;
0201       }
0202       ACTS_VERBOSE("updateStrawResidual() - Calculate Hessian for parameters "
0203                    << parName(partial1) << ", " << parName(partial2) << ".");
0204       // Second derivative w.r.t to the position parameters is zero
0205       const auto hIdx1 = toUnderlying(partial1);
0206       const auto hIdx2 = toUnderlying(partial2);
0207       const std::size_t resIdx = vecIdxFromSymMat<s_nPars>(hIdx1, hIdx2);
0208       if (!(isDirectionParam(partial1) || isDirectionParam(partial2)) ||
0209           partial2 == FitParIndex::t0 || partial1 == FitParIndex::t0) {
0210         m_hessian[resIdx].setZero();
0211         continue;
0212       }
0213       const std::size_t projDirHess =
0214           vecIdxFromSymMat<s_nLinePars>(hIdx1, hIdx2);
0215       // Pure angular derivatives of the residual
0216       if (isDirectionParam(partial1) && isDirectionParam(partial2)) {
0217         // clang-format off
0218         const double partialSqDist = m_hessianProjDir[projDirHess].cross(wireDir).dot(hitMinSeg);
0219         // clang-format on
0220         m_hessian[resIdx] = partialSqDist * Vector3::Unit(bendingComp);
0221       } else {
0222         // Angular & Spatial mixed terms
0223         const auto angParam = isDirectionParam(partial1) ? hIdx1 : hIdx2;
0224         const auto posParam = static_cast<LineIndex>(
0225             isDirectionParam(partial1) ? partial2 : partial1);
0226         // clang-format off
0227         m_hessian[resIdx] = -m_gradProjDir[angParam].cross(wireDir).dot(line.gradient(posParam)) * Vector3::Unit(bendingComp);
0228         // clang-format on
0229       }
0230       ACTS_VERBOSE("updateStrawResidual() - Hessian of the residual is "
0231                    << m_hessian[resIdx][bendingComp]);
0232     }
0233   }
0234   return true;
0235 }
0236 inline bool CompSpacePointAuxiliaries::updateStrawAuxiliaries(
0237     const Line_t& line, const Vector& wireDir) {
0238   const Vector& lineDir = line.direction();
0239   // Between two calls the wire projection has not changed
0240   const double wireProject = lineDir.dot(wireDir);
0241
0242   m_wireProject = wireProject;
0243   const double projDirLenSq = 1. - square(m_wireProject);
0244   // The line is parallel to the wire
0245   if (projDirLenSq < s_tolerance) {
0246     ACTS_VERBOSE("updateStrawAuxiliaries() - Line & wire are parallel: "
0247                  << toString(wireDir) << " vs. " << toString(lineDir));
0248     m_invProjDirLenSq = 0.;
0249     reset();
0250     return false;
0251   }
0252   m_invProjDirLenSq = 1. / projDirLenSq;
0253   m_invProjDirLen = std::sqrt(m_invProjDirLenSq);
0254   // Project the segment line onto the wire plane and normalize
0255   Vector newProjDir = (lineDir - m_wireProject * wireDir) * m_invProjDirLen;
0256   ACTS_VERBOSE("updateStrawAuxiliaries() - Projected direction is "
0257                << toString(newProjDir) << ", |D| " << newProjDir.norm());
0258
0259   if (Acts::abs(newProjDir.dot(m_projDir) - 1.) <
0260       2. * std::numeric_limits<double>::epsilon()) {
0261     ACTS_VERBOSE(
0262         "updateStrawAuxiliaries() - The old & the new dir projection coincide. "
0263         << "Don't update the auxiliaries");
0264     return true;
0265   }
0266
0267   ACTS_VERBOSE("updateStrawAuxiliaries() - Update variables.");
0268   m_projDir = std::move(newProjDir);
0269
0270   // Loop over all configured parameters and calculate the partials
0271   // of the wire projection
0272   for (auto partial : m_cfg.parsToUse) {
0273     // Skip the parameters that are not directional
0274     if (!isDirectionParam(partial)) {
0275       continue;
0276     }
0277     const std::size_t pIdx = toUnderlying(partial);
0278     const Vector& lGrad{line.gradient(static_cast<LineIndex>(partial))};
0279     m_projDirLenPartial[pIdx] = lGrad.dot(wireDir);
0280     // clang-format off
0281     m_gradProjDir[pIdx] = m_invProjDirLen * lGrad - (m_invProjDirLen *m_projDirLenPartial[pIdx]) * wireDir +
0282                           (m_projDirLenPartial[pIdx] * m_wireProject * m_invProjDirLenSq) * m_projDir;
0283     // clang-format on
0284     ACTS_VERBOSE("updateStrawAuxiliaries() - First derivative w.r.t. "
0285                  << parName(partial) << " is "
0286                  << toString(m_gradProjDir[pIdx]));
0287     // skip the Hessian calculation, if toggled
0288     if (!m_cfg.useHessian) {
0289       continue;
0290     }
0291     for (auto partial1 : m_cfg.parsToUse) {
0292       // Skip the parameters that are not directional or avoid double counting
0293       if (!isDirectionParam(partial1)) {
0294         continue;
0295       } else if (partial1 > partial) {
0296         break;
0297       }
0298       const auto param = toUnderlying(partial);
0299       const auto param1 = toUnderlying(partial1);
0300       const auto resIdx = vecIdxFromSymMat<s_nLinePars>(param, param1);
0301       const Vector& lHessian = line.hessian(static_cast<LineIndex>(partial),
0302                                             static_cast<LineIndex>(partial1));
0303       const double partSqLineProject = lHessian.dot(wireDir);
0304
0305       auto calcMixedTerms = [this](const std::size_t p1, const std::size_t p2) {
0306         return m_projDirLenPartial[p1] * m_wireProject * m_gradProjDir[p2] +
0307                0.5 * m_projDirLenPartial[p1] * m_projDirLenPartial[p2] *
0308                    m_invProjDirLenSq * m_projDir;
0309       };
0310       auto calcMixedHessian = [&]() -> Vector {
0311         if (param1 != param) {
0312           return calcMixedTerms(param1, param) + calcMixedTerms(param, param1);
0313         }
0314         return 2. * calcMixedTerms(param, param1);
0315       };
0316       // clang-format off
0317       m_hessianProjDir[resIdx] = (lHessian - partSqLineProject * wireDir) * m_invProjDirLen +
0318                                m_invProjDirLenSq * (calcMixedHessian() + (partSqLineProject * m_wireProject) * m_projDir);
0319       // clang-format on
0320       ACTS_VERBOSE("updateStrawAuxiliaries() - Hessian w.r.t. "
0321                    << parName(partial) << ", " << parName(partial1) << " is "
0322                    << toString(m_hessianProjDir[resIdx]));
0323     }
0324   }
0325   return true;
0326 }
0327
0328 void CompSpacePointAuxiliaries::updateAlongTheStraw(const Line_t& line,
0329                                                     const Vector& hitMinSeg,
0330                                                     const Vector& wireDir) {
0331   // clang-format off
0332   m_residual[nonBendingComp] = m_invProjDirLenSq *
0333                             hitMinSeg.dot(m_wireProject * line.direction() - wireDir);
0334   // clang-format on
0335   ACTS_VERBOSE("updateAlongTheStraw() - Residual is "
0336                << m_residual[nonBendingComp]);
0337
0338   for (const auto partial : m_cfg.parsToUse) {
0339     if (partial == FitParIndex::t0) {
0340       continue;
0341     }
0342     const auto param = toUnderlying(partial);
0343     const Vector& lGradient = line.gradient(static_cast<LineIndex>(partial));
0344     if (isDirectionParam(partial)) {
0345       // clang-format off
0346       m_gradient[param][nonBendingComp] = m_invProjDirLenSq * (
0347                                        hitMinSeg.dot(m_wireProject * lGradient + m_projDirLenPartial[param] * line.direction()) +
0348                                        2. * m_residual[nonBendingComp] * m_wireProject * m_projDirLenPartial[param]);
0349       // clang-format on
0350
0351     } else if (isPositionParam(partial)) {
0352       // clang-format off
0353       m_gradient[param][nonBendingComp] = -m_invProjDirLenSq * (lGradient.dot(m_wireProject * line.direction() - wireDir));
0354       // clang-format on
0355     }
0356     ACTS_VERBOSE("updateAlongTheStraw() - First derivative w.r.t "
0357                  << parName(partial) << " is "
0358                  << m_gradient[param][nonBendingComp]);
0359   }
0360   if (!m_cfg.useHessian) {
0361     return;
0362   }
0363
0364   for (auto partial1 : m_cfg.parsToUse) {
0365     if (partial1 == FitParIndex::t0) {
0366       continue;
0367     }
0368     for (auto partial2 : m_cfg.parsToUse) {
0369       if (partial2 == FitParIndex::t0) {
0370         continue;
0371       } else if (partial2 > partial1) {
0372         break;
0373       }
0374       // second derivative w.r.t intercepts is always 0
0375       if (!(isDirectionParam(partial2) || isDirectionParam(partial1)) ||
0376           partial2 == FitParIndex::t0 || partial1 == FitParIndex::t0) {
0377         continue;
0378       }
0379       const auto param = toUnderlying(partial1);
0380       const auto param1 = toUnderlying(partial2);
0381       const auto hessIdx = vecIdxFromSymMat<s_nPars>(param, param1);
0382       if (isDirectionParam(partial1) && isDirectionParam(partial2)) {
0383         // clang-format off
0384         auto calcMixedHessian = [this, &hitMinSeg, &line](const std::size_t p1,
0385                                                           const std::size_t p2) -> double {
0386           const double term1 = hitMinSeg.dot(line.gradient(static_cast<LineIndex>(p1))) * m_projDirLenPartial[p2];
0387           const double term2 = m_residual[nonBendingComp] * m_projDirLenPartial[p1] * m_projDirLenPartial[p2];
0388           const double term3 = 2. * m_wireProject * m_projDirLenPartial[p1] * m_gradient[p2][nonBendingComp];
0389           return (term1 + term2 + term3) * m_invProjDirLenSq;
0390         };
0391         const Vector& lHessian = line.hessian(static_cast<LineIndex>(partial1), static_cast<LineIndex>(partial2));
0392         const double projHess = lHessian.dot(wireDir);
0393         m_hessian[hessIdx][nonBendingComp] = (param != param1 ? calcMixedHessian(param, param1) + calcMixedHessian(param1, param)
0394                                                               : 2. * calcMixedHessian(param1, param)) +
0395                                         m_invProjDirLenSq * (hitMinSeg.dot(m_wireProject * lHessian  + projHess* line.direction()) +
0396                                                              2. * m_residual[nonBendingComp] * m_wireProject * projHess);
0397         // clang-format on
0398       } else {
0399         // Mixed positional and angular derivative
0400         const auto angParam = static_cast<LineIndex>(
0401             isDirectionParam(partial1) ? partial1 : partial2);
0402         const auto posParam = static_cast<LineIndex>(
0403             isDirectionParam(partial1) ? partial2 : partial1);
0404         const auto angIdx = toUnderlying(angParam);
0405         // clang-format off
0406         m_hessian[hessIdx][nonBendingComp] = m_invProjDirLenSq * (
0407                   2.* (m_gradient[toUnderlying(posParam)][nonBendingComp] * m_wireProject * m_projDirLenPartial[angIdx])
0408                   - line.gradient(posParam).dot(m_wireProject * line.gradient(angParam) + m_projDirLenPartial[angIdx] * line.direction()));
0409         // clang-format on
0410       }
0411       ACTS_VERBOSE("updateAlongTheStraw() - Second derivative w.r.t. "
0412                    << parName(partial1) << ", " << parName(partial2) << " is. "
0413                    << m_hessian[hessIdx][nonBendingComp]);
0414     }
0415   }
0416 }
0417
0418 void CompSpacePointAuxiliaries::updateStripResidual(
0419     const Line_t& line, const Vector& normal, const Vector& sensorN,
0420     const Vector& sensorD, const Vector& stripPos, const bool isBending,
0421     const bool isNonBending) {
0422   using namespace Acts::UnitLiterals;
0423
0424   const double normDot = normal.dot(line.direction());
0425
0426   constexpr double tolerance = 1.e-12;
0427   if (std::abs(normDot) < tolerance) {
0428     reset();
0429     ACTS_WARNING(
0430         "updateStripResidual() - Segment line is embedded into the strip plane "
0431         << toString(line.direction()) << ", normal: " << toString(normal));
0432     return;
0433   }
0434   const double planeOffSet = normal.dot(stripPos);
0435   ACTS_VERBOSE("updateStripResidual() - Plane normal: "
0436                << toString(normal) << " |" << normal.norm()
0437                << "|, line direction: " << toString(line.direction())
0438                << ", angle: " << angle(normal, line.direction()));
0439   const double invNormDot = 1. / normDot;
0440   const double travelledDist =
0441       (planeOffSet - line.position().dot(normal)) * invNormDot;
0442   ACTS_VERBOSE("updateStripResidual() - Need to travel "
0443                << travelledDist << " mm to reach the strip plane. ");
0444
0445   const Vector& b1 = isBending ? sensorN : sensorD;
0446   const Vector& b2 = isBending ? sensorD : sensorN;
0447
0448   const double b1DotB2 = b1.dot(b2);
0449   if (Acts::abs(Acts::abs(b1DotB2) - 1.) < tolerance) {
0450     ACTS_WARNING("updateStripResidual() The two vectors "
0451                  << toString(b1) << ", " << toString(b2) << " with angle: "
0452                  << angle(b1, b2) << " don't span the plane.");
0453     reset();
0454     return;
0455   }
0456   // Linear independent vectors
0457   // Normal vector is indeed normal onto the plane spaned by these two vectors
0458   assert(Acts::abs(b1.dot(normal)) < tolerance);
0459   assert(Acts::abs(b2.dot(normal)) < tolerance);
0460   assert(isBending || isNonBending);
0461   const bool decompStereo =
0462       (m_cfg.calcAlongStrip || (isBending && isNonBending)) &&
0463       Acts::abs(b1DotB2) > tolerance;
0464   if (decompStereo) {
0465     // A = lambda B1 + kappa B2
0466     //                <A, B2> - <A,B1><B1,B2>
0467     //   --> kappa = --------------------------
0468     //                    1 - <B1,B2>^{2}
0469     //                <A, B1> - <A,B2><B1,B2>
0470     //   --> lambda = --------------------------
0471     //                    1 - <B1,B2>^{2}
0472     const double invDist = 1. / (1. - square(b1DotB2));
0473     m_stereoTrf(bendingComp, bendingComp) =
0474         m_stereoTrf(nonBendingComp, nonBendingComp) = invDist;
0475     m_stereoTrf(bendingComp, nonBendingComp) =
0476         m_stereoTrf(nonBendingComp, bendingComp) = -b1DotB2 * invDist;
0477   }
0478   ACTS_VERBOSE("updateStripResidual() - Measures bending "
0479                << (isBending ? "yes" : "no") << ", measures non-bending "
0480                << (isNonBending ? "yes" : "no"));
0481   ACTS_VERBOSE("updateStripResidual() - Strip orientation b1: "
0482                << toString(b1) << " |" << b1.norm() << "|"
0483                << ", b2: " << toString(b2) << " |" << b2.norm() << "|"
0484                << ", stereo angle: " << angle(b1, b2));
0485
0486   auto assignResidual = [&](const Vector& calcDistance, Vector& residual) {
0487     residual[bendingComp] =
0488         isBending || m_cfg.calcAlongStrip ? b1.dot(calcDistance) : 0.;
0489     residual[nonBendingComp] =
0490         isNonBending || m_cfg.calcAlongStrip ? b2.dot(calcDistance) : 0.;
0491     if (decompStereo) {
0492       auto spatial = residual.block<2, 1>(0, 0);
0493       spatial = m_stereoTrf * spatial;
0494     }
0495     residual[timeComp] = 0.;
0496     ACTS_VERBOSE(
0497         "updateStripResidual() - Distance: "
0498         << toString(calcDistance) << ", <calc, n> =" << calcDistance.dot(normal)
0499         << " -> residual: " << toString(residual) << " --> closure test:"
0500         << toString(residual[bendingComp] * b1 +
0501                     residual[nonBendingComp] * b2));
0502   };
0503   // Update the residual accordingly
0504   assignResidual(line.position() + travelledDist * line.direction() - stripPos,
0505                  m_residual);
0506   for (const auto partial : m_cfg.parsToUse) {
0507     ACTS_VERBOSE("updateStripResidual() - Calculate partial derivative w.r.t "
0508                  << parName(partial));
0509     switch (partial) {
0510       case FitParIndex::phi:
0511       case FitParIndex::theta: {
0512         // clang-format off
0513         const Vector& lGrad = line.gradient(static_cast<LineIndex>(partial));
0514         const double partialDist = -travelledDist * invNormDot * normal.dot(lGrad);
0515         const Vector gradCmp = travelledDist * lGrad +
0516                                partialDist * line.direction();
0517         // clang-format on
0518         assignResidual(gradCmp, m_gradient[toUnderlying(partial)]);
0519         break;
0520       }
0521       case FitParIndex::y0:
0522       case FitParIndex::x0: {
0523         // clang-format off
0524         const Vector& lGrad = line.gradient(static_cast<LineIndex>(partial));
0525         const Vector gradCmp = lGrad - lGrad.dot(normal) * invNormDot * line.direction();
0526         assignResidual(gradCmp, m_gradient[toUnderlying(partial)]);
0527         // clang-format on
0528         break;
0529       }
0530       default:
0531         // Don't calculate anything for time
0532         break;
0533     }
0534   }
0535
0536   if (!m_cfg.useHessian) {
0537     return;
0538   }
0539   for (const auto partial1 : m_cfg.parsToUse) {
0540     for (const auto partial2 : m_cfg.parsToUse) {
0541       // Avoid double counting
0542       if (partial2 > partial1) {
0543         break;
0544       }
0545       const auto param = toUnderlying(partial1);
0546       const auto param1 = toUnderlying(partial2);
0547       const auto resIdx = vecIdxFromSymMat<s_nPars>(param, param1);
0548       // At least one parameter needs to be a directional one
0549       if (!(isDirectionParam(partial1) || isDirectionParam(partial2)) ||
0550           partial2 == FitParIndex::t0 || partial1 == FitParIndex::t0) {
0551         m_hessian[resIdx].setZero();
0552         continue;
0553       }
0554       ACTS_VERBOSE(
0555           "updateStripResidual() - Calculate second partial derivative w.r.t "
0556           << parName(partial1) << ", " << parName(partial2));
0557
0558       if (isDirectionParam(partial1) && isDirectionParam(partial2)) {
0559         // clang-format off
0560         auto calcMixedTerms = [&line, &normal, &invNormDot, &b1, &b2, this](const std::size_t p1,
0561                                                                             const std::size_t p2) {
0562
0563           return -normal.dot(line.gradient(static_cast<LineIndex>(p1))) * invNormDot *
0564                           (m_gradient[p2][bendingComp] * b1 + m_gradient[p2][nonBendingComp] * b2);
0565         };
0566         const Vector& lHessian = line.hessian(static_cast<LineIndex>(partial1),
0567                                               static_cast<LineIndex>(partial2));
0568         const Vector hessianCmp = travelledDist * (lHessian - normal.dot(lHessian) * invNormDot * line.direction()) +
0569                                   calcMixedTerms(param1, param) + calcMixedTerms(param, param1);
0570         assignResidual(hessianCmp, m_hessian[resIdx]);
0571         // clang-format on
0572       } else {
0573         // clang-format off
0574         const auto angParam = static_cast<LineIndex>(isDirectionParam(partial1) ? partial1 : partial2);
0575         const auto posParam = static_cast<LineIndex>(isDirectionParam(partial1) ? partial2 : partial1);
0576         const double lH = (normal.dot(line.gradient(posParam)) * invNormDot);
0577
0578         const Vector hessianCmp = lH * ((normal.dot(line.gradient(angParam)) * invNormDot) * line.direction() -
0579                                         line.gradient(angParam));
0580         // clang-format on
0581         assignResidual(hessianCmp, m_hessian[resIdx]);
0582       }
0583     }
0584   }
0585 }
0586
0587 void CompSpacePointAuxiliaries::updateTimeStripRes(
0588     const Vector& sensorN, const Vector& sensorD, const Vector& stripPos,
0589     const bool isBending, const double recordTime, const double timeOffset) {
0590   const Vector& b1 = isBending ? sensorN : sensorD;
0591   const Vector& b2 = isBending ? sensorD : sensorN;
0592
0593   auto positionInPlane = [&b1, &b2](const Vector& residVec) {
0594     return b1 * residVec[bendingComp] + b2 * residVec[nonBendingComp];
0595   };
0596
0597   // Calculate the line intersection in the global frame
0598   const Vector globIsect =
0599       m_cfg.includeToF
0600           ? m_cfg.localToGlobal * (positionInPlane(residual()) + stripPos)
0601           : Vector::Zero();
0602   // To calculate the time of flight
0603   const double globDist = globIsect.norm();
0604   const double ToF = globDist / PhysicalConstants::c + timeOffset;
0605   ACTS_VERBOSE("Global intersection point: "
0606                << toString(globIsect) << " -> distance: " << globDist
0607                << " -> time of flight takinig t0 into account: " << ToF);
0608
0609   m_residual[timeComp] = recordTime - ToF;
0610   constexpr auto timeIdx = toUnderlying(FitParIndex::t0);
0611
0612   const double invDist = m_cfg.includeToF && globDist > s_tolerance
0613                              ? 1. / (PhysicalConstants::c * globDist)
0614                              : 0.;
0615   for (const auto partial1 : m_cfg.parsToUse) {
0616     if (partial1 == FitParIndex::t0) {
0617       m_gradient[timeIdx] = -Vector::Unit(timeComp);
0618     }
0619     // Time component of the spatial residual needs to be updated
0620     else if (m_cfg.includeToF) {
0621       Vector& gradVec = m_gradient[toUnderlying(partial1)];
0622       gradVec[timeComp] = -globIsect.dot(m_cfg.localToGlobal.linear() *
0623                                          positionInPlane(gradVec)) *
0624                           invDist;
0625       ACTS_VERBOSE("Partial of the time residual  w.r.t. "
0626                    << parName(partial1) << ": " << gradVec[timeComp] << ".");
0627     }
0628   }
0629
0630   if (!m_cfg.useHessian || !m_cfg.includeToF) {
0631     return;
0632   }
0633   for (const auto partial1 : m_cfg.parsToUse) {
0634     for (const auto partial2 : m_cfg.parsToUse) {
0635       if (partial2 > partial1) {
0636         break;
0637       }
0638       const auto param1 = toUnderlying(partial1);
0639       const auto param2 = toUnderlying(partial2);
0640       Vector& hessVec = m_hessian[vecIdxFromSymMat<s_nPars>(param1, param2)];
0641       if (partial1 != FitParIndex::t0) {
0642         // clang-format off
0643         hessVec[timeComp] = -( globIsect.dot(m_cfg.localToGlobal.linear()*positionInPlane(hessVec))  +
0644                            positionInPlane(gradient(partial1)).dot(positionInPlane(gradient(partial2)))) * invDist
0645                         + m_gradient[param1][timeComp] * m_gradient[param2][timeComp] * invDist;
0646         // clang-format on
0647         ACTS_VERBOSE("Hessian of the time residual w.r.t. "
0648                      << parName(partial1) << ", " << parName(partial2) << ": "
0649                      << hessVec[timeComp] << ".");
0650       } else {
0651         hessVec.setZero();
0652       }
0653     }
0654   }
0655 }
0656
0657 void CompSpacePointAuxiliaries::updateTimeStrawRes(
0658     const Line_t& line, const Vector& hitMinSeg, const Vector& wireDir,
0659     const double driftR, const double driftV, const double driftA) {
0660   using namespace Acts::detail::LineHelper;
0661   using namespace Acts::UnitLiterals;
0662
0663   const double dSign = std::copysign(1., driftR);
0664   // Only assign drift velocity and acceleration
0665   if (!m_cfg.includeToF) {
0666     resetTime();
0667     constexpr auto timeIdx = toUnderlying(FitParIndex::t0);
0668     constexpr auto hessIdx = vecIdxFromSymMat<s_nPars>(timeIdx, timeIdx);
0669     m_gradient[timeIdx] = dSign * driftV * Vector::Unit(bendingComp);
0670     m_hessian[hessIdx] = -dSign * driftA * Vector::Unit(bendingComp);
0671     return;
0672   }
0673
0674   const double travDist = hitMinSeg.dot(m_projDir * m_invProjDirLen);
0675
0676   const Vector clApproach = line.point(travDist);
0677   const Vector globApproach = m_cfg.localToGlobal * clApproach;
0678   ACTS_VERBOSE("updateTimeStrawRes() - Point of closest approach along line "
0679                << toString(clApproach));
0680
0681   const double distance = globApproach.norm();
0682   const double ToF = distance / PhysicalConstants::c;
0683   ACTS_VERBOSE("updateTimeStrawRes() - Distance from the global origin: "
0684                << distance << " -> time of flight: " << ToF / 1._ns);
0685   ACTS_VERBOSE("updateTimeStrawRes() - Drift radius: "
0686                << driftR << ", driftV: " << driftV << ", driftA: " << driftA);
0687
0688   const double invDist =
0689       distance > s_tolerance ? 1. / (distance * PhysicalConstants::c) : 0.;
0690   for (const auto partial : m_cfg.parsToUse) {
0691     const auto idx = toUnderlying(partial);
0692     if (partial == FitParIndex::t0) {
0693       m_gradient[idx] = dSign * driftV * Vector::Unit(bendingComp);
0694       continue;
0695     }
0696     const Vector3& lGrad = line.gradient(static_cast<LineIndex>(partial));
0697     if (isPositionParam(partial)) {
0698       // clang-format off
0699       m_gradCloseApproach[idx] = lGrad - lGrad.dot(m_projDir * m_invProjDirLen) * line.direction();
0700       // clang-format on
0701     } else if (isDirectionParam(partial)) {
0702       // clang-format off
0703       m_gradCloseApproach[idx] =  hitMinSeg.dot(m_gradProjDir[idx]) * m_invProjDirLen * line.direction()
0704                         + travDist * lGrad
0705                         + m_wireProject  * m_projDirLenPartial[idx] * m_invProjDirLenSq * travDist  * line.direction();
0706       // clang-format on
0707     }
0708     m_partialApproachDist[idx] = globApproach.dot(m_cfg.localToGlobal.linear() *
0709                                                   m_gradCloseApproach[idx]) *
0710                                  invDist;
0711     ACTS_VERBOSE("updateTimeStrawRes() - Correct the partial derivative w.r.t. "
0712                  << parName(partial) << " " << m_gradient[idx][bendingComp]
0713                  << " by " << toString(m_gradCloseApproach[idx])
0714                  << " dCoA: " << m_partialApproachDist[idx] * driftV << " -> "
0715                  << (m_gradient[idx][bendingComp] +
0716                      dSign * m_partialApproachDist[idx] * driftV));
0717     m_gradient[idx][bendingComp] -=
0718         -dSign * m_partialApproachDist[idx] * driftV;
0719   }
0720
0721   if (!m_cfg.useHessian) {
0722     return;
0723   }
0724
0725   for (const auto partial1 : m_cfg.parsToUse) {
0726     for (const auto partial2 : m_cfg.parsToUse) {
0727       if (partial2 > partial1) {
0728         break;
0729       }
0730       const auto idx1 = toUnderlying(partial1);
0731       const auto idx2 = toUnderlying(partial2);
0732       const auto hessIdx = vecIdxFromSymMat<s_nPars>(idx1, idx2);
0733       if (partial1 == FitParIndex::t0) {
0734         if (partial2 == FitParIndex::t0) {
0735           m_hessian[hessIdx] = -dSign * driftA * Vector::Unit(bendingComp);
0736         } else {
0737           m_hessian[hessIdx] = -dSign * driftA * m_partialApproachDist[idx2] *
0738                                Vector::Unit(bendingComp);
0739         }
0740         ACTS_VERBOSE("updateTimeStrawRes() -"
0741                      << " Second partial derivative of the drift "
0742                         "radius w.r.t. "
0743                      << parName(partial1) << ", " << parName(partial2) << " is "
0744                      << toString(m_hessian[hessIdx]));
0745         continue;
0746       }
0747       Vector hessCmp{Vector::Zero()};
0748       if (isDirectionParam(partial1) && isDirectionParam(partial2)) {
0749         // clang-format off
0750         auto calcMixedTerms = [this, &hitMinSeg, &travDist, &line]
0751            (const std::size_t pIdx1, const std::size_t pIdx2) ->Vector {
0752            return m_projDirLenPartial[pIdx1] * m_wireProject * m_invProjDirLenSq * m_gradCloseApproach[pIdx2]
0753                   + hitMinSeg.dot(m_gradProjDir[pIdx1]) * m_invProjDirLen * line.gradient(static_cast<LineIndex>(pIdx2))
0754                   + 0.5 * m_invProjDirLenSq *  m_projDirLenPartial[pIdx1] * m_projDirLenPartial[pIdx2] *
0755                    travDist*( 1. + Acts::pow(m_wireProject* m_invProjDirLen, 2)) * line.direction();
0756         };
0757
0758         const Vector& lHessian = line.hessian(static_cast<LineIndex>(partial1),
0759                                               static_cast<LineIndex>(partial2));
0760
0761         const std::size_t hessProjIdx = vecIdxFromSymMat<s_nLinePars>(idx1, idx2);
0762         hessCmp = m_invProjDirLen * hitMinSeg.dot(m_hessianProjDir[hessProjIdx]) * line.direction()
0763                  + travDist*lHessian
0764                  + travDist*m_wireProject*lHessian.dot(wireDir)*m_invProjDirLenSq *line.direction();
0765         // clang-format on
0766         if (idx1 != idx2) {
0767           hessCmp += calcMixedTerms(idx1, idx2) + calcMixedTerms(idx2, idx1);
0768         } else {
0769           hessCmp += 2. * calcMixedTerms(idx1, idx2);
0770         }
0771       } else if (isDirectionParam(partial1) || isDirectionParam(partial2)) {
0772         // clang-format off
0773         const auto angParam = isDirectionParam(partial1) ? idx1 : idx2;
0774         const Vector& posPartial = line.gradient(static_cast<LineIndex>(isDirectionParam(partial1) ? partial2 : partial1));
0775         const Vector& angPartial = line.gradient(static_cast<LineIndex>(angParam));
0776         const double gradProjDist = - posPartial.dot(m_projDir) * m_invProjDirLen;
0777         hessCmp = - posPartial.dot(m_gradProjDir[angParam]) * m_invProjDirLen * line.direction()
0778                 + gradProjDist * angPartial
0779                 + m_wireProject  * m_projDirLenPartial[angParam] *m_invProjDirLenSq * gradProjDist * line.direction();
0780         // clang-format on
0781       }
0782       // clang-format off
0783       const double partialCoA = m_gradCloseApproach[idx1].dot(m_gradCloseApproach[idx2]) * invDist +
0784                                 globApproach.dot(m_cfg.localToGlobal.linear()*hessCmp) * invDist -
0785                                 m_partialApproachDist[idx1] * m_partialApproachDist[idx2]* invDist * Acts::pow(PhysicalConstants::c,2);
0786       const double hessianR = driftA * m_partialApproachDist[idx1] * m_partialApproachDist[idx2]
0787                             - driftV * partialCoA;
0788       // clang-format on
0789       ACTS_VERBOSE(
0790           "updateTimeStrawRes() - Correct the second partial derivative w.r.t "
0791           << parName(partial1) << ", " << parName(partial2) << " "
0792           << m_hessian[hessIdx][bendingComp] << " by hessianR: " << hessianR
0793           << ", partialCoA: " << partialCoA << " -> "
0794           << m_hessian[hessIdx][bendingComp] - dSign * hessianR);
0795
0796       m_hessian[hessIdx][bendingComp] -= dSign * hessianR;
0797     }
0798   }
0799 }
0800
0801 }  // namespace Acts::Experimental::detail