diff --git a/Event/RecEvent/src/PrimaryVertices.cpp b/Event/RecEvent/src/PrimaryVertices.cpp
index 2d3c6984d9131cfd58d47c85745d7a15a40f883f..50edafbdeed890ef4d00f6beaed3131dac7d22fa 100644
--- a/Event/RecEvent/src/PrimaryVertices.cpp
+++ b/Event/RecEvent/src/PrimaryVertices.cpp
@@ -79,7 +79,11 @@ namespace LHCb::Event::PV {
pv.setNDoF( -1 );
return PVFitStatus::Failed;
}
- auto cov = accumulator.halfD2Chi2DX2.invChol();
+ auto [success, cov] = accumulator.halfD2Chi2DX2.invChol();
+ if ( !success ) {
+ pv.setNDoF( -1 );
+ return PVFitStatus::Failed;
+ }
// compute the delta w.r.t. the reference
const auto delta = cov * accumulator.halfDChi2DX * -1.0;
// update the position
diff --git a/Kernel/LHCbMath/include/LHCbMath/MatVec.h b/Kernel/LHCbMath/include/LHCbMath/MatVec.h
index f277feab61e0db6d5235b34ffabb0ec0b9e08eaf..cdb43d54166a9ba03980e4ed2b4ec7732509544f 100644
--- a/Kernel/LHCbMath/include/LHCbMath/MatVec.h
+++ b/Kernel/LHCbMath/include/LHCbMath/MatVec.h
@@ -520,17 +520,20 @@ namespace LHCb::LinAlg {
/// Cholesky factorization and substitution based on:
/// https://hal.archives-ouvertes.fr/hal-01550129/document
- [[gnu::pure]] MatSym<T, N> invChol() const {
+ [[gnu::pure]] auto invChol() const {
const MatSym<T, N>& A = *this;
MatSym<T, N> L;
- auto inv = initialize_with_zeros<Mat<T, N>>();
+ auto inv = initialize_with_zeros<Mat<T, N>>();
+ auto mask = decltype( T{} > T{} ){true};
// Cholesky factorization
Utils::unwind<0, N>( [&]( auto j ) {
T s = A( j, j );
Utils::unwind<0, j>( [&]( auto k ) { s = s - L( j, k ) * L( j, k ); } );
const T rsqrt_s = T{1} / sqrt( s );
- L( j, j ) = rsqrt_s; // store directly the reciprocal
+ // check positive definite
+ mask = mask && ( s > 0 );
+ L( j, j ) = rsqrt_s; // store directly the reciprocal
Utils::unwind<j + 1, N>(
[]( auto i, auto j, auto& L, const auto& A, auto rsqrt_s ) {
T s = A( i, j );
@@ -540,7 +543,10 @@ namespace LHCb::LinAlg {
},
j, L, A, rsqrt_s );
} );
-
+ // return early on non positive definite in scalar case
+ if constexpr ( std::is_arithmetic_v<T> ) {
+ if ( !mask ) return std::pair{false, inv.cast_to_sym()};
+ }
// Forward substitution
Utils::unwind<0, N>( [&]( auto k ) {
inv( k, k ) = L( k, k );
@@ -558,7 +564,7 @@ namespace LHCb::LinAlg {
inv( i, k ) = s * L( i, i ); // multiply by the reciprocal
} );
} );
- return inv.cast_to_sym();
+ return std::pair{mask, inv.cast_to_sym()};
}
/// elementwise addition
diff --git a/Kernel/LHCbMath/include/LHCbMath/StateVertexUtils.h b/Kernel/LHCbMath/include/LHCbMath/StateVertexUtils.h
index 348eaa9551489e53e77dd8f4282f14b4b641e696..99e332919fac448c1762a661627414658584114b 100644
--- a/Kernel/LHCbMath/include/LHCbMath/StateVertexUtils.h
+++ b/Kernel/LHCbMath/include/LHCbMath/StateVertexUtils.h
@@ -101,22 +101,31 @@ namespace LHCb {
const auto dir = mom * ( 1.f / p3mag ); // rsqrt
const auto a = dot( dir, dx ) / p3mag;
- LHCb::LinAlg::resize_t<Sym6x6_t, 3> W{};
- W.fill( [&]( auto i, auto j ) {
+ LHCb::LinAlg::resize_t<Sym6x6_t, 3> Wtmp{};
+ Wtmp.fill( [&]( auto i, auto j ) {
return mothercov( i, j ) + cov6( i, j ) + a * a * cov6( i + 3, j + 3 ) -
a * ( cov6( i + 3, j ) + cov6( j + 3, i ) );
} );
// TODO check if easier to just give 3x3 cov matrices instead of creating 6x6 cov matrix
- W = W.invChol();
-
+ const auto [success, W] = Wtmp.invChol();
+ using mask_t = decltype( success );
+ constexpr auto nan = std::numeric_limits<float>::quiet_NaN();
+ if constexpr ( std::is_arithmetic_v<mask_t> ) {
+ if ( !success ) return std::tuple{nan, nan, nan};
+ }
const auto halfdChi2dLam2 = similarity( dir, W );
const auto decaylength = dir.dot( W * dx ) / halfdChi2dLam2;
- const auto decaylengtherr = sqrt( 1 / halfdChi2dLam2 ); // TODO: put rsqrt here
+ const auto decaylengtherr = sqrt( 1 / halfdChi2dLam2 );
const auto res = dx - dir * decaylength;
const auto chi2 = similarity( res, W );
- return std::tuple{chi2, decaylength, decaylengtherr};
+ if constexpr ( std::is_arithmetic_v<mask_t> ) {
+ return std::tuple{chi2, decaylength, decaylengtherr};
+ } else {
+ return std::tuple{select( success, chi2, nan ), select( success, decaylength, nan ),
+ select( success, decaylengtherr, nan )};
+ }
}
/////////////////////////////////////////////////////////////////////////
diff --git a/Kernel/LHCbMath/tests/test_matvec.cpp b/Kernel/LHCbMath/tests/test_matvec.cpp
index a502ef4d96f33b11068e23a3cce2fe19398b105b..e0b4497e3cc903a74f52d739e54717d61a05a921 100644
--- a/Kernel/LHCbMath/tests/test_matvec.cpp
+++ b/Kernel/LHCbMath/tests/test_matvec.cpp
@@ -162,7 +162,7 @@ BOOST_AUTO_TEST_CASE( test_MatSym ) {
MatSym<float, 3> someMatrixInverse{0.75f, 0.5f, 1.f, 0.25f, 0.5f, 0.75f};
//clang-format on
bool isEqual = true;
- auto shouldBeSomeMatrixInverse = someMatrix.invChol();
+ auto shouldBeSomeMatrixInverse = someMatrix.invChol().second;
for ( auto i = 0u; i < someMatrixInverse.m.size(); ++i ) {
if ( fabs( someMatrixInverse.m[i] - shouldBeSomeMatrixInverse.m[i] ) > std::numeric_limits<float>::epsilon() ) {
isEqual = false;
@@ -176,7 +176,7 @@ BOOST_AUTO_TEST_CASE( test_MatSym_vectorized ) {
using simd = SIMDWrapper::best::types;
MatSym<simd::float_v, 3> someMatrix{2, -1, 2, 0, -1, 2};
MatSym<simd::float_v, 3> someMatrixInverse{0.75f, 0.5f, 1.f, 0.25f, 0.5f, 0.75f};
- auto shouldBeSomeMatrixInverse = someMatrix.invChol();
+ auto shouldBeSomeMatrixInverse = someMatrix.invChol().second;
for ( auto i = 0u; i < someMatrixInverse.n_rows; ++i ) {
for ( auto j = 0u; j < someMatrixInverse.n_cols; ++j ) {
BOOST_CHECK( !any( abs( someMatrixInverse( i, j ) - shouldBeSomeMatrixInverse( i, j ) ) >