From 72521a7ce8707afe3eb6221ccf0441a6b0ad6964 Mon Sep 17 00:00:00 2001
From: Wouter Hulsbergen <wouter.hulsbergen@nikhef.nl>
Date: Fri, 26 Jan 2018 15:13:59 +0100
Subject: [PATCH 1/3] Added ParticleVertexFitter

---
 Phys/VertexFit/src/ParticleVertexFitter.cpp | 860 ++++++++++++++++++++
 1 file changed, 860 insertions(+)
 create mode 100644 Phys/VertexFit/src/ParticleVertexFitter.cpp

diff --git a/Phys/VertexFit/src/ParticleVertexFitter.cpp b/Phys/VertexFit/src/ParticleVertexFitter.cpp
new file mode 100644
index 000000000..c408ba5d7
--- /dev/null
+++ b/Phys/VertexFit/src/ParticleVertexFitter.cpp
@@ -0,0 +1,860 @@
+#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"
+
+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 final {
+    StatusCode sc = GaudiTool::initialize() ;
+    if( !sc.isSuccess() ) return sc ;
+    sc = m_stateprovider.retrieve() ;
+    if( !sc.isSuccess() ) return sc ;
+    sc = m_trajpoca.retrieve() ;
+    return sc ;
+  }
+
+  /// Finalize
+  StatusCode finalize() override final {
+    m_stateprovider.release().ignore() ;
+    m_trajpoca.release().ignore() ;
+    return GaudiTool::finalize() ;
+  }
+
+  /// 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 ;
+  VertexDaughter* createVertexDaughter(const LHCb::Particle& p) const ;
+private:
+  int m_maxnumiter = 10 ;
+  double m_maxdchisq = 0.1;
+  ToolHandle<ITrackStateProvider> m_stateprovider ;
+  ToolHandle<ITrajPoca> m_trajpoca ;
+} ;
+
+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")
+{
+}
+
+namespace {
+
+  LHCb::State linearstateinterpolation(LHCb::State s1,
+				       LHCb::State s2,
+				       double z)
+  {
+    const double w1 = (s2.z() - z) / (s2.z() - s1.z()) ;
+    if( w1>=1 ) return s1 ;
+    if( w1<=0 ) return s2 ;
+    s1.linearTransportTo(z);
+    s2.linearTransportTo(z);    
+    return
+      // w1>1 ? s1 :
+      // ( w1<0 ? s2 :
+	LHCb::State( w1*s1.stateVector() + (1-w1)*s2.stateVector(),
+		     w1*s1.covariance()  + (1-w1)*s2.covariance(),
+		     z, LHCb::State::LocationUnknown ) ;//  );
+  }
+  
+  enum DaughterType { TrackWithVelo, TrackWithoutVelo, Composite, Resonance, Other, NumTypes } ;
+   
+  class VertexDaughter
+  {
+  public:
+    VertexDaughter( const LHCb::Particle& p ) : m_particle(&p) {}
+    /* I removed the virtual interface entirely
+    virtual void project( const ROOT::Math::SVector<double,3>& vertexpos,
+			  ROOT::Math::SVector<double,3>& halfDChisqDX,
+			  Gaudi::SymMatrix3x3&           halfD2ChisqDX2,
+			  double& chi2,
+			  int& ndof) = 0 ;
+    virtual void updateSlopes( const ROOT::Math::SVector<double,3>& vertexpos ) = 0 ;
+    virtual void addToFourVector( const ROOT::Math::SVector<double,3>& vertexpos,
+				  Gaudi::LorentzVector& p4,
+				  Gaudi::SymMatrix4x4& p4cov,
+				  ROOT::Math::SMatrix<double,4,3>& gainmatrix ) const = 0 ;
+    */
+    const LHCb::Particle& particle() const { return *m_particle ; }
+  protected:
+    const LHCb::Particle* m_particle ;
+  } ;
+
+  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)) ;
+      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) ;
+      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 EXTREMELY 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)) ;
+    // std::cout << "cov44 fast calculation: " << std::endl
+    // 		<< s.covXX() << std::endl
+    // 		<< s.covTT() << std::endl
+    // 		<< s.covXT() << std::endl ;
+    return s ;
+  }
+
+  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 ;
+      //m_q    += Vba*m_G*res ;
+      //m_q    += m_K * 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<>
+  // VertexComposite::VertexTraj(const LHCb::Particle& p)
+  //   : VertexDaughter(p),
+  //     m_state( stateVectorFromComposite(p) ),
+  //     m_q( m_state.slopes() )
+  // {
+  // }
+  
+  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
+    // The important part is not to forget V43.
+    //
+    // Now, something is wrong in this story: I am a factor -1 wrong
+    // in the gain matrix. So, for now multiply B and C by a minus
+    // factor and figure out the problem later.
+    Gaudi::Matrix2x3 m_A ;
+    m_A(0,0)=m_A(1,1)=1 ;
+    m_A(0,2)=-tx ;
+    m_A(1,2)=-ty ;
+
+    Gaudi::Matrix3x2 CT{ ROOT::Math::Transpose(m_A) } ;
+    //CT(0,0) = CT(1,1) = -1;
+    //CT(2,0) = tx ;
+    //CT(2,1) = ty ;
+    ROOT::Math::SMatrix<double,4,2> BT ; // only half non-zero!
+    BT(0,0) = +dz/pz;
+    BT(2,0) = -dz*tx/pz ;
+    BT(1,1) = +dz/pz ;
+    BT(2,1) = -dz*ty/pz ;
+    ROOT::Math::SMatrix<double,4,2> K42 = ( particle().posMomCovMatrix() * CT + particle().momCovMatrix() * BT ) * Rinv ;
+    // Then again, this is also already part of the computation of
+    // R. So, you can also do it once using the initial z-position,
+    // then forget about it.
+    
+    // Suppose that we would indeed use a 4-D state to filter the
+    // information from the composite. How much quicker is that to
+    // compute that what we do above? 
+    
+    // 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<>
+  // VertexTrack::VertexTraj(const LHCb::Particle& p)
+  //   : VertexDaughter(p),
+  //     m_state( p.proto()->track()->firstState() ),
+  //     m_q( m_state.stateVector().Sub<Gaudi::Vector2>(2))
+  // {
+  // }
+
+  // this should also work for electrons with bremcorrection, I hope.
+  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 ;
+  }
+
+  //*************************************************
+  // 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 ;
+    }
+  } ;
+}
+
+namespace {
+  DaughterType derivetype( const LHCb::Particle& p )
+  {
+    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) {
+      // can cipy this from OfflineVtxFitter
+      //auto prop = LoKi::Particles::ppFromPID(particle.particleID()) ;
+      bool isresonance = false ;//prop && prop->ctau() < 0.001*Gaudi::Units::mm ;
+      if( isresonance )
+	rc = Resonance ;
+      else
+	rc = Composite ;
+    } else {
+      rc = Other ;
+    }
+    return rc ;
+  }
+}
+
+// VertexDaughter* ParticleVertexFitter::createVertexDaughter(const LHCb::Particle& p) const
+// {
+//   VertexDaughter* rc(0) ;
+//   DaughterType thetype = derivetype(p) ;
+//   switch( thetype ) {
+//   case VeloTrack: rc =  new VertexTrack(p); break ;
+//   case Composite: rc = new VertexComposite(p); break ;
+//   default:
+//     warning() << "Cannot yet deal with this daughter: " << p.particleID() << endmsg ;
+//   }
+//   return rc ;
+// }   
+
+
+StatusCode ParticleVertexFitter::fit( const LHCb::Particle::ConstVector& daughters,
+			       LHCb::Vertex&                      vertex,
+			       LHCb::Particle*                    mother) const
+{
+  // need to separate between
+  // - tracks with velo information
+  // - tracks without velo information (only downstream, hopefully)
+  // - long-lived composites
+  // - short-lived composites
+  // - neutrals
+
+  // 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) ; } ) ;
+  std::array<unsigned char,NumTypes> counttypes{0} ;
+  std::for_each( types.begin(), types.end(), [&]( const DaughterType& t ) { counttypes[int(t)] += 1 ; } ) ;
+
+  // It all comes to this: can we get a reasonably estimate of the
+  // vertex position before actually creating the objects that go into
+  // the vertex? Or can we at least delay the expensive work until we
+  // know the vertex position? Should we use the trajectory
+  // approximation or not?
+
+  // let's work this out later. unfortunately, later is now. Here is wat we do:
+  // 1. make a list of resonance daughters
+  // 2. make a list of StateVector4 for velotracks and composites
+  // 3. make a list of tracktraj for downstream tracks
+  // the only problem is: how do we then deal with 'mixed' objects. we
+  // can also say: first try option 1; if that does not work, try
+  // option 2; if that doesn't work, try option 3 with TrajPoca etc.
+
+  bool posinitialized=false ;
+  Gaudi::XYZPoint position ;
+  if( counttypes[Resonance] > 0 ) {
+    // try 1
+    for(size_t i=0; i<N; ++i) 
+      if( types[i] == Resonance ) {
+	position = daughters[i]->endVertex()->position() ;
+	posinitialized = true ;
+	break ;
+      }
+    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 ;
+  }
+
+  /*
+  std::vector<VertexDaughter*> vertexdaughters ;
+  for( auto& dau: daughters) {
+    auto vdau = createVertexDaughter(*dau) ;
+    if( vdau) vertexdaughters.push_back( vdau ) ;
+  }
+  */
+
+  // it would of course be much simpler if we can apply a single loop
+  // to add all track-like objects. then we indeed compute the
+  // momentum update separately for each type after the vertex fit. let's implement something
+
+  // let's still try this for an alternative
+  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.
+  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:
+      {
+	// 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 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 ;
+    }
+  }
+  
+  //info() << "position: " << position << endmsg ;
+  /*
+  if( resonances.size() > 0 ) {
+    // take the position from the first resonances
+    position = resonances.front().endVertex()->position() ;
+  } else if( velotracks.size()>=2 ) {
+  */
+
+  // suppose that we now have the position, then what's the next step?
+
+  // run 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 ;
+  for(iter=0; iter<m_maxnumiter && !converged; ++iter) {
+    chi2=0 ;
+    ndof=-3 ;
+    Gaudi::SymMatrix3x3 halfD2ChisqDX2 ;
+    ROOT::Math::SVector<double,3> halfDChisqDX ;
+
+    // add all particles
+    
+    //for( const auto& dau : vertexdaughters)
+    //dau->project( posvec, halfDChisqDX, halfD2ChisqDX2, chi2, ndof ) ;
+    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 ) ;
+  //for( auto& daughter : daughters) 
+  //vertex.addToOutgoingParticles(daughter) ;
+  
+  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& itrack : vertexdaughters )
+    //itrack->addToFourVector( posvec, p4, p4cov, 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 ) ;
+    
+    // now add the daughters to particle and vertex
+    //for( auto& daughter : daughters) 
+    //mother->addToDaughters(daughter) ;
+  }
+  return StatusCode::SUCCESS ;
+}
+
-- 
GitLab


