GaussianTrackDensity.cxx

Go to the documentation of this file.

/*
  Copyright (C) 2002-2023 CERN for the benefit of the ATLAS collaboration
*/
 
#include "TrkVertexSeedFinderUtils/GaussianTrackDensity.h"
 
#include "GaudiKernel/PhysicalConstants.h"
#include "TrkEventPrimitives/ParamDefs.h"
#include "TrkTrack/Track.h"
#include <algorithm>
#include <cmath>
#include <limits>
 
namespace Trk
{
  double GaussianTrackDensity::globalMaximum (const std::vector<const Track*>& vectorTrk) const
  {
    std::vector<const TrackParameters*> perigeeList;
    perigeeList.reserve(vectorTrk.size());
 
    for (const Track* itrk : vectorTrk)
    {
      perigeeList.push_back(itrk->perigeeParameters());
    }
 
    TrackDensity d (m_gaussStep);
    return globalMaximumImpl (perigeeList, d);
  }
 
 
  double GaussianTrackDensity::globalMaximum (const std::vector<const Track*>& vectorTrk,
                                              std::unique_ptr<ITrackDensity>& density) const
  {
    std::vector<const TrackParameters*> perigeeList;
    perigeeList.reserve(vectorTrk.size());
 
    for (const Track* itrk : vectorTrk)
    {
      perigeeList.push_back(itrk->perigeeParameters());
    }
    return globalMaximum (perigeeList, density);
  }
 
  double
  GaussianTrackDensity::globalMaximum (const std::vector<const TrackParameters*>& perigeeList) const
  {
    TrackDensity d (m_gaussStep);
    return globalMaximumImpl (perigeeList, d);
  }
 
  double
  GaussianTrackDensity::globalMaximum (const std::vector<const TrackParameters*>& perigeeList,
                                       std::unique_ptr<ITrackDensity>& density) const
  {
    auto d = std::make_unique<TrackDensity> (m_gaussStep);
    TrackDensity* dp = d.get();
    density = std::move(d);
    return globalMaximumImpl (perigeeList, *dp);
  }
 
  std::pair<double, double> GaussianTrackDensity::globalMaximumWithWidth(
      const std::vector<const TrackParameters*>& perigeeList) const {
    TrackDensity d (m_gaussStep);
    return globalMaximumWithWidthImpl (perigeeList, d);
  }
 
  double GaussianTrackDensity::globalMaximumImpl (const std::vector<const TrackParameters*>& perigeeList,
                                                  TrackDensity& density) const
  {
    addTracks (perigeeList, density);
    return density.globalMaximum ();
  }
 
  std::pair<double,double> GaussianTrackDensity::globalMaximumWithWidthImpl (const std::vector<const TrackParameters*>& perigeeList,
                                                  TrackDensity& density) const
  {
    addTracks (perigeeList, density);
    return density.globalMaximumWithWidth ();
  }
 
  void GaussianTrackDensity::addTracks(const std::vector<const TrackParameters*>& perigeeList,
                                       TrackDensity& density) const
  {
    const double d0SignificanceCut = m_d0MaxSignificance * m_d0MaxSignificance;
    const double z0SignificanceCut = m_z0MaxSignificance * m_z0MaxSignificance;
 
    for (const TrackParameters* iparam : perigeeList) {
      if (iparam && iparam->surfaceType() == Trk::SurfaceType::Perigee) {
        density.addTrack(*(static_cast<const Perigee*>(iparam)),
                         d0SignificanceCut,
                         z0SignificanceCut);
      }
    }
  }
 
 
  //***********************************************************************
  double GaussianTrackDensity::TrackDensity::trackDensity (double z) const
  {
    double firstDeriv, secondDeriv = 0;  // unused in this case
    double density = 0;
    // use the existing trackDensity method to avoid duplication of logic
    trackDensity(z,density,firstDeriv,secondDeriv);
    return density;
  }
 
 
  void
  GaussianTrackDensity::TrackDensity::trackDensity (double z,
                                                    double& density,
                                                    double& firstDerivative,
                                                    double& secondDerivative) const
  {
    TrackDensityEval densityResult(z);
    for (const auto & trackAndPerigeePair : m_lowerMap){
      densityResult.addTrack(trackAndPerigeePair.first);
    }
    density = densityResult.density();
    firstDerivative = densityResult.firstDerivative();
    secondDerivative = densityResult.secondDerivative();
  }
 
