feat: Free parameter estimation form seed

Core/include/Acts/Seeding/EstimateTrackParamsFromSeed.hpp

@@ -20,9 +20,134 @@
 #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 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 spBegin is the begin iterator for the space points
+/// @param spEnd is the end iterator for the space points
+/// @param bField is the magnetic field vector
+///
+/// @return optional bound parameters
+template <typename spacepoint_iterator_t>
+FreeVector estimateTrackParamsFromSeed(spacepoint_iterator_t spBegin,
+                                       spacepoint_iterator_t spEnd,
+                                       const Vector3& bField) {
+  // Check the number of provided space points
+  std::size_t numSP = std::distance(spBegin, spEnd);
+  if (numSP != 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};
+  // 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) {
+      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();
+  }
+
+  // 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 * 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) = spGlobalPositions[0];
+  params[eFreeTime] = spGlobalTimes[0].value_or(0.);
+
+  // 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;
+}
+
 /// Estimate the full track parameters from three space points
 ///
 /// This method is based on the conformal map transformation. It estimates the
@@ -142,7 +267,7 @@ std::optional<BoundVector> estimateTrackParamsFromSeed(
   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)));
+      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);