From 7c01270c5d1b645f0320f69dc84930747c57c73b Mon Sep 17 00:00:00 2001
From: Wouter Hulsbergen <wouter.hulsbergen@nikhef.nl>
Date: Sun, 28 Jan 2018 11:12:56 +0100
Subject: [PATCH 2/3] bug fix for resonances; cleanup

---
 Phys/VertexFit/src/ParticleVertexFitter.cpp | 321 ++++++++------------
 1 file changed, 127 insertions(+), 194 deletions(-)

diff --git a/Phys/VertexFit/src/ParticleVertexFitter.cpp b/Phys/VertexFit/src/ParticleVertexFitter.cpp
index c408ba5d7..0da6cb37a 100644
--- a/Phys/VertexFit/src/ParticleVertexFitter.cpp
+++ b/Phys/VertexFit/src/ParticleVertexFitter.cpp
@@ -7,6 +7,8 @@
 #include "TrackInterfaces/ITrackStateProvider.h"
 #include "Kernel/ITrajPoca.h"
 #include "Kernel/LineTraj.h"
+#include "Kernel/IParticlePropertySvc.h"
+#include "Kernel/ParticleProperty.h"
 
 namespace {
   class VertexDaughter ;
@@ -23,22 +25,11 @@ public:
 		 const IInterface* parent);
   
   /// Initialize
