Skip to content
Snippets Groups Projects
Commit 4c735a43 authored by Andre Gunther's avatar Andre Gunther :island:
Browse files

Merge branch 'sevda-v2PV-packing-particlepackingfixed' into 'master' 

Fixed packing of particle covariance matrix

Closes #347

See merge request !4467
parents 41afd41e dc5dfb3c
No related branches found
No related tags found
1 merge request!4467Fixed packing of particle covariance matrix
Pipeline #7112253 passed
Stage: test
    Hide whitespace changes
    Inline Side-by-side
    Event/EventPacker/src/lib/PackedParticle.cpp
    + 135
    − 49
    View file @ 4c735a43
    ......@@ -25,6 +25,85 @@ namespace {
    static auto sqrt_or_0( const TYPE x ) {
    return ( x > TYPE( 0 ) ? std::sqrt( x ) : TYPE( 0 ) );
    }
    // ParticlePacker converts the XPE matrix to XMP representation, because that better reproduces the error on the mass.
    // However, if the mass is too small (like for a photon), then the conversion does not work. Therefore, we apply a
    // threshold. As long as the same mass is used in packer and unpacker, it does not matter. The latter is guaranteed by
    // using in the packer the 'unpacked p4' for the conversion.
    template <typename PELorentzVector>
    double packingmass( const PELorentzVector& p4 ) {
    constexpr double threshold = 1.0 / LHCb::Packer::ENERGY_SCALE;
    constexpr double threshold2 = threshold * threshold;
    const double m2 = p4.M2();
    return m2 < threshold2 ? 0.0 : std::sqrt( m2 );
    }
    /// transform a covariance matrix in the position-momentum-energy basis to the position-momentum-mass basis
    template <typename SymMatrix4x4, typename Matrix4x4, typename PELorentzVector>
    void convertToXPM( SymMatrix4x4& momcov, Matrix4x4& momposcov, const PELorentzVector& p4 ) {
    // This is a formulation that perhaps anyone can understand:
    // Gaudi::Matrix4x4 jacobian ; // (P,E) --> (P,M)
    // jacobian(0,0) = jacobian(1,1) = jacobian(2,2) = 1 ;
    // jacobian(3,0) = -p4.Px() / mass ;
    // jacobian(3,1) = -p4.Py() / mass ;
    // jacobian(3,2) = -p4.Pz() / mass ;
    // jacobian(3,3) = p4.E() / mass ;
    // // Sim(H,C) = H * C * H^T
    // momcov = ROOT::Math::Similarity( jacobian, momcov ) ;
    // momposcov = jacobian * momposcov;
    // But this one computes 3x less elements by exploiting the 0s and 1s:
    using Matrix1x4 = ROOT::Math::SMatrix<double, 1, 4>;
    Matrix1x4 jacobian{}; // only the M-row of the jacobian for (P,E) --> (P,M)
    auto mass = packingmass( p4 );
    if ( mass > 0 ) {
    jacobian( 0, 0 ) = -p4.Px() / mass;
    jacobian( 0, 1 ) = -p4.Py() / mass;
    jacobian( 0, 2 ) = -p4.Pz() / mass;
    jacobian( 0, 3 ) = p4.E() / mass;
    Matrix1x4 JC = jacobian * momcov;
    momcov( 3, 3 ) = ( JC * ROOT::Math::Transpose( jacobian ) )( 0, 0 );
    // If the masscov is so small that it would pack to zero, make sure all correlation elements are zero as well.
    constexpr auto masscovscale = LHCb::Packer::ENERGY_SCALE * LHCb::Packer::ENERGY_SCALE;
    if ( momcov( 3, 3 ) * masscovscale > 1 ) {
    for ( int i = 0; i < 3; ++i ) momcov( 3, i ) = JC( 0, i );
    momposcov.Place_at( jacobian * momposcov, 3, 0 );
    } else {
    for ( int i = 0; i < 4; ++i ) momcov( 3, i ) = 0;
    momposcov.Place_at( ROOT::Math::SMatrix<double, 1, 3>{}, 3, 0 );
    }
    } else {
    for ( int i = 0; i < 4; ++i ) momcov( 3, i ) = 0;
    momposcov.Place_at( ROOT::Math::SMatrix<double, 1, 3>{}, 3, 0 );
    }
    }
    /// transform a covariance matrix in the position-momentum-mass basis to the position-momentum-energy basis
    template <typename SymMatrix4x4, typename Matrix4x4, typename PELorentzVector>
    void convertToXPE( SymMatrix4x4& momcov, Matrix4x4& momposcov, const PELorentzVector& p4 ) {
    using Matrix1x4 = ROOT::Math::SMatrix<double, 1, 4>;
    Matrix1x4 jacobian{}; // only the E-row of the jacobian (P,M) --> (P,E)
    const auto mass = packingmass( p4 );
    jacobian( 0, 0 ) = p4.Px() / p4.E();
    jacobian( 0, 1 ) = p4.Py() / p4.E();
    jacobian( 0, 2 ) = p4.Pz() / p4.E();
    jacobian( 0, 3 ) = mass / p4.E();
    Matrix1x4 JC = jacobian * momcov;
    momcov( 3, 3 ) = ( JC * ROOT::Math::Transpose( jacobian ) )( 0, 0 );
    for ( int i = 0; i < 3; ++i ) momcov( 3, i ) = JC( 0, i );
    momposcov.Place_at( jacobian * momposcov, 3, 0 );
    }
    /// Convert packed momentum back to lorentzvector. Turned this into a standalone function such that it can also be
    /// used in the packer, as described above.
    auto unpackmomentum( const LHCb::ParticlePacker::PackedData& ppart, bool isVZero = false ) {
    // Lorentz momentum vector (force double precision to compute E)
    const double pz = StandardPacker::energy( ppart.lv_pz );
    const double px = ( isVZero ? StandardPacker::slope( ppart.lv_px ) * pz : StandardPacker::energy( ppart.lv_px ) );
    const double py = ( isVZero ? StandardPacker::slope( ppart.lv_py ) * pz : StandardPacker::energy( ppart.lv_py ) );
    const double mass = ppart.lv_mass;
    const double E = std::sqrt( ( px * px ) + ( py * py ) + ( pz * pz ) + ( mass * mass ) );
    return Gaudi::LorentzVector{px, py, pz, E};
    }
    } // namespace
    using namespace LHCb;
    ......@@ -47,31 +126,38 @@ void ParticlePacker::pack( const Data& part, PackedData& ppart, PackedDataVector
    ppart.measMassErr = StandardPacker::mass( part.measuredMassErr() );
    // Lorentz vector
    ppart.lv_px = StandardPacker::energy( part.momentum().px() );
    ppart.lv_py = StandardPacker::energy( part.momentum().py() );
    ppart.lv_pz = StandardPacker::energy( part.momentum().pz() );
    ppart.lv_mass = (float)part.momentum().M();
    ppart.lv_px = StandardPacker::energy( part.momentum().px() );
    ppart.lv_py = StandardPacker::energy( part.momentum().py() );
    ppart.lv_pz = StandardPacker::energy( part.momentum().pz() );
    const double mass = sqrt_or_0( part.momentum().M2() );
    ppart.lv_mass = static_cast<float>( mass );
    // reference point
    ppart.refx = StandardPacker::position( part.referencePoint().x() );
    ppart.refy = StandardPacker::position( part.referencePoint().y() );
    ppart.refz = StandardPacker::position( part.referencePoint().z() );
    // Rotate cov matrices from (X,P,E) to (X,P,M) space. To make sure that the rotation is correctly inverted
    // in the unpacking, we rotate with the 'unpacked' momentum.
    auto momcov = part.momCovMatrix();
    auto momposcov = part.posMomCovMatrix(); // actually momposcov!
    convertToXPM( momcov, momposcov, unpackmomentum( ppart ) );
    // Mom Cov
    const auto merr00 = sqrt_or_0( part.momCovMatrix()( 0, 0 ) );
    const auto merr11 = sqrt_or_0( part.momCovMatrix()( 1, 1 ) );
    const auto merr22 = sqrt_or_0( part.momCovMatrix()( 2, 2 ) );
    const auto merr33 = sqrt_or_0( part.momCovMatrix()( 3, 3 ) );
    const auto merr00 = sqrt_or_0( momcov( 0, 0 ) );
    const auto merr11 = sqrt_or_0( momcov( 1, 1 ) );
    const auto merr22 = sqrt_or_0( momcov( 2, 2 ) );
    const auto merr33 = sqrt_or_0( momcov( 3, 3 ) );
    ppart.momCov00 = StandardPacker::energy( merr00 );
    ppart.momCov11 = StandardPacker::energy( merr11 );
    ppart.momCov22 = StandardPacker::energy( merr22 );
    ppart.momCov33 = StandardPacker::energy( merr33 );
    ppart.momCov10 = StandardPacker::fraction( part.momCovMatrix()( 1, 0 ), ( merr11 * merr00 ) );
    ppart.momCov20 = StandardPacker::fraction( part.momCovMatrix()( 2, 0 ), ( merr22 * merr00 ) );
    ppart.momCov21 = StandardPacker::fraction( part.momCovMatrix()( 2, 1 ), ( merr22 * merr11 ) );
    ppart.momCov30 = StandardPacker::fraction( part.momCovMatrix()( 3, 0 ), ( merr33 * merr00 ) );
    ppart.momCov31 = StandardPacker::fraction( part.momCovMatrix()( 3, 1 ), ( merr33 * merr11 ) );
    ppart.momCov32 = StandardPacker::fraction( part.momCovMatrix()( 3, 2 ), ( merr33 * merr22 ) );
    ppart.momCov10 = StandardPacker::fraction( momcov( 1, 0 ), ( merr11 * merr00 ) );
    ppart.momCov20 = StandardPacker::fraction( momcov( 2, 0 ), ( merr22 * merr00 ) );
    ppart.momCov21 = StandardPacker::fraction( momcov( 2, 1 ), ( merr22 * merr11 ) );
    ppart.momCov30 = StandardPacker::fraction( momcov( 3, 0 ), ( merr33 * merr00 ) );
    ppart.momCov31 = StandardPacker::fraction( momcov( 3, 1 ), ( merr33 * merr11 ) );
    ppart.momCov32 = StandardPacker::fraction( momcov( 3, 2 ), ( merr33 * merr22 ) );
    // Pos Cov
    const auto perr00 = sqrt_or_0( part.posCovMatrix()( 0, 0 ) );
    ......@@ -85,37 +171,37 @@ void ParticlePacker::pack( const Data& part, PackedData& ppart, PackedDataVector
    ppart.posCov21 = StandardPacker::fraction( part.posCovMatrix()( 2, 1 ), ( perr22 * perr11 ) );
    // PosMom Cov
    ppart.pmCov00 = StandardPacker::fltPacked( part.posMomCovMatrix()( 0, 0 ) );
    ppart.pmCov01 = StandardPacker::fltPacked( part.posMomCovMatrix()( 0, 1 ) );
    ppart.pmCov02 = StandardPacker::fltPacked( part.posMomCovMatrix()( 0, 2 ) );
    ppart.pmCov10 = StandardPacker::fltPacked( part.posMomCovMatrix()( 1, 0 ) );
    ppart.pmCov11 = StandardPacker::fltPacked( part.posMomCovMatrix()( 1, 1 ) );
    ppart.pmCov12 = StandardPacker::fltPacked( part.posMomCovMatrix()( 1, 2 ) );
    ppart.pmCov20 = StandardPacker::fltPacked( part.posMomCovMatrix()( 2, 0 ) );
    ppart.pmCov21 = StandardPacker::fltPacked( part.posMomCovMatrix()( 2, 1 ) );
    ppart.pmCov22 = StandardPacker::fltPacked( part.posMomCovMatrix()( 2, 2 ) );
    ppart.pmCov30 = StandardPacker::fltPacked( part.posMomCovMatrix()( 3, 0 ) );
    ppart.pmCov31 = StandardPacker::fltPacked( part.posMomCovMatrix()( 3, 1 ) );
    ppart.pmCov32 = StandardPacker::fltPacked( part.posMomCovMatrix()( 3, 2 ) );
    ppart.pmCov00 = StandardPacker::fltPacked( momposcov( 0, 0 ) );
    ppart.pmCov01 = StandardPacker::fltPacked( momposcov( 0, 1 ) );
    ppart.pmCov02 = StandardPacker::fltPacked( momposcov( 0, 2 ) );
    ppart.pmCov10 = StandardPacker::fltPacked( momposcov( 1, 0 ) );
    ppart.pmCov11 = StandardPacker::fltPacked( momposcov( 1, 1 ) );
    ppart.pmCov12 = StandardPacker::fltPacked( momposcov( 1, 2 ) );
    ppart.pmCov20 = StandardPacker::fltPacked( momposcov( 2, 0 ) );
    ppart.pmCov21 = StandardPacker::fltPacked( momposcov( 2, 1 ) );
    ppart.pmCov22 = StandardPacker::fltPacked( momposcov( 2, 2 ) );
    ppart.pmCov30 = StandardPacker::fltPacked( momposcov( 3, 0 ) );
    ppart.pmCov31 = StandardPacker::fltPacked( momposcov( 3, 1 ) );
    ppart.pmCov32 = StandardPacker::fltPacked( momposcov( 3, 2 ) );
    // extra info
    ppart.firstExtra = pparts.extra().size();
    for ( const auto& [k, v] : part.extraInfo() ) {
    pparts.extra().emplace_back( k, StandardPacker::fltPacked( v ) );
    parent().debug() << "extra info " << k << " " << v << endmsg;
    if ( parent().msgLevel( MSG::DEBUG ) ) parent().debug() << "extra info " << k << " " << v << endmsg;
    }
    ppart.lastExtra = pparts.extra().size();
    // end vertex
    if ( part.endVertex() ) {
    ppart.vertex = StandardPacker::reference64( &pparts, part.endVertex() );
    parent().debug() << "endvertex " << part.endVertex()->key() << endmsg;
    if ( parent().msgLevel( MSG::DEBUG ) ) parent().debug() << "endvertex " << part.endVertex()->key() << endmsg;
    }
    // protoparticle
    if ( part.proto() ) {
    ppart.proto = StandardPacker::reference64( &pparts, part.proto() );
    parent().debug() << "proto " << part.proto()->key() << endmsg;
    if ( parent().msgLevel( MSG::DEBUG ) ) parent().debug() << "proto " << part.proto()->key() << endmsg;
    }
    // daughters
    ......@@ -123,14 +209,14 @@ void ParticlePacker::pack( const Data& part, PackedData& ppart, PackedDataVector
    for ( const auto& P : part.daughters() ) {
    if ( P.target() ) {
    pparts.daughters().push_back( StandardPacker::reference64( &pparts, P ) );
    parent().debug() << "daughter " << P->key() << endmsg;
    if ( parent().msgLevel( MSG::DEBUG ) ) parent().debug() << "daughter " << P->key() << endmsg;
    }
    }
    ppart.lastDaughter = pparts.daughters().size();
    if ( part.pv() ) {
    ppart.pv = StandardPacker::reference64( &pparts, part.pv() );
    parent().debug() << "pv " << part.pv()->key() << endmsg;
    if ( parent().msgLevel( MSG::DEBUG ) ) parent().debug() << "pv " << part.pv()->key() << endmsg;
    }
    }
    ......@@ -151,12 +237,7 @@ StatusCode ParticlePacker::unpack( const PackedData& ppart, Data& part, const Pa
    part.setMeasuredMassErr( StandardPacker::mass( ppart.measMassErr ) );
    // Lorentz momentum vector
    const auto pz = StandardPacker::energy( ppart.lv_pz );
    const auto px = ( isVZero ? StandardPacker::slope( ppart.lv_px ) * pz : StandardPacker::energy( ppart.lv_px ) );
    const auto py = ( isVZero ? StandardPacker::slope( ppart.lv_py ) * pz : StandardPacker::energy( ppart.lv_py ) );
    const auto mass = ppart.lv_mass;
    const auto E = std::sqrt( ( px * px ) + ( py * py ) + ( pz * pz ) + ( mass * mass ) );
    part.setMomentum( Gaudi::LorentzVector( px, py, pz, E ) );
    part.setMomentum( unpackmomentum( ppart, isVZero ) );
    // reference point
    part.setReferencePoint( Gaudi::XYZPoint( StandardPacker::position( ppart.refx ),
    ......@@ -164,11 +245,12 @@ StatusCode ParticlePacker::unpack( const PackedData& ppart, Data& part, const Pa
    StandardPacker::position( ppart.refz ) ) );
    // Mom Cov
    auto& momCov = *( const_cast<Gaudi::SymMatrix4x4*>( &part.momCovMatrix() ) );
    const auto merr00 =
    ( isVZero ? StandardPacker::slope( ppart.momCov00 ) * px : StandardPacker::energy( ppart.momCov00 ) );
    const auto merr11 =
    ( isVZero ? StandardPacker::slope( ppart.momCov11 ) * py : StandardPacker::energy( ppart.momCov11 ) );
    Gaudi::SymMatrix4x4 momCov{};
    const auto merr00 = ( isVZero ? StandardPacker::slope( ppart.momCov00 ) * part.momentum().Px()
    : StandardPacker::energy( ppart.momCov00 ) );
    const auto merr11 = ( isVZero ? StandardPacker::slope( ppart.momCov11 ) * part.momentum().Py()
    : StandardPacker::energy( ppart.momCov11 ) );
    const auto merr22 = StandardPacker::energy( ppart.momCov22 );
    const auto merr33 = StandardPacker::energy( ppart.momCov33 );
    momCov( 0, 0 ) = std::pow( merr00, 2 );
    ......@@ -195,7 +277,7 @@ StatusCode ParticlePacker::unpack( const PackedData& ppart, Data& part, const Pa
    posCov( 2, 1 ) = perr22 * perr11 * StandardPacker::fraction( ppart.posCov21 );
    // Pos Mom Cov
    auto& pmCov = *( const_cast<Gaudi::Matrix4x3*>( &part.posMomCovMatrix() ) );
    Gaudi::Matrix4x3 pmCov{};
    pmCov( 0, 0 ) = StandardPacker::fltPacked( ppart.pmCov00 );
    pmCov( 0, 1 ) = StandardPacker::fltPacked( ppart.pmCov01 );
    pmCov( 0, 2 ) = StandardPacker::fltPacked( ppart.pmCov02 );
    ......@@ -209,6 +291,10 @@ StatusCode ParticlePacker::unpack( const PackedData& ppart, Data& part, const Pa
    pmCov( 3, 1 ) = StandardPacker::fltPacked( ppart.pmCov31 );
    pmCov( 3, 2 ) = StandardPacker::fltPacked( ppart.pmCov32 );
    if ( pparts.packingVersion() >= 2 ) convertToXPE( momCov, pmCov, part.momentum() );
    part.setMomCovMatrix( momCov );
    part.setPosMomCovMatrix( pmCov );
    // extra info
    for ( const auto& [k, v] : with_carry( pparts.extra(), ppart.firstExtra, ppart.lastExtra ) ) {
    part.addInfo( k, StandardPacker::fltPacked( v ) );
    ......@@ -219,7 +305,7 @@ StatusCode ParticlePacker::unpack( const PackedData& ppart, Data& part, const Pa
    int hintID( 0 ), key( 0 );
    if ( StandardPacker::hintAndKey64( ppart.vertex, &pparts, &parts, hintID, key ) ) {
    part.setEndVertex( {&parts, hintID, key} );
    parent().debug() << "endvertex " << ppart.vertex << endmsg;
    if ( parent().msgLevel( MSG::DEBUG ) ) parent().debug() << "endvertex " << ppart.vertex << endmsg;
    } else {
    parent().error() << "Corrupt Particle Vertex SmartRef found" << endmsg;
    }
    ......@@ -230,7 +316,7 @@ StatusCode ParticlePacker::unpack( const PackedData& ppart, Data& part, const Pa
    int hintID( 0 ), key( 0 );
    if ( StandardPacker::hintAndKey64( ppart.proto, &pparts, &parts, hintID, key ) ) {
    part.setProto( {&parts, hintID, key} );
    parent().debug() << "proto " << ppart.proto << endmsg;
    if ( parent().msgLevel( MSG::DEBUG ) ) parent().debug() << "proto " << ppart.proto << endmsg;
    } else {
    parent().error() << "Corrupt Particle ProtoParticle SmartRef found" << endmsg;
    }
    ......@@ -241,7 +327,7 @@ StatusCode ParticlePacker::unpack( const PackedData& ppart, Data& part, const Pa
    int hintID( 0 ), key( 0 );
    if ( StandardPacker::hintAndKey64( d, &pparts, &parts, hintID, key ) ) {
    part.addToDaughters( {&parts, hintID, key} );
    parent().debug() << "daughter " << d << endmsg;
    if ( parent().msgLevel( MSG::DEBUG ) ) parent().debug() << "daughter " << d << endmsg;
    } else {
    parent().error() << "Corrupt Particle Daughter Particle SmartRef found" << endmsg;
    }
    ......@@ -251,7 +337,7 @@ StatusCode ParticlePacker::unpack( const PackedData& ppart, Data& part, const Pa
    int hintID( 0 ), key( 0 );
    if ( StandardPacker::hintAndKey64( ppart.pv, &pparts, &parts, hintID, key ) ) {
    part.setPV( {&parts, hintID, key} );
    parent().debug() << "pv " << ppart.pv << endmsg;
    if ( parent().msgLevel( MSG::DEBUG ) ) parent().debug() << "pv " << ppart.pv << endmsg;
    } else {
    parent().error() << "Corrupt Particle PV SmartRef found" << endmsg;
    }
    ......@@ -327,9 +413,9 @@ StatusCode ParticlePacker::check( const Data* dataA, const Data* dataB ) const {
    // Mom Cov
    const std::array<double, 4> tolDiagMomCov = {
    {Packer::ENERGY_TOL, Packer::ENERGY_TOL, Packer::ENERGY_TOL, Packer::ENERGY_TOL}};
    {Packer::ENERGY_TOL, Packer::ENERGY_TOL, Packer::ENERGY_TOL, 2 * Packer::ENERGY_TOL}};
    ok &= ch.compareCovMatrices<Gaudi::SymMatrix4x4, 4>( "MomCov", dataA->momCovMatrix(), dataB->momCovMatrix(),
    tolDiagMomCov, Packer::FRACTION_TOL );
    tolDiagMomCov, 3 * Packer::FRACTION_TOL );
    // Pos Cov
    const std::array<double, 3> tolDiagPosCov = {{Packer::POSITION_TOL, Packer::POSITION_TOL, Packer::POSITION_TOL}};
    ......
    0% or .
    You are about to add 0 people to the discussion. Proceed with caution.
    Finish editing this message first!
    Please register or to comment