From 045212cc35bd7fe202398dc04a6ae3d6248909d3 Mon Sep 17 00:00:00 2001
From: Dave Casper <dcasper@uci.edu>
Date: Thu, 8 Mar 2018 21:23:16 -0800
Subject: [PATCH] Fix important logic error

Former-commit-id: 2b87fd021bf532058354038cb16e44e529d7d582
---
 .../GaussianTrackDensity.h                    |  2 +
 .../src/GaussianDensityTestAlg.cxx            | 40 ++++++++-----
 .../src/GaussianDensityTestAlg.h              |  1 +
 .../src/GaussianTrackDensity.cxx              | 58 +++++++++++++------
 4 files changed, 70 insertions(+), 31 deletions(-)

diff --git a/Tracking/TrkVertexFitter/TrkVertexSeedFinderUtils/TrkVertexSeedFinderUtils/GaussianTrackDensity.h b/Tracking/TrkVertexFitter/TrkVertexSeedFinderUtils/TrkVertexSeedFinderUtils/GaussianTrackDensity.h
index 9d64a2d92bfe..d61ef0f7570c 100644
--- a/Tracking/TrkVertexFitter/TrkVertexSeedFinderUtils/TrkVertexSeedFinderUtils/GaussianTrackDensity.h
+++ b/Tracking/TrkVertexFitter/TrkVertexSeedFinderUtils/TrkVertexSeedFinderUtils/GaussianTrackDensity.h
@@ -157,6 +157,8 @@ namespace Trk
     lowerMap m_lowerMap;
     upperMap m_upperMap;
 
+    double m_maxRange;
+
     //  Cuts set by configurable properties
     
     //  Maximum allowed d0 significance to use (in sigma)
diff --git a/Tracking/TrkVertexFitter/TrkVertexSeedFinderUtils/src/GaussianDensityTestAlg.cxx b/Tracking/TrkVertexFitter/TrkVertexSeedFinderUtils/src/GaussianDensityTestAlg.cxx
index f7940784e21b..33b47a9f06df 100644
--- a/Tracking/TrkVertexFitter/TrkVertexSeedFinderUtils/src/GaussianDensityTestAlg.cxx
+++ b/Tracking/TrkVertexFitter/TrkVertexSeedFinderUtils/src/GaussianDensityTestAlg.cxx
@@ -34,6 +34,7 @@ GaussianDensityTestAlg::GaussianDensityTestAlg( const std::string& name,
 			  ISvcLocator* pSvcLocator ) : 
   ::AthAlgorithm( name, pSvcLocator ),
   m_useBeamConstraint(true),
+  m_firstEvent(true),
   m_iBeamCondSvc("BeamCondSvc", name),
   m_iTHistSvc("THistSvc", name)
 {
@@ -98,19 +99,24 @@ StatusCode GaussianDensityTestAlg::execute()
   m_estimator->reset();
   m_estimator->addTracks(perigeeList);
 
-  //  for (int i = 0; i < 800; i++)
-  //  {
-  //    double z = -200.0 + 0.25 + i*0.5;
-  //    double density = m_estimator->trackDensity(z);
-  //    m_h_density->Fill((float) z, (float) density);
-  //  }
+  if (m_firstEvent)
+  {
+    for (int i = 0; i < 800; i++)
+    {
+      double z = -200.0 + 0.25 + i*0.5;
+      double density = m_estimator->trackDensity(z);
+      m_h_density->Fill((float) z, (float) density);
+    }
+  }
   ATH_MSG_VERBOSE("Analyzing MC truth");
   std::vector<Amg::Vector3D> truth;
   ATH_CHECK( findTruth(trackVector, truth) );
-  ATH_MSG_VERBOSE("Filling truth vertex histogram");
-  //for (auto& v : truth) m_h_truthVertices->Fill( v[2] );
-
-
+  if (m_firstEvent)
+  {
+    ATH_MSG_VERBOSE("Filling truth vertex histogram");
+    for (auto& v : truth) m_h_truthVertices->Fill( v[2] );
+  }
+  m_firstEvent = false;
   return StatusCode::SUCCESS;
 }
 
