Back to home page

sPhenix code displayed by LXR
 
 

    

File indexing completed on 2025-08-06 08:17:59 

0001 /*!
0002  *  \file       Track.cc
0003  *  \brief      Data structure and output of the fitting.
0004  *  \author     Haiwang Yu <yuhw@nmsu.edu>
0005  */
0006 
0007 // PHGenFit
0008 #include "Track.h"
0009 #include "Measurement.h"
0010 
0011 #include <trackbase/TrkrDefs.h>
0012 
0013 // GenFit
0014 #include <GenFit/AbsFitterInfo.h>
0015 #include <GenFit/AbsHMatrix.h>
0016 #include <GenFit/AbsMeasurement.h>
0017 #include <GenFit/AbsTrackRep.h>
0018 #include <GenFit/DetPlane.h>
0019 #include <GenFit/Exception.h>
0020 #include <GenFit/FitStatus.h>
0021 #include <GenFit/KalmanFittedStateOnPlane.h>
0022 #include <GenFit/KalmanFitterInfo.h>
0023 #include <GenFit/MeasuredStateOnPlane.h>
0024 #include <GenFit/MeasurementOnPlane.h>
0025 #include <GenFit/SharedPlanePtr.h>
0026 #include <GenFit/Tools.h>
0027 #include <GenFit/Track.h>
0028 #include <GenFit/TrackPoint.h>
0029 
0030 #include <TMatrixDSymfwd.h>  // for TMatrixDSym
0031 #include <TMatrixDfwd.h>     // for TMatrixD
0032 #include <TMatrixT.h>        // for TMatrixT
0033 #include <TMatrixTSym.h>     // for TMatrixTSym
0034 #include <TVector3.h>        // for TVector3
0035 #include <TVectorDfwd.h>     // for TVectorD
0036 #include <TVectorT.h>        // for TVectorT, operator-
0037 
0038 // STL
0039 #include <cassert>
0040 #include <cstddef>
0041 #include <iostream>
0042 #include <limits>
0043 #include <utility>
0044 
0045 #define LogDebug(exp) std::cout << "DEBUG: " << __FILE__ << ": " << __LINE__ << ": " << (exp) << std::endl
0046 #define LogError(exp) std::cout << "ERROR: " << __FILE__ << ": " << __LINE__ << ": " << (exp) << std::endl
0047 #define LogWarning(exp) std::cout << "WARNING: " << __FILE__ << ": " << __LINE__ << ": " << (exp) << std::endl
0048 
0049 #define WILD_DOUBLE (-999999)
0050 
0051 //#define _DEBUG_
0052 //#define _PRINT_MATRIX_
0053 
0054 #ifdef _DEBUG_
0055 #include <fstream>
0056 #include <iostream>
0057 ofstream fout_matrix("matrix.txt");
0058 #endif
0059 
0060 namespace PHGenFit
0061 {
0062   Track::Track(genfit::AbsTrackRep* rep, const TVector3& seed_pos, const TVector3& seed_mom, const TMatrixDSym& seed_cov, const int v)
0063   {
0064     // TODO Add input param check
0065 
0066     verbosity = v;
0067 
0068     genfit::MeasuredStateOnPlane seedMSoP(rep);
0069     seedMSoP.setPosMomCov(seed_pos, seed_mom, seed_cov);
0070     // const genfit::StateOnPlane seedSoP(seedMSoP);
0071 
0072     TVectorD seedState(6);
0073     TMatrixDSym seedCov(6);
0074     seedMSoP.get6DStateCov(seedState, seedCov);
0075 
0076     _track = new genfit::Track(rep, seedState, seedCov);
0077     //_track = NEW(genfit::Track)(rep, seedState, seedCov);
0078   }
0079 
0080   Track::Track(const PHGenFit::Track& t)
0081   {
0082     _track = new genfit::Track(*(t.getGenFitTrack()));
0083     verbosity = t.verbosity;
0084     _clusterIDs = t.get_cluster_IDs();
0085     _clusterkeys = t.get_cluster_keys();
0086     _vertex_id = t.get_vertex_id();
0087   }
0088 
0089   int Track::addMeasurement(PHGenFit::Measurement* measurement)
0090   {
0091     std::vector<genfit::AbsMeasurement*> msmts;
0092     msmts.push_back(measurement->getMeasurement());
0093     _track->insertPoint(new genfit::TrackPoint(msmts, _track));
0094 
0095     _clusterIDs.push_back(measurement->get_cluster_ID());
0096     _clusterkeys.push_back(measurement->get_cluster_key());
0097 
0098     delete measurement;
0099 
0100     return 0;
0101   }
0102 
0103   int Track::addMeasurements(std::vector<PHGenFit::Measurement*>& measurements)
0104   {
0105     for (PHGenFit::Measurement* measurement : measurements)
0106     {
0107       std::vector<genfit::AbsMeasurement*> msmts;
0108       msmts.push_back(measurement->getMeasurement());
0109       _track->insertPoint(
0110           new genfit::TrackPoint(msmts, _track));
0111 
0112       //_measurements.push_back(measurement);
0113       _clusterIDs.push_back(measurement->get_cluster_ID());
0114       _clusterkeys.push_back(measurement->get_cluster_key());
0115 
0116       delete measurement;
0117     }
0118 
0119     // measurements.clear();
0120 
0121     return 0;
0122   }
0123 
0124   int Track::deleteLastMeasurement()
0125   {
0126     _track->deletePoint(-1);
0127 
0128     _clusterIDs.pop_back();
0129     _clusterkeys.pop_back();
0130 
0131     return 0;
0132   }
0133 
0134   Track::~Track()
0135   {
0136     //  std::cout << "DTOR: " << __LINE__ <<std::endl;
0137     delete _track;
0138 
0139     //  for(PHGenFit::Measurement* measurement : _measurements)
0140     //  {
0141     //      delete measurement;
0142     //  }
0143     //  _measurements.clear();
0144 
0145     _clusterIDs.clear();
0146     _clusterkeys.clear();
0147   }
0148 
0149   double Track::extrapolateToPlane(genfit::MeasuredStateOnPlane& state, const TVector3& O, const TVector3& n, const int tr_point_id) const
0150   {
0151     double pathlenth = WILD_DOUBLE;
0152 
0153     genfit::SharedPlanePtr destPlane(new genfit::DetPlane(O, n));
0154 
0155     genfit::AbsTrackRep* rep = _track->getCardinalRep();
0156     genfit::TrackPoint* tp = _track->getPointWithMeasurementAndFitterInfo(
0157         tr_point_id, rep);
0158     if (tp == nullptr)
0159     {
0160       std::cout << "Track has no TrackPoint with fitterInfo! \n";
0161       return WILD_DOUBLE;
0162     }
0163     std::unique_ptr<genfit::KalmanFittedStateOnPlane> kfsop(new genfit::KalmanFittedStateOnPlane(
0164         *(static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(rep))->getBackwardUpdate())));
0165     // extrapolate back to reference plane.
0166     try
0167     {
0168       pathlenth = rep->extrapolateToPlane(*kfsop, destPlane);
0169     }
0170     catch (genfit::Exception& e)
0171     {
0172       std::cerr << "Exception, next track" << std::endl;
0173       std::cerr << e.what();
0174       // delete kfsop;
0175       return WILD_DOUBLE;
0176     }
0177 
0178     state = *dynamic_cast<genfit::MeasuredStateOnPlane*>(kfsop.get());
0179 
0180     // delete kfsop;
0181 
0182     return pathlenth;
0183   }
0184 
0185   genfit::MeasuredStateOnPlane* Track::extrapolateToPlane(const TVector3& O, const TVector3& n, const int tr_point_id) const
0186   {
0187     genfit::MeasuredStateOnPlane* state = new genfit::MeasuredStateOnPlane();
0188     double pathlenth = this->extrapolateToPlane(*state, O, n, tr_point_id);
0189     if (pathlenth <= WILD_DOUBLE)
0190     {
0191       delete state;
0192       return nullptr;
0193     }
0194     else
0195     {
0196       return state;
0197     }
0198   }
0199 
0200   double Track::extrapolateToLine(genfit::MeasuredStateOnPlane& state, const TVector3& line_point, const TVector3& line_direction, const int tr_point_id) const
0201   {
0202     double pathlenth = WILD_DOUBLE;
0203 
0204     genfit::AbsTrackRep* rep = _track->getCardinalRep();
0205     genfit::TrackPoint* tp = _track->getPointWithMeasurementAndFitterInfo(
0206         tr_point_id, rep);
0207     if (tp == nullptr)
0208     {
0209       std::cout << "Track has no TrackPoint with fitterInfo! \n";
0210       return WILD_DOUBLE;
0211     }
0212     std::unique_ptr<genfit::KalmanFittedStateOnPlane> kfsop(new genfit::KalmanFittedStateOnPlane(
0213         *(static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(rep))->getBackwardUpdate())));
0214     // extrapolate back to reference plane.
0215     try
0216     {
0217       pathlenth = rep->extrapolateToLine(*kfsop, line_point, line_direction);
0218     }
0219     catch (genfit::Exception& e)
0220     {
0221       std::cerr << "Exception, next track" << std::endl;
0222       std::cerr << e.what();
0223       // delete kfsop;
0224       return WILD_DOUBLE;
0225     }
0226 
0227     state = *dynamic_cast<genfit::MeasuredStateOnPlane*>(kfsop.get());
0228 
0229     // delete kfsop;
0230 
0231     return pathlenth;
0232   }
0233 
0234   genfit::MeasuredStateOnPlane* Track::extrapolateToLine(const TVector3& line_point, const TVector3& line_direction, const int tr_point_id) const
0235   {
0236     genfit::MeasuredStateOnPlane* state = new genfit::MeasuredStateOnPlane();
0237     double pathlenth = this->extrapolateToLine(*state, line_point, line_direction, tr_point_id);
0238     if (pathlenth <= WILD_DOUBLE)
0239     {
0240       delete state;
0241       return nullptr;
0242     }
0243     else
0244     {
0245       return state;
0246     }
0247   }
0248 
0249   double Track::extrapolateToCylinder(genfit::MeasuredStateOnPlane& state, double radius, const TVector3& line_point, const TVector3& line_direction, const int tr_point_id, const int direction) const
0250   {
0251 #ifdef _DEBUG_
0252     std::cout << __LINE__ << std::endl;
0253     std::cout
0254         << __LINE__
0255         << ": tr_point_id: " << tr_point_id
0256         << ": direction: " << direction
0257         << std::endl;
0258 #endif
0259     assert(direction == 1 or direction == -1);
0260 
0261     double pathlenth = WILD_DOUBLE;
0262 
0263     genfit::AbsTrackRep* rep = _track->getCardinalRep();
0264 
0265     //  genfit::TrackPoint* tp = _track->getPointWithMeasurementAndFitterInfo(
0266     //          tr_point_id, rep);
0267     //  if (tp == NULL) {
0268     //      std::cout << "Track has no TrackPoint with fitterInfo! \n";
0269     //      return WILD_DOUBLE;
0270     //  }
0271 
0272     bool have_tp_with_fit_info = false;
0273     std::unique_ptr<genfit::MeasuredStateOnPlane> kfsop = nullptr;
0274     if (_track->getNumPointsWithMeasurement() > 0)
0275     {
0276 #ifdef _DEBUG_
0277 //      std::cout<<__LINE__ <<std::endl;
0278 #endif
0279       genfit::TrackPoint* tp = _track->getPointWithMeasurement(tr_point_id);
0280       if (tp == nullptr)
0281       {
0282         LogError("tp == NULL!");
0283         return WILD_DOUBLE;
0284       }
0285       if (dynamic_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(rep)))
0286       {
0287         if (direction == 1)
0288         {
0289           if (static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(
0290                                                          rep))
0291                   ->getForwardUpdate())
0292           {
0293             have_tp_with_fit_info = true;
0294             kfsop =
0295                 std::unique_ptr<genfit::MeasuredStateOnPlane>(new genfit::KalmanFittedStateOnPlane(
0296                     *(static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(
0297                                                                  rep))
0298                           ->getForwardUpdate())));
0299           }
0300         }
0301         else
0302         {
0303           if (static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(
0304                                                          rep))
0305                   ->getBackwardUpdate())
0306           {
0307             have_tp_with_fit_info = true;
0308             kfsop =
0309                 std::unique_ptr<genfit::MeasuredStateOnPlane>(new genfit::KalmanFittedStateOnPlane(
0310                     *(static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(
0311                                                                  rep))
0312                           ->getBackwardUpdate())));
0313           }
0314         }
0315       }
0316     }
0317 
0318     if (!have_tp_with_fit_info)
0319     {
0320 #ifdef _DEBUG_
0321       std::cout << __LINE__ << ": !have_tp_with_fit_info" << std::endl;
0322 #endif
0323       kfsop = std::unique_ptr<genfit::MeasuredStateOnPlane>(new genfit::MeasuredStateOnPlane(rep));
0324       rep->setPosMomCov(*kfsop, _track->getStateSeed(), _track->getCovSeed());
0325     }
0326 
0327     if (!kfsop)
0328     {
0329       return pathlenth;
0330     }
0331     // extrapolate back to reference plane.
0332     try
0333     {
0334       // rep->extrapolateToLine(*kfsop, line_point, line_direction);
0335       pathlenth = rep->extrapolateToCylinder(*kfsop, radius, line_point, line_direction);
0336     }
0337     catch (genfit::Exception& e)
0338     {
0339       if (verbosity > 1)
0340       {
0341         LogWarning("Can't extrapolate to cylinder!");
0342         std::cerr << e.what();
0343       }
0344       return WILD_DOUBLE;
0345     }
0346 
0347     state = *dynamic_cast<genfit::MeasuredStateOnPlane*>(kfsop.get());
0348 
0349     // delete kfsop;
0350 
0351     return pathlenth;
0352   }
0353 
0354   genfit::MeasuredStateOnPlane* Track::extrapolateToCylinder(double radius, const TVector3& line_point, const TVector3& line_direction, const int tr_point_id, const int direction) const
0355   {
0356     assert(direction == 1 or direction == -1);
0357     genfit::MeasuredStateOnPlane* state = new genfit::MeasuredStateOnPlane();
0358     double pathlenth = this->extrapolateToCylinder(*state, radius, line_point, line_direction, tr_point_id, direction);
0359     if (pathlenth <= WILD_DOUBLE)
0360     {
0361       delete state;
0362       return nullptr;
0363     }
0364     else
0365     {
0366       return state;
0367     }
0368   }
0369 
0370   int Track::updateOneMeasurementKalman(
0371       const std::vector<PHGenFit::Measurement*>& measurements,
0372       std::map<double, std::shared_ptr<PHGenFit::Track> >& incr_chi2s_new_tracks,
0373       const int base_tp_idx,
0374       const int direction,
0375       const float blowup_factor,
0376       const bool use_fitted_state) const
0377   {
0378 #ifdef _DEBUG_
0379     std::cout
0380         << __LINE__
0381         << " : base_tp_idx: " << base_tp_idx
0382         << " : direction: " << direction
0383         << " : blowup_factor: " << blowup_factor
0384         << " : use_fitted_state: " << use_fitted_state
0385         << std::endl;
0386 #endif
0387 
0388     if (measurements.size() == 0)
0389     {
0390       return -1;
0391     }
0392 
0393     for (PHGenFit::Measurement* measurement : measurements)
0394     {
0395       std::shared_ptr<PHGenFit::Track> new_track = nullptr;
0396 
0397       new_track = std::shared_ptr<PHGenFit::Track>(new PHGenFit::Track(*this));
0398 
0399       //        if(incr_chi2s_new_tracks.size() == 0)
0400       //            new_track = const_cast<PHGenFit::Track*>(this);
0401       //        else
0402       //            new_track = new PHGenFit::Track(*this);
0403 
0404       genfit::Track* track = new_track->getGenFitTrack();
0405       genfit::AbsTrackRep* rep = track->getCardinalRep();
0406 
0407       bool newFi(true);
0408       genfit::TrackPoint* tp_base = nullptr;
0409       std::unique_ptr<genfit::MeasuredStateOnPlane> currentState = nullptr;
0410       genfit::SharedPlanePtr plane = nullptr;
0411 
0412       if (track->getNumPointsWithMeasurement() > 0)
0413       {
0414         tp_base = track->getPointWithMeasurement(base_tp_idx);
0415         newFi = !(tp_base->hasFitterInfo(rep));
0416         // tp_base->Print();
0417       }
0418 #ifdef _DEBUG_
0419       std::cout << __LINE__ << ": "
0420                 << "newFi: " << newFi << std::endl;
0421 #endif
0422       if (newFi)
0423       {
0424         currentState = std::unique_ptr<genfit::MeasuredStateOnPlane>(new genfit::MeasuredStateOnPlane(rep));
0425         rep->setPosMomCov(*currentState, track->getStateSeed(),
0426                           track->getCovSeed());
0427       }
0428       else
0429       {
0430         try
0431         {
0432           genfit::KalmanFitterInfo* kfi = static_cast<genfit::KalmanFitterInfo*>(tp_base->getFitterInfo(rep));
0433           if (!kfi)
0434           {
0435 #ifdef _DEBUG_
0436             LogDebug("!kfi");
0437 #endif
0438             continue;
0439           }
0440           //#ifdef _DEBUG_
0441           //                std::cout << __LINE__ << "\n ###################################################################"<<std::endl;
0442           //                kfi->Print();
0443           //                std::cout << __LINE__ << "\n ###################################################################"<<std::endl;
0444           //#endif
0445           if (use_fitted_state)
0446           {
0447             const genfit::MeasuredStateOnPlane* tempFS = &(kfi->getFittedState(true));
0448             if (!tempFS)
0449             {
0450 #ifdef _DEBUG_
0451               LogDebug("!tempFS");
0452 #endif
0453               continue;
0454             }
0455             currentState = std::unique_ptr<genfit::MeasuredStateOnPlane>(new genfit::MeasuredStateOnPlane(*tempFS));
0456           }
0457           else
0458           {
0459             genfit::MeasuredStateOnPlane* tempUpdate = kfi->getUpdate(direction);
0460             if (!tempUpdate)
0461             {
0462 #ifdef _DEBUG_
0463               LogDebug("!tempUpdate");
0464 #endif
0465               continue;
0466             }
0467             currentState = std::unique_ptr<genfit::MeasuredStateOnPlane>(new genfit::MeasuredStateOnPlane(*tempUpdate));
0468           }
0469 
0470 #ifdef _DEBUG_
0471 //              std::cout << __LINE__ << "\n ###################################################################"<<std::endl;
0472 //              kfi->Print();
0473 //              std::cout << __LINE__ << "\n ###################################################################"<<std::endl;
0474 //              tempFS->Print();
0475 //              std::cout << __LINE__ << "\n ###################################################################"<<std::endl;
0476 //              tempUpdate->Print();
0477 //              std::cout << __LINE__ << "\n ###################################################################"<<std::endl;
0478 #endif
0479 
0480           if (blowup_factor > 1)
0481           {
0482             currentState->blowUpCov(blowup_factor, true, 1e6);
0483           }
0484         }
0485         catch (genfit::Exception& e)
0486         {
0487 #ifdef _DEBUG_
0488           std::cout
0489               << __LINE__
0490               << ": Fitted state not found!"
0491               << std::endl;
0492           std::cerr << e.what() << std::endl;
0493 #endif
0494           currentState = std::unique_ptr<genfit::MeasuredStateOnPlane>(new genfit::MeasuredStateOnPlane(rep));
0495           rep->setPosMomCov(*currentState, track->getStateSeed(),
0496                             track->getCovSeed());
0497         }
0498       }
0499 #ifdef _DEBUG_
0500       std::cout << __LINE__ << std::endl;
0501 #endif
0502       // std::vector<genfit::AbsMeasurement*> msmts;
0503       // msmts.push_back(measurement->getMeasurement());
0504 
0505       // genfit::TrackPoint *tp = new genfit::TrackPoint(msmts, track);
0506       // track->insertPoint(tp); // genfit
0507 
0508       /*!
0509        * A new TrackPoint created in addMeasurement
0510        * PHGenFit: clusterID also registerd
0511        */
0512       new_track->addMeasurement(measurement);
0513 
0514 #ifdef _DEBUG_
0515       std::cout << __LINE__ << ": clusterIDs size: " << new_track->get_cluster_IDs().size() << std::endl;
0516       std::cout << __LINE__ << ": clusterkeyss size: " << new_track->get_cluster_keys().size() << std::endl;
0517 #endif
0518 
0519       //! Get the pointer of the TrackPoint just created
0520       genfit::TrackPoint* tp = new_track->getGenFitTrack()->getPoint(-1);
0521 #ifdef _DEBUG_
0522       std::cout << __LINE__ << std::endl;
0523 #endif
0524       genfit::KalmanFitterInfo* fi = new genfit::KalmanFitterInfo(tp, rep);
0525       tp->setFitterInfo(fi);
0526 #ifdef _DEBUG_
0527       std::cout
0528           << __LINE__
0529           << ": track->getPointWithMeasurement(): " << track->getPointWithMeasurement(-1)
0530           << std::endl;
0531 #endif
0532 //      if (track->getNumPointsWithMeasurement() > 0) {
0533 //          tp_base = track->getPointWithMeasurement(-1);
0534 //          if (tp_base->hasFitterInfo(rep)) {
0535 //              std::cout << "TP has FI!" << std::endl;
0536 //          }
0537 //      }
0538 #ifdef _DEBUG_
0539       std::cout << __LINE__ << std::endl;
0540 #endif
0541       const std::vector<genfit::AbsMeasurement*>& rawMeasurements =
0542           tp->getRawMeasurements();
0543       // construct plane with first measurement
0544       plane = rawMeasurements[0]->constructPlane(*currentState);
0545 
0546       // double extLen = rep->extrapolateToPlane(*state, plane);
0547 
0548       try
0549       {
0550         rep->extrapolateToPlane(*currentState, plane);
0551       }
0552       catch (...)
0553       {
0554         if (verbosity > 1)
0555         {
0556           LogWarning("Can not extrapolate to measuremnt with cluster_ID and cluster key: ") << measurement->get_cluster_ID()
0557                                                                                             << "    " << measurement->get_cluster_key()
0558                                                                                             << std::endl;
0559         }
0560         continue;
0561       }
0562 #ifdef _DEBUG_
0563       std::cout << __LINE__ << std::endl;
0564 #endif
0565       fi->setPrediction(currentState->clone(), direction);
0566       genfit::MeasuredStateOnPlane* state = fi->getPrediction(direction);
0567 #ifdef _DEBUG_
0568       std::cout << __LINE__ << std::endl;
0569 #endif
0570       TVectorD stateVector(state->getState());
0571       TMatrixDSym cov(state->getCov());
0572 #ifdef _DEBUG_
0573       {
0574         std::cout << __LINE__ << std::endl;
0575         //          TMatrixDSym cov6d = state->get6DCov();
0576         //          float err_rphi = sqrt(
0577         //                  cov6d[0][0] + cov6d[1][1] + cov6d[0][1] + cov6d[1][0]);
0578         //          float err_z = sqrt(cov6d[2][2]);
0579         //          std::cout << err_phi << "\t" << err_z << "\t";
0580       }
0581 #endif
0582       for (auto rawMeasurement : rawMeasurements)
0583       {
0584         fi->addMeasurementsOnPlane(
0585             rawMeasurement->constructMeasurementsOnPlane(*state));
0586       }
0587 
0588       double chi2inc = 0;
0589       double ndfInc = 0;
0590 #ifdef _DEBUG_
0591       std::cout << __LINE__ << std::endl;
0592 #endif
0593       // update(s)
0594       const std::vector<genfit::MeasurementOnPlane*>& measurements_on_plane = fi->getMeasurementsOnPlane();
0595 #ifdef _DEBUG_
0596       std::cout
0597           << __LINE__
0598           << ": size of fi's MeasurementsOnPlane: " << measurements_on_plane.size()
0599           << std::endl;
0600 #endif
0601       for (auto it : measurements_on_plane)
0602       {
0603         const genfit::MeasurementOnPlane& mOnPlane = *it;
0604         // const double weight = mOnPlane.getWeight();
0605 
0606         const TVectorD& measurementA(mOnPlane.getState());
0607         const genfit::AbsHMatrix* H(mOnPlane.getHMatrix());
0608         // (weighted) cov
0609         const TMatrixDSym& V(mOnPlane.getCov());  // Covariance of measurement noise v_{k}
0610 
0611         TVectorD res(measurementA - H->Hv(stateVector));
0612 #ifdef _DEBUG_
0613         {
0614           std::cout << __LINE__ << std::endl;
0615           //            std::cout
0616           //            <<res(0) <<"\t"
0617           //            <<res(1) <<"\t";
0618         }
0619 #endif
0620         // If hit, do Kalman algebra.
0621         {
0622           // calculate kalman gain ------------------------------
0623           // calculate covsum (V + HCH^T) and invert
0624           TMatrixDSym covSumInv(cov);
0625           H->HMHt(covSumInv);
0626           covSumInv += V;
0627           try
0628           {
0629             genfit::tools::invertMatrix(covSumInv);
0630           }
0631           catch (genfit::Exception& e)
0632           {
0633 #ifdef _DEBUG_
0634             LogDebug("cannot invert matrix.");
0635 #endif
0636             continue;
0637           }
0638 
0639           TMatrixD CHt(H->MHt(cov));
0640 #ifdef _PRINT_MATRIX_
0641           std::cout << __LINE__ << ": V_{k}:" << std::endl;
0642           V.Print();
0643           std::cout << __LINE__ << ": R_{k}^{-1}:" << std::endl;
0644           covSumInv.Print();
0645           std::cout << __LINE__ << ": C_{k|k-1}:" << std::endl;
0646           cov.Print();
0647           std::cout << __LINE__ << ": C_{k|k-1} H_{k}^{T} :" << std::endl;
0648           CHt.Print();
0649           std::cout << __LINE__ << ": K_{k} :" << std::endl;
0650           TMatrixD Kk(CHt, TMatrixD::kMult, covSumInv);
0651           Kk.Print();
0652           std::cout << __LINE__ << ": res:" << std::endl;
0653           res.Print();
0654 #endif
0655           TVectorD update(
0656               TMatrixD(CHt, TMatrixD::kMult, covSumInv) * res);
0657           // TMatrixD(CHt, TMatrixD::kMult, covSumInv).Print();
0658 
0659           stateVector += update;      // x_{k|k} = x_{k|k-1} + K_{k} r_{k|k-1}
0660           covSumInv.Similarity(CHt);  // with (C H^T)^T = H C^T = H C  (C is symmetric)
0661           cov -= covSumInv;           // C_{k|k}
0662 #ifdef _DEBUG_
0663           {
0664             std::cout << __LINE__ << std::endl;
0665             //          TMatrixDSym cov6d = state->get6DCov();
0666             //          float err_rphi     = sqrt(cov6d[0][0] + cov6d[1][1] + cov6d[0][1] + cov6d[1][0]);
0667             //          float err_z   = sqrt(cov6d[2][2]);
0668             //          std::cout
0669             //          <<err_phi <<"\t"
0670             //          <<err_z <<"\t";
0671           }
0672 #endif
0673         }
0674 
0675         TVectorD resNew(measurementA - H->Hv(stateVector));
0676 
0677         // Calculate chi2
0678         TMatrixDSym HCHt(cov);  // C_{k|k}
0679         H->HMHt(HCHt);
0680         HCHt -= V;
0681         HCHt *= -1;
0682 
0683         try
0684         {
0685           genfit::tools::invertMatrix(HCHt);
0686         }
0687         catch (genfit::Exception& e)
0688         {
0689 #ifdef _DEBUG_
0690           LogDebug("cannot invert matrix.");
0691 #endif
0692           continue;
0693         }
0694         chi2inc += HCHt.Similarity(resNew);
0695 
0696         ndfInc += measurementA.GetNrows();
0697 
0698 #ifdef _PRINT_MATRIX_
0699         std::cout << __LINE__ << ": V - HCHt:" << std::endl;
0700         HCHt.Print();
0701         std::cout << __LINE__ << ": resNew:" << std::endl;
0702         resNew.Print();
0703 #endif
0704 
0705 #ifdef _DEBUG_
0706         std::cout << __LINE__ << ": ndfInc:  " << ndfInc << std::endl;
0707         std::cout << __LINE__ << ": chi2inc: " << chi2inc << std::endl;
0708 #endif
0709 
0710         currentState->setStateCovPlane(stateVector, cov, plane);
0711         currentState->setAuxInfo(state->getAuxInfo());
0712 
0713         genfit::KalmanFittedStateOnPlane* updatedSOP =
0714             new genfit::KalmanFittedStateOnPlane(*currentState, chi2inc,
0715                                                  ndfInc);
0716         fi->setUpdate(updatedSOP, direction);
0717       }  // loop measurements_on_plane
0718 
0719       // FIXME why chi2 could be smaller than 0?
0720       if (chi2inc > 0)
0721       {
0722         incr_chi2s_new_tracks.insert(std::make_pair(chi2inc, new_track));
0723       }
0724 
0725     }  // loop measurments
0726 
0727     return 0;
0728   }
0729 
0730   double Track::extrapolateToPoint(genfit::MeasuredStateOnPlane& state, const TVector3& P, const int tr_point_id) const
0731   {
0732     double pathlenth = WILD_DOUBLE;
0733     genfit::AbsTrackRep* rep = _track->getCardinalRep();
0734     genfit::TrackPoint* tp = _track->getPointWithMeasurementAndFitterInfo(
0735         tr_point_id, rep);
0736     if (tp == nullptr)
0737     {
0738       std::cout << "Track has no TrackPoint with fitterInfo! \n";
0739       return WILD_DOUBLE;
0740     }
0741     std::unique_ptr<genfit::KalmanFittedStateOnPlane> kfsop(new genfit::KalmanFittedStateOnPlane(
0742         *(static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(rep))->getBackwardUpdate())));
0743     // extrapolate back to reference plane.
0744     try
0745     {
0746       pathlenth = rep->extrapolateToPoint(*kfsop, P);
0747     }
0748     catch (genfit::Exception& e)
0749     {
0750       std::cerr << "Exception, next track" << std::endl;
0751       std::cerr << e.what();
0752       // delete kfsop;
0753       return WILD_DOUBLE;
0754     }
0755 
0756     state = *dynamic_cast<genfit::MeasuredStateOnPlane*>(kfsop.get());
0757 
0758     // delete kfsop;
0759 
0760     return pathlenth;
0761   }
0762 
0763   genfit::MeasuredStateOnPlane* Track::extrapolateToPoint(const TVector3& P, const int tr_point_id) const
0764   {
0765     genfit::MeasuredStateOnPlane* state = new genfit::MeasuredStateOnPlane();
0766     double pathlenth = this->extrapolateToPoint(*state, P, tr_point_id);
0767     if (pathlenth <= WILD_DOUBLE)
0768     {
0769       delete state;
0770       return nullptr;
0771     }
0772     else
0773     {
0774       return state;
0775     }
0776   }
0777 
0778   double Track::get_chi2() const
0779   {
0780     double chi2 = std::numeric_limits<double>::quiet_NaN();
0781 
0782     genfit::AbsTrackRep* rep = _track->getCardinalRep();
0783     if (rep)
0784     {
0785       genfit::FitStatus* fs = _track->getFitStatus(rep);
0786       if (fs)
0787       {
0788         chi2 = fs->getChi2();
0789       }
0790     }
0791     return chi2;
0792   }
0793 
0794   double Track::get_ndf() const
0795   {
0796     double ndf = std::numeric_limits<double>::quiet_NaN();
0797 
0798     genfit::AbsTrackRep* rep = _track->getCardinalRep();
0799     if (rep)
0800     {
0801       genfit::FitStatus* fs = _track->getFitStatus(rep);
0802       if (fs)
0803       {
0804         ndf = fs->getNdf();
0805       }
0806     }
0807     return ndf;
0808   }
0809 
0810   double Track::get_charge() const
0811   {
0812     double charge = std::numeric_limits<double>::quiet_NaN();
0813 
0814     if (!_track)
0815     {
0816       return charge;
0817     }
0818 
0819     try
0820     {
0821       genfit::TrackPoint* tp_base = nullptr;
0822 
0823       if (_track->getNumPointsWithMeasurement() > 0)
0824       {
0825         tp_base = _track->getPointWithMeasurement(0);
0826       }
0827 
0828       if (!tp_base)
0829       {
0830         return charge;
0831       }
0832 
0833       genfit::AbsTrackRep* rep = _track->getCardinalRep();
0834       if (rep)
0835       {
0836         genfit::KalmanFitterInfo* kfi = static_cast<genfit::KalmanFitterInfo*>(tp_base->getFitterInfo(rep));
0837 
0838         if (!kfi)
0839         {
0840           return charge;
0841         }
0842 
0843         const genfit::MeasuredStateOnPlane* state = &(kfi->getFittedState(true));
0844 
0845         // std::unique_ptr<genfit::StateOnPlane> state (this->extrapolateToLine(TVector3(0, 0, 0), TVector3(1, 0, 0)));
0846 
0847         if (state)
0848         {
0849           charge = rep->getCharge(*state);
0850         }
0851       }
0852     }
0853     catch (...)
0854     {
0855       if (verbosity >= 1)
0856       {
0857         std::cerr << "Track::get_charge - Error - obtaining charge failed. Returning NAN as charge." << std::endl;
0858       }
0859     }
0860 
0861     return charge;
0862   }
0863 
0864   TVector3 Track::get_mom() const
0865   {
0866     TVector3 mom(0, 0, 0);
0867 
0868     if (!_track)
0869     {
0870       return mom;
0871     }
0872 
0873     genfit::TrackPoint* tp_base = nullptr;
0874 
0875     if (_track->getNumPointsWithMeasurement() > 0)
0876     {
0877       tp_base = _track->getPointWithMeasurement(0);
0878     }
0879 
0880     if (!tp_base)
0881     {
0882       return mom;
0883     }
0884 
0885     genfit::AbsTrackRep* rep = _track->getCardinalRep();
0886     if (rep)
0887     {
0888       genfit::KalmanFitterInfo* kfi = static_cast<genfit::KalmanFitterInfo*>(tp_base->getFitterInfo(rep));
0889 
0890       if (!kfi)
0891       {
0892         return mom;
0893       }
0894 
0895       const genfit::MeasuredStateOnPlane* state = &(kfi->getFittedState(true));
0896 
0897       if (state)
0898       {
0899         return state->getMom();
0900       }
0901     }
0902 
0903     return mom;
0904   }
0905 
0906 }  // namespace PHGenFit
[ 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!