Back to home page

EIC code displayed by LXR
 
 

    

File indexing completed on 2025-01-18 09:28:00 

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