Back to home page

EIC code displayed by LXR
 
 

    

File indexing completed on 2025-01-18 09:11:14 

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 <algorithm>
0010 #include <cmath>
0011 #include <numbers>
0012 #include <system_error>
0013 
0014 #include <Eigen/Eigenvalues>
0015 
0016 template <typename spacepoint_t>
0017 Acts::SingleSeedVertexFinder<spacepoint_t>::SingleSeedVertexFinder(
0018     const Acts::SingleSeedVertexFinder<spacepoint_t>::Config& cfg,
0019     std::unique_ptr<const Logger> lgr)
0020     : m_cfg(cfg), m_logger(std::move(lgr)) {
0021   if (cfg.numPhiSlices < 3) {
0022     ACTS_INFO("value of numPhiSlices is "
0023               << cfg.numPhiSlices
0024               << ", which is less than 3. There will be duplicate triplets.");
0025   }
0026   if (cfg.useFracPhiSlices <= 0. || cfg.useFracPhiSlices > 1.) {
0027     ACTS_ERROR("value of useFracPhiSlices is "
0028                << cfg.useFracPhiSlices
0029                << ", allowed values are between 0 and 1");
0030   }
0031   if (cfg.useFracZSlices <= 0. || cfg.useFracZSlices > 1.) {
0032     ACTS_ERROR("value of useFracZSlices is "
0033                << cfg.useFracZSlices << ", allowed values are between 0 and 1");
0034   }
0035   if (cfg.minimalizeWRT != "planes" && cfg.minimalizeWRT != "rays") {
0036     ACTS_ERROR("value of minimalizeWRT is "
0037                << cfg.minimalizeWRT
0038                << ", allowed values are \"planes\" or \"rays\" ");
0039   }
0040   if (cfg.removeFraction < 0. || cfg.removeFraction >= 1.) {
0041     ACTS_ERROR("value of removeFraction is "
0042                << cfg.removeFraction << ", allowed values are between 0 and 1");
0043   }
0044 }
0045 
0046 template <typename spacepoint_t>
0047 Acts::Result<Acts::Vector3>
0048 Acts::SingleSeedVertexFinder<spacepoint_t>::findVertex(
0049     const std::vector<spacepoint_t>& spacepoints) const {
0050   // sort spacepoints to different phi and z slices
0051   Acts::SingleSeedVertexFinder<spacepoint_t>::SortedSpacepoints
0052       sortedSpacepoints = sortSpacepoints(spacepoints);
0053 
0054   // find triplets
0055   std::vector<Acts::SingleSeedVertexFinder<spacepoint_t>::Triplet> triplets =
0056       findTriplets(sortedSpacepoints);
0057 
0058   // if no valid triplets found
0059   if (triplets.empty()) {
0060     return Acts::Result<Acts::Vector3>::failure(std::error_code());
0061   }
0062 
0063   Acts::Vector3 vtx = Acts::Vector3::Zero();
0064   if (m_cfg.minimalizeWRT == "planes") {
0065     // find a point closest to all planes defined by the triplets
0066     vtx = findClosestPointFromPlanes(triplets);
0067   } else if (m_cfg.minimalizeWRT == "rays") {
0068     // find a point closest to all rays fitted through the triplets
0069     vtx = findClosestPointFromRays(triplets);
0070   } else {
0071     ACTS_ERROR("value of minimalizeWRT is "
0072                << m_cfg.minimalizeWRT
0073                << ", allowed values are \"planes\" or \"rays\" ");
0074   }
0075 
0076   return Acts::Result<Acts::Vector3>::success(vtx);
0077 }
0078 
0079 template <typename spacepoint_t>
0080 typename Acts::SingleSeedVertexFinder<spacepoint_t>::SortedSpacepoints
0081 Acts::SingleSeedVertexFinder<spacepoint_t>::sortSpacepoints(
0082     const std::vector<spacepoint_t>& spacepoints) const {
0083   Acts::SingleSeedVertexFinder<spacepoint_t>::SortedSpacepoints
0084       sortedSpacepoints(m_cfg.numPhiSlices, m_cfg.numZSlices);
0085 
0086   for (const auto& sp : spacepoints) {
0087     // phi will be saved for later
0088     double phi = detail::radian_pos(std::atan2(sp.y(), sp.x()));
0089     std::uint32_t phislice = static_cast<std::uint32_t>(
0090         phi / (2 * std::numbers::pi) * m_cfg.numPhiSlices);
0091     if (phislice >= m_cfg.numPhiSlices) {
0092       phislice = 0;
0093     }
0094 
0095     if (std::abs(sp.z()) >= m_cfg.maxAbsZ) {
0096       continue;
0097     }
0098     std::uint32_t zslice = static_cast<std::uint32_t>(
0099         (sp.z() + m_cfg.maxAbsZ) / (2 * m_cfg.maxAbsZ) * m_cfg.numZSlices);
0100 
0101     // input spacepoint is sorted into one subset
0102     if (sp.r() < m_cfg.rMinMiddle) {
0103       if (m_cfg.rMinNear < sp.r() && sp.r() < m_cfg.rMaxNear) {
0104         if (std::fmod(m_cfg.useFracPhiSlices * phislice, 1.0) >=
0105             m_cfg.useFracPhiSlices) {
0106           continue;
0107         }
0108         sortedSpacepoints.addSP(0, phislice, zslice)
0109             .emplace_back(static_cast<spacepoint_t const*>(&sp), phi);
0110       }
0111     } else if (sp.r() < m_cfg.rMinFar) {
0112       if (sp.r() < m_cfg.rMaxMiddle) {
0113         if (std::fmod(m_cfg.useFracZSlices * zslice, 1.0) >=
0114             m_cfg.useFracZSlices) {
0115           continue;
0116         }
0117         sortedSpacepoints.addSP(1, phislice, zslice)
0118             .emplace_back(static_cast<spacepoint_t const*>(&sp), phi);
0119       }
0120     } else if (sp.r() < m_cfg.rMaxFar) {
0121       sortedSpacepoints.addSP(2, phislice, zslice)
0122           .emplace_back(static_cast<spacepoint_t const*>(&sp), phi);
0123     }
0124   }
0125 
0126   return sortedSpacepoints;
0127 }
0128 
0129 template <typename spacepoint_t>
0130 std::vector<typename Acts::SingleSeedVertexFinder<spacepoint_t>::Triplet>
0131 Acts::SingleSeedVertexFinder<spacepoint_t>::findTriplets(
0132     const Acts::SingleSeedVertexFinder<spacepoint_t>::SortedSpacepoints&
0133         sortedSpacepoints) const {
0134   std::vector<Acts::SingleSeedVertexFinder<spacepoint_t>::Triplet> triplets;
0135 
0136   std::uint32_t phiStep =
0137       static_cast<std::uint32_t>(m_cfg.maxPhideviation /
0138                                  (2 * std::numbers::pi / m_cfg.numPhiSlices)) +
0139       1;
0140 
0141   // calculate limits for middle spacepoints
0142   Acts::Vector2 vecA{-m_cfg.maxAbsZ + m_cfg.maxZPosition, m_cfg.rMinFar};
0143   vecA /= 2.;
0144   Acts::Vector2 vecB = {vecA[1], -vecA[0]};
0145   vecB /= std::tan(m_cfg.maxXYZdeviation);
0146   Acts::Vector2 posR = Acts::Vector2(-m_cfg.maxZPosition, 0.) + vecA + vecB;
0147   double R = vecA.norm() / std::sin(m_cfg.maxXYZdeviation);
0148   double constB = -2. * posR[0];
0149   double constC = posR[0] * posR[0] +
0150                   (posR[1] - m_cfg.rMaxNear) * (posR[1] - m_cfg.rMaxNear) -
0151                   R * R;
0152   double maxZMiddle =
0153       -1. * (-constB - sqrt(constB * constB - 4. * constC)) / 2.;
0154   if (maxZMiddle <= 0) {
0155     ACTS_WARNING(
0156         "maximum position of middle spacepoints is not positive, maxZMiddle = "
0157         << maxZMiddle << ", check your config; setting maxZMiddle to "
0158         << m_cfg.maxAbsZ);
0159     maxZMiddle = m_cfg.maxAbsZ;
0160   }
0161 
0162   // save some constant values for later
0163   double rNearRatio[2] = {m_cfg.rMinNear / m_cfg.rMaxMiddle,
0164                           m_cfg.rMaxNear / m_cfg.rMinMiddle};
0165   double rMiddle[2] = {m_cfg.rMaxMiddle, m_cfg.rMinMiddle};
0166   double rFarDelta[2] = {
0167       m_cfg.rMaxFar - m_cfg.rMinMiddle,
0168       m_cfg.rMinFar - m_cfg.rMaxMiddle,
0169   };
0170   double zBinLength = 2. * m_cfg.maxAbsZ / m_cfg.numZSlices;
0171 
0172   // limits in terms of slice numbers
0173   std::uint32_t limitMiddleSliceFrom =
0174       static_cast<std::uint32_t>((-maxZMiddle + m_cfg.maxAbsZ) / zBinLength);
0175   std::uint32_t limitMiddleSliceTo =
0176       static_cast<std::uint32_t>((maxZMiddle + m_cfg.maxAbsZ) / zBinLength + 1);
0177   std::uint32_t limitAbsZSliceFrom = static_cast<std::uint32_t>(
0178       (-m_cfg.maxZPosition + m_cfg.maxAbsZ) / zBinLength + 0.01);
0179   std::uint32_t limitAbsZSliceTo = static_cast<std::uint32_t>(
0180       (m_cfg.maxZPosition + m_cfg.maxAbsZ) / zBinLength + 1.01);
0181 
0182   for (std::uint32_t middleZ = limitMiddleSliceFrom;
0183        middleZ < limitMiddleSliceTo; ++middleZ) {
0184     // skip slices that are empty anyway
0185     if (std::fmod(m_cfg.useFracZSlices * middleZ, 1.0) >=
0186         m_cfg.useFracZSlices) {
0187       continue;
0188     }
0189 
0190     // calculate limits for near spacepoints, assuming the middle spacepoints
0191     // are within some boundaries
0192     bool isLessFrom = (middleZ <= limitAbsZSliceFrom);
0193     double deltaZfrom = (middleZ - limitAbsZSliceFrom - 1) * zBinLength;
0194     double angleZfrom =
0195         std::atan2(rMiddle[isLessFrom], deltaZfrom) + m_cfg.maxXYZdeviation;
0196     std::uint32_t nearZFrom = 0;
0197     if (angleZfrom < std::numbers::pi) {
0198       double new_deltaZfrom =
0199           rMiddle[isLessFrom] / std::tan(angleZfrom) / zBinLength;
0200       nearZFrom = static_cast<std::uint32_t>(std::max(
0201           new_deltaZfrom * rNearRatio[isLessFrom] + limitAbsZSliceFrom, 0.));
0202     }
0203 
0204     bool isLessTo = (middleZ < limitAbsZSliceTo);
0205     double deltaZto = (middleZ - limitAbsZSliceTo + 1) * zBinLength;
0206     double angleZto =
0207         std::atan2(rMiddle[!isLessTo], deltaZto) - m_cfg.maxXYZdeviation;
0208     std::uint32_t nearZTo = m_cfg.numZSlices;
0209     if (angleZto > 0) {
0210       double new_deltaZto =
0211           rMiddle[!isLessTo] / std::tan(angleZto) / zBinLength;
0212       nearZTo = static_cast<std::uint32_t>(std::max(
0213           new_deltaZto * rNearRatio[!isLessTo] + limitAbsZSliceTo, 0.));
0214       if (nearZTo > m_cfg.numZSlices) {
0215         nearZTo = m_cfg.numZSlices;
0216       }
0217     }
0218 
0219     for (std::uint32_t nearZ = nearZFrom; nearZ < nearZTo; ++nearZ) {
0220       // calculate limits for far spacepoints, assuming middle and near
0221       // spacepoints are within some boundaries
0222       bool isMiddleLess = (middleZ <= nearZ);
0223 
0224       double delta2Zfrom = (middleZ - nearZ - 1) * zBinLength;
0225       double angle2Zfrom = std::atan2(rFarDelta[isMiddleLess], delta2Zfrom) +
0226                            m_cfg.maxXYZdeviation;
0227       std::uint32_t farZFrom = 0;
0228       if (angle2Zfrom < std::numbers::pi) {
0229         farZFrom = static_cast<std::uint32_t>(std::max(
0230             (rFarDelta[isMiddleLess] / std::tan(angle2Zfrom) / zBinLength) +
0231                 middleZ,
0232             0.));
0233         if (farZFrom >= m_cfg.numZSlices) {
0234           continue;
0235         }
0236       }
0237 
0238       isMiddleLess = (middleZ < nearZ);
0239       double delta2Zto = (middleZ - nearZ + 1) * zBinLength;
0240       double angle2Zto = std::atan2(rFarDelta[!isMiddleLess], delta2Zto) -
0241                          m_cfg.maxXYZdeviation;
0242       std::uint32_t farZTo = m_cfg.numZSlices;
0243       if (angle2Zto > 0) {
0244         farZTo = static_cast<std::uint32_t>(std::max(
0245             (rFarDelta[!isMiddleLess] / std::tan(angle2Zto) / zBinLength) +
0246                 middleZ + 1,
0247             0.));
0248         if (farZTo > m_cfg.numZSlices) {
0249           farZTo = m_cfg.numZSlices;
0250         } else if (farZTo == 0) {
0251           continue;
0252         }
0253       }
0254 
0255       for (std::uint32_t farZ = farZFrom; farZ < farZTo; farZ++) {
0256         // loop over near phi slices
0257         for (std::uint32_t nearPhi = 0; nearPhi < m_cfg.numPhiSlices;
0258              ++nearPhi) {
0259           // skip slices that are empty anyway
0260           if (std::fmod(m_cfg.useFracPhiSlices * nearPhi, 1.0) >=
0261               m_cfg.useFracPhiSlices) {
0262             continue;
0263           }
0264 
0265           // loop over some middle phi slices
0266           for (std::uint32_t middlePhi_h =
0267                    m_cfg.numPhiSlices + nearPhi - phiStep;
0268                middlePhi_h <= m_cfg.numPhiSlices + nearPhi + phiStep;
0269                ++middlePhi_h) {
0270             std::uint32_t middlePhi = middlePhi_h % m_cfg.numPhiSlices;
0271 
0272             // loop over some far phi slices
0273             for (std::uint32_t farPhi_h =
0274                      m_cfg.numPhiSlices + middlePhi - phiStep;
0275                  farPhi_h <= m_cfg.numPhiSlices + middlePhi + phiStep;
0276                  ++farPhi_h) {
0277               std::uint32_t farPhi = farPhi_h % m_cfg.numPhiSlices;
0278 
0279               // for all near spacepoints in this slice
0280               for (const auto& nearSP :
0281                    sortedSpacepoints.getSP(0, nearPhi, nearZ)) {
0282                 double phiA = nearSP.second;
0283 
0284                 // for all middle spacepoints in this slice
0285                 for (const auto& middleSP :
0286                      sortedSpacepoints.getSP(1, middlePhi, middleZ)) {
0287                   double phiB = middleSP.second;
0288                   double deltaPhiAB = detail::difference_periodic(
0289                       phiA, phiB, 2 * std::numbers::pi);
0290                   if (std::abs(deltaPhiAB) > m_cfg.maxPhideviation) {
0291                     continue;
0292                   }
0293 
0294                   // for all far spacepoints in this slice
0295                   for (const auto& farSP :
0296                        sortedSpacepoints.getSP(2, farPhi, farZ)) {
0297                     double phiC = farSP.second;
0298                     double deltaPhiBC = detail::difference_periodic(
0299                         phiB, phiC, 2 * std::numbers::pi);
0300                     if (std::abs(deltaPhiBC) > m_cfg.maxPhideviation) {
0301                       continue;
0302                     }
0303 
0304                     Acts::SingleSeedVertexFinder<spacepoint_t>::Triplet tr(
0305                         *nearSP.first, *middleSP.first, *farSP.first);
0306 
0307                     if (tripletValidationAndUpdate(tr)) {
0308                       triplets.push_back(tr);
0309                     }
0310                   }  // loop over far spacepoints
0311                 }  // loop over middle spacepoints
0312               }  // loop over near spacepoints
0313             }  // loop over far phi slices
0314           }  // loop over middle phi slices
0315         }  // loop over near phi slices
0316       }  // loop over far Z slices
0317     }  // loop over near Z slices
0318   }  // loop over middle Z slices
0319 
0320   return triplets;
0321 }
0322 
0323 template <typename spacepoint_t>
0324 bool Acts::SingleSeedVertexFinder<spacepoint_t>::tripletValidationAndUpdate(
0325     Acts::SingleSeedVertexFinder<spacepoint_t>::Triplet& triplet) const {
0326   // slope for near+middle spacepoints
0327   double alpha1 =
0328       std::atan2(triplet.a.y() - triplet.b.y(), triplet.a.x() - triplet.b.x());
0329   // slope for middle+far spacepoints
0330   double alpha2 =
0331       std::atan2(triplet.b.y() - triplet.c.y(), triplet.b.x() - triplet.c.x());
0332   // these two slopes shouldn't be too different
0333   double deltaAlpha =
0334       detail::difference_periodic(alpha1, alpha2, 2 * std::numbers::pi);
0335   if (std::abs(deltaAlpha) > m_cfg.maxXYdeviation) {
0336     return false;
0337   }
0338 
0339   // near-middle ray
0340   Acts::Vector3 ab{triplet.a.x() - triplet.b.x(), triplet.a.y() - triplet.b.y(),
0341                    triplet.a.z() - triplet.b.z()};
0342   // middle-far ray
0343   Acts::Vector3 bc{triplet.b.x() - triplet.c.x(), triplet.b.y() - triplet.c.y(),
0344                    triplet.b.z() - triplet.c.z()};
0345   // dot product of these two
0346   double cosTheta = (ab.dot(bc)) / (ab.norm() * bc.norm());
0347   double theta = std::acos(cosTheta);
0348   if (theta > m_cfg.maxXYZdeviation) {
0349     return false;
0350   }
0351 
0352   // reject the ray if it doesn't come close to the z-axis
0353   Acts::Ray3D ray = makeRayFromTriplet(triplet);
0354   const Acts::Vector3& startPoint = ray.origin();
0355   const Acts::Vector3& direction = ray.dir();
0356   // norm to z-axis and to the ray
0357   Acts::Vector3 norm{-1. * direction[1], 1. * direction[0], 0};
0358   double norm_size = norm.norm();
0359 
0360   double tanTheta = norm_size / direction[2];
0361   if (std::abs(tanTheta) < std::tan(m_cfg.minTheta)) {
0362     return false;
0363   }
0364 
0365   // nearest distance from the ray to z-axis
0366   double dist = std::abs(startPoint.dot(norm)) / norm_size;
0367   if (dist > m_cfg.maxRPosition) {
0368     return false;
0369   }
0370 
0371   // z coordinate of the nearest distance from the ray to z-axis
0372   double zDist =
0373       direction.cross(norm).dot(startPoint) / (norm_size * norm_size);
0374   if (std::abs(zDist) > m_cfg.maxZPosition) {
0375     return false;
0376   }
0377 
0378   if (m_cfg.minimalizeWRT == "rays") {
0379     // save for later
0380     triplet.ray = ray;
0381   }
0382 
0383   return true;
0384 }
0385 
0386 template <typename spacepoint_t>
0387 std::pair<Acts::Vector3, double>
0388 Acts::SingleSeedVertexFinder<spacepoint_t>::makePlaneFromTriplet(
0389     const Acts::SingleSeedVertexFinder<spacepoint_t>::Triplet& triplet) {
0390   Acts::Vector3 a{triplet.a.x(), triplet.a.y(), triplet.a.z()};
0391   Acts::Vector3 b{triplet.b.x(), triplet.b.y(), triplet.b.z()};
0392   Acts::Vector3 c{triplet.c.x(), triplet.c.y(), triplet.c.z()};
0393 
0394   Acts::Vector3 ba = b - a, ca = c - a;
0395 
0396   // vector (alpha,beta,gamma) normalized to unity for convenience
0397   Acts::Vector3 abg = ba.cross(ca).normalized();
0398   double delta = -1. * abg.dot(a);
0399 
0400   // plane (alpha*x + beta*y + gamma*z + delta = 0), split to {{alpha, beta,
0401   // gamma}, delta} for convenience
0402   return {abg, delta};
0403 }
0404 
0405 template <typename spacepoint_t>
0406 Acts::Vector3
0407 Acts::SingleSeedVertexFinder<spacepoint_t>::findClosestPointFromPlanes(
0408     const std::vector<Acts::SingleSeedVertexFinder<spacepoint_t>::Triplet>&
0409         triplets) const {
0410   // 1. define function f = sum over all triplets [distance from an unknown
0411   // point
0412   //    (x_0,y_0,z_0) to the plane defined by the triplet]
0413   // 2. find minimum of "f" by partial derivations over x_0, y_0, and z_0
0414   // 3. each derivation has parts linearly depending on x_0, y_0, and z_0
0415   //    (will fill A[deriv][3]) or to nothing (will fill B[deriv])
0416   // 4. solve A*(x_0,y_0,z_0) = B
0417 
0418   Acts::Vector3 vtx = Acts::Vector3::Zero();
0419   Acts::Vector3 vtxPrev{m_cfg.rMaxFar, m_cfg.rMaxFar, m_cfg.maxAbsZ};
0420 
0421   // (alpha-beta-gamma, delta), distance
0422   std::vector<std::pair<std::pair<Acts::Vector3, double>, double>>
0423       tripletsWithPlanes;
0424   tripletsWithPlanes.reserve(triplets.size());
0425 
0426   for (const auto& triplet : triplets) {
0427     auto abgd = makePlaneFromTriplet(triplet);
0428     tripletsWithPlanes.emplace_back(abgd, -1.);
0429   }
0430 
0431   // elements of the linear equations to solve
0432   Acts::SquareMatrix3 A = Acts::SquareMatrix3::Zero();
0433   Acts::Vector3 B = Acts::Vector3::Zero();
0434   for (const auto& triplet : tripletsWithPlanes) {
0435     const auto& abg = triplet.first.first;
0436     const auto& delta = triplet.first.second;
0437 
0438     A += 2. * (abg * abg.transpose());
0439     B -= 2. * delta * abg;
0440   }
0441 
0442   for (std::uint32_t iter = 0; iter <= m_cfg.maxIterations; iter++) {
0443     // new vertex position
0444     vtx = A.lu().solve(B);
0445 
0446     Acts::Vector3 vtxDiff = vtx - vtxPrev;
0447 
0448     if (vtxDiff.norm() < m_cfg.minVtxShift) {
0449       // difference between the new vertex and the old vertex is not so large
0450       break;
0451     }
0452 
0453     if (iter != m_cfg.maxIterations) {
0454       // is not the last iteration
0455       vtxPrev = vtx;
0456 
0457       for (auto& triplet : tripletsWithPlanes) {
0458         const auto& abg = triplet.first.first;
0459         const auto& delta = triplet.first.second;
0460         double distance = std::abs(abg.dot(vtx) + delta);
0461         triplet.second = distance;
0462       }
0463 
0464       std::ranges::sort(tripletsWithPlanes, {},
0465                         [](const auto& t) { return t.second; });
0466 
0467       std::uint32_t threshold = static_cast<std::uint32_t>(
0468           tripletsWithPlanes.size() * (1. - m_cfg.removeFraction));
0469 
0470       for (std::uint32_t tr = threshold + 1; tr < tripletsWithPlanes.size();
0471            ++tr) {
0472         const auto& abg = tripletsWithPlanes[tr].first.first;
0473         const auto& delta = tripletsWithPlanes[tr].first.second;
0474 
0475         // remove this triplet from A and B
0476         A -= 2. * (abg * abg.transpose());
0477         B += 2. * delta * abg;
0478       }
0479 
0480       // remove all excessive triplets
0481       tripletsWithPlanes.resize(threshold);
0482     }
0483   }
0484 
0485   return vtx;
0486 }
0487 
0488 template <typename spacepoint_t>
0489 Acts::Ray3D Acts::SingleSeedVertexFinder<spacepoint_t>::makeRayFromTriplet(
0490     const Acts::SingleSeedVertexFinder<spacepoint_t>::Triplet& triplet) {
0491   Acts::SquareMatrix3 mat;
0492   mat.row(0) = Acts::Vector3(triplet.a.x(), triplet.a.y(), triplet.a.z());
0493   mat.row(1) = Acts::Vector3(triplet.b.x(), triplet.b.y(), triplet.b.z());
0494   mat.row(2) = Acts::Vector3(triplet.c.x(), triplet.c.y(), triplet.c.z());
0495 
0496   Acts::Vector3 mean = mat.colwise().mean();
0497   Acts::SquareMatrix3 cov = (mat.rowwise() - mean.transpose()).transpose() *
0498                             (mat.rowwise() - mean.transpose()) / 3.;
0499 
0500   // "cov" is self-adjoint matrix
0501   Eigen::SelfAdjointEigenSolver<Acts::SquareMatrix3> saes(cov);
0502   // eigenvalues are sorted in increasing order
0503   Acts::Vector3 eivec = saes.eigenvectors().col(2);
0504 
0505   return {mean, eivec};
0506 }
0507 
0508 template <typename spacepoint_t>
0509 Acts::Vector3
0510 Acts::SingleSeedVertexFinder<spacepoint_t>::findClosestPointFromRays(
0511     const std::vector<Acts::SingleSeedVertexFinder<spacepoint_t>::Triplet>&
0512         triplets) const {
0513   // 1. define function f = sum over all triplets [distance from an unknown
0514   // point
0515   //    (x_0,y_0,z_0) to the ray defined by the triplet]
0516   // 2. find minimum of "f" by partial derivations over x_0, y_0, and z_0
0517   // 3. each derivation has parts linearly depending on x_0, y_0, and z_0
0518   //    (will fill A[][3]) or to nothing (will fill B[])
0519   // 4. solve A*(x_0,y_0,z_0) = B
0520 
0521   Acts::Vector3 vtx = Acts::Vector3::Zero();
0522   Acts::Vector3 vtxPrev{m_cfg.rMaxFar, m_cfg.rMaxFar, m_cfg.maxAbsZ};
0523 
0524   // (startPoint, direction), distance
0525   std::vector<std::pair<std::pair<Acts::Vector3, Acts::Vector3>, double>>
0526       tripletsWithRays;
0527   tripletsWithRays.reserve(triplets.size());
0528 
0529   for (const auto& triplet : triplets) {
0530     tripletsWithRays.emplace_back(
0531         std::make_pair(triplet.ray.origin(), triplet.ray.dir()), -1.);
0532   }
0533 
0534   // elements of the linear equations to solve
0535   Acts::SquareMatrix3 A =
0536       Acts::SquareMatrix3::Identity() * 2. * triplets.size();
0537   Acts::Vector3 B = Acts::Vector3::Zero();
0538   for (const auto& triplet : tripletsWithRays) {
0539     // use ray saved from earlier
0540     const auto& startPoint = triplet.first.first;
0541     const auto& direction = triplet.first.second;
0542 
0543     A -= 2. * (direction * direction.transpose());
0544     B += -2. * direction * (direction.dot(startPoint)) + 2. * startPoint;
0545   }
0546 
0547   for (std::uint32_t iter = 0; iter <= m_cfg.maxIterations; iter++) {
0548     // new vertex position
0549     vtx = A.lu().solve(B);
0550 
0551     Acts::Vector3 vtxDiff = vtx - vtxPrev;
0552 
0553     if (vtxDiff.norm() < m_cfg.minVtxShift) {
0554       // difference between the new vertex and the old vertex is not so large
0555       break;
0556     }
0557 
0558     if (iter != m_cfg.maxIterations) {
0559       // is not the last iteration
0560       vtxPrev = vtx;
0561 
0562       for (auto& triplet : tripletsWithRays) {
0563         const auto& startPoint = triplet.first.first;
0564         const auto& direction = triplet.first.second;
0565         double distance = (vtx - startPoint).cross(direction).norm();
0566         triplet.second = distance;
0567       }
0568 
0569       std::ranges::sort(tripletsWithRays, {},
0570                         [](const auto& t) { return t.second; });
0571 
0572       std::uint32_t threshold = static_cast<std::uint32_t>(
0573           tripletsWithRays.size() * (1. - m_cfg.removeFraction));
0574 
0575       for (std::uint32_t tr = threshold + 1; tr < tripletsWithRays.size();
0576            ++tr) {
0577         const auto& startPoint = tripletsWithRays[tr].first.first;
0578         const auto& direction = tripletsWithRays[tr].first.second;
0579 
0580         // remove this triplet from A and B
0581         A -= Acts::SquareMatrix3::Identity() * 2.;
0582         A += 2. * (direction * direction.transpose());
0583         B -= -2. * direction * (direction.dot(startPoint)) + 2. * startPoint;
0584       }
0585 
0586       // remove all excessive triplets
0587       tripletsWithRays.resize(threshold);
0588     }
0589   }
0590 
0591   return vtx;
0592 }
[ Source navigation ] [ Diff markup ] [ Identifier search ] [ general search ]

This page was automatically generated by the 2.3.7 LXR engine.

The LXR team
Valid CSS 2.1! Valid HTML 4.01!