9 #include <phgenfit/Measurement.h>
14 #include <GenFit/AbsFitterInfo.h>
15 #include <GenFit/AbsHMatrix.h>
16 #include <GenFit/AbsMeasurement.h>
17 #include <GenFit/AbsTrackRep.h>
18 #include <GenFit/DetPlane.h>
19 #include <GenFit/Exception.h>
20 #include <GenFit/FitStatus.h>
21 #include <GenFit/KalmanFittedStateOnPlane.h>
22 #include <GenFit/KalmanFitterInfo.h>
23 #include <GenFit/MeasuredStateOnPlane.h>
24 #include <GenFit/MeasurementOnPlane.h>
25 #include <GenFit/SharedPlanePtr.h>
28 #include <GenFit/TrackPoint.h>
30 #include <TMatrixDSymfwd.h>
31 #include <TMatrixDfwd.h>
33 #include <TMatrixTSym.h>
35 #include <TVectorDfwd.h>
45 #define LogDebug(exp) std::cout << "DEBUG: " << __FILE__ << ": " << __LINE__ << ": " << exp << std::endl
46 #define LogError(exp) std::cout << "ERROR: " << __FILE__ << ": " << __LINE__ << ": " << exp << std::endl
47 #define LogWarning(exp) std::cout << "WARNING: " << __FILE__ << ": " << __LINE__ << ": " << exp << std::endl
49 #define WILD_DOUBLE -999999
57 ofstream fout_matrix(
"matrix.txt");
62 Track::Track(genfit::AbsTrackRep* rep, TVector3 seed_pos, TVector3 seed_mom, TMatrixDSym seed_cov,
const int v)
69 genfit::MeasuredStateOnPlane seedMSoP(rep);
70 seedMSoP.setPosMomCov(seed_pos, seed_mom, seed_cov);
73 TVectorD seedState(6);
74 TMatrixDSym seedCov(6);
75 seedMSoP.get6DStateCov(seedState, seedCov);
77 _track =
new genfit::Track(rep, seedState, seedCov);
159 genfit::SharedPlanePtr destPlane(
new genfit::DetPlane(O, n));
161 genfit::AbsTrackRep* rep =
_track->getCardinalRep();
162 genfit::TrackPoint*
tp =
_track->getPointWithMeasurementAndFitterInfo(
166 std::cout <<
"Track has no TrackPoint with fitterInfo! \n";
169 std::unique_ptr<genfit::KalmanFittedStateOnPlane> kfsop(
new genfit::KalmanFittedStateOnPlane(
170 *(static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(rep))->getBackwardUpdate())));
174 pathlenth = rep->extrapolateToPlane(*kfsop, destPlane);
176 catch (genfit::Exception&
e)
178 std::cerr <<
"Exception, next track" << std::endl;
179 std::cerr << e.what();
184 state = *
dynamic_cast<genfit::MeasuredStateOnPlane*
>(kfsop.get());
193 genfit::MeasuredStateOnPlane* state =
new genfit::MeasuredStateOnPlane();
204 double Track::extrapolateToLine(genfit::MeasuredStateOnPlane& state, TVector3 line_point, TVector3 line_direction,
const int tr_point_id)
const
208 genfit::AbsTrackRep* rep =
_track->getCardinalRep();
209 genfit::TrackPoint*
tp =
_track->getPointWithMeasurementAndFitterInfo(
213 std::cout <<
"Track has no TrackPoint with fitterInfo! \n";
216 std::unique_ptr<genfit::KalmanFittedStateOnPlane> kfsop(
new genfit::KalmanFittedStateOnPlane(
217 *(static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(rep))->getBackwardUpdate())));
221 pathlenth = rep->extrapolateToLine(*kfsop, line_point, line_direction);
223 catch (genfit::Exception&
e)
225 std::cerr <<
"Exception, next track" << std::endl;
226 std::cerr << e.what();
231 state = *
dynamic_cast<genfit::MeasuredStateOnPlane*
>(kfsop.get());
240 genfit::MeasuredStateOnPlane* state =
new genfit::MeasuredStateOnPlane();
241 double pathlenth = this->
extrapolateToLine(*state, line_point, line_direction, tr_point_id);
251 double Track::extrapolateToCylinder(genfit::MeasuredStateOnPlane& state,
double radius, TVector3 line_point, TVector3 line_direction,
const int tr_point_id,
const int direction)
const
254 std::cout << __LINE__ << std::endl;
257 <<
": tr_point_id: " << tr_point_id
258 <<
": direction: " << direction
261 assert(direction == 1 or direction == -1);
265 genfit::AbsTrackRep* rep =
_track->getCardinalRep();
274 bool have_tp_with_fit_info =
false;
275 std::unique_ptr<genfit::MeasuredStateOnPlane> kfsop = NULL;
276 if (
_track->getNumPointsWithMeasurement() > 0)
281 genfit::TrackPoint*
tp =
_track->getPointWithMeasurement(tr_point_id);
287 if (dynamic_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(rep)))
291 if (static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(
293 ->getForwardUpdate())
295 have_tp_with_fit_info =
true;
297 std::unique_ptr<genfit::MeasuredStateOnPlane>(
new genfit::KalmanFittedStateOnPlane(
298 *(static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(
300 ->getForwardUpdate())));
305 if (static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(
307 ->getBackwardUpdate())
309 have_tp_with_fit_info =
true;
311 std::unique_ptr<genfit::MeasuredStateOnPlane>(
new genfit::KalmanFittedStateOnPlane(
312 *(static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(
314 ->getBackwardUpdate())));
320 if (!have_tp_with_fit_info)
323 std::cout << __LINE__ <<
": !have_tp_with_fit_info" << std::endl;
325 kfsop = std::unique_ptr<genfit::MeasuredStateOnPlane>(
new genfit::MeasuredStateOnPlane(rep));
326 rep->setPosMomCov(*kfsop,
_track->getStateSeed(),
_track->getCovSeed());
329 if (!kfsop)
return pathlenth;
334 pathlenth = rep->extrapolateToCylinder(*kfsop, radius, line_point, line_direction);
336 catch (genfit::Exception&
e)
341 std::cerr << e.what();
346 state = *
dynamic_cast<genfit::MeasuredStateOnPlane*
>(kfsop.get());
355 assert(direction == 1 or direction == -1);
356 genfit::MeasuredStateOnPlane* state =
new genfit::MeasuredStateOnPlane();
357 double pathlenth = this->
extrapolateToCylinder(*state, radius, line_point, line_direction, tr_point_id, direction);
368 const std::vector<PHGenFit::Measurement*>& measurements,
369 std::map<
double, std::shared_ptr<PHGenFit2::Track> >& incr_chi2s_new_tracks,
370 const int base_tp_idx,
372 const float blowup_factor,
373 const bool use_fitted_state)
const
378 <<
" : base_tp_idx: " << base_tp_idx
379 <<
" : direction: " << direction
380 <<
" : blowup_factor: " << blowup_factor
381 <<
" : use_fitted_state: " << use_fitted_state
385 if (measurements.size() == 0)
return -1;
389 std::shared_ptr<PHGenFit2::Track> new_track = NULL;
398 genfit::Track*
track = new_track->getGenFitTrack();
399 genfit::AbsTrackRep* rep = track->getCardinalRep();
402 genfit::TrackPoint* tp_base = NULL;
403 std::unique_ptr<genfit::MeasuredStateOnPlane> currentState = NULL;
404 genfit::SharedPlanePtr
plane = NULL;
406 if (track->getNumPointsWithMeasurement() > 0)
408 tp_base = track->getPointWithMeasurement(base_tp_idx);
409 newFi = !(tp_base->hasFitterInfo(rep));
413 std::cout << __LINE__ <<
": "
414 <<
"newFi: " << newFi << std::endl;
418 currentState = std::unique_ptr<genfit::MeasuredStateOnPlane>(
new genfit::MeasuredStateOnPlane(rep));
419 rep->setPosMomCov(*currentState, track->getStateSeed(),
420 track->getCovSeed());
426 genfit::KalmanFitterInfo* kfi =
static_cast<genfit::KalmanFitterInfo*
>(tp_base->getFitterInfo(rep));
439 if (use_fitted_state)
441 const genfit::MeasuredStateOnPlane* tempFS = &(kfi->getFittedState(
true));
449 currentState = std::unique_ptr<genfit::MeasuredStateOnPlane>(
new genfit::MeasuredStateOnPlane(*tempFS));
453 genfit::MeasuredStateOnPlane* tempUpdate = kfi->getUpdate(direction);
461 currentState = std::unique_ptr<genfit::MeasuredStateOnPlane>(
new genfit::MeasuredStateOnPlane(*tempUpdate));
474 if (blowup_factor > 1)
476 currentState->blowUpCov(blowup_factor,
true, 1
e6);
479 catch (genfit::Exception&
e)
484 <<
": Fitted state not found!"
486 std::cerr << e.what() << std::endl;
488 currentState = std::unique_ptr<genfit::MeasuredStateOnPlane>(
new genfit::MeasuredStateOnPlane(rep));
489 rep->setPosMomCov(*currentState, track->getStateSeed(),
490 track->getCovSeed());
494 std::cout << __LINE__ << std::endl;
506 new_track->addMeasurement(measurement);
509 std::cout << __LINE__ <<
": clusterIDs size: " << new_track->get_cluster_IDs().size() << std::endl;
510 std::cout << __LINE__ <<
": clusterkeyss size: " << new_track->get_cluster_keys().size() << std::endl;
514 genfit::TrackPoint*
tp = new_track->getGenFitTrack()->getPoint(-1);
516 std::cout << __LINE__ << std::endl;
518 genfit::KalmanFitterInfo* fi =
new genfit::KalmanFitterInfo(tp, rep);
519 tp->setFitterInfo(fi);
523 <<
": track->getPointWithMeasurement(): " << track->getPointWithMeasurement(-1)
533 std::cout << __LINE__ << std::endl;
535 const std::vector<genfit::AbsMeasurement*>& rawMeasurements =
536 tp->getRawMeasurements();
538 plane = rawMeasurements[0]->constructPlane(*currentState);
544 rep->extrapolateToPlane(*currentState, plane);
550 LogWarning(
"Can not extrapolate to measuremnt with cluster_ID and cluster key: ") << measurement->get_cluster_ID()
551 <<
" " << measurement->get_cluster_key()
557 std::cout << __LINE__ << std::endl;
559 fi->setPrediction(currentState->clone(), direction);
560 genfit::MeasuredStateOnPlane* state = fi->getPrediction(direction);
562 std::cout << __LINE__ << std::endl;
564 TVectorD stateVector(state->getState());
565 TMatrixDSym cov(state->getCov());
568 std::cout << __LINE__ << std::endl;
576 for (std::vector<genfit::AbsMeasurement*>::const_iterator
it =
577 rawMeasurements.begin();
578 it != rawMeasurements.end(); ++
it)
580 fi->addMeasurementsOnPlane(
581 (*it)->constructMeasurementsOnPlane(*state));
587 std::cout << __LINE__ << std::endl;
590 const std::vector<genfit::MeasurementOnPlane*>& measurements_on_plane = fi->getMeasurementsOnPlane();
594 <<
": size of fi's MeasurementsOnPlane: " << measurements_on_plane.size()
597 for (std::vector<genfit::MeasurementOnPlane*>::const_iterator
it =
598 measurements_on_plane.begin();
599 it != measurements_on_plane.end(); ++
it)
601 const genfit::MeasurementOnPlane& mOnPlane = **
it;
604 const TVectorD& measurement(mOnPlane.getState());
605 const genfit::AbsHMatrix*
H(mOnPlane.getHMatrix());
607 const TMatrixDSym& V(mOnPlane.getCov());
609 TVectorD res(measurement -
H->Hv(stateVector));
612 std::cout << __LINE__ << std::endl;
622 TMatrixDSym covSumInv(cov);
627 genfit::tools::invertMatrix(covSumInv);
629 catch (genfit::Exception&
e)
637 TMatrixD CHt(
H->MHt(cov));
638 #ifdef _PRINT_MATRIX_
639 std::cout << __LINE__ <<
": V_{k}:" << std::endl;
641 std::cout << __LINE__ <<
": R_{k}^{-1}:" << std::endl;
643 std::cout << __LINE__ <<
": C_{k|k-1}:" << std::endl;
645 std::cout << __LINE__ <<
": C_{k|k-1} H_{k}^{T} :" << std::endl;
647 std::cout << __LINE__ <<
": K_{k} :" << std::endl;
648 TMatrixD Kk(CHt, TMatrixD::kMult, covSumInv);
650 std::cout << __LINE__ <<
": res:" << std::endl;
654 TMatrixD(CHt, TMatrixD::kMult, covSumInv) * res);
658 covSumInv.Similarity(CHt);
662 std::cout << __LINE__ << std::endl;
673 TVectorD resNew(measurement -
H->Hv(stateVector));
676 TMatrixDSym HCHt(cov);
683 genfit::tools::invertMatrix(HCHt);
685 catch (genfit::Exception&
e)
692 chi2inc += HCHt.Similarity(resNew);
694 ndfInc += measurement.GetNrows();
696 #ifdef _PRINT_MATRIX_
697 std::cout << __LINE__ <<
": V - HCHt:" << std::endl;
699 std::cout << __LINE__ <<
": resNew:" << std::endl;
704 std::cout << __LINE__ <<
": ndfInc: " << ndfInc << std::endl;
705 std::cout << __LINE__ <<
": chi2inc: " << chi2inc << std::endl;
708 currentState->setStateCovPlane(stateVector, cov, plane);
709 currentState->setAuxInfo(state->getAuxInfo());
711 genfit::KalmanFittedStateOnPlane* updatedSOP =
712 new genfit::KalmanFittedStateOnPlane(*currentState, chi2inc,
714 fi->setUpdate(updatedSOP, direction);
719 incr_chi2s_new_tracks.insert(std::make_pair(chi2inc, new_track));
729 genfit::AbsTrackRep* rep =
_track->getCardinalRep();
730 genfit::TrackPoint*
tp =
_track->getPointWithMeasurementAndFitterInfo(
734 std::cout <<
"Track has no TrackPoint with fitterInfo! \n";
737 std::unique_ptr<genfit::KalmanFittedStateOnPlane> kfsop(
new genfit::KalmanFittedStateOnPlane(
738 *(static_cast<genfit::KalmanFitterInfo*>(tp->getFitterInfo(rep))->getBackwardUpdate())));
742 pathlenth = rep->extrapolateToPoint(*kfsop, P);
744 catch (genfit::Exception&
e)
746 std::cerr <<
"Exception, next track" << std::endl;
747 std::cerr << e.what();
752 state = *
dynamic_cast<genfit::MeasuredStateOnPlane*
>(kfsop.get());
761 genfit::MeasuredStateOnPlane* state =
new genfit::MeasuredStateOnPlane();
774 double chi2 = std::numeric_limits<double>::quiet_NaN();
776 genfit::AbsTrackRep* rep =
_track->getCardinalRep();
779 genfit::FitStatus*
fs =
_track->getFitStatus(rep);
781 chi2 = fs->getChi2();
788 double ndf = std::numeric_limits<double>::quiet_NaN();
790 genfit::AbsTrackRep* rep =
_track->getCardinalRep();
793 genfit::FitStatus*
fs =
_track->getFitStatus(rep);
802 double charge = std::numeric_limits<double>::quiet_NaN();
808 genfit::TrackPoint* tp_base =
nullptr;
810 if (
_track->getNumPointsWithMeasurement() > 0)
812 tp_base =
_track->getPointWithMeasurement(0);
815 if (!tp_base)
return charge;
817 genfit::AbsTrackRep* rep =
_track->getCardinalRep();
820 genfit::KalmanFitterInfo* kfi =
static_cast<genfit::KalmanFitterInfo*
>(tp_base->getFitterInfo(rep));
824 const genfit::MeasuredStateOnPlane* state = &(kfi->getFittedState(
true));
829 charge = rep->getCharge(*state);
835 std::cerr <<
"Track::get_charge - Error - obtaining charge failed. Returning NAN as charge." << std::endl;
843 TVector3
mom(0, 0, 0);
847 genfit::TrackPoint* tp_base =
nullptr;
849 if (
_track->getNumPointsWithMeasurement() > 0)
851 tp_base =
_track->getPointWithMeasurement(0);
854 if (!tp_base)
return mom;
856 genfit::AbsTrackRep* rep =
_track->getCardinalRep();
859 genfit::KalmanFitterInfo* kfi =
static_cast<genfit::KalmanFitterInfo*
>(tp_base->getFitterInfo(rep));
861 if (!kfi)
return mom;
863 const genfit::MeasuredStateOnPlane* state = &(kfi->getFittedState(
true));
866 return state->getMom();
874 genfit::TrackPoint*
pt =
_track->getPointWithMeasurementAndFitterInfo(0);
879 genfit::KalmanFitterInfo* kfinfo = pt->getKalmanFitterInfo();
884 const genfit::MeasuredStateOnPlane& kfstate = kfinfo->getFittedState();
885 kfstate.getPosMom(pos, mom);
886 charge = kfstate.getCharge();
887 nhits =
_track->getNumPoints();
888 length =
_track->getTrackLen();