Back to home page

sPhenix code displayed by LXR
 
 

    

File indexing completed on 2025-08-06 08:10:19 

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         (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 = (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((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((spacepoint_t const*)&sp, phi);
0117       }
0118     } else if (sp.r() < m_cfg.rMaxFar) {
0119       sortedSpacepoints.addSP(2, phislice, zslice)
0120           .emplace_back((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       (std::uint32_t)(m_cfg.maxPhideviation / (2 * M_PI / m_cfg.numPhiSlices)) +
0136       1;
0137 
0138   // calculate limits for middle spacepoints
0139   Acts::Vector2 vecA{-m_cfg.maxAbsZ + m_cfg.maxZPosition, m_cfg.rMinFar};
0140   vecA /= 2.;
0141   Acts::Vector2 vecB = {vecA[1], -vecA[0]};
0142   vecB /= std::tan(m_cfg.maxXYZdeviation);
0143   Acts::Vector2 posR = Acts::Vector2(-m_cfg.maxZPosition, 0.) + vecA + vecB;
0144   Acts::ActsScalar R = vecA.norm() / std::sin(m_cfg.maxXYZdeviation);
0145   Acts::ActsScalar constB = -2. * posR[0];
0146   Acts::ActsScalar constC =
0147       posR[0] * posR[0] +
0148       (posR[1] - m_cfg.rMaxNear) * (posR[1] - m_cfg.rMaxNear) - R * R;
0149   Acts::ActsScalar maxZMiddle =
0150       -1. * (-constB - sqrt(constB * constB - 4. * constC)) / 2.;
0151   if (maxZMiddle <= 0) {
0152     ACTS_WARNING(
0153         "maximum position of middle spacepoints is not positive, maxZMiddle = "
0154         << maxZMiddle << ", check your config; setting maxZMiddle to "
0155         << m_cfg.maxAbsZ);
0156     maxZMiddle = m_cfg.maxAbsZ;
0157   }
0158 
0159   // save some constant values for later
0160   Acts::ActsScalar rNearRatio[2] = {m_cfg.rMinNear / m_cfg.rMaxMiddle,
0161                                     m_cfg.rMaxNear / m_cfg.rMinMiddle};
0162   Acts::ActsScalar rMiddle[2] = {m_cfg.rMaxMiddle, m_cfg.rMinMiddle};
0163   Acts::ActsScalar rFarDelta[2] = {
0164       m_cfg.rMaxFar - m_cfg.rMinMiddle,
0165       m_cfg.rMinFar - m_cfg.rMaxMiddle,
0166   };
0167   Acts::ActsScalar zBinLength = 2. * m_cfg.maxAbsZ / m_cfg.numZSlices;
0168 
0169   // limits in terms of slice numbers
0170   std::uint32_t limitMiddleSliceFrom =
0171       (std::uint32_t)((-maxZMiddle + m_cfg.maxAbsZ) / zBinLength);
0172   std::uint32_t limitMiddleSliceTo =
0173       (std::uint32_t)((maxZMiddle + m_cfg.maxAbsZ) / zBinLength + 1);
0174   std::uint32_t limitAbsZSliceFrom = (std::uint32_t)(
0175       (-m_cfg.maxZPosition + m_cfg.maxAbsZ) / zBinLength + 0.01);
0176   std::uint32_t limitAbsZSliceTo =
0177       (std::uint32_t)((m_cfg.maxZPosition + m_cfg.maxAbsZ) / zBinLength + 1.01);
0178 
0179   for (std::uint32_t middleZ = limitMiddleSliceFrom;
0180        middleZ < limitMiddleSliceTo; ++middleZ) {
0181     // skip slices that are empty anyway
0182     if (std::fmod(m_cfg.useFracZSlices * middleZ, 1.0) >=
0183         m_cfg.useFracZSlices) {
0184       continue;
0185     }
0186 
0187     // calculate limits for near spacepoints, assuming the middle spacepoints
0188     // are within some boundaries
0189     bool isLessFrom = (middleZ <= limitAbsZSliceFrom);
0190     Acts::ActsScalar deltaZfrom =
0191         (middleZ - limitAbsZSliceFrom - 1) * zBinLength;
0192     Acts::ActsScalar angleZfrom =
0193         std::atan2(rMiddle[isLessFrom], deltaZfrom) + m_cfg.maxXYZdeviation;
0194     std::uint32_t nearZFrom = 0;
0195     if (angleZfrom < M_PI) {
0196       Acts::ActsScalar new_deltaZfrom =
0197           rMiddle[isLessFrom] / std::tan(angleZfrom) / zBinLength;
0198       nearZFrom = (std::uint32_t)std::max(
0199           new_deltaZfrom * rNearRatio[isLessFrom] + limitAbsZSliceFrom, 0.);
0200     }
0201 
0202     bool isLessTo = (middleZ < limitAbsZSliceTo);
0203     Acts::ActsScalar deltaZto = (middleZ - limitAbsZSliceTo + 1) * zBinLength;
0204     Acts::ActsScalar angleZto =
0205         std::atan2(rMiddle[!isLessTo], deltaZto) - m_cfg.maxXYZdeviation;
0206     std::uint32_t nearZTo = m_cfg.numZSlices;
0207     if (angleZto > 0) {
0208       Acts::ActsScalar new_deltaZto =
0209           rMiddle[!isLessTo] / std::tan(angleZto) / zBinLength;
0210       nearZTo = (std::uint32_t)std::max(
0211           new_deltaZto * rNearRatio[!isLessTo] + limitAbsZSliceTo, 0.);
0212       if (nearZTo > m_cfg.numZSlices) {
0213         nearZTo = m_cfg.numZSlices;
0214       }
0215     }
0216 
0217     for (std::uint32_t nearZ = nearZFrom; nearZ < nearZTo; ++nearZ) {
0218       // calculate limits for far spacepoits, assuming middle and near
0219       // spacepoits are within some boundaries
0220       bool isMiddleLess = (middleZ <= nearZ);
0221 
0222       Acts::ActsScalar delta2Zfrom = (middleZ - nearZ - 1) * zBinLength;
0223       Acts::ActsScalar angle2Zfrom =
0224           std::atan2(rFarDelta[isMiddleLess], delta2Zfrom) +
0225           m_cfg.maxXYZdeviation;
0226       std::uint32_t farZFrom = 0;
0227       if (angle2Zfrom < M_PI) {
0228         farZFrom = (std::uint32_t)std::max(
0229             (rFarDelta[isMiddleLess] / std::tan(angle2Zfrom) / zBinLength) +
0230                 middleZ,
0231             0.);
0232         if (farZFrom >= m_cfg.numZSlices) {
0233           continue;
0234         }
0235       }
0236 
0237       isMiddleLess = (middleZ < nearZ);
0238       Acts::ActsScalar delta2Zto = (middleZ - nearZ + 1) * zBinLength;
0239       Acts::ActsScalar angle2Zto =
0240           std::atan2(rFarDelta[!isMiddleLess], delta2Zto) -
0241           m_cfg.maxXYZdeviation;
0242       std::uint32_t farZTo = m_cfg.numZSlices;
0243       if (angle2Zto > 0) {
0244         farZTo = (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                 Acts::ActsScalar phiA = nearSP.second;
0283 
0284                 // for all middle spacepoints in this slice
0285                 for (const auto& middleSP :
0286                      sortedSpacepoints.getSP(1, middlePhi, middleZ)) {
0287                   Acts::ActsScalar phiB = middleSP.second;
0288                   Acts::ActsScalar deltaPhiAB =
0289                       detail::difference_periodic(phiA, phiB, 2 * M_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                     Acts::ActsScalar phiC = farSP.second;
0298                     Acts::ActsScalar deltaPhiBC =
0299                         detail::difference_periodic(phiB, phiC, 2 * M_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   Acts::ActsScalar alpha1 =
0328       std::atan2(triplet.a.y() - triplet.b.y(), triplet.a.x() - triplet.b.x());
0329   // slope for middle+far spacepoints
0330   Acts::ActsScalar 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   Acts::ActsScalar deltaAlpha =
0334       detail::difference_periodic(alpha1, alpha2, 2 * M_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   Acts::ActsScalar cosTheta = (ab.dot(bc)) / (ab.norm() * bc.norm());
0347   Acts::ActsScalar 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   Acts::ActsScalar norm_size = norm.norm();
0359 
0360   Acts::ActsScalar 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   Acts::ActsScalar 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   Acts::ActsScalar 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, Acts::ActsScalar>
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   Acts::ActsScalar 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<
0423       std::pair<std::pair<Acts::Vector3, Acts::ActsScalar>, Acts::ActsScalar>>
0424       tripletsWithPlanes;
0425   tripletsWithPlanes.reserve(triplets.size());
0426 
0427   for (const auto& triplet : triplets) {
0428     auto abgd = makePlaneFromTriplet(triplet);
0429     tripletsWithPlanes.emplace_back(abgd, -1.);
0430   }
0431 
0432   // elements of the linear equations to solve
0433   Acts::SquareMatrix3 A = Acts::SquareMatrix3::Zero();
0434   Acts::Vector3 B = Acts::Vector3::Zero();
0435   for (const auto& triplet : tripletsWithPlanes) {
0436     const auto& abg = triplet.first.first;
0437     const auto& delta = triplet.first.second;
0438 
0439     A += 2. * (abg * abg.transpose());
0440     B -= 2. * delta * abg;
0441   }
0442 
0443   for (std::uint32_t iter = 0; iter <= m_cfg.maxIterations; iter++) {
0444     // new vertex position
0445     vtx = A.lu().solve(B);
0446 
0447     Acts::Vector3 vtxDiff = vtx - vtxPrev;
0448 
0449     if (vtxDiff.norm() < m_cfg.minVtxShift) {
0450       // difference between the new vertex and the old vertex is not so large
0451       break;
0452     }
0453 
0454     if (iter != m_cfg.maxIterations) {
0455       // is not the last iteration
0456       vtxPrev = vtx;
0457 
0458       for (auto& triplet : tripletsWithPlanes) {
0459         const auto& abg = triplet.first.first;
0460         const auto& delta = triplet.first.second;
0461         Acts::ActsScalar distance = std::abs(abg.dot(vtx) + delta);
0462         triplet.second = distance;
0463       }
0464 
0465       std::sort(tripletsWithPlanes.begin(), tripletsWithPlanes.end(),
0466                 [](const auto& lhs, const auto& rhs) {
0467                   return lhs.second < rhs.second;
0468                 });
0469 
0470       std::uint32_t threshold = (std::uint32_t)(tripletsWithPlanes.size() *
0471                                                 (1. - m_cfg.removeFraction));
0472 
0473       for (std::uint32_t tr = threshold + 1; tr < tripletsWithPlanes.size();
0474            ++tr) {
0475         const auto& abg = tripletsWithPlanes[tr].first.first;
0476         const auto& delta = tripletsWithPlanes[tr].first.second;
0477 
0478         // remove this triplet from A and B
0479         A -= 2. * (abg * abg.transpose());
0480         B += 2. * delta * abg;
0481       }
0482 
0483       // remove all excessive triplets
0484       tripletsWithPlanes.resize(threshold);
0485     }
0486   }
0487 
0488   return vtx;
0489 }
0490 
0491 template <typename spacepoint_t>
0492 Acts::Ray3D Acts::SingleSeedVertexFinder<spacepoint_t>::makeRayFromTriplet(
0493     const Acts::SingleSeedVertexFinder<spacepoint_t>::Triplet& triplet) {
0494   Acts::SquareMatrix3 mat;
0495   mat.row(0) = Acts::Vector3(triplet.a.x(), triplet.a.y(), triplet.a.z());
0496   mat.row(1) = Acts::Vector3(triplet.b.x(), triplet.b.y(), triplet.b.z());
0497   mat.row(2) = Acts::Vector3(triplet.c.x(), triplet.c.y(), triplet.c.z());
0498 
0499   Acts::Vector3 mean = mat.colwise().mean();
0500   Acts::SquareMatrix3 cov = (mat.rowwise() - mean.transpose()).transpose() *
0501                             (mat.rowwise() - mean.transpose()) / 3.;
0502 
0503   // "cov" is self-adjoint matrix
0504   Eigen::SelfAdjointEigenSolver<Acts::SquareMatrix3> saes(cov);
0505   // eigenvalues are sorted in increasing order
0506   Acts::Vector3 eivec = saes.eigenvectors().col(2);
0507 
0508   return {mean, eivec};
0509 }
0510 
0511 template <typename spacepoint_t>
0512 Acts::Vector3
0513 Acts::SingleSeedVertexFinder<spacepoint_t>::findClosestPointFromRays(
0514     const std::vector<Acts::SingleSeedVertexFinder<spacepoint_t>::Triplet>&
0515         triplets) const {
0516   // 1. define function f = sum over all triplets [distance from an unknown
0517   // point
0518   //    (x_0,y_0,z_0) to the ray defined by the triplet]
0519   // 2. find minimum of "f" by partial derivations over x_0, y_0, and z_0
0520   // 3. each derivation has parts linearly depending on x_0, y_0, and z_0
0521   //    (will fill A[][3]) or to nothing (will fill B[])
0522   // 4. solve A*(x_0,y_0,z_0) = B
0523 
0524   Acts::Vector3 vtx = Acts::Vector3::Zero();
0525   Acts::Vector3 vtxPrev{m_cfg.rMaxFar, m_cfg.rMaxFar, m_cfg.maxAbsZ};
0526 
0527   // (startPoint, direction), distance
0528   std::vector<
0529       std::pair<std::pair<Acts::Vector3, Acts::Vector3>, Acts::ActsScalar>>
0530       tripletsWithRays;
0531   tripletsWithRays.reserve(triplets.size());
0532 
0533   for (const auto& triplet : triplets) {
0534     tripletsWithRays.emplace_back(
0535         std::make_pair(triplet.ray.origin(), triplet.ray.dir()), -1.);
0536   }
0537 
0538   // elements of the linear equations to solve
0539   Acts::SquareMatrix3 A =
0540       Acts::SquareMatrix3::Identity() * 2. * triplets.size();
0541   Acts::Vector3 B = Acts::Vector3::Zero();
0542   for (const auto& triplet : tripletsWithRays) {
0543     // use ray saved from earlier
0544     const auto& startPoint = triplet.first.first;
0545     const auto& direction = triplet.first.second;
0546 
0547     A -= 2. * (direction * direction.transpose());
0548     B += -2. * direction * (direction.dot(startPoint)) + 2. * startPoint;
0549   }
0550 
0551   for (std::uint32_t iter = 0; iter <= m_cfg.maxIterations; iter++) {
0552     // new vertex position
0553     vtx = A.lu().solve(B);
0554 
0555     Acts::Vector3 vtxDiff = vtx - vtxPrev;
0556 
0557     if (vtxDiff.norm() < m_cfg.minVtxShift) {
0558       // difference between the new vertex and the old vertex is not so large
0559       break;
0560     }
0561 
0562     if (iter != m_cfg.maxIterations) {
0563       // is not the last iteration
0564       vtxPrev = vtx;
0565 
0566       for (auto& triplet : tripletsWithRays) {
0567         const auto& startPoint = triplet.first.first;
0568         const auto& direction = triplet.first.second;
0569         Acts::ActsScalar distance = (vtx - startPoint).cross(direction).norm();
0570         triplet.second = distance;
0571       }
0572 
0573       std::sort(tripletsWithRays.begin(), tripletsWithRays.end(),
0574                 [](const auto& lhs, const auto& rhs) {
0575                   return lhs.second < rhs.second;
0576                 });
0577 
0578       std::uint32_t threshold = (std::uint32_t)(tripletsWithRays.size() *
0579                                                 (1. - m_cfg.removeFraction));
0580 
0581       for (std::uint32_t tr = threshold + 1; tr < tripletsWithRays.size();
0582            ++tr) {
0583         const auto& startPoint = tripletsWithRays[tr].first.first;
0584         const auto& direction = tripletsWithRays[tr].first.second;
0585 
0586         // remove this triplet from A and B
0587         A -= Acts::SquareMatrix3::Identity() * 2.;
0588         A += 2. * (direction * direction.transpose());
0589         B -= -2. * direction * (direction.dot(startPoint)) + 2. * startPoint;
0590       }
0591 
0592       // remove all excessive triplets
0593       tripletsWithRays.resize(threshold);
0594     }
0595   }
0596 
0597   return vtx;
0598 }
[ 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!