32 #include "boost/scoped_ptr.hpp"
34 #include <TDecompChol.h>
35 #include <Math/ProbFunc.h>
49 unsigned int dim = rep->
getDim();
64 C_.ResizeTo(dim, dim);
68 assert(direction == +1 || direction == -1);
71 else if (direction == -1)
76 debugOut <<
"TrackPoint " << i <<
" has no fitterInfo -> continue. \n";
83 if (alreadyFitted && fi->
hasUpdate(direction)) {
85 debugOut <<
"TrackPoint " << i <<
" is already fitted -> continue. \n";
93 alreadyFitted =
false;
96 debugOut <<
" process TrackPoint nr. " << i <<
"\n";
112 Exception exc(
"KalmanFitterRefTrack::processTrack: Cannot process pruned track!", __LINE__,__FILE__);
116 double oldChi2FW = 1e6;
117 double oldPvalFW = 0.;
118 double oldChi2BW = 1e6;
119 double oldPvalBW = 0.;
120 double chi2FW(0), ndfFW(0);
121 double chi2BW(0), ndfBW(0);
134 debugOut <<
" KalmanFitterRefTrack::processTrack with rep " << rep
135 <<
" (id == " << tr->
getIdForRep(rep) <<
")"<<
", iteration nr. " << nIt <<
"\n";
141 debugOut <<
"KalmanFitterRefTrack::processTrack. Track preparation did not change anything!\n";
147 if (nFailedHits == 0)
168 debugOut <<
"KalmanFitterRefTrack::processTrack. Prepared Track:";
177 debugOut <<
"KalmanFitterRefTrack::processTrack. Resorted Track:";
183 debugOut <<
"KalmanFitterRefTrack::processTrack. Prepared resorted Track:";
192 debugOut <<
"KalmanFitterRefTrack::forward fit\n";
197 debugOut <<
"KalmanFitterRefTrack::backward fit\n";
201 if (lastProcessedPoint != NULL) {
207 debugOut <<
"blow up cov for backward fit at TrackPoint " << lastProcessedPoint <<
"\n";
212 fitTrack(tr, rep, chi2BW, ndfBW, -1);
217 double PvalBW = std::max(0.,ROOT::Math::chisquared_cdf_c(chi2BW, ndfBW));
218 double PvalFW = (
debugLvl_ > 0) ? std::max(0.,ROOT::Math::chisquared_cdf_c(chi2FW, ndfFW)) : 0;
221 debugOut <<
"KalmanFitterRefTrack::Track after fit:"; tr->
Print(
"C");
223 debugOut <<
"old chi2s: " << oldChi2BW <<
", " << oldChi2FW
224 <<
" new chi2s: " << chi2BW <<
", " << chi2FW << std::endl;
225 debugOut <<
"old pVals: " << oldPvalBW <<
", " << oldPvalFW
226 <<
" new pVals: " << PvalBW <<
", " << PvalFW << std::endl;
234 bool converged(
false);
235 bool finished(
false);
258 if (nFailedHits == 0)
279 debugOut <<
"KalmanFitterRefTrack::number of max iterations reached!\n";
303 if (static_cast<KalmanFitterInfo*>(tp->
getFitterInfo(rep))->hasBackwardUpdate())
304 charge = static_cast<KalmanFitterInfo*>(tp->
getFitterInfo(rep))->getBackwardUpdate()->getCharge();
334 debugOut <<
"KalmanFitterRefTrack::prepareTrack \n";
337 int notChangedUntil, notChangedFrom;
340 bool changedSmthg =
removeOutdated(tr, rep, notChangedUntil, notChangedFrom);
356 boost::scoped_ptr<MeasuredStateOnPlane> firstBackwardUpdate;
366 bool newRefState(
false);
367 bool prevNewRefState(
false);
377 for (; i<nPoints; ++i) {
382 debugOut <<
"Prepare TrackPoint " << i <<
"\n";
390 debugOut <<
"TrackPoint has no rawMeasurements -> continue \n";
401 fitterInfo = dynamic_cast<KalmanFitterInfo*>(trackPoint->
getFitterInfo(rep));
404 if (fitterInfo == NULL) {
406 debugOut <<
"create new KalmanFitterInfo \n";
414 debugOut <<
"TrackPoint " << i <<
" (" << trackPoint <<
") already has KalmanFitterInfo \n";
417 if (prevFitterInfo == NULL) {
427 debugOut <<
"got smoothed state \n";
432 smoothedState = NULL;
441 if (!prevNewRefState) {
443 debugOut <<
"TrackPoint already has referenceState and previous referenceState has not been altered -> continue \n";
446 if (setSortingParams)
449 prevNewRefState = newRefState;
450 prevReferenceState = referenceState;
451 prevFitterInfo = fitterInfo;
452 prevSmoothedState = smoothedState;
458 if (prevReferenceState == NULL) {
460 debugOut <<
"TrackPoint already has referenceState but previous referenceState is NULL -> reset forward info of current reference state and continue \n";
465 if (setSortingParams)
468 prevNewRefState = newRefState;
469 prevReferenceState = referenceState;
470 prevFitterInfo = fitterInfo;
471 prevSmoothedState = smoothedState;
478 debugOut <<
"TrackPoint already has referenceState but previous referenceState has been altered -> update transport matrices and continue \n";
488 debugOut <<
"extrapolated stateToExtrapolate (prevReferenceState) by " << segmentLen <<
" cm.\n";
490 trackLen += segmentLen;
492 if (segmentLen == 0) {
517 if (setSortingParams)
520 prevNewRefState = newRefState;
521 prevReferenceState = referenceState;
522 prevFitterInfo = fitterInfo;
523 prevSmoothedState = smoothedState;
531 if (smoothedState != NULL) {
533 debugOut <<
"construct plane with smoothedState \n";
536 else if (prevSmoothedState != NULL) {
538 debugOut <<
"construct plane with prevSmoothedState \n";
543 else if (prevReferenceState != NULL) {
545 debugOut <<
"construct plane with prevReferenceState \n";
554 debugOut <<
"construct plane with smoothed state of cardinal rep fit \n";
566 debugOut <<
"construct plane with state from track \n";
573 if (plane.get() == NULL) {
574 Exception exc(
"KalmanFitterRefTrack::prepareTrack ==> construced plane is NULL!",__LINE__,__FILE__);
582 boost::scoped_ptr<StateOnPlane> stateToExtrapolate(NULL);
583 if (prevFitterInfo == NULL) {
585 debugOut <<
"prevFitterInfo == NULL \n";
587 if (smoothedState != NULL) {
589 debugOut <<
"extrapolate smoothedState to plane\n";
591 stateToExtrapolate.reset(
new StateOnPlane(*smoothedState));
593 else if (referenceState != NULL) {
595 debugOut <<
"extrapolate referenceState to plane\n";
597 stateToExtrapolate.reset(
new StateOnPlane(*referenceState));
604 debugOut <<
"extrapolate smoothed state of cardinal rep fit to plane\n";
608 stateToExtrapolate.reset(
new StateOnPlane(fittedState));
612 debugOut <<
"extrapolate seed from track to plane\n";
620 if (prevSmoothedState != NULL) {
622 debugOut <<
"extrapolate prevSmoothedState to plane \n";
624 stateToExtrapolate.reset(
new StateOnPlane(*prevSmoothedState));
627 assert (prevReferenceState != NULL);
629 debugOut <<
"extrapolate prevReferenceState to plane \n";
631 stateToExtrapolate.reset(
new StateOnPlane(*prevReferenceState));
636 if (prevReferenceState != NULL)
640 if (prevFitterInfo != NULL) {
643 debugOut <<
"extrapolated stateToExtrapolate to plane of prevFitterInfo (plane could have changed!) \n";
648 trackLen += segmentLen;
650 debugOut <<
"extrapolated stateToExtrapolate by " << segmentLen <<
" cm.\t";
651 debugOut <<
"charge of stateToExtrapolate: " << rep->
getCharge(*stateToExtrapolate) <<
" \n";
655 if (segmentLen == 0) {
675 if (setSortingParams)
680 if (prevReferenceState != NULL) {
692 stateToExtrapolate->getPlane(),
693 stateToExtrapolate->getRep(),
694 stateToExtrapolate->getAuxInfo());
706 std::vector<double> oldWeights = fitterInfo->
getWeights();
709 const std::vector<AbsMeasurement*>& rawMeasurements = trackPoint->
getRawMeasurements();
710 for ( std::vector< genfit::AbsMeasurement* >::const_iterator measurement = rawMeasurements.begin(), lastMeasurement = rawMeasurements.end(); measurement != lastMeasurement; ++measurement) {
711 assert((*measurement) != NULL);
721 prevNewRefState = newRefState;
722 prevReferenceState = referenceState;
723 prevFitterInfo = fitterInfo;
724 prevSmoothedState = smoothedState;
730 errorOut <<
"exception at hit " << i <<
"\n";
737 prevNewRefState =
true;
738 referenceState = NULL;
739 smoothedState = NULL;
742 if (setSortingParams)
746 debugOut <<
"There was an exception, try to continue with next TrackPoint " << i+1 <<
" \n";
757 for (; i<nPoints; ++i) {
760 if (setSortingParams)
780 debugOut <<
"set backwards update of first point as forward prediction (with blown up cov) \n";
782 if (fi->
getPlane() != firstBackwardUpdate->getPlane()) {
791 if (fitStatus != NULL)
795 debugOut <<
"trackLen of reference track = " << trackLen <<
"\n";
810 debugOut <<
"KalmanFitterRefTrack::removeOutdated \n";
813 bool changedSmthg(
false);
816 notChangedUntil = nPoints-1;
820 for (
unsigned int i=0; i<nPoints; ++i) {
827 debugOut <<
"TrackPoint has no rawMeasurements -> continue \n";
835 fitterInfo = dynamic_cast<KalmanFitterInfo*>(trackPoint->
getFitterInfo(rep));
837 if (fitterInfo == NULL)
846 debugOut <<
"reference state but not all predictions & updates -> do not touch reference state. \n";
859 double* resArray =
resM_.GetMatrixArray();
860 for (
int j=0; j<smoothedState.
getCov().GetNcols(); ++j)
861 chi2 += resArray[j]*resArray[j] / smoothedState.
getCov()(j,j);
866 debugOut <<
"reference state is near smoothed state -> do not update reference state, chi2 = " << chi2 <<
"\n";
871 debugOut <<
"reference state is not close to smoothed state, chi2 = " << chi2 <<
"\n";
877 debugOut <<
"remove reference info \n";
884 if (notChangedUntil == (
int)nPoints-1)
885 notChangedUntil = i-1;
887 notChangedFrom = i+1;
912 if (notChangedUntil != (
int)nPoints-1) {
915 if (notChangedFrom != 0)
939 if (prevFi != NULL) {
941 assert(F.GetNcols() == (int)dim);
954 debugOut <<
"F (Transport Matrix) "; F.Print();
963 debugOut <<
" Use prediction as start \n";
970 debugOut <<
" Use reference state and seed cov as start \n";
976 TMatrixDSym dummy(
p_.GetNrows());
993 debugOut <<
" p_{k|k-1} (state prediction)";
p_.Print();
994 debugOut <<
" C_{k|k-1} (covariance prediction)";
C_.Print();
1001 const std::vector<MeasurementOnPlane *> measurements =
getMeasurements(fi, tp, direction);
1002 for (std::vector<MeasurementOnPlane *>::const_iterator it = measurements.begin(); it != measurements.end(); ++it) {
1007 debugOut <<
"Weight of measurement is almost 0, continue ... /n";
1025 const TMatrixD& CHt(H->MHt(
C_));
1050 debugOut <<
" p_{k|k} (updated state)";
p_.Print();
1051 debugOut <<
" C_{k|k} (updated covariance)";
C_.Print();
1073 bool couldInvert(
true);
1081 couldInvert =
false;
1113 debugOut <<
" chi² inc " << chi2inc <<
"\t";
1114 debugOut <<
" ndf inc " << ndfInc <<
"\t";
1128 const TrackPoint* tp,
double& chi2,
double& ndf,
int direction)
1139 TMatrixD S(dim, dim);
1142 if (prevFi != NULL) {
1144 assert(F.GetNcols() == (int)dim);
1155 decompS.Decompose();
1163 debugOut <<
"F (Transport Matrix) "; F.Print();
1172 debugOut <<
" Use prediction as start \n";
1176 decompS.Decompose();
1181 debugOut <<
" Use reference state and seed cov as start \n";
1187 TMatrixDSym dummy(
p_.GetNrows());
1195 TDecompChol decompS(mop.getCov());
1196 decompS.Decompose();
1206 debugOut <<
" p_{k|k-1} (state prediction)";
p_.Print();
1207 debugOut <<
" C_{k|k-1} (covariance prediction)";
C_.Print();
1215 const std::vector<MeasurementOnPlane *> measurements =
getMeasurements(fi, tp, direction);
1216 for (std::vector<MeasurementOnPlane *>::const_iterator it = measurements.begin(); it != measurements.end(); ++it) {
1221 debugOut <<
"Weight of measurement is almost 0, continue ... /n";
1231 TDecompChol decompR(V);
1232 decompR.Decompose();
1233 const TMatrixD& R(decompR.GetU());
1254 debugOut <<
" update"; update.Print();
1261 debugOut <<
" p_{k|k} (updated state)";
p_.Print();
1262 debugOut <<
" C_{k|k} (updated covariance)";
C_.Print();
1268 res_ -= H->Hv(update);
1278 Rinv_ = V - TMatrixDSym(TMatrixDSym::kAtA, H->MHt(S));
1280 bool couldInvert(
true);
1288 couldInvert =
false;
1309 C_ = TMatrixDSym(TMatrixDSym::kAtA, S);
1321 debugOut <<
" chi² inc " << chi2inc <<
"\t";
1322 debugOut <<
" ndf inc " << ndfInc <<
"\t";
double blowUpMaxVal_
Limit the cov entries to this maxuimum value when blowing up the cov.
TVectorD backwardDeltaState_
void setBackwardDeltaState(const TVectorD &mat)
const AbsHMatrix * getHMatrix() const
const AbsTrackRep * getRep() const
TrackPoint * getPointWithMeasurementAndFitterInfo(int id, const AbsTrackRep *rep=NULL) const
void deleteReferenceInfo()
void setWeights(const std::vector< double > &)
Set weights of measurements.
bool areWeightsFixed() const
Are the weights fixed?
unsigned int maxIterations_
Maximum number of iterations to attempt. Forward and backward are counted as one iteration.
virtual void getForwardJacobianAndNoise(TMatrixD &jacobian, TMatrixDSym &noise, TVectorD &deltaState) const =0
Get the jacobian and noise matrix of the last extrapolation.
bool hasBackwardPrediction() const
void setTrackLen(double trackLen)
boost::shared_ptr< genfit::DetPlane > SharedPlanePtr
Shared Pointer to a DetPlane.
bool hasUpdate(int direction) const
virtual SharedPlanePtr constructPlane(const StateOnPlane &state) const =0
void setUpdate(KalmanFittedStateOnPlane *update, int direction)
void setPrediction(MeasuredStateOnPlane *prediction, int direction)
void setSortingParameter(double sortingParameter)
void processTrackPointSqrt(KalmanFitterInfo *fi, const KalmanFitterInfo *prevFi, const TrackPoint *tp, double &chi2, double &ndf, int direction)
void deleteBackwardInfo(int startId=0, int endId=-1, const AbsTrackRep *rep=NULL)
virtual bool checkConsistency(const genfit::PruneFlags *=NULL) const
TVectorD forwardDeltaState_
AbsTrackRep * getCardinalRep() const
Get cardinal track representation.
const TMatrixD & getTransportMatrix(int direction) const
void setForwardSegmentLength(double len)
const TVectorD & getState() const
bool hasReferenceState() const
bool hasFitterInfo(const AbsTrackRep *rep) const
bool resetOffDiagonals_
Reset the off-diagonals to 0 when blowing up the cov.
virtual double extrapolateToPlane(StateOnPlane &state, const genfit::SharedPlanePtr &plane, bool stopAtBoundary=false, bool calcJacobianNoise=false) const =0
Extrapolates the state to plane, and returns the extrapolation length and, via reference, the extrapolated state.
void setBackwardNdf(double bNdf)
const TVectorD & getAuxInfo() const
bool prepareTrack(Track *tr, const AbsTrackRep *rep, bool setSortingParams, int &nFailedHits)
Prepare the track.
HMatrix for projecting from AbsTrackRep parameters to measured parameters in a DetPlane.
unsigned int getNumPoints() const
void processTrackPoint(KalmanFitterInfo *fi, const KalmanFitterInfo *prevFi, const TrackPoint *tp, double &chi2, double &ndf, int direction)
ReferenceStateOnPlane * getReferenceState() const
unsigned int minIterations_
Minimum number of iterations to attempt. Forward and backward are counted as one iteration.
void deleteMeasurementInfo()
void setAuxInfo(const TVectorD &auxInfo)
KalmanFittedStateOnPlane * getBackwardUpdate() const
double deltaPval_
Convergence criterion.
const std::vector< genfit::AbsMeasurement * > & getRawMeasurements() const
virtual void setPosMom(StateOnPlane &state, const TVector3 &pos, const TVector3 &mom) const =0
Set position and momentum of state.
void setIsFittedWithReferenceTrack(bool fittedWithReferenceTrack=true)
void deleteForwardInfo(int startId=0, int endId=-1, const AbsTrackRep *rep=NULL)
void setNumIterations(unsigned int numIterations)
void setForwardPrediction(MeasuredStateOnPlane *forwardPrediction)
unsigned int getNumMeasurements() const
void setBackwardNoiseMatrix(const TMatrixDSym &mat)
double getTimeSeed() const
void setNFailedPoints(int nFailedPoints)
int getIdForRep(const AbsTrackRep *rep) const
This is used when streaming TrackPoints.
const SharedPlanePtr & getPlane() const
void setForwardTransportMatrix(const TMatrixD &mat)
const TMatrixDSym & getCovSeed() const
AbsMeasurement * getRawMeasurement(int i=0) const
TMatrixDSym FNoiseMatrix_
MeasuredStateOnPlane with additional info produced by a Kalman filter or DAF.
bool squareRootFormalism_
virtual void getBackwardJacobianAndNoise(TMatrixD &jacobian, TMatrixDSym &noise, TVectorD &deltaState) const =0
Get the jacobian and noise matrix of the last extrapolation if it would have been done in opposite di...
void setBackwardSegmentLength(double len)
StateOnPlane with linearized transport to that ReferenceStateOnPlane from previous and next Reference...
unsigned int getNumPointsWithMeasurement() const
bool canIgnoreWeights() const
returns if the fitter can ignore the weights and handle the MeasurementOnPlanes as if they had weight...
void setIsFitted(bool fitted=true)
bool removeOutdated(Track *tr, const AbsTrackRep *rep, int ¬ChangedUntil, int ¬ChangedFrom)
Remove referenceStates if they are too far from smoothed states.
StateOnPlane with additional covariance matrix.
virtual void setTime(StateOnPlane &state, double time) const =0
Set time at which the state was defined.
TMatrixD FTransportMatrix_
void setBackwardChi2(double bChi2)
TrackPoint * getPoint(int id) const
bool sort()
Sort TrackPoint and according to their sorting parameters.
void addMeasurementsOnPlane(const std::vector< genfit::MeasurementOnPlane * > &measurementsOnPlane)
const TMatrixDSym & getNoiseMatrix(int direction) const
virtual void Print(const Option_t *="") const
virtual const char * what() const
Standard error message handling for exceptions. use like "std::cerr << e.what();".
void setFatal(bool b=true)
Set fatal flag.
void setBackwardTransportMatrix(const TMatrixD &mat)
void setReferenceState(ReferenceStateOnPlane *referenceState)
virtual void setQop(StateOnPlane &state, double qop) const =0
Set charge/momentum.
Abstract base class for a track representation.
bool hasPredictionsAndUpdates() const
TrackPoint * fitTrack(Track *tr, const AbsTrackRep *rep, double &chi2, double &ndf, int direction)
Fit the track.
Collects information needed and produced by a AbsKalmanFitter implementations and is specific to one ...
virtual void getPosMom(const StateOnPlane &state, TVector3 &pos, TVector3 &mom) const =0
Get cartesian position and momentum vector of a state.
virtual bool hasPrediction(int direction) const
bool isTrackPruned() const
Has the track been pruned after the fit?
void fixWeights(bool arg=true)
TMatrixD BTransportMatrix_
Exception class for error handling in GENFIT (provides storage for diagnostic information) ...
void setForwardNdf(double fNdf)
std::vector< double > getWeights() const
Get weights of measurements.
MeasuredStateOnPlane * getBackwardPrediction() const
Object containing AbsMeasurement and AbsFitterInfo objects.
bool isTrackPrepared(const Track *tr, const AbsTrackRep *rep) const
double blowUpFactor_
Blow up the covariance of the forward (backward) fit by this factor before seeding the backward (forw...
const TVectorD & getDeltaState(int direction) const
void setForwardDeltaState(const TVectorD &mat)
bool hasForwardPrediction() const
KalmanFittedStateOnPlane * getForwardUpdate() const
void blowUpCov(double blowUpFac, bool resetOffDiagonals=true, double maxVal=-1.)
Blow up covariance matrix with blowUpFac. Per default, off diagonals are reset to 0 and the maximum v...
double getChiSquareIncrement() const
void setCharge(double charge)
void setForwardNoiseMatrix(const TMatrixDSym &mat)
void processTrackWithRep(Track *tr, const AbsTrackRep *rep, bool resortHits=false)
virtual void setPosMomCov(MeasuredStateOnPlane &state, const TVector3 &pos, const TVector3 &mom, const TMatrixDSym &cov6x6) const =0
Set position, momentum and covariance of state.
const SharedPlanePtr & getPlane() const
A state with arbitrary dimension defined in a DetPlane.
bool hasBackwardUpdate() const
KalmanFittedStateOnPlane * getUpdate(int direction) const
TrackPoint * getPointWithMeasurement(int id) const
FitStatus for use with AbsKalmanFitter implementations.
void setHasTrackChanged(bool trackChanged=true)
bool hasRawMeasurements() const
MeasuredStateOnPlane * getPrediction(int direction) const
const AbsTrackRep * getRep() const
const std::vector< MeasurementOnPlane * > getMeasurements(const KalmanFitterInfo *fi, const TrackPoint *tp, int direction) const
get the measurementsOnPlane taking the multipleMeasurementHandling_ into account
Collection of TrackPoint objects, AbsTrackRep objects and FitStatus objects.
const TrackPoint * getTrackPoint() const
void setBackwardPrediction(MeasuredStateOnPlane *backwardPrediction)
TMatrixDSym BNoiseMatrix_
Measured coordinates on a plane.
virtual double getQop(const StateOnPlane &state) const =0
Get charge over momentum.
void setIsFitConvergedPartially(bool fitConverged=true)
const TMatrixDSym & getCov() const
const MeasuredStateOnPlane & getFittedState(bool biased=true) const
Get unbiased or biased (default) smoothed state.
virtual double getCharge(const StateOnPlane &state) const =0
Get the (fitted) charge of a state. This is not always equal the pdg charge (e.g. if the charge sign ...
void removeForwardBackwardInfo(Track *tr, const AbsTrackRep *rep, int notChangedUntil, int notChangedFrom) const
If refitAll_, remove all information.
void Print(const Option_t *="") const
double getForwardSegmentLength() const
void setIsFitConvergedFully(bool fitConverged=true)
AbsFitterInfo * getFitterInfo(const AbsTrackRep *rep=NULL) const
Get fitterInfo for rep. Per default, use cardinal rep.
void setForwardChi2(double fChi2)
Defines for I/O streams used for error and debug printing.
void setFitStatus(FitStatus *fitStatus, const AbsTrackRep *rep)
void setFitterInfo(genfit::AbsFitterInfo *fitterInfo)
Takes Ownership.
virtual unsigned int getDim() const =0
Get the dimension of the state vector used by the track representation.
void deleteFitterInfo(const AbsTrackRep *rep)
const TVectorD & getStateSeed() const
FitStatus * getFitStatus(const AbsTrackRep *rep=NULL) const
Get FitStatus for a AbsTrackRep. Per default, return FitStatus for cardinalRep.