src/Vertexing/AdaptiveGridTrackDensity.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/AdaptiveGridTrackDensity.hpp"
0010
0011 #include "Acts/Utilities/AlgebraHelpers.hpp"
0012 #include "Acts/Vertexing/VertexingError.hpp"
0013
0014 #include <algorithm>
0015
0016 namespace Acts {
0017
0018 namespace {
0019
0020 /// @brief Helper to retrieve values of an nDim-dimensional normal
0021 /// distribution
0022 /// @note The constant prefactor (2 * pi)^(- nDim / 2) is discarded
0023 ///
0024 /// @param args Coordinates where the Gaussian should be evaluated
0025 /// @note args must be in a coordinate system with origin at the mean
0026 /// values of the Gaussian
0027 /// @param cov Covariance matrix
0028 ///
0029 /// @return Multivariate Gaussian evaluated at args
0030 template <unsigned int nDim>
0031 double multivariateGaussian(const ActsVector<nDim>& args,
0032                             const ActsSquareMatrix<nDim>& cov) {
0033   double exponent = -0.5 * args.transpose().dot(cov.inverse() * args);
0034   double gaussianDensity = safeExp(exponent) / std::sqrt(cov.determinant());
0035   return gaussianDensity;
0036 }
0037
0038 }  // namespace
0039
0040 double AdaptiveGridTrackDensity::getBinCenter(std::int32_t bin,
0041                                               double binExtent) {
0042   return bin * binExtent;
0043 }
0044
0045 std::int32_t AdaptiveGridTrackDensity::getBin(double value, double binExtent) {
0046   return static_cast<std::int32_t>(std::floor(value / binExtent - 0.5) + 1);
0047 }
0048
0049 std::uint32_t AdaptiveGridTrackDensity::getTrkGridSize(
0050     double sigma, double nTrkSigmas, double binExtent,
0051     const GridSizeRange& trkGridSizeRange) {
0052   std::uint32_t size =
0053       static_cast<std::uint32_t>(std::ceil(2 * nTrkSigmas * sigma / binExtent));
0054   // Make sure the grid size is odd
0055   if (size % 2 == 0) {
0056     ++size;
0057   }
0058   // Make sure the grid size is within the allowed range
0059   if (trkGridSizeRange.first && size < trkGridSizeRange.first.value()) {
0060     size = trkGridSizeRange.first.value();
0061   }
0062   if (trkGridSizeRange.second && size > trkGridSizeRange.second.value()) {
0063     size = trkGridSizeRange.second.value();
0064   }
0065   return size;
0066 }
0067
0068 std::int32_t AdaptiveGridTrackDensity::getSpatialBin(double value) const {
0069   return getBin(value, m_cfg.spatialBinExtent);
0070 }
0071
0072 std::int32_t AdaptiveGridTrackDensity::getTemporalBin(double value) const {
0073   if (!m_cfg.useTime) {
0074     return 0;
0075   }
0076   return getBin(value, m_cfg.temporalBinExtent);
0077 }
0078
0079 double AdaptiveGridTrackDensity::getSpatialBinCenter(std::int32_t bin) const {
0080   return getBinCenter(bin, m_cfg.spatialBinExtent);
0081 }
0082
0083 double AdaptiveGridTrackDensity::getTemporalBinCenter(std::int32_t bin) const {
0084   if (!m_cfg.useTime) {
0085     return 0.;
0086   }
0087   return getBinCenter(bin, m_cfg.temporalBinExtent);
0088 }
0089
0090 std::uint32_t AdaptiveGridTrackDensity::getSpatialTrkGridSize(
0091     double sigma) const {
0092   return getTrkGridSize(sigma, m_cfg.nSpatialTrkSigmas, m_cfg.spatialBinExtent,
0093                         m_cfg.spatialTrkGridSizeRange);
0094 }
0095
0096 std::uint32_t AdaptiveGridTrackDensity::getTemporalTrkGridSize(
0097     double sigma) const {
0098   if (!m_cfg.useTime) {
0099     return 1;
0100   }
0101   return getTrkGridSize(sigma, m_cfg.nTemporalTrkSigmas,
0102                         m_cfg.temporalBinExtent,
0103                         m_cfg.temporalTrkGridSizeRange);
0104 }
0105
0106 AdaptiveGridTrackDensity::AdaptiveGridTrackDensity(const Config& cfg)
0107     : m_cfg(cfg) {
0108   if (m_cfg.spatialTrkGridSizeRange.first &&
0109       m_cfg.spatialTrkGridSizeRange.first.value() % 2 == 0) {
0110     throw std::invalid_argument(
0111         "AdaptiveGridTrackDensity: spatialTrkGridSizeRange.first must be odd");
0112   }
0113   if (m_cfg.spatialTrkGridSizeRange.second &&
0114       m_cfg.spatialTrkGridSizeRange.second.value() % 2 == 0) {
0115     throw std::invalid_argument(
0116         "AdaptiveGridTrackDensity: spatialTrkGridSizeRange.second must be odd");
0117   }
0118   if (m_cfg.temporalTrkGridSizeRange.first &&
0119       m_cfg.temporalTrkGridSizeRange.first.value() % 2 == 0) {
0120     throw std::invalid_argument(
0121         "AdaptiveGridTrackDensity: temporalTrkGridSizeRange.first must be odd");
0122   }
0123   if (m_cfg.temporalTrkGridSizeRange.second &&
0124       m_cfg.temporalTrkGridSizeRange.second.value() % 2 == 0) {
0125     throw std::invalid_argument(
0126         "AdaptiveGridTrackDensity: temporalTrkGridSizeRange.second must be "
0127         "odd");
0128   }
0129 }
0130
0131 AdaptiveGridTrackDensity::DensityMap::const_iterator
0132 AdaptiveGridTrackDensity::highestDensityEntry(
0133     const DensityMap& densityMap) const {
0134   auto maxEntry = std::max_element(
0135       std::begin(densityMap), std::end(densityMap),
0136       [](const auto& a, const auto& b) { return a.second < b.second; });
0137   return maxEntry;
0138 }
0139
0140 Result<AdaptiveGridTrackDensity::ZTPosition>
0141 AdaptiveGridTrackDensity::getMaxZTPosition(DensityMap& densityMap) const {
0142   if (densityMap.empty()) {
0143     return VertexingError::EmptyInput;
0144   }
0145
0146   Bin bin;
0147   if (!m_cfg.useHighestSumZPosition) {
0148     bin = highestDensityEntry(densityMap)->first;
0149   } else {
0150     // Get z position with highest density sum
0151     // of surrounding bins
0152     bin = highestDensitySumBin(densityMap);
0153   }
0154
0155   // Derive corresponding z and t value
0156   double maxZ = getSpatialBinCenter(bin.first);
0157   double maxT = getTemporalBinCenter(bin.second);
0158
0159   return std::pair(maxZ, maxT);
0160 }
0161
0162 Result<AdaptiveGridTrackDensity::ZTPositionAndWidth>
0163 AdaptiveGridTrackDensity::getMaxZTPositionAndWidth(
0164     DensityMap& densityMap) const {
0165   // Get z value where the density is the highest
0166   auto maxZTRes = getMaxZTPosition(densityMap);
0167   if (!maxZTRes.ok()) {
0168     return maxZTRes.error();
0169   }
0170   ZTPosition maxZT = *maxZTRes;
0171
0172   // Get seed width estimate
0173   auto widthRes = estimateSeedWidth(densityMap, maxZT);
0174   if (!widthRes.ok()) {
0175     return widthRes.error();
0176   }
0177   double width = *widthRes;
0178   ZTPositionAndWidth maxZTAndWidth{maxZT, width};
0179   return maxZTAndWidth;
0180 }
0181
0182 AdaptiveGridTrackDensity::DensityMap AdaptiveGridTrackDensity::addTrack(
0183     const BoundTrackParameters& trk, DensityMap& mainDensityMap) const {
0184   Vector3 impactParams = trk.impactParameters();
0185   ActsSquareMatrix<3> cov = trk.impactParameterCovariance().value();
0186
0187   std::uint32_t spatialTrkGridSize =
0188       getSpatialTrkGridSize(std::sqrt(cov(1, 1)));
0189   std::uint32_t temporalTrkGridSize =
0190       getTemporalTrkGridSize(std::sqrt(cov(2, 2)));
0191
0192   // Calculate bin in d direction
0193   std::int32_t centralDBin = getBin(impactParams(0), m_cfg.spatialBinExtent);
0194   // Check if current track affects grid density
0195   if (std::abs(centralDBin) > (spatialTrkGridSize - 1) / 2.) {
0196     // Return empty map
0197     return {};
0198   }
0199
0200   // Calculate bin in z and t direction
0201   std::int32_t centralZBin = getSpatialBin(impactParams(1));
0202   std::int32_t centralTBin = getTemporalBin(impactParams(2));
0203
0204   Bin centralBin = {centralZBin, centralTBin};
0205
0206   DensityMap trackDensityMap = createTrackGrid(
0207       impactParams, centralBin, cov, spatialTrkGridSize, temporalTrkGridSize);
0208
0209   for (const auto& [bin, density] : trackDensityMap) {
0210     mainDensityMap[bin] += density;
0211   }
0212
0213   return trackDensityMap;
0214 }
0215
0216 void AdaptiveGridTrackDensity::subtractTrack(const DensityMap& trackDensityMap,
0217                                              DensityMap& mainDensityMap) const {
0218   for (const auto& [bin, density] : trackDensityMap) {
0219     mainDensityMap[bin] -= density;
0220   }
0221 }
0222
0223 AdaptiveGridTrackDensity::DensityMap AdaptiveGridTrackDensity::createTrackGrid(
0224     const Vector3& impactParams, const Bin& centralBin,
0225     const SquareMatrix3& cov, std::uint32_t spatialTrkGridSize,
0226     std::uint32_t temporalTrkGridSize) const {
0227   DensityMap trackDensityMap;
0228
0229   std::uint32_t halfSpatialTrkGridSize = (spatialTrkGridSize - 1) / 2;
0230   std::int32_t firstZBin = centralBin.first - halfSpatialTrkGridSize;
0231
0232   // If we don't do time vertex seeding, firstTBin will be 0.
0233   std::uint32_t halfTemporalTrkGridSize = (temporalTrkGridSize - 1) / 2;
0234   std::int32_t firstTBin = centralBin.second - halfTemporalTrkGridSize;
0235
0236   // Loop over bins
0237   for (std::uint32_t i = 0; i < temporalTrkGridSize; i++) {
0238     std::int32_t tBin = firstTBin + i;
0239     double t = getTemporalBinCenter(tBin);
0240     if (t < m_cfg.temporalWindow.first || t > m_cfg.temporalWindow.second) {
0241       continue;
0242     }
0243     for (std::uint32_t j = 0; j < spatialTrkGridSize; j++) {
0244       std::int32_t zBin = firstZBin + j;
0245       double z = getSpatialBinCenter(zBin);
0246       if (z < m_cfg.spatialWindow.first || z > m_cfg.spatialWindow.second) {
0247         continue;
0248       }
0249       // Bin coordinates in the d-z-t plane
0250       Vector3 binCoords(0., z, t);
0251       // Transformation to coordinate system with origin at the track center
0252       binCoords -= impactParams;
0253       Bin bin = {zBin, tBin};
0254       double density = 0;
0255       if (m_cfg.useTime) {
0256         density = multivariateGaussian<3>(binCoords, cov);
0257       } else {
0258         density = multivariateGaussian<2>(binCoords.head<2>(),
0259                                           cov.topLeftCorner<2, 2>());
0260       }
0261       // Only add density if it is positive (otherwise it is 0)
0262       if (density > 0) {
0263         trackDensityMap[bin] = density;
0264       }
0265     }
0266   }
0267
0268   return trackDensityMap;
0269 }
0270
0271 Result<double> AdaptiveGridTrackDensity::estimateSeedWidth(
0272     const DensityMap& densityMap, const ZTPosition& maxZT) const {
0273   if (densityMap.empty()) {
0274     return VertexingError::EmptyInput;
0275   }
0276
0277   // Get z and t bin of max density
0278   std::int32_t zMaxBin = getBin(maxZT.first, m_cfg.spatialBinExtent);
0279   std::int32_t tMaxBin = getBin(maxZT.second, m_cfg.temporalBinExtent);
0280   double maxValue = densityMap.at({zMaxBin, tMaxBin});
0281
0282   std::int32_t rhmBin = zMaxBin;
0283   double gridValue = maxValue;
0284   // Boolean indicating whether we find a filled bin that has a densityValue <=
0285   // maxValue/2
0286   bool binFilled = true;
0287   while (gridValue > maxValue / 2) {
0288     // Check if we are still operating on continuous z values
0289     if (!densityMap.contains({rhmBin + 1, tMaxBin})) {
0290       binFilled = false;
0291       break;
0292     }
0293     rhmBin += 1;
0294     gridValue = densityMap.at({rhmBin, tMaxBin});
0295   }
0296
0297   // Use linear approximation to find better z value for FWHM between bins
0298   double rightDensity = 0;
0299   if (binFilled) {
0300     rightDensity = densityMap.at({rhmBin, tMaxBin});
0301   }
0302   double leftDensity = densityMap.at({rhmBin - 1, tMaxBin});
0303   double deltaZ1 = m_cfg.spatialBinExtent * (maxValue / 2 - leftDensity) /
0304                    (rightDensity - leftDensity);
0305
0306   std::int32_t lhmBin = zMaxBin;
0307   gridValue = maxValue;
0308   binFilled = true;
0309   while (gridValue > maxValue / 2) {
0310     // Check if we are still operating on continuous z values
0311     if (!densityMap.contains({lhmBin - 1, tMaxBin})) {
0312       binFilled = false;
0313       break;
0314     }
0315     lhmBin -= 1;
0316     gridValue = densityMap.at({lhmBin, tMaxBin});
0317   }
0318
0319   // Use linear approximation to find better z value for FWHM between bins
0320   rightDensity = densityMap.at({lhmBin + 1, tMaxBin});
0321   if (binFilled) {
0322     leftDensity = densityMap.at({lhmBin, tMaxBin});
0323   } else {
0324     leftDensity = 0;
0325   }
0326   double deltaZ2 = m_cfg.spatialBinExtent * (rightDensity - maxValue / 2) /
0327                    (rightDensity - leftDensity);
0328
0329   double fwhm = (rhmBin - lhmBin) * m_cfg.spatialBinExtent - deltaZ1 - deltaZ2;
0330
0331   // FWHM = 2.355 * sigma
0332   double width = fwhm / 2.355;
0333
0334   return std::isnormal(width) ? width : 0.0;
0335 }
0336
0337 AdaptiveGridTrackDensity::Bin AdaptiveGridTrackDensity::highestDensitySumBin(
0338     DensityMap& densityMap) const {
0339   // The global maximum
0340   auto firstMax = highestDensityEntry(densityMap);
0341   Bin binFirstMax = firstMax->first;
0342   double valueFirstMax = firstMax->second;
0343   double firstSum = getDensitySum(densityMap, binFirstMax);
0344   // Smaller maxima must have a density of at least:
0345   // valueFirstMax - densityDeviation
0346   double densityDeviation = valueFirstMax * m_cfg.maxRelativeDensityDev;
0347
0348   // Get the second highest maximum
0349   densityMap[binFirstMax] = 0;
0350   auto secondMax = highestDensityEntry(densityMap);
0351   Bin binSecondMax = secondMax->first;
0352   double valueSecondMax = secondMax->second;
0353   double secondSum = 0;
0354   if (valueFirstMax - valueSecondMax < densityDeviation) {
0355     secondSum = getDensitySum(densityMap, binSecondMax);
0356   } else {
0357     // If the second maximum is not sufficiently large the third maximum won't
0358     // be either
0359     densityMap[binFirstMax] = valueFirstMax;
0360     return binFirstMax;
0361   }
0362
0363   // Get the third highest maximum
0364   densityMap[binSecondMax] = 0;
0365   auto thirdMax = highestDensityEntry(densityMap);
0366   Bin binThirdMax = thirdMax->first;
0367   double valueThirdMax = thirdMax->second;
0368   double thirdSum = 0;
0369   if (valueFirstMax - valueThirdMax < densityDeviation) {
0370     thirdSum = getDensitySum(densityMap, binThirdMax);
0371   }
0372
0373   // Revert back to original values
0374   densityMap[binFirstMax] = valueFirstMax;
0375   densityMap[binSecondMax] = valueSecondMax;
0376
0377   // Return the z bin position of the highest density sum
0378   if (secondSum > firstSum && secondSum > thirdSum) {
0379     return binSecondMax;
0380   }
0381   if (thirdSum > secondSum && thirdSum > firstSum) {
0382     return binThirdMax;
0383   }
0384   return binFirstMax;
0385 }
0386
0387 double AdaptiveGridTrackDensity::getDensitySum(const DensityMap& densityMap,
0388                                                const Bin& bin) const {
0389   auto valueOrZero = [&densityMap](const Bin& b) {
0390     if (auto it = densityMap.find(b); it != densityMap.end()) {
0391       return it->second;
0392     }
0393     return 0.0f;
0394   };
0395
0396   // Add density from the bin.
0397   // Check if neighboring bins are part of the densityMap and add them (if they
0398   // are not part of the map, we assume them to be 0).
0399   return valueOrZero(bin) + valueOrZero({bin.first - 1, bin.second}) +
0400          valueOrZero({bin.first + 1, bin.second});
0401 }
0402
0403 }  // namespace Acts