diff --git a/Phys/GenericVertexFinder/CMakeLists.txt b/Phys/GenericVertexFinder/CMakeLists.txt
index f35102155876bcde9b5feaa873cdebb32460c249..cb20488887698a0ceb70c435d8598cfc2d10fc6a 100644
--- a/Phys/GenericVertexFinder/CMakeLists.txt
+++ b/Phys/GenericVertexFinder/CMakeLists.txt
@@ -29,6 +29,7 @@ gaudi_add_module(GenericVertexFinder
src/GenericVertexFinderTwo.cpp
src/JetVertexAlg.cpp
src/ParticleTrajProvider.cpp
+ src/SigmaReco.cpp
src/TrackGenericVertexFinderAlg.cpp
src/TrajParticle.cpp
src/TrajParticleVertex.cpp
diff --git a/Phys/GenericVertexFinder/src/SigmaReco.cpp b/Phys/GenericVertexFinder/src/SigmaReco.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0a3346fce5f4a9cb6eb5d1150119fff7c4ac4fc2
--- /dev/null
+++ b/Phys/GenericVertexFinder/src/SigmaReco.cpp
@@ -0,0 +1,330 @@
+/*****************************************************************************\
+* (c) Copyright 2023 CERN for the benefit of the LHCb Collaboration *
+* *
+* This software is distributed under the terms of the GNU General Public *
+* Licence version 3 (GPL Version 3), copied verbatim in the file "COPYING". *
+* *
+* In applying this licence, CERN does not waive the privileges and immunities *
+* granted to it by virtue of its status as an Intergovernmental Organization *
+* or submit itself to any jurisdiction. *
+\*****************************************************************************/
+#include "SigmaReco.h"
+// from
+// https://gitlab.cern.ch/lhcb/Phys/-/blob/071bfe3f2bbf2c4033f82f8a79e626415651c6ea/Phys/GenericVertexFinder/src/ParticleTrajProvider.cpp
+namespace {
+ LHCb::State compositeParticleToState( const LHCb::Particle& particle ) {
+ // This replaces the function "Particle2State::particle2State"
+ // which is just wrong if the result is to be used in a vertex
+ // fit. See also ParticleVertexFitter.
+
+ LHCb::State state;
+ const auto& p4 = particle.momentum();
+ const auto& px = p4.Px();
+ const auto& py = p4.Py();
+ const auto& pz = p4.Pz();
+ const double tx = px / pz;
+ const double ty = py / pz;
+
+ double p = p4.P();
+ state.setTx( tx );
+ state.setTy( ty );
+ const int Q = particle.charge();
+ state.setQOverP( Q / p );
+ const Gaudi::XYZPoint& pos = particle.endVertex()->position();
+ state.setX( pos.x() );
+ state.setY( pos.y() );
+ state.setZ( pos.z() );
+
+ // For the computation of the Jacobian it is important to understand the following.
+ // x' = x + (z' - z) * tx
+ // --> dx/dz = - tx ;
+ //
+ // Notation:
+ // x = (x,y)
+ // tx = (tx,ty,q/p)
+ // J_{5,6} = ( Jxpos Jxmom )
+ // ( Jtxpos Jtxmom )
+ 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, 3, 3> Jtxmom;
+ Jtxmom( 0, 0 ) = 1 / pz; // dtx/dpx
+ Jtxmom( 1, 1 ) = 1 / pz; // dty/dpy
+ Jtxmom( 0, 2 ) = -tx / pz; // dtx/dpz
+ Jtxmom( 1, 2 ) = -ty / pz; // dty/dpz
+ const double dqopdp = -Q / ( p * p );
+ Jtxmom( 2, 0 ) = dqopdp * px / p; // dqop/dpx
+ Jtxmom( 2, 1 ) = dqopdp * py / p; // dqop/dpy
+ Jtxmom( 2, 2 ) = dqopdp * pz / p; // dqop/dpz
+
+ // perhaps it would be more efficient to fill one jacobian and one
+ // covariance and then use similatiry once, but I don't think
+ // so...
+ const auto& momposcov = particle.posMomCovMatrix(); // it has the wrong name: it is mompos not posmom
+ const Gaudi::Matrix2x3 xtxcov =
+ Jxpos * ROOT::Math::Transpose( momposcov.Sub<Gaudi::Matrix3x3>( 0, 0 ) ) * ROOT::Math::Transpose( Jtxmom );
+ const Gaudi::SymMatrix2x2 xcov = ROOT::Math::Similarity( Jxpos, particle.posCovMatrix() );
+ const Gaudi::SymMatrix3x3 txcov =
+ ROOT::Math::Similarity( Jtxmom, particle.momCovMatrix().Sub<Gaudi::SymMatrix3x3>( 0, 0 ) );
+ auto& cov = state.covariance();
+ for ( int i = 0; i < 2; ++i )
+ for ( int j = 0; j <= i; ++j ) cov( i, j ) = xcov( i, j );
+ for ( int i = 0; i < 2; ++i )
+ for ( int j = 0; j < 3; ++j ) cov( i, j + 2 ) = xtxcov( i, j );
+ for ( int i = 0; i < 3; ++i )
+ for ( int j = 0; j <= i; ++j ) cov( i + 2, j + 2 ) = txcov( i, j );
+ return state;
+ }
+
+ template <typename STATEA, typename STATEB>
+ auto vertexChi2( const STATEA& stateA, const STATEB& stateB ) {
+ // first compute the cross product of the directions. we'll need this in any case
+ const auto txA = stateA.tx();
+ const auto tyA = stateA.ty();
+ const auto txB = stateB.tx();
+ const auto tyB = stateB.ty();
+ auto nx = tyA - tyB; // y1 * z2 - y2 * z1
+ auto ny = txB - txA; // - x1 * z2 + x2 * z1
+ auto nz = txA * tyB - tyA * txB; // x1 * y2 - x2 * y1
+ // auto n2 = nx*nx + ny*ny + nz*nz ;
+
+ // compute doca. we don't divide by the normalization to save time. we call it 'ndoca'
+ auto dx = stateA.x() - stateB.x();
+ auto dy = stateA.y() - stateB.y();
+ auto dz = stateA.z() - stateB.z();
+ auto ndoca = dx * nx + dy * ny + dz * nz;
+
+ // figure out what floating point type to use for the covariance matrix manipulations
+ using float_t = std::decay_t<decltype( stateA.covariance()( 0, 0 ) )>;
+ using Vector4 = ROOT::Math::SVector<float_t, 4>;
+ using SymMatrix4x4 = ROOT::Math::SMatrix<float_t, 4, 4, ROOT::Math::MatRepSym<float_t, 4>>;
+
+ // the hard part: compute the jacobians :-)
+ Vector4 jacA, jacB;
+ jacA( 0 ) = nx;
+ jacA( 1 ) = ny;
+ jacA( 2 ) = -dy + dz * tyB;
+ jacA( 3 ) = dx - dz * txB;
+ jacB( 0 ) = -nx;
+ jacB( 1 ) = -ny;
+ jacB( 2 ) = dy - dz * tyA;
+ jacB( 3 ) = -dx + dz * txA;
+
+ // compute the variance on ndoca
+ decltype( auto ) covA = stateA.covariance();
+ decltype( auto ) covB = stateB.covariance();
+ using ROOT::Math::Similarity;
+ auto const varndoca = Similarity( jacA, covA.template Sub<SymMatrix4x4>( 0, 0 ) ) +
+ Similarity( jacB, covB.template Sub<SymMatrix4x4>( 0, 0 ) );
+
+ // return the chi2
+ return ndoca * ndoca / varndoca;
+ }
+
+ template <typename StateVector>
+ auto doca( const StateVector& stateA, const StateVector& stateB ) {
+ // first compute the cross product of the directions.
+ const auto txA = stateA.tx();
+ const auto tyA = stateA.ty();
+ const auto txB = stateB.tx();
+ const auto tyB = stateB.ty();
+ auto nx = tyA - tyB; // y1 * z2 - y2 * z1
+ auto ny = txB - txA; // - x1 * z2 + x2 * z1
+ auto nz = txA * tyB - tyA * txB; // x1 * y2 - x2 * y1
+ auto n = std::sqrt( nx * nx + ny * ny + nz * nz );
+ // compute the doca
+ auto dx = stateA.x() - stateB.x();
+ auto dy = stateA.y() - stateB.y();
+ auto dz = stateA.z() - stateB.z();
+ auto ndoca = dx * nx + dy * ny + dz * nz;
+ return fabs( ndoca / n );
+ }
+
+} // namespace
+
+//=============================================================================
+// Standard constructor, initializes variables
+//=============================================================================
+SigmaReco::SigmaReco( const std::string& name, ISvcLocator* pSvcLocator ) : GaudiAlgorithm( name, pSvcLocator ) {
+
+ declareProperty( "InputVeloTracks", m_velo_loc = LHCb::TrackLocation::Velo );
+ declareProperty( "InputSeeds", m_seed_loc );
+ declareProperty( "OutputTracks", m_output_loc = "Rec/Track/SigmaRecoedVelo" );
+ declareProperty( "ExtrapolatorName", m_extrapolator_name = "TrackMasterExtrapolator" );
+ declareProperty( "TrackFitterName", m_fitter_name = "FastVeloFitLHCbIDs" );
+ declareProperty( "VeloHyperonIP", m_ip_cut = 5. * Gaudi::Units::mm );
+ declareProperty( "MomentumCorrectionFactor", m_mcf = 1. );
+ declareProperty( "MatchChi2", m_chi2_cut = 9999.f );
+}
+
+//=============================================================================
+// Initialization
+//=============================================================================
+StatusCode SigmaReco::initialize() {
+
+ StatusCode sc = GaudiAlgorithm::initialize(); // must be executed first
+ if ( sc.isFailure() ) return sc; // error printed already by GaudiAlgorithm
+
+ m_extrapolator = tool<ITrackExtrapolator>( m_extrapolator_name, this );
+ if ( !m_extrapolator ) {
+ error() << "Failed to create the track extrapolator." << endmsg;
+ return StatusCode::FAILURE;
+ }
+
+ m_velo_track_refitter = tool<ITrackFitter>( m_fitter_name, this );
+ if ( !m_velo_track_refitter ) {
+ error() << "Failed to create the VELO track fitter." << endmsg;
+ return StatusCode::FAILURE;
+ }
+ return sc;
+}
+
+StatusCode SigmaReco::execute() {
+
+ StatusCode sc = StatusCode::SUCCESS;
+ // get the velo tracks and copy them, as they will be manipulated and serve as output
+ const LHCb::Tracks* velo_tracks = get<LHCb::Tracks>( m_velo_loc );
+ debug() << "Got " << velo_tracks->size() << " Velo tracks" << endmsg;
+ std::vector<LHCb::Track*> copied_velo_tracks;
+ copied_velo_tracks.reserve( velo_tracks->size() );
+ copied_velo_tracks.insert( copied_velo_tracks.end(), velo_tracks->begin(), velo_tracks->end() );
+ // get the downstream part of the decay chain
+ LHCb::Particle::Range seed_particles = get<LHCb::Particle::Range>( m_seed_loc );
+ debug() << "Got " << seed_particles.size() << " Seeds" << endmsg;
+
+ LHCb::Tracks* best_velo_tracks;
+ if ( exist<LHCb::Tracks>( m_output_loc ) )
+ best_velo_tracks = get<LHCb::Tracks>( m_output_loc );
+ else {
+ best_velo_tracks = new LHCb::Tracks();
+ put( best_velo_tracks, m_output_loc );
+ }
+ best_velo_tracks->reserve( seed_particles.size() );
+
+ for ( auto seed : seed_particles ) {
+ // output track
+ LHCb::Track* best_velo_track{nullptr};
+ double best_chi2 = m_chi2_cut, best_ip = -1., best_doca = -1., best_vchi2 = -1.;
+ for ( auto velo_track : copied_velo_tracks ) {
+ // check the rough IP of the VELO w.r.t. the seed's decay vertex
+ // N.B. the pattern recognition doesn't set LHCb::State::LastMeasurement, but LHCb::State::EndVelo is effectively
+ // the last measurement and not the usual EndVelo position z=770
+ LHCb::State* last_velo_state{nullptr};
+ if ( velo_track->hasStateAt( LHCb::State::EndVelo ) )
+ last_velo_state = velo_track->stateAt( LHCb::State::EndVelo );
+ else
+ last_velo_state = &velo_track->closestState( 770 * Gaudi::Units::mm );
+ if ( !seed->isBasicParticle() ) {
+ const auto distance = last_velo_state->position() - seed->endVertex()->position();
+ const auto ip = ( distance - last_velo_state->slopes() * distance.Dot( last_velo_state->slopes() ) /
+ last_velo_state->slopes().Mag2() )
+ .r();
+ verbose() << "IP " << ip << endmsg;
+ if ( ip > m_ip_cut ) continue;
+ }
+ // the seed is charged, so we need to properly transport it from it's decay vertex to the last measurement of
+ // the Velo track
+ LHCb::State seed_as_state = seed->isBasicParticle()
+ ? seed->proto()->track()->closestState( 770 * Gaudi::Units::mm )
+ : compositeParticleToState( *seed );
+ verbose() << "Seed state at endvtx / closest to EndVelo" << endmsg;
+ if ( msgLevel( MSG::VERBOSE ) ) seed_as_state.fillStream( std::cout );
+
+ // there are 2 places where we may want to refit the Velo track. put it in a lambda
+ auto fit_velo_track = [&velo_track, &seed_as_state, &last_velo_state, this]() -> StatusCode {
+ for ( auto state : velo_track->states() ) {
+ // TODO: need Sigma momentum here
+ state->setQOverP( seed_as_state.qOverP() / m_mcf );
+ state->setErrQOverP2( seed_as_state.errQOverP2() / ( m_mcf * m_mcf ) );
+ }
+ const auto fsc = m_velo_track_refitter->fit( *velo_track );
+ // after fitting, we get the state at the last measurement
+ last_velo_state = velo_track->stateAt( LHCb::State::LastMeasurement );
+ if ( msgLevel( MSG::VERBOSE ) ) last_velo_state->fillStream( std::cout );
+ return fsc;
+ };
+
+ // extrapolate seed state to last measurement in Velo
+ if ( m_extrapolator->propagate( seed_as_state, last_velo_state->z(),
+ /*TrackMatrix (transport update matrix)*/ nullptr,
+ seed->particleID() ) == StatusCode::FAILURE )
+ continue;
+ const auto match_doca = doca( *last_velo_state, seed_as_state );
+ verbose() << "match_doca " << match_doca << endmsg;
+ if ( seed->isBasicParticle() && match_doca > m_ip_cut ) continue;
+
+ // fit velo track with momentum estimate from seed state at EndVelo to get a better estimate of the covariance
+ // matrix
+ if ( fit_velo_track() == StatusCode::FAILURE ) continue;
+
+ debug() << "Seed state at end velo" << endmsg;
+ if ( msgLevel( MSG::VERBOSE ) ) seed_as_state.fillStream( std::cout );
+
+ last_velo_state = velo_track->stateAt( LHCb::State::LastMeasurement );
+ debug() << "Velo state after fitting (LastMeasurement)" << endmsg;
+ if ( msgLevel( MSG::VERBOSE ) ) last_velo_state->fillStream( std::cout );
+
+ // get a chi^2 for deciding which velo track fits best
+ const auto vx_chi2 = vertexChi2( *last_velo_state, seed_as_state );
+ const auto cov_seed = seed_as_state.covariance();
+ const auto cov_velo = last_velo_state->covariance();
+ Gaudi::Matrix5x5 tmp_cov = cov_seed + cov_velo;
+ Gaudi::Matrix4x4 cov = tmp_cov.Sub<Gaudi::Matrix4x4>( 0, 0 );
+ bool isc = cov.InvertFast();
+ if ( !isc ) continue;
+ // TODO: need match-chi2 of tracks that are not supposed to be colinear
+ Gaudi::Vector4 dv{seed_as_state.x() - last_velo_state->x(), seed_as_state.y() - last_velo_state->y(),
+ seed_as_state.tx() - last_velo_state->tx(), seed_as_state.ty() - last_velo_state->ty()};
+ const auto match_chi2 = ROOT::Math::Similarity( dv, cov );
+
+ if ( ( !seed->isBasicParticle() && match_chi2 < best_chi2 ) ||
+ ( seed->isBasicParticle() && vx_chi2 < best_vchi2 ) ) {
+ best_chi2 = match_chi2;
+ best_velo_track = velo_track;
+ for ( auto state : best_velo_track->states() ) {
+ state->setQOverP( seed_as_state.qOverP() );
+ state->setErrQOverP2( seed_as_state.errQOverP2() );
+ }
+ const auto new_distance = last_velo_state->position() - seed_as_state.position();
+ best_ip = ( new_distance - last_velo_state->slopes() * new_distance.Dot( last_velo_state->slopes() ) /
+ last_velo_state->slopes().Mag2() )
+ .r();
+ best_doca = match_doca;
+ best_vchi2 = vx_chi2;
+ }
+ }
+ verbose() << "Done looping Velo tracks." << endmsg;
+ if ( best_velo_track ) {
+ if ( msgLevel( MSG::VERBOSE ) ) best_velo_track->fillStream( std::cout );
+ debug() << "KEY " << seed->key() << " IP " << best_ip << " DOCA " << best_doca << " VCHI2 " << best_vchi2
+ << " SCHI2 " << best_chi2 << endmsg;
+ auto ei = best_velo_track->extraInfo();
+ ei.insert( 360, static_cast<double>( seed->key() ) );
+ ei.insert( 361, best_ip );
+ ei.insert( 362, best_doca );
+ ei.insert( 363, best_vchi2 );
+ ei.insert( 364, best_chi2 );
+ best_velo_track->setExtraInfo( ei );
+ best_velo_tracks->insert( best_velo_track->clone() );
+ } else { // hack for being able to tuple both the downstream candidate and the decay chain including the matched
+ // Velo track later.
+ debug() << "Found no matching track."
+ << " KEY " << seed->key() << endmsg;
+ best_velo_track = copied_velo_tracks.front();
+ auto ei = best_velo_track->extraInfo();
+ ei.insert( 360, static_cast<double>( seed->key() ) );
+ ei.insert( 361, -1. );
+ ei.insert( 362, -1. );
+ ei.insert( 363, -1. );
+ ei.insert( 364, -1. );
+ best_velo_track->setExtraInfo( ei );
+ best_velo_tracks->insert( best_velo_track->clone() );
+ }
+ }
+ verbose() << "Done looping Seed tracks." << endmsg;
+
+ return sc;
+}
+
+DECLARE_COMPONENT( SigmaReco )
diff --git a/Phys/GenericVertexFinder/src/SigmaReco.h b/Phys/GenericVertexFinder/src/SigmaReco.h
new file mode 100644
index 0000000000000000000000000000000000000000..a5e8ef3f782927cf6f3d8b4ebfe5e1eebcc48ebc
--- /dev/null
+++ b/Phys/GenericVertexFinder/src/SigmaReco.h
@@ -0,0 +1,50 @@
+/*****************************************************************************\
+* (c) Copyright 2023 CERN for the benefit of the LHCb Collaboration *
+* *
+* This software is distributed under the terms of the GNU General Public *
+* Licence version 3 (GPL Version 3), copied verbatim in the file "COPYING". *
+* *
+* In applying this licence, CERN does not waive the privileges and immunities *
+* granted to it by virtue of its status as an Intergovernmental Organization *
+* or submit itself to any jurisdiction. *
+\*****************************************************************************/
+#ifndef SigmaReco_H
+#define SigmaReco_H
+
+// from Gaudi
+#include "GaudiAlg/GaudiAlgorithm.h"
+// from LHCb
+#include "Event/Particle.h"
+#include "Event/State.h"
+#include "Event/Track.h"
+#include "VeloDet/DeVelo.h"
+// for master branch: #include "LHCbMath/StateVertexUtils.h"
+// from Rec
+#include "TrackInterfaces/ITrackExtrapolator.h"
+#include "TrackInterfaces/ITrackFitter.h"
+
+/**
+
+ @class SigmaReco SigmaReco.h
+ @brief Match a Velo stub of a charged particle decaying before the TT to its reconstructed downstream decay products.
+ @author Marian Stahl, Laurent Dufour
+
+**/
+
+class SigmaReco : public GaudiAlgorithm {
+public:
+ // Standard constructor
+ SigmaReco( const std::string& name, ISvcLocator* pSvcLocator );
+
+ StatusCode initialize() override;
+ StatusCode execute() override;
+
+private:
+ // Tools
+ const ITrackFitter* m_velo_track_refitter;
+ const ITrackExtrapolator* m_extrapolator;
+ // members
+ double m_ip_cut, m_chi2_cut, m_mcf;
+ std::string m_velo_loc, m_seed_loc, m_output_loc, m_fitter_name, m_extrapolator_name;
+};
+#endif // SigmaReco_H