Seeding/detail/FastStrawLineFitter.ipp

0001 // This file is part of the ACTS project.
0002 //
0003 // Copyright (C) 2016 CERN for the benefit of the ACTS project
0004 //
0005 // This Source Code Form is subject to the terms of the Mozilla Public
0006 // License, v. 2.0. If a copy of the MPL was not distributed with this
0007 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
0008
0009 #pragma once
0010
0011 #include "Acts/Seeding/detail/FastStrawLineFitter.hpp"
0012
0013 #include "Acts/Definitions/Units.hpp"
0014 #include "Acts/Utilities/Enumerate.hpp"
0015
0016 #include <format>
0017 namespace Acts::Experimental::detail {
0018
0019 template <CompositeSpacePointContainer StrawCont_t>
0020 std::optional<FastStrawLineFitter::FitResult> FastStrawLineFitter::fit(
0021     const StrawCont_t& measurements, const std::vector<int>& signs) const {
0022   if (measurements.size() != signs.size()) {
0023     ACTS_WARNING(
0024         __func__ << "() - " << __LINE__
0025                  << ": Not all measurements are associated with a drift sign");
0026     return std::nullopt;
0027   }
0028
0029   auto result = fit(fillAuxiliaries(measurements, signs));
0030   if (!result) {
0031     return std::nullopt;
0032   }
0033   /// Calculate the chi2
0034   calcPostFitChi2(measurements, *result);
0035   return result;
0036 }
0037
0038 template <CompositeSpacePointContainer StrawCont_t>
0039 void FastStrawLineFitter::calcPostFitChi2(const StrawCont_t& measurements,
0040                                           FitResult& result) const {
0041   const TrigonomHelper angles{result.theta};
0042   result.chi2 = 0.;
0043   for (const auto& strawMeas : measurements) {
0044     result.chi2 += chi2Term(angles, result.y0, *strawMeas);
0045   }
0046   ACTS_DEBUG(__func__ << "() - " << __LINE__ << ": Overall chi2: "
0047                       << result.chi2 << ", nDoF: " << result.nDoF
0048                       << ", redChi2: " << (result.chi2 / result.nDoF));
0049 }
0050
0051 template <CompositeSpacePoint Point_t>
0052 double FastStrawLineFitter::chi2Term(const TrigonomHelper& angle,
0053                                      const double y0, const Point_t& strawMeas,
0054                                      std::optional<double> r) const {
0055   if (!strawMeas.isStraw()) {
0056     return 0.;
0057   }
0058   const double cov = strawMeas.covariance()[s_covIdx];
0059   if (cov < std::numeric_limits<double>::epsilon()) {
0060     return 0.;
0061   }
0062   const Vector& pos = strawMeas.localPosition();
0063   const double y = pos.dot(strawMeas.toNextSensor());
0064   const double z = pos.dot(strawMeas.planeNormal());
0065   const double dist = Acts::abs((y - y0) * angle.cosTheta - z * angle.sinTheta);
0066   ACTS_VERBOSE(__func__ << "() - " << __LINE__ << ": Distance straw (" << y
0067                         << ", " << z
0068                         << "), r: " << r.value_or(strawMeas.driftRadius())
0069                         << " - track: " << dist);
0070   return Acts::pow(dist - r.value_or(strawMeas.driftRadius()), 2) / cov;
0071 }
0072
0073 template <CompositeSpacePointContainer StrawCont_t>
0074 FastStrawLineFitter::FitAuxiliaries FastStrawLineFitter::fillAuxiliaries(
0075     const StrawCont_t& measurements, const std::vector<int>& signs) const {
0076   FitAuxiliaries auxVars{};
0077   auxVars.invCovs.resize(signs.size(), -1.);
0078   Vector centerOfGravity{Vector::Zero()};
0079
0080   /// Calculate first the center of gravity
0081   for (const auto& [sIdx, strawMeas] : enumerate(measurements)) {
0082     if (!strawMeas->isStraw()) {
0083       ACTS_WARNING(__func__ << "() - " << __LINE__
0084                             << ": The measurement is not a straw");
0085       continue;
0086     }
0087     const double cov = strawMeas->covariance()[s_covIdx];
0088     if (cov < std::numeric_limits<double>::epsilon()) {
0089       ACTS_WARNING(__func__ << "() - " << __LINE__ << ": The covariance ("
0090                             << cov << ") of the measurement is invalid.");
0091       continue;
0092     }
0093     auto& invCov = (auxVars.invCovs[sIdx] = 1. / cov);
0094     auxVars.covNorm += invCov;
0095     centerOfGravity += invCov * strawMeas->localPosition();
0096     ++auxVars.nDoF;
0097   }
0098   if (auxVars.nDoF < 3) {
0099     std::stringstream sstr{};
0100     for (const auto& [sIdx, strawMeas] : enumerate(measurements)) {
0101       sstr << " --- " << (sIdx + 1) << ") "
0102            << toString(strawMeas->localPosition())
0103            << ", r: " << strawMeas->driftRadius()
0104            << ", weight: " << auxVars.invCovs[sIdx] << std::endl;
0105     }
0106     ACTS_WARNING(__func__ << "() - " << __LINE__
0107                           << ": At least 3 measurements are required to "
0108                              "perform the straw line fit\n"
0109                           << sstr.str());
0110     return auxVars;
0111   }
0112   /// Reduce the number of degrees of freedom by 2 to account for the two free
0113   /// parameters
0114   auxVars.nDoF -= 2u;
0115   auxVars.covNorm = 1. / auxVars.covNorm;
0116   centerOfGravity *= auxVars.covNorm;
0117
0118   // Now calculate the fit constants
0119   bool centerSet{false};
0120   for (const auto& [sIdx, strawMeas] : enumerate(measurements)) {
0121     const auto& invCov = auxVars.invCovs[sIdx];
0122     /// The invalid measurements were marked
0123     if (invCov < 0.) {
0124       continue;
0125     }
0126     if (!centerSet) {
0127       auxVars.centerY = centerOfGravity.dot(strawMeas->toNextSensor());
0128       auxVars.centerZ = centerOfGravity.dot(strawMeas->planeNormal());
0129       centerSet = true;
0130     }
0131     const Vector pos = strawMeas->localPosition() - centerOfGravity;
0132     const double y = pos.dot(strawMeas->toNextSensor());
0133     const double z = pos.dot(strawMeas->planeNormal());
0134     const double r = strawMeas->driftRadius();
0135
0136     auxVars.T_zzyy += invCov * (Acts::square(z) - Acts::square(y));
0137     auxVars.T_yz += invCov * z * y;
0138     const double sInvCov = -invCov * signs[sIdx];
0139     auxVars.T_rz += sInvCov * z * r;
0140     auxVars.T_ry += sInvCov * y * r;
0141     auxVars.fitY0 += sInvCov * r;
0142   }
0143   auxVars.fitY0 *= auxVars.covNorm;
0144
0145   return auxVars;
0146 }
0147 }  // namespace Acts::Experimental::detail