diff --git a/Phys/VertexFit/CMakeLists.txt b/Phys/VertexFit/CMakeLists.txt index 4d799059c87c404ce13377b422326f4a06e4d299..a1a55246f7819b3b8fc292d28b4abdf2e6413619 100644 --- a/Phys/VertexFit/CMakeLists.txt +++ b/Phys/VertexFit/CMakeLists.txt @@ -8,7 +8,8 @@ gaudi_depends_on_subdirs(GaudiAlg Phys/DaVinciInterfaces Phys/DaVinciKernel Phys/LoKiPhys - Tr/TrackInterfaces) + Tr/TrackInterfaces + Tr/TrackKernel) find_package(ROOT) find_package(Boost) @@ -17,7 +18,8 @@ include_directories(SYSTEM ${ROOT_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) gaudi_add_module(VertexFit src/*.cpp INCLUDE_DIRS Tr/TrackInterfaces - LINK_LIBRARIES GaudiAlgLib LHCbMathLib DaVinciInterfacesLib DaVinciKernelLib LoKiPhysLib) + LINK_LIBRARIES GaudiAlgLib LHCbMathLib + DaVinciInterfacesLib DaVinciKernelLib LoKiPhysLib TrackKernel) gaudi_add_test(QMTest QMTEST) diff --git a/Phys/VertexFit/src/ParticleVertexFitter.cpp b/Phys/VertexFit/src/ParticleVertexFitter.cpp new file mode 100644 index 0000000000000000000000000000000000000000..0da6cb37a5bb553610fabfdbbdff0f9483696d20 --- /dev/null +++ b/Phys/VertexFit/src/ParticleVertexFitter.cpp @@ -0,0 +1,793 @@ +#include "GaudiAlg/GaudiTool.h" +#include "Kernel/IVertexFit.h" +#include "LHCbMath/MatrixTransforms.h" +#include "TrackKernel/TrackVertexUtils.h" +#include "GaudiKernel/ToolHandle.h" +#include "TrackKernel/TrackTraj.h" +#include "TrackInterfaces/ITrackStateProvider.h" +#include "Kernel/ITrajPoca.h" +#include "Kernel/LineTraj.h" +#include "Kernel/IParticlePropertySvc.h" +#include "Kernel/ParticleProperty.h" + +namespace { + class VertexDaughter ; +} + +class ParticleVertexFitter : public GaudiTool, + virtual public IVertexFit +{ +public: + + /// Standard constructor + ParticleVertexFitter( const std::string& type, + const std::string& name, + const IInterface* parent); + + /// Initialize + StatusCode initialize() override ; + + /// Finalize + StatusCode finalize() override ; + + /// Method to fit a vertex + StatusCode fit( LHCb::Vertex& vertex, + const LHCb::Particle::ConstVector& daughters) const override final + { + LHCb::Particle* mother = 0 ; + return fit( daughters, vertex, mother ) ; + } + + /// Method to fit a vertex returning a Particle (that should already know its PID) + StatusCode fit + ( const LHCb::Particle::ConstVector& daughters, + LHCb::Vertex& vertex, + LHCb::Particle& mother) const override final + { + return fit(daughters,vertex,&mother) ; + } + + + StatusCode reFit( LHCb::Particle& particle ) const override final + { + LHCb::Vertex* vertex = particle.endVertex() ; + return fit( particle.daughtersVector(), + *vertex , particle ) ; + } + + StatusCode combine + ( const LHCb::Particle::ConstVector& daughter, + LHCb::Particle& mother , + LHCb::Vertex& vertex ) const override final + { + //FIXME: shouldn't this also set the pointers to the daughters?! + return fit ( daughter , vertex , mother ) ; + } + + + StatusCode add(const LHCb::Particle*, + LHCb::Vertex& ) const override final { + Error("add is not implemented for ParticleVertexFitterter (though quite trivial)"); + return StatusCode::FAILURE; + } + + StatusCode remove(const LHCb::Particle*, + LHCb::Vertex& ) const override final { + Error("remove is not implemented for ParticleVertexFitterter"); + return StatusCode::FAILURE; + } + +private: + StatusCode fit ( const LHCb::Particle::ConstVector&,LHCb::Vertex&, LHCb::Particle* ) const ; +private: + int m_maxnumiter = 5 ; + double m_maxdchisq = 0.01; + ToolHandle<ITrackStateProvider> m_stateprovider ; + ToolHandle<ITrajPoca> m_trajpoca ; + const LHCb::IParticlePropertySvc* m_ppsvc ; +} ; + +DECLARE_TOOL_FACTORY( ParticleVertexFitter ) + +ParticleVertexFitter::ParticleVertexFitter( const std::string& type, + const std::string& name, + const IInterface* parent ) +: GaudiTool ( type, name , parent ), + m_stateprovider("TrackStateProvider"), + m_trajpoca("TrajPoca"), + m_ppsvc(0) +{ +} + +/// Initialize +StatusCode ParticleVertexFitter::initialize() +{ + StatusCode sc = GaudiTool::initialize() ; + if( !sc.isSuccess() ) return sc ; + sc = m_stateprovider.retrieve() ; + if( !sc.isSuccess() ) return sc ; + sc = m_trajpoca.retrieve() ; + m_ppsvc = svc<LHCb::IParticlePropertySvc>("LHCb::ParticlePropertySvc"); + return sc ; +} + +/// Finalize +StatusCode ParticleVertexFitter::finalize() +{ + m_stateprovider.release().ignore() ; + m_trajpoca.release().ignore() ; + return GaudiTool::finalize() ; +} + +namespace { + + // Class that represents a state-vector (x,y,tx,ty) at a given z. In + // constrast to 'LHCb/StateVector' it does not have q/p, becaudse we + // do not need that for vertexing. + class StateVector4 + { + private: + double m_z ; + Gaudi::Vector2 m_x ; + Gaudi::Vector2 m_tx ; + public: + StateVector4() = default ; + template<class State> + StateVector4( const State& s ) + : m_z(s.z()) { + m_x(0) = s.x() ; + m_x(1) = s.y() ; + m_tx(0) = s.tx() ; + m_tx(1) = s.ty() ; + } + template<class XYZPoint, class XYZVector> + StateVector4( const XYZPoint& point, const XYZVector& direction) + : m_z(point.z()) { + m_x(0) = point.x() ; + m_x(1) = point.y() ; + m_tx(0) = direction.x()/direction.z() ; + m_tx(1) = direction.y()/direction.z() ; + } + double z() const { return m_z; } + double x() const { return m_x(0) ; } + double y() const { return m_x(1) ; } + double tx() const { return m_tx(0) ; } + double ty() const { return m_tx(1) ; } + const auto& slopes() const { return m_tx ; } + auto& z() { return m_z; } + auto& x() { return m_x(0) ; } + auto& y() { return m_x(1) ; } + auto& tx() { return m_tx(0) ; } + auto& ty() { return m_tx(1) ; } + } ; + + class State4 : public StateVector4 + { + public: + const auto& covXX() const { return m_covXX ; } + const auto& covXT() const { return m_covXT ; } + const auto& covTT() const { return m_covTT ; } + auto& covXX() { return m_covXX ; } + auto& covXT() { return m_covXT ; } + auto& covTT() { return m_covTT ; } + + // if we return sub matrices, it must be via expressions. wouldn't + // it just make sense to store the + //auto& covariance() { return m_covariance ; } + void linearTransportTo(double z) { + const double dz = z - StateVector4::z() ; + const double dz2 = dz*dz ; + x() += dz * tx() ; + y() += dz * ty() ; + m_covXX(0,0) += dz2*m_covTT(0,0) + 2*dz*m_covXT(0,0) ; + m_covXX(1,1) += dz2*m_covTT(1,1) + 2*dz*m_covXT(1,1) ; + m_covXX(1,0) += dz2*m_covTT(1,0) + dz*(m_covXT(0,1)+m_covXT(1,0)) ; + // would this make covXT always symmetric as well? + m_covXT(0,0) += dz*m_covTT(0,0) ; + m_covXT(0,1) += dz*m_covTT(0,1) ; + m_covXT(1,0) += dz*m_covTT(1,0) ; + m_covXT(1,1) += dz*m_covTT(1,1) ; + //m_covXT += dz*m_covTT ; + StateVector4::z() = z ; + } + private: + //Gaudi::SymMatrix4x4 m_covariance ; + Gaudi::SymMatrix2x2 m_covXX ; + Gaudi::SymMatrix2x2 m_covTT ; + Gaudi::Matrix2x2 m_covXT ; + } ; + + inline Gaudi::SymMatrix2x2 covXX( const State4& s) { return s.covXX() ; } + inline Gaudi::SymMatrix2x2 covXX( const LHCb::State& s ) { return s.covariance().Sub<Gaudi::SymMatrix2x2>(0,0) ; } + inline Gaudi::Matrix2x2 covXT( const State4& s) { return s.covXT() ; } + inline Gaudi::Matrix2x2 covXT( const LHCb::State& s ) { return s.covariance().Sub<Gaudi::Matrix2x2>(0,2) ; } + + inline State4 stateVectorFromComposite( const LHCb::Particle& p ) + { + const auto& px = p.momentum().Px() ; + const auto& py = p.momentum().Py() ; + const auto& pz = p.momentum().Pz() ; + const double tx = px/pz ; + const double ty = py/pz ; + + State4 s; //( p.endVertex()->position(), p.momentum() ) ; + s.x() = p.endVertex()->position().x() ; + s.y() = p.endVertex()->position().y() ; + s.z() = p.endVertex()->position().z() ; + s.tx() = tx ; + s.ty() = ty ; + + // For the computation of the Jacobian it is important to understand the following. + // x' = x + (z' - z) * tx + // --> dx/dz = - tx ; + // + // Notation: + // J_{4,6} = ( Jxpos Jxmom ) + // ( Jtxpos Jtxmom ) + // Jtxpos = 0 and Jxmom=0, so we rather do not introduce them. Instead, we'll compute Jxpos and Jxtxmom + // + // however, to test what we are doing, we first go full monty: + // { + // // only 8 out of 24 elements are non-zero, so this is far too slow. we'll fix that later + // ROOT::Math::SMatrix<double,4,6> J ; + // J(0,0) = J(1,1) = 1 ; + // J(0,2) = -tx ; + // J(1,2) = -ty ; + // J(2,3) = 1/pz ; + // J(3,4) = 1/pz ; + // J(2,5) = -tx/pz ; + // J(3,5) = -ty/pz ; + // auto cov44 = ROOT::Math::Similarity( J, p.covMatrix().Sub<Gaudi::SymMatrix6x6>(0,0) ) ; + // std::cout << "cov44 full calculation: " << cov44 << std::endl ; + // } + ROOT::Math::SMatrix<double,2,3> Jxpos ; + Jxpos(0,0) = Jxpos(1,1) = 1 ; + Jxpos(0,2) = -tx ; + Jxpos(1,2) = -ty ; + ROOT::Math::SMatrix<double,2,3> Jtxmom ; + Jtxmom(0,0) = 1/pz ; + Jtxmom(1,1) = 1/pz ; + Jtxmom(0,2) = -tx/pz ; + Jtxmom(1,2) = -ty/pz ; + Gaudi::SymMatrix4x4 cov44alt ; + const auto& momposcov = p.posMomCovMatrix() ; // it has the wrong name: it mompos not posmom + s.covXT() = Jxpos * ROOT::Math::Transpose( momposcov.Sub<Gaudi::Matrix3x3>(0,0) ) * + ROOT::Math::Transpose( Jtxmom ) ; + //Jtxmom * momposcov.Sub<Gaudi::Matrix3x3>(0,0) * ROOT::Math::Transpose(Jxpos) ; + s.covXX() = ROOT::Math::Similarity( Jxpos, p.posCovMatrix()) ; + s.covTT() = ROOT::Math::Similarity( Jtxmom, p.momCovMatrix().Sub<Gaudi::SymMatrix3x3>(0,0)) ; + return s ; + } + + enum DaughterType { TrackWithVelo, TrackWithoutVelo, Composite, Resonance, Other, NumTypes } ; + + class VertexDaughter + { + public: + VertexDaughter( const LHCb::Particle& p ) : m_particle(&p) {} + const LHCb::Particle& particle() const { return *m_particle ; } + protected: + const LHCb::Particle* m_particle ; + } ; + + template<typename Trait> + class VertexTraj : public VertexDaughter + { + private: + typename Trait::State m_state ; + ROOT::Math::SVector<double,2> m_q ; // predicted/fitted slope (tx,ty) + Gaudi::SymMatrix2x2 m_G ; // weight matrix of (x,y) of state + ROOT::Math::SMatrix<double,2,3> m_A ; // projection matrix for vertex position + public: + VertexTraj( const LHCb::Particle& p, const typename Trait::State& state ) + : VertexDaughter{p}, m_state{state}, m_q{state.tx(),state.ty()} {} + + const auto& state() const { return m_state ; } + + void project( const ROOT::Math::SVector<double,3>& vertexpos, + ROOT::Math::SVector<double,3>& halfDChisqDX, + Gaudi::SymMatrix3x3& halfD2ChisqDX2, + double& chi2, + int& ndof) /* override final */ + { + // move the state (not sure we should do it this way: maybe better to just cache the z.) + m_state.linearTransportTo( vertexpos(2) ) ; + + // compute the weight matrix + m_G = covXX(m_state) ; + m_G.Invert() ; + + // compute residual + Gaudi::Vector2 res ; + res(0) = vertexpos(0) - m_state.x() ; + res(1) = vertexpos(1) - m_state.y() ; + + // fill the projection matrix: use the fitted momentum! + m_A(0,0) = m_A(1,1) = 1 ; + m_A(0,2) = - m_q(0) ; + m_A(1,2) = - m_q(1) ; + + // I tried to make this faster by writing it out, but H does + // not contain sufficiently manby zeroes. Better to + // parallelize. + halfD2ChisqDX2 += ROOT::Math::Similarity( ROOT::Math::Transpose(m_A), m_G ) ; + halfDChisqDX += ( ROOT::Math::Transpose(m_A) * m_G) * res ; + chi2 += ROOT::Math::Similarity(res,m_G) ; + ndof += 2 ; + } + + void updateSlopes( const ROOT::Math::SVector<double,3>& vertexpos ) /* override final */ + { + // first update the residual. (note the subtle difference with that in project!) + Gaudi::Vector2 res ; + const double dz = vertexpos(2) - m_state.z() ; + res(0) = vertexpos(0) - (m_state.x() + m_state.tx()*dz) ; + res(1) = vertexpos(1) - (m_state.y() + m_state.ty()*dz) ; + + // get the matrix that is the correlation of (x,y) and (tx,ty,qop) + //ROOT::Math::SMatrix<double,2,2> Vba = m_state.covariance().template Sub<ROOT::Math::SMatrix<double,2,2>>(2,0) ; + // compute the corresponding gain matrix (this is WBG in the BFR fit, but here it is Vba * Vaa^-1) + //ROOT::Math::SMatrix<double,2,2> K = Vba*m_G ; + // compute the momentum vector + m_q(0) = m_state.tx() ; + m_q(1) = m_state.ty() ; + m_q += ROOT::Math::Transpose(covXT(m_state)) * m_G * res ; + } + + void addToFourVector( const ROOT::Math::SVector<double,3>& vertexpos, + Gaudi::LorentzVector& p4, + Gaudi::SymMatrix4x4& p4cov, + ROOT::Math::SMatrix<double,4,3>& gainmatrix ) const /* override final */ ; + } ; + + /*********** + * class to deal with composites + ***********/ + struct CompositeTrait + { + using State = State4 ; + } ; + + using VertexComposite = VertexTraj<CompositeTrait> ; + + template<> + void VertexComposite::addToFourVector( const ROOT::Math::SVector<double,3>& vertexpos, + Gaudi::LorentzVector& p4, + Gaudi::SymMatrix4x4& p4cov, + ROOT::Math::SMatrix<double,4,3>& gainmatrix ) const + { + // first need to 'update' the momentum vector. but for that we + // first need to 'transport'. or probably not? + const auto& ppos = particle().endVertex()->position() ; + const auto& mom = particle().momentum() ; + const double pz = mom.Pz() ; + const double tx = mom.Px()/pz ; + const double ty = mom.Py()/pz ; + + // first update the residual + Gaudi::Vector2 res ; + const double dz = vertexpos(2) - ppos.z() ; + res(0) = vertexpos(0) - (ppos.x() + tx*dz) ; + res(1) = vertexpos(1) - (ppos.y() + ty*dz) ; + + const auto& R = m_state.covXX() ; + const auto& Rinv = m_G ; + + // To do this right we need THREE projection matrices for the residual: + // r = A*x_vertex + B*mom_dau + C*x_dau + // Note that C=-A. + // Lets call the matrix F^T = (B^T C^T). then the uncertainty on the residual is + // R = F V77 F^T where V77 is the 7x7 covariance matrix of the daughter + // We will need R every iteration + // The correlation matrix between residual and the 7 parameters of the daughter is + // Vr_xmom = V77 F^T + // Consequently, the gain matrix for the momentum-decayvertex is + // K72 = V77 F^T R^-1 + // Now, we do not need the updated parameters for the decay + // vertex. If we just need the momentum part, then this reduces to + // K42 = ( V43 * C^T + V44 * B^T ) R^-1 + // Projection matrix for position + Gaudi::Matrix2x3 m_A ; + m_A(0,0)=m_A(1,1)=1 ; + m_A(0,2)=-tx ; + m_A(1,2)=-ty ; + // This is the full calculation, but because there are so many + // zeroes in these matrices, it is now written out. Somebody could + // sitll have some fun for vector computations here. + + const auto& momCov = particle().momCovMatrix() ; + const auto& momPosCov = particle().posMomCovMatrix() ; + // // only 2/3 of CT is nonzero + // Gaudi::Matrix3x2 CT{ ROOT::Math::Transpose(m_A) } ; + // ROOT::Math::SMatrix<double,4,2> BT ; + // // only half of B is non-zero + // BT(0,0) = +dz/pz; + // BT(2,0) = -dz/pz*tx ; + // BT(1,1) = +dz/pz ; + // BT(2,1) = -dz/pz*ty ; + // ROOT::Math::SMatrix<double,4,2> K42 = ( momPosCov * CT + momCov * BT ) * Rinv ; + ROOT::Math::SMatrix<double,4,2> K42part ; + for(int i=0; i<4; ++i) + K42part(i,0) = momPosCov(i,0) + momPosCov(i,2) * (-tx) + momCov(i,0) * (dz/pz) + momCov(i,2) * (-dz/pz*tx) ; + for(int i=0; i<4; ++i) + K42part(i,1) = momPosCov(i,1) + momPosCov(i,2) * (-ty) + momCov(i,1) * (dz/pz) + momCov(i,2) * (-dz/pz*ty) ; + ROOT::Math::SMatrix<double,4,2> K42 = K42part * Rinv ; + + // update the fourvector + auto deltap4 = K42 * res ; + p4 += particle().momentum() + + Gaudi::LorentzVector( deltap4(0), deltap4(1), deltap4(2), deltap4(3) ) ; + // to understand this, compare to what is done for the track + p4cov += particle().momCovMatrix() ; + p4cov -= ROOT::Math::Similarity( K42, R ) ; + gainmatrix += K42*m_A ; + } + + /*********** + * class to deal with tracks + ***********/ + + struct TrackTrait + { + using State = LHCb::State ; + } ; + + using VertexTrack = VertexTraj<TrackTrait> ; + + template<> + void VertexTrack::addToFourVector( const ROOT::Math::SVector<double,3>& vertexpos, + Gaudi::LorentzVector& p4, + Gaudi::SymMatrix4x4& p4cov, + ROOT::Math::SMatrix<double,4,3>& gainmatrix ) const + { + // we first need to update q/p. since we also need the full gain matrix, we could as well redo that part. + // get the matrix that is the correlation of (x,y) and (tx,ty,qop) + Gaudi::Vector2 res ; + const double dz = vertexpos(2) - m_state.z() ; + res(0) = vertexpos(0) - (m_state.x() + m_q(0)*dz) ; + res(1) = vertexpos(1) - (m_state.y() + m_q(1)*dz) ; + auto Vba = m_state.covariance().Sub<ROOT::Math::SMatrix<double,3,2> >(2,0) ; + ROOT::Math::SMatrix<double,3,2> K = Vba*m_G ; + Gaudi::Vector3 q = m_state.stateVector().Sub<Gaudi::Vector3>(2) ; + q += K*res ; + // now transform to p4 + Gaudi::LorentzVector p4tmp ; + const double mass = particle().measuredMass() ; + Gaudi::Math::geo2LA(q,mass,p4tmp); + p4 += p4tmp ; + ROOT::Math::SMatrix<double,4,3> dP4dMom ; + Gaudi::Math::JacobdP4dMom(q,mass,dP4dMom ); + ROOT::Math::SMatrix<double,4,2> FK = dP4dMom * K ; + p4cov += ROOT::Math::Similarity( dP4dMom, m_state.covariance().Sub<Gaudi::SymMatrix3x3>(2,2) ) ; + p4cov -= ROOT::Math::Similarity( FK, m_state.covariance().Sub<Gaudi::SymMatrix2x2>(0,0) ) ; + gainmatrix += FK * m_A ; + + // For brem-recovered electrons, we need to do something + // special. So, electrons are an ugly exception here. We could + // also add to q/p instead, which is cheaper, but perhaps even + // more ugly. + if( particle().particleID().abspid() == 11 ) { + const double absqop = std::abs(m_state.qOverP() ) ; + const double momentumFromParticle = particle().momentum().P() ; + // is 1% a reasonable threshold? + if( momentumFromParticle * absqop > 1.01 ) { + const double momentumFromTrack = 1/absqop ; + const double momentumError2FromTrack = m_state.covariance()(4,4) * std::pow(momentumFromTrack,4) ; + const double momentumError2FromParticle = + particle().momCovMatrix()(3,3) * std::pow(particle().momentum().E()/momentumFromParticle,2) ; + const double bremEnergyCov = momentumError2FromParticle - momentumError2FromTrack ; + const double bremEnergy = momentumFromParticle - momentumFromTrack ; + // if the correction is too small or unphysical, ignore it. the units are MeV. + if( bremEnergy>1*Gaudi::Units::MeV && + bremEnergyCov >1*Gaudi::Units::MeV*Gaudi::Units::MeV ) { + const auto tx = m_state.tx() ; + const auto ty = m_state.ty() ; + auto t = std::sqrt(1+tx*tx+ty*ty) ; + // we could also 'scale' the momentum, but we anyway need the components for the jacobian + Gaudi::LorentzVector p4brem{ bremEnergy*tx/t, bremEnergy*ty/t, bremEnergy/t, bremEnergy} ; + p4 += p4brem ; + ROOT::Math::SMatrix<double,4,1> J; + J(0,0) = tx/t; J(1,0) = ty/t ; J(2,0) = 1/t; J(3,0) = 1; + p4cov += ROOT::Math::Similarity( J, Gaudi::SymMatrix1x1{bremEnergyCov} ) ; + } + } + } + } + + //************************************************* + // Resonance + //************************************************* + class VertexResonance : public VertexDaughter + { + private: + Gaudi::Vector3 m_pos ; + Gaudi::SymMatrix3x3 m_G ; + public: + VertexResonance(const LHCb::Particle& p) + : VertexDaughter(p) + { + const auto& pos = p.endVertex()->position() ; + m_pos = Gaudi::Vector3{pos.x(),pos.y(),pos.z()} ; + const auto& cov = p.endVertex()->covMatrix() ; + m_G = cov ; + m_G.InvertChol() ; + } + + void project( const ROOT::Math::SVector<double,3>& vertexpos, + ROOT::Math::SVector<double,3>& halfDChisqDX, + Gaudi::SymMatrix3x3& halfD2ChisqDX2, + double& chi2, + int& ndof) /* override final */ + { + Gaudi::Vector3 res = vertexpos - m_pos ; + + + // I tried to make this faster by writing it out, but H does + // not contain sufficiently manby zeroes. Better to + // parallelize. + halfD2ChisqDX2 += m_G ; + halfDChisqDX += m_G * res ; + chi2 += ROOT::Math::Similarity(res,m_G) ; + ndof += 3 ; + } + + void addToFourVector( const ROOT::Math::SVector<double,3>& vertexpos, + Gaudi::LorentzVector& p4, + Gaudi::SymMatrix4x4& p4cov, + ROOT::Math::SMatrix<double,4,3>& gainmatrix ) const + { + // compute momentum gain matrix + ROOT::Math::SMatrix<double,4,3> K43 = particle().posMomCovMatrix() * m_G ; + // update the fourvector + Gaudi::Vector3 res = vertexpos - m_pos ; + auto deltap4 = K43 * res ; + p4 += particle().momentum() + + Gaudi::LorentzVector( deltap4(0), deltap4(1), deltap4(2), deltap4(3) ) ; + // to understand this, compare to what is done for the track + p4cov += particle().momCovMatrix() ; + const auto& poscov = particle().endVertex()->covMatrix() ; + p4cov -= ROOT::Math::Similarity( K43, poscov) ; + gainmatrix += K43 ; + } + } ; + + inline DaughterType derivetype( const LHCb::Particle& p, + const LHCb::IParticlePropertySvc& ppsvc ) + { + DaughterType rc = Other ; + if( p.proto() && p.proto()->track() ) { + if( p.proto()->track()->hasVelo() ) + rc = TrackWithVelo ; + else + rc = TrackWithoutVelo ; + } else if(p.daughters().size() > 0 && p.endVertex() && p.endVertex()->nDoF()>0) { + const LHCb::ParticleProperty* prop = ppsvc.find(p.particleID()); + bool isresonance = prop && prop->ctau() < 0.001*Gaudi::Units::mm ; + if( isresonance ) + rc = Resonance ; + else + rc = Composite ; + } else { + rc = Other ; + } + return rc ; + } +} + + +StatusCode ParticleVertexFitter::fit( const LHCb::Particle::ConstVector& daughters, + LHCb::Vertex& vertex, + LHCb::Particle* mother) const +{ + // voor the vertex fit, we distinguish: + // - TracksWithVelo + // - TracksWithoutVelo tracks without velo information (only downstream, hopefully) + // - Resonances: composites with ctau<1micron + // - Composities: other composites + // - Others: everything else, e.g. photons, pi0, jets + + // before we do anything, we actually need an estimate of the + // vertex position. consequently, let's first try to do that. + size_t N = daughters.size() ; + std::vector<DaughterType> types(N) ; + std::transform( daughters.begin(), daughters.end(), types.begin(), + [=]( const LHCb::Particle* p) { return derivetype(*p,*m_ppsvc) ; } ) ; + std::array<unsigned char,NumTypes> counttypes{0} ; + std::for_each( types.begin(), types.end(), [&]( const DaughterType& t ) { counttypes[int(t)] += 1 ; } ) ; + + // Here is wat we do: + // 1. if there are 'resonance' daughters, take the vertex position of the first. + // 2. if not, use the states of the first two tracks with velo or composites + // 3. if not, try with downstream tracks and trajpoca + bool posinitialized=false ; + Gaudi::XYZPoint position ; + if( counttypes[Resonance] > 0 ) { + // try 1 + for(size_t i=0; i<N && !posinitialized; ++i) + if( types[i] == Resonance ) { + position = daughters[i]->endVertex()->position() ; + posinitialized = true ; + } + assert(posinitialized) ; + } else if( counttypes[TrackWithVelo]+counttypes[Composite]>=2) { + // try 2 + StateVector4 velotrajs[2] ; + unsigned char nvelotrajs(0) ; + for(size_t i=0; i<N && nvelotrajs<2; ++i) { + if( types[i] == TrackWithVelo ) { + velotrajs[nvelotrajs++] = StateVector4( daughters[i]->proto()->track()->firstState() ) ; + } else if( types[i] == Composite) { + velotrajs[nvelotrajs++] = StateVector4( daughters[i]->endVertex()->position(), + daughters[i]->momentum() ) ; + } + } + if( nvelotrajs>=2 ) { + LHCb::TrackVertexUtils::poca( velotrajs[0],velotrajs[1],position) ; + posinitialized = true ; + } + assert(posinitialized) ; + } else if( counttypes[TrackWithVelo]+counttypes[Composite]+counttypes[TrackWithoutVelo]>=2) { + // try 3. create two trajectories, then use trajpoca + std::vector< LHCb::LineTraj > ownedtrajs ; + const LHCb::Trajectory* trajs[2] ; + unsigned char ntrajs(0) ; + for(size_t i=0; i<N && ntrajs<2; ++i) { + if( types[i] == TrackWithVelo || types[i] == TrackWithoutVelo) { + auto traj = m_stateprovider->trajectory( *(daughters[i]->proto()->track() ) ) ; + if(traj) trajs[ntrajs++] = traj ; + } else if( types[i] == Composite) { + ownedtrajs.emplace_back(daughters[i]->endVertex()->position(), + daughters[i]->momentum().Vect(), std::make_pair(0.,1.) ) ; + trajs[ntrajs++] = &(ownedtrajs.back()) ; + } + } + if(ntrajs>=2) { + double mu0(0), mu1(0) ; + Gaudi::XYZVector deltaX; + StatusCode sc = m_trajpoca->minimize( *(trajs[0]),mu0,*(trajs[1]),mu1,deltaX,0.1*Gaudi::Units::mm ); + if( sc.isSuccess() ) { + auto pos0 = trajs[0]->position( mu0 ) ; + auto pos1 = trajs[1]->position( mu1 ) ; + position = pos0 + 0.5*(pos1-pos0) ; + posinitialized = true ; + } + } + } + + if(!posinitialized) { + error() << "ParticleVertexFitter didn't properly initialize: " + << " #tracks with velo: " << counttypes[TrackWithVelo] + << " #tracks without velo: " << counttypes[TrackWithoutVelo] + << " #composites: " << counttypes[Composite] + << endmsg ; + return StatusCode::FAILURE ; + } + + // Now that we have an estimate of the vertex, we initialize everything that goes into the vertex + // that we are fitting. + std::vector< VertexTrack > vertextracks ; + std::vector< VertexComposite > vertexcomposites ; + std::vector< VertexResonance > vertexresonances ; + std::vector< const LHCb::Particle* > otherdaughters ; + // the last trick: as long as the current estimate tells us + // that we are within the beampipe, we could as well just use + // the first state. + const bool vertexinsidefoil = position.x()*position.x() + position.y()*position.y() < 8.*8.; // mm + for(size_t i=0; i<N; ++i) { + auto p = daughters[i] ; + switch( types[i] ) { + case TrackWithVelo: + { + const LHCb::Track* track = p->proto()->track() ; + if( vertexinsidefoil ) + vertextracks.emplace_back( *p,track->firstState() ); + else { + //state = linearstateinterpolation( *(p->proto()->track()->states()[0]), + //*(p->proto()->track()->states()[1] ), + //position.z() ) ; + //vertextracks.push_back( VertexTrack{*p,std::move(state)} ) ; + const LHCb::TrackTraj* tracktraj = m_stateprovider->trajectory(*(p->proto()->track())) ; + vertextracks.emplace_back( *p,tracktraj?tracktraj->state(position.z()):track->firstState() ) ; + } + } + break ; + case TrackWithoutVelo: + { + const LHCb::Track* track = p->proto()->track() ; + const LHCb::TrackTraj* tracktraj = m_stateprovider->trajectory(*(p->proto()->track())) ; + vertextracks.emplace_back( *p,tracktraj?tracktraj->state(position.z()):track->firstState() ) ; + } + break ; + case Composite: + vertexcomposites.emplace_back( *p,stateVectorFromComposite(*p) ) ; + break ; + case Resonance: + vertexresonances.emplace_back( *p ) ; + break ; + case Other: + otherdaughters.push_back( p ) ; + default: + warning() << "Cannot yet deal with this daughter: " << p->particleID() << endmsg ; + } + } + + // run vertex fit iterations + Gaudi::Vector3 posvec ; + posvec(0) = position.x() ; + posvec(1) = position.y() ; + posvec(2) = position.z() ; + Gaudi::SymMatrix3x3 poscov ; + double chi2(0) ; + int ndof(0) ; + bool converged(false) ; + StatusCode sc = StatusCode::SUCCESS ; + unsigned short iter(0) ; + for(iter=0; iter<m_maxnumiter && !converged; ++iter) { + chi2=0 ; + ndof=-3 ; + Gaudi::SymMatrix3x3 halfD2ChisqDX2 ; + ROOT::Math::SVector<double,3> halfDChisqDX ; + + // add all particles that contribute to the vertex chi2 + for( auto& dau : vertextracks) + dau.project( posvec, halfDChisqDX, halfD2ChisqDX2, chi2, ndof ) ; + for( auto& dau : vertexcomposites) + dau.project( posvec, halfDChisqDX, halfD2ChisqDX2, chi2, ndof ) ; + for( auto& dau : vertexresonances) + dau.project( posvec, halfDChisqDX, halfD2ChisqDX2, chi2, ndof ) ; + + // calculate the covariance and the change in the position + poscov = halfD2ChisqDX2 ; + const bool ok = poscov.InvertChol(); + if( !ok ) { + warning() << "Problem inverting matrix!" << endmsg ; + return StatusCode::FAILURE ; + } else { + ROOT::Math::SVector<double,3> dpos = - poscov * halfDChisqDX ; + // update the position + posvec += dpos ; + // compute the delta-chisquare + double dchisq = ROOT::Math::Dot(dpos,halfDChisqDX) ; + chi2 += dchisq ; + converged = -dchisq < m_maxdchisq ; + + // update the slopes + if(!converged) { + for( auto& dau : vertextracks) dau.updateSlopes(posvec) ; + for( auto& dau : vertexcomposites) dau.updateSlopes(posvec) ; + } + } + } + + // Now fill the particle + position.SetX( posvec(0) ) ; + position.SetY( posvec(1) ) ; + position.SetZ( posvec(2) ) ; + + vertex.setPosition( position ) ; + vertex.setChi2AndDoF( chi2, ndof ) ; + vertex.setCovMatrix( poscov ) ; + if(mother) { + // copy some stuff from the vertex + mother->setReferencePoint( vertex.position()) ; + mother->setPosCovMatrix( vertex.covMatrix() ) ; + // compute the total fourvector and all other parts of the covariance matrix + Gaudi::LorentzVector p4 ; + Gaudi::SymMatrix4x4 p4cov ; + ROOT::Math::SMatrix<double,4,3> gainmatrix ; + for( const auto& dau : vertextracks) dau.addToFourVector( posvec, p4, p4cov, gainmatrix ) ; + for( const auto& dau : vertexcomposites) dau.addToFourVector( posvec, p4, p4cov, gainmatrix ) ; + for( const auto& dau : vertexresonances) dau.addToFourVector( posvec, p4, p4cov, gainmatrix ) ; + p4cov += ROOT::Math::Similarity( gainmatrix, poscov ) ; + for( const auto& dau : otherdaughters) { + p4 += dau->momentum() ; + p4cov += dau->momCovMatrix() ; + } + Gaudi::Matrix4x3 covmompos = gainmatrix * poscov ; + mother->setMomentum(p4) ; + mother->setMomCovMatrix( p4cov ) ; + mother->setPosMomCovMatrix( covmompos ) ; + } + return StatusCode::SUCCESS ; +} +