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