MuonSeedGenerator/src/SETFilter.cc

0001 /** \class SETFilter
0002    I. Bloch, E. James, S. Stoynev
0003  */
0004 #include "RecoMuon/MuonSeedGenerator/interface/SETFilter.h"
0005
0006 #include "FWCore/ParameterSet/interface/ParameterSet.h"
0007
0008 // FIXME: remove this
0009 #include "FWCore/Framework/interface/Event.h"
0010
0011 #include "TrackingTools/KalmanUpdators/interface/Chi2MeasurementEstimator.h"
0012
0013 #include "TrackingTools/TrajectoryState/interface/FreeTrajectoryState.h"
0014 #include "TrackingTools/PatternTools/interface/TrajectoryMeasurement.h"
0015 #include "TrackingTools/DetLayers/interface/DetLayer.h"
0016
0017 #include "TrackingTools/Records/interface/TrackingComponentsRecord.h"
0018 #include "TrackingTools/TrackFitters/interface/TrajectoryFitter.h"
0019
0020 #include "FWCore/MessageLogger/interface/MessageLogger.h"
0021 #include "FWCore/Utilities/interface/Exception.h"
0022
0023 #include "DataFormats/Math/interface/Matrix.h"
0024
0025 #include <vector>
0026
0027 #include <iostream>
0028 #include <fstream>
0029
0030 // there is an existing sorter somewhere in the CMSSW code (I think) - delete that
0031 struct sorter {
0032   //bigger first!
0033   sorter() {}
0034   bool operator()(TransientTrackingRecHit::ConstRecHitPointer hit_1,
0035                   TransientTrackingRecHit::ConstRecHitPointer hit_2) const {
0036     if (hit_1->det()->subDetector() != GeomDetEnumerators::CSC ||
0037         hit_2->det()->subDetector() != GeomDetEnumerators::CSC) {
0038       // this is a piculiar "fix" for CSCs
0039       return (hit_1->globalPosition().mag2() > hit_2->globalPosition().mag2());
0040     } else {
0041       return (fabs(hit_1->globalPosition().z()) > fabs(hit_2->globalPosition().z()));
0042     }
0043   }
0044 } const sortRadius;  // bigger first
0045
0046 using namespace edm;
0047 using namespace std;
0048
0049 SETFilter::SETFilter(const ParameterSet &par,
0050                      const MuonServiceProxy *service)
0051     : theService(service)  //,
0052                            //theOverlappingChambersFlag(true)
0053 {
0054   thePropagatorName = par.getParameter<string>("Propagator");
0055   useSegmentsInTrajectory = par.getUntrackedParameter<bool>("UseSegmentsInTrajectory");
0056 }
0057
0058 SETFilter::~SETFilter() { LogTrace("Muon|RecoMuon|SETFilter") << "SETFilter destructor called" << endl; }
0059
0060 void SETFilter::setEvent(const Event &event) {}
0061
0062 void SETFilter::reset() {
0063   totalChambers = dtChambers = cscChambers = rpcChambers = 0;
0064
0065   theLastUpdatedTSOS = theLastButOneUpdatedTSOS = TrajectoryStateOnSurface();
0066
0067   theDetLayers.clear();
0068 }
0069
0070 const Propagator *SETFilter::propagator() const { return &*theService->propagator(thePropagatorName); }
0071
0072 void SETFilter::incrementChamberCounters(const DetLayer *layer) {
0073   if (layer->subDetector() == GeomDetEnumerators::DT)
0074     dtChambers++;
0075   else if (layer->subDetector() == GeomDetEnumerators::CSC)
0076     cscChambers++;
0077   else if (layer->subDetector() == GeomDetEnumerators::RPCBarrel ||
0078            layer->subDetector() == GeomDetEnumerators::RPCEndcap)
0079     rpcChambers++;
0080   else
0081     LogError("Muon|RecoMuon|SETFilter") << "Unrecognized module type in incrementChamberCounters";
0082   // FIXME:
0083   //   << layer->module() << " " <<layer->Part() << endl;
0084
0085   totalChambers++;
0086 }
0087
0088 //---- the SET FW-fitter within a cluster
0089 bool SETFilter::fwfit_SET(std::vector<SeedCandidate> &validSegmentsSet_in,
0090                           std::vector<SeedCandidate> &validSegmentsSet_out) {
0091   // this is the SET algorithm fit
0092   validSegmentsSet_out.clear();
0093
0094   //---- It is supposed to be called within a loop over "segment clusters";
0095   //---- so std::vector < SeedCandidate> consists of "valid" combinations (sets) within a "cluster"
0096   //---- "seed" above has nothing to do with the "Seed" in the STA code
0097
0098   // a trajectory is not really build but a TSOS is build and is checked (below)
0099   bool validStep = true;
0100   std::vector<double> chi2AllCombinations(validSegmentsSet_in.size());
0101   std::vector<TrajectoryStateOnSurface> lastUpdatedTSOS_Vect(validSegmentsSet_in.size());
0102   // loop over all valid sets
0103   for (unsigned int iSet = 0; iSet < validSegmentsSet_in.size(); ++iSet) {
0104     //std::cout<<"   iSET = "<<iSet<<std::endl;
0105     //---- start fit from the origin
0106     CLHEP::Hep3Vector origin(0., 0., 0.);
0107     Trajectory::DataContainer trajectoryMeasurementsInTheSet_tmp;
0108     //---- Find minimum chi2 (corresponding to a specific 3D-momentum)
0109     chi2AllCombinations[iSet] =
0110         findMinChi2(iSet, origin, validSegmentsSet_in[iSet], lastUpdatedTSOS_Vect, trajectoryMeasurementsInTheSet_tmp);
0111   }
0112   //---- Find the best muon candidate (min chi2) in the cluster; find more candidates?
0113   std::vector<double>::iterator itMin = min_element(chi2AllCombinations.begin(), chi2AllCombinations.end());
0114
0115   int positionMin = itMin - chi2AllCombinations.begin();
0116
0117   // "the best" set; have to find reasonable conditions to include more than one set
0118   validSegmentsSet_out.push_back(validSegmentsSet_in[positionMin]);
0119
0120   return validStep;
0121 }
0122
0123 //---- the SET FW-fitter
0124 bool SETFilter::buildTrajectoryMeasurements(SeedCandidate *finalMuon, Trajectory::DataContainer &finalCandidate) {
0125   // this is the SET algorithm fit
0126   bool validTrajectory = true;
0127   // reset the fitter
0128   reset();  // the layer counters
0129   finalCandidate.clear();
0130
0131   //---- Check if (only last?) TSOS is valid and build a trajectory (for the backward filter)
0132
0133   if (!finalMuon->trajectoryMeasurementsInTheSet.empty() &&
0134       finalMuon->trajectoryMeasurementsInTheSet.back().forwardPredictedState().isValid()) {
0135     // loop over all measurements in the set
0136     for (unsigned int iMeas = 0; iMeas < finalMuon->trajectoryMeasurementsInTheSet.size(); ++iMeas) {
0137       // strore the measurements
0138       finalCandidate.push_back(finalMuon->trajectoryMeasurementsInTheSet[iMeas]);
0139       const DetLayer *layer = finalMuon->trajectoryMeasurementsInTheSet[iMeas].layer();
0140
0141       incrementChamberCounters(layer);
0142
0143       theDetLayers.push_back(layer);
0144     }
0145     theLastUpdatedTSOS =
0146         finalMuon->trajectoryMeasurementsInTheSet.at(finalMuon->trajectoryMeasurementsInTheSet.size() - 1)
0147             .forwardPredictedState();
0148     //std::cout<<"  THE OUTPUT FROM FW FILTER: |P| = "<<finalMuon->momentum.mag()<<
0149     //" theta = "<<finalMuon->momentum.theta()<<" phi = "<<finalMuon->momentum.phi()<<std::endl;
0150   } else {
0151     validTrajectory = false;
0152     //std::cout<<" TSOS not valid; no trajectory build"<<std::endl;
0153   }
0154   return validTrajectory;
0155 }
0156
0157 //
0158 bool SETFilter::transform(Trajectory::DataContainer &measurements_segments,
0159                           TransientTrackingRecHit::ConstRecHitContainer &hitContainer,
0160                           TrajectoryStateOnSurface &firstTSOS) {
0161   // transforms "segment trajectory" to "rechit container"
0162   //sort(measurements_segments.begin(),measurements_segments.end(),sortRadius);
0163   bool success = true;
0164   // loop over all segments in the trajectory
0165   for (int iMeas = measurements_segments.size() - 1; iMeas > -1; --iMeas) {
0166     TransientTrackingRecHit ::ConstRecHitContainer sortedHits;
0167     // loop over the rechits contained in the segments
0168     for (unsigned int jMeas = 0; jMeas < measurements_segments[iMeas].recHit()->transientHits().size(); ++jMeas) {
0169       if (measurements_segments[iMeas].recHit()->transientHits().at(jMeas)->transientHits().size() > 1) {
0170         // loop over the rechits contained in the rechits contained in the segments (OK, OK - this is for DT only;
0171         // the convention there is a bit different from the CSCs)
0172         for (unsigned int kMeas = 0;
0173              kMeas < measurements_segments[iMeas].recHit()->transientHits().at(jMeas)->transientHits().size();
0174              ++kMeas) {
0175           sortedHits.push_back(
0176               measurements_segments[iMeas].recHit()->transientHits().at(jMeas)->transientHits().at(kMeas));
0177         }
0178       } else {
0179         sortedHits = measurements_segments[iMeas].recHit()->transientHits();
0180       }
0181     }
0182     // sort the rechits by radius (or z) and put them in a container
0183     sort(sortedHits.begin(), sortedHits.end(), sortRadius);
0184     hitContainer.insert(hitContainer.end(), sortedHits.begin(), sortedHits.end());
0185   }
0186
0187   // this is the last segment state
0188   FreeTrajectoryState ftsStart =
0189       *(measurements_segments.at(measurements_segments.size() - 1).forwardPredictedState().freeState());
0190
0191   // this is the last (from the IP) rechit
0192   TransientTrackingRecHit::ConstRecHitPointer muonRecHit = hitContainer[0];
0193   DetId detId_last = hitContainer[0]->hit()->geographicalId();
0194   const GeomDet *layer_last = theService->trackingGeometry()->idToDet(detId_last);
0195
0196   // get the last rechit TSOS
0197   TrajectoryStateOnSurface tSOSDest = propagator()->propagate(ftsStart, layer_last->surface());
0198   firstTSOS = tSOSDest;
0199   // ftsStart should be at the last rechit surface
0200   if (!tSOSDest.isValid()) {
0201     success = false;
0202     //     ftsStart = *tSOSDest.freeState();
0203   }
0204   return success;
0205 }
0206
0207 bool SETFilter::transformLight(Trajectory::DataContainer &measurements_segments,
0208                                TransientTrackingRecHit::ConstRecHitContainer &hitContainer,
0209                                TrajectoryStateOnSurface &firstTSOS) {
0210   // transforms "segment trajectory" to "segment container"
0211
0212   bool success = true;
0213   // loop over all segments in the trajectory
0214   if (useSegmentsInTrajectory) {  // if segments the "backword fit" (rechits)
0215                                   // performed later is actually a forward one (?!)
0216     for (unsigned int iMeas = 0; iMeas < measurements_segments.size(); ++iMeas) {
0217       hitContainer.push_back(measurements_segments[iMeas].recHit());
0218     }
0219   } else {
0220     for (int iMeas = measurements_segments.size() - 1; iMeas > -1; --iMeas) {
0221       hitContainer.push_back(measurements_segments[iMeas].recHit());
0222     }
0223   }
0224   // this is the last segment state
0225   firstTSOS = measurements_segments.at(0).forwardPredictedState();
0226   return success;
0227 }
0228
0229 double SETFilter::findChi2(double pX,
0230                            double pY,
0231                            double pZ,
0232                            const CLHEP::Hep3Vector &r3T,
0233                            SeedCandidate &muonCandidate,
0234                            TrajectoryStateOnSurface &lastUpdatedTSOS,
0235                            Trajectory::DataContainer &trajectoryMeasurementsInTheSet,
0236                            bool detailedOutput) {
0237   //---- actual chi2 calculations; only the measurement error is taken into accout!
0238   //---- chi2 is to compare an extrapolated point to various measurements so
0239   //---- the extrapolation error is not an issue (is it?)
0240
0241   if (detailedOutput) {
0242     trajectoryMeasurementsInTheSet.clear();
0243   }
0244
0245   int charge = muonCandidate.charge;
0246   GlobalVector p3GV(pX, pY, pZ);
0247   GlobalPoint r3GP(r3T.x(), r3T.y(), r3T.z());
0248   //---- how to disable error propagation?
0249   // VI: just not set it!
0250   FreeTrajectoryState ftsStart(r3GP, p3GV, charge, &*(theService->magneticField()));
0251   // VI let's be backward compatible...
0252   if (detailedOutput) {
0253     AlgebraicSymMatrix55 cov;
0254     cov *= 1e-20;
0255     ftsStart.setCurvilinearError(cov);
0256   }
0257   TrajectoryStateOnSurface tSOSDest;
0258
0259   double chi2_loc = 0.;
0260   for (unsigned int iMeas = 0; iMeas < muonCandidate.theSet.size(); ++iMeas) {
0261     MuonTransientTrackingRecHit::MuonRecHitPointer muonRecHit = muonCandidate.theSet[iMeas];
0262     DetId detId = muonRecHit->hit()->geographicalId();
0263     const GeomDet *layer = theService->trackingGeometry()->idToDet(detId);
0264
0265     //---- propagate the muon starting from position r3T and momentum p3T
0266
0267     //    bool radX0CorrectionMode_ = false; // not needed here?
0268     //if (radX0CorrectionMode_ ){
0269     //} else {
0270
0271     tSOSDest = propagator()->propagate(ftsStart, layer->surface());
0272     lastUpdatedTSOS = tSOSDest;
0273     if (tSOSDest.isValid()) {
0274       //---- start next step ("adding" measurement) from the last TSOS
0275       ftsStart = *tSOSDest.freeState();
0276     } else {
0277       //std::cout<<"... not valid TSOS"<<std::endl;
0278       chi2_loc = 9999999999.;
0279       break;
0280     }
0281     //}
0282
0283     LocalPoint locHitPos = muonRecHit->localPosition();
0284     LocalError locHitErr = muonRecHit->localPositionError();
0285     const GlobalPoint globPropPos = ftsStart.position();
0286     LocalPoint locPropPos = layer->toLocal(globPropPos);
0287
0288     //
0289     //---- chi2 calculated in local system; correlation taken into accont
0290     CLHEP::HepMatrix dist(1, 2);  //, distT(2,1);
0291     double chi2_intermed = -9;
0292     int ierr = 0;
0293     dist(1, 1) = locPropPos.x() - locHitPos.x();
0294     dist(1, 2) = locPropPos.y() - locHitPos.y();
0295     CLHEP::HepMatrix IC(2, 2);
0296     IC(1, 1) = locHitErr.xx();
0297     IC(2, 1) = locHitErr.xy();
0298     IC(2, 2) = locHitErr.yy();
0299     IC(1, 2) = IC(2, 1);
0300
0301     //---- Special care is needed for "1-dim measurements" (RPCs, outer DT(?))
0302     if (4 != muonRecHit->hit()->dimension()) {
0303       for (int iE = 1; iE < 3; ++iE) {
0304         // this is bellow is a DT fix; hopefully it will not be needed
0305         if (fabs(IC(iE, iE)) < 0.00000001) {
0306           IC(iE, iE) = 62500. / 12.;  // error squared - ( 250 cm /sqrt(12) )^2; large
0307         }
0308       }
0309     }
0310     //---- Invert covariance matrix
0311     IC.invert(ierr);
0312     //if (ierr != 0) {
0313     //std::cout << "failed to invert covariance matrix (2x2) =\n" << IC << std::endl;;
0314     //}
0315     chi2_intermed =
0316         pow(dist(1, 1), 2) * IC(1, 1) + 2. * dist(1, 1) * dist(1, 2) * IC(1, 2) + pow(dist(1, 2), 2) * IC(2, 2);
0317     if (chi2_intermed < 0) {  // should we check?
0318       chi2_intermed = 9999999999.;
0319     }
0320     chi2_loc += chi2_intermed;
0321
0322     // that's for the last call; we don't need to construct a TrajectoryMeasurement at every chi2 step
0323     if (detailedOutput) {
0324       DetId detId = muonRecHit->hit()->geographicalId();
0325       const DetLayer *layer = theService->detLayerGeometry()->idToLayer(detId);
0326       //std::cout<<"    seg pos in traj : "<<lastUpdatedTSOS.globalPosition()<<std::endl;
0327       // put the measurement into the set
0328       // VI set the error as the fit needs it... (it is nonsense anyhow...)
0329       // (do it on the tsos)
0330       /*
0331       if (!lastUpdatedTSOS.hasError()){
0332     AlgebraicSymMatrix55 cov; cov*=1e6;
0333     lastUpdatedTSOS.freeTrajectoryState().setCurvilinearError(cov);
0334       }
0335       */
0336       trajectoryMeasurementsInTheSet.push_back(
0337           TrajectoryMeasurement(lastUpdatedTSOS, muonRecHit, chi2_intermed, layer));
0338     }
0339   }
0340   return chi2_loc;
0341 }
0342
0343 double SETFilter::findMinChi2(unsigned int iSet,
0344                               const CLHEP::Hep3Vector &r3T,
0345                               SeedCandidate &muonCandidate,
0346                               std::vector<TrajectoryStateOnSurface> &lastUpdatedTSOS_Vect,  // delete
0347                               Trajectory::DataContainer &trajectoryMeasurementsInTheSet) {
0348   // a chi2 minimization procedure
0349
0350   //---- Which three variables to use?
0351   //---- (1/|P|, theta, phi) ? in that case many sin() and cos() operations :-/
0352   //---- maybe vary directly sin() and cos()?
0353   bool detailedOutput = false;
0354
0355   double cosTheta = muonCandidate.momentum.cosTheta();
0356   double theta = acos(cosTheta);
0357   double phi = muonCandidate.momentum.phi();
0358   double pMag = muonCandidate.momentum.mag();
0359
0360   double minChi2 = -999.;
0361   TrajectoryStateOnSurface lastUpdatedTSOS;
0362
0363   //---- Fit Parameters
0364
0365   if (pMag < 5.) {  // hardcoded - remove it! same in SETSeedFinder
0366     pMag = 5.;      // GeV
0367   }
0368   //---- This offset helps the minimization to go faster (in the specific case)
0369   pMag *= 1.2;
0370   double invP = 1. / pMag;
0371   //std::cout<<"    INIT pMag = "<<pMag<<" invP = "<<invP<<" theta = "<<theta<<" phi = "<<phi<<std::endl;
0372
0373   //---- apply downhill SIMPLEX minimization (also "amoeba" method; thus the "feet" below are  amoeba's feet)
0374
0375   //std::cout<<"    SIMPLEX minimization"<<std::endl;
0376   //---- parameters ; the should be hardcoded
0377   const double reflect = 1;
0378   const double expand = -0.5;
0379   const double contract = 0.25;
0380
0381   const int nDim = 3;  // invP, theta, phi
0382   //---- Now choose nDim + 1 points
0383   std::vector<CLHEP::Hep3Vector> feet(nDim + 1);
0384   std::vector<double> chi2Feet(nDim + 1);
0385   std::vector<TrajectoryStateOnSurface *> lastUpdatedTSOS_pointer(nDim + 1);  // probably not needed; to be deleted
0386
0387   //---- The minimization procedure strats from these nDim+1 points (feet)
0388
0389   CLHEP::Hep3Vector foot1(
0390       invP, theta, phi);  // well obviuosly it is not a real Hep3Vector; better change it to a simple vector
0391   feet[0] = foot1;
0392   chi2Feet[0] =
0393       chi2AtSpecificStep(feet[0], r3T, muonCandidate, lastUpdatedTSOS, trajectoryMeasurementsInTheSet, detailedOutput);
0394   lastUpdatedTSOS_pointer[0] = &lastUpdatedTSOS;
0395
0396   std::vector<CLHEP::Hep3Vector> morePoints = find3MoreStartingPoints(feet[0], r3T, muonCandidate);
0397   feet[1] = morePoints[0];
0398   feet[2] = morePoints[1];
0399   feet[3] = morePoints[2];
0400
0401   //--- SIMPLEX initial step(s)
0402   for (unsigned int iFoot = 1; iFoot < feet.size(); ++iFoot) {
0403     chi2Feet[iFoot] = chi2AtSpecificStep(
0404         feet[iFoot], r3T, muonCandidate, lastUpdatedTSOS, trajectoryMeasurementsInTheSet, detailedOutput);
0405     lastUpdatedTSOS_pointer[iFoot] = &lastUpdatedTSOS;
0406   }
0407
0408   unsigned int high, second_high, low;
0409   //const unsigned int iterations = 1;//---- to be replaced by something better
0410   int iCalls = 0;
0411   //for(unsigned int iIt = 0;iIt<iterations;++iIt){
0412   while (iCalls < 3.) {
0413     //---- SIMPLEX step 1
0414     pickElements(chi2Feet, high, second_high, low);
0415     ++iCalls;
0416     feet[high] = reflectFoot(feet, high, reflect);
0417     chi2Feet[high] = chi2AtSpecificStep(
0418         feet[high], r3T, muonCandidate, lastUpdatedTSOS, trajectoryMeasurementsInTheSet, detailedOutput);
0419     lastUpdatedTSOS_pointer[high] = &lastUpdatedTSOS;
0420     //---- SIMPLEX step 2.1
0421     if (chi2Feet[high] < chi2Feet[low]) {
0422       ++iCalls;
0423       feet[high] = reflectFoot(feet, high, expand);
0424       chi2Feet[high] = chi2AtSpecificStep(
0425           feet[high], r3T, muonCandidate, lastUpdatedTSOS, trajectoryMeasurementsInTheSet, detailedOutput);
0426       lastUpdatedTSOS_pointer[high] = &lastUpdatedTSOS;
0427     }
0428     //---- SIMPLEX step 2.2
0429     else if (chi2Feet[high] > chi2Feet[second_high]) {
0430       double worstChi2 = chi2Feet[high];
0431       ++iCalls;
0432       feet[high] = reflectFoot(feet, high, contract);
0433       chi2Feet[high] = chi2AtSpecificStep(
0434           feet[high], r3T, muonCandidate, lastUpdatedTSOS, trajectoryMeasurementsInTheSet, detailedOutput);
0435       lastUpdatedTSOS_pointer[high] = &lastUpdatedTSOS;
0436       //---- SIMPLEX step 2.2.1
0437       if (chi2Feet[high] < worstChi2) {
0438         nDimContract(feet, low);
0439         for (unsigned int iFoot = 0; iFoot < feet.size(); ++iFoot) {
0440           ++iCalls;
0441           chi2Feet[iFoot] = chi2AtSpecificStep(
0442               feet[iFoot], r3T, muonCandidate, lastUpdatedTSOS, trajectoryMeasurementsInTheSet, detailedOutput);
0443           lastUpdatedTSOS_pointer[iFoot] = &lastUpdatedTSOS;
0444         }
0445         --iCalls;  // one of the above is not changed
0446       }
0447     }
0448   }
0449   //---- Here the SIMPLEX minimization ends
0450
0451   // this is the minimum found
0452   int bestFitElement = min_element(chi2Feet.begin(), chi2Feet.end()) - chi2Feet.begin();
0453
0454   //---- repeat to get the trajectoryMeasurementsInTheSet (optimize?)
0455   detailedOutput = true;
0456   chi2Feet[bestFitElement] = chi2AtSpecificStep(
0457       feet[bestFitElement], r3T, muonCandidate, lastUpdatedTSOS, trajectoryMeasurementsInTheSet, detailedOutput);
0458   minChi2 = chi2Feet[bestFitElement];
0459
0460   double pMag_updated = 1. / feet[bestFitElement].x();
0461   double sin_theta = sin(feet[bestFitElement].y());
0462   double cos_theta = cos(feet[bestFitElement].y());
0463   double sin_phi = sin(feet[bestFitElement].z());
0464   double cos_phi = cos(feet[bestFitElement].z());
0465
0466   double best_pX = pMag_updated * sin_theta * cos_phi;
0467   double best_pY = pMag_updated * sin_theta * sin_phi;
0468   double best_pZ = pMag_updated * cos_theta;
0469   CLHEP::Hep3Vector bestP(best_pX, best_pY, best_pZ);
0470   //---- update the best momentum estimate
0471   //if(minChi2<999999. && pMag_updated>0.){//fit failed?! check
0472   muonCandidate.momentum = bestP;
0473   //}
0474   //---- update the trajectory
0475   muonCandidate.trajectoryMeasurementsInTheSet = trajectoryMeasurementsInTheSet;
0476   // do we need that?
0477   lastUpdatedTSOS_Vect[iSet] = *(lastUpdatedTSOS_pointer[bestFitElement]);
0478
0479   //std::cout<<"   FINAL:  P = "<<muonCandidate.momentum.mag()<<" theta = "<<muonCandidate.momentum.theta()<<
0480   //" phi = "<<muonCandidate.momentum.phi()<<"   chi = "<<chi2Feet[bestFitElement]<<std::endl;
0481   return minChi2;
0482 }
0483
0484 double SETFilter::chi2AtSpecificStep(CLHEP::Hep3Vector &foot,
0485                                      const CLHEP::Hep3Vector &r3T,
0486                                      SeedCandidate &muonCandidate,
0487                                      TrajectoryStateOnSurface &lastUpdatedTSOS,
0488                                      Trajectory::DataContainer &trajectoryMeasurementsInTheSet,
0489                                      bool detailedOutput) {
0490   // specific input parameters - find chi2
0491   double chi2 = 999999999999.;
0492   if (foot.x() > 0) {  // this is |P|; maybe return a flag too?
0493     double pMag_updated = 1. / foot.x();
0494     double sin_theta = sin(foot.y());
0495     double cos_theta = cos(foot.y());
0496     double sin_phi = sin(foot.z());
0497     double cos_phi = cos(foot.z());
0498
0499     double pX = pMag_updated * sin_theta * cos_phi;
0500     double pY = pMag_updated * sin_theta * sin_phi;
0501     double pZ = pMag_updated * cos_theta;
0502     chi2 = findChi2(pX, pY, pZ, r3T, muonCandidate, lastUpdatedTSOS, trajectoryMeasurementsInTheSet, detailedOutput);
0503   }
0504   return chi2;
0505 }
0506
0507 std::vector<CLHEP::Hep3Vector> SETFilter::find3MoreStartingPoints(CLHEP::Hep3Vector &key_foot,
0508                                                                   const CLHEP::Hep3Vector &r3T,
0509                                                                   SeedCandidate &muonCandidate) {
0510   // SIMPLEX uses nDim + 1 starting points;
0511   // so here we need 3 more (one we already have)
0512   std::vector<CLHEP::Hep3Vector> morePoints;  // again - CLHEP::Hep3Vector is not a good choice here
0513   double invP = key_foot.x();
0514   double theta = key_foot.y();
0515   double phi = key_foot.z();
0516
0517   double deltaPhi_init = 0.005;
0518   double deltaTheta_init = 0.005;
0519   //double deltaInvP_init = 1.1 * invP;
0520   double deltaInvP_init = 0.1 * invP;
0521   //deltaInvP_init = 0.5 * invP;
0522
0523   // try to chose better point to start with
0524   bool optimized = true;
0525   if (optimized) {
0526     //---- Find a minimum chi2 for every variable (others are fixed) by supposing chi2 is a parabola
0527     //---- Then these points ("minima") are probably better starting points for the real minimization
0528
0529     TrajectoryStateOnSurface lastUpdatedTSOS;                  // fake here
0530     Trajectory::DataContainer trajectoryMeasurementsInTheSet;  // fake here
0531     bool detailedOutput = false;                               // fake here
0532
0533     std::vector<double> pInv_init(3);
0534     std::vector<double> theta_init(3);
0535     std::vector<double> phi_init(3);
0536     std::vector<double> chi2_init(3);
0537
0538     pInv_init[0] = invP - deltaInvP_init;
0539     pInv_init[1] = invP;
0540     pInv_init[2] = invP + deltaInvP_init;
0541     //pInv_init[2] = invP +  0.1 * invP;
0542
0543     theta_init[0] = theta - deltaTheta_init;
0544     theta_init[1] = theta;
0545     theta_init[2] = theta + deltaTheta_init;
0546
0547     phi_init[0] = phi - deltaPhi_init;
0548     phi_init[1] = phi;
0549     phi_init[2] = phi + deltaPhi_init;
0550
0551     double sin_theta_nom = sin(theta_init[1]);
0552     double cos_theta_nom = cos(theta_init[1]);
0553     double sin_phi_nom = sin(phi_init[1]);
0554     double cos_phi_nom = cos(phi_init[1]);
0555     double pMag_updated_nom = 1. / pInv_init[1];
0556
0557     //--- invP
0558     for (int i = 0; i < 3; ++i) {
0559       double pMag_updated = 1. / pInv_init[i];
0560       double pX = pMag_updated * sin_theta_nom * cos_phi_nom;
0561       double pY = pMag_updated * sin_theta_nom * sin_phi_nom;
0562       double pZ = pMag_updated * cos_theta_nom;
0563       chi2_init[i] =
0564           findChi2(pX, pY, pZ, r3T, muonCandidate, lastUpdatedTSOS, trajectoryMeasurementsInTheSet, detailedOutput);
0565     }
0566     std::pair<double, double> result_pInv = findParabolaMinimum(pInv_init, chi2_init);
0567
0568     //---- theta
0569     for (int i = 0; i < 3; ++i) {
0570       double sin_theta = sin(theta_init[i]);
0571       double cos_theta = cos(theta_init[i]);
0572       double pX = pMag_updated_nom * sin_theta * cos_phi_nom;
0573       double pY = pMag_updated_nom * sin_theta * sin_phi_nom;
0574       double pZ = pMag_updated_nom * cos_theta;
0575       chi2_init[i] =
0576           findChi2(pX, pY, pZ, r3T, muonCandidate, lastUpdatedTSOS, trajectoryMeasurementsInTheSet, detailedOutput);
0577     }
0578     std::pair<double, double> result_theta = findParabolaMinimum(theta_init, chi2_init);
0579
0580     //---- phi
0581     for (int i = 0; i < 3; ++i) {
0582       double sin_phi = sin(phi_init[i]);
0583       double cos_phi = cos(phi_init[i]);
0584       double pX = pMag_updated_nom * sin_theta_nom * cos_phi;
0585       double pY = pMag_updated_nom * sin_theta_nom * sin_phi;
0586       double pZ = pMag_updated_nom * cos_theta_nom;
0587       chi2_init[i] =
0588           findChi2(pX, pY, pZ, r3T, muonCandidate, lastUpdatedTSOS, trajectoryMeasurementsInTheSet, detailedOutput);
0589     }
0590     std::pair<double, double> result_phi = findParabolaMinimum(phi_init, chi2_init);
0591     // should we use that?
0592     /* not used, keep it in case some interest comes back
0593     double newPhi = result_phi.first;
0594     if(fabs(newPhi - phi)<0.02){// too close?
0595       newPhi = phi + 0.02;
0596     }
0597     */
0598     CLHEP::Hep3Vector foot2(invP, theta, result_phi.first);
0599     CLHEP::Hep3Vector foot3(invP, result_theta.first, phi);
0600     /* not used, keep it in case some interest comes back
0601     double newInvP = result_pInv.first;
0602     if(fabs(newInvP - invP)<0.001){//too close
0603       newInvP = invP + 0.001;
0604     }
0605     */
0606     CLHEP::Hep3Vector foot4(result_pInv.first, theta, phi);
0607     morePoints.push_back(foot2);
0608     morePoints.push_back(foot3);
0609     morePoints.push_back(foot4);
0610   } else {
0611     // the points
0612     CLHEP::Hep3Vector foot2(invP, theta, phi - deltaPhi_init);
0613     CLHEP::Hep3Vector foot3(invP, theta - deltaTheta_init, phi);
0614     CLHEP::Hep3Vector foot4(invP - deltaInvP_init, theta, phi);
0615     morePoints.push_back(foot2);
0616     morePoints.push_back(foot3);
0617     morePoints.push_back(foot4);
0618   }
0619   return morePoints;
0620 }
0621
0622 std::pair<double, double> SETFilter::findParabolaMinimum(std::vector<double> &quadratic_var,
0623                                                          std::vector<double> &quadratic_chi2) {
0624   // quadratic equation minimization
0625
0626   double paramAtMin = -99.;
0627   std::vector<double> quadratic_param(3);
0628
0629   math::Matrix<3, 3>::type denominator;
0630   math::Matrix<3, 3>::type enumerator_1;
0631   math::Matrix<3, 3>::type enumerator_2;
0632   math::Matrix<3, 3>::type enumerator_3;
0633
0634   for (int iCol = 0; iCol < 3; ++iCol) {
0635     denominator(0, iCol) = 1;
0636     denominator(1, iCol) = quadratic_var.at(iCol);
0637     denominator(2, iCol) = pow(quadratic_var.at(iCol), 2);
0638
0639     enumerator_1(0, iCol) = quadratic_chi2.at(iCol);
0640     enumerator_1(1, iCol) = denominator(1, iCol);
0641     enumerator_1(2, iCol) = denominator(2, iCol);
0642
0643     enumerator_2(0, iCol) = denominator(0, iCol);
0644     enumerator_2(1, iCol) = quadratic_chi2.at(iCol);
0645     enumerator_2(2, iCol) = denominator(2, iCol);
0646
0647     enumerator_3(0, iCol) = denominator(0, iCol);
0648     enumerator_3(1, iCol) = denominator(1, iCol);
0649     enumerator_3(2, iCol) = quadratic_chi2.at(iCol);
0650   }
0651   const double mult = 5.;  // "save" accuracy"; result is independent on "mult"
0652   denominator *= mult;
0653   enumerator_1 *= mult;
0654   enumerator_2 *= mult;
0655   enumerator_3 *= mult;
0656
0657   std::vector<math::Matrix<3, 3>::type> enumerator;
0658   enumerator.push_back(enumerator_1);
0659   enumerator.push_back(enumerator_2);
0660   enumerator.push_back(enumerator_3);
0661
0662   double determinant = 0;
0663   denominator.Det2(determinant);
0664   if (fabs(determinant) > 0.00000000001) {
0665     for (int iPar = 0; iPar < 3; ++iPar) {
0666       double d = 0.;
0667       enumerator.at(iPar).Det2(d);
0668       quadratic_param.at(iPar) = d / determinant;
0669     }
0670   } else {
0671     //std::cout<<" determinant 0? Check the code..."<<std::endl;
0672   }
0673   if (quadratic_param.at(2) > 0) {
0674     paramAtMin = -quadratic_param.at(1) / quadratic_param.at(2) / 2;
0675   } else {
0676     //std::cout<<" parabola has a maximum or division by zero... Using initial value. "<<std::endl;
0677     paramAtMin = quadratic_var.at(1);
0678   }
0679   double chi2_min =
0680       quadratic_param.at(0) + quadratic_param.at(1) * paramAtMin + quadratic_param.at(2) * pow(paramAtMin, 2);
0681   std::pair<double, double> result;
0682   result = std::make_pair(paramAtMin, chi2_min);
0683   return result;
0684 }
0685
0686 void SETFilter::pickElements(std::vector<double> &chi2Feet,
0687                              unsigned int &high,
0688                              unsigned int &second_high,
0689                              unsigned int &low) {
0690   // a SIMPLEX function
0691   std::vector<double> chi2Feet_tmp = chi2Feet;
0692   std::vector<double>::iterator minEl = min_element(chi2Feet.begin(), chi2Feet.end());
0693   std::vector<double>::iterator maxEl = max_element(chi2Feet.begin(), chi2Feet.end());
0694   high = maxEl - chi2Feet.begin();
0695   low = minEl - chi2Feet.begin();
0696   chi2Feet_tmp[high] = chi2Feet_tmp[low];
0697   std::vector<double>::iterator second_maxEl = max_element(chi2Feet_tmp.begin(), chi2Feet_tmp.end());
0698   second_high = second_maxEl - chi2Feet_tmp.begin();
0699
0700   return;
0701 }
0702
0703 CLHEP::Hep3Vector SETFilter::reflectFoot(std::vector<CLHEP::Hep3Vector> &feet, unsigned int key_foot, double scale) {
0704   // a SIMPLEX function
0705   CLHEP::Hep3Vector newPosition(0., 0., 0.);
0706   if (scale == 0.5) {
0707     //std::cout<<" STA muon: scale parameter for simplex method incorrect : "<<scale<<std::endl;
0708     return newPosition;
0709   }
0710   CLHEP::Hep3Vector centroid(0, 0, 0);
0711   for (unsigned int iFoot = 0; iFoot < feet.size(); ++iFoot) {
0712     if (iFoot == key_foot) {
0713       continue;
0714     }
0715     centroid += feet[iFoot];
0716   }
0717   centroid /= (feet.size() - 1);
0718   CLHEP::Hep3Vector displacement = 2. * (centroid - feet[key_foot]);
0719   newPosition = feet[key_foot] + scale * displacement;
0720   return newPosition;
0721 }
0722
0723 void SETFilter::nDimContract(std::vector<CLHEP::Hep3Vector> &feet, unsigned int low) {
0724   for (unsigned int iFoot = 0; iFoot < feet.size(); ++iFoot) {
0725     // a SIMPLEX function
0726     if (iFoot == low) {
0727       continue;
0728     }
0729     feet[iFoot] += feet[low];
0730     feet[iFoot] /= 2.;
0731   }
0732   return;
0733 }