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