From cd179b4d43ee196a0dac01bb0fbabb51c0fa8e1c Mon Sep 17 00:00:00 2001
From: Sebastien Ponce <sebastien.ponce@cern.ch>
Date: Tue, 18 Jul 2017 13:03:35 +0200
Subject: [PATCH] Created HLT1Fitter to replace Master/VectorEventFitter
The intention is to have a simplified and streamlined fitter for the HLT case
---
Calo/CaloTools/src/L0Calo2CaloTool.h | 1 -
Tf/TrackSys/python/TrackSys/Configuration.py | 4 +-
.../python/TrackSys/RecoUpgradeTracking.py | 19 +-
Tr/TrackFitEvent/Event/FitNode.h | 8 +
Tr/TrackFitEvent/Event/SebFitNode.h | 195 +++
Tr/TrackFitEvent/src/FitNode.cpp | 140 ++-
Tr/TrackFitEvent/src/SebFitnode.cpp | 275 ++++
.../python/TrackFitter/ConfiguredFitters.py | 12 +-
Tr/TrackFitter/src/HLT1Fitter.cpp | 1107 +++++++++++++++++
Tr/TrackFitter/src/HLT1Fitter.h | 157 +++
Tr/TrackFitter/src/TrackMasterFitter.cpp | 4 +-
11 files changed, 1903 insertions(+), 19 deletions(-)
create mode 100644 Tr/TrackFitEvent/Event/SebFitNode.h
create mode 100644 Tr/TrackFitEvent/src/SebFitnode.cpp
create mode 100644 Tr/TrackFitter/src/HLT1Fitter.cpp
create mode 100644 Tr/TrackFitter/src/HLT1Fitter.h
diff --git a/Calo/CaloTools/src/L0Calo2CaloTool.h b/Calo/CaloTools/src/L0Calo2CaloTool.h
index 0b77aa0275d..e9692bf7785 100644
--- a/Calo/CaloTools/src/L0Calo2CaloTool.h
+++ b/Calo/CaloTools/src/L0Calo2CaloTool.h
@@ -13,7 +13,6 @@
#include "CaloInterfaces/ICaloClusterization.h"
#include "CaloDAQ/ICaloDataProvider.h"
-#include "CaloDAQ/ICaloEnergyFromRaw.h"
// forward declarations
class DeCalorimeter;
diff --git a/Tf/TrackSys/python/TrackSys/Configuration.py b/Tf/TrackSys/python/TrackSys/Configuration.py
index ab666863be5..a0c4561d49c 100755
--- a/Tf/TrackSys/python/TrackSys/Configuration.py
+++ b/Tf/TrackSys/python/TrackSys/Configuration.py
@@ -220,7 +220,9 @@ class TrackSys(LHCbConfigurableUser):
from TrackSys import RecoUpgradeTracking
defTr = self.DefaultTrackLocations
sType = "None"
- if "TrFast" in self.getProp("TrackingSequence"):
+ if "TrHlt" in self.getProp("TrackingSequence"):
+ defTr = RecoUpgradeTracking.RecoFastTrackingStage(simplifiedGeometryFit = self.simplifiedGeometry(), defTracks = defTr, useHltFitter=True)
+ elif "TrFast" in self.getProp("TrackingSequence"):
seq = GaudiSequencer("RecoTrFastSeq")
decoding_seq = GaudiSequencer("RecoDecodingSeq")
defTr = RecoUpgradeTracking.RecoFastTrackingStage(simplifiedGeometryFit = self.simplifiedGeometry(),
diff --git a/Tf/TrackSys/python/TrackSys/RecoUpgradeTracking.py b/Tf/TrackSys/python/TrackSys/RecoUpgradeTracking.py
index 30e3f0aa6b6..5131e15d874 100755
--- a/Tf/TrackSys/python/TrackSys/RecoUpgradeTracking.py
+++ b/Tf/TrackSys/python/TrackSys/RecoUpgradeTracking.py
@@ -166,7 +166,8 @@ def RecoForward(seqType = "Fast",
fit = True,
simplifiedGeometry = True,
output_tracks_fitted = "Rec/Track/FittedForward",
- tuning = 0):
+ tuning = 0,
+ useHltFitter=False):
from TrackFitter.ConfiguredFitters import ConfiguredMasterFitter
from Configurables import TrackUsedLHCbID
@@ -265,13 +266,18 @@ def RecoForward(seqType = "Fast",
forwardFitter.addTool(vectorFitter, name="Fitter")
algs.append(forwardFitter)
else:
- from Configurables import TrackEventFitter, TrackMasterFitter
+ from Configurables import TrackEventFitter, HLT1Fitter, TrackMasterFitter
from TrackFitter.ConfiguredFitters import ConfiguredMasterFitter
- forwardFitter = TrackEventFitter('ForwardFitterAlg'+seqType)
+ if useHltFitter:
+ forwardFitter = HLT1Fitter('ForwardFitterAlg'+seqType)
+ # the fitter is ourselves !
+ ConfiguredMasterFitter(forwardFitter, SimplifiedGeometry = simplifiedGeometry)
+ else:
+ forwardFitter = TrackEventFitter('ForwardFitterAlg'+seqType)
+ forwardFitter.addTool(TrackMasterFitter, name="Fitter")
+ ConfiguredMasterFitter( forwardFitter.Fitter, SimplifiedGeometry = simplifiedGeometry)
forwardFitter.TracksInContainer = output_tracks
forwardFitter.TracksOutContainer = output_tracks_fitted
- forwardFitter.addTool(TrackMasterFitter, name="Fitter")
- ConfiguredMasterFitter( forwardFitter.Fitter, SimplifiedGeometry = simplifiedGeometry)
algs.append( forwardFitter )
return algs
@@ -344,7 +350,8 @@ def RecoFastTrackingStage(defTracks = {}, sequence = None, simplifiedGeometryFit
output_tracks = defTracks["ForwardFast"]["Location"],
fit = fitForward,
simplifiedGeometry = simplifiedGeometryFit,
- output_tracks_fitted = defTracks["ForwardFastFitted"]["Location"])
+ output_tracks_fitted = defTracks["ForwardFastFitted"]["Location"],
+ useHltFitter=useHltFitter)
defTracks["ForwardFastFitted"]["BestUsed"] = True
### Do we have a different sequence for decoding?
diff --git a/Tr/TrackFitEvent/Event/FitNode.h b/Tr/TrackFitEvent/Event/FitNode.h
index 9d3d9551197..d93c951ebb7 100755
--- a/Tr/TrackFitEvent/Event/FitNode.h
+++ b/Tr/TrackFitEvent/Event/FitNode.h
@@ -165,6 +165,10 @@ namespace LHCb
/// retrieve the residual of the reference
double refResidual() const { return m_refResidual ; }
+ double chi2Seb() const {
+ return m_residual*m_residual/(m_errResidual*m_errResidual);
+ }
+
/// set the delta-energy
void setDeltaEnergy( double e) { m_deltaEnergy = e; }
@@ -247,6 +251,10 @@ namespace LHCb
resetFilterStatus(Backward,s) ;
}
+ public:
+ void computePredictedStateSeb( int direction , FitNode* prevNode, bool hasInfoUpstream) ;
+ void computeFilteredStateSeb( int direction, FitNode* prevNode, int nTrackParam ) ;
+ void computeBiSmoothedStateSeb(bool hasInfoUpstreamForward, bool hasInfoUpstreamBackWard) ;
private:
void computePredictedState( int direction ) ;
diff --git a/Tr/TrackFitEvent/Event/SebFitNode.h b/Tr/TrackFitEvent/Event/SebFitNode.h
new file mode 100644
index 00000000000..cc53277e609
--- /dev/null
+++ b/Tr/TrackFitEvent/Event/SebFitNode.h
@@ -0,0 +1,195 @@
+#ifndef TRACKFITEVENT_FITNODE_H
+#define TRACKFITEVENT_FITNODE_H 1
+
+// from TrackEvent
+#include "Event/State.h"
+#include "Event/Measurement.h"
+#include "Event/ChiSquare.h"
+#include "LHCbMath/ValueWithError.h"
+
+// From LHCbMath
+#include "LHCbMath/MatrixManip.h"
+
+namespace LHCb {
+
+ /**
+ * This File contains the declaration of the SebFitNode.
+ *
+ * A SebFitNode is a basket of objects at a given z position.
+ * The information inside the SebFitNode has to be sufficient
+ * to allow smoothing and refitting.
+ */
+
+ class SebFitNode final {
+ public:
+
+ /// enumerator for the type of Node
+ enum Type { Unknown, HitOnTrack, Outlier, Reference };
+
+ // important note: for the Forward fit, smoothed means
+ // 'classical'. for the backward fit, it means 'bidirectional'.
+ enum Direction {Forward=0, Backward=1} ;
+
+ /// Constructor from a z position and a location
+ SebFitNode(double zPos, LHCb::State::Location location = LHCb::State::LocationUnknown);
+
+ /// Constructor from a Measurement
+ SebFitNode(Measurement& meas);
+
+ /// set transport matrix
+ void setTransportMatrix(const Gaudi::TrackMatrix& transportMatrix);
+
+ /// set transport vector
+ void setTransportVector(const Gaudi::TrackVector& transportVector) {
+ m_transportVector = transportVector;
+ }
+
+ /// set noise matrix
+ void setNoiseMatrix(const Gaudi::TrackSymMatrix& noiseMatrix) {
+ m_noiseMatrix = noiseMatrix;
+ }
+
+ /// Update the reference vector
+ void setRefVector(const Gaudi::TrackVector& value) {
+ m_refVector.parameters() = value;
+ }
+
+ /// Sets the reference vector
+ void setRefVector(const LHCb::StateVector& value) { m_refVector = value; };
+
+ /// Retrieve the state
+ const LHCb::State& state() const { return m_state; };
+
+ /// retrieve the filtered state
+ const LHCb::State& filteredState(int direction) const {
+ return m_filteredState[direction];
+ }
+
+ /// Retrieve the residual
+ double residual() const;
+
+ /// Retrieve the error in the residual
+ double errResidual() const;
+
+ /// retrieve the total chi2 of the filter including this node
+ const LHCb::ChiSquare& totalChi2(int direction) const { return m_totalChi2[direction%2]; }
+
+ /// Update Signed doca (of ref-traj). For ST/velo this is equal to minus (ref)residual
+ void setDoca(double value) { m_doca = value; }
+
+ /// Update Unit vector perpendicular to state and measurement
+ void setPocaVector(const Gaudi::XYZVector& value) { m_pocaVector = value; }
+
+ /// Retrieve the reference to the measurement
+ Measurement & measurement() const { return *m_measurement; }
+
+ /// Set the measurement
+ void setMeasurement(LHCb::Measurement& meas);
+
+ /// Return the measure error squared
+ double errMeasure2() const {
+ return m_errMeasure2;
+ }
+
+ /// set the measure error squared
+ void setErrMeasure2(double value) {
+ m_errMeasure2 = value;
+ }
+
+ /// Return true if this Node has a valid pointer to measurement
+ bool hasMeasurement() const { return (m_measurement != 0); }
+
+ /// Remove measurement from the node
+ void removeMeasurement();
+
+ /// z position of Node
+ double z() const { return m_refVector.z(); }
+
+ /// Position of the State on the Node
+ Gaudi::XYZPoint position() const;
+
+ /// Set the location of the state in this node.
+ void setLocation(LHCb::State::Location& location);
+
+ /// Retrieve const type of node
+ const Type& type() const { return m_type; };
+
+ /// Update type of node
+ void setType(const Type& value) { m_type = value; };
+
+ /// Retrieve const the reference vector
+ const LHCb::StateVector& refVector() const { return m_refVector; };
+
+ /// Retrieve const the projection matrix
+ const Gaudi::TrackProjectionMatrix& projectionMatrix() const {
+ return m_projectionMatrix;
+ };
+
+ /// set the seed matrix
+ void setSeedCovariance(const Gaudi::TrackSymMatrix& cov )
+ {
+ m_predictedState[Forward].covariance() = cov ;
+ m_predictedState[Backward].covariance() = cov ;
+ }
+
+ /// update the projection
+ void updateProjection( const Gaudi::TrackProjectionMatrix& H,
+ double refresidual) {
+ m_projectionMatrix = H;
+ m_refResidual = refresidual;
+ m_residual = 0;
+ m_errResidual = 0;
+ }
+
+ double chi2Seb() const {
+ return m_residual*m_residual/(m_errResidual*m_errResidual);
+ }
+
+ /// set the delta-energy
+ void setDeltaEnergy( double e) { m_deltaEnergy = e; }
+
+ /// get the delta-energy
+ double deltaEnergy() const { return m_deltaEnergy; }
+
+ /// update node residual using a smoothed state
+ Gaudi::Math::ValueWithError computeResidual(const LHCb::State& state, bool biased) const ;
+
+ void computePredictedStateSeb( int direction , SebFitNode* prevNode, bool hasInfoUpstream) ;
+ void computeFilteredStateSeb( int direction, SebFitNode* prevNode, int nTrackParam ) ;
+ void computeBiSmoothedStateSeb(bool hasInfoUpstreamForward, bool hasInfoUpstreamBackWard) ;
+
+ private:
+
+ // ! check that the contents of the cov matrix are fine
+ bool isPositiveDiagonal( const Gaudi::TrackSymMatrix& mat ) const;
+
+ /// update node residual using a smoothed state
+ void updateResidual(const LHCb::State& state) ;
+
+ Type m_type; ///< type of node
+ LHCb::State m_state; ///< state
+ LHCb::StateVector m_refVector; ///< the reference vector
+ LHCb::Measurement* m_measurement; ///< pointer to the measurement (not owner)
+ double m_residual {0.0}; ///< the residual value
+ double m_errResidual {0.0}; ///< the residual error
+ double m_errMeasure2 {0.0}; ///< square root of the measurement error
+ Gaudi::TrackProjectionMatrix m_projectionMatrix; ///< the projection matrix
+ Gaudi::TrackMatrix m_transportMatrix; ///< transport matrix for propagation from previous node to this one
+ Gaudi::TrackMatrix m_invertTransportMatrix; ///< transport matrix for propagation from this node to the previous one
+ Gaudi::TrackVector m_transportVector; ///< transport vector for propagation from previous node to this one
+ Gaudi::TrackSymMatrix m_noiseMatrix; ///< noise in propagation from previous node to this one
+ double m_deltaEnergy = 0; ///< change in energy in propagation from previous node to this one
+ double m_refResidual = 0; ///< residual of the reference
+ State m_predictedState[2]; ///< predicted state of forward/backward filter
+ State m_filteredState[2]; ///< filtered state of forward/backward filter
+ LHCb::ChiSquare m_totalChi2[2]; ///< total chi2 after this filterstep
+ Gaudi::TrackMatrix m_smootherGainMatrix ; ///< smoother gain matrix (smoothedfit only)
+ double m_doca; ///< Signed doca (of ref-traj). For ST/velo this is equal to minus (ref)residual
+ Gaudi::XYZVector m_pocaVector; ///< Unit vector perpendicular to state and measurement
+
+ };
+
+} // namespace LHCb
+
+#endif // TRACKFITEVENT_FITNODE_H
+
diff --git a/Tr/TrackFitEvent/src/FitNode.cpp b/Tr/TrackFitEvent/src/FitNode.cpp
index df259817ee4..156ed9afe59 100644
--- a/Tr/TrackFitEvent/src/FitNode.cpp
+++ b/Tr/TrackFitEvent/src/FitNode.cpp
@@ -293,7 +293,7 @@ namespace LHCb {
stateVec = F * previousState.stateVector() + transportVector() ;
transportcovariance(F,previousState.covariance(),stateCov) ;
stateCov += this->noiseMatrix();
- } else {
+ } else {
const TrackMatrix& invF = prevnode->invertTransportMatrix();
TrackSymMatrix tempCov ;
tempCov = previousState.covariance() + prevnode->noiseMatrix();
@@ -318,6 +318,60 @@ namespace LHCb {
}
}
+ //=========================================================================
+ // Predict the state to this node
+ //=========================================================================
+ void FitNode::computePredictedStateSeb(int direction, FitNode* prevNode, bool hasInfoUpstream)
+ {
+ // get the filtered state from the previous node. if there wasn't
+ // any, we will want to copy the reference vector and leave the
+ // covariance the way it is
+ m_predictedState[direction].setZ( z() );
+ TrackVector& stateVec = m_predictedState[direction].stateVector();
+ TrackSymMatrix& stateCov = m_predictedState[direction].covariance();
+ // start by copying the refvector. that's always the starting point
+ stateVec = refVector().parameters();
+ if (prevNode) {
+ const LHCb::State& previousState = prevNode->m_filteredState[direction];
+ if (!hasInfoUpstream) {
+ // just _copy_ the covariance matrix from upstream, assuming
+ // that this is the seed matrix. (that saves us from copying
+ // the seed matrix to every state from the start.
+ stateCov = previousState.covariance();
+ // new: start the backward filter from the forward filter
+ if (direction==Backward) {
+ stateVec = m_filteredState[Forward].stateVector();
+ }
+ } else {
+ if (direction==Forward) {
+ const TrackMatrix& F = transportMatrix();
+ stateVec = F * previousState.stateVector() + transportVector();
+ transportcovariance(F,previousState.covariance(),stateCov);
+ stateCov += noiseMatrix();
+ } else {
+ const TrackMatrix& invF = prevNode->invertTransportMatrix();
+ TrackSymMatrix tempCov;
+ tempCov = previousState.covariance() + prevNode->noiseMatrix();
+ stateVec = invF * ( previousState.stateVector() - prevNode->transportVector());
+ transportcovariance(invF,tempCov,stateCov);
+ }
+ }
+ } else {
+ if (!(stateCov(0,0) > 0)) {
+ throw std::string("Error in predict ") +
+ (direction==Forward ? "forward" : "backward") +
+ " function: seed covariance is not set!";
+ }
+ }
+ // update the status flag
+ m_filterStatus[direction] = Predicted;
+
+ if (!(m_predictedState[direction].covariance()(0,0) > 0)) {
+ throw std::string("Error in predict ") +
+ (direction==Forward ? "forward" : "backward") +
+ " function: something goes wrong in the prediction";
+ }
+ }
//=========================================================================
// Filter this node
@@ -348,11 +402,9 @@ namespace LHCb {
this->projectionMatrix(),
this->refResidual(),
this->errMeasure2() );
-
// set the chisquare contribution
m_deltaChi2[direction] = LHCb::ChiSquare(chi2,1);
}
-
m_totalChi2[direction] += m_deltaChi2[direction] ;
m_filterStatus[direction] = Filtered ;
@@ -364,6 +416,43 @@ namespace LHCb {
}
}
+ //=========================================================================
+ // Filter this node
+ //=========================================================================
+ void FitNode::computeFilteredStateSeb(int direction , FitNode* pn, int nTrackParam) {
+ // get a reference on the filtered state
+ LHCb::State& state = m_filteredState[direction];
+ // copy the predicted state
+ state = predictedState(direction);
+ m_totalChi2[direction] = pn ? pn->totalChi2(direction) : LHCb::ChiSquare(0,-nTrackParam);
+ // apply the filter if needed
+ if (type() == HitOnTrack) {
+ const TrackProjectionMatrix& H = this->projectionMatrix();
+ if (!( std::abs(H(0,0)) + std::abs(H(0,1))>0)) {
+ throw std::string("Error in filter ") +
+ (direction==Forward ? "forward" : "backward") +
+ " projection matrix is not set!";
+ }
+ double chi2 = LHCb::Math::Filter(state.stateVector(), state.covariance(),
+ refVector().parameters(),
+ projectionMatrix(),
+ refResidual(),
+ errMeasure2());
+ // set the chisquare contribution
+ m_deltaChi2[direction] = LHCb::ChiSquare(chi2,1);
+ } else {
+ m_deltaChi2[direction] = LHCb::ChiSquare();
+ }
+ m_totalChi2[direction] += m_deltaChi2[direction];
+ m_filterStatus[direction] = Filtered;
+ if (!(state.covariance()(0,0) > 0 &&
+ state.covariance()(1,1) > 0)) {
+ throw std::string("Error in filter ") +
+ (direction==Forward ? "forward" : "backward") +
+ " something goes wrong in the filtering";
+ }
+ }
+
//=========================================================================
// Bi-directional smoother
//=========================================================================
@@ -408,6 +497,51 @@ namespace LHCb {
m_filterStatus[Backward] = Smoothed ;
}
+ //=========================================================================
+ // Bi-directional smoother
+ //=========================================================================
+ void FitNode::computeBiSmoothedStateSeb(bool hasInfoUpstreamForward, bool hasInfoUpstreamBackward) {
+ LHCb::State& state = m_state;
+ if (!hasInfoUpstreamForward) {
+ // last node in backward direction
+ state = m_filteredState[Backward];
+ } else if (!hasInfoUpstreamBackward) {
+ // last node in forward direction
+ state = m_filteredState[Forward];
+ } else {
+ // Take the weighted average of two states. We now need to
+ // choose for which one we take the filtered state. AFAIU the
+ // weighted average behaves better if the weights are more
+ // equal. So, we filter the 'worst' prediction. In the end, it
+ // all doesn't seem to make much difference.
+ const LHCb::State *s1, *s2 ;
+ if (m_predictedState[Backward].covariance()(0,0) > m_predictedState[Forward].covariance()(0,0)) {
+ s1 = &(m_filteredState[Backward]);
+ s2 = &(m_predictedState[Forward]);
+ } else {
+ s1 = &(m_filteredState[Forward]);
+ s2 = &(m_predictedState[Backward]);
+ }
+ state.setZ(z()); // the disadvantage of having this information more than once
+ bool success = LHCb::Math::Average(s1->stateVector(), s1->covariance(),
+ s2->stateVector(), s2->covariance(),
+ state.stateVector(), state.covariance());
+ if (!success) {
+ throw std::string("Error in smooth function: error in matrix inversion");
+ }
+ }
+ if (!isPositiveDiagonal(state.covariance())) {
+ throw std::string("Error in smooth function: non positive diagonal element in coveriance matrix");
+ }
+ updateResidual(state);
+ m_filterStatus[Backward] = Smoothed;
+
+ // copy back state's vector into the Node's refVector. This was previouly done in a
+ // separate step and extra iteration at the MasterFitter level. In the new HLT1Fitter
+ // we count on this to be done here
+ setRefVector(state.stateVector());
+ }
+
//=========================================================================
// Classical smoother
//=========================================================================
diff --git a/Tr/TrackFitEvent/src/SebFitnode.cpp b/Tr/TrackFitEvent/src/SebFitnode.cpp
new file mode 100644
index 00000000000..a85cd9cfd64
--- /dev/null
+++ b/Tr/TrackFitEvent/src/SebFitnode.cpp
@@ -0,0 +1,275 @@
+// Include files
+
+// local
+#include "Event/SebFitNode.h"
+#include "Event/KalmanFitResult.h"
+#include "LHCbMath/Similarity.h"
+
+using namespace Gaudi;
+using namespace Gaudi::Math;
+using namespace LHCb;
+/** @file SebFitNode.cpp
+ *
+ * This File contains the implementation of the SebFitNode.
+ * A SebFitNode is a basket of objects at a given z position.
+ * The information inside the SebFitNode has to be sufficient
+ * to allow smoothing and refitting.
+ */
+
+namespace {
+
+ void transportcovariance( const Gaudi::TrackMatrix& F,
+ const Gaudi::TrackSymMatrix& origin,
+ Gaudi::TrackSymMatrix& target)
+ {
+ bool isLine = F(0,4)==0 ;
+ if ( LIKELY( !isLine ) ) {
+
+ // The temporary is actually important here. SMatrix is not
+ // computing A*B*C very optimally.
+ // static ROOT::Math::SMatrix<double, 5,5> FC ;
+ // FC = F*origin ;
+ // ROOT::Math::AssignSym::Evaluate(target,FC*Transpose(F)) ;
+
+ // use vectorized similarity transform
+ LHCb::Math::Similarity(F,origin,target);
+
+ } else {
+
+ target = origin;
+ target(0,0) += 2 * origin(2,0) * F(0,2) + origin(2,2) * F(0,2) * F(0,2);
+ target(2,0) += origin(2,2) * F(0,2) ;
+ target(1,1) += 2 * origin(3,1) * F(1,3) + origin(3,3) * F(1,3) * F(1,3);
+ target(3,1) += origin(3,3) * F(1,3);
+ target(1,0) += origin(2,1) * F(0,2) + origin(3,0) * F(1,3) + origin(3,2) * F(0,2) * F(1,3);
+ target(2,1) += origin(3,2) * F(1,3);
+ target(3,0) += origin(3,2) * F(0,2);
+
+ }
+ }
+
+ void inverttransport( const Gaudi::TrackMatrix& F,
+ Gaudi::TrackMatrix& Finv )
+ {
+ // it would save time if qw assume that Finv is either diagonal or filled with 0?
+ Finv = F ;
+ bool isLine = F(0,4)==0 ;
+ if( isLine ) {
+ Finv(0,2) = -F(0,2) ;
+ Finv(1,3) = -F(1,3) ;
+ } else {
+ //Finv(0,0) = Finv(1,1) = Finv(4,4) = 1 ;
+ // write
+ // ( 1 0 | S00 S01 | U0 )
+ // ( 0 1 | S10 S01 | U1 )
+ // F = ( 0 0 | T00 T01 | V0 )
+ // ( 0 0 | T10 T11 | V1 )
+ // ( 0 0 | 0 0 | 1 )
+ // then we have
+ // Tinv = T^{-1}
+ double det = F(2,2)*F(3,3)-F(2,3)*F(3,2) ;
+ Finv(2,2) = F(3,3)/det ;
+ Finv(3,3) = F(2,2)/det ;
+ Finv(2,3) = -F(2,3)/det ;
+ Finv(3,2) = -F(3,2)/det ;
+ // Vinv = - T^-1 * V
+ Finv(2,4) = -Finv(2,2)*F(2,4) -Finv(2,3)*F(3,4) ;
+ Finv(3,4) = -Finv(3,2)*F(2,4) -Finv(3,3)*F(3,4) ;
+ // Uinv = S * T^-1 * V - U = - S * Vinv - U
+ Finv(0,4) = -F(0,4) - F(0,2)*Finv(2,4) - F(0,3)*Finv(3,4) ;
+ Finv(1,4) = -F(1,4) - F(1,2)*Finv(2,4) - F(1, 3)*Finv(3,4) ;
+ // Sinv = - S * T^{-1}
+ Finv(0,2) = - F(0,2)*Finv(2,2) - F(0,3)*Finv(3,2) ;
+ Finv(0,3) = - F(0,2)*Finv(2,3) - F(0,3)*Finv(3,3) ;
+ Finv(1,2) = - F(1,2)*Finv(2,2) - F(1,3)*Finv(3,2) ;
+ Finv(1,3) = - F(1,2)*Finv(2,3) - F(1,3)*Finv(3,3) ;
+ }
+ }
+}
+
+namespace LHCb {
+
+ /// Constructor from a z position
+ /// eg. SebFitNode(StateParameters::ZEndVelo, State::EndVelo)
+ SebFitNode::SebFitNode( double zPos, LHCb::State::Location location ):
+ m_type(Reference),
+ m_state(location),
+ m_measurement(0) {
+ m_refVector.setZ(zPos) ;
+ m_predictedState[Forward].setLocation(location) ;
+ m_predictedState[Backward].setLocation(location) ;
+ }
+
+ /// Constructor from a Measurement
+ SebFitNode::SebFitNode(Measurement& aMeas) :
+ m_type(HitOnTrack),
+ m_measurement(&aMeas) {
+ m_refVector.setZ(aMeas.z());
+ }
+
+ void SebFitNode::setTransportMatrix( const Gaudi::TrackMatrix& transportMatrix ) {
+ m_transportMatrix = transportMatrix;
+ // invert the transport matrix. We could save some time by doing this on demand only.
+ inverttransport( m_transportMatrix, m_invertTransportMatrix ) ;
+ }
+
+ //=========================================================================
+ // Predict the state to this node
+ //=========================================================================
+ void SebFitNode::computePredictedStateSeb(int direction, SebFitNode* prevNode, bool hasInfoUpstream)
+ {
+ // get the filtered state from the previous node. if there wasn't
+ // any, we will want to copy the reference vector and leave the
+ // covariance the way it is
+ m_predictedState[direction].setZ( z() );
+ TrackVector& stateVec = m_predictedState[direction].stateVector();
+ TrackSymMatrix& stateCov = m_predictedState[direction].covariance();
+ // start by copying the refvector. that's always the starting point
+ stateVec = refVector().parameters();
+ if (prevNode) {
+ const LHCb::State& previousState = prevNode->m_filteredState[direction];
+ if (!hasInfoUpstream) {
+ // just _copy_ the covariance matrix from upstream, assuming
+ // that this is the seed matrix. (that saves us from copying
+ // the seed matrix to every state from the start.
+ stateCov = previousState.covariance();
+ // new: start the backward filter from the forward filter
+ if (direction==Backward) {
+ stateVec = m_filteredState[Forward].stateVector();
+ }
+ } else {
+ if (direction==Forward) {
+ const TrackMatrix& F = m_transportMatrix;
+ stateVec = F * previousState.stateVector() + m_transportVector;
+ transportcovariance(F,previousState.covariance(),stateCov);
+ stateCov += m_noiseMatrix;
+ } else {
+ const TrackMatrix& invF = prevNode->m_invertTransportMatrix;
+ TrackSymMatrix tempCov;
+ tempCov = previousState.covariance() + prevNode->m_noiseMatrix;
+ stateVec = invF * ( previousState.stateVector() - prevNode->m_transportVector);
+ transportcovariance(invF,tempCov,stateCov);
+ }
+ }
+ } else {
+ if (!(stateCov(0,0) > 0)) {
+ throw std::string("Error in predict ") +
+ (direction==Forward ? "forward" : "backward") +
+ " function: seed covariance is not set!";
+ }
+ }
+
+ if (!(m_predictedState[direction].covariance()(0,0) > 0)) {
+ throw std::string("Error in predict ") +
+ (direction==Forward ? "forward" : "backward") +
+ " function: something goes wrong in the prediction";
+ }
+ }
+
+ //=========================================================================
+ // Filter this node
+ //=========================================================================
+ void SebFitNode::computeFilteredStateSeb(int direction , SebFitNode* pn, int nTrackParam) {
+ // get a reference on the filtered state
+ LHCb::State& state = m_filteredState[direction];
+ // copy the predicted state
+ state = m_predictedState[direction];
+ m_totalChi2[direction] = pn ? pn->totalChi2(direction) : LHCb::ChiSquare(0,-nTrackParam);
+ // apply the filter if needed
+ if (type() == HitOnTrack) {
+ const TrackProjectionMatrix& H = this->projectionMatrix();
+ if (!( std::abs(H(0,0)) + std::abs(H(0,1))>0)) {
+ throw std::string("Error in filter ") +
+ (direction==Forward ? "forward" : "backward") +
+ " projection matrix is not set!";
+ }
+ double chi2 = LHCb::Math::Filter(state.stateVector(), state.covariance(),
+ refVector().parameters(),
+ projectionMatrix(),
+ m_refResidual,
+ errMeasure2());
+ // set the chisquare contribution
+ m_totalChi2[direction] += LHCb::ChiSquare(chi2,1);
+ } else {
+ m_totalChi2[direction] += LHCb::ChiSquare();
+ }
+ if (!(state.covariance()(0,0) > 0 &&
+ state.covariance()(1,1) > 0)) {
+ throw std::string("Error in filter ") +
+ (direction==Forward ? "forward" : "backward") +
+ " something goes wrong in the filtering";
+ }
+ }
+
+ //=========================================================================
+ // Bi-directional smoother
+ //=========================================================================
+ void SebFitNode::computeBiSmoothedStateSeb(bool hasInfoUpstreamForward, bool hasInfoUpstreamBackward) {
+ LHCb::State& state = m_state;
+ if (!hasInfoUpstreamForward) {
+ // last node in backward direction
+ state = m_filteredState[Backward];
+ } else if (!hasInfoUpstreamBackward) {
+ // last node in forward direction
+ state = m_filteredState[Forward];
+ } else {
+ // Take the weighted average of two states. We now need to
+ // choose for which one we take the filtered state. AFAIU the
+ // weighted average behaves better if the weights are more
+ // equal. So, we filter the 'worst' prediction. In the end, it
+ // all doesn't seem to make much difference.
+ const LHCb::State *s1, *s2 ;
+ if (m_predictedState[Backward].covariance()(0,0) > m_predictedState[Forward].covariance()(0,0)) {
+ s1 = &(m_filteredState[Backward]);
+ s2 = &(m_predictedState[Forward]);
+ } else {
+ s1 = &(m_filteredState[Forward]);
+ s2 = &(m_predictedState[Backward]);
+ }
+ state.setZ(z()); // the disadvantage of having this information more than once
+ bool success = LHCb::Math::Average(s1->stateVector(), s1->covariance(),
+ s2->stateVector(), s2->covariance(),
+ state.stateVector(), state.covariance());
+ if (!success) {
+ throw std::string("Error in smooth function: error in matrix inversion");
+ }
+ }
+ if (!isPositiveDiagonal(state.covariance())) {
+ throw std::string("Error in smooth function: non positive diagonal element in coveriance matrix");
+ }
+ updateResidual(state);
+
+ // copy back state's vector into the Node's refVector. This was previouly done in a
+ // separate step and extra iteration at the MasterFitter level. In the new HLT1Fitter
+ // we count on this to be done here
+ setRefVector(state.stateVector());
+ }
+
+ void SebFitNode::updateResidual( const LHCb::State& smoothedState ) {
+ Gaudi::Math::ValueWithError res;
+ double r(0),R(0) ;
+ if (hasMeasurement()) {
+ // We should put this inside the This->
+ const TrackProjectionMatrix& H = this->projectionMatrix();
+ const TrackVector& refX = this->refVector().parameters() ;
+ r = m_refResidual + (H*(refX - smoothedState.stateVector()))(0) ;
+ double V = errMeasure2();
+ double HCH = LHCb::Math::Similarity( H, smoothedState.covariance() );
+ double sign = type()==LHCb::SebFitNode::HitOnTrack ? -1 : 1 ;
+ R = V + sign * HCH;
+ if (R <= 0) {
+ throw std::string("Error in compute residual: non positive variance");
+ }
+ }
+ m_residual = r;
+ m_errResidual = std::sqrt(R);
+ }
+
+ inline bool SebFitNode::isPositiveDiagonal( const Gaudi::TrackSymMatrix& mat ) const
+ {
+ unsigned int i = 0u;
+ for ( ; i < Gaudi::TrackSymMatrix::kRows && mat(i,i) > 0.0 ; ++i ) {}
+ return i == Gaudi::TrackSymMatrix::kRows ;
+ }
+
+}
diff --git a/Tr/TrackFitter/python/TrackFitter/ConfiguredFitters.py b/Tr/TrackFitter/python/TrackFitter/ConfiguredFitters.py
index 8a5cddbab04..829143a3260 100755
--- a/Tr/TrackFitter/python/TrackFitter/ConfiguredFitters.py
+++ b/Tr/TrackFitter/python/TrackFitter/ConfiguredFitters.py
@@ -35,12 +35,12 @@ def ConfiguredMasterFitter( Name,
if KalmanSmoother is None: KalmanSmoother = TrackSys().kalmanSmoother()
if ApplyMaterialCorrections is None: ApplyMaterialCorrections = not TrackSys().noMaterialCorrections()
- if isinstance(Name, ConfigurableAlgTool) :
- fitter = Name
- else :
- if allConfigurables.get( Name ) :
- raise ValueError, 'ConfiguredMasterFitter: instance with name '+Name+' already exists'
- fitter = TrackMasterFitter(Name)
+ #if isinstance(Name, ConfigurableAlgTool) :
+ fitter = Name
+ #else :
+ # if allConfigurables.get( Name ) :
+ # raise ValueError, 'ConfiguredMasterFitter: instance with name '+Name+' already exists'
+ # fitter = TrackMasterFitter(Name)
# apply material corrections
if not ApplyMaterialCorrections:
diff --git a/Tr/TrackFitter/src/HLT1Fitter.cpp b/Tr/TrackFitter/src/HLT1Fitter.cpp
new file mode 100644
index 00000000000..57775013e51
--- /dev/null
+++ b/Tr/TrackFitter/src/HLT1Fitter.cpp
@@ -0,0 +1,1107 @@
+#ifdef _WIN32
+#pragma warning (disable : 4355) // This used in initializer list, needed for ToolHandles
+#endif
+
+#include <tuple>
+#include "HLT1Fitter.h"
+#include "Kernel/ParticleID.h"
+#include <range/v3/utility/any.hpp>
+#include "Event/OTMeasurement.h"
+#include "Event/ChiSquare.h"
+#include "TrackKernel/TrackTraj.h"
+#include "Event/StateParameters.h"
+#include "TrackInterfaces/ITrackProjector.h"
+#include "LHCbMath/LHCbMath.h"
+#include "TrackKernel/StateZTraj.h"
+#include <boost/range/adaptor/reversed.hpp>
+
+DECLARE_ALGORITHM_FACTORY(HLT1Fitter)
+
+/**
+ * Default Constructor
+ * Only defines deafult input and output paths in TES
+ */
+HLT1Fitter::HLT1Fitter(const std::string& name,
+ ISvcLocator* pSvcLocator) :
+Transformer(name, pSvcLocator,
+ KeyValue{"TracksInContainer", LHCb::TrackLocation::Default},
+ KeyValue{"TracksOutContainer", LHCb::TrackLocation::Default}) {
+ declareProperty("MeasProvider", m_measProvider);
+ declareProperty("Extrapolator", m_extrapolator);
+ declareProperty("VeloExtrapolator", m_veloExtrapolator);
+ declareProperty("MaterialLocator", m_materialLocator);
+}
+
+/**
+ * operator () : main method orchestrating the work
+ */
+LHCb::Tracks HLT1Fitter::operator()(const std::vector<LHCb::Track>& tracksCont) const {
+
+ LHCb::Tracks tracksNewCont;
+ ranges::v3::any cache = m_materialLocator->createCache();
+ unsigned int nFitFail = 0;
+ unsigned int nBadInput = 0;
+
+ // Loop over the tracks and fit them
+ for (const auto& iTrack : tracksCont) {
+ // If needed make a new track keeping the same key
+ LHCb::Track *track = new LHCb::Track(iTrack);
+ if (track->nStates()==0 || track->checkFlag(LHCb::Track::Invalid)) {
+ track->setFlag(LHCb::Track::Invalid, true);
+ // don't put failures on the output container. this is how they want it in HLT.
+ ++nBadInput;
+ } else {
+ double qopBefore = track->firstState().qOverP();
+ try {
+ unsigned int nbOutliers = fit(*track, cache);
+ std::string prefix = Gaudi::Utils::toString(track->type());
+ // Update counters
+ if (track->checkFlag(LHCb::Track::Backward)) prefix += "Backward";
+ prefix += '.';
+ if (track->nDoF()>0) {
+ double chisqprob = track->probChi2();
+ counter(prefix + "chisqprobSum") += chisqprob;
+ counter(prefix + "badChisq") += bool(chisqprob<0.01);
+ }
+ counter(prefix + "flipCharge") += bool(qopBefore * track->firstState().qOverP() <0);
+ counter(prefix + "numOutliers") += nbOutliers;
+ // Add the track to the new Tracks container if it passes the chi2-cut
+ if (m_maxChi2DoF > track->chi2PerDoF()) {
+ tracksNewCont.add(track);
+ } else {
+ delete track;
+ counter(prefix + "RejectedChisqCut") += 1;
+ }
+ } catch (const StatusCode &sc) {
+ track->setFlag(LHCb::Track::Invalid, true);
+ ++nFitFail;
+ }
+ }
+ } // loop over input Tracks
+
+ // Update counters
+ unsigned int nTracks = tracksCont.size();
+ counter("nTracks") += nTracks;
+ counter("nFitted") += (nTracks - nFitFail - nBadInput);
+ counter("nBadInput") += nBadInput;
+
+ return tracksNewCont;
+}
+
+/// finalize : reporting performance
+StatusCode HLT1Fitter::finalize() {
+ if (msgLevel(MSG::INFO)) {
+ float perf = 0.;
+ double nTracks = counter("nTracks").flag();
+ if (nTracks > 1e-3) {
+ perf = float(100.0*counter("nFitted").flag() / nTracks);
+ }
+ info() << " Fitting performance : " << format( " %7.2f %%", perf ) << endmsg;
+ }
+ // must be called after all other actions
+ return Gaudi::Functional::Transformer<LHCb::Tracks(const std::vector<LHCb::Track>&)>::finalize();
+}
+
+/**
+ * This routine calculates the chisquare contributions from
+ * different segments of the track. It uses the 'delta-chisquare'
+ * contributions from the bi-directional kalman fit. Summing these
+ * leads to a real chisquare only if the contributions are
+ * uncorrelated. For a Velo-TT-T track you can then calculate:
+ * - the chisuare of the T segment and the T-TT segment by using the
+ * 'upstream' contributions
+ * - the chisquare of the Velo segment and the Velo-TT segment by
+ * using the 'downstream' contributions
+ * Note that you cannot calculate the contribution of the TT segment
+ * seperately (unless there are no T or no Velo hits). Also, if
+ * there are Muon hits, you cannot calculate the T station part, so
+ * for now this only works for tracks without muon hits.
+ * @return a tuple with DownStream, Velo and Match chi2 in this order
+ */
+static inline std::array<LHCb::ChiSquare, 3>
+computeChiSquares(const std::vector<LHCb::SebFitNode> &nodes) {
+ if (nodes.empty()) {
+ return {LHCb::ChiSquare(), LHCb::ChiSquare(), LHCb::ChiSquare()};
+ } else {
+ LHCb::ChiSquare chi2Downstream, chi2Velo, chi2Muon, chi2Upstream, chi2;
+ auto firstVelo = nodes.end();
+ auto lastVelo = nodes.end();
+ auto firstTT = nodes.end();
+ auto lastTT = nodes.end();
+ auto firstT = nodes.end();
+ auto lastT = nodes.end();
+ auto firstMuon = nodes.end();
+ auto lastMuon = nodes.end();
+ for (auto it = nodes.cbegin(); it != nodes.cend(); it++) {
+ if (it->type() == LHCb::SebFitNode::HitOnTrack) {
+ switch (it->measurement().type()) {
+ case LHCb::Measurement::VP:
+ case LHCb::Measurement::VeloR:
+ case LHCb::Measurement::VeloPhi:
+ case LHCb::Measurement::VeloLiteR:
+ case LHCb::Measurement::VeloLitePhi:
+ case LHCb::Measurement::Origin:
+ if (firstVelo==nodes.end()) {
+ firstVelo = it;
+ }
+ lastVelo = it;
+ break;
+ case LHCb::Measurement::TT:
+ case LHCb::Measurement::TTLite:
+ case LHCb::Measurement::UT:
+ case LHCb::Measurement::UTLite:
+ if (firstTT==nodes.end()) {
+ firstTT = it;
+ }
+ lastTT = it;
+ break;
+ case LHCb::Measurement::OT:
+ case LHCb::Measurement::IT:
+ case LHCb::Measurement::ITLite:
+ case LHCb::Measurement::FT:
+ if (firstT==nodes.end()) {
+ firstT = it;
+ }
+ lastT = it;
+ break;
+ case LHCb::Measurement::Muon:
+ if (firstMuon==nodes.end()) {
+ firstMuon = it;
+ }
+ lastMuon = it;
+ break;
+ case LHCb::Measurement::Unknown:
+ case LHCb::Measurement::Calo:
+ default:
+ break;
+ }
+ }
+ }
+ bool upstream = nodes.front().z() > nodes.back().z();
+ if (firstMuon != nodes.end()) {
+ chi2Muon = upstream ? lastMuon->totalChi2(LHCb::SebFitNode::Forward) : firstMuon->totalChi2(LHCb::SebFitNode::Backward);
+ } else {
+ chi2Muon = LHCb::ChiSquare();
+ }
+ if (firstT != nodes.end()) {
+ chi2Downstream = upstream ? lastT->totalChi2(LHCb::SebFitNode::Forward) : firstT->totalChi2(LHCb::SebFitNode::Backward);
+ } else {
+ chi2Downstream = chi2Muon;
+ }
+ if (firstVelo != nodes.end()) {
+ chi2Velo = upstream ? firstVelo->totalChi2(LHCb::SebFitNode::Backward) : lastVelo->totalChi2(LHCb::SebFitNode::Forward);
+ } else {
+ chi2Velo = LHCb::ChiSquare();
+ }
+
+ if (firstTT != nodes.end()) {
+ chi2Upstream = upstream ? firstTT->totalChi2(LHCb::SebFitNode::Backward) : lastTT->totalChi2(LHCb::SebFitNode::Forward);
+ } else {
+ chi2Upstream = chi2Velo;
+ }
+ const LHCb::ChiSquare& chi2A = nodes.front().totalChi2(LHCb::SebFitNode::Backward);
+ const LHCb::ChiSquare& chi2B = nodes.back().totalChi2(LHCb::SebFitNode::Forward);
+ chi2 = (chi2A.chi2() > chi2B.chi2()) ? chi2A : chi2B;
+ return {chi2Downstream, chi2Velo, chi2-chi2Upstream-chi2Downstream};
+ }
+}
+
+static inline unsigned int countOTMesurements(const std::vector<LHCb::Measurement*> &measurements) {
+ return std::count_if(measurements.begin(), measurements.end(),
+ [&](const LHCb::Measurement* m) {
+ return m->type() == LHCb::Measurement::OT;
+ });
+}
+
+static inline unsigned int computeActiveOTTime(const std::vector<LHCb::SebFitNode> &nodes) {
+ return std::count_if
+ (nodes.cbegin(), nodes.cend(),
+ [](const LHCb::SebFitNode& node) {
+ if (node.type() != LHCb::SebFitNode::HitOnTrack ||
+ node.measurement().type() != LHCb::Measurement::OT) return false;
+ const auto& ot = static_cast<const LHCb::OTMeasurement&>(node.measurement());
+ return ot.driftTimeStrategy() == LHCb::OTMeasurement::FitDistance ||
+ ot.driftTimeStrategy() == LHCb::OTMeasurement::FitTime;
+ });
+}
+
+/**
+ * Add info from fit as extrainfo to track
+ */
+static inline void fillExtraInfo(LHCb::Track& track,
+ const std::vector<LHCb::SebFitNode>& nodes,
+ const std::vector<LHCb::Measurement*>& measurements,
+ const LHCb::Track::Types trackType) {
+ // Clean up the track info
+ track.eraseInfo(LHCb::Track::FitVeloChi2);
+ track.eraseInfo(LHCb::Track::FitVeloNDoF);
+ track.eraseInfo(LHCb::Track::FitTChi2);
+ track.eraseInfo(LHCb::Track::FitTNDoF);
+ track.eraseInfo(LHCb::Track::FitMatchChi2);
+ track.eraseInfo(LHCb::Track::FitFracUsedOTTimes);
+
+ // get chi2s in this order : downstream, Velo, Match
+ std::array<LHCb::ChiSquare, 3> chi2s = computeChiSquares(nodes);
+
+ // Case of T track
+ if (trackType == LHCb::Track::Types::Ttrack ||
+ trackType == LHCb::Track::Types::Downstream ||
+ trackType == LHCb::Track::Types::Long) {
+ // Downstream chi2
+ track.addInfo(LHCb::Track::FitTChi2, chi2s[0].chi2());
+ track.addInfo(LHCb::Track::FitTNDoF, chi2s[0].nDoF());
+ unsigned int nOTMeas = countOTMesurements(measurements);
+ if (nOTMeas > 0) {
+ unsigned int actirOTTime = computeActiveOTTime(nodes);
+ track.addInfo(LHCb::Track::FitFracUsedOTTimes, actirOTTime / double(nOTMeas));
+ }
+ }
+
+ // Case of Velo track
+ if (trackType == LHCb::Track::Types::Velo ||
+ trackType == LHCb::Track::Types::VeloR ||
+ trackType == LHCb::Track::Types::Upstream ||
+ trackType == LHCb::Track::Types::Long) {
+ // Velo chi2
+ track.addInfo(LHCb::Track::FitVeloChi2, chi2s[1].chi2());
+ track.addInfo(LHCb::Track::FitVeloNDoF, chi2s[1].nDoF());
+ // case of Velo and T track, i.e Long track
+ if (trackType == LHCb::Track::Types::Long) {
+ // Match chi2
+ track.addInfo(LHCb::Track::FitMatchChi2, chi2s[2].chi2());
+ }
+ }
+}
+
+unsigned int HLT1Fitter::fit(LHCb::Track& track, ranges::v3::any& accelCache) const {
+ // any track that doesnt make it to the end is failed
+ track.setFitStatus(LHCb::Track::FitFailed);
+
+ // Store results of the Kalman fit
+ std::vector<LHCb::SebFitNode> nodes;
+ std::vector<LHCb::Measurement*> measurements;
+ int nTrackParameters;
+
+ // Make the nodes and measurements
+ try {
+ nodes = makeNodes(track, accelCache, measurements);
+ // create the transport of the reference (after the noise, because it uses energy loss)
+ nTrackParameters = updateTransport(nodes, track.type());
+ } catch (const StatusCode& sc) {
+ debug() << "unable to make nodes from the measurements" << endmsg;
+ throw sc;
+ }
+
+ // create a covariance matrix to seed the Kalman fit
+ Gaudi::TrackSymMatrix seedCov; // Set off-diagonal elements to zero
+ seedCov(0,0) = m_errorX*m_errorX;
+ seedCov(1,1) = m_errorY*m_errorY;
+ seedCov(2,2) = m_errorTx*m_errorTx;
+ seedCov(3,3) = m_errorTy*m_errorTy;
+ seedCov(4,4) = std::pow(m_errorQoP[0] * track.firstState().qOverP(), 2) + std::pow(m_errorQoP[1],2);
+ nodes.front().setSeedCovariance(seedCov);
+ nodes.back().setSeedCovariance(seedCov);
+
+ // Iterate the track fit for linearisation. Be careful with chi2
+ // convergence here: The first iteration might not be using OT
+ // drifttimes in which case the chi2 can actually go up in the 2nd
+ // iteration.
+ bool converged = false;
+ LHCb::SebFitNode *firstOnTrackNode = nullptr;
+ LHCb::SebFitNode *lastOnTrackNode = nullptr;
+ for (int iter = 1; iter <= m_numFitIter && !converged; ++iter) {
+ // update reference trajectories with smoothed states
+ if (iter > 1) {
+ try {
+ // update the projections. need to be done every time ref is updated
+ updateRefVectors(nodes, track.type(), firstOnTrackNode, lastOnTrackNode);
+ } catch (const StatusCode &sc) {
+ debug() << "problem updating ref vectors" << endmsg;
+ throw sc;
+ }
+ }
+ auto prevchi2 = track.chi2();
+ try {
+ std::tuple<double, int> chi2AndNDoF = fitIteration(nodes, nTrackParameters, seedCov, &firstOnTrackNode, &lastOnTrackNode);
+ auto dchi2 = prevchi2 - std::get<0>(chi2AndNDoF);
+ // require at least 3 iterations, because of the OT prefit.
+ converged = ((iter>1) && std::abs(dchi2) < m_maxDeltaChi2Converged * std::get<1>(chi2AndNDoF));
+ track.setChi2AndDoF(std::get<0>(chi2AndNDoF), std::get<1>(chi2AndNDoF));
+ } catch (const std::string& errorMsg) {
+ debug() << std::string("unable to fit the track") << errorMsg << endmsg;
+ throw StatusCode::FAILURE;
+ }
+ }
+
+ // Outlier removal iterations
+ unsigned int nbOutliers = std::count_if(nodes.begin(), nodes.end(),
+ [](const LHCb::SebFitNode& node) {
+ return node.type() == LHCb::SebFitNode::Outlier;
+ });
+ while (nbOutliers < m_numOutlierIter && track.nDoF() > 1) {
+ // try to remove an outlier, stop the loop if none left
+ if (!tryAndRemoveOutlier(nodes, track.type(), firstOnTrackNode, lastOnTrackNode)) {
+ break;
+ }
+ // Call the track fit
+ try {
+ std::tuple<double, int> chi2AndNDoF = fitIteration(nodes, nTrackParameters, seedCov, &firstOnTrackNode, &lastOnTrackNode);
+ track.setChi2AndDoF(std::get<0>(chi2AndNDoF), std::get<1>(chi2AndNDoF));
+ } catch (const std::string& errorMsg) {
+ debug() << "unable to fit the track " << errorMsg << endmsg;
+ throw StatusCode::FAILURE;
+ }
+ ++nbOutliers;
+ }
+
+ // determine the track states at user defined z positions
+ try {
+ auto states = determineStates(nodes, track.checkFlag(LHCb::Track::Backward), firstOnTrackNode, lastOnTrackNode);
+ track.addToStates(states);
+ } catch (const StatusCode& sc) {
+ debug() << "failed in determining states" << endmsg;
+ throw sc;
+ }
+
+ // fill extra info
+ if (m_fillExtraInfo.value()) {
+ fillExtraInfo(track, nodes, measurements, track.type());
+ }
+
+ track.setFitStatus(LHCb::Track::Fitted);
+ return nbOutliers;
+}
+
+std::vector<LHCb::Measurement*>
+HLT1Fitter::loadMeasurement(const std::vector<LHCb::State*> &states,
+ const std::vector<LHCb::LHCbID> &lhcbIDs) const {
+ std::vector<LHCb::LHCbID> newids;
+ newids.reserve(lhcbIDs.size());
+ for (const auto& id : lhcbIDs) {
+ newids.push_back(id);
+ }
+
+ // create a reference trajectory
+ LHCb::TrackTraj reftraj(states);
+
+ // create all measurements for selected IDs
+ std::vector<LHCb::Measurement*> newmeasurements;
+ newmeasurements.reserve(newids.size());
+ m_measProvider->addToMeasurements(newids, newmeasurements, reftraj);
+
+ // remove all zeros, just in case.
+ auto newend = std::remove_if(newmeasurements.begin(),newmeasurements.end(),
+ [](const LHCb::Measurement* m) { return m==nullptr; });
+ if (newend != newmeasurements.end()) {
+ debug() << "Some measurement pointers are zero: " << int(newmeasurements.end() - newend) << endmsg;
+ newmeasurements.erase(newend,newmeasurements.end());
+ throw StatusCode::FAILURE;
+ }
+ return std::move(newmeasurements);
+}
+
+/*
+ * given existing states, this function creates additional states at fixed
+ * z-positions. If a state already exists sufficiently close to the
+ * desired state, it will not create the new state.
+ * @return the additional states created
+ */
+std::vector<LHCb::State*>
+HLT1Fitter::createAdditionalRefStates(const std::vector<LHCb::State*> &states,
+ bool isBackward,
+ LHCb::Track::Types trackType) const {
+ // first need to make sure all states already on track have
+ // reasonable momentum. still needs to check that this works for
+ // velo-TT
+ auto iter = std::find_if(states.cbegin(), states.cend(),
+ [](const LHCb::State* s) {
+ return s->checkLocation(LHCb::State::AtT);
+ });
+ const LHCb::State* refstate = (iter != states.cend()) ? *iter :
+ (isBackward ? states.front() : states.back());
+ for (auto* state : states) {
+ state->setQOverP(refstate->qOverP());
+ }
+
+ // collect the z-positions where we want the states
+ boost::container::static_vector<double,4> zpositions;
+ if (trackType == LHCb::Track::Ttrack ||
+ trackType == LHCb::Track::Downstream ||
+ trackType == LHCb::Track::Long) {
+ zpositions.push_back(StateParameters::ZBegT);
+ zpositions.push_back(StateParameters::ZEndT);
+ }
+ if (trackType == LHCb::Track::Downstream ||
+ trackType == LHCb::Track::Upstream ||
+ trackType == LHCb::Track::Long) {
+ zpositions.push_back(StateParameters::ZEndTT);
+ }
+ if (trackType == LHCb::Track::Velo ||
+ trackType == LHCb::Track::VeloR ||
+ trackType == LHCb::Track::Upstream ||
+ trackType == LHCb::Track::Long) {
+ zpositions.push_back(StateParameters::ZEndVelo);
+ }
+
+ // the following container is going to hold pairs of 'desired'
+ // z-positions and actual states. the reason for the gymnastics
+ // is that we always want to propagate from the closest availlable
+ // state, but then recursively. this will make the parabolic
+ // approximation reasonably accurate.
+ typedef std::pair<double, const LHCb::State*> ZPosWithState;
+ std::vector<ZPosWithState> posStates;
+ posStates.reserve(states.size());
+ // we first add the states we already have
+ std::transform(states.begin(), states.end(),
+ std::back_inserter(posStates),
+ [](const LHCb::State* s) {
+ return std::make_pair(s->z(),s);
+ });
+
+ // now add the other z-positions, provided nothing close exists
+ const double maxDistance = 50*Gaudi::Units::cm;
+ for (auto z : zpositions) {
+ bool not_found = std::none_of(posStates.begin(), posStates.end(),
+ [&](const ZPosWithState& s) {
+ return std::abs(z - s.first) < maxDistance;
+ });
+ if (not_found) posStates.emplace_back(z, nullptr);
+ }
+ std::sort(posStates.begin(), posStates.end(),
+ [](const ZPosWithState& s1, const ZPosWithState& s2){
+ return s1.first < s2.first;
+ });
+
+ // create the states in between
+ const ITrackExtrapolator* extrap = extrapolator(trackType);
+ std::vector<LHCb::State*> newStates;
+ newStates.reserve(posStates.size());
+ for (auto it = posStates.begin(); it != posStates.end(); ++it) {
+ if (it->second) continue;
+ // find the nearest existing state to it
+ auto best = posStates.end();
+ for (auto jt = posStates.begin(); jt != posStates.end(); ++jt)
+ if (it != jt && jt->second
+ && (best==posStates.end() || std::abs(jt->first - it->first) < std::abs(best->first - it->first)))
+ best = jt;
+ assert(best != posStates.end());
+ // move from that state to this iterator, using the extrapolator and filling all states in between.
+ int direction = best > it ? -1 : +1;
+ LHCb::StateVector statevec(best->second->stateVector(), best->second->z());
+ for (auto jt = best+direction; jt != it+direction; jt += direction) {
+ StatusCode thissc = extrap->propagate(statevec, jt->first, 0, LHCb::Tr::PID::Pion());
+ if (!thissc.isSuccess()) {
+ std::ostringstream msg;
+ msg << "Problem propagating state: " << statevec << " to z= " << jt->first
+ << "\ninitializeRefStates() fails in propagating state";
+ throw msg.str();
+ } else {
+ newStates.push_back(new LHCb::State(statevec));
+ jt->second = newStates.back();
+ }
+ }
+ }
+
+ return std::move(newStates);
+}
+
+static inline double closestToBeamLine(const LHCb::State& state) {
+ const Gaudi::TrackVector& vec = state.stateVector();
+ auto z = state.z();
+ // check on division by zero (track parallel to beam line!)
+ if (vec[2] != 0 || vec[3] != 0) {
+ z -= (vec[0]*vec[2] + vec[1]*vec[3]) / (vec[2]*vec[2] + vec[3]*vec[3]);
+ }
+ // don't go outside the sensible volume
+ return std::min(std::max(z,-100*Gaudi::Units::cm), StateParameters::ZBegRich2) ;
+}
+
+namespace {
+ /** small inline helper function helping in creating a list of nodes ordered
+ * by z from a list of measurements and a list of extra reference nodes to
+ * be added. The 2 lists are passed via iterators and their types are templated
+ * do that we can use it both for forward and backward tracks
+ */
+ template <class MeasIterator, class RefIterator, class CompareOp>
+ inline void createNodesFromMeasurements(MeasIterator measBegin, MeasIterator measEnd,
+ RefIterator refBegin, RefIterator refEnd,
+ std::vector<LHCb::SebFitNode>& nodes) {
+ auto refIt = refBegin;
+ for (auto measIt = measBegin; measIt != measEnd; measIt++) {
+ while (refIt != refEnd && CompareOp()(refIt->first, (*measIt)->z())) {
+ nodes.emplace_back(refIt->first, refIt->second);
+ refIt++;
+ }
+ nodes.emplace_back(**measIt);
+ }
+ for (; refIt != refEnd; refIt++) {
+ nodes.emplace_back(refIt->first, refIt->second);
+ }
+ }
+}
+
+std::vector<LHCb::SebFitNode>
+HLT1Fitter::makeNodes(LHCb::Track& track,
+ ranges::v3::any& accelCache,
+ std::vector<LHCb::Measurement*> &measurements) const {
+ try {
+ if (track.states().empty()) {
+ throw Error("Track has no state! Can not fit.", StatusCode::FAILURE);
+ }
+ // make sure the track has sufficient reference states
+ auto newStates = createAdditionalRefStates(track.states(),
+ track.checkFlag(LHCb::Track::Backward),
+ track.type());
+ track.addToStates(newStates);
+ } catch (const std::string &msg) {
+ debug() << msg << endmsg;
+ throw Warning("Problems setting reference info", StatusCode::FAILURE, 1);
+ }
+
+ // make measurements
+ try {
+ measurements = loadMeasurement(track.states(), track.lhcbIDs());
+ } catch (const StatusCode &sc) {
+ throw Error("Unable to load measurements!", StatusCode::FAILURE);
+ }
+
+ // check that there are sufficient measurements. in fact, one is
+ // enough for the fit not to fail
+ if (measurements.empty()) {
+ throw Warning("No measurements on track", StatusCode::FAILURE, 0);
+ }
+ // order measurements in z
+ bool isBackward = track.checkFlag(LHCb::Track::Backward);
+ if (isBackward) {
+ std::stable_sort(measurements.begin(), measurements.end(),
+ [](const LHCb::Measurement* m1, const LHCb::Measurement* m2) {return m1->z() > m2->z();});
+ } else {
+ std::stable_sort(measurements.begin(), measurements.end(),
+ [](const LHCb::Measurement* m1, const LHCb::Measurement* m2) {return m1->z() < m2->z();});
+ }
+
+ // Prepare a list of reference nodes to be added, ordered by z
+ typedef std::pair<double, LHCb::State::Location> RefNodeData;
+ std::vector<RefNodeData> extraRefNodes;
+ extraRefNodes.reserve(6);
+ if (m_addDefaultRefNodes.value()) {
+ if (track.hasVelo() && !track.checkFlag(LHCb::Track::Backward))
+ extraRefNodes.emplace_back(StateParameters::ZEndVelo, LHCb::State::EndVelo);
+ if (track.hasTT()) {
+ extraRefNodes.emplace_back(StateParameters::ZBegRich1, LHCb::State::BegRich1);
+ extraRefNodes.emplace_back(StateParameters::ZEndRich1, LHCb::State::EndRich1);
+ }
+ if (track.hasT()) {
+ extraRefNodes.emplace_back(StateParameters::ZBegT, LHCb::State::AtT);
+ extraRefNodes.emplace_back(StateParameters::ZBegRich2, LHCb::State::BegRich2);
+ extraRefNodes.emplace_back(StateParameters::ZEndRich2, LHCb::State::EndRich2);
+ }
+ }
+ // Add a node for the position at the beamline, put it in the right place
+ if (m_stateAtBeamLine.value() && (track.hasTT() || track.hasVelo())) {
+ const LHCb::State& refstate = *(track.checkFlag(LHCb::Track::Backward) ? track.states().back() : track.states().front());
+ double closestToBeamLineZ = closestToBeamLine(refstate);
+ auto it = extraRefNodes.begin();
+ while (it->first < closestToBeamLineZ) it++;
+ extraRefNodes.emplace(it, closestToBeamLineZ, LHCb::State::ClosestToBeam);
+ }
+
+ // We want to create the nodes in the right order (ordered by z), but the order depends
+ // on the track, specifically whether it's a Forward or Backward track
+ std::vector<LHCb::SebFitNode> nodes;
+ nodes.reserve(measurements.size() + 6);
+ if (!isBackward) {
+ createNodesFromMeasurements<std::vector<LHCb::Measurement*>::reverse_iterator,
+ std::vector<RefNodeData>::reverse_iterator,
+ std::greater<double>>
+ (measurements.rbegin(), measurements.rend(),
+ extraRefNodes.rbegin(), extraRefNodes.rend(),
+ nodes);
+ } else {
+ createNodesFromMeasurements<std::vector<LHCb::Measurement*>::reverse_iterator,
+ std::vector<RefNodeData>::iterator,
+ std::less<double>>
+ (measurements.rbegin(), measurements.rend(),
+ extraRefNodes.begin(), extraRefNodes.end(),
+ nodes);
+ }
+
+ // Set the reference using a TrackTraj
+ std::vector<LHCb::State*> states;
+ states.insert(states.end(), track.states().begin(), track.states().end());
+ LHCb::TrackTraj tracktraj(states);
+ std::for_each(nodes.begin(), nodes.end(),
+ [&](LHCb::SebFitNode& node) { node.setRefVector(tracktraj.stateVector(node.z()));
+ });
+
+ // update the projections. need to be done every time ref is updated
+ try {
+ projectReference(nodes);
+ } catch (const StatusCode &sc) {
+ debug() << "problem updating ref vectors" << endmsg;
+ throw sc;
+ }
+
+ // add all the noise, if required
+ if (m_applyMaterialCorrections.value()) {
+ updateMaterialCorrections(nodes, states, track.type(), accelCache);
+ }
+
+ return std::move(nodes);
+}
+
+void HLT1Fitter::updateRefVectors(std::vector<LHCb::SebFitNode> &nodes,
+ LHCb::Track::Types trackType,
+ LHCb::SebFitNode* firstOnTrackNode,
+ LHCb::SebFitNode* lastOnTrackNode) const {
+ // force the smoothing.
+ bool hasinfoStreamForward = false;
+ bool hasinfoStreamBackward = (lastOnTrackNode != nullptr);
+ for(auto& node : nodes) {
+ hasinfoStreamBackward = hasinfoStreamBackward && (&nodes.front() != lastOnTrackNode);
+ node.computeBiSmoothedStateSeb(hasinfoStreamForward, hasinfoStreamBackward);
+ hasinfoStreamForward = hasinfoStreamForward || (&node == firstOnTrackNode);
+ }
+ for (auto & node : nodes) {
+ node.setRefVector(node.state().stateVector());
+ }
+ // update the projections. need to be done every time ref is
+ // updated. we can move this code here at some point.
+ projectReference(nodes);
+
+ // update the transport using the new ref vectors
+ updateTransport(nodes, trackType);
+}
+
+typedef Gaudi::Matrix1x3 DualVector;
+DualVector dual(const Gaudi::XYZVector& v) {
+ DualVector d;
+ v.GetCoordinates( d.Array() );
+ return d;
+}
+
+double HLT1Fitter::getToleranceForMeasurement(const LHCb::Measurement::Type& type) const {
+ switch (type) {
+ case LHCb::Measurement::VP:
+ return 0.0005*Gaudi::Units::mm;
+ case LHCb::Measurement::UT:
+ case LHCb::Measurement::UTLite:
+ return 0.002*Gaudi::Units::mm;
+ case LHCb::Measurement::FT:
+ return 0.002*Gaudi::Units::mm;
+ case LHCb::Measurement::Muon:
+ return 0.002*Gaudi::Units::mm;
+ default:
+ throw Warning("unknown measurement in projectReference", StatusCode::FAILURE, 0);
+ }
+}
+
+HLT1Fitter::InternalProjectResult
+HLT1Fitter::internal_project(const LHCb::StateVector& statevector,
+ const LHCb::Measurement& meas,
+ double tolerance) const {
+ // Project onto the reference. First create the StateTraj without BField information.
+ Gaudi::XYZVector bfield(0,0,0) ;
+ const LHCb::StateZTraj refTraj(statevector, bfield);
+
+ // Get the measurement trajectory representing the centre of gravity
+ const LHCb::Trajectory& measTraj = meas.trajectory();
+
+ // Determine initial estimates of s1 and s2
+ double sState = statevector.z(); // Assume state is already close to the minimum
+ double sMeas = measTraj.muEstimate(refTraj.position(sState));
+
+ // Determine the actual minimum with the Poca tool
+ Gaudi::XYZVector dist;
+ StatusCode sc = m_poca->minimize(refTraj, sState,
+ measTraj, sMeas, dist, tolerance);
+ if (sc.isFailure()) {
+ throw sc;
+ }
+
+ // Set up the vector onto which we project everything. This should
+ // actually be parallel to dist.
+ Gaudi::XYZVector unitPocaVector = (measTraj.direction(sMeas).Cross(refTraj.direction(sState))).Unit();
+ double doca = unitPocaVector.Dot(dist);
+
+ // compute the projection matrix from parameter space onto the (signed!) unit
+ Gaudi::TrackProjectionMatrix H = dual(unitPocaVector) * refTraj.derivative(sState);
+
+ // Set the error on the measurement so that it can be used in the fit
+ double errMeasure2 = meas.resolution2(refTraj.position(sState),
+ refTraj.direction(sState));
+
+ return {sMeas, doca, std::move(H), std::move(unitPocaVector), errMeasure2};
+}
+
+void HLT1Fitter::projectReference(std::vector<LHCb::SebFitNode> &nodes) const {
+ for (auto& node: nodes) {
+ if (node.hasMeasurement()) {
+ try {
+ // if the reference is not set, issue an error
+ auto result = internal_project(node.refVector(), node.measurement(),
+ getToleranceForMeasurement(node.measurement().type()));
+ node.updateProjection(std::move(result.H), -result.doca);
+ node.setPocaVector(std::move(result.unitPocaVector));
+ node.setDoca(result.doca);
+ node.setErrMeasure2(result.errMeasure2);
+ } catch (StatusCode scr) {
+ throw Warning("unable to project statevector", scr, 0);
+ }
+ }
+ }
+}
+
+namespace {
+ enum HitType {VeloR, VeloPhi, TT, T, Muon, Unknown};
+ inline int hittypemap(LHCb::Measurement::Type type) {
+ HitType rc = Unknown;
+ switch(type) {
+ case LHCb::Measurement::Unknown: rc = Unknown; break;
+ case LHCb::Measurement::Muon: rc = Muon; break;
+ case LHCb::Measurement::VP: rc = VeloR; break;
+ case LHCb::Measurement::FT : rc = T; break;
+ case LHCb::Measurement::UT : rc = TT; break;
+ case LHCb::Measurement::UTLite : rc = TT; break;
+ default : rc = Unknown; break;
+ }
+ return rc;
+ }
+}
+
+bool HLT1Fitter::tryAndRemoveOutlier(std::vector<LHCb::SebFitNode>& nodes,
+ LHCb::Track::Types trackType,
+ LHCb::SebFitNode* firstOnTrackNode,
+ LHCb::SebFitNode* lastOnTrackNode) const {
+ // return false if outlier chi2 cut < 0
+ if (m_chi2Outliers < 0.0) return false;
+
+ // Count the number of hits of each type
+ const size_t minNumHits[5] = { m_minNumVeloRHits, m_minNumVeloPhiHits, m_minNumTTHits, m_minNumTHits, m_minNumMuonHits };
+ size_t numHits[5] = {0,0,0,0,0};
+ for (const auto& node : nodes) {
+ if (node.type() == LHCb::SebFitNode::HitOnTrack){
+ ++numHits[hittypemap(node.measurement().type())];
+ }
+ }
+
+ // loop over the nodes and find the one with the highest chi2 >
+ // m_chi2Outliers, provided there is enough hits of this type left.
+
+ // Computing the chi2 will trigger the smoothing. Especially in the
+ // trigger, where we don't update the reference, this makes a big
+ // difference. One way to save some time is to first test a number
+ // of which we know that it is either equal to or bigger than the
+ // chi2 contribution of the hit, namely the chi2 sum of the match
+ // and the hit at this node. We then sort the hits in this number,
+ // and only compute chi2 if it can be bigger than the current worst
+ // one.
+ const double totalchi2 = std::max(nodes.front().totalChi2(LHCb::SebFitNode::Backward).chi2(),
+ nodes.back().totalChi2(LHCb::SebFitNode::Forward).chi2());
+ using NodeWithChi2 = std::tuple<LHCb::SebFitNode*, double, bool, bool>;
+ std::vector<NodeWithChi2> nodesWithChi2UL;
+ nodesWithChi2UL.reserve(nodes.size());
+ int numtried(0), numcalled(0);
+ bool infoForward = false;
+ bool infoBackward = (lastOnTrackNode != nullptr);
+ LHCb::SebFitNode *prevNode = nullptr;
+ for (auto it = nodes.begin(); it != nodes.end(); it++) {
+ infoBackward = infoBackward && (!(&(*it) == lastOnTrackNode));
+ if (it->hasMeasurement() &&
+ it->type() == LHCb::SebFitNode::HitOnTrack) {
+ int hittype = hittypemap(it->measurement().type());
+ if (numHits[hittype] > minNumHits[hittype]) {
+ ++numtried;
+ auto chi2MatchAndHit = totalchi2;
+ if (prevNode) {chi2MatchAndHit -= prevNode->totalChi2(0).chi2();
+ }
+ auto nextIt = it + 1;
+ if (nextIt != nodes.end()) {
+ chi2MatchAndHit -= nextIt->totalChi2(1).chi2();
+ }
+ if (chi2MatchAndHit > m_chi2Outliers) {
+ nodesWithChi2UL.emplace_back(&(*it), chi2MatchAndHit, infoForward, infoBackward);
+ }
+ }
+ }
+ prevNode = &(*it);
+ infoForward = infoForward || (prevNode == firstOnTrackNode);
+ }
+ // now sort them
+ auto worstChi2 = m_chi2Outliers;
+ std::sort(nodesWithChi2UL.begin(), nodesWithChi2UL.end(),
+ [](const NodeWithChi2 &n1, const NodeWithChi2 &n2){
+ return std::get<1>(n1) > std::get<1>(n2);
+ });
+
+ // -- if the first is smaller than worstChi2, and it is sorted in decreasing order
+ // -- the 'if' will never be true and we can return here (M. De Cian)
+ if (!nodesWithChi2UL.empty() && std::get<1>(nodesWithChi2UL.front()) < worstChi2) {
+ return false;
+ }
+ LHCb::SebFitNode* outlier{nullptr};
+ for (auto& node : nodesWithChi2UL) {
+ if (std::get<1>(node) > worstChi2) {
+ std::get<0>(node)->computeBiSmoothedStateSeb(std::get<2>(node), std::get<3>(node));
+ const auto chi2 = std::get<0>(node)->chi2Seb();
+ ++numcalled;
+ if (chi2 > worstChi2) {
+ worstChi2 = chi2;
+ outlier = std::get<0>(node);
+ }
+ }
+ }
+
+ if (outlier) {
+ // update reference trajectories with smoothed states
+ if (m_updateReferenceInOutlierIters.value()) {
+ try {
+ updateRefVectors(nodes, trackType, firstOnTrackNode, lastOnTrackNode);
+ } catch (const StatusCode &sc) {
+ debug() << "problem updating ref vectors" << endmsg;
+ throw sc;
+ }
+ }
+
+ // deactivate the outlier and return
+ outlier->setType(LHCb::SebFitNode::Outlier);
+ return true;
+ }
+ return false;
+}
+
+std::vector<LHCb::State*> HLT1Fitter::determineStates(const std::vector<LHCb::SebFitNode> &nodes,
+ bool isBackward,
+ LHCb::SebFitNode* firstOnTrackNode,
+ LHCb::SebFitNode* lastOnTrackNode) const {
+ // Add the state at the first and last measurement position
+ if (firstOnTrackNode == nullptr) {
+ debug() << "HLT1Fitter::determineStates : unable to find first measurement" << endmsg;
+ throw StatusCode::FAILURE;
+ }
+ // force the smoothing
+ firstOnTrackNode->computeBiSmoothedStateSeb(false, lastOnTrackNode != nullptr);
+ auto& firstState = firstOnTrackNode->state();
+ if (lastOnTrackNode == nullptr) {
+ debug() << "HLT1Fitter::determineStates : unable to find last measurement" << endmsg;
+ throw StatusCode::FAILURE;
+ }
+ // force the smoothing
+ lastOnTrackNode->computeBiSmoothedStateSeb(firstOnTrackNode != nullptr, false);
+ auto& lastState = lastOnTrackNode->state();
+
+ bool upstream = nodes.front().z() > nodes.back().z();
+ bool reversed = (upstream && !isBackward) || (!upstream && isBackward);
+
+ // This state is not filtered for a forward only fit.
+ std::vector<LHCb::State*> states;
+ if (m_addDefaultRefNodes.value()) {
+ states.push_back(firstState.clone());
+ states.back()->setLocation(reversed ? LHCb::State::LastMeasurement : LHCb::State::FirstMeasurement);
+ }
+ // This state is always filtered
+ states.push_back(lastState.clone());
+ states.back()->setLocation(reversed ? LHCb::State::FirstMeasurement : LHCb::State::LastMeasurement);
+
+ // Add the states at the reference positions
+ for (const auto& node : nodes)
+ if (node.type() == LHCb::SebFitNode::Reference)
+ states.push_back(node.state().clone());
+
+ return std::move(states);
+}
+
+void HLT1Fitter::updateMaterialCorrections(std::vector<LHCb::SebFitNode> &nodes,
+ const std::vector<LHCb::State*> &states,
+ LHCb::Track::Types trackType,
+ ranges::v3::any& accelCache) const {
+ if (nodes.size() < 1) {
+ return;
+ }
+
+ // the noise in each node is the noise in the propagation between
+ // the previous node and this node.
+ // first collect all volumes on the track. The advantages of collecting them all at once
+ // is that it is much faster. (Less call to TransportSvc.)
+
+ // if m_scatteringP is set, use it
+ auto scatteringMomentum = m_scatteringP.value();
+ if (m_scatteringP.value() <= 0) {
+ // if only velo, or magnet off, use a fixed momentum based on pt.
+ scatteringMomentum = states[0]->p();
+ if (m_scatteringPt.value() > 0 &&
+ // exclude Long, Upstream, DownStream, Ttrack, so check !hasT && !hasTT
+ (trackType < LHCb::Track::Long || trackType > LHCb::Track::Ttrack)) {
+ auto tx = states[0]->tx();
+ auto ty = states[0]->ty();
+ auto slope2 = tx*tx+ty*ty;
+ auto tanth = std::max(std::sqrt(slope2/(1+slope2)),1e-4);
+ scatteringMomentum = m_scatteringPt/tanth;
+ }
+ // set some limits for the momentum used for scattering
+ scatteringMomentum = std::min(scatteringMomentum, m_maxMomentumForScattering.value());
+ scatteringMomentum = std::max(scatteringMomentum, m_minMomentumForScattering.value());
+ }
+
+ // this is fairly tricky now: we want to use TracjTraj, but we
+ // cannot create it directly from the nodes, because that would
+ // trigger the filter!
+ LHCb::TrackTraj tracktraj(states);
+ auto zmin = nodes.front().z();
+ auto zmax = nodes.back().z();
+ if (zmin>zmax) std::swap(zmin,zmax);
+ tracktraj.setRange(zmin,zmax);
+
+ // make sure we have the space we need in intersections so we don't need to
+ // reallocate (offline, I've seen tracks with more than 670 intersections
+ // in 100 events; we stay a bit above that to be on the safe side - and we
+ // don't mind the occasional reallocate if it's a rare track that has even
+ // more intersections)
+ IMaterialLocator::Intersections intersections;
+ intersections.reserve(1024);
+ m_materialLocator->intersect_r(tracktraj, intersections, accelCache);
+
+ // now we need to redistribute the result between the nodes. the first node cannot have any noise.
+ auto node = nodes.begin();
+ auto zorigin = node->z();
+ for (++node; node != nodes.end(); ++node) {
+ auto ztarget = node->z();
+ LHCb::State state(node->refVector());
+ state.covariance() = Gaudi::TrackSymMatrix();
+ state.setQOverP(1/scatteringMomentum);
+ // only apply energyloss correction for tracks that traverse magnet
+ bool applyenergyloss = m_applyEnergyLossCorrections.value() &&
+ (trackType == LHCb::Track::Downstream || trackType == LHCb::Track::Long);
+ m_materialLocator->applyMaterialCorrections(state, intersections, zorigin,
+ LHCb::Tr::PID::Pion(),
+ true, applyenergyloss);
+ auto deltaE = 1/state.qOverP() - scatteringMomentum;
+ node->setNoiseMatrix(state.covariance());
+ node->setDeltaEnergy(deltaE);
+ zorigin = ztarget;
+ }
+}
+
+int HLT1Fitter::updateTransport(std::vector<LHCb::SebFitNode> &nodes,
+ LHCb::Track::Types trackType) const {
+ bool hasMomentum = false;
+ // sets the propagation between the previous node and this. node that the reference
+ // of the previous node is used.
+ if (nodes.size()>1) {
+ const ITrackExtrapolator* extrap = extrapolator(trackType);
+ auto node = nodes.begin();
+ const LHCb::StateVector* refvector = &(node->refVector());
+ Gaudi::TrackMatrix F = ROOT::Math::SMatrixIdentity();
+ for (++node; node!=nodes.end(); ++node) {
+ LHCb::StateVector statevector = *refvector;
+ StatusCode sc = extrap->propagate(statevector, node->z(), &F);
+ if (sc.isFailure()) {
+ debug() << "unable to propagate reference vector from z=" << refvector->z()
+ << " to " << node->z()
+ << "; track type = " << trackType
+ << ": vec = " << refvector->parameters() << endmsg;
+ throw sc;
+ }
+
+ // correct for energy loss
+ auto dE = node->deltaEnergy();
+ if (std::abs(statevector.qOverP()) > LHCb::Math::lowTolerance) {
+ auto charge = statevector.qOverP() > 0 ? 1. : -1.;
+ auto momnew = std::max(m_minMomentumForELossCorr.value(),
+ std::abs(1/statevector.qOverP()) + dE);
+ if (std::abs(momnew) > m_minMomentumForELossCorr.value())
+ statevector.setQOverP(charge/momnew);
+ }
+
+ // calculate the 'transport vector' (need to replace that)
+ Gaudi::TrackVector tranportvec = statevector.parameters() - F * refvector->parameters();
+ node->setTransportMatrix(F);
+ node->setTransportVector(tranportvec);
+
+ // update the reference
+ refvector = &(node->refVector());
+
+ // test dtx/dqop to see if the momentum affects this track.
+ if (std::abs(F(2,4)) != 0) hasMomentum = true;
+ }
+ }
+
+ if (m_useSeedStateErrors.value()) {
+ // we need to do this until we can properly deal with the seed state
+ return 0;
+ } else {
+ return hasMomentum ? 5 : 4;
+ }
+}
+
+std::tuple<double, int> HLT1Fitter::fitIteration(std::vector<LHCb::SebFitNode>& nodes,
+ int& nTrackParameters,
+ const Gaudi::TrackSymMatrix& seedCov,
+ LHCb::SebFitNode** prevFirstOnTrackNode,
+ LHCb::SebFitNode** prevLastOnTrackNode) const {
+ // This is set up with the aim to trigger the cache such that there
+ // will be no nested calls. That makes it easier to profile the
+ // fit. Eventually, we could do without all of this, since
+ // everything is anyway computed on demand.
+ LHCb::SebFitNode *prevNode = nullptr;
+ LHCb::SebFitNode *firstOnTrackNode = nullptr;
+ bool hasInfoUpstream = false;
+ for (auto& node : nodes) {
+ node.computePredictedStateSeb(LHCb::SebFitNode::Forward, prevNode, hasInfoUpstream);
+ node.computeFilteredStateSeb(LHCb::SebFitNode::Forward, prevNode, nTrackParameters);
+ prevNode = &node;
+ if (!hasInfoUpstream) {
+ hasInfoUpstream = (node.type() == LHCb::SebFitNode::HitOnTrack);
+ if (hasInfoUpstream) {
+ firstOnTrackNode = &node;
+ }
+ }
+ }
+ LHCb::SebFitNode *lastOnTrackNode = nullptr;
+ prevNode = nullptr;
+ hasInfoUpstream = false;
+ for (auto& node : boost::adaptors::reverse(nodes)) {
+ node.computePredictedStateSeb(LHCb::SebFitNode::Backward, prevNode, hasInfoUpstream);
+ node.computeFilteredStateSeb(LHCb::SebFitNode::Backward, prevNode, nTrackParameters);
+ prevNode = &(node);
+ if (!hasInfoUpstream) {
+ hasInfoUpstream = (node.type() == LHCb::SebFitNode::HitOnTrack);
+ if (hasInfoUpstream) {
+ lastOnTrackNode = &node;
+ }
+ }
+ }
+ *prevFirstOnTrackNode = firstOnTrackNode;
+ *prevLastOnTrackNode = lastOnTrackNode;
+ // force the smoothing.
+ /*bool hasinfoStreamForward = false;
+ bool hasinfoStreamBackward = (lastOnTrackNode != nullptr);
+ for(auto& node : nodes) {
+ hasinfoStreamBackward = hasinfoStreamBackward && (&nodes.front() != lastOnTrackNode);
+ node.computeBiSmoothedStateSeb(hasinfoStreamForward, hasinfoStreamBackward);
+ hasinfoStreamForward = hasinfoStreamForward || (&node == firstOnTrackNode);
+ }*/
+
+ // set the total chisquare of the track
+ // Count the number of active track parameters. For now, just look at the momentum.
+ size_t npar = m_DoF;
+ if (npar == 5u) {
+ const LHCb::State* laststate(nullptr);
+ for (auto inode = nodes.begin(); inode != nodes.end(); ++inode) {
+ if (inode->type() == LHCb::SebFitNode::HitOnTrack) {
+ laststate = &(inode->filteredState(LHCb::SebFitNode::Forward));
+ }
+ }
+ if (laststate) {
+ const double threshold = 0.1;
+ nTrackParameters = (laststate->covariance()(4,4) / seedCov(4,4) < threshold ? 5 : 4);
+ }
+ }
+
+ LHCb::ChiSquare chisq = nodes.back().totalChi2(LHCb::SebFitNode::Forward);
+ int ndof = chisq.nDoF() - (npar - nTrackParameters);
+ // FIXME: why don't we take the maximum, like in KalmanFitResult?
+ LHCb::ChiSquare chisqbw = nodes.front().totalChi2(LHCb::SebFitNode::Backward);
+ if (chisqbw.chi2() < chisq.chi2()) chisq = chisqbw;
+ return std::make_tuple(chisq.chi2(), ndof);
+}
diff --git a/Tr/TrackFitter/src/HLT1Fitter.h b/Tr/TrackFitter/src/HLT1Fitter.h
new file mode 100644
index 00000000000..293acaade59
--- /dev/null
+++ b/Tr/TrackFitter/src/HLT1Fitter.h
@@ -0,0 +1,157 @@
+#pragma once
+
+// Include files
+#include "GaudiAlg/Transformer.h"
+#include "GaudiKernel/ToolHandle.h"
+#include "TrackInterfaces/ITrackFitter.h"
+#include "TrackInterfaces/IMeasurementProvider.h"
+#include "TrackInterfaces/IMaterialLocator.h"
+#include "Kernel/ITrajPoca.h"
+#include "TrackInterfaces/ITrackExtrapolator.h"
+#include "Event/Measurement.h"
+#include "Event/StateVector.h"
+#include "Event/SebFitNode.h"
+#include "Event/Track.h"
+
+/**
+ * Class dedicated to fitting HLT1 tracks
+ * @author Sebastien Ponce
+ */
+class HLT1Fitter : public Gaudi::Functional::Transformer<LHCb::Tracks(const std::vector<LHCb::Track>&)>{
+
+public:
+
+ /// Standard constructor
+ HLT1Fitter( const std::string& name, ISvcLocator* pSvcLocator );
+
+ /// main execution
+ LHCb::Tracks operator()(const std::vector<LHCb::Track>&) const override;
+
+ /// finalization (pure reporting)
+ StatusCode finalize() override;
+
+private:
+
+ struct InternalProjectResult final {
+ double sMeas;
+ double doca;
+ Gaudi::TrackProjectionMatrix H;
+ Gaudi::XYZVector unitPocaVector;
+ double errMeasure2;
+ };
+
+ /**
+ * fits on track, returns number of outliers found
+ */
+ unsigned int fit(LHCb::Track& track, ranges::v3::any& accelCache) const;
+
+ // helper function that should be removed once MeasurementProvider is refactored
+ std::vector<LHCb::Measurement*> loadMeasurement(const std::vector<LHCb::State*> &states,
+ const std::vector<LHCb::LHCbID> &lhcbIDs) const;
+
+ /// node creation
+ std::vector<LHCb::SebFitNode> makeNodes(LHCb::Track& track,
+ ranges::v3::any& accelCache,
+ std::vector<LHCb::Measurement*> &measurements) const;
+
+ void updateRefVectors(std::vector<LHCb::SebFitNode> &nodes,
+ LHCb::Track::Types trackType,
+ LHCb::SebFitNode* firstOnTrackNode,
+ LHCb::SebFitNode* lastOnTrackNode) const;
+
+ double getToleranceForMeasurement(const LHCb::Measurement::Type& type) const;
+
+ InternalProjectResult internal_project(const LHCb::StateVector& statevector,
+ const LHCb::Measurement& meas,
+ double tolerance) const;
+
+ void projectReference(std::vector<LHCb::SebFitNode> &nodes) const;
+
+ /*
+ * Tries to find and remove an outlier node
+ * If one was found and remove returns true, else returns false
+ */
+ bool tryAndRemoveOutlier(std::vector<LHCb::SebFitNode>& nodes,
+ LHCb::Track::Types trackType,
+ LHCb::SebFitNode* firstOnTrackNode,
+ LHCb::SebFitNode* lastOnTrackNode) const;
+
+ /// Build states from node and measurements
+ std::vector<LHCb::State*> determineStates(const std::vector<LHCb::SebFitNode> &nodes,
+ bool isBackward,
+ LHCb::SebFitNode* firstOnTrackNode,
+ LHCb::SebFitNode* lastOnTrackNode) const;
+
+ /*
+ * given existing states, this function creates additional states at fixed
+ * z-positions. If a state already exists sufficiently close to the
+ * desired state, it will not create the new state.
+ * @return the additional states created
+ */
+ std::vector<LHCb::State*> createAdditionalRefStates(const std::vector<LHCb::State*> &states,
+ bool isBackward,
+ LHCb::Track::Types trackType) const;
+
+ /**
+ * Performs one iteration of the Kalmanfit
+ * @returns chi2 and nDoF of the track after the fit
+ * Also sets nTrackParameters, firstOnTrackNode and lastOnTrackNode
+ */
+ std::tuple<double, int> fitIteration(std::vector<LHCb::SebFitNode>& nodes,
+ int& nTrackParameters,
+ const Gaudi::TrackSymMatrix& seedCov,
+ LHCb::SebFitNode** firstOnTrackNode,
+ LHCb::SebFitNode** lastOnTrackNode) const;
+
+ int updateTransport(std::vector<LHCb::SebFitNode> &nodes,
+ LHCb::Track::Types trackType) const;
+
+ void updateMaterialCorrections(std::vector<LHCb::SebFitNode> &nodes,
+ const std::vector<LHCb::State*> &states,
+ LHCb::Track::Types trackType,
+ ranges::v3::any& accelCache) const;
+
+ const ITrackExtrapolator* extrapolator(LHCb::Track::Types tracktype) const {
+ if (tracktype == LHCb::Track::Velo || tracktype == LHCb::Track::VeloR) return &(*m_veloExtrapolator);
+ return &(*m_extrapolator);
+ }
+
+private:
+
+ ToolHandle<ITrackExtrapolator> m_extrapolator{"TrackMasterExtrapolator", this}; ///< extrapolator
+ ToolHandle<ITrackExtrapolator> m_veloExtrapolator{"TrackLinearExtrapolator",this}; ///< extrapolator for Velo-only tracks
+ ToolHandle<IMeasurementProvider> m_measProvider{"MeasurementProvider", this };
+ ToolHandle<IMaterialLocator> m_materialLocator{"DetailedMaterialLocator",this};
+ ToolHandle<ITrajPoca> m_poca{"TrajPoca",this};
+
+ Gaudi::Property<bool> m_forceSmooth {this, "ForceSmooth", false, "Flag for force the smoothing (for debug reason)"};
+ Gaudi::Property<unsigned int> m_DoF {this, "DoF", 5u, "Degrees of freedom"};
+ Gaudi::Property<double> m_maxChi2DoF{this, "MaxChi2DoF", 999999, "Max chi2 per track when output is a new container"};
+ Gaudi::Property<double> m_maxDeltaChi2Converged{this, "MaxDeltaChiSqConverged", 0.01, "Maximum change in chisquare for converged fit"};
+ Gaudi::Property<unsigned int> m_numOutlierIter{this, "MaxNumberOutliers", 2, "max number of outliers to be removed"};
+ Gaudi::Property<bool> m_fillExtraInfo{this, "FillExtraInfo", true, "Fill the extra info"};
+ Gaudi::Property<bool> m_addDefaultRefNodes{this, "AddDefaultReferenceNodes", true, "add default reference nodes"};
+ Gaudi::Property<bool> m_stateAtBeamLine{this, "StateAtBeamLine", true, "add state closest to the beam-line"};
+ Gaudi::Property<bool> m_applyMaterialCorrections{this, "ApplyMaterialCorrections", true, "Apply material corrections"};
+ Gaudi::Property<bool> m_applyEnergyLossCorrections{this, "ApplyEnergyLossCorr", true, "Apply energy loss corrections"};
+ Gaudi::Property<double> m_chi2Outliers{this, "Chi2Outliers", 9.0, "chi2 of outliers to be removed"};
+ Gaudi::Property<bool> m_useSeedStateErrors{this, "UseSeedStateErrors", false, "use errors of the seed state"};
+ Gaudi::Property<double> m_minMomentumForScattering{this, "MinMomentumForScattering", 100.*Gaudi::Units::MeV, "Minimum momentum used for scattering"};
+ Gaudi::Property<double> m_maxMomentumForScattering{this, "MaxMomentumForScattering", 500.*Gaudi::Units::GeV, "Maximum momentum used for scattering"};
+ Gaudi::Property<double> m_errorX{this, "ErrorX", 20.0*Gaudi::Units::mm, "Seed error on x"};
+ Gaudi::Property<double> m_errorY{this, "ErrorY", 20.0*Gaudi::Units::mm, "Seed error on y"};
+ Gaudi::Property<double> m_errorTx{this, "ErrorTx", 0.1, "Seed error on slope x"};
+ Gaudi::Property<double> m_errorTy{this, "ErrorTy", 0.1, "Seed error on slope y"};
+ Gaudi::Property<std::vector<double>> m_errorQoP{this, "ErrorQoP", {0.0, 0.01}, "Seed error on QoP"};
+ Gaudi::Property<int> m_numFitIter{this, "NumberFitIterations", 10, "number of fit iterations to perform"};
+ Gaudi::Property<size_t> m_minNumVeloRHits{this, "MinNumVeloRHitsForOutlierRemoval", 3, "Minimum number of VeloR hits"};
+ Gaudi::Property<size_t> m_minNumVeloPhiHits{this, "MinNumVeloPhiHitsForOutlierRemoval", 3, "Minimum number of VeloPhi hits"};
+ Gaudi::Property<size_t> m_minNumTTHits{this, "MinNumTTHitsForOutlierRemoval", 3, "Minimum number of TT hits"};
+ Gaudi::Property<size_t> m_minNumTHits{this, "MinNumTHitsForOutlierRemoval", 6, "Minimum number of T hits"};
+ Gaudi::Property<size_t> m_minNumMuonHits{this, "MinNumMuonHitsForOutlierRemoval", 4, "Minimum number of Muon hits"};
+ Gaudi::Property<double> m_minMomentumForELossCorr{this, "MinMomentumELossCorr", 10.*Gaudi::Units::MeV, "Minimum momentum used in correction for energy loss"};
+ Gaudi::Property<double> m_scatteringPt{this, "TransverseMomentumForScattering", 400.*Gaudi::Units::MeV, "transverse momentum used for scattering if track has no good momentum estimate"};
+ Gaudi::Property<double> m_scatteringP{this, "MomentumForScattering", -1, "momentum used for scattering in e.g. magnet off data"};
+ Gaudi::Property<bool> m_updateReferenceInOutlierIters{this, "UpdateReferenceInOutlierIterations", true, "Update projection in iterations in which outliers are removed"};
+
+};
diff --git a/Tr/TrackFitter/src/TrackMasterFitter.cpp b/Tr/TrackFitter/src/TrackMasterFitter.cpp
index 78e66810d9d..f2936b32276 100644
--- a/Tr/TrackFitter/src/TrackMasterFitter.cpp
+++ b/Tr/TrackFitter/src/TrackMasterFitter.cpp
@@ -749,9 +749,9 @@ StatusCode TrackMasterFitter::makeNodes(Track& track,
bool backward = track.checkFlag(LHCb::Track::Flags::Backward);
bool upstream = (m_upstream.value()&&!backward) || (!m_upstream.value()&&backward);
if (upstream) {
- std::sort(nodes.begin(), nodes.end(), TrackFunctor::decreasingByZ());
+ std::stable_sort(nodes.begin(), nodes.end(), TrackFunctor::decreasingByZ());
} else {
- std::sort(nodes.begin(), nodes.end(), TrackFunctor::increasingByZ());
+ std::stable_sort(nodes.begin(), nodes.end(), TrackFunctor::increasingByZ());
}
// Set the reference using a TrackTraj
--
GitLab