fix problem with truth seeded track finder

Tracking/Acts/FaserActsKalmanFilter/src/TruthSeededTrackFinderTool.cxx

@@ -67,7 +67,9 @@ StatusCode TruthSeededTrackFinderTool::run() {
     // const Acts::Surface *surface = m_trackingGeometryTool->trackingGeometry()->findSurface(geoId);
 
     auto par = spacePoint->localParameters();
+    ATH_MSG_DEBUG("par " << par);
     auto cov = spacePoint->localCovariance();
+    ATH_MSG_DEBUG("cov " << cov);
     ATH_MSG_DEBUG(cov(0, 0) << ", " << cov(0, 1) << ", " << cov(1, 0) << ", " << cov(1, 1));
     Acts::ActsSymMatrix<2> myCov = Acts::ActsSymMatrix<2>::Zero();
     myCov(0, 0) = m_covMeas00;
@@ -99,6 +101,12 @@ StatusCode TruthSeededTrackFinderTool::run() {
     }
   }
 
+  // check if there is atleast one space point in each station
+
+  if (map2vector(spacePoints, 1).empty() || map2vector(spacePoints, 2).empty() || map2vector(spacePoints, 0).empty()) {
+    return StatusCode::RECOVERABLE;
+  }
+
   Acts::Vector4 smearedPosition4;
   Acts::Vector3 smearedDirection;
   double smearedAbsoluteMomentum;
@@ -131,15 +139,10 @@ StatusCode TruthSeededTrackFinderTool::run() {
         smearedAbsoluteMomentum = abs_momentum * (1 + m_sigmaP * norm(rng));
 
         smearedPosition4 = Acts::Vector4(vertex.x() + m_sigmaLoc0 * norm(rng), vertex.y() + m_sigmaLoc1 * norm(rng), vertex.z(), 0);
-        ATH_MSG_DEBUG("?? position: x=" << vertex.x() << ", y=" << vertex.y() << ", z=" << vertex.z());
-        ATH_MSG_DEBUG("?? smeared position: x=" << smearedDirection.x() << ", y=" << smearedDirection.y() << ", z=" << smearedDirection.z());
-        ATH_MSG_DEBUG("?? direction: x=" << direction.x() << ", y=" << direction.y() << ", z=" << direction.z());
-        ATH_MSG_DEBUG("?? smeared direction: x=" << smearedDirection.x() << ", y=" << smearedDirection.y() << ", z=" << smearedDirection.z());
       }
     }
   } else {
     // get initial parameters from track seed
-    // TODO convert momentum to GeV?
     auto [p, q] = momentum2(spacePoints);
     double abs_momentum = p;
     charge = q;
@@ -150,8 +153,12 @@ StatusCode TruthSeededTrackFinderTool::run() {
       ATH_MSG_ERROR("Could not find space point on first layer");
     }
     smearedPosition4 = Acts::Vector4(initPos.x(), initPos.y(), initPos.z(), 0);
-    auto [pos, dir] = linear_fit(map2vector(spacePoints, 1));
-    smearedDirection = dir;
+
+    auto [pos1, dir1] = linear_fit(map2vector(spacePoints, 1));
+    auto [pos2, dir2] = linear_fit(map2vector(spacePoints, 2));
+    smearedDirection = pos2 - pos1;
+//    smearedDirection = dir;
+    smearedAbsoluteMomentum = p;
   }
 
   Acts::BoundSymMatrix cov = Acts::BoundSymMatrix::Zero();
@@ -193,7 +200,7 @@ Acts::Vector3 TruthSeededTrackFinderTool::average(const std::vector<Acts::Vector
     for (const Acts::Vector3& spacePoint : spacePoints) {
       ret += spacePoint;
     }
-    ret / spacePoints.size();
+    ret /= spacePoints.size();
   }
   return ret;
 }
@@ -227,19 +234,19 @@ TruthSeededTrackFinderTool::map2vector(const std::map<int, Acts::Vector3>& map,
 
 
 std::pair<double, double> TruthSeededTrackFinderTool::momentum2(const std::map<int, Acts::Vector3>& hits, double B) {
-  Acts::Vector3 pos1 = average(map2vector(hits, 1));
-  Acts::Vector3 pos2 = average(map2vector(hits, 2));
-  Acts::Vector3 pos3 = average(map2vector(hits, 3));
+  Acts::Vector3 pos1 = average(map2vector(hits, 0));
+  Acts::Vector3 pos2 = average(map2vector(hits, 1));
+  Acts::Vector3 pos3 = average(map2vector(hits, 2));
 
   Acts::Vector3 vec_l = pos3 - pos1;
   double abs_l = std::sqrt(vec_l.y() * vec_l.y() + vec_l.z() * vec_l.z());
-  double t = double (pos2.z() - pos1.z()) / (pos3.z() - pos1.z());
-  Amg::Vector3D vec_m = pos1 + t * vec_l;
-  Amg::Vector3D vec_s = pos2 - vec_m;
-
+  double t = (pos2.z() - pos1.z()) / (pos3.z() - pos1.z());
+  Acts::Vector3 vec_m = pos1 + t * vec_l;
+  Acts::Vector3 vec_s = pos2 - vec_m;
   double abs_s = std::sqrt(vec_s.y() * vec_s.y() + vec_s.z() * vec_s.z());
-  double p_yz = 0.3 * abs_l * abs_l * B / (8 * abs_s);
-  double charge = vec_s.y() < 0 ? 1 : -1;
+  double p_yz = 0.3 * abs_l * abs_l * B / (8 * abs_s * 1000); // in GeV
+  // double charge = vec_s.y() < 0 ? 1 : -1;
+  double charge = 1;
 
   return std::make_pair(p_yz, charge);
 }