refactor!: Deduplicate estimateTrackParamsFromSeed code

Core/include/Acts/Seeding/EstimateTrackParamsFromSeed.hpp

@@ -8,30 +8,31 @@
 
 #pragma once
 
+#include "Acts/Definitions/Algebra.hpp"
 #include "Acts/Definitions/TrackParametrization.hpp"
 #include "Acts/Definitions/Units.hpp"
 #include "Acts/Geometry/GeometryContext.hpp"
 #include "Acts/Surfaces/Surface.hpp"
 #include "Acts/Utilities/Logger.hpp"
 #include "Acts/Utilities/MathHelpers.hpp"
+#include "Acts/Utilities/Zip.hpp"
 
 #include <array>
-#include <cmath>
-#include <iostream>
-#include <iterator>
 #include <optional>
+#include <stdexcept>
 
 namespace Acts {
 
 /// Estimate the full track parameters from three space points
 ///
 /// This method is based on the conformal map transformation. It estimates the
-/// full bound track parameters, i.e. (loc0, loc1, phi, theta, q/p, t) at the
+/// full free track parameters, i.e. (x, y, z, t, dx, dy, dz, q/p) at the
 /// bottom space point. The bottom space is assumed to be the first element
-/// in the range defined by the iterators. It must lie on the surface
-/// provided for the representation of the bound track parameters. The magnetic
-/// field (which might be along any direction) is also necessary for the
-/// momentum estimation.
+/// in the range defined by the iterators. The magnetic field (which might be
+/// along any direction) is also necessary for the momentum estimation.
+///
+/// This is a purely spatial estimation, i.e. the time parameter will be set to
+/// 0.
 ///
 /// It resembles the method used in ATLAS for the track parameters
 /// estimated from seed, i.e. the function InDet::SiTrackMaker_xk::getAtaPlane
@@ -40,151 +41,120 @@ namespace Acts {
 ///
 /// @tparam spacepoint_iterator_t  The type of space point iterator
 ///
-/// @param gctx is the geometry context
-/// @param spBegin is the begin iterator for the space points
-/// @param spEnd is the end iterator for the space points
-/// @param surface is the surface of the bottom space point. The estimated bound
-/// track parameters will be represented also at this surface
+/// @param sp0 is the bottom space point
+/// @param sp1 is the middle space point
+/// @param sp2 is the top space point
 /// @param bField is the magnetic field vector
-/// @param bFieldMin is the minimum magnetic field required to trigger the
-/// estimation of q/pt
-/// @param logger A logger instance
-///
-/// @return optional bound parameters
-template <typename spacepoint_iterator_t>
-std::optional<BoundVector> estimateTrackParamsFromSeed(
-    const GeometryContext& gctx, spacepoint_iterator_t spBegin,
-    spacepoint_iterator_t spEnd, const Surface& surface, const Vector3& bField,
-    ActsScalar bFieldMin, const Acts::Logger& logger = getDummyLogger()) {
-  // Check the number of provided space points
-  std::size_t numSP = std::distance(spBegin, spEnd);
-  if (numSP != 3) {
-    ACTS_ERROR("There should be exactly three space points provided.");
-    return std::nullopt;
-  }
+///
+/// @return the free parameters
+FreeVector estimateTrackParamsFromSeed(const Vector3& sp0, const Vector3& sp1,
+                                       const Vector3& sp2,
+                                       const Vector3& bField);
 
-  // Convert bField to Tesla
-  ActsScalar bFieldInTesla = bField.norm() / UnitConstants::T;
-  ActsScalar bFieldMinInTesla = bFieldMin / UnitConstants::T;
-  // Check if magnetic field is too small
-  if (bFieldInTesla < bFieldMinInTesla) {
-    // @todo shall we use straight-line estimation and use default q/pt in such
-    // case?
-    ACTS_WARNING("The magnetic field at the bottom space point: B = "
-                 << bFieldInTesla << " T is smaller than |B|_min = "
-                 << bFieldMinInTesla << " T. Estimation is not performed.");
-    return std::nullopt;
+/// Estimate the full track parameters from three space points
+///
+/// This method is based on the conformal map transformation. It estimates the
+/// full free track parameters, i.e. (x, y, z, t, dx, dy, dz, q/p) at the
+/// bottom space point. The bottom space is assumed to be the first element
+/// in the range defined by the iterators. The magnetic field (which might be
+/// along any direction) is also necessary for the momentum estimation.
+///
+/// It resembles the method used in ATLAS for the track parameters
+/// estimated from seed, i.e. the function InDet::SiTrackMaker_xk::getAtaPlane
+/// here:
+/// https://acode-browser.usatlas.bnl.gov/lxr/source/athena/InnerDetector/InDetRecTools/SiTrackMakerTool_xk/src/SiTrackMaker_xk.cxx
+///
+/// @tparam spacepoint_iterator_t  The type of space point iterator
+///
+/// @param spRange is the range of space points
+/// @param bField is the magnetic field vector
+///
+/// @return the free parameters
+template <std::ranges::range spacepoint_range_t>
+FreeVector estimateTrackParamsFromSeed(spacepoint_range_t spRange,
+                                       const Vector3& bField) {
+  // Check the number of provided space points
+  if (spRange.size() != 3) {
+    throw std::invalid_argument(
+        "There should be exactly three space points provided.");
   }
 
   // The global positions of the bottom, middle and space points
-  std::array<Vector3, 3> spGlobalPositions = {Vector3::Zero(), Vector3::Zero(),
-                                              Vector3::Zero()};
-  std::array<std::optional<float>, 3> spGlobalTimes = {
-      std::nullopt, std::nullopt, std::nullopt};
+  std::array<Vector3, 3> spPositions = {Vector3::Zero(), Vector3::Zero(),
+                                        Vector3::Zero()};
+  std::array<std::optional<double>, 3> spTimes = {std::nullopt, std::nullopt,
+                                                  std::nullopt};
   // The first, second and third space point are assumed to be bottom, middle
   // and top space point, respectively
-  for (std::size_t isp = 0; isp < 3; ++isp) {
-    spacepoint_iterator_t it = std::next(spBegin, isp);
-    if (*it == nullptr) {
-      ACTS_ERROR("Empty space point found. This should not happen.");
-      return std::nullopt;
+  for (auto [sp, spPosition, spTime] :
+       Acts::zip(spRange, spPositions, spTimes)) {
+    if (sp == nullptr) {
+      throw std::invalid_argument("Empty space point found.");
     }
-    const auto& sp = *it;
-    spGlobalPositions[isp] = Vector3(sp->x(), sp->y(), sp->z());
-    spGlobalTimes[isp] = sp->t();
+    spPosition = Vector3(sp->x(), sp->y(), sp->z());
+    spTime = sp->t();
   }
 
-  // Define a new coordinate frame with its origin at the bottom space point, z
-  // axis long the magnetic field direction and y axis perpendicular to vector
-  // from the bottom to middle space point. Hence, the projection of the middle
-  // space point on the transverse plane will be located at the x axis of the
-  // new frame.
-  Vector3 relVec = spGlobalPositions[1] - spGlobalPositions[0];
-  Vector3 newZAxis = bField.normalized();
-  Vector3 newYAxis = newZAxis.cross(relVec).normalized();
-  Vector3 newXAxis = newYAxis.cross(newZAxis);
-  RotationMatrix3 rotation;
-  rotation.col(0) = newXAxis;
-  rotation.col(1) = newYAxis;
-  rotation.col(2) = newZAxis;
-  // The center of the new frame is at the bottom space point
-  Translation3 trans(spGlobalPositions[0]);
-  // The transform which constructs the new frame
-  Transform3 transform(trans * rotation);
-
-  // The coordinate of the middle and top space point in the new frame
-  Vector3 local1 = transform.inverse() * spGlobalPositions[1];
-  Vector3 local2 = transform.inverse() * spGlobalPositions[2];
-
-  // In the new frame the bottom sp is at the origin, while the middle
-  // sp in along the x axis. As such, the x-coordinate of the circle is
-  // at: x-middle / 2.
-  // The y coordinate can be found by using the straight line passing
-  // between the mid point between the middle and top sp and perpendicular to
-  // the line connecting them
-  Vector2 circleCenter;
-  circleCenter(0) = 0.5 * local1(0);
-
-  ActsScalar deltaX21 = local2(0) - local1(0);
-  ActsScalar sumX21 = local2(0) + local1(0);
-  // straight line connecting the two points
-  // y = a * x + c (we don't care about c right now)
-  // we simply need the slope
-  // we compute 1./a since this is what we need for the following computation
-  ActsScalar ia = deltaX21 / local2(1);
-  // Perpendicular line is then y = -1/a *x + b
-  // we can evaluate b given we know a already by imposing
-  // the line passes through P = (0.5 * (x2 + x1), 0.5 * y2)
-  ActsScalar b = 0.5 * (local2(1) + ia * sumX21);
-  circleCenter(1) = -ia * circleCenter(0) + b;
-  // Radius is a signed distance between circleCenter and first sp, which is at
-  // (0, 0) in the new frame. Sign depends on the slope a (positive vs negative)
-  int sign = ia > 0 ? -1 : 1;
-  const ActsScalar R = circleCenter.norm();
-  ActsScalar invTanTheta =
-      local2.z() / (2.f * R * std::asin(local2.head<2>().norm() / (2.f * R)));
-  // The momentum direction in the new frame (the center of the circle has the
-  // coordinate (-1.*A/(2*B), 1./(2*B)))
-  ActsScalar A = -circleCenter(0) / circleCenter(1);
-  Vector3 transDirection(1., A, fastHypot(1, A) * invTanTheta);
-  // Transform it back to the original frame
-  Vector3 direction = rotation * transDirection.normalized();
-
-  // Initialize the bound parameters vector
-  BoundVector params = BoundVector::Zero();
+  FreeVector params = estimateTrackParamsFromSeed(
+      spPositions[0], spPositions[1], spPositions[2], bField);
+  params[eFreeTime] = spTimes[0].value_or(0);
+  return params;
+}
+
+/// Estimate the full track parameters from three space points
+///
+/// This method is based on the conformal map transformation. It estimates the
+/// full bound track parameters, i.e. (loc0, loc1, phi, theta, q/p, t) at the
+/// bottom space point. The bottom space is assumed to be the first element
+/// in the range defined by the iterators. It must lie on the surface
+/// provided for the representation of the bound track parameters. The magnetic
+/// field (which might be along any direction) is also necessary for the
+/// momentum estimation.
+///
+/// It resembles the method used in ATLAS for the track parameters
+/// estimated from seed, i.e. the function InDet::SiTrackMaker_xk::getAtaPlane
+/// here:
+/// https://acode-browser.usatlas.bnl.gov/lxr/source/athena/InnerDetector/InDetRecTools/SiTrackMakerTool_xk/src/SiTrackMaker_xk.cxx
+///
+/// @tparam spacepoint_iterator_t  The type of space point iterator
+///
+/// @param gctx is the geometry context
+/// @param spRange is the range of space points
+/// @param surface is the surface of the bottom space point. The estimated bound
+/// track parameters will be represented also at this surface
+/// @param bField is the magnetic field vector
+///
+/// @return bound parameters
+template <std::ranges::range spacepoint_range_t>
+BoundVector estimateTrackParamsFromSeed(const GeometryContext& gctx,
+                                        spacepoint_range_t spRange,
+                                        const Surface& surface,
+                                        const Vector3& bField) {
+  FreeVector freeParams = estimateTrackParamsFromSeed(spRange, bField);
+
+  const auto* sp0 = *spRange.begin();
+  Vector3 origin = Vector3(sp0->x(), sp0->y(), sp0->z());
+  Vector3 direction = freeParams.segment<3>(eFreeDir0);
 
-  // The estimated phi and theta
+  BoundVector params = BoundVector::Zero();
   params[eBoundPhi] = VectorHelpers::phi(direction);
   params[eBoundTheta] = VectorHelpers::theta(direction);
+  params[eBoundQOverP] = freeParams[eFreeQOverP];
 
   // Transform the bottom space point to local coordinates of the provided
   // surface
-  auto lpResult = surface.globalToLocal(gctx, spGlobalPositions[0], direction);
+  auto lpResult = surface.globalToLocal(gctx, origin, direction);
   if (!lpResult.ok()) {
-    ACTS_ERROR(
-        "Global to local transformation did not succeed. Please make sure the "
-        "bottom space point lies on the provided surface.");
-    return std::nullopt;
+    throw std::runtime_error(
+        "Failed to transform the space point to local coordinates of the "
+        "surface.");
   }
   Vector2 bottomLocalPos = lpResult.value();
   // The estimated loc0 and loc1
   params[eBoundLoc0] = bottomLocalPos.x();
   params[eBoundLoc1] = bottomLocalPos.y();
-  params[eBoundTime] = spGlobalTimes[0].value_or(0.);
-
-  // The estimated q/pt in [GeV/c]^-1 (note that the pt is the projection of
-  // momentum on the transverse plane of the new frame)
-  ActsScalar qOverPt = sign * (UnitConstants::m) / (0.3 * bFieldInTesla * R);
-  // The estimated q/p in [GeV/c]^-1
-  params[eBoundQOverP] = qOverPt / fastHypot(1., invTanTheta);
-
-  if (params.hasNaN()) {
-    ACTS_ERROR(
-        "The NaN value exists at the estimated track parameters from seed with"
-        << "\nbottom sp: " << spGlobalPositions[0] << "\nmiddle sp: "
-        << spGlobalPositions[1] << "\ntop sp: " << spGlobalPositions[2]);
-    return std::nullopt;
-  }
+  params[eBoundTime] = sp0->t().value_or(0);
+
   return params;
 }
 

Core/src/Seeding/EstimateTrackParamsFromSeed.cpp

@@ -12,6 +12,84 @@
 
 #include <numbers>
 
+Acts::FreeVector Acts::estimateTrackParamsFromSeed(const Vector3& sp0,
+                                                   const Vector3& sp1,
+                                                   const Vector3& sp2,
+                                                   const Vector3& bField) {
+  // Define a new coordinate frame with its origin at the bottom space point, z
+  // axis long the magnetic field direction and y axis perpendicular to vector
+  // from the bottom to middle space point. Hence, the projection of the middle
+  // space point on the transverse plane will be located at the x axis of the
+  // new frame.
+  Vector3 relVec = sp1 - sp0;
+  Vector3 newZAxis = bField.normalized();
+  Vector3 newYAxis = newZAxis.cross(relVec).normalized();
+  Vector3 newXAxis = newYAxis.cross(newZAxis);
+  RotationMatrix3 rotation;
+  rotation.col(0) = newXAxis;
+  rotation.col(1) = newYAxis;
+  rotation.col(2) = newZAxis;
+  // The center of the new frame is at the bottom space point
+  Translation3 trans(sp0);
+  // The transform which constructs the new frame
+  Transform3 transform(trans * rotation);
+
+  // The coordinate of the middle and top space point in the new frame
+  Vector3 local1 = transform.inverse() * sp1;
+  Vector3 local2 = transform.inverse() * sp2;
+
+  // In the new frame the bottom sp is at the origin, while the middle
+  // sp in along the x axis. As such, the x-coordinate of the circle is
+  // at: x-middle / 2.
+  // The y coordinate can be found by using the straight line passing
+  // between the mid point between the middle and top sp and perpendicular to
+  // the line connecting them
+  Vector2 circleCenter;
+  circleCenter(0) = 0.5 * local1(0);
+
+  ActsScalar deltaX21 = local2(0) - local1(0);
+  ActsScalar sumX21 = local2(0) + local1(0);
+  // straight line connecting the two points
+  // y = a * x + c (we don't care about c right now)
+  // we simply need the slope
+  // we compute 1./a since this is what we need for the following computation
+  ActsScalar ia = deltaX21 / local2(1);
+  // Perpendicular line is then y = -1/a *x + b
+  // we can evaluate b given we know a already by imposing
+  // the line passes through P = (0.5 * (x2 + x1), 0.5 * y2)
+  ActsScalar b = 0.5 * (local2(1) + ia * sumX21);
+  circleCenter(1) = -ia * circleCenter(0) + b;
+  // Radius is a signed distance between circleCenter and first sp, which is at
+  // (0, 0) in the new frame. Sign depends on the slope a (positive vs negative)
+  int sign = ia > 0 ? -1 : 1;
+  const ActsScalar R = circleCenter.norm();
+  ActsScalar invTanTheta =
+      local2.z() / (2 * R * std::asin(local2.head<2>().norm() / (2 * R)));
+  // The momentum direction in the new frame (the center of the circle has the
+  // coordinate (-1.*A/(2*B), 1./(2*B)))
+  ActsScalar A = -circleCenter(0) / circleCenter(1);
+  Vector3 transDirection(1., A, fastHypot(1, A) * invTanTheta);
+  // Transform it back to the original frame
+  Vector3 direction = rotation * transDirection.normalized();
+
+  // Initialize the free parameters vector
+  FreeVector params = FreeVector::Zero();
+
+  // The bottom space point position
+  params.segment<3>(eFreePos0) = sp0;
+
+  // The estimated direction
+  params.segment<3>(eFreeDir0) = direction;
+
+  // The estimated q/pt in [GeV/c]^-1 (note that the pt is the projection of
+  // momentum on the transverse plane of the new frame)
+  ActsScalar qOverPt = sign / (bField.norm() * R);
+  // The estimated q/p in [GeV/c]^-1
+  params[eFreeQOverP] = qOverPt / fastHypot(1., invTanTheta);
+
+  return params;
+}
+
 Acts::BoundMatrix Acts::estimateTrackParamCovariance(
     const EstimateTrackParamCovarianceConfig& config, const BoundVector& params,
     bool hasTime) {

Examples/Algorithms/TrackFinding/src/TrackParamsEstimationAlgorithm.cpp

@@ -112,17 +112,15 @@ ProcessCode TrackParamsEstimationAlgorithm::execute(
     }
     Acts::Vector3 field = *fieldRes;
 
-    // Estimate the track parameters from seed
-    auto optParams = Acts::estimateTrackParamsFromSeed(
-        ctx.geoContext, seed.sp().begin(), seed.sp().end(), *surface, field,
-        m_cfg.bFieldMin, logger());
-    if (!optParams.has_value()) {
-      ACTS_WARNING("Estimation of track parameters for seed " << iseed
-                                                              << " failed.");
+    if (field.norm() < m_cfg.bFieldMin) {
+      ACTS_WARNING("Magnetic field at seed " << iseed << " is too small "
+                                             << field.norm());
       continue;
     }
 
-    const auto& params = optParams.value();
+    // Estimate the track parameters from seed
+    const auto params = Acts::estimateTrackParamsFromSeed(
+        ctx.geoContext, seed.sp(), *surface, field);
 
     Acts::EstimateTrackParamCovarianceConfig config{
         .initialSigmas =