  std::pair<double,double>
  GaussianTrackDensity::TrackDensity::globalMaximumWithWidth () const
  {
    // strategy:
    // the global maximum must be somewhere near a track...
    // since we can calculate the first and second derivatives, at each point we can determine
    // a) whether the function is curved up (minimum) or down (maximum)
    // b) the distance to nearest maximum, assuming either Newton (parabolic) or Gaussian local behavior
    //
    // For each track where the second derivative is negative, find step to nearest maximum
    // Take that step, and then do one final refinement
    // The largest density encountered in this procedure (after checking all tracks) is considered the maximum
    //
    double maximumPosition = 0.0;
    double maximumDensity = 0.0;
    double maxCurvature = 0. ;
    const std::pair<double,double> invalidResult(0.,std::numeric_limits<double>::quiet_NaN());
    if (m_trackMap.empty()) return invalidResult;
    for (const auto& entry : m_trackMap)
    {
      double trialZ = entry.first.parameters()[Trk::z0];
      double density   = 0.0;
      double slope     = 0.0;
      double curvature = 0.0;
      trackDensity( trialZ, density, slope, curvature );
      if ( curvature >= 0.0 || density <= 0.0 ) {
        continue;
      }
      updateMaximum( trialZ, density, curvature, maximumPosition, maximumDensity, maxCurvature);
      trialZ += stepSize( density, slope, curvature );
      trackDensity( trialZ, density, slope, curvature );
      if ( curvature >= 0.0 || density <= 0.0 ) {
        continue;
      }
      updateMaximum( trialZ, density, curvature, maximumPosition, maximumDensity, maxCurvature);
      trialZ += stepSize( density, slope, curvature );
      trackDensity( trialZ, density, slope, curvature );
      if ( curvature >= 0.0 || density <= 0.0) {
        continue;
      }
      updateMaximum( trialZ, density, curvature, maximumPosition, maximumDensity, maxCurvature);
    }
    if (maxCurvature == 0.) return invalidResult;
    return {maximumPosition,std::sqrt(-maximumDensity/maxCurvature)};
  }
 
  double
  GaussianTrackDensity::TrackDensity::globalMaximum () const
  {
    return GaussianTrackDensity::TrackDensity::globalMaximumWithWidth().first;
  }
 
 
  void GaussianTrackDensity::TrackDensity::addTrack (const Perigee& itrk,
                                                     const double d0SignificanceCut,
                                                     const double z0SignificanceCut)
  {
    if (m_trackMap.count(itrk) != 0) return;
    const double d0 = itrk.parameters()[Trk::d0];
    const double z0 = itrk.parameters()[Trk::z0];
    const AmgSymMatrix(5) & perigeeCov = *(itrk.covariance());
    const double cov_dd = perigeeCov(Trk::d0, Trk::d0);
    if ( cov_dd <= 0 ) return;
    if ( d0*d0/cov_dd > d0SignificanceCut ) return;
    const double cov_zz = perigeeCov(Trk::z0, Trk::z0);
    if (cov_zz <= 0 ) return;
    const double cov_dz = perigeeCov(Trk::d0, Trk::z0);
    const double covDeterminant = cov_dd*cov_zz - cov_dz*cov_dz;
    if ( covDeterminant <= 0 ) return;
    double constantTerm = -(d0*d0*cov_zz + z0*z0*cov_dd + 2*d0*z0*cov_dz) / (2*covDeterminant);
    const double linearTerm = (d0*cov_dz + z0*cov_dd) / covDeterminant ; // minus signs and factors of 2 cancel...
    const double quadraticTerm = -cov_dd / (2*covDeterminant);
    double discriminant = linearTerm*linearTerm - 4*quadraticTerm*(constantTerm + 2*z0SignificanceCut);
    if (discriminant < 0) return;
    discriminant = std::sqrt(discriminant);
    const double zMax = (-linearTerm - discriminant)/(2*quadraticTerm);
    const double zMin = (-linearTerm + discriminant)/(2*quadraticTerm);
    m_maxRange = std::max(m_maxRange, std::max(zMax-z0, z0-zMin));
    constantTerm -= std::log(2*M_PI*std::sqrt(covDeterminant));
    m_trackMap.emplace(std::piecewise_construct,
                       std::forward_as_tuple(itrk),
                       std::forward_as_tuple(constantTerm, linearTerm, quadraticTerm, zMin, zMax));
    m_lowerMap.emplace(std::piecewise_construct,
                       std::forward_as_tuple(constantTerm, linearTerm, quadraticTerm, zMin, zMax),
                       std::forward_as_tuple(itrk));
  }
 
 
  void GaussianTrackDensity::TrackDensityEval::addTrack (const TrackEntry & entry){
    if (entry.lowerBound < m_z && entry.upperBound > m_z) {
      double delta = std::exp(entry.c_0+m_z*(entry.c_1 + m_z*entry.c_2));
      double qPrime = entry.c_1 + 2*m_z*entry.c_2;
      double deltaPrime = delta * qPrime;
      m_density += delta;
      m_firstDerivative += deltaPrime;
      m_secondDerivative += 2*entry.c_2*delta + qPrime*deltaPrime;
    }
  }
 
}