@@ -206,8 +212,11 @@ void GaussianDensityTestAlg::selectTracks(const xAOD::TrackParticleContainer* tr
 			      nGoodTracks++;
 			      const Trk::Perigee* perigee = dynamic_cast<const Trk::Perigee*>(trk->parameters());
 			      if (perigee == nullptr) ATH_MSG_ERROR("Invalid Perigee");
-			      //m_h_truthDensity->Fill(vLink->z());
-			      ATH_MSG_VERBOSE("Filled truth density histogram");
+			      if (m_firstEvent) 
+			      {
+				m_h_truthDensity->Fill(vLink->z());
+				ATH_MSG_VERBOSE("Filled truth density histogram");
+			      }
 			    }
 			    break;
 			}
@@ -265,8 +274,11 @@ void GaussianDensityTestAlg::selectTracks(const xAOD::TrackParticleContainer* tr
 			      nGoodTracks++;
 			      const Trk::Perigee* perigee = dynamic_cast<const Trk::Perigee*>(trk->parameters());
 			      if (perigee == nullptr) ATH_MSG_ERROR("Invalid Perigee");
-			      m_h_truthDensity->Fill(vLink->z());
-			      ATH_MSG_VERBOSE("Filled truth density histogram");
+			      if (m_firstEvent)
+			      {
+				m_h_truthDensity->Fill(vLink->z());
+				ATH_MSG_VERBOSE("Filled truth density histogram");
+			      }
 			    }
 			    break;
 			}
diff --git a/Tracking/TrkVertexFitter/TrkVertexSeedFinderUtils/src/GaussianDensityTestAlg.h b/Tracking/TrkVertexFitter/TrkVertexSeedFinderUtils/src/GaussianDensityTestAlg.h
index 2ef16e3ff4cf..24097ae1e3a1 100644
--- a/Tracking/TrkVertexFitter/TrkVertexSeedFinderUtils/src/GaussianDensityTestAlg.h
+++ b/Tracking/TrkVertexFitter/TrkVertexSeedFinderUtils/src/GaussianDensityTestAlg.h
@@ -101,6 +101,7 @@ class GaussianDensityTestAlg
                                              "Minimum associated reconstructed tracks for vertex to be considered visible" };
 
   bool m_useBeamConstraint;
+  bool m_firstEvent;
 
   // Tools
   ToolHandle< InDet::IInDetTrackSelectionTool > m_trackFilter { this, "TrackSelector", 
diff --git a/Tracking/TrkVertexFitter/TrkVertexSeedFinderUtils/src/GaussianTrackDensity.cxx b/Tracking/TrkVertexFitter/TrkVertexSeedFinderUtils/src/GaussianTrackDensity.cxx
index 0d7f3e3bfdb6..e1bd78c51dec 100644
--- a/Tracking/TrkVertexFitter/TrkVertexSeedFinderUtils/src/GaussianTrackDensity.cxx
+++ b/Tracking/TrkVertexFitter/TrkVertexSeedFinderUtils/src/GaussianTrackDensity.cxx
@@ -13,7 +13,7 @@
 namespace Trk
 {
   GaussianTrackDensity::GaussianTrackDensity(const std::string& t, const std::string& n, const IInterface* p):
-    AthAlgTool(t, n, p)
+    AthAlgTool(t, n, p), m_maxRange(0.0)
   {
     declareInterface<IVertexTrackDensityEstimator>(this);
   }
@@ -34,34 +34,56 @@ namespace Trk
     firstDerivative = 0.0;
     secondDerivative = 0.0;
     TrackEntry target(z);
-    lowerMapIterator first = m_lowerMap.lower_bound(target);  // first track whose UPPER bound is not less than z
-    upperMapIterator final = m_upperMap.upper_bound(target);  // first track whose LOWER bound is greater than z
-    trackMapIterator firstLoop = m_trackMap.find(first->second);  // this will be the first track we include
-    trackMapIterator finalLoop = m_trackMap.find(final->second);  // this will be the track after the last we include
-    if (firstLoop->second.lowerBound > z || finalLoop->second.upperBound < z) return;
-    for (auto itrk = firstLoop; itrk != finalLoop && itrk != m_trackMap.end(); itrk++)
+    trackMap overlaps;
+    lowerMapIterator left = m_lowerMap.lower_bound(target);  // first track whose UPPER bound is not less than z
+    if (left == m_lowerMap.end()) return;                    // z is to the right of every track's range
+    upperMapIterator right = m_upperMap.upper_bound(target); // first track whose LOWER bound is greater than z
+    if (right == m_upperMap.begin()) return;                 // z is to the left of every track's range
+    for (auto itrk = left; itrk != m_lowerMap.end(); itrk++)
     {
-      double delta = exp(itrk->second.c_0+z*(itrk->second.c_1 + z*itrk->second.c_2));
+      if ( itrk->first.upperBound > z + m_maxRange ) break;
+      if ( z >= itrk->first.lowerBound && z <= itrk->first.upperBound ) overlaps[itrk->second] = itrk->first;
+    }
+    for (auto itrk = right; itrk-- != m_upperMap.begin(); )
+    {
+      if ( itrk->first.lowerBound < z - m_maxRange ) break;
+      if (z >= itrk->first.lowerBound && z <= itrk->first.upperBound ) overlaps[itrk->second] = itrk->first;
+    }
+    for (const auto& entry : overlaps)
+    {
+      if (entry.second.lowerBound > z || entry.second.upperBound < z) continue;
+      double delta = exp(entry.second.c_0+z*(entry.second.c_1 + z*entry.second.c_2));
       density += delta;
-      double qPrime = itrk->second.c_1 + 2*z*itrk->second.c_2;
+      double qPrime = entry.second.c_1 + 2*z*entry.second.c_2;
       double deltaPrime = delta * qPrime;
       firstDerivative += deltaPrime;
-      secondDerivative += 2*itrk->second.c_2*delta + qPrime*deltaPrime;
+      secondDerivative += 2*entry.second.c_2*delta + qPrime*deltaPrime;
     }
   }
 
   double GaussianTrackDensity::trackDensity(double z) const
   {
+    ATH_MSG_VERBOSE("Inside trackDensity function; z=" << z);
     double sum = 0.0;
     TrackEntry target(z);
-    lowerMapIterator first = m_lowerMap.lower_bound(target);
-    upperMapIterator final = m_upperMap.upper_bound(target);
-    trackMapIterator firstLoop = m_trackMap.find(first->second);
-    trackMapIterator finalLoop = m_trackMap.find(final->second);
-    if (firstLoop->second.lowerBound > z || finalLoop->second.upperBound < z) return sum;
-    for (auto itrk = firstLoop; itrk != finalLoop && itrk != m_trackMap.end(); itrk++)
+    trackMap overlaps;
+    lowerMapIterator left = m_lowerMap.lower_bound(target);  // first track whose UPPER bound is not less than z
+    if (left == m_lowerMap.end()) return sum;                // z is to the right of every track's range
+    upperMapIterator right = m_upperMap.upper_bound(target); // first track whose LOWER bound is greater than z
+    if (right == m_upperMap.begin()) return sum;             // z is to the left of every track's range
+    for (auto itrk = left; itrk != m_lowerMap.end(); itrk++)
+    {
+      if ( itrk->first.upperBound > z + m_maxRange ) break;
+      if ( z >= itrk->first.lowerBound && z <= itrk->first.upperBound ) overlaps[itrk->second] = itrk->first;
+    }
+    for (auto itrk = right; itrk-- != m_upperMap.begin(); )
+    {
+      if ( itrk->first.lowerBound < z - m_maxRange ) break;
+      if (z >= itrk->first.lowerBound && z <= itrk->first.upperBound ) overlaps[itrk->second] = itrk->first;
+    }
+    for (const auto& entry : overlaps)
     {
-      sum += exp(itrk->second.c_0+z*(itrk->second.c_1 + z*itrk->second.c_2));
+      sum += exp(entry.second.c_0+z*(entry.second.c_1 + z*entry.second.c_2));
     }
     return sum;
   }
@@ -155,6 +177,7 @@ namespace Trk
 	discriminant = sqrt(discriminant);
 	double zMax = (-linearTerm - discriminant)/(2*quadraticTerm);
 	double zMin = (-linearTerm + discriminant)/(2*quadraticTerm);
+	m_maxRange = std::max(m_maxRange, std::max(zMax-z0, z0-zMin));
 	constantTerm -= log(2*Gaudi::Units::pi*sqrt(covDeterminant));
 	m_trackMap.emplace(std::piecewise_construct,
                            std::forward_as_tuple(*itrk),
@@ -190,6 +213,7 @@ namespace Trk
     m_trackMap.clear();
     m_lowerMap.clear();
     m_upperMap.clear();
+    m_maxRange = 0.0;
   }
 
   GaussianTrackDensity::TrackEntry::TrackEntry(double c0, double c1, double c2, double zMin, double zMax) 
-- 
GitLab