-  StatusCode initialize() override final {
-    StatusCode sc = GaudiTool::initialize() ;
-    if( !sc.isSuccess() ) return sc ;
-    sc = m_stateprovider.retrieve() ;
-    if( !sc.isSuccess() ) return sc ;
-    sc = m_trajpoca.retrieve() ;
-    return sc ;
-  }
-
+  StatusCode initialize() override ;
+  
   /// Finalize
-  StatusCode finalize() override final {
-    m_stateprovider.release().ignore() ;
-    m_trajpoca.release().ignore() ;
-    return GaudiTool::finalize() ;
-  }
-
+  StatusCode finalize() override ;
+  
   /// Method to fit a vertex
   StatusCode fit( LHCb::Vertex& vertex,
                   const LHCb::Particle::ConstVector& daughters) const override final
@@ -88,12 +79,12 @@ public:
 
 private:
   StatusCode fit ( const LHCb::Particle::ConstVector&,LHCb::Vertex&, LHCb::Particle* ) const ;
-  VertexDaughter* createVertexDaughter(const LHCb::Particle& p) const ;
 private:
-  int m_maxnumiter = 10 ;
-  double m_maxdchisq = 0.1;
+  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 )
@@ -103,52 +94,36 @@ ParticleVertexFitter::ParticleVertexFitter( const std::string& type,
 			      const IInterface* parent )
 : GaudiTool  ( type, name , parent ),
   m_stateprovider("TrackStateProvider"),
-  m_trajpoca("TrajPoca")
+  m_trajpoca("TrajPoca"),
+  m_ppsvc(0)
 {
 }
 
-namespace {
+/// 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 ;
+}
 
-  LHCb::State linearstateinterpolation(LHCb::State s1,
-				       LHCb::State s2,
-				       double z)
-  {
-    const double w1 = (s2.z() - z) / (s2.z() - s1.z()) ;
-    if( w1>=1 ) return s1 ;
-    if( w1<=0 ) return s2 ;
-    s1.linearTransportTo(z);
-    s2.linearTransportTo(z);    
-    return
-      // w1>1 ? s1 :
-      // ( w1<0 ? s2 :
-	LHCb::State( w1*s1.stateVector() + (1-w1)*s2.stateVector(),
-		     w1*s1.covariance()  + (1-w1)*s2.covariance(),
-		     z, LHCb::State::LocationUnknown ) ;//  );
-  }
-  
-  enum DaughterType { TrackWithVelo, TrackWithoutVelo, Composite, Resonance, Other, NumTypes } ;
-   
-  class VertexDaughter
-  {
-  public:
-    VertexDaughter( const LHCb::Particle& p ) : m_particle(&p) {}
-    /* I removed the virtual interface entirely
-    virtual void project( const ROOT::Math::SVector<double,3>& vertexpos,
-			  ROOT::Math::SVector<double,3>& halfDChisqDX,
-			  Gaudi::SymMatrix3x3&           halfD2ChisqDX2,
-			  double& chi2,
-			  int& ndof) = 0 ;
-    virtual void updateSlopes( const ROOT::Math::SVector<double,3>& vertexpos ) = 0 ;
-    virtual void addToFourVector( const ROOT::Math::SVector<double,3>& vertexpos,
-				  Gaudi::LorentzVector& p4,
-				  Gaudi::SymMatrix4x4& p4cov,
-				  ROOT::Math::SMatrix<double,4,3>& gainmatrix ) const = 0 ;
-    */
-    const LHCb::Particle& particle() const { return *m_particle ; }
-  protected:
-    const LHCb::Particle* m_particle ;
-  } ;
+/// 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:
@@ -207,10 +182,12 @@ namespace {
       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:
@@ -240,7 +217,7 @@ namespace {
     s.tx() = tx ;
     s.ty() = ty ;
 
-    // For the computation of the Jacobian it is EXTREMELY important to understand the following.
+    // For the computation of the Jacobian it is important to understand the following.
     //  x' = x + (z' - z) * tx
     // --> dx/dz = - tx ;
     //
@@ -279,13 +256,20 @@ namespace {
     //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)) ;
-    // std::cout << "cov44 fast calculation: " << std::endl
-    // 		<< s.covXX() << std::endl
-    // 		<< s.covTT() << std::endl
-    // 		<< s.covXT() << std::endl ;
     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
   {
@@ -348,8 +332,6 @@ namespace {
       m_q(0) = m_state.tx() ;
       m_q(1) = m_state.ty() ;
       m_q    += ROOT::Math::Transpose(covXT(m_state)) * m_G * res ;
-      //m_q    += Vba*m_G*res ;
-      //m_q    += m_K * res ;
     }
     
     void addToFourVector( const ROOT::Math::SVector<double,3>& vertexpos,
@@ -367,14 +349,6 @@ namespace {
   } ;
 
   using VertexComposite = VertexTraj<CompositeTrait> ;
-
-  // template<>
-  // VertexComposite::VertexTraj(const LHCb::Particle& p)
-  //   : VertexDaughter(p),
-  //     m_state( stateVectorFromComposite(p) ),
-  //     m_q( m_state.slopes() )
-  // {
-  // }
   
   template<>
   void VertexComposite::addToFourVector( const ROOT::Math::SVector<double,3>& vertexpos,
@@ -401,7 +375,7 @@ namespace {
     
     // 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!
+    // 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
@@ -412,34 +386,33 @@ namespace {
     // 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
-    // The important part is not to forget V43.
-    //
-    // Now, something is wrong in this story: I am a factor -1 wrong
-    // in the gain matrix. So, for now multiply B and C by a minus
-    // factor and figure out the problem later.
+    // 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 ;
-
-    Gaudi::Matrix3x2 CT{ ROOT::Math::Transpose(m_A) } ;
-    //CT(0,0) = CT(1,1) = -1;
-    //CT(2,0) = tx ;
-    //CT(2,1) = ty ;
-    ROOT::Math::SMatrix<double,4,2> BT ; // only half non-zero!
-    BT(0,0) = +dz/pz;
-    BT(2,0) = -dz*tx/pz ;
-    BT(1,1) = +dz/pz ;
-    BT(2,1) = -dz*ty/pz ;
-    ROOT::Math::SMatrix<double,4,2> K42 = ( particle().posMomCovMatrix() * CT + particle().momCovMatrix() * BT ) * Rinv ;
-    // Then again, this is also already part of the computation of
-    // R. So, you can also do it once using the initial z-position,
-    // then forget about it.
-    
-    // Suppose that we would indeed use a 4-D state to filter the
-    // information from the composite. How much quicker is that to
-    // compute that what we do above? 
-    
+    // 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() +
@@ -461,15 +434,6 @@ namespace {
 
   using VertexTrack = VertexTraj<TrackTrait> ;
 
-  // template<>
-  // VertexTrack::VertexTraj(const LHCb::Particle& p)
-  //   : VertexDaughter(p),
-  //     m_state( p.proto()->track()->firstState() ),
-  //     m_q( m_state.stateVector().Sub<Gaudi::Vector2>(2))
-  // {
-  // }
-
-  // this should also work for electrons with bremcorrection, I hope.
   template<>
   void VertexTrack::addToFourVector( const ROOT::Math::SVector<double,3>& vertexpos,
 					Gaudi::LorentzVector& p4,
@@ -497,6 +461,37 @@ namespace {
     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} ) ;
+	}
+      }
+    }
   }
 
   //*************************************************
@@ -555,10 +550,9 @@ namespace {
       gainmatrix += K43 ;
     }
   } ;
-}
 
-namespace {
-  DaughterType derivetype( const LHCb::Particle& p )
+  inline DaughterType derivetype( const LHCb::Particle& p,
+				  const LHCb::IParticlePropertySvc& ppsvc )
   {
     DaughterType rc = Other ;
     if( p.proto() && p.proto()->track() ) {
@@ -567,9 +561,8 @@ namespace {
       else
 	rc = TrackWithoutVelo ;
     } else if(p.daughters().size() > 0 && p.endVertex() && p.endVertex()->nDoF()>0) {
-      // can cipy this from OfflineVtxFitter
-      //auto prop = LoKi::Particles::ppFromPID(particle.particleID()) ;
-      bool isresonance = false ;//prop && prop->ctau() < 0.001*Gaudi::Units::mm ;
+      const LHCb::ParticleProperty* prop = ppsvc.find(p.particleID());
+      bool isresonance = prop && prop->ctau() < 0.001*Gaudi::Units::mm ;
       if( isresonance )
 	rc = Resonance ;
       else
@@ -581,63 +574,39 @@ namespace {
   }
 }
 
-// VertexDaughter* ParticleVertexFitter::createVertexDaughter(const LHCb::Particle& p) const
-// {
-//   VertexDaughter* rc(0) ;
-//   DaughterType thetype = derivetype(p) ;
-//   switch( thetype ) {
-//   case VeloTrack: rc =  new VertexTrack(p); break ;
-//   case Composite: rc = new VertexComposite(p); break ;
-//   default:
-//     warning() << "Cannot yet deal with this daughter: " << p.particleID() << endmsg ;
-//   }
-//   return rc ;
-// }   
-
 
 StatusCode ParticleVertexFitter::fit( const LHCb::Particle::ConstVector& daughters,
 			       LHCb::Vertex&                      vertex,
 			       LHCb::Particle*                    mother) const
 {
-  // need to separate between
-  // - tracks with velo information
-  // - tracks without velo information (only downstream, hopefully)
-  // - long-lived composites
-  // - short-lived composites
-  // - neutrals
+  // 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) ; } ) ;
+		  [=]( 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 ; } ) ;
 
-  // It all comes to this: can we get a reasonably estimate of the
-  // vertex position before actually creating the objects that go into
-  // the vertex? Or can we at least delay the expensive work until we
-  // know the vertex position? Should we use the trajectory
-  // approximation or not?
-
-  // let's work this out later. unfortunately, later is now. Here is wat we do:
-  // 1. make a list of resonance daughters
-  // 2. make a list of StateVector4 for velotracks and composites
-  // 3. make a list of tracktraj for downstream tracks
-  // the only problem is: how do we then deal with 'mixed' objects. we
-  // can also say: first try option 1; if that does not work, try
-  // option 2; if that doesn't work, try option 3 with TrajPoca etc.
-
+  // 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; ++i) 
+    for(size_t i=0; i<N && !posinitialized; ++i) 
       if( types[i] == Resonance ) {
 	position = daughters[i]->endVertex()->position() ;
 	posinitialized = true ;
-	break ;
       }
     assert(posinitialized) ;
   } else if( counttypes[TrackWithVelo]+counttypes[Composite]>=2) {
@@ -694,19 +663,8 @@ StatusCode ParticleVertexFitter::fit( const LHCb::Particle::ConstVector& daughte
     return StatusCode::FAILURE ;
   }
 
-  /*
-  std::vector<VertexDaughter*> vertexdaughters ;
-  for( auto& dau: daughters) {
-    auto vdau = createVertexDaughter(*dau) ;
-    if( vdau) vertexdaughters.push_back( vdau ) ;
-  }
-  */
-
-  // it would of course be much simpler if we can apply a single loop
-  // to add all track-like objects. then we indeed compute the
-  // momentum update separately for each type after the vertex fit. let's implement something
-
-  // let's still try this for an alternative
+  // 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 ;
@@ -714,15 +672,12 @@ StatusCode ParticleVertexFitter::fit( const LHCb::Particle::ConstVector& daughte
   // 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.
-  bool vertexinsidefoil = position.x()*position.x() + position.y()*position.y() < 8.*8.; // mm
+  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:
       {
-	// 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 LHCb::Track* track = p->proto()->track() ;
 	if( vertexinsidefoil )
 	  vertextracks.emplace_back( *p,track->firstState() );
@@ -756,17 +711,7 @@ StatusCode ParticleVertexFitter::fit( const LHCb::Particle::ConstVector& daughte
     }
   }
   
-  //info() << "position: " << position << endmsg ;
-  /*
-  if( resonances.size() > 0 ) {
-    // take the position from the first resonances
-    position = resonances.front().endVertex()->position() ;
-  } else if( velotracks.size()>=2 ) {
-  */
-
-  // suppose that we now have the position, then what's the next step?
-
-  // run iterations
+  // run vertex fit iterations
   Gaudi::Vector3 posvec ;
   posvec(0) = position.x() ;
   posvec(1) = position.y() ;
@@ -776,18 +721,14 @@ StatusCode ParticleVertexFitter::fit( const LHCb::Particle::ConstVector& daughte
   int ndof(0) ;
   bool converged(false) ;
   StatusCode sc = StatusCode::SUCCESS ;
-  
-  unsigned short iter ;
+  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
-    
-    //for( const auto& dau : vertexdaughters)
-    //dau->project( posvec, halfDChisqDX, halfD2ChisqDX2, chi2, ndof ) ;
+    // add all particles that contribute to the vertex chi2
     for( auto& dau : vertextracks)
       dau.project( posvec, halfDChisqDX, halfD2ChisqDX2, chi2, ndof ) ;
     for( auto& dau : vertexcomposites)
@@ -806,9 +747,10 @@ StatusCode ParticleVertexFitter::fit( const LHCb::Particle::ConstVector& daughte
       // update the position
       posvec += dpos ;
       // compute the delta-chisquare
-      double dchisq = ROOT::Math::Dot(dpos,halfDChisqDX) ;      
+      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) ;
@@ -825,9 +767,6 @@ StatusCode ParticleVertexFitter::fit( const LHCb::Particle::ConstVector& daughte
   vertex.setPosition( position ) ;
   vertex.setChi2AndDoF( chi2, ndof ) ;
   vertex.setCovMatrix( poscov ) ;
-  //for( auto& daughter : daughters) 
-  //vertex.addToOutgoingParticles(daughter) ;
-  
   if(mother) {
     // copy some stuff from the vertex
     mother->setReferencePoint( vertex.position()) ;
@@ -836,8 +775,6 @@ StatusCode ParticleVertexFitter::fit( const LHCb::Particle::ConstVector& daughte
     Gaudi::LorentzVector p4 ;
     Gaudi::SymMatrix4x4 p4cov ;
     ROOT::Math::SMatrix<double,4,3> gainmatrix ;
-    //for( const auto& itrack : vertexdaughters )
-    //itrack->addToFourVector( posvec, p4, p4cov, 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 ) ;
@@ -850,10 +787,6 @@ StatusCode ParticleVertexFitter::fit( const LHCb::Particle::ConstVector& daughte
     mother->setMomentum(p4) ;
     mother->setMomCovMatrix( p4cov ) ;
     mother->setPosMomCovMatrix( covmompos ) ;
-    
-    // now add the daughters to particle and vertex
-    //for( auto& daughter : daughters) 
-    //mother->addToDaughters(daughter) ;
   }
   return StatusCode::SUCCESS ;
 }
-- 
GitLab


From ae14a2e390e12c5603fb92d549a8a55fa402b3ef Mon Sep 17 00:00:00 2001
From: Wouter Hulsbergen <wouter.hulsbergen@nikhef.nl>
Date: Sun, 28 Jan 2018 11:46:52 +0100
Subject: [PATCH 3/3] modified to include dependencies of ParticleVertexFitter

---
 Phys/VertexFit/CMakeLists.txt | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/Phys/VertexFit/CMakeLists.txt b/Phys/VertexFit/CMakeLists.txt
index 4d799059c..a1a55246f 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)
-- 
GitLab