src/Seeding/StrawLineFitAuxillaries.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/StrawLineFitAuxiliaries.hpp"
0010 #include "Acts/Utilities/StringHelpers.hpp"
0011 namespace Acts::Experimental::detail {
0012
0013 using Vector = StrawLineFitAuxiliaries::Vector;
0014
0015 namespace {
0016 double angle(const Vector& v1, const Vector& v2) {
0017   using namespace Acts::UnitLiterals;
0018   return std::acos(std::clamp(v1.dot(v2), -1., 1.)) / 1_degree;
0019 }
0020 }  // namespace
0021
0022 std::string StrawLineFitAuxiliaries::parName(const FitParIndex idx) {
0023   switch (idx) {
0024     using enum FitParIndex;
0025     case x0:
0026       return "x0";
0027     case y0:
0028       return "y0";
0029     case theta:
0030       return "theta";
0031     case phi:
0032       return "phi";
0033     case t0:
0034       return "t0";
0035     case nPars:
0036       return "nPars";
0037   }
0038   return "unknown";
0039 }
0040 StrawLineFitAuxiliaries::StrawLineFitAuxiliaries(
0041     const Config& cfg, std::unique_ptr<const Acts::Logger> logger)
0042     : m_cfg{cfg}, m_logger{std::move(logger)} {
0043   std::ranges::sort(m_cfg.parsToUse);
0044 }
0045 const Vector& StrawLineFitAuxiliaries::residual() const {
0046   return m_residual;
0047 }
0048 const Vector& StrawLineFitAuxiliaries::gradient(const FitParIndex param) const {
0049   const auto par = static_cast<std::uint8_t>(param);
0050   assert(par < m_gradient.size());
0051   return m_gradient[par];
0052 }
0053 const Vector& StrawLineFitAuxiliaries::hessian(const FitParIndex param1,
0054                                                const FitParIndex param2) const {
0055   const auto idx = vecIdxFromSymMat<s_nPars>(static_cast<std::size_t>(param1),
0056                                              static_cast<std::size_t>(param2));
0057   assert(idx < m_hessian.size());
0058   return m_hessian[idx];
0059 }
0060
0061 void StrawLineFitAuxiliaries::reset() {
0062   m_residual.setZero();
0063   for (Vector3& grad : m_gradient) {
0064     grad.setZero();
0065   }
0066   if (m_cfg.useHessian) {
0067     for (Vector3& hess : m_hessian) {
0068       hess.setZero();
0069     }
0070   }
0071 }
0072
0073 bool StrawLineFitAuxiliaries::updateStrawResidual(const Line_t& line,
0074                                                   const Vector& hitMinSeg,
0075                                                   const Vector& wireDir,
0076                                                   const double driftRadius) {
0077   // Fetch the hit position & direction
0078   if (!updateStrawAuxiliaries(line, wireDir)) {
0079     ACTS_WARNING("updateStrawResidual() - The line "
0080                  << toString(line.direction())
0081                  << " is parallel to the measurement: " << toString(wireDir));
0082     return false;
0083   }
0084   /** Distance from the segment line to the tube wire */
0085   const double lineDist = m_projDir.cross(wireDir).dot(hitMinSeg);
0086   const double resVal = lineDist - driftRadius;
0087   m_residual = resVal * Vector::Unit(bending);
0088
0089   /** Calculate the first derivative of the residual */
0090   for (const auto partial : m_cfg.parsToUse) {
0091     const auto pIdx = static_cast<std::uint8_t>(partial);
0092     if (isDirectionParam(partial)) {
0093       const double partialDist =
0094           m_gradProjDir[pIdx].cross(wireDir).dot(hitMinSeg);
0095       ACTS_VERBOSE("updateStrawResidual() - Partial "
0096                    << parName(partial) << ": " << toString(m_gradProjDir[pIdx])
0097                    << ", residual grad: " << partialDist);
0098       m_gradient[pIdx] = partialDist * Vector::Unit(bending);
0099
0100     } else if (isPositionParam(partial)) {
0101       const double partialDist = -m_projDir.cross(wireDir).dot(
0102           line.gradient(static_cast<LineIndex>(partial)));
0103       ACTS_VERBOSE("updateStrawResidual() - Partial " << parName(partial)
0104                                                       << ": " << partialDist);
0105       m_gradient[pIdx] = partialDist * Vector3::Unit(bending);
0106     }
0107   }
0108
0109   if (!m_cfg.useHessian) {
0110     ACTS_VERBOSE("updateStrawResidual() - Skip Hessian calculation");
0111     return true;
0112   }
0113   /** Loop to include the second order derivatvies */
0114   for (const auto partial1 : m_cfg.parsToUse) {
0115     if (partial1 == FitParIndex::t0) {
0116       continue;
0117     }
0118     for (const auto partial2 : m_cfg.parsToUse) {
0119       if (partial2 == FitParIndex::t0) {
0120         continue;
0121       } else if (partial2 > partial1) {
0122         break;
0123       }
0124       ACTS_VERBOSE("updateStrawResidual() - Calculate Hessian for parameters "
0125                    << parName(partial1) << ", " << parName(partial2) << ".");
0126       // Second derivative w.r.t to the position parameters is zero
0127       if (!(isDirectionParam(partial1) || isDirectionParam(partial2))) {
0128         continue;
0129       }
0130       const auto hIdx1 = static_cast<std::size_t>(partial1);
0131       const auto hIdx2 = static_cast<std::size_t>(partial2);
0132
0133       const std::size_t resIdx = vecIdxFromSymMat<s_nLinePars>(hIdx1, hIdx2);
0134       /// Pure angular derivatives of the residual
0135       if (isDirectionParam(partial1) && isDirectionParam(partial2)) {
0136         // clang-format off
0137         const double partialSqDist = m_hessianProjDir[resIdx].cross(wireDir).dot(hitMinSeg);
0138         // clang-format on
0139         m_hessian[resIdx] = partialSqDist * Vector3::Unit(bending);
0140       } else {
0141         /// Angular & Spatial mixed terms
0142         const auto angParam = isDirectionParam(partial1) ? hIdx1 : hIdx2;
0143         const auto posParam = static_cast<LineIndex>(
0144             isDirectionParam(partial1) ? partial2 : partial1);
0145         // clang-format off
0146         m_hessian[resIdx] = -m_gradProjDir[angParam].cross(wireDir).dot(line.gradient(posParam)) * Vector3::Unit(bending);
0147         // clang-format on
0148       }
0149       ACTS_VERBOSE("updateStrawResidual() - Hessian of the residual is "
0150                    << m_hessian[resIdx][bending]);
0151     }
0152   }
0153   return true;
0154 }
0155 bool StrawLineFitAuxiliaries::updateStrawAuxiliaries(const Line_t& line,
0156                                                      const Vector& wireDir) {
0157   const Vector& lineDir = line.direction();
0158   /// Between two calls the wire projection has not changed
0159   const double wireProject = lineDir.dot(wireDir);
0160   constexpr double s_tolerance = 1.e-12;
0161
0162   if (false && std::abs(wireProject - m_wireProject) < s_tolerance) {
0163     ACTS_VERBOSE("Projection of the line matches the previous one."
0164                  << " Don't update the auxiliaries");
0165     return m_invProjDirLenSq > s_tolerance;
0166   }
0167   m_wireProject = wireProject;
0168   const double projDirLenSq = 1. - square(m_wireProject);
0169   /// The line is parallel to the wire
0170   if (projDirLenSq < s_tolerance) {
0171     ACTS_VERBOSE("Line & wire are parallel: " << toString(wireDir) << " vs. "
0172                                               << toString(lineDir));
0173     m_invProjDirLenSq = 0.;
0174     reset();
0175     return false;
0176   }
0177   m_invProjDirLenSq = 1. / projDirLenSq;
0178   m_invProjDirLen = std::sqrt(m_invProjDirLenSq);
0179   /// Project the segment line onto the wire plane and normalize
0180   m_projDir = (lineDir - m_wireProject * wireDir) * m_invProjDirLen;
0181   /// Loop over all configured parameters and calculate the partials
0182   /// of the wire projection
0183   for (auto partial : m_cfg.parsToUse) {
0184     /// Skip the parameters that are not directional
0185     if (!isDirectionParam(partial)) {
0186       continue;  // skip these parameters
0187     }
0188     const std::size_t pIdx = static_cast<std::size_t>(partial);
0189     const Vector& lGrad{line.gradient(static_cast<LineIndex>(partial))};
0190     m_projDirLenPartial[pIdx] = lGrad.dot(wireDir);
0191     // clang-format off
0192     m_gradProjDir[pIdx] = m_invProjDirLen * (lGrad - m_projDirLenPartial[pIdx] * wireDir) +
0193                            m_projDirLenPartial[pIdx] * m_wireProject * m_projDir * m_invProjDirLenSq;
0194     // clang-format on
0195     if (!m_cfg.useHessian) {
0196       continue;  // skip the Hessian calculation
0197     }
0198     for (auto partial1 : m_cfg.parsToUse) {
0199       /// Skip the parameters that are not directional or avoid double counting
0200       if (!isDirectionParam(partial1)) {
0201         continue;
0202       } else if (partial1 > partial) {
0203         break;
0204       }
0205       const auto param = static_cast<std::size_t>(partial);
0206       const auto param1 = static_cast<std::size_t>(partial1);
0207       const auto resIdx = vecIdxFromSymMat<s_nLinePars>(param, param1);
0208       const Vector& lHessian = line.hessian(static_cast<LineIndex>(partial),
0209                                             static_cast<LineIndex>(partial1));
0210       const double partSqLineProject = lHessian.dot(wireDir);
0211
0212       auto calcMixedTerms = [this](const std::size_t p1, const std::size_t p2) {
0213         return m_projDirLenPartial[p1] * m_wireProject * m_gradProjDir[p2] +
0214                0.5 * m_projDirLenPartial[p1] * m_projDirLenPartial[p2] *
0215                    m_invProjDirLenSq * m_projDir;
0216       };
0217       auto calcMixedHessian = [&]() -> Vector {
0218         if (param1 != param) {
0219           return calcMixedTerms(param1, param) + calcMixedTerms(param, param1);
0220         }
0221         return 2. * calcMixedTerms(param, param1);
0222       };
0223       // clang-format off
0224       m_hessianProjDir[resIdx] = (lHessian - partSqLineProject * wireDir) * m_invProjDirLen +
0225                                m_invProjDirLenSq * (calcMixedHessian() + (partSqLineProject * m_wireProject) * m_projDir);
0226       // clang-format on
0227       ACTS_VERBOSE("updateStrawAuxiliaries() - Hessian w.r.t. "
0228                    << parName(partial) << ", " << parName(partial1) << " is "
0229                    << toString(m_hessianProjDir[resIdx]));
0230     }
0231   }
0232   return true;
0233 }
0234
0235 void StrawLineFitAuxiliaries::updateAlongTheStraw(const Line_t& line,
0236                                                   const Vector& hitMinSeg,
0237                                                   const Vector& wireDir) {
0238   m_residual[nonBending] =
0239       m_invProjDirLenSq * (hitMinSeg.dot(line.direction()) * m_wireProject -
0240                            hitMinSeg.dot(wireDir));
0241   ACTS_VERBOSE("updateAlongTheStraw() - Residual is "
0242                << m_residual[nonBending]);
0243
0244   for (const auto partial : m_cfg.parsToUse) {
0245     if (partial == FitParIndex::t0) {
0246       continue;
0247     }
0248     const auto param = static_cast<std::size_t>(partial);
0249     const Vector& lGradient = line.gradient(static_cast<LineIndex>(partial));
0250     if (isDirectionParam(partial)) {
0251       // clang-format off
0252       m_gradient[param][nonBending] = m_invProjDirLenSq * (
0253                                        hitMinSeg.dot(lGradient) * m_wireProject +
0254                                        hitMinSeg.dot(line.direction()) * m_projDirLenPartial[param] +
0255                                        2. * m_residual[nonBending] * m_wireProject * m_projDirLenPartial[param]);
0256       // clang-format on
0257
0258     } else if (isPositionParam(partial)) {
0259       // clang-format off
0260       m_gradient[param][nonBending] = -m_invProjDirLenSq * (lGradient.dot(line.direction()* m_wireProject - wireDir));
0261       // clang-format on
0262     }
0263     ACTS_VERBOSE("updateAlongTheStraw() - First derivative w.r.t "
0264                  << parName(partial) << " is "
0265                  << m_gradient[param][nonBending]);
0266   }
0267   if (!m_cfg.useHessian) {
0268     return;
0269   }
0270
0271   for (auto partial1 : m_cfg.parsToUse) {
0272     if (partial1 == FitParIndex::t0) {
0273       continue;
0274     }
0275     for (auto partial2 : m_cfg.parsToUse) {
0276       if (partial2 == FitParIndex::t0) {
0277         continue;
0278       } else if (partial2 > partial1) {
0279         break;
0280       }
0281       // second derivative w.r.t intercepts is always 0
0282       if (!(isDirectionParam(partial2) || isDirectionParam(partial1))) {
0283         continue;
0284       }
0285       const auto param = static_cast<std::size_t>(partial1);
0286       const auto param1 = static_cast<std::size_t>(partial2);
0287       const auto hessIdx = vecIdxFromSymMat<s_nPars>(param, param1);
0288       if (isDirectionParam(partial1) && isDirectionParam(partial2)) {
0289         // clang-format off
0290         auto calcMixedHessian = [this, &hitMinSeg, &line](const std::size_t p1,
0291                                                           const std::size_t p2) -> double {
0292           const double term1 = hitMinSeg.dot(line.gradient(static_cast<LineIndex>(p1))) * m_projDirLenPartial[p2];
0293           const double term2 = m_residual[nonBending] * m_projDirLenPartial[p1] * m_projDirLenPartial[p2];
0294           const double term3 = 2. * m_wireProject * m_projDirLenPartial[p1] * m_gradient[p2][nonBending];
0295           return (term1 + term2 + term3) * m_invProjDirLenSq;
0296         };
0297         const Vector& lHessian = line.hessian(static_cast<LineIndex>(partial1), static_cast<LineIndex>(partial2));
0298         const double projHess = lHessian.dot(wireDir);
0299         m_hessian[hessIdx][nonBending] = (param != param1 ? calcMixedHessian(param, param1) + calcMixedHessian(param1, param)
0300                                                           : 2. * calcMixedHessian(param1, param)) +
0301                                         m_invProjDirLenSq * (hitMinSeg.dot(lHessian) * m_wireProject +
0302                                                              hitMinSeg.dot(line.direction()) * projHess +
0303                                                              2. * m_residual[nonBending] * m_wireProject * projHess);
0304         // clang-format on
0305       } else {
0306         /// Mixed positional and angular derivative
0307         const auto angParam = static_cast<LineIndex>(
0308             isDirectionParam(partial1) ? partial1 : partial2);
0309         const auto posParam = static_cast<LineIndex>(
0310             isDirectionParam(partial1) ? partial2 : partial1);
0311         const auto angIdx = static_cast<std::size_t>(angParam);
0312         // clang-format off
0313         m_hessian[hessIdx][nonBending] = m_invProjDirLenSq * (
0314                 -line.gradient(posParam).dot(line.gradient(angParam)) * m_wireProject -
0315                  line.gradient(posParam).dot(line.direction()) * m_projDirLenPartial[angIdx] +
0316                  2. * m_gradient[static_cast<std::size_t>(posParam)][nonBending] * m_wireProject * m_projDirLenPartial[angIdx]);
0317         // clang-format on
0318       }
0319       ACTS_VERBOSE("updateAlongTheStraw() - Second derivative w.r.t. "
0320                    << parName(partial1) << ", " << parName(partial2) << " is. "
0321                    << m_hessian[hessIdx][nonBending]);
0322     }
0323   }
0324 }
0325
0326 void StrawLineFitAuxiliaries::updateStripResidual(
0327     const Line_t& line, const Vector& normal, const Vector& sensorN,
0328     const Vector& sensorD, const Vector& stripPos, const bool isBending,
0329     const bool isNonBending) {
0330   using namespace Acts::UnitLiterals;
0331
0332   const double normDot = normal.dot(line.direction());
0333
0334   constexpr double tolerance = 1.e-12;
0335   if (std::abs(normDot) < tolerance) {
0336     reset();
0337     ACTS_WARNING("Segment line is embedded into the strip plane "
0338                  << toString(line.direction())
0339                  << ", normal: " << toString(normal));
0340     return;
0341   }
0342   const double planeOffSet = normal.dot(stripPos);
0343   ACTS_VERBOSE("Plane normal: "
0344                << toString(normal) << " |" << normal.norm()
0345                << "|, line direction: " << toString(line.direction())
0346                << ", angle: " << angle(normal, line.direction()));
0347   const double travelledDist =
0348       (planeOffSet - line.position().dot(normal)) / normDot;
0349   ACTS_VERBOSE("Need to travel " << travelledDist
0350                                  << " mm to reach the strip plane. ");
0351
0352   const Vector& b1 = isBending ? sensorN : sensorD;
0353   const Vector& b2 = isBending ? sensorD : sensorN;
0354
0355   const double b1DotB2 = b1.dot(b2);
0356   if (Acts::abs(Acts::abs(b1DotB2) - 1.) < tolerance) {
0357     ACTS_WARNING("updateStripResidual() The two vectors "
0358                  << toString(b1) << ", " << toString(b2) << " with angle: "
0359                  << angle(b1, b2) << " don't span the plane.");
0360     reset();
0361     return;
0362   }
0363   /// Linear independent vectors
0364   /// Normal vector is indeed normal onto the plane spaned by these two vectors
0365   assert(Acts::abs(b1.dot(normal)) < tolerance);
0366   assert(Acts::abs(b2.dot(normal)) < tolerance);
0367   assert(isBending || isNonBending);
0368   const bool decompStereo =
0369       (m_cfg.calcAlongStrip || (isBending && isNonBending)) &&
0370       Acts::abs(b1DotB2) > tolerance;
0371   if (decompStereo) {
0372     /// A = lambda B1 + kappa B2
0373     ///                <A, B2> - <A,B1><B1,B2>
0374     ///   --> kappa = --------------------------
0375     ///                    1 - <B1,B2>^{2}
0376     ///                <A, B1> - <A,B2><B1,B2>
0377     ///   --> lambda = --------------------------
0378     ///                    1 - <B1,B2>^{2}
0379     const double invDist = 1. / (1. - square(b1DotB2));
0380     m_stereoTrf(bending, bending) = m_stereoTrf(nonBending, nonBending) =
0381         invDist;
0382     m_stereoTrf(bending, nonBending) = m_stereoTrf(nonBending, bending) =
0383         -b1DotB2 * invDist;
0384   }
0385   ACTS_VERBOSE("Meausres bending "
0386                << (isBending ? "yes" : "no")
0387                << ", measures non-bending:" << (isNonBending ? "yes" : "no"));
0388   ACTS_VERBOSE("strip orientation b1: "
0389                << toString(b1) << " |" << b1.norm() << "|"
0390                << ", b2: " << toString(b2) << " |" << b2.norm() << "|"
0391                << ", stereo angle: " << angle(b1, b2));
0392
0393   auto assignResidual = [&](const Vector& calcDistance, Vector& residual) {
0394     residual[bending] =
0395         isBending || m_cfg.calcAlongStrip ? b1.dot(calcDistance) : 0.;
0396     residual[nonBending] =
0397         isNonBending || m_cfg.calcAlongStrip ? b2.dot(calcDistance) : 0.;
0398     if (decompStereo) {
0399       auto spatial = residual.block<2, 1>(0, 0);
0400       spatial = m_stereoTrf * spatial;
0401     }
0402     residual[time] = 0.;
0403     ACTS_VERBOSE("Distance: " << toString(calcDistance)
0404                               << ", <calc, n> =" << calcDistance.dot(normal)
0405                               << " -> residual: " << toString(residual)
0406                               << " --> closure test:"
0407                               << toString(residual[bending] * b1 +
0408                                           residual[nonBending] * b2));
0409   };
0410   /// Update the residual accordingly
0411   assignResidual(line.position() + travelledDist * line.direction() - stripPos,
0412                  m_residual);
0413   for (const auto partial : m_cfg.parsToUse) {
0414     ACTS_VERBOSE("stripResidual() - Calculate partial derivative w.r.t "
0415                  << parName(partial));
0416     switch (partial) {
0417       case FitParIndex::phi:
0418       case FitParIndex::theta: {
0419         // clang-format off
0420         const Vector& lGrad = line.gradient(static_cast<LineIndex>(partial));
0421         const double partialDist = -travelledDist / normDot * normal.dot(lGrad);
0422         const Vector gradCmp = travelledDist * lGrad +
0423                                partialDist * line.direction();
0424         // clang-format on
0425         assignResidual(gradCmp, m_gradient[static_cast<std::uint8_t>(partial)]);
0426         break;
0427       }
0428       case FitParIndex::y0:
0429       case FitParIndex::x0: {
0430         // clang-format off
0431         const Vector& lGrad = line.gradient(static_cast<LineIndex>(partial));
0432         const Vector gradCmp = lGrad - lGrad.dot(normal) / normDot * line.direction();
0433         assignResidual(gradCmp, m_gradient[static_cast<std::uint8_t>(partial)]);
0434         // clang-format on
0435         break;
0436       }
0437       default:
0438         /// Don't calculate anything for time
0439         break;
0440     }
0441   }
0442
0443   if (!m_cfg.useHessian) {
0444     return;
0445   }
0446   for (const auto partial1 : m_cfg.parsToUse) {
0447     for (const auto partial2 : m_cfg.parsToUse) {
0448       /// Avoid double counting
0449       if (partial2 > partial1) {
0450         break;
0451       }
0452       /// At least one parameter needs to be a directional one
0453       if (!(isDirectionParam(partial1) || isDirectionParam(partial2))) {
0454         continue;
0455       }
0456       ACTS_VERBOSE(
0457           "stripResidual() - Calculate second partial derivative w.r.t "
0458           << parName(partial1) << ", " << parName(partial2));
0459       const auto param = static_cast<std::size_t>(partial1);
0460       const auto param1 = static_cast<std::size_t>(partial2);
0461       const auto resIdx = vecIdxFromSymMat<s_nPars>(param, param1);
0462       if (isDirectionParam(partial1) && isDirectionParam(partial2)) {
0463         // clang-format off
0464         auto calcMixedTerms = [&line, &normal, &normDot, &b1, &b2, this](const std::size_t p1,
0465                                                                          const std::size_t p2) {
0466
0467           return -normal.dot(line.gradient(static_cast<LineIndex>(p1))) / normDot *
0468                           (m_gradient[p2][bending] * b1 + m_gradient[p2][nonBending] * b2);
0469         };
0470         const Vector& lHessian = line.hessian(static_cast<LineIndex>(partial1),
0471                                               static_cast<LineIndex>(partial2));
0472         const Vector hessianCmp = travelledDist * (lHessian - normal.dot(lHessian) / normDot * line.direction()) +
0473                                   calcMixedTerms(param1, param) + calcMixedTerms(param, param1);
0474         assignResidual(hessianCmp, m_hessian[resIdx]);
0475         // clang-format on
0476       } else {
0477         // clang-format off
0478         const auto angParam = static_cast<LineIndex>(isDirectionParam(partial1) ? partial1 : partial2);
0479         const auto posParam = static_cast<LineIndex>(isDirectionParam(partial1) ? partial2 : partial1);
0480         const double lH = (normal.dot(line.gradient(posParam)) / normDot);
0481
0482         const Vector hessianCmp = lH * ((normal.dot(line.gradient(angParam)) / normDot) * line.direction() -
0483                                         line.gradient(angParam));
0484         // clang-format on
0485         assignResidual(hessianCmp, m_hessian[resIdx]);
0486       }
0487     }
0488   }
0489 }
0490
0491 }  // namespace Acts::Experimental::detail