CkfPattern/plugins/TrajectorySegmentBuilder.cc

0001 #include <algorithm>
0002
0003 #include "TrajectorySegmentBuilder.h"
0004
0005 #include "TrackingTools/PatternTools/interface/TempTrajectory.h"
0006
0007 #include "DataFormats/TrajectorySeed/interface/TrajectorySeed.h"
0008 #include "TrackingTools/KalmanUpdators/interface/KFUpdator.h"
0009 #include "TrackingTools/DetLayers/interface/MeasurementEstimator.h"
0010 #include "TrackingTools/PatternTools/interface/Trajectory.h"
0011 #include "TrackingTools/PatternTools/interface/TrajectoryMeasurement.h"
0012 #include "TrackingTools/PatternTools/interface/TrajMeasLessEstim.h"
0013 #include "TrackingTools/MeasurementDet/interface/TrajectoryMeasurementGroup.h"
0014 #include "TrackingTools/DetLayers/interface/DetGroup.h"
0015 #include "TrackingTools/DetLayers/interface/DetLayer.h"
0016 #include "TrajectoryLessByFoundHits.h"
0017 #include "TrackingTools/PatternTools/interface/TrajectoryStateUpdator.h"
0018 #include "TrackingTools/DetLayers/interface/GeomDetCompatibilityChecker.h"
0019 #include "TrackingTools/MeasurementDet/interface/MeasurementDet.h"
0020
0021 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0022 #include "FWCore/Utilities/interface/thread_safety_macros.h"
0023
0024 // #define DBG_TSB
0025 // #define STAT_TSB
0026
0027 namespace {
0028 #ifdef STAT_TSB
0029   struct StatCount {
0030     long long totGroup;
0031     long long totSeg;
0032     long long totLockHits;
0033     long long totInvCand;
0034     long long trunc;
0035     void zero() { totGroup = totSeg = totLockHits = totInvCand = trunc = 0; }
0036     void incr(long long g, long long s, long long l) {
0037       totGroup += g;
0038       totSeg += s;
0039       totLockHits += l;
0040     }
0041     void truncated() { ++trunc; }
0042     void invalid() { ++totInvCand; }
0043     void print() const {
0044       std::cout << "TrajectorySegmentBuilder stat\nGroup/Seg/Lock/Inv/Trunc " << totGroup << '/' << totSeg << '/'
0045                 << totLockHits << '/' << totInvCand << '/' << trunc << std::endl;
0046     }
0047     StatCount() { zero(); }
0048     ~StatCount() { print(); }
0049   };
0050   StatCount statCount;
0051
0052 #else
0053   struct StatCount {
0054     void incr(long long, long long, long long) {}
0055     void truncated() {}
0056     void invalid() {}
0057   };
0058   CMS_THREAD_SAFE StatCount statCount;
0059 #endif
0060
0061 }  // namespace
0062
0063 using namespace std;
0064
0065 TrajectorySegmentBuilder::TempTrajectoryContainer TrajectorySegmentBuilder::segments(const TSOS startingState) {
0066   //
0067   // create empty trajectory
0068   //
0069   theLockedHits.clear();
0070   TempTrajectory startingTrajectory(theFullPropagator.propagationDirection(), 0);
0071   //
0072   // get measurement groups
0073   //
0074   auto&& measGroups =
0075       theLayerMeasurements->groupedMeasurements(theLayer, startingState, theFullPropagator, theEstimator);
0076
0077 #ifdef DBG_TSB
0078   cout << "TSB: number of measurement groups = " << measGroups.size() << endl;
0079   //  theDbgFlg = measGroups.size()>1;
0080   theDbgFlg = true;
0081 #else
0082   theDbgFlg = false;
0083 #endif
0084
0085 #ifdef TSB_TRUNCATE
0086   //  V.I. to me makes things slower...
0087
0088   //
0089   // check number of combinations
0090   //
0091   constexpr long long MAXCOMB = 100000000;
0092   long long ncomb(1);
0093   int ngrp(0);
0094   bool truncate(false);
0095   for (auto const& gr : measGroups) {
0096     ++ngrp;
0097     int nhit(0);
0098     for (auto const& m : gr.measurements())
0099       if LIKELY (m.recHitR().isValid())
0100         nhit++;
0101
0102     if (nhit > 1)
0103       ncomb *= nhit;
0104     if UNLIKELY (ncomb > MAXCOMB) {
0105       edm::LogInfo("TrajectorySegmentBuilder")
0106           << " found " << measGroups.size() << " groups and more than " << static_cast<unsigned int>(MAXCOMB)
0107           << " combinations - limiting to " << (ngrp - 1) << " groups";
0108       truncate = true;
0109
0110       statCount.truncated();
0111
0112       break;
0113     }
0114   }
0115   //   cout << "Groups / combinations = " << measGroups.size() << " " << ncomb << endl;
0116   if UNLIKELY (truncate && ngrp > 0)
0117     measGroups.resize(ngrp - 1);
0118
0119 #endif
0120
0121 #ifdef DBG_TSB
0122   if (theDbgFlg) {
0123     int ntot(1);
0124     for (vector<TMG>::const_iterator ig = measGroups.begin(); ig != measGroups.end(); ++ig) {
0125       int ngrp(0);
0126       const vector<TM>& measurements = ig->measurements();
0127       for (vector<TM>::const_iterator im = measurements.begin(); im != measurements.end(); ++im) {
0128         if (im->recHit()->isValid())
0129           ngrp++;
0130       }
0131       cout << " " << ngrp;
0132       if (ngrp > 0)
0133         ntot *= ngrp;
0134     }
0135     cout << endl;
0136     cout << "TrajectorySegmentBuilder::partialTrajectories:: det ids & hit types / group" << endl;
0137     for (vector<TMG>::const_iterator ig = measGroups.begin(); ig != measGroups.end(); ++ig) {
0138       const vector<TM>& measurements = ig->measurements();
0139       for (vector<TM>::const_iterator im = measurements.begin(); im != measurements.end(); ++im) {
0140         if (im != measurements.begin())
0141           cout << " / ";
0142         if (im->recHit()->det())
0143           cout << im->recHit()->det()->geographicalId().rawId() << " " << im->recHit()->getType();
0144         else
0145           cout << "no det";
0146       }
0147       cout << endl;
0148     }
0149
0150     //   if ( measGroups.size()>4 ) {
0151     cout << typeid(theLayer).name() << endl;
0152     cout << startingState.localError().matrix() << endl;
0153     //     for (vector<TMG>::const_iterator ig=measGroups.begin();
0154     //   ig!=measGroups.end(); ig++) {
0155     //       cout << "Nr. of measurements = " << ig->measurements().size() << endl;
0156     //       const DetGroup& dg = ig->detGroup();
0157     //       for ( DetGroup::const_iterator id=dg.begin();
0158     //      id!=dg.end(); id++ ) {
0159     //  GlobalPoint p(id->det()->position());
0160     //  GlobalVector v(id->det()->toGlobal(LocalVector(0.,0.,1.)));
0161     //  cout << p.perp() << " " << p.phi() << " " << p.z() << " ; "
0162     //       << v.phi() << " " << v.z() << endl;
0163     //       }
0164     //     }
0165     //   }
0166   }
0167 #endif
0168
0169   TempTrajectoryContainer candidates = addGroup(startingTrajectory, measGroups.begin(), measGroups.end());
0170
0171   if UNLIKELY (theDbgFlg)
0172     cout << "TSB: back with " << candidates.size() << " candidates" << endl;
0173
0174   //
0175   // add invalid hit - try to get first detector hit by the extrapolation
0176   //
0177
0178   updateWithInvalidHit(startingTrajectory, measGroups, candidates);
0179
0180   if UNLIKELY (theDbgFlg)
0181     cout << "TSB: " << candidates.size() << " candidates after invalid hit" << endl;
0182
0183   statCount.incr(measGroups.size(), candidates.size(), theLockedHits.size());
0184
0185   theLockedHits.clear();
0186
0187   return candidates;
0188 }
0189
0190 void TrajectorySegmentBuilder::updateTrajectory(TempTrajectory& traj, TM tm) const {
0191   auto&& predictedState = tm.predictedState();
0192   auto&& hit = tm.recHit();
0193
0194   if (hit->isValid()) {
0195     auto&& upState = theUpdator.update(predictedState, *hit);
0196     traj.emplace(predictedState, std::move(upState), hit, tm.estimate(), tm.layer());
0197
0198     //     TrajectoryMeasurement tm(traj.lastMeasurement());
0199     //     if ( tm.updatedState().isValid() ) {
0200     //       if ( !hit.det().surface()->bounds().inside(tm.updatedState().localPosition(),
0201     //                      tm.updatedState().localError().positionError(),3.f) ) {
0202     //  cout << "Incompatibility after update for det at " << hit.det().position() << ":" << endl;
0203     //  cout << tm.predictedState().localPosition() << " "
0204     //       << tm.predictedState().localError().positionError() << endl;
0205     //  cout << hit.localPosition() << " " << hit.localPositionError() << endl;
0206     //  cout << tm.updatedState().localPosition() << " "
0207     //       << tm.updatedState().localError().positionError() << endl;
0208     //       }
0209     //     }
0210   } else {
0211     traj.emplace(predictedState, hit, 0, tm.layer());
0212   }
0213 }
0214
0215 TrajectorySegmentBuilder::TempTrajectoryContainer TrajectorySegmentBuilder::addGroup(TempTrajectory const& traj,
0216                                                                                      vector<TMG>::const_iterator begin,
0217                                                                                      vector<TMG>::const_iterator end) {
0218   vector<TempTrajectory> ret;
0219   if (begin == end) {
0220     //std::cout << "TrajectorySegmentBuilder::addGroup" << " traj.empty()=" << traj.empty() << "EMPTY" << std::endl;
0221     if UNLIKELY (theDbgFlg)
0222       cout << "TSB::addGroup : no groups left" << endl;
0223     if (!traj.empty())
0224       ret.push_back(traj);
0225     return ret;
0226   }
0227
0228   if UNLIKELY (theDbgFlg)
0229     cout << "TSB::addGroup : traj.size() = " << traj.measurements().size() << " first group at "
0230          << &(*begin)
0231          //        << " nr. of candidates = " << candidates.size()
0232          << endl;
0233
0234   TempTrajectoryContainer updatedTrajectories;
0235   updatedTrajectories.reserve(2);
0236   if (traj.measurements().empty()) {
0237     if (theMaxCand == 1) {
0238       auto&& firstMeasurements = unlockedMeasurements(begin->measurements());
0239       if (!firstMeasurements.empty())
0240         updateCandidatesWithBestHit(traj, std::move(firstMeasurements.front()), updatedTrajectories);
0241     } else {
0242       updateCandidates(traj, begin->measurements(), updatedTrajectories);
0243     }
0244     if UNLIKELY (theDbgFlg)
0245       cout << "TSB::addGroup : updating with first group - " << updatedTrajectories.size() << " trajectories" << endl;
0246   } else {
0247     auto&& meas = redoMeasurements(traj, begin->detGroup());
0248     if (!meas.empty()) {
0249       if (theBestHitOnly) {
0250         updateCandidatesWithBestHit(traj, std::move(meas.front()), updatedTrajectories);
0251       } else {
0252         updateCandidates(traj, meas, updatedTrajectories);
0253       }
0254       if UNLIKELY (theDbgFlg)
0255         cout << "TSB::addGroup : updating" << updatedTrajectories.size() << " trajectories-1" << endl;
0256     }
0257   }
0258   // keep old trajectory
0259   //
0260   updatedTrajectories.push_back(traj);
0261
0262   if (begin + 1 != end) {
0263     ret.reserve(4);  // a good upper bound
0264     for (auto const& ut : updatedTrajectories) {
0265       if UNLIKELY (theDbgFlg)
0266         cout << "TSB::addGroup : trying to extend candidate at " << &ut << " size " << ut.measurements().size() << endl;
0267       vector<TempTrajectory>&& finalTrajectories = addGroup(ut, begin + 1, end);
0268       if UNLIKELY (theDbgFlg)
0269         cout << "TSB::addGroup : " << finalTrajectories.size() << " finalised candidates before cleaning" << endl;
0270       //B.M. to be ported later
0271       // V.I. only mark invalidate
0272       cleanCandidates(finalTrajectories);
0273
0274       if UNLIKELY (theDbgFlg) {
0275         int ntf = 0;
0276         for (auto const& t : finalTrajectories)
0277           if (t.isValid())
0278             ++ntf;
0279         cout << "TSB::addGroup : got " << ntf << " finalised candidates" << endl;
0280       }
0281
0282       for (auto& t : finalTrajectories)
0283         if (t.isValid())
0284           ret.push_back(std::move(t));
0285
0286       //        ret.insert(ret.end(),make_move_iterator(finalTrajectories.begin()),
0287       //       make_move_iterator(finalTrajectories.end()));
0288     }
0289   } else {
0290     ret.reserve(updatedTrajectories.size());
0291     for (auto& t : updatedTrajectories)
0292       if (!t.empty())
0293         ret.push_back(std::move(t));
0294   }
0295
0296   //std::cout << "TrajectorySegmentBuilder::addGroup" <<
0297   //             " traj.empty()=" << traj.empty() <<
0298   //             " end-begin=" << (end-begin)  <<
0299   //             " #updated=" << updatedTrajectories.size() <<
0300   //             " #result=" << ret.size() << std::endl;
0301   return ret;
0302 }
0303
0304 void TrajectorySegmentBuilder::updateCandidates(TempTrajectory const& traj,
0305                                                 const vector<TM>& measurements,
0306                                                 TempTrajectoryContainer& candidates) {
0307   //
0308   // generate updated candidates with all valid hits
0309   //
0310   for (auto im = measurements.begin(); im != measurements.end(); ++im) {
0311     if (im->recHit()->isValid()) {
0312       candidates.push_back(traj);
0313       updateTrajectory(candidates.back(), *im);
0314       if (theLockHits)
0315         lockMeasurement(*im);
0316     }
0317   }
0318 }
0319
0320 void TrajectorySegmentBuilder::updateCandidatesWithBestHit(TempTrajectory const& traj,
0321                                                            TM measurement,
0322                                                            TempTrajectoryContainer& candidates) {
0323   // here we arrive with only valid hits and sorted.
0324   //so the best is the first!
0325
0326   if (theLockHits)
0327     lockMeasurement(measurement);
0328   candidates.push_back(traj);
0329   updateTrajectory(candidates.back(), std::move(measurement));
0330 }
0331
0332 vector<TrajectoryMeasurement> TrajectorySegmentBuilder::redoMeasurements(const TempTrajectory& traj,
0333                                                                          const DetGroup& detGroup) const {
0334   vector<TM> result;
0335   //
0336   // loop over all dets
0337   //
0338   if UNLIKELY (theDbgFlg)
0339     cout << "TSB::redoMeasurements : nr. of measurements / group =";
0340
0341   tracking::TempMeasurements tmps;
0342
0343   for (auto const& det : detGroup) {
0344     pair<bool, TrajectoryStateOnSurface> compat = GeomDetCompatibilityChecker().isCompatible(
0345         det.det(), traj.lastMeasurement().updatedState(), theGeomPropagator, theEstimator);
0346
0347     if UNLIKELY (theDbgFlg && !compat.first)
0348       std::cout << " 0";
0349
0350     if (!compat.first)
0351       continue;
0352
0353     MeasurementDetWithData mdet = theLayerMeasurements->idToDet(det.det()->geographicalId());
0354     // verify also that first (and only!) not be inactive..
0355     if (mdet.measurements(compat.second, theEstimator, tmps) && tmps.hits[0]->isValid())
0356       for (std::size_t i = 0; i != tmps.size(); ++i)
0357         result.emplace_back(compat.second, std::move(tmps.hits[i]), tmps.distances[i], &theLayer);
0358
0359     if UNLIKELY (theDbgFlg)
0360       std::cout << " " << tmps.size();
0361     tmps.clear();
0362   }
0363
0364   if UNLIKELY (theDbgFlg)
0365     cout << endl;
0366
0367   std::sort(result.begin(), result.end(), TrajMeasLessEstim());
0368
0369   return result;
0370 }
0371
0372 void TrajectorySegmentBuilder::updateWithInvalidHit(TempTrajectory& traj,
0373                                                     const vector<TMG>& groups,
0374                                                     TempTrajectoryContainer& candidates) const {
0375   //
0376   // first try to find an inactive hit with dets crossed by the prediction,
0377   //   then take any inactive hit
0378   //
0379   // loop over groups
0380   for (int iteration = 0; iteration < 2; iteration++) {
0381     for (auto const& gr : groups) {
0382       auto const& measurements = gr.measurements();
0383       for (auto im = measurements.rbegin(); im != measurements.rend(); ++im) {
0384         auto const& hit = im->recHitR();
0385         if ((hit.getType() == TrackingRecHit::valid) | (hit.getType() == TrackingRecHit::missing))
0386           continue;
0387         //
0388         // check, if the extrapolation traverses the Det or
0389         // if 2nd iteration
0390         //
0391         if (hit.det()) {
0392           auto const& predState = im->predictedState();
0393           if (iteration > 0 ||
0394               (predState.isValid() && hit.det()->surface().bounds().inside(predState.localPosition()))) {
0395             // add the hit
0396             candidates.push_back(traj);
0397             updateTrajectory(candidates.back(), *im);
0398             if UNLIKELY (theDbgFlg)
0399               cout << "TrajectorySegmentBuilder::updateWithInvalidHit "
0400                    << "added inactive hit" << endl;
0401             return;
0402           }
0403         }
0404       }
0405     }
0406   }
0407   //
0408   // No suitable inactive hit: add a missing one
0409   //
0410   for (int iteration = 0; iteration < 2; iteration++) {
0411     //
0412     // loop over groups
0413     //
0414     for (auto const& gr : groups) {
0415       auto const& measurements = gr.measurements();
0416       for (auto im = measurements.rbegin(); im != measurements.rend(); ++im) {
0417         // only use invalid hits
0418         auto const& hit = im->recHitR();
0419         if LIKELY (hit.isValid())
0420           continue;
0421
0422         // check, if the extrapolation traverses the Det
0423         auto const& predState = im->predictedState();
0424         if (iteration > 0 || (predState.isValid() && hit.surface()->bounds().inside(predState.localPosition()))) {
0425           // add invalid hit
0426           candidates.push_back(traj);
0427           updateTrajectory(candidates.back(), *im);
0428           return;
0429         }
0430       }
0431     }
0432     if UNLIKELY (theDbgFlg && iteration == 0)
0433       cout << "TrajectorySegmentBuilder::updateWithInvalidHit: "
0434            << " did not find invalid hit on 1st iteration" << endl;
0435   }
0436
0437   if UNLIKELY (theDbgFlg)
0438     cout << "TrajectorySegmentBuilder::updateWithInvalidHit: "
0439          << " did not find invalid hit" << endl;
0440 }
0441
0442 vector<TrajectoryMeasurement> TrajectorySegmentBuilder::unlockedMeasurements(const vector<TM>& measurements) const {
0443   //   if ( !theLockHits )  return measurements;
0444
0445   vector<TM> result;
0446   result.reserve(measurements.size());
0447
0448   //RecHitEqualByChannels recHitEqual(false,true);
0449
0450   for (auto const& m : measurements) {
0451     auto const& testHit = m.recHitR();
0452     if UNLIKELY (!testHit.isValid())
0453       continue;
0454     bool found(false);
0455     if LIKELY (theLockHits) {
0456       for (auto const& h : theLockedHits) {
0457         if (h->hit()->sharesInput(testHit.hit(), TrackingRecHit::all)) {
0458           found = true;
0459           break;
0460         }
0461       }
0462     }
0463     if LIKELY (!found)
0464       result.push_back(m);
0465   }
0466   return result;
0467 }
0468
0469 void TrajectorySegmentBuilder::lockMeasurement(const TM& measurement) { theLockedHits.push_back(measurement.recHit()); }
0470
0471 // ================= B.M. to be ported later ===============================
0472 void TrajectorySegmentBuilder::cleanCandidates(vector<TempTrajectory>& candidates) const {
0473   //
0474   // remove candidates which are subsets of others
0475   // assumptions: no invalid hits and no duplicates
0476   //
0477   if (candidates.size() <= 1)
0478     return;
0479   //RecHitEqualByChannels recHitEqual(false,true);
0480   //
0481   const int NC = candidates.size();
0482   int index[NC];
0483   for (int i = 0; i != NC; ++i)
0484     index[i] = i;
0485   std::sort(index, index + NC, [&candidates](int i, int j) { return lessByFoundHits(candidates[i], candidates[j]); });
0486   //   cout << "SortedCandidates.foundHits";
0487   //   for (auto i1 : index)
0488   //     cout << " " << candidates[i1].foundHits();
0489   //   cout << endl;
0490   //
0491   for (auto i1 = index; i1 != index + NC - 1; ++i1) {
0492     // get measurements of candidate to be checked
0493     const TempTrajectory::DataContainer& measurements1 = candidates[*i1].measurements();
0494     for (auto i2 = i1 + 1; i2 != index + NC; ++i2) {
0495       // no duplicates: two candidates of same size are different
0496       if (candidates[*i2].foundHits() == candidates[*i1].foundHits())
0497         continue;
0498       // get measurements of "reference"
0499       const TempTrajectory::DataContainer& measurements2 = candidates[*i2].measurements();
0500       //
0501       // use the fact that TMs are ordered:
0502       // start search in trajectory#1 from last hit match found
0503       //
0504       bool allFound(true);
0505       TempTrajectory::DataContainer::const_iterator from2 = measurements2.rbegin(), im2end = measurements2.rend();
0506       for (TempTrajectory::DataContainer::const_iterator im1 = measurements1.rbegin(), im1end = measurements1.rend();
0507            im1 != im1end;
0508            --im1) {
0509         // redundant protection - segments should not contain invalid RecHits
0510         // assert( im1->recHit()->isValid());
0511         bool found(false);
0512         for (TempTrajectory::DataContainer::const_iterator im2 = from2; im2 != im2end; --im2) {
0513           // redundant protection - segments should not contain invalid RecHits
0514           // assert (im2->recHit()->isValid());
0515           if (im1->recHitR().hit()->sharesInput(im2->recHitR().hit(), TrackingRecHit::all)) {
0516             found = true;
0517             from2 = im2;
0518             --from2;
0519             break;
0520           }
0521         }
0522         if (!found) {
0523           allFound = false;
0524           break;
0525         }
0526       }
0527       if (allFound) {
0528         candidates[*i1].invalidate();
0529         statCount.invalid();
0530       }
0531     }
0532   }
0533 }
0534
0535 //==================================================