src/Vertexing/GaussianGridTrackDensity.cpp

0001 // This file is part of the ACTS project.
0002 //
0003 // Copyright (C) 2016 CERN for the benefit of the ACTS project
0004 //
0005 // This Source Code Form is subject to the terms of the Mozilla Public
0006 // License, v. 2.0. If a copy of the MPL was not distributed with this
0007 // file, You can obtain one at https://mozilla.org/MPL/2.0/.
0008
0009 #include "Acts/Vertexing/GaussianGridTrackDensity.hpp"
0010
0011 #include "Acts/Utilities/AlgebraHelpers.hpp"
0012 #include "Acts/Vertexing/VertexingError.hpp"
0013
0014 #include <algorithm>
0015
0016 namespace Acts {
0017
0018 Result<float> GaussianGridTrackDensity::getMaxZPosition(
0019     MainGridVector& mainGrid) const {
0020   if (mainGrid.isZero()) {
0021     return VertexingError::EmptyInput;
0022   }
0023
0024   int zbin = -1;
0025   if (!m_cfg.useHighestSumZPosition) {
0026     // Get bin with maximum content
0027     mainGrid.maxCoeff(&zbin);
0028   } else {
0029     // Get z position with highest density sum
0030     // of surrounding bins
0031     zbin = getHighestSumZPosition(mainGrid);
0032   }
0033
0034   // Derive corresponding z value
0035   return (zbin - m_cfg.mainGridSize / 2.0f + 0.5f) * m_cfg.binSize;
0036 }
0037
0038 Result<std::pair<float, float>>
0039 GaussianGridTrackDensity::getMaxZPositionAndWidth(
0040     MainGridVector& mainGrid) const {
0041   // Get z maximum value
0042   auto maxZRes = getMaxZPosition(mainGrid);
0043   if (!maxZRes.ok()) {
0044     return maxZRes.error();
0045   }
0046   float maxZ = *maxZRes;
0047
0048   // Get seed width estimate
0049   auto widthRes = estimateSeedWidth(mainGrid, maxZ);
0050   if (!widthRes.ok()) {
0051     return widthRes.error();
0052   }
0053   float width = *widthRes;
0054   std::pair<float, float> returnPair{maxZ, width};
0055   return returnPair;
0056 }
0057
0058 std::pair<int, GaussianGridTrackDensity::TrackGridVector>
0059 GaussianGridTrackDensity::addTrack(const BoundTrackParameters& trk,
0060                                    MainGridVector& mainGrid) const {
0061   SquareMatrix2 cov = trk.spatialImpactParameterCovariance().value();
0062   float d0 = trk.parameters()[0];
0063   float z0 = trk.parameters()[1];
0064
0065   // Calculate offset in d direction to central bin at z-axis
0066   int dOffset = static_cast<int>(std::floor(d0 / m_cfg.binSize - 0.5) + 1);
0067   // Check if current track does affect grid density
0068   // in central bins at z-axis
0069   if (std::abs(dOffset) > (m_cfg.trkGridSize - 1) / 2.) {
0070     // Current track is too far away to contribute
0071     // to track density at z-axis bins
0072     return {-1, TrackGridVector::Zero(m_cfg.trkGridSize)};
0073   }
0074
0075   // Calculate bin in z
0076   int zBin = static_cast<int>(z0 / m_cfg.binSize + m_cfg.mainGridSize / 2.);
0077
0078   if (zBin < 0 || zBin >= m_cfg.mainGridSize) {
0079     return {-1, TrackGridVector::Zero(m_cfg.trkGridSize)};
0080   }
0081   // Calculate the positions of the bin centers
0082   float binCtrZ = (zBin + 0.5f) * m_cfg.binSize - m_cfg.zMinMax;
0083
0084   // Calculate the distance between IP values and their
0085   // corresponding bin centers
0086   float distCtrZ = z0 - binCtrZ;
0087
0088   // Create the track grid
0089   TrackGridVector trackGrid = createTrackGrid(d0, distCtrZ, cov);
0090   // Add the track grid to the main grid
0091   addTrackGridToMainGrid(zBin, trackGrid, mainGrid);
0092
0093   return {zBin, trackGrid};
0094 }
0095
0096 void GaussianGridTrackDensity::addTrackGridToMainGrid(
0097     int zBin, const TrackGridVector& trkGrid, MainGridVector& mainGrid) const {
0098   modifyMainGridWithTrackGrid(zBin, trkGrid, mainGrid, +1);
0099 }
0100
0101 void GaussianGridTrackDensity::removeTrackGridFromMainGrid(
0102     int zBin, const TrackGridVector& trkGrid, MainGridVector& mainGrid) const {
0103   modifyMainGridWithTrackGrid(zBin, trkGrid, mainGrid, -1);
0104 }
0105
0106 void GaussianGridTrackDensity::modifyMainGridWithTrackGrid(
0107     int zBin, const TrackGridVector& trkGrid, MainGridVector& mainGrid,
0108     int modifyModeSign) const {
0109   int width = (m_cfg.trkGridSize - 1) / 2;
0110   // Overlap left
0111   int leftOL = zBin - width;
0112   // Overlap right
0113   int rightOL = zBin + width - m_cfg.mainGridSize + 1;
0114   if (leftOL < 0) {
0115     int totalTrkSize = m_cfg.trkGridSize + leftOL;
0116     mainGrid.segment(0, totalTrkSize) +=
0117         modifyModeSign * trkGrid.segment(-leftOL, totalTrkSize);
0118     return;
0119   }
0120   if (rightOL > 0) {
0121     int totalTrkSize = m_cfg.trkGridSize - rightOL;
0122     mainGrid.segment(m_cfg.mainGridSize - totalTrkSize, totalTrkSize) +=
0123         modifyModeSign * trkGrid.segment(0, totalTrkSize);
0124     return;
0125   }
0126
0127   mainGrid.segment(zBin - width, m_cfg.trkGridSize) += modifyModeSign * trkGrid;
0128 }
0129
0130 GaussianGridTrackDensity::TrackGridVector
0131 GaussianGridTrackDensity::createTrackGrid(float d0, float distCtrZ,
0132                                           const SquareMatrix2& cov) const {
0133   TrackGridVector trackGrid(TrackGridVector::Zero(m_cfg.trkGridSize));
0134   float floorHalfTrkGridSize = static_cast<float>(m_cfg.trkGridSize) / 2 - 0.5f;
0135
0136   // Loop over columns
0137   for (int j = 0; j < m_cfg.trkGridSize; j++) {
0138     float z = (j - floorHalfTrkGridSize) * m_cfg.binSize;
0139     trackGrid(j) = normal2D(-d0, z - distCtrZ, cov);
0140   }
0141   return trackGrid;
0142 }
0143
0144 Result<float> GaussianGridTrackDensity::estimateSeedWidth(
0145     MainGridVector& mainGrid, float maxZ) const {
0146   if (mainGrid.isZero()) {
0147     return VertexingError::EmptyInput;
0148   }
0149   // Get z bin of max density z value
0150   int zBin = static_cast<int>(maxZ / m_cfg.binSize + m_cfg.mainGridSize / 2.);
0151
0152   const float maxValue = mainGrid(zBin);
0153   float gridValue = mainGrid(zBin);
0154
0155   // Find right half-maximum bin
0156   int rhmBin = zBin;
0157   while (gridValue > maxValue / 2) {
0158     rhmBin += 1;
0159     gridValue = mainGrid(rhmBin);
0160   }
0161
0162   // Use linear approximation to find better z value for FWHM between bins
0163   float deltaZ1 = m_cfg.binSize * (maxValue / 2 - mainGrid(rhmBin - 1)) /
0164                   (mainGrid(rhmBin) - mainGrid(rhmBin - 1));
0165
0166   // Find left half-maximum bin
0167   int lhmBin = zBin;
0168   gridValue = mainGrid(zBin);
0169   while (gridValue > maxValue / 2) {
0170     lhmBin -= 1;
0171     gridValue = mainGrid(lhmBin);
0172   }
0173
0174   // Use linear approximation to find better z value for FWHM between bins
0175   float deltaZ2 = m_cfg.binSize * (mainGrid(lhmBin + 1) - maxValue / 2) /
0176                   (mainGrid(lhmBin + 1) - mainGrid(lhmBin));
0177
0178   // Approximate FWHM
0179   float fwhm =
0180       rhmBin * m_cfg.binSize - deltaZ1 - lhmBin * m_cfg.binSize - deltaZ2;
0181
0182   // FWHM = 2.355 * sigma
0183   float width = fwhm / 2.355f;
0184
0185   return std::isnormal(width) ? width : 0.0f;
0186 }
0187
0188 float GaussianGridTrackDensity::normal2D(float d, float z,
0189                                          const SquareMatrix2& cov) const {
0190   float det = cov.determinant();
0191   float coef = 1 / std::sqrt(det);
0192   float expo =
0193       -1 / (2 * det) *
0194       (cov(1, 1) * d * d - (cov(0, 1) + cov(1, 0)) * d * z + cov(0, 0) * z * z);
0195   return coef * safeExp(expo);
0196 }
0197
0198 int GaussianGridTrackDensity::getHighestSumZPosition(
0199     MainGridVector& mainGrid) const {
0200   // Checks the first (up to) 3 density maxima, if they are close, checks which
0201   // one has the highest surrounding density sum (the two neighboring bins)
0202
0203   // The global maximum
0204   int zbin = -1;
0205   mainGrid.maxCoeff(&zbin);
0206   int zFirstMax = zbin;
0207   double firstDensity = mainGrid(zFirstMax);
0208   double firstSum = getDensitySum(mainGrid, zFirstMax);
0209
0210   // Get the second highest maximum
0211   mainGrid[zFirstMax] = 0;
0212   mainGrid.maxCoeff(&zbin);
0213   int zSecondMax = zbin;
0214   double secondDensity = mainGrid(zSecondMax);
0215   double secondSum = 0;
0216   if (firstDensity - secondDensity <
0217       firstDensity * m_cfg.maxRelativeDensityDev) {
0218     secondSum = getDensitySum(mainGrid, zSecondMax);
0219   }
0220
0221   // Get the third highest maximum
0222   mainGrid[zSecondMax] = 0;
0223   mainGrid.maxCoeff(&zbin);
0224   int zThirdMax = zbin;
0225   double thirdDensity = mainGrid(zThirdMax);
0226   double thirdSum = 0;
0227   if (firstDensity - thirdDensity <
0228       firstDensity * m_cfg.maxRelativeDensityDev) {
0229     thirdSum = getDensitySum(mainGrid, zThirdMax);
0230   }
0231
0232   // Revert back to original values
0233   mainGrid[zFirstMax] = firstDensity;
0234   mainGrid[zSecondMax] = secondDensity;
0235
0236   // Return the z-bin position of the highest density sum
0237   if (secondSum > firstSum && secondSum > thirdSum) {
0238     return zSecondMax;
0239   }
0240   if (thirdSum > secondSum && thirdSum > firstSum) {
0241     return zThirdMax;
0242   }
0243   return zFirstMax;
0244 }
0245
0246 double GaussianGridTrackDensity::getDensitySum(const MainGridVector& mainGrid,
0247                                                int pos) const {
0248   double sum = mainGrid(pos);
0249   // Sum up only the density contributions from the
0250   // neighboring bins if they are still within bounds
0251   if (pos - 1 >= 0) {
0252     sum += mainGrid(pos - 1);
0253   }
0254   if (pos + 1 <= mainGrid.size() - 1) {
0255     sum += mainGrid(pos + 1);
0256   }
0257   return sum;
0258 }
0259
0260 }  // namespace Acts