From f3a9139940a75131ce712cab38998f9eda6db84c Mon Sep 17 00:00:00 2001 From: Wouter Hulsbergen Date: Thu, 31 Jul 2025 13:18:48 +0200 Subject: [PATCH 1/2] updated DecayLengthSignicance and Lifetime functions to use the same math --- .../include/SelTools/DistanceCalculator.h | 415 +++++++----------- 1 file changed, 166 insertions(+), 249 deletions(-) diff --git a/Phys/SelTools/include/SelTools/DistanceCalculator.h b/Phys/SelTools/include/SelTools/DistanceCalculator.h index dccce9a8b86..44152217226 100644 --- a/Phys/SelTools/include/SelTools/DistanceCalculator.h +++ b/Phys/SelTools/include/SelTools/DistanceCalculator.h @@ -192,11 +192,11 @@ namespace Sel { } // end of namespace helper - // The following functions provide the p4 and its cov matrix in the - // "(Px,Py,Pz,M)" representation. This is required if you'd like to - // do anything with the mass or covariance matrix in single - // precision. namespace { + // The following functions provide the p4 and its cov matrix in the + // "(Px,Py,Pz,M)" representation. This is required if you'd like to + // do anything with the mass or covariance matrix in single + // precision. These functions should be moved to the LHCb project. template auto fourMomentumPM( const Particle& p ) { const auto mom = fourMomentum( p ); @@ -253,6 +253,112 @@ namespace Sel { const ROOT::Math::SMatrix c = dPMdPE( p ) * p.posMomCovMatrix(); return LHCb::LinAlg::convert( c ); // cannot convert expressions } + + // The following two routines implement the IPconstraint decaylength fit. + // They are used both by 'DecayLengthSignificance' (DLS) and 'Lifetime' (LTIME). + // They could be moved to LHCbMath. + + // Routines that takes a measured displacement vector and direction m=(dx,dy,dz,tx,ty) + // and fits it to the model x=(dz,tx,ty). + template + auto iterateIPconstraintFit( const LHCb::LinAlg::Vec& m, const LHCb::LinAlg::MatSym& V ) { + using Sel::Utils::all; + using std::abs; + // Extract the initial state-vector x={Lz,tx,ty} + const auto x0 = m.template sub, 2>(); + // Compute the covariance matrix as it will be after filtering the state-vector part of m + const auto C0 = V.template sub, 2, 2>(); + // Compute the 2D IP residual using the initial statevecor. + const LHCb::LinAlg::Vec r0{ m( 0 ) - x0( 1 ) * x0( 0 ), m( 1 ) - x0( 2 ) * x0( 0 ) }; + // Initialize the 'running' state-vector + auto x = x0; + // allocate some matrices that we need after the loop to update the covariance matrix + LHCb::LinAlg::Mat covCR; + LHCb::LinAlg::MatSym Rinv; + auto converged = float_v{ 1 } < 0; + auto success = float_v{ 1 } > 0; + auto chi2 = std::numeric_limits::max(); + unsigned niter = 0; + const float_v max_delta_chi2 = 0.001; + const float_v max_reldelta_chi2 = 0.001; + + for ( ; niter < MaxIter && !all( converged ); ++niter ) { + // in every iteration, the current state-vector determines the reference. Use this to compute reference residual + // and jacobian + const auto x_ref = x; + const LHCb::LinAlg::Vec r_ref{ m( 0 ) - x( 1 ) * x( 0 ), m( 1 ) - x( 2 ) * x( 0 ) }; + LHCb::LinAlg::Mat Hm; + Hm( 0, 0 ) = 1; + Hm( 1, 1 ) = 1; + Hm( 0, 2 ) = -x( 1 ); // dIPx/dLz = - tx + Hm( 1, 2 ) = -x( 2 ); // dIPy/dLz = - ty + Hm( 0, 3 ) = -x( 0 ); // dIPx/dtx = - Lz + Hm( 1, 4 ) = -x( 0 ); // dIPy/dty = - Lz + // filter the state-vector part of the input: this is trivial + x = x0; + // compute the linearized residual + const auto Hr = Hm.template sub, 0, 2>(); + const auto r = r_ref + Hr * ( x - x_ref ); + // filter the IP part: compute the covariance of the residual and invert it + const auto R = LHCb::LinAlg::similarity( Hm, V ); + const auto [stepsuccess, inv] = R.invChol(); + Rinv = inv; + // Now compute the correlation, needed for the gain matrix. + // Since Hp is trivial, we will not actually declare it, but just take the relevant submatrix + covCR = ( V * Hm.transpose() ).template sub, 2, 0>(); + // Compute the updated state vector and its covariance. Of course we need only part of it, but that's for later. + const auto dx = -1.0 * ( covCR * ( Rinv * r ) ); + x = x + dx; + // Estimate the change in the chi2: of many options this expression seems to work best + const auto dchi2 = LHCb::LinAlg::similarity( Hr * ( x - x_ref ), Rinv ); + chi2 = LHCb::LinAlg::similarity( r, Rinv ) + dchi2; + converged = ( abs( dchi2 ) < max_delta_chi2 ) || ( abs( dchi2 ) < max_reldelta_chi2 * chi2 ); + success = success && stepsuccess; + } + // once we are done iterating, compute the covariance matrix + const auto C = C0 - LHCb::LinAlg::similarity( covCR, Rinv ); + return std::tuple{ x, C, chi2, success, converged, niter }; + } + + // routine that computes the vector m and matrix V=cov(m) for a Particle and PV. this is the input to the + // decaylength fit above. + template + auto initializeIPconstraintFit( VContainer const& primary, Particle const& particle ) { + // invariants that are not changed during iteration + const auto momCov = momPMCovMatrix( particle ); + const auto posCov = posCovMatrix( particle ); + const auto momPosCov = momPMPosCovMatrix( particle ); + const auto mom = fourMomentumPM( particle ); + const auto pos = endVertexPos( particle ); + const auto primaryPosCov = posCovMatrix( primary ); + const auto primaryPos = endVertexPos( primary ); + + using float_v = std::decay_t; + const auto dpos = pos - primaryPos; + LHCb::LinAlg::Vec m = { dpos( 0 ), dpos( 1 ), dpos( 2 ), mom( 0 ) / mom( 2 ), mom( 1 ) / mom( 2 ) }; + // compute its covariance matrix. this can be done much more efficiently, + // but it is difficult to get right without first creating the 7x7 matrix. + LHCb::LinAlg::MatSym V7x7; + // For some reason, these do not do anything. + // V7x7.template place_at<0,0>( posCov + primaryPosCov ) ; + // V7x7.template place_at<3,3>( momCov) ; + // V7x7.template place_at<3,0>( momPosCov ) ; + for ( int irow = 0; irow < 3; ++irow ) + for ( int icol = 0; icol <= irow; ++icol ) + V7x7( irow, icol ) = posCov( irow, icol ) + primaryPosCov( irow, icol ); + for ( int irow = 0; irow < 4; ++irow ) + for ( int icol = 0; icol <= irow; ++icol ) V7x7( irow + 3, icol + 3 ) = momCov( irow, icol ); + for ( int irow = 0; irow < 4; ++irow ) + for ( int icol = 0; icol < 3; ++icol ) V7x7( irow + 3, icol ) = momPosCov( irow, icol ); + LHCb::LinAlg::Mat J; + J( 0, 0 ) = J( 1, 1 ) = J( 2, 2 ) = 1; + J( 3, 3 ) = J( 4, 4 ) = 1 / mom( 2 ); + J( 3, 5 ) = -mom( 0 ) / ( mom( 2 ) * mom( 2 ) ); + J( 4, 5 ) = -mom( 1 ) / ( mom( 2 ) * mom( 2 ) ); + const auto V = LHCb::LinAlg::similarity( J, V7x7 ); + return std::tuple{ m, V, J, dpos, mom, V7x7 }; + } + } // namespace struct LifetimeFitter { @@ -271,16 +377,22 @@ namespace Sel { throw GaudiException( "Calling LifetimeFitter::bind with nullptr makes no sense!", "SelTools::LifetimeFitter", StatusCode::FAILURE ); } - m_no_convergence.emplace( m_owning_algorithm, "Lifetime fit did not converge. Aborting." ); m_fit_failure.emplace( m_owning_algorithm, "Lifetime fit failure." ); m_dls_failure.emplace( m_owning_algorithm, "DLS failure" ); +#ifdef DCDEBUG + m_no_convergence.emplace( m_owning_algorithm, "Lifetime fit did not converge. Aborting." ); + m_fit_numiter.emplace( m_owning_algorithm, "Lifetime fit number of iterations" ); +#endif } private: Gaudi::Algorithm* m_owning_algorithm{ nullptr }; - mutable std::optional> m_no_convergence; mutable std::optional> m_fit_failure; mutable std::optional> m_dls_failure; +#ifdef DCDEBUG + mutable std::optional> m_no_convergence; + mutable std::optional> m_fit_numiter; +#endif static constexpr auto NonPhysicalValue = std::numeric_limits::quiet_NaN(); @@ -292,269 +404,74 @@ namespace Sel { template auto DecayLengthSignificance( VContainer const& primary, Particle const& particle ) const { - using LHCb::Event::covMatrix; - using LHCb::Event::endVertexPos; - using LHCb::Event::posCovMatrix; - using LHCb::Event::threeMomentum; - using std::sqrt; - - // Calculate the distance between the particle and the vertex we hold. - const auto flight = endVertexPos( particle ) - endVertexPos( primary ); - - // Get the 3 momentum vectors for following calculations. - const auto p3 = threeMomentum( particle ); - const auto dir = p3 / p3.mag(); - - // Get the covariance matrix of the vertex. - const auto vertexCov = posCovMatrix( primary ); - - // Get the fulll particle covariance matrix. - const auto pCov = covMatrix( particle ); - - // Project the momentum of the particle onto its distance to the vertex - const auto a = dot( dir, flight ) / p3.mag(); - - // Update the covariance matrix - std::decay_t W; - for ( size_t row = 0; row < 3; ++row ) { - for ( size_t col = 0; col <= row; ++col ) { - W( row, col ) = vertexCov( row, col ) + pCov( row, col ) + pCov( row + 3, col + 3 ) * a * a - - ( pCov( row + 3, col ) + pCov( col + 3, row ) ) * a; - } - } - // WH July 2025: This is a slightly more efficient method to compute the same thing. It was used to find a bug in - // the lines above. - // const auto posCov = posCovMatrix( particle ); - // const auto momCov = momCovMatrix( particle ); - // const auto momPosCov = momPosCovMatrix( particle ); - // for ( size_t row = 0; row < 3; ++row ) { - // for ( size_t col = 0; col <= row; ++col ) { - // W( row, col ) = vertexCov( row, col ) + posCov( row, col ) + momCov( row, col) * a * a - - // ( momPosCov( row, col) + momPosCov( col, row ) ) * a; - // } - // } - - auto [success, W_Inv] = W.invChol(); - const auto nfailures = popcount( !success ); + // To remain consistent with what was done before, we run a 'single iteration' fit + auto [m, V, J, dpos, mom, cov] = initializeIPconstraintFit( primary, particle ); + auto [x, C, chi2, success, converged, niter] = iterateIPconstraintFit<1>( m, V ); + // Count the number of failures + const auto nfailures = popcount( !success ); if ( m_dls_failure.has_value() && nfailures > 0 ) *m_dls_failure += nfailures; - // Temporary fix: If W is not invertible due to neg Det, exclude 'a' terms. - if ( !success ) { - for ( size_t row = 0; row < 3; ++row ) { - for ( size_t col = 0; col <= row; ++col ) { W( row, col ) = vertexCov( row, col ) + pCov( row, col ); } - } - std::tie( success, W_Inv ) = W.invChol(); - } - auto halfdChi2dLam2 = similarity( dir, W_Inv ); - auto decayLength = dot( dir, W_Inv * flight ) / halfdChi2dLam2; - auto decayLengthErr = sqrt( 1 / halfdChi2dLam2 ); + // Return the Lz significance + const auto dls = x( 0 ) / sqrt( C( 0, 0 ) ); if constexpr ( std::is_arithmetic_v ) { - return success ? decayLength / decayLengthErr : NonPhysicalValue; + return success ? dls : NonPhysicalValue; } else { - return select( success, decayLength / decayLengthErr, NonPhysicalValue ); + return select( success, dls, NonPhysicalValue ); } } /** @fn Lifetime * @brief Calculate the lifetime between the particle and the best PV. * - * The implementation here is a very close reproduction of the one in - * LoKi::DirectionFitBase::iterate, with some condensing of various helper - * functions into three main parts: - * - * 1. The `ctau0` call, which computes a first-order approximation of the - * lifetime. - * 2. The `iterate` call, which refines the approximation based on an - * iterative fit. - * 3. The `ctau_step` call, which is used by `iterate` to compute the - * momentum and position updates made during each fit step. + * This uses an iterative fit: it reasonably closely reproduces the decaytime computed with the decay tree fit. + * It is not suitable for particles that do not originate from the PV (e.g. daughters from secondaries) */ - template + template auto Lifetime( VContainer const& primary, Particle const& particle ) const { - - // LoKi fit runs on a 'transported' particle, transporting p1 to position - // z, saving result to p2; the lifetime fit then acts on p2 - // Transporter is an instance of ParticleTransporter - // LoKi calls the transported particle 'good' - // auto [status, transported] = m_transporter->transport( p1, z, p2 ); - // LoKi fitter defines a 'decay' variable as particle.endVertex - auto ctau = ctau0( primary, particle ); - using float_v = decltype( ctau ); - float_v error = -1.e+10 * Gaudi::Units::mm; - float_v chi2 = std::numeric_limits::max(); - iterate( primary, particle, ctau, error, chi2 ); - auto lifetime = ctau / Gaudi::Units::c_light; - error /= Gaudi::Units::c_light; - return std::tuple{ lifetime, error, chi2 }; - } - - private: - /** @fn iterate - * @brief Calculate the lifetime between the particle and the best PV. - * - */ - - template - auto iterate( VContainer const& primary, Particle const& particle, float_v& ctau, float_v& error, - float_v& chi2 ) const { - - using Sel::Utils::all; - using std::abs; - using std::isnan; - using std::sqrt; - - // convergence parameters - const float_v max_delta_chi2 = 0.001; - const float_v max_reldelta_chi2 = 0.001; - const int m_max_iter = 5; - - // invariants which are not changed during iteration - const auto momCov = momPMCovMatrix( particle ); - const auto posCov = posCovMatrix( particle ); - const auto momPosCov = momPMPosCovMatrix( particle ); - const auto initMom = fourMomentumPM( particle ); - const auto initPos = endVertexPos( particle ); - const auto primaryPosCov = posCovMatrix( primary ); - const auto primaryPos = endVertexPos( primary ); - - // Copies which will be modified during the iteration - auto momentum = initMom; - auto decvertex = initPos; - auto primvertex = primaryPos; - - // need to find a better way to get a mask with values false for both simd types, scalars and doubles - // auto converged = LHCb::type_map::mask_v{false} ; - auto converged = float_v{ 1 } < 0; - auto success = float_v{ 1 } > 0; - for ( auto iter = 0; iter < m_max_iter; iter++ ) { - const auto& [stepsuccess, new_chi2] = ctau_step( primaryPos, primaryPosCov, initMom, initPos, momCov, posCov, - momPosCov, momentum, decvertex, primvertex, ctau, error ); - const auto delta_chi2 = new_chi2 - chi2; // note: in first iteration chi2=limits::max ; - success = success && stepsuccess; - converged = iter > 0 && - ( ( abs( delta_chi2 ) < max_delta_chi2 ) || ( abs( delta_chi2 ) < max_reldelta_chi2 * new_chi2 ) ); - chi2 = new_chi2; - if ( all( converged || !stepsuccess ) ) { break; } + // initialize the constraint and fit the model + const auto [m, V, J, dpos, mom, cov7] = initializeIPconstraintFit( primary, particle ); + const auto [x, C, chi2, fitsuccess, converged, niter] = iterateIPconstraintFit<5>( m, V ); + // compute the change in (Lz,tx,ty) + using float_v = std::decay_t; + // compute the covariance of (pos,mom) and (Lz,tx,ty) + const auto covXPX = cov7 * J.template sub, 2, 0>().transpose(); + // compute the gain matrix + const auto Vsub = V.template sub, 2, 2>(); + const auto [invsuccess, Vsubinv] = Vsub.invChol(); + const auto K = covXPX * Vsubinv.cast_to_mat(); + // compute the updated parameters (dpos,mom) + const auto dx = x - m.template sub, 2>(); + LHCb::LinAlg::Vec par7{ dpos( 0 ), dpos( 1 ), dpos( 2 ), mom( 0 ), mom( 1 ), mom( 2 ), mom( 3 ) }; + const auto newpar7 = par7 + K * dx; + // compute ctau and its variance + const auto ctau = newpar7( 2 ) * newpar7( 6 ) / newpar7( 5 ); // Lz * M / Pz + float_v ctauerr{ 0 }; + if constexpr ( ComputeCTauError ) { + // compute the jacobian par -> ctau + LHCb::LinAlg::Vec dctaudpar{ + 0, 0, newpar7( 6 ) / newpar7( 5 ), 0, 0, -ctau / newpar7( 5 ), newpar7( 2 ) / newpar7( 5 ) }; + // compute the updated covariance: we make this a lot more efficient by directly computing the gain matrix for + // ctau, but I'll leave the full expression in case useful for other application: + // const auto newcov7 = cov7 + LHCb::LinAlg::similarity(K,C - Vsub) ; + // const auto ctaucov = LHCb::LinAlg::similarity(dctaudpar,newcov7) ; + const auto ctaucov = + LHCb::LinAlg::similarity( dctaudpar, cov7 ) + + // LHCb::LinAlg::similarity( dctaudpar*K ,C - Vsub) ; // This should compile but it doesn't + LHCb::LinAlg::similarity( K.transpose() * dctaudpar, C - Vsub ); + ctauerr = sqrt( ctaucov ); } - + const auto success = fitsuccess && invsuccess; const auto nfailures = popcount( !success ); if ( m_fit_failure.has_value() ) for ( int i = 0; i < nfailures; ++i ) ++( *m_fit_failure ); - +#ifdef DCDEBUG const auto nnonconverged = popcount( !converged ); if ( m_no_convergence.has_value() ) for ( int i = 0; i < nnonconverged; ++i ) ++( *m_no_convergence ); - } - /** @fn ctau_step - * @brief Calculate one step of the var-fit. - * - * The `momentum`, `decvertex`, and `primvertex` inputs are mutated by this - * method based on updates computed in a single iteration of the fit. The - * `primary` and `particle` inputs hold the 'reference' points against which - * the updates will be applied, e.g. `momentum = particle.momentum() + - * momentum_update;`. - */ - template - auto ctau_step( Vec3D const& primaryPos, PosCov const& primaryPosCov, Vec4D const& initMom, Vec3D const& initPos, - MomCov const& momCov, PosCov const& posCov, MomPosCov const& momPosCov, Vec4D& momentum, - Vec3D& decvertex, Vec3D& primvertex, float_v& ctau, float_v& error ) const { - - using std::sqrt; - auto const px = X( momentum ); - auto const py = Y( momentum ); - auto const pz = Z( momentum ); - auto const m = E( momentum ); - // auto const m2 = m*m ; - // auto const e = sqrt( px * px + py * py + pz * pz + m2 ) ; - - // LoKi::Fitters::e_ctau - auto const vec_E = LHCb::LinAlg::Vec{ px, py, pz } / m; - - // This is the residual - auto const m_d = vec_E * ctau + ( primvertex - decvertex ); - - // This is the derivative of the residual to the momentum - LHCb::LinAlg::Mat mat_W; - mat_W( 0, 0 ) = 1.0; - mat_W( 1, 1 ) = 1.0; - mat_W( 2, 2 ) = 1.0; - mat_W( 0, 3 ) = -px / m; - mat_W( 1, 3 ) = -py / m; - mat_W( 2, 3 ) = -pz / m; - mat_W = mat_W * ctau / m; - - auto const mat_VD_part = mat_W * momPosCov; - auto const mat_VD_tmp = similarity( mat_W, momCov ) + posCov + primaryPosCov - - ( mat_VD_part + mat_VD_part.transpose() ).cast_to_sym(); - auto const [invsuccess, mat_VD] = mat_VD_tmp.invChol(); - - const auto m_Da0 = mat_W * ( initMom - momentum ) - ( initPos - decvertex ) + ( primaryPos - primvertex ); - - const auto m_l0 = mat_VD * ( m_Da0 + m_d ); - - const auto ctau_variance = 1. / similarity( vec_E, mat_VD ); - - const auto success = invsuccess && ctau_variance > 0; - const auto delta_ctau = select( success, -ctau_variance * dot( vec_E, m_l0 ), 0 ); - - const auto m_D1 = momCov * mat_W.transpose() - momPosCov; - const auto m_D2 = ( mat_W * momPosCov ).transpose() - posCov; - const auto& m_D3 = primaryPosCov; - - // select doesn't work very well together with LinAlg - // const auto m_l = select(success,m_l0 + ( mat_VD * vec_E * delta_ctau ),LHCb::LinAlg::Vec{}); - auto m_l = m_l0 + ( mat_VD * vec_E * delta_ctau ); - for ( int i = 0; i < 3; ++i ) m_l( i ) = select( success, m_l( i ), float_v{ 0 } ); - - const auto delta_momentum = m_D1 * m_l * -1; - const auto delta_decay_pos = m_D2 * m_l * -1; - const auto delta_primary_pos = m_D3 * m_l * -1; - - // Fill in 'output' values - auto const chi2 = LHCb::LinAlg::dot( m_l, m_Da0 + m_d ); - error = sqrt( ctau_variance ); - - // Update for the next iteration - ctau = ctau + delta_ctau; - momentum = initMom + delta_momentum; - decvertex = initPos + delta_decay_pos; - primvertex = primaryPos + delta_primary_pos; - - return std::tuple{ success, chi2 }; - } - - /** @brief Fast, approximate evaluation of c * tau. - * - * This neglects the particle momentum covariance, taking into account only - * the covariances of the primary and secondary vertex positions. - * - * Implementation from LoKi::DirectionFitBase::ctau0. - */ - template - auto ctau0( VContainer const& primary, Particle const& particle ) const { - // Retrieve position and position covariance of the decay and primary vertices - using LHCb::Event::endVertexPos; - using LHCb::Event::mass2; - using LHCb::Event::posCovMatrix; - using LHCb::Event::threeMomentum; - using std::sqrt; - const auto decay_pos = endVertexPos( particle ); - const auto decay_pos_cov = posCovMatrix( particle ); - const auto primary_pos = endVertexPos( primary ); - const auto primary_pos_cov = posCovMatrix( primary ); - const auto mat_VD_tmp = primary_pos_cov + decay_pos_cov; - const auto [success, mat_VD] = mat_VD_tmp.invChol(); - if constexpr ( std::is_arithmetic_v ) { - if ( !success ) return NonPhysicalValue; - } - auto const vec_E = threeMomentum( particle ) / sqrt( mass2( particle ) ); - auto const lam0 = mat_VD * ( primary_pos - decay_pos ); - if constexpr ( std::is_arithmetic_v ) { - return -1.0 * dot( vec_E, lam0 ) / similarity( vec_E, mat_VD ); - } else { - return select( success, -1.0 * dot( vec_E, lam0 ) / similarity( vec_E, mat_VD ), NonPhysicalValue ); - } + if ( m_fit_numiter.has_value() ) *m_fit_numiter += niter; +#endif + return std::tuple{ ctau / Gaudi::Units::c_light, ctauerr / Gaudi::Units::c_light, chi2 }; } }; } // namespace Sel -- GitLab From a64ff4c9f210496c762b0e5628c0b51ebf802b9e Mon Sep 17 00:00:00 2001 From: Wouter Hulsbergen Date: Tue, 12 Aug 2025 15:57:32 +0200 Subject: [PATCH 2/2] switched to TxTyPzM representation to simplify ltime fit --- Phys/FunctorCore/include/Functors/Composite.h | 3 +- .../include/SelTools/DistanceCalculator.h | 293 ++++++++++-------- 2 files changed, 170 insertions(+), 126 deletions(-) diff --git a/Phys/FunctorCore/include/Functors/Composite.h b/Phys/FunctorCore/include/Functors/Composite.h index 175e9c55c12..92feb936b00 100644 --- a/Phys/FunctorCore/include/Functors/Composite.h +++ b/Phys/FunctorCore/include/Functors/Composite.h @@ -290,8 +290,7 @@ namespace Functors::Composite { template auto operator()( Vertex_t const& vertex, Composite const& composite ) const { - return std::get<0>( - m_lifetime_calc.Lifetime( Sel::Utils::deref_if_ptr( vertex ), Sel::Utils::deref_if_ptr( composite ) ) ); + return m_lifetime_calc.Lifetime( Sel::Utils::deref_if_ptr( vertex ), Sel::Utils::deref_if_ptr( composite ) ).tau; } private: diff --git a/Phys/SelTools/include/SelTools/DistanceCalculator.h b/Phys/SelTools/include/SelTools/DistanceCalculator.h index 44152217226..a1b0cdbce9c 100644 --- a/Phys/SelTools/include/SelTools/DistanceCalculator.h +++ b/Phys/SelTools/include/SelTools/DistanceCalculator.h @@ -191,75 +191,140 @@ namespace Sel { } } // end of namespace helper +} // namespace Sel + +namespace Sel { + + namespace details { + + // The following functions provide the four-vector in TxTyPzM representation, which is useful for the decay length + // fit. The computation of the 'mass row' of the covariance matrix needs to be done in double precision. Therefore, + // at the moment this actually only works properly for LHCb::Particle. + // using LHCb::Event::threeMomentum ; + // using LHCb::Event::mass2 ; - namespace { - // The following functions provide the p4 and its cov matrix in the - // "(Px,Py,Pz,M)" representation. This is required if you'd like to - // do anything with the mass or covariance matrix in single - // precision. These functions should be moved to the LHCb project. template - auto fourMomentumPM( const Particle& p ) { - const auto mom = fourMomentum( p ); - const auto m2 = mass2( p ); + auto fourMomentumTPM( const Particle& p ) { + const auto mom = threeMomentum( p ); using float_v = std::decay_t; - return LHCb::LinAlg::Vec{ mom( 0 ), mom( 1 ), mom( 2 ), sqrt( m2 ) }; + return LHCb::LinAlg::Vec{ mom( 0 ) / mom( 2 ), mom( 1 ) / mom( 2 ), mom( 2 ), sqrt( mass2( p ) ) }; + } + + template + auto dTPMdPE( const auto px, const auto py, const auto pz, const auto m ) { + const auto e = sqrt( px * px + py * py + pz * pz + m * m ); + const auto pzinv = 1 / pz; + const auto minv = 1 / m; + MatrixType J; + J( 0, 0 ) = J( 1, 1 ) = pzinv; + J( 0, 2 ) = -px * pzinv * pzinv; + J( 1, 2 ) = -py * pzinv * pzinv; + J( 2, 2 ) = 1; + J( 3, 0 ) = -px * minv; + J( 3, 1 ) = -py * minv; + J( 3, 2 ) = -pz * minv; + J( 3, 3 ) = e * minv; + return J; } template - auto dPMdPE( const Particle& p ) { - // create the PE->PM Jacobian - const auto mom = fourMomentumPM( p ); - const auto e = sqrt( LHCb::LinAlg::dot( mom, mom ) ); - const auto m = mom( 3 ); + auto dTPMdPE( const Particle& p ) { + const auto mom = threeMomentum( p ); + const auto m = sqrt( mass2( p ) ); using float_v = std::decay_t; - LHCb::LinAlg::Mat J; - J( 0, 0 ) = J( 1, 1 ) = J( 2, 2 ) = 1; - J( 3, 0 ) = -mom( 0 ) / m; - J( 3, 1 ) = -mom( 1 ) / m; - J( 3, 2 ) = -mom( 2 ) / m; - J( 3, 3 ) = e / m; - return J; + return dTPMdPE>( mom( 0 ), mom( 1 ), mom( 2 ), m ); } template - auto momPMCovMatrix( const Particle& p ) { - return similarity( dPMdPE( p ), momCovMatrix( p ) ); + auto momTPMCovMatrix( const Particle& p ) { + return similarity( dTPMdPE( p ), momCovMatrix( p ) ); } template - auto momPMPosCovMatrix( const Particle& p ) { - return dPMdPE( p ) * momPosCovMatrix( p ); + auto momTPMPosCovMatrix( const Particle& p ) { + return dTPMdPE( p ) * momPosCovMatrix( p ); } - // To get this to work, we need to perform these computations in double precision, - // which now leads to some annoying code duplication - inline auto dPMdPE( const LHCb::Particle& p ) { - const auto mom = p.momentum(); - const auto m = mom.M(); - ROOT::Math::SMatrix J; - // LHCb::LinAlg::Mat J ; - J( 0, 0 ) = J( 1, 1 ) = J( 2, 2 ) = 1; - J( 3, 0 ) = -mom.Px() / m; - J( 3, 1 ) = -mom.Py() / m; - J( 3, 2 ) = -mom.Pz() / m; - J( 3, 3 ) = mom.E() / m; - return J; + // Specialization for LHCb::Particle in double precision + inline auto dTPMdPE( const LHCb::Particle& p ) { + const auto mom = p.momentum(); + const auto m = mom.M(); + return dTPMdPE>( mom.Px(), mom.Py(), mom.Pz(), m ); } - inline auto momPMCovMatrix( const LHCb::Particle& p ) { - const Gaudi::SymMatrix4x4 c = ROOT::Math::Similarity( dPMdPE( p ), p.momCovMatrix() ); + + inline auto momTPMCovMatrix( const LHCb::Particle& p ) { + const Gaudi::SymMatrix4x4 c = ROOT::Math::Similarity( dTPMdPE( p ), p.momCovMatrix() ); return LHCb::LinAlg::convert( c ); // cannot convert expressions } - inline auto momPMPosCovMatrix( const LHCb::Particle& p ) { - const ROOT::Math::SMatrix c = dPMdPE( p ) * p.posMomCovMatrix(); + inline auto momTPMPosCovMatrix( const LHCb::Particle& p ) { + const ROOT::Math::SMatrix c = dTPMdPE( p ) * p.posMomCovMatrix(); return LHCb::LinAlg::convert( c ); // cannot convert expressions } - // The following two routines implement the IPconstraint decaylength fit. - // They are used both by 'DecayLengthSignificance' (DLS) and 'Lifetime' (LTIME). - // They could be moved to LHCbMath. + // Computes the vector m=(dpos,mom,mass) and matrix V=cov(m) for a Particle and PV. This is the input to the + // decaylength fit. + template + concept HasFourMomentum = requires( T t ) { LHCb::Event::fourMomentum( t ); }; + + template + auto convertToDXYZTxTyPz( VContainer const& primary, Particle const& particle ) { + // invariants that are not changed during iteration + const auto momCov = momTPMCovMatrix( particle ); + const auto posCov = posCovMatrix( particle ); + const auto momPosCov = momTPMPosCovMatrix( particle ); + const auto mom = fourMomentumTPM( particle ); + const auto pos = endVertexPos( particle ); + const auto primaryPosCov = posCovMatrix( primary ); + const auto primaryPos = endVertexPos( primary ); + const auto dpos = pos - primaryPos; + using float_v = std::decay_t; + LHCb::LinAlg::Vec par7 = { dpos( 0 ), dpos( 1 ), dpos( 2 ), mom( 0 ), mom( 1 ), mom( 2 ), mom( 3 ) }; + LHCb::LinAlg::MatSym cov7; + // For some reason, these do not do anything. + // V7x7.template place_at<0,0>( posCov + primaryPosCov ) ; + // V7x7.template place_at<3,3>( momCov) ; + // V7x7.template place_at<3,0>( momPosCov ) ; + for ( int irow = 0; irow < 3; ++irow ) + for ( int icol = 0; icol <= irow; ++icol ) + cov7( irow, icol ) = posCov( irow, icol ) + primaryPosCov( irow, icol ); + for ( int irow = 0; irow < 4; ++irow ) + for ( int icol = 0; icol <= irow; ++icol ) cov7( irow + 3, icol + 3 ) = momCov( irow, icol ); + for ( int irow = 0; irow < 4; ++irow ) + for ( int icol = 0; icol < 3; ++icol ) cov7( irow + 3, icol ) = momPosCov( irow, icol ); + return std::tuple{ par7, cov7 }; + } + + // Same but for objects without a mass. Note: this one does not perform the covariance matrix rotation in double + // precision. + template + requires( !HasFourMomentum ) + auto convertToDXYZTxTyPz( VContainer const& primary, Particle const& particle ) { + // This is only used by FunctorCore tests at the moment. + const auto dpos = endVertexPos( particle ) - endVertexPos( primary ); + const auto mom = threeMomentum( particle ); + const auto vertexCov = posCovMatrix( primary ); + using LHCb::Event::covMatrix; + auto pCov = covMatrix( particle ); + // Add the PV position covariance + for ( int irow = 0; irow < 3; ++irow ) + for ( int icol = 0; icol <= irow; ++icol ) pCov( irow, icol ) += vertexCov( irow, icol ); + // Create the jacobian for the conversion to (x,y,z,Tx,Ty,Pz) + using float_v = std::decay_t; + LHCb::LinAlg::Mat J; + J( 0, 0 ) = J( 1, 1 ) = J( 2, 2 ) = J( 5, 5 ) = 1; + const auto pzinv = 1 / mom( 2 ); + const auto tx = mom( 0 ) * pzinv; + const auto ty = mom( 1 ) * pzinv; + J( 3, 3 ) = J( 4, 4 ) = pzinv; + J( 3, 5 ) = -tx * pzinv; + J( 4, 5 ) = -ty * pzinv; + const LHCb::LinAlg::Vec par{ dpos( 0 ), dpos( 1 ), dpos( 2 ), tx, ty, mom( 2 ) }; + return std::tuple{ par, LHCb::LinAlg::similarity( J, pCov ) }; + } - // Routines that takes a measured displacement vector and direction m=(dx,dy,dz,tx,ty) - // and fits it to the model x=(dz,tx,ty). + // Take a measured displacement vector and direction m=(dx,dy,dz,tx,ty) + // and fits it to the model x=(dz,tx,ty). Used by 'DecayLengthSignificance' (DLS) and 'Lifetime' (LTIME). + // Could be moved to LHCbMath. template auto iterateIPconstraintFit( const LHCb::LinAlg::Vec& m, const LHCb::LinAlg::MatSym& V ) { using Sel::Utils::all; @@ -272,24 +337,23 @@ namespace Sel { const LHCb::LinAlg::Vec r0{ m( 0 ) - x0( 1 ) * x0( 0 ), m( 1 ) - x0( 2 ) * x0( 0 ) }; // Initialize the 'running' state-vector auto x = x0; - // allocate some matrices that we need after the loop to update the covariance matrix - LHCb::LinAlg::Mat covCR; - LHCb::LinAlg::MatSym Rinv; + // allocate some stuff that we need after the loop to update the covariance matrix auto converged = float_v{ 1 } < 0; auto success = float_v{ 1 } > 0; auto chi2 = std::numeric_limits::max(); - unsigned niter = 0; const float_v max_delta_chi2 = 0.001; const float_v max_reldelta_chi2 = 0.001; - - for ( ; niter < MaxIter && !all( converged ); ++niter ) { + unsigned niter = 0; + LHCb::LinAlg::Mat covCR; + LHCb::LinAlg::MatSym Rinv; + for ( niter = 0; niter < MaxIter && !all( converged ); ++niter ) { // in every iteration, the current state-vector determines the reference. Use this to compute reference residual // and jacobian const auto x_ref = x; const LHCb::LinAlg::Vec r_ref{ m( 0 ) - x( 1 ) * x( 0 ), m( 1 ) - x( 2 ) * x( 0 ) }; LHCb::LinAlg::Mat Hm; - Hm( 0, 0 ) = 1; - Hm( 1, 1 ) = 1; + Hm( 0, 0 ) = 1; // dIPx/ddxpos + Hm( 1, 1 ) = 1; // dIPx/ddypos Hm( 0, 2 ) = -x( 1 ); // dIPx/dLz = - tx Hm( 1, 2 ) = -x( 2 ); // dIPy/dLz = - ty Hm( 0, 3 ) = -x( 0 ); // dIPx/dtx = - Lz @@ -299,9 +363,11 @@ namespace Sel { // compute the linearized residual const auto Hr = Hm.template sub, 0, 2>(); const auto r = r_ref + Hr * ( x - x_ref ); - // filter the IP part: compute the covariance of the residual and invert it + // filter the IP part: compute the covariance of the residual and invert it. + // FIXME: since computing V*Hm is expensive, we may want do cache it. const auto R = LHCb::LinAlg::similarity( Hm, V ); const auto [stepsuccess, inv] = R.invChol(); + success = success && stepsuccess; Rinv = inv; // Now compute the correlation, needed for the gain matrix. // Since Hp is trivial, we will not actually declare it, but just take the relevant submatrix @@ -309,57 +375,29 @@ namespace Sel { // Compute the updated state vector and its covariance. Of course we need only part of it, but that's for later. const auto dx = -1.0 * ( covCR * ( Rinv * r ) ); x = x + dx; - // Estimate the change in the chi2: of many options this expression seems to work best - const auto dchi2 = LHCb::LinAlg::similarity( Hr * ( x - x_ref ), Rinv ); - chi2 = LHCb::LinAlg::similarity( r, Rinv ) + dchi2; - converged = ( abs( dchi2 ) < max_delta_chi2 ) || ( abs( dchi2 ) < max_reldelta_chi2 * chi2 ); - success = success && stepsuccess; + // Compute the chi2 + const auto chi2prev = chi2; + chi2 = LHCb::LinAlg::similarity( r, Rinv ); + if constexpr ( MaxIter > 0 ) { + // Naive expressions for the dchi2, such as + // dchi2 = - LHCb::LinAlg::similarity( Hr * ( x - x_ref ), Rinv ); + // are not correct. Tried many alternatives, but so far the only that is remotely reliable is the full + // expression: + // dchi2 = - LHCb::LinAlg::similarity( (x-x_rev), Cinv) + // As the computation of Cinv is expensive, we could cache it in the first iteration. + // In the end, the quickest method is just to look at the actual change in the chi2. + if ( niter > 0 ) { + const auto dchi2 = abs( chi2prev - chi2 ); + converged = ( ( dchi2 < max_delta_chi2 ) || ( dchi2 < max_reldelta_chi2 * chi2 ) ); + } + } } // once we are done iterating, compute the covariance matrix const auto C = C0 - LHCb::LinAlg::similarity( covCR, Rinv ); return std::tuple{ x, C, chi2, success, converged, niter }; } - // routine that computes the vector m and matrix V=cov(m) for a Particle and PV. this is the input to the - // decaylength fit above. - template - auto initializeIPconstraintFit( VContainer const& primary, Particle const& particle ) { - // invariants that are not changed during iteration - const auto momCov = momPMCovMatrix( particle ); - const auto posCov = posCovMatrix( particle ); - const auto momPosCov = momPMPosCovMatrix( particle ); - const auto mom = fourMomentumPM( particle ); - const auto pos = endVertexPos( particle ); - const auto primaryPosCov = posCovMatrix( primary ); - const auto primaryPos = endVertexPos( primary ); - - using float_v = std::decay_t; - const auto dpos = pos - primaryPos; - LHCb::LinAlg::Vec m = { dpos( 0 ), dpos( 1 ), dpos( 2 ), mom( 0 ) / mom( 2 ), mom( 1 ) / mom( 2 ) }; - // compute its covariance matrix. this can be done much more efficiently, - // but it is difficult to get right without first creating the 7x7 matrix. - LHCb::LinAlg::MatSym V7x7; - // For some reason, these do not do anything. - // V7x7.template place_at<0,0>( posCov + primaryPosCov ) ; - // V7x7.template place_at<3,3>( momCov) ; - // V7x7.template place_at<3,0>( momPosCov ) ; - for ( int irow = 0; irow < 3; ++irow ) - for ( int icol = 0; icol <= irow; ++icol ) - V7x7( irow, icol ) = posCov( irow, icol ) + primaryPosCov( irow, icol ); - for ( int irow = 0; irow < 4; ++irow ) - for ( int icol = 0; icol <= irow; ++icol ) V7x7( irow + 3, icol + 3 ) = momCov( irow, icol ); - for ( int irow = 0; irow < 4; ++irow ) - for ( int icol = 0; icol < 3; ++icol ) V7x7( irow + 3, icol ) = momPosCov( irow, icol ); - LHCb::LinAlg::Mat J; - J( 0, 0 ) = J( 1, 1 ) = J( 2, 2 ) = 1; - J( 3, 3 ) = J( 4, 4 ) = 1 / mom( 2 ); - J( 3, 5 ) = -mom( 0 ) / ( mom( 2 ) * mom( 2 ) ); - J( 4, 5 ) = -mom( 1 ) / ( mom( 2 ) * mom( 2 ) ); - const auto V = LHCb::LinAlg::similarity( J, V7x7 ); - return std::tuple{ m, V, J, dpos, mom, V7x7 }; - } - - } // namespace + } // namespace details struct LifetimeFitter { // FIXME: remove default argument... @@ -379,20 +417,16 @@ namespace Sel { } m_fit_failure.emplace( m_owning_algorithm, "Lifetime fit failure." ); m_dls_failure.emplace( m_owning_algorithm, "DLS failure" ); -#ifdef DCDEBUG m_no_convergence.emplace( m_owning_algorithm, "Lifetime fit did not converge. Aborting." ); m_fit_numiter.emplace( m_owning_algorithm, "Lifetime fit number of iterations" ); -#endif } private: - Gaudi::Algorithm* m_owning_algorithm{ nullptr }; - mutable std::optional> m_fit_failure; - mutable std::optional> m_dls_failure; -#ifdef DCDEBUG + Gaudi::Algorithm* m_owning_algorithm{ nullptr }; + mutable std::optional> m_fit_failure; + mutable std::optional> m_dls_failure; mutable std::optional> m_no_convergence; mutable std::optional> m_fit_numiter; -#endif static constexpr auto NonPhysicalValue = std::numeric_limits::quiet_NaN(); @@ -401,13 +435,14 @@ namespace Sel { * @brief Calculate the significance of a non-zero decay length. * */ - template auto DecayLengthSignificance( VContainer const& primary, Particle const& particle ) const { - // To remain consistent with what was done before, we run a 'single iteration' fit - auto [m, V, J, dpos, mom, cov] = initializeIPconstraintFit( primary, particle ); - auto [x, C, chi2, success, converged, niter] = iterateIPconstraintFit<1>( m, V ); + const auto [par, cov] = details::convertToDXYZTxTyPz( primary, particle ); + using float_v = std::decay_t; + const auto m = par.template sub, 0>(); + const auto V = cov.template sub, 0, 0>(); + auto [x, C, chi2, success, converged, niter] = details::iterateIPconstraintFit<1>( m, V ); // Count the number of failures const auto nfailures = popcount( !success ); if ( m_dls_failure.has_value() && nfailures > 0 ) *m_dls_failure += nfailures; @@ -429,20 +464,20 @@ namespace Sel { template auto Lifetime( VContainer const& primary, Particle const& particle ) const { // initialize the constraint and fit the model - const auto [m, V, J, dpos, mom, cov7] = initializeIPconstraintFit( primary, particle ); - const auto [x, C, chi2, fitsuccess, converged, niter] = iterateIPconstraintFit<5>( m, V ); - // compute the change in (Lz,tx,ty) - using float_v = std::decay_t; - // compute the covariance of (pos,mom) and (Lz,tx,ty) - const auto covXPX = cov7 * J.template sub, 2, 0>().transpose(); + const auto [par7, cov7] = details::convertToDXYZTxTyPz( primary, particle ); + using float_v = std::decay_t; + const auto m = par7.template sub, 0>(); + const auto V = cov7.template sub, 0, 0>(); + const auto [x, C, chi2, fitsuccess, converged, niter] = details::iterateIPconstraintFit<5>( m, V ); + // compute the covariance of (pos,mom) and (Lz,tx,ty). if we are in the right rep, this is really easy: + const auto covXPX = cov7.template sub, 0, 2>(); // compute the gain matrix const auto Vsub = V.template sub, 2, 2>(); const auto [invsuccess, Vsubinv] = Vsub.invChol(); const auto K = covXPX * Vsubinv.cast_to_mat(); // compute the updated parameters (dpos,mom) - const auto dx = x - m.template sub, 2>(); - LHCb::LinAlg::Vec par7{ dpos( 0 ), dpos( 1 ), dpos( 2 ), mom( 0 ), mom( 1 ), mom( 2 ), mom( 3 ) }; - const auto newpar7 = par7 + K * dx; + const auto dx = x - m.template sub, 2>(); + const auto newpar7 = par7 + K * dx; // compute ctau and its variance const auto ctau = newpar7( 2 ) * newpar7( 6 ) / newpar7( 5 ); // Lz * M / Pz float_v ctauerr{ 0 }; @@ -464,14 +499,24 @@ namespace Sel { const auto nfailures = popcount( !success ); if ( m_fit_failure.has_value() ) for ( int i = 0; i < nfailures; ++i ) ++( *m_fit_failure ); -#ifdef DCDEBUG + const auto nnonconverged = popcount( !converged ); if ( m_no_convergence.has_value() ) for ( int i = 0; i < nnonconverged; ++i ) ++( *m_no_convergence ); if ( m_fit_numiter.has_value() ) *m_fit_numiter += niter; -#endif - return std::tuple{ ctau / Gaudi::Units::c_light, ctauerr / Gaudi::Units::c_light, chi2 }; + + if constexpr ( ComputeCTauError ) { + struct R { + float_v tau, err, chi2; + }; + return R{ ctau / Gaudi::Units::c_light, ctauerr / Gaudi::Units::c_light, chi2 }; + } else { + struct R { + float_v tau, chi2; + }; + return R{ ctau / Gaudi::Units::c_light, chi2 }; + } } }; } // namespace Sel -- GitLab