diff --git a/analysis/src/main/java/org/hps/analysis/MC/TrackToMCParticleRelationsDriver.java b/analysis/src/main/java/org/hps/analysis/MC/TrackToMCParticleRelationsDriver.java index 451c4fb68d..ed8bf661fd 100644 --- a/analysis/src/main/java/org/hps/analysis/MC/TrackToMCParticleRelationsDriver.java +++ b/analysis/src/main/java/org/hps/analysis/MC/TrackToMCParticleRelationsDriver.java @@ -118,7 +118,7 @@ protected void process(EventHeader event) { MCParticle mcp = ttm.getMCParticle(); if (mcp != null) { - trackToMCParticleRelations.add(new BaseLCRelation(track,mcp)); + trackToMCParticleRelations.add(new BaseLCRelation(track,mcp,ttm.getPurity())); //Hep3Vector origin = new BasicHep3Vector(0.,0.,0.); HelicalTrackFit mcp_htf = TrackUtils.getHTF(mcp,bfield); @@ -178,8 +178,9 @@ protected void process(EventHeader event) { } int flag = 1 << LCIOConstants.TRBIT_HITS; + int relFlag = 1 << LCIOConstants.LCREL_WEIGHTED; event.put(trackCollectionName+"Truth", truthTrackCollection, Track.class, flag); event.put(trackCollectionName+"ToTruthTrackRelations", trackToTruthTrackRelations, LCRelation.class, 0); - event.put(trackCollectionName+"ToMCParticleRelations", trackToMCParticleRelations, LCRelation.class, 0); + event.put(trackCollectionName+"ToMCParticleRelations", trackToMCParticleRelations, LCRelation.class, relFlag); }//closes process } diff --git a/analysis/src/main/java/org/hps/analysis/tuple/CascadeVertexTupleDriver.java b/analysis/src/main/java/org/hps/analysis/tuple/CascadeVertexTupleDriver.java new file mode 100644 index 0000000000..d57ad31f0c --- /dev/null +++ b/analysis/src/main/java/org/hps/analysis/tuple/CascadeVertexTupleDriver.java @@ -0,0 +1,929 @@ +package org.hps.analysis.tuple; + +import java.io.FileNotFoundException; +import java.io.PrintWriter; +import java.util.Arrays; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +import hep.physics.vec.Hep3Vector; + +import org.hps.recon.vertexing.BilliorVertex; +import org.hps.recon.vertexing.CascadeVertexer; +import org.lcsim.event.EventHeader; +import org.lcsim.event.LCRelation; +import org.lcsim.event.MCParticle; +import org.lcsim.event.ReconstructedParticle; +import org.lcsim.event.Track; +import org.lcsim.event.Vertex; +import org.lcsim.geometry.Detector; +import org.lcsim.util.Driver; + +/** + * Writes a flat ASCII ntuple of cascade (V0 + recoil-electron production-vertex) fit + * quantities, for offline inspection/plotting of {@link CascadeVertexer} output. + * One row per cascade candidate. Header line is variable names joined by ":"; data + * rows are tab-separated, matching the convention used by {@link TupleMaker#writeTuple} + * (not reused directly here since that class requires a hardware trigger bank and other + * DQM-specific setup that doesn't apply to this validation driver). + */ +public class CascadeVertexTupleDriver extends Driver { + + private static final double ELECTRON_MASS = 0.000511; + + private static final List VARIABLES = Arrays.asList( + "run/I", "event/I", + "cascadeVtxX/D", "cascadeVtxY/D", "cascadeVtxZ/D", + "cascadeVtxXErr/D", "cascadeVtxYErr/D", "cascadeVtxZErr/D", + "cascadeChi2/D", "cascadeNdf/I", "cascadeMass/D", + "v0PX/D", "v0PY/D", "v0PZ/D", "v0PXErr/D", "v0PYErr/D", "v0PZErr/D", + "v0UncPX/D", "v0UncPY/D", "v0UncPZ/D", "v0UncMass/D", + "recoilPX/D", "recoilPY/D", "recoilPZ/D", "recoilPXErr/D", "recoilPYErr/D", "recoilPZErr/D", + "recoilUncPX/D", "recoilUncPY/D", "recoilUncPZ/D", + "eleFitPX/D", "eleFitPY/D", "eleFitPZ/D", "eleFitPXErr/D", "eleFitPYErr/D", "eleFitPZErr/D", + "posFitPX/D", "posFitPY/D", "posFitPZ/D", "posFitPXErr/D", "posFitPYErr/D", "posFitPZErr/D", + "eleUncPX/D", "eleUncPY/D", "eleUncPZ/D", "posUncPX/D", "posUncPY/D", "posUncPZ/D", + "v0VtxX/D", "v0VtxY/D", "v0VtxZ/D", + "v0VtxXErr/D", "v0VtxYErr/D", "v0VtxZErr/D", "v0Mass/D", "v0Chi2/D", + "v0InputVtxX/D", "v0InputVtxY/D", "v0InputVtxZ/D", + "v0InputVtxXErr/D", "v0InputVtxYErr/D", "v0InputVtxZErr/D", "v0InputMass/D", "v0InputChi2/D", + "v0ProjX/D", "v0ProjY/D", "v0ProjXErr/D", "v0ProjYErr/D", + "recoilProjX/D", "recoilProjY/D", "recoilProjXErr/D", "recoilProjYErr/D", + "apMassMC/D", "apVtxXMC/D", "apVtxYMC/D", "apVtxZMC/D", + "apOriginXMC/D", "apOriginYMC/D", "apOriginZMC/D", + "eleMomXMC/D", "eleMomYMC/D", "eleMomZMC/D", + "posMomXMC/D", "posMomYMC/D", "posMomZMC/D", + "recoilMomXMC/D", "recoilMomYMC/D", "recoilMomZMC/D", + "v0EleTruthMatched/I", "v0PosTruthMatched/I", "v0BothTruthMatchedToAp/I", + "recoilTruthMatched/I", + "v0ElePurity/D", "v0PosPurity/D", "recoilPurity/D", + "cascadeBeamMomConstrainedVtxX/D", "cascadeBeamMomConstrainedVtxY/D", "cascadeBeamMomConstrainedVtxZ/D", + "cascadeBeamMomConstrainedVtxXErr/D", "cascadeBeamMomConstrainedVtxYErr/D", "cascadeBeamMomConstrainedVtxZErr/D", + "cascadeBeamMomConstrainedChi2/D", "cascadeBeamMomConstrainedNdf/I", "cascadeBeamMomConstrainedMass/D", + "cascadeBeamMomConstrainedV0PX/D", "cascadeBeamMomConstrainedV0PY/D", "cascadeBeamMomConstrainedV0PZ/D", + "cascadeBeamMomConstrainedV0PXErr/D", "cascadeBeamMomConstrainedV0PYErr/D", "cascadeBeamMomConstrainedV0PZErr/D", + "cascadeBeamMomConstrainedRecoilPX/D", "cascadeBeamMomConstrainedRecoilPY/D", "cascadeBeamMomConstrainedRecoilPZ/D", + "cascadeBeamMomConstrainedRecoilPXErr/D", "cascadeBeamMomConstrainedRecoilPYErr/D", "cascadeBeamMomConstrainedRecoilPZErr/D", + "cascadeBeamMomConstrainedElePX/D", "cascadeBeamMomConstrainedElePY/D", "cascadeBeamMomConstrainedElePZ/D", + "cascadeBeamMomConstrainedElePXErr/D", "cascadeBeamMomConstrainedElePYErr/D", "cascadeBeamMomConstrainedElePZErr/D", + "cascadeBeamMomConstrainedPosPX/D", "cascadeBeamMomConstrainedPosPY/D", "cascadeBeamMomConstrainedPosPZ/D", + "cascadeBeamMomConstrainedPosPXErr/D", "cascadeBeamMomConstrainedPosPYErr/D", "cascadeBeamMomConstrainedPosPZErr/D", + "cascadeBeamMomConstrainedV0VtxX/D", "cascadeBeamMomConstrainedV0VtxY/D", "cascadeBeamMomConstrainedV0VtxZ/D", + "cascadeBeamMomConstrainedV0VtxXErr/D", "cascadeBeamMomConstrainedV0VtxYErr/D", "cascadeBeamMomConstrainedV0VtxZErr/D", + "cascadeBeamMomConstrainedV0Mass/D", "cascadeBeamMomConstrainedV0Chi2/D", + "cascadeBeamspotConstrainedVtxX/D", "cascadeBeamspotConstrainedVtxY/D", "cascadeBeamspotConstrainedVtxZ/D", + "cascadeBeamspotConstrainedVtxXErr/D", "cascadeBeamspotConstrainedVtxYErr/D", "cascadeBeamspotConstrainedVtxZErr/D", + "cascadeBeamspotConstrainedChi2/D", "cascadeBeamspotConstrainedNdf/I", "cascadeBeamspotConstrainedMass/D", + "cascadeBeamspotConstrainedV0PX/D", "cascadeBeamspotConstrainedV0PY/D", "cascadeBeamspotConstrainedV0PZ/D", + "cascadeBeamspotConstrainedV0PXErr/D", "cascadeBeamspotConstrainedV0PYErr/D", "cascadeBeamspotConstrainedV0PZErr/D", + "cascadeBeamspotConstrainedRecoilPX/D", "cascadeBeamspotConstrainedRecoilPY/D", "cascadeBeamspotConstrainedRecoilPZ/D", + "cascadeBeamspotConstrainedRecoilPXErr/D", "cascadeBeamspotConstrainedRecoilPYErr/D", "cascadeBeamspotConstrainedRecoilPZErr/D", + "cascadeBeamspotConstrainedElePX/D", "cascadeBeamspotConstrainedElePY/D", "cascadeBeamspotConstrainedElePZ/D", + "cascadeBeamspotConstrainedElePXErr/D", "cascadeBeamspotConstrainedElePYErr/D", "cascadeBeamspotConstrainedElePZErr/D", + "cascadeBeamspotConstrainedPosPX/D", "cascadeBeamspotConstrainedPosPY/D", "cascadeBeamspotConstrainedPosPZ/D", + "cascadeBeamspotConstrainedPosPXErr/D", "cascadeBeamspotConstrainedPosPYErr/D", "cascadeBeamspotConstrainedPosPZErr/D", + "cascadeBeamspotConstrainedV0VtxX/D", "cascadeBeamspotConstrainedV0VtxY/D", "cascadeBeamspotConstrainedV0VtxZ/D", + "cascadeBeamspotConstrainedV0VtxXErr/D", "cascadeBeamspotConstrainedV0VtxYErr/D", "cascadeBeamspotConstrainedV0VtxZErr/D", + "cascadeBeamspotConstrainedV0Mass/D", "cascadeBeamspotConstrainedV0Chi2/D", + "cascadeBothConstrainedVtxX/D", "cascadeBothConstrainedVtxY/D", "cascadeBothConstrainedVtxZ/D", + "cascadeBothConstrainedVtxXErr/D", "cascadeBothConstrainedVtxYErr/D", "cascadeBothConstrainedVtxZErr/D", + "cascadeBothConstrainedChi2/D", "cascadeBothConstrainedNdf/I", "cascadeBothConstrainedMass/D", + "cascadeBothConstrainedV0PX/D", "cascadeBothConstrainedV0PY/D", "cascadeBothConstrainedV0PZ/D", + "cascadeBothConstrainedV0PXErr/D", "cascadeBothConstrainedV0PYErr/D", "cascadeBothConstrainedV0PZErr/D", + "cascadeBothConstrainedRecoilPX/D", "cascadeBothConstrainedRecoilPY/D", "cascadeBothConstrainedRecoilPZ/D", + "cascadeBothConstrainedRecoilPXErr/D", "cascadeBothConstrainedRecoilPYErr/D", "cascadeBothConstrainedRecoilPZErr/D", + "cascadeBothConstrainedElePX/D", "cascadeBothConstrainedElePY/D", "cascadeBothConstrainedElePZ/D", + "cascadeBothConstrainedElePXErr/D", "cascadeBothConstrainedElePYErr/D", "cascadeBothConstrainedElePZErr/D", + "cascadeBothConstrainedPosPX/D", "cascadeBothConstrainedPosPY/D", "cascadeBothConstrainedPosPZ/D", + "cascadeBothConstrainedPosPXErr/D", "cascadeBothConstrainedPosPYErr/D", "cascadeBothConstrainedPosPZErr/D", + "cascadeBothConstrainedV0VtxX/D", "cascadeBothConstrainedV0VtxY/D", "cascadeBothConstrainedV0VtxZ/D", + "cascadeBothConstrainedV0VtxXErr/D", "cascadeBothConstrainedV0VtxYErr/D", "cascadeBothConstrainedV0VtxZErr/D", + "cascadeBothConstrainedV0Mass/D", "cascadeBothConstrainedV0Chi2/D", + "ntrackVtxX/D", "ntrackVtxY/D", "ntrackVtxZ/D", + "ntrackVtxXErr/D", "ntrackVtxYErr/D", "ntrackVtxZErr/D", + "ntrackChi2/D", "ntrackNdf/I", "ntrackMass/D", + "ntrackElePX/D", "ntrackElePY/D", "ntrackElePZ/D", + "ntrackElePXErr/D", "ntrackElePYErr/D", "ntrackElePZErr/D", + "ntrackPosPX/D", "ntrackPosPY/D", "ntrackPosPZ/D", + "ntrackPosPXErr/D", "ntrackPosPYErr/D", "ntrackPosPZErr/D", + "ntrackRecoilPX/D", "ntrackRecoilPY/D", "ntrackRecoilPZ/D", + "ntrackRecoilPXErr/D", "ntrackRecoilPYErr/D", "ntrackRecoilPZErr/D", + "ntrackBeamMomConstrainedVtxX/D", "ntrackBeamMomConstrainedVtxY/D", "ntrackBeamMomConstrainedVtxZ/D", + "ntrackBeamMomConstrainedVtxXErr/D", "ntrackBeamMomConstrainedVtxYErr/D", "ntrackBeamMomConstrainedVtxZErr/D", + "ntrackBeamMomConstrainedChi2/D", "ntrackBeamMomConstrainedNdf/I", "ntrackBeamMomConstrainedMass/D", + "ntrackBeamMomConstrainedElePX/D", "ntrackBeamMomConstrainedElePY/D", "ntrackBeamMomConstrainedElePZ/D", + "ntrackBeamMomConstrainedElePXErr/D", "ntrackBeamMomConstrainedElePYErr/D", "ntrackBeamMomConstrainedElePZErr/D", + "ntrackBeamMomConstrainedPosPX/D", "ntrackBeamMomConstrainedPosPY/D", "ntrackBeamMomConstrainedPosPZ/D", + "ntrackBeamMomConstrainedPosPXErr/D", "ntrackBeamMomConstrainedPosPYErr/D", "ntrackBeamMomConstrainedPosPZErr/D", + "ntrackBeamMomConstrainedRecoilPX/D", "ntrackBeamMomConstrainedRecoilPY/D", "ntrackBeamMomConstrainedRecoilPZ/D", + "ntrackBeamMomConstrainedRecoilPXErr/D", "ntrackBeamMomConstrainedRecoilPYErr/D", "ntrackBeamMomConstrainedRecoilPZErr/D", + "ntrackBeamspotConstrainedVtxX/D", "ntrackBeamspotConstrainedVtxY/D", "ntrackBeamspotConstrainedVtxZ/D", + "ntrackBeamspotConstrainedVtxXErr/D", "ntrackBeamspotConstrainedVtxYErr/D", "ntrackBeamspotConstrainedVtxZErr/D", + "ntrackBeamspotConstrainedChi2/D", "ntrackBeamspotConstrainedNdf/I", "ntrackBeamspotConstrainedMass/D", + "ntrackBeamspotConstrainedElePX/D", "ntrackBeamspotConstrainedElePY/D", "ntrackBeamspotConstrainedElePZ/D", + "ntrackBeamspotConstrainedElePXErr/D", "ntrackBeamspotConstrainedElePYErr/D", "ntrackBeamspotConstrainedElePZErr/D", + "ntrackBeamspotConstrainedPosPX/D", "ntrackBeamspotConstrainedPosPY/D", "ntrackBeamspotConstrainedPosPZ/D", + "ntrackBeamspotConstrainedPosPXErr/D", "ntrackBeamspotConstrainedPosPYErr/D", "ntrackBeamspotConstrainedPosPZErr/D", + "ntrackBeamspotConstrainedRecoilPX/D", "ntrackBeamspotConstrainedRecoilPY/D", "ntrackBeamspotConstrainedRecoilPZ/D", + "ntrackBeamspotConstrainedRecoilPXErr/D", "ntrackBeamspotConstrainedRecoilPYErr/D", "ntrackBeamspotConstrainedRecoilPZErr/D", + "ntrackBothConstrainedVtxX/D", "ntrackBothConstrainedVtxY/D", "ntrackBothConstrainedVtxZ/D", + "ntrackBothConstrainedVtxXErr/D", "ntrackBothConstrainedVtxYErr/D", "ntrackBothConstrainedVtxZErr/D", + "ntrackBothConstrainedChi2/D", "ntrackBothConstrainedNdf/I", "ntrackBothConstrainedMass/D", + "ntrackBothConstrainedElePX/D", "ntrackBothConstrainedElePY/D", "ntrackBothConstrainedElePZ/D", + "ntrackBothConstrainedElePXErr/D", "ntrackBothConstrainedElePYErr/D", "ntrackBothConstrainedElePZErr/D", + "ntrackBothConstrainedPosPX/D", "ntrackBothConstrainedPosPY/D", "ntrackBothConstrainedPosPZ/D", + "ntrackBothConstrainedPosPXErr/D", "ntrackBothConstrainedPosPYErr/D", "ntrackBothConstrainedPosPZErr/D", + "ntrackBothConstrainedRecoilPX/D", "ntrackBothConstrainedRecoilPY/D", "ntrackBothConstrainedRecoilPZ/D", + "ntrackBothConstrainedRecoilPXErr/D", "ntrackBothConstrainedRecoilPYErr/D", "ntrackBothConstrainedRecoilPZErr/D"); + + private String cascadeVertexCandidatesColName = "CascadeVertexCandidates"; + // Beam-momentum-constrained refit of cascadeVertexCandidatesColName, index-aligned with + // it (see ReconParticleDriver#findCascadeVertices / CascadeVertexer#placeholderCascade). + // Null/off by default, matching cascadeVertexCandidatesColName's own opt-in convention. + private String cascadeVertexCandidatesBeamConstrainedColName = null; + // Beamspot-position-constrained refit of cascadeVertexCandidatesColName, index-aligned with + // it. Null/off by default, matching cascadeVertexCandidatesColName's own opt-in convention. + private String cascadeVertexCandidatesBeamspotConstrainedColName = null; + // Refit of cascadeVertexCandidatesColName with both the beamspot-position and + // beam-momentum constraints applied together, index-aligned with it. Null/off by default. + private String cascadeVertexCandidatesBothConstrainedColName = null; + // Single-common-vertex ("N-track") fit of the same three tracks as + // cascadeVertexCandidatesColName, index-aligned with it (see + // ReconParticleDriver#findCascadeVertices / NTrackVertexer). Null/off by default, + // matching cascadeVertexCandidatesColName's own opt-in convention. + private String ntrackVertexCandidatesColName = null; + // Beam-momentum-constrained refit of ntrackVertexCandidatesColName, index-aligned with it. + private String ntrackVertexCandidatesBeamConstrainedColName = null; + // Beamspot-position-constrained refit of ntrackVertexCandidatesColName, index-aligned with + // it (see ReconParticleDriver#findCascadeVertices / NTrackVertexer#fitVertexBeamspotConstrained). + // Null/off by default, matching ntrackVertexCandidatesColName's own opt-in convention. + private String ntrackVertexCandidatesBeamspotConstrainedColName = null; + // Refit of ntrackVertexCandidatesColName with both the beamspot-position and + // beam-momentum constraints applied together, index-aligned with it. Null/off by default. + private String ntrackVertexCandidatesBothConstrainedColName = null; + private String mcParticlesColName = null; + private String trackToMCParticleRelationsColName = null; + private String tupleFile = null; + private PrintWriter tupleWriter = null; + + public void setCascadeVertexCandidatesColName(String cascadeVertexCandidatesColName) { + this.cascadeVertexCandidatesColName = cascadeVertexCandidatesColName; + } + + public void setCascadeVertexCandidatesBeamConstrainedColName(String cascadeVertexCandidatesBeamConstrainedColName) { + this.cascadeVertexCandidatesBeamConstrainedColName = cascadeVertexCandidatesBeamConstrainedColName; + } + + public void setCascadeVertexCandidatesBeamspotConstrainedColName(String cascadeVertexCandidatesBeamspotConstrainedColName) { + this.cascadeVertexCandidatesBeamspotConstrainedColName = cascadeVertexCandidatesBeamspotConstrainedColName; + } + + public void setCascadeVertexCandidatesBothConstrainedColName(String cascadeVertexCandidatesBothConstrainedColName) { + this.cascadeVertexCandidatesBothConstrainedColName = cascadeVertexCandidatesBothConstrainedColName; + } + + public void setNtrackVertexCandidatesColName(String ntrackVertexCandidatesColName) { + this.ntrackVertexCandidatesColName = ntrackVertexCandidatesColName; + } + + public void setNtrackVertexCandidatesBeamConstrainedColName(String ntrackVertexCandidatesBeamConstrainedColName) { + this.ntrackVertexCandidatesBeamConstrainedColName = ntrackVertexCandidatesBeamConstrainedColName; + } + + public void setNtrackVertexCandidatesBeamspotConstrainedColName(String ntrackVertexCandidatesBeamspotConstrainedColName) { + this.ntrackVertexCandidatesBeamspotConstrainedColName = ntrackVertexCandidatesBeamspotConstrainedColName; + } + + public void setNtrackVertexCandidatesBothConstrainedColName(String ntrackVertexCandidatesBothConstrainedColName) { + this.ntrackVertexCandidatesBothConstrainedColName = ntrackVertexCandidatesBothConstrainedColName; + } + + public void setMcParticlesColName(String mcParticlesColName) { + this.mcParticlesColName = mcParticlesColName; + } + + public void setTrackToMCParticleRelationsColName(String trackToMCParticleRelationsColName) { + this.trackToMCParticleRelationsColName = trackToMCParticleRelationsColName; + } + + public void setTupleFile(String tupleFile) { + this.tupleFile = tupleFile; + } + + @Override + protected void detectorChanged(Detector detector) { + if (tupleFile == null) { + return; + } + try { + tupleWriter = new PrintWriter(tupleFile); + } catch (FileNotFoundException e) { + throw new RuntimeException("Could not open cascade vertex tuple file " + tupleFile, e); + } + tupleWriter.println(String.join(":", VARIABLES)); + } + + @Override + public void process(EventHeader event) { + if (tupleWriter == null || !event.hasCollection(ReconstructedParticle.class, cascadeVertexCandidatesColName)) { + return; + } + + Map trackToMC = new HashMap(); + Map trackPurity = new HashMap(); + if (trackToMCParticleRelationsColName != null + && event.hasCollection(LCRelation.class, trackToMCParticleRelationsColName)) { + for (LCRelation rel : event.get(LCRelation.class, trackToMCParticleRelationsColName)) { + trackToMC.put((Track) rel.getFrom(), (MCParticle) rel.getTo()); + trackPurity.put((Track) rel.getFrom(), (double) rel.getWeight()); + } + } + + MCParticle apMC = null; + MCParticle eleMC = null; + MCParticle posMC = null; + MCParticle recoilMC = null; + // For trident MC (no PDGID-622 A' present -- see below), the two truth electrons + // are indistinguishable at truth level; which one plays "ele" (paired with posMC to + // form the V0) vs "recoil" is only decidable per-candidate, once we know which + // reconstructed track a given cascade candidate assigned to v0EleDaughter. These two + // hold the pair of truth electrons pending that per-candidate disambiguation below. + MCParticle tridentEle1MC = null; + MCParticle tridentEle2MC = null; + if (mcParticlesColName != null && event.hasCollection(MCParticle.class, mcParticlesColName)) { + List mcParticles = event.get(MCParticle.class, mcParticlesColName); + for (MCParticle mcp : mcParticles) { + if (mcp.getPDGID() == 622 && mcp.getDaughters().size() == 2) { + apMC = mcp; + for (MCParticle daughter : mcp.getDaughters()) { + if (daughter.getPDGID() == 11) { + eleMC = daughter; + } else if (daughter.getPDGID() == -11) { + posMC = daughter; + } + } + break; + } + } + // Recoil electron convention (verified against real ap_pulser MC truth): + // the A' (622) is its own top-level record with no parent, and the recoil + // electron is the single PDGID-11 daughter of a separate top-level PDGID + // 623 "reaction" particle -- the two top-level records are not linked to + // each other, so the recoil cannot be found via the A''s parent chain. + if (apMC != null) { + for (MCParticle mcp : mcParticles) { + if (mcp.getPDGID() == 623) { + for (MCParticle daughter : mcp.getDaughters()) { + if (daughter.getPDGID() == 11) { + recoilMC = daughter; + break; + } + } + break; + } + } + } else { + // Trident convention (verified against tritrig_pulser MC truth, same scheme + // already used by NTrackVertexComparisonTupleDriver): with no PDGID-622 A' + // present, PDGID 623 is instead the trident "reaction" pseudo-particle itself, + // with 3 direct daughters (2 e- + 1 e+) from a single common production + // vertex -- architecturally different from the A' sample's 623 (a separate, + // unrelated single-daughter recoil-electron record). + for (MCParticle mcp : mcParticles) { + if (mcp.getPDGID() == 623) { + List daughters = mcp.getDaughters(); + if (daughters.size() == 3) { + MCParticle e1 = null; + MCParticle e2 = null; + MCParticle p1 = null; + for (MCParticle daughter : daughters) { + if (daughter.getPDGID() == 11) { + if (e1 == null) { + e1 = daughter; + } else { + e2 = daughter; + } + } else if (daughter.getPDGID() == -11) { + p1 = daughter; + } + } + if (e1 != null && e2 != null && p1 != null) { + tridentEle1MC = e1; + tridentEle2MC = e2; + posMC = p1; + } + } + break; + } + } + } + } + + List cascadeCandidates = event.get(ReconstructedParticle.class, cascadeVertexCandidatesColName); + List bcCandidates = null; + if (cascadeVertexCandidatesBeamConstrainedColName != null + && event.hasCollection(ReconstructedParticle.class, cascadeVertexCandidatesBeamConstrainedColName)) { + bcCandidates = event.get(ReconstructedParticle.class, cascadeVertexCandidatesBeamConstrainedColName); + } + List bsCandidates = null; + if (cascadeVertexCandidatesBeamspotConstrainedColName != null + && event.hasCollection(ReconstructedParticle.class, cascadeVertexCandidatesBeamspotConstrainedColName)) { + bsCandidates = event.get(ReconstructedParticle.class, cascadeVertexCandidatesBeamspotConstrainedColName); + } + List bothCandidates = null; + if (cascadeVertexCandidatesBothConstrainedColName != null + && event.hasCollection(ReconstructedParticle.class, cascadeVertexCandidatesBothConstrainedColName)) { + bothCandidates = event.get(ReconstructedParticle.class, cascadeVertexCandidatesBothConstrainedColName); + } + List ntrackCandidates = null; + if (ntrackVertexCandidatesColName != null + && event.hasCollection(Vertex.class, ntrackVertexCandidatesColName)) { + ntrackCandidates = event.get(Vertex.class, ntrackVertexCandidatesColName); + } + List ntrackBcCandidates = null; + if (ntrackVertexCandidatesBeamConstrainedColName != null + && event.hasCollection(Vertex.class, ntrackVertexCandidatesBeamConstrainedColName)) { + ntrackBcCandidates = event.get(Vertex.class, ntrackVertexCandidatesBeamConstrainedColName); + } + List ntrackBscCandidates = null; + if (ntrackVertexCandidatesBeamspotConstrainedColName != null + && event.hasCollection(Vertex.class, ntrackVertexCandidatesBeamspotConstrainedColName)) { + ntrackBscCandidates = event.get(Vertex.class, ntrackVertexCandidatesBeamspotConstrainedColName); + } + List ntrackBothCandidates = null; + if (ntrackVertexCandidatesBothConstrainedColName != null + && event.hasCollection(Vertex.class, ntrackVertexCandidatesBothConstrainedColName)) { + ntrackBothCandidates = event.get(Vertex.class, ntrackVertexCandidatesBothConstrainedColName); + } + for (int candidateIndex = 0; candidateIndex < cascadeCandidates.size(); candidateIndex++) { + ReconstructedParticle cascade = cascadeCandidates.get(candidateIndex); + BilliorVertex cascadeVtx = (BilliorVertex) cascade.getStartVertex(); + ReconstructedParticle v0Particle = cascade.getParticles().get(0); + ReconstructedParticle recoilElectron = cascade.getParticles().get(1); + BilliorVertex v0Vtx = (BilliorVertex) v0Particle.getStartVertex(); + + Hep3Vector cascadePos = cascadeVtx.getPosition(); + Hep3Vector pV0 = cascadeVtx.getFittedMomentum(0); + Hep3Vector pRecoil = cascadeVtx.getFittedMomentum(1); + Hep3Vector pV0Unc = v0Particle.getMomentum(); + Hep3Vector pRecoilUnc = recoilElectron.getMomentum(); + Double ndf = cascadeVtx.getCustomParameters().get("ndf"); + Hep3Vector v0Pos = v0Vtx.getPosition(); + // Per-daughter fitted momenta/errors from the two vertex fits (v0Vtx: e-/e+, + // index 0/1 by construction -- see CascadeVertexer.fit's charge-sorted + // eleDaughter/posDaughter; cascadeVtx: V0(combined)/recoil, index 0/1). Errors + // are null (-> NaN -> -9999 sentinel) unless the producing vertexer populated + // the corresponding track-momentum covariance. + Hep3Vector eleFitP = v0Vtx.getFittedMomentum(0); + Hep3Vector posFitP = v0Vtx.getFittedMomentum(1); + Hep3Vector eleFitPErr = v0Vtx.getFittedMomentumError(0); + Hep3Vector posFitPErr = v0Vtx.getFittedMomentumError(1); + Hep3Vector v0PErr = cascadeVtx.getFittedMomentumError(0); + Hep3Vector recoilPErr = cascadeVtx.getFittedMomentumError(1); + + List v0Daughters = v0Particle.getParticles(); + ReconstructedParticle v0EleDaughter = v0Daughters.get(0).getCharge() < 0 ? v0Daughters.get(0) : v0Daughters.get(1); + ReconstructedParticle v0PosDaughter = v0Daughters.get(0).getCharge() < 0 ? v0Daughters.get(1) : v0Daughters.get(0); + // Trident truth-electron disambiguation: with no PDGID-622 A' present, the two + // truth electrons (tridentEle1MC/tridentEle2MC) are only distinguishable as + // "ele" (paired with posMC to form the V0) vs "recoil" via which reconstructed + // track this candidate assigned to v0EleDaughter -- whichever truth electron that + // track is matched to (via the real LCRelation-based trackToMC map, not an + // arbitrary energy-ranking) becomes eleMC for this candidate; the other becomes + // recoilMC. Left null (unmatched) if v0EleDaughter's track matches neither. + if (apMC == null && tridentEle1MC != null && tridentEle2MC != null) { + MCParticle v0EleTruth = trackToMC.get(v0EleDaughter.getTracks().get(0)); + if (v0EleTruth == tridentEle1MC) { + eleMC = tridentEle1MC; + recoilMC = tridentEle2MC; + } else if (v0EleTruth == tridentEle2MC) { + eleMC = tridentEle2MC; + recoilMC = tridentEle1MC; + } else { + eleMC = null; + recoilMC = null; + } + } + Hep3Vector eleMomMC = eleMC != null ? eleMC.getMomentum() : null; + Hep3Vector posMomMC = posMC != null ? posMC.getMomentum() : null; + Hep3Vector recoilMomMC = recoilMC != null ? recoilMC.getMomentum() : null; + // Raw, pre-vertex-fit daughter momenta and their invariant mass (mirrors the + // existing v0Unc/recoilUnc "raw reco" convention above, but per-daughter and + // as a mass rather than just a momentum sum). Computed directly from the raw + // momenta under the electron-mass assumption (same ELECTRON_MASS value used in + // CascadeVertexer/NTrackVertexer/TridentAnalysis) rather than + // via ReconstructedParticle.asFourVector(), since that relies on whatever mass the + // upstream particle-builder happened to assign each daughter. + Hep3Vector eleUncP = v0EleDaughter.getMomentum(); + Hep3Vector posUncP = v0PosDaughter.getMomentum(); + double eEleUnc = Math.sqrt(eleUncP.magnitudeSquared() + ELECTRON_MASS * ELECTRON_MASS); + double ePosUnc = Math.sqrt(posUncP.magnitudeSquared() + ELECTRON_MASS * ELECTRON_MASS); + double v0UncPxSum = eleUncP.x() + posUncP.x(); + double v0UncPySum = eleUncP.y() + posUncP.y(); + double v0UncPzSum = eleUncP.z() + posUncP.z(); + double v0UncMass = Math.sqrt(Math.max(0.0, (eEleUnc + ePosUnc) * (eEleUnc + ePosUnc) + - v0UncPxSum * v0UncPxSum - v0UncPySum * v0UncPySum - v0UncPzSum * v0UncPzSum)); + boolean eleMatched = eleMC != null && eleMC.equals(trackToMC.get(v0EleDaughter.getTracks().get(0))); + boolean posMatched = posMC != null && posMC.equals(trackToMC.get(v0PosDaughter.getTracks().get(0))); + boolean recoilMatched = recoilMC != null && recoilMC.equals(trackToMC.get(recoilElectron.getTracks().get(0))); + // apMC.getEndPoint() is unreliable for this generator sample: BaseMCParticle's + // endpoint field defaults to (0,0,0) and is only ever set by an external writer + // (e.g. Geant4 propagation), which doesn't apply to a generator-level, promptly- + // decaying A'. Use a daughter's own production point instead -- daughters are + // produced exactly at the parent's decay vertex (same convention already used in + // APrimeMCAnalysisDriver). + Hep3Vector apVtxMC = eleMC != null ? eleMC.getOrigin() : (posMC != null ? posMC.getOrigin() : null); + // apMC.getOrigin() is ALSO contaminated for this sample: the .dat/LHE-to-stdhep + // converters (DatFileToStdhepTVM/DatFileToStdhep/ConvertToStdhep) apply the + // decay-length vertex shift to the A' record itself, not just its daughters, so + // apMC.getOrigin() actually returns the decay vertex too. The recoil electron's + // own vertex is never touched by that shift and still holds the true, unshifted + // production point -- same convention already used in APrimeMCAnalysisDriver + // (recoilMC.getOrigin() as the interaction/production point). For trident (no + // A'), all 3 daughters share one common production vertex and getOrigin() is + // never contaminated by that converter, so apVtxMC/apOriginMC come out identical + // -- expected, not a bug. + Hep3Vector apOriginMC = recoilMC != null ? recoilMC.getOrigin() : null; + + Map row = new HashMap(); + row.put("run/I", (double) event.getRunNumber()); + row.put("event/I", (double) event.getEventNumber()); + row.put("cascadeVtxX/D", cascadePos.x()); + row.put("cascadeVtxY/D", cascadePos.y()); + row.put("cascadeVtxZ/D", cascadePos.z()); + row.put("cascadeVtxXErr/D", Math.sqrt(Math.abs(cascadeVtx.getCovMatrix().e(0, 0)))); + row.put("cascadeVtxYErr/D", Math.sqrt(Math.abs(cascadeVtx.getCovMatrix().e(1, 1)))); + row.put("cascadeVtxZErr/D", Math.sqrt(Math.abs(cascadeVtx.getCovMatrix().e(2, 2)))); + row.put("cascadeChi2/D", cascadeVtx.getChi2()); + row.put("cascadeNdf/I", ndf != null ? ndf : -9999.0); + row.put("cascadeMass/D", cascadeVtx.getInvMass()); + row.put("v0PX/D", pV0.x()); + row.put("v0PY/D", pV0.y()); + row.put("v0PZ/D", pV0.z()); + row.put("v0PXErr/D", v0PErr != null ? v0PErr.x() : Double.NaN); + row.put("v0PYErr/D", v0PErr != null ? v0PErr.y() : Double.NaN); + row.put("v0PZErr/D", v0PErr != null ? v0PErr.z() : Double.NaN); + row.put("v0UncPX/D", pV0Unc.x()); + row.put("v0UncPY/D", pV0Unc.y()); + row.put("v0UncPZ/D", pV0Unc.z()); + row.put("v0UncMass/D", v0UncMass); + row.put("recoilPX/D", pRecoil.x()); + row.put("recoilPY/D", pRecoil.y()); + row.put("recoilPZ/D", pRecoil.z()); + row.put("recoilPXErr/D", recoilPErr != null ? recoilPErr.x() : Double.NaN); + row.put("recoilPYErr/D", recoilPErr != null ? recoilPErr.y() : Double.NaN); + row.put("recoilPZErr/D", recoilPErr != null ? recoilPErr.z() : Double.NaN); + row.put("recoilUncPX/D", pRecoilUnc.x()); + row.put("recoilUncPY/D", pRecoilUnc.y()); + row.put("recoilUncPZ/D", pRecoilUnc.z()); + row.put("eleFitPX/D", eleFitP != null ? eleFitP.x() : Double.NaN); + row.put("eleFitPY/D", eleFitP != null ? eleFitP.y() : Double.NaN); + row.put("eleFitPZ/D", eleFitP != null ? eleFitP.z() : Double.NaN); + row.put("eleFitPXErr/D", eleFitPErr != null ? eleFitPErr.x() : Double.NaN); + row.put("eleFitPYErr/D", eleFitPErr != null ? eleFitPErr.y() : Double.NaN); + row.put("eleFitPZErr/D", eleFitPErr != null ? eleFitPErr.z() : Double.NaN); + row.put("posFitPX/D", posFitP != null ? posFitP.x() : Double.NaN); + row.put("posFitPY/D", posFitP != null ? posFitP.y() : Double.NaN); + row.put("posFitPZ/D", posFitP != null ? posFitP.z() : Double.NaN); + row.put("posFitPXErr/D", posFitPErr != null ? posFitPErr.x() : Double.NaN); + row.put("posFitPYErr/D", posFitPErr != null ? posFitPErr.y() : Double.NaN); + row.put("posFitPZErr/D", posFitPErr != null ? posFitPErr.z() : Double.NaN); + row.put("eleUncPX/D", eleUncP.x()); + row.put("eleUncPY/D", eleUncP.y()); + row.put("eleUncPZ/D", eleUncP.z()); + row.put("posUncPX/D", posUncP.x()); + row.put("posUncPY/D", posUncP.y()); + row.put("posUncPZ/D", posUncP.z()); + row.put("v0VtxX/D", v0Pos.x()); + row.put("v0VtxY/D", v0Pos.y()); + row.put("v0VtxZ/D", v0Pos.z()); + row.put("v0VtxXErr/D", Math.sqrt(Math.abs(v0Vtx.getCovMatrix().e(0, 0)))); + row.put("v0VtxYErr/D", Math.sqrt(Math.abs(v0Vtx.getCovMatrix().e(1, 1)))); + row.put("v0VtxZErr/D", Math.sqrt(Math.abs(v0Vtx.getCovMatrix().e(2, 2)))); + row.put("v0Mass/D", v0Vtx.getInvMass()); + row.put("v0Chi2/D", v0Vtx.getChi2()); + Map cascadeParams = cascadeVtx.getCustomParameters(); + row.put("v0InputVtxX/D", cascadeParams.get("v0InputVtxX")); + row.put("v0InputVtxY/D", cascadeParams.get("v0InputVtxY")); + row.put("v0InputVtxZ/D", cascadeParams.get("v0InputVtxZ")); + row.put("v0InputVtxXErr/D", cascadeParams.get("v0InputVtxXErr")); + row.put("v0InputVtxYErr/D", cascadeParams.get("v0InputVtxYErr")); + row.put("v0InputVtxZErr/D", cascadeParams.get("v0InputVtxZErr")); + row.put("v0InputMass/D", cascadeParams.get("v0InputMass")); + row.put("v0InputChi2/D", cascadeParams.get("v0InputChi2")); + row.put("v0ProjX/D", cascadeParams.get("v0ProjX")); + row.put("v0ProjY/D", cascadeParams.get("v0ProjY")); + row.put("v0ProjXErr/D", cascadeParams.get("v0ProjXErr")); + row.put("v0ProjYErr/D", cascadeParams.get("v0ProjYErr")); + row.put("recoilProjX/D", cascadeParams.get("recoilProjX")); + row.put("recoilProjY/D", cascadeParams.get("recoilProjY")); + row.put("recoilProjXErr/D", cascadeParams.get("recoilProjXErr")); + row.put("recoilProjYErr/D", cascadeParams.get("recoilProjYErr")); + row.put("apMassMC/D", apMC != null ? apMC.getMass() : Double.NaN); + row.put("apVtxXMC/D", apVtxMC != null ? apVtxMC.x() : Double.NaN); + row.put("apVtxYMC/D", apVtxMC != null ? apVtxMC.y() : Double.NaN); + row.put("apVtxZMC/D", apVtxMC != null ? apVtxMC.z() : Double.NaN); + row.put("apOriginXMC/D", apOriginMC != null ? apOriginMC.x() : Double.NaN); + row.put("apOriginYMC/D", apOriginMC != null ? apOriginMC.y() : Double.NaN); + row.put("apOriginZMC/D", apOriginMC != null ? apOriginMC.z() : Double.NaN); + row.put("eleMomXMC/D", eleMomMC != null ? eleMomMC.x() : Double.NaN); + row.put("eleMomYMC/D", eleMomMC != null ? eleMomMC.y() : Double.NaN); + row.put("eleMomZMC/D", eleMomMC != null ? eleMomMC.z() : Double.NaN); + row.put("posMomXMC/D", posMomMC != null ? posMomMC.x() : Double.NaN); + row.put("posMomYMC/D", posMomMC != null ? posMomMC.y() : Double.NaN); + row.put("posMomZMC/D", posMomMC != null ? posMomMC.z() : Double.NaN); + row.put("recoilMomXMC/D", recoilMomMC != null ? recoilMomMC.x() : Double.NaN); + row.put("recoilMomYMC/D", recoilMomMC != null ? recoilMomMC.y() : Double.NaN); + row.put("recoilMomZMC/D", recoilMomMC != null ? recoilMomMC.z() : Double.NaN); + row.put("v0EleTruthMatched/I", eleMatched ? 1.0 : 0.0); + row.put("v0PosTruthMatched/I", posMatched ? 1.0 : 0.0); + row.put("v0BothTruthMatchedToAp/I", eleMatched && posMatched ? 1.0 : 0.0); + row.put("recoilTruthMatched/I", recoilMatched ? 1.0 : 0.0); + row.put("v0ElePurity/D", trackPurity.get(v0EleDaughter.getTracks().get(0))); + row.put("v0PosPurity/D", trackPurity.get(v0PosDaughter.getTracks().get(0))); + row.put("recoilPurity/D", trackPurity.get(recoilElectron.getTracks().get(0))); + + if (bcCandidates != null) { + ReconstructedParticle bcCascade = bcCandidates.get(candidateIndex); + BilliorVertex bcVtxCheck = (BilliorVertex) bcCascade.getStartVertex(); + Double bcNdfCheck = bcVtxCheck.getCustomParameters().get("ndf"); + boolean isPlaceholder = bcNdfCheck == null || bcNdfCheck == -9999.0; + if (!isPlaceholder) { + BilliorVertex bcVtx = bcVtxCheck; + ReconstructedParticle bcV0Particle = bcCascade.getParticles().get(0); + BilliorVertex bcV0Vtx = (BilliorVertex) bcV0Particle.getStartVertex(); + Hep3Vector bcPos = bcVtx.getPosition(); + Hep3Vector bcV0P = bcVtx.getFittedMomentum(0); + Hep3Vector bcRecoilP = bcVtx.getFittedMomentum(1); + Hep3Vector bcV0PErr = bcVtx.getFittedMomentumError(0); + Hep3Vector bcRecoilPErr = bcVtx.getFittedMomentumError(1); + Hep3Vector bcEleFitP = bcV0Vtx.getFittedMomentum(0); + Hep3Vector bcPosFitP = bcV0Vtx.getFittedMomentum(1); + Hep3Vector bcEleFitPErr = bcV0Vtx.getFittedMomentumError(0); + Hep3Vector bcPosFitPErr = bcV0Vtx.getFittedMomentumError(1); + Double bcNdf = bcVtx.getCustomParameters().get("ndf"); + Hep3Vector bcV0Pos = bcV0Vtx.getPosition(); + row.put("cascadeBeamMomConstrainedVtxX/D", bcPos.x()); + row.put("cascadeBeamMomConstrainedVtxY/D", bcPos.y()); + row.put("cascadeBeamMomConstrainedVtxZ/D", bcPos.z()); + row.put("cascadeBeamMomConstrainedVtxXErr/D", Math.sqrt(Math.abs(bcVtx.getCovMatrix().e(0, 0)))); + row.put("cascadeBeamMomConstrainedVtxYErr/D", Math.sqrt(Math.abs(bcVtx.getCovMatrix().e(1, 1)))); + row.put("cascadeBeamMomConstrainedVtxZErr/D", Math.sqrt(Math.abs(bcVtx.getCovMatrix().e(2, 2)))); + row.put("cascadeBeamMomConstrainedChi2/D", bcVtx.getChi2()); + row.put("cascadeBeamMomConstrainedNdf/I", bcNdf != null ? bcNdf : -9999.0); + row.put("cascadeBeamMomConstrainedMass/D", bcVtx.getInvMass()); + row.put("cascadeBeamMomConstrainedV0PX/D", bcV0P.x()); + row.put("cascadeBeamMomConstrainedV0PY/D", bcV0P.y()); + row.put("cascadeBeamMomConstrainedV0PZ/D", bcV0P.z()); + row.put("cascadeBeamMomConstrainedV0PXErr/D", bcV0PErr != null ? bcV0PErr.x() : Double.NaN); + row.put("cascadeBeamMomConstrainedV0PYErr/D", bcV0PErr != null ? bcV0PErr.y() : Double.NaN); + row.put("cascadeBeamMomConstrainedV0PZErr/D", bcV0PErr != null ? bcV0PErr.z() : Double.NaN); + row.put("cascadeBeamMomConstrainedRecoilPX/D", bcRecoilP.x()); + row.put("cascadeBeamMomConstrainedRecoilPY/D", bcRecoilP.y()); + row.put("cascadeBeamMomConstrainedRecoilPZ/D", bcRecoilP.z()); + row.put("cascadeBeamMomConstrainedRecoilPXErr/D", bcRecoilPErr != null ? bcRecoilPErr.x() : Double.NaN); + row.put("cascadeBeamMomConstrainedRecoilPYErr/D", bcRecoilPErr != null ? bcRecoilPErr.y() : Double.NaN); + row.put("cascadeBeamMomConstrainedRecoilPZErr/D", bcRecoilPErr != null ? bcRecoilPErr.z() : Double.NaN); + row.put("cascadeBeamMomConstrainedElePX/D", bcEleFitP != null ? bcEleFitP.x() : Double.NaN); + row.put("cascadeBeamMomConstrainedElePY/D", bcEleFitP != null ? bcEleFitP.y() : Double.NaN); + row.put("cascadeBeamMomConstrainedElePZ/D", bcEleFitP != null ? bcEleFitP.z() : Double.NaN); + row.put("cascadeBeamMomConstrainedElePXErr/D", bcEleFitPErr != null ? bcEleFitPErr.x() : Double.NaN); + row.put("cascadeBeamMomConstrainedElePYErr/D", bcEleFitPErr != null ? bcEleFitPErr.y() : Double.NaN); + row.put("cascadeBeamMomConstrainedElePZErr/D", bcEleFitPErr != null ? bcEleFitPErr.z() : Double.NaN); + row.put("cascadeBeamMomConstrainedPosPX/D", bcPosFitP != null ? bcPosFitP.x() : Double.NaN); + row.put("cascadeBeamMomConstrainedPosPY/D", bcPosFitP != null ? bcPosFitP.y() : Double.NaN); + row.put("cascadeBeamMomConstrainedPosPZ/D", bcPosFitP != null ? bcPosFitP.z() : Double.NaN); + row.put("cascadeBeamMomConstrainedPosPXErr/D", bcPosFitPErr != null ? bcPosFitPErr.x() : Double.NaN); + row.put("cascadeBeamMomConstrainedPosPYErr/D", bcPosFitPErr != null ? bcPosFitPErr.y() : Double.NaN); + row.put("cascadeBeamMomConstrainedPosPZErr/D", bcPosFitPErr != null ? bcPosFitPErr.z() : Double.NaN); + row.put("cascadeBeamMomConstrainedV0VtxX/D", bcV0Pos.x()); + row.put("cascadeBeamMomConstrainedV0VtxY/D", bcV0Pos.y()); + row.put("cascadeBeamMomConstrainedV0VtxZ/D", bcV0Pos.z()); + row.put("cascadeBeamMomConstrainedV0VtxXErr/D", Math.sqrt(Math.abs(bcV0Vtx.getCovMatrix().e(0, 0)))); + row.put("cascadeBeamMomConstrainedV0VtxYErr/D", Math.sqrt(Math.abs(bcV0Vtx.getCovMatrix().e(1, 1)))); + row.put("cascadeBeamMomConstrainedV0VtxZErr/D", Math.sqrt(Math.abs(bcV0Vtx.getCovMatrix().e(2, 2)))); + row.put("cascadeBeamMomConstrainedV0Mass/D", bcV0Vtx.getInvMass()); + row.put("cascadeBeamMomConstrainedV0Chi2/D", bcV0Vtx.getChi2()); + } + } + + if (bsCandidates != null) { + ReconstructedParticle bsCascade = bsCandidates.get(candidateIndex); + BilliorVertex bsVtxCheck = (BilliorVertex) bsCascade.getStartVertex(); + Double bsNdfCheck = bsVtxCheck.getCustomParameters().get("ndf"); + boolean isPlaceholder = bsNdfCheck == null || bsNdfCheck == -9999.0; + if (!isPlaceholder) { + BilliorVertex bsVtx = bsVtxCheck; + ReconstructedParticle bsV0Particle = bsCascade.getParticles().get(0); + BilliorVertex bsV0Vtx = (BilliorVertex) bsV0Particle.getStartVertex(); + Hep3Vector bsPos = bsVtx.getPosition(); + Hep3Vector bsV0P = bsVtx.getFittedMomentum(0); + Hep3Vector bsRecoilP = bsVtx.getFittedMomentum(1); + Hep3Vector bsV0PErr = bsVtx.getFittedMomentumError(0); + Hep3Vector bsRecoilPErr = bsVtx.getFittedMomentumError(1); + Hep3Vector bsEleFitP = bsV0Vtx.getFittedMomentum(0); + Hep3Vector bsPosFitP = bsV0Vtx.getFittedMomentum(1); + Hep3Vector bsEleFitPErr = bsV0Vtx.getFittedMomentumError(0); + Hep3Vector bsPosFitPErr = bsV0Vtx.getFittedMomentumError(1); + Double bsNdf = bsVtx.getCustomParameters().get("ndf"); + Hep3Vector bsV0Pos = bsV0Vtx.getPosition(); + row.put("cascadeBeamspotConstrainedVtxX/D", bsPos.x()); + row.put("cascadeBeamspotConstrainedVtxY/D", bsPos.y()); + row.put("cascadeBeamspotConstrainedVtxZ/D", bsPos.z()); + row.put("cascadeBeamspotConstrainedVtxXErr/D", Math.sqrt(Math.abs(bsVtx.getCovMatrix().e(0, 0)))); + row.put("cascadeBeamspotConstrainedVtxYErr/D", Math.sqrt(Math.abs(bsVtx.getCovMatrix().e(1, 1)))); + row.put("cascadeBeamspotConstrainedVtxZErr/D", Math.sqrt(Math.abs(bsVtx.getCovMatrix().e(2, 2)))); + row.put("cascadeBeamspotConstrainedChi2/D", bsVtx.getChi2()); + row.put("cascadeBeamspotConstrainedNdf/I", bsNdf != null ? bsNdf : -9999.0); + row.put("cascadeBeamspotConstrainedMass/D", bsVtx.getInvMass()); + row.put("cascadeBeamspotConstrainedV0PX/D", bsV0P.x()); + row.put("cascadeBeamspotConstrainedV0PY/D", bsV0P.y()); + row.put("cascadeBeamspotConstrainedV0PZ/D", bsV0P.z()); + row.put("cascadeBeamspotConstrainedV0PXErr/D", bsV0PErr != null ? bsV0PErr.x() : Double.NaN); + row.put("cascadeBeamspotConstrainedV0PYErr/D", bsV0PErr != null ? bsV0PErr.y() : Double.NaN); + row.put("cascadeBeamspotConstrainedV0PZErr/D", bsV0PErr != null ? bsV0PErr.z() : Double.NaN); + row.put("cascadeBeamspotConstrainedRecoilPX/D", bsRecoilP.x()); + row.put("cascadeBeamspotConstrainedRecoilPY/D", bsRecoilP.y()); + row.put("cascadeBeamspotConstrainedRecoilPZ/D", bsRecoilP.z()); + row.put("cascadeBeamspotConstrainedRecoilPXErr/D", bsRecoilPErr != null ? bsRecoilPErr.x() : Double.NaN); + row.put("cascadeBeamspotConstrainedRecoilPYErr/D", bsRecoilPErr != null ? bsRecoilPErr.y() : Double.NaN); + row.put("cascadeBeamspotConstrainedRecoilPZErr/D", bsRecoilPErr != null ? bsRecoilPErr.z() : Double.NaN); + row.put("cascadeBeamspotConstrainedElePX/D", bsEleFitP != null ? bsEleFitP.x() : Double.NaN); + row.put("cascadeBeamspotConstrainedElePY/D", bsEleFitP != null ? bsEleFitP.y() : Double.NaN); + row.put("cascadeBeamspotConstrainedElePZ/D", bsEleFitP != null ? bsEleFitP.z() : Double.NaN); + row.put("cascadeBeamspotConstrainedElePXErr/D", bsEleFitPErr != null ? bsEleFitPErr.x() : Double.NaN); + row.put("cascadeBeamspotConstrainedElePYErr/D", bsEleFitPErr != null ? bsEleFitPErr.y() : Double.NaN); + row.put("cascadeBeamspotConstrainedElePZErr/D", bsEleFitPErr != null ? bsEleFitPErr.z() : Double.NaN); + row.put("cascadeBeamspotConstrainedPosPX/D", bsPosFitP != null ? bsPosFitP.x() : Double.NaN); + row.put("cascadeBeamspotConstrainedPosPY/D", bsPosFitP != null ? bsPosFitP.y() : Double.NaN); + row.put("cascadeBeamspotConstrainedPosPZ/D", bsPosFitP != null ? bsPosFitP.z() : Double.NaN); + row.put("cascadeBeamspotConstrainedPosPXErr/D", bsPosFitPErr != null ? bsPosFitPErr.x() : Double.NaN); + row.put("cascadeBeamspotConstrainedPosPYErr/D", bsPosFitPErr != null ? bsPosFitPErr.y() : Double.NaN); + row.put("cascadeBeamspotConstrainedPosPZErr/D", bsPosFitPErr != null ? bsPosFitPErr.z() : Double.NaN); + row.put("cascadeBeamspotConstrainedV0VtxX/D", bsV0Pos.x()); + row.put("cascadeBeamspotConstrainedV0VtxY/D", bsV0Pos.y()); + row.put("cascadeBeamspotConstrainedV0VtxZ/D", bsV0Pos.z()); + row.put("cascadeBeamspotConstrainedV0VtxXErr/D", Math.sqrt(Math.abs(bsV0Vtx.getCovMatrix().e(0, 0)))); + row.put("cascadeBeamspotConstrainedV0VtxYErr/D", Math.sqrt(Math.abs(bsV0Vtx.getCovMatrix().e(1, 1)))); + row.put("cascadeBeamspotConstrainedV0VtxZErr/D", Math.sqrt(Math.abs(bsV0Vtx.getCovMatrix().e(2, 2)))); + row.put("cascadeBeamspotConstrainedV0Mass/D", bsV0Vtx.getInvMass()); + row.put("cascadeBeamspotConstrainedV0Chi2/D", bsV0Vtx.getChi2()); + } + } + + if (bothCandidates != null) { + ReconstructedParticle bothCascade = bothCandidates.get(candidateIndex); + BilliorVertex bothVtxCheck = (BilliorVertex) bothCascade.getStartVertex(); + Double bothNdfCheck = bothVtxCheck.getCustomParameters().get("ndf"); + boolean isPlaceholder = bothNdfCheck == null || bothNdfCheck == -9999.0; + if (!isPlaceholder) { + BilliorVertex bothVtx = bothVtxCheck; + ReconstructedParticle bothV0Particle = bothCascade.getParticles().get(0); + BilliorVertex bothV0Vtx = (BilliorVertex) bothV0Particle.getStartVertex(); + Hep3Vector bothPos = bothVtx.getPosition(); + Hep3Vector bothV0P = bothVtx.getFittedMomentum(0); + Hep3Vector bothRecoilP = bothVtx.getFittedMomentum(1); + Hep3Vector bothV0PErr = bothVtx.getFittedMomentumError(0); + Hep3Vector bothRecoilPErr = bothVtx.getFittedMomentumError(1); + Hep3Vector bothEleFitP = bothV0Vtx.getFittedMomentum(0); + Hep3Vector bothPosFitP = bothV0Vtx.getFittedMomentum(1); + Hep3Vector bothEleFitPErr = bothV0Vtx.getFittedMomentumError(0); + Hep3Vector bothPosFitPErr = bothV0Vtx.getFittedMomentumError(1); + Double bothNdf = bothVtx.getCustomParameters().get("ndf"); + Hep3Vector bothV0Pos = bothV0Vtx.getPosition(); + row.put("cascadeBothConstrainedVtxX/D", bothPos.x()); + row.put("cascadeBothConstrainedVtxY/D", bothPos.y()); + row.put("cascadeBothConstrainedVtxZ/D", bothPos.z()); + row.put("cascadeBothConstrainedVtxXErr/D", Math.sqrt(Math.abs(bothVtx.getCovMatrix().e(0, 0)))); + row.put("cascadeBothConstrainedVtxYErr/D", Math.sqrt(Math.abs(bothVtx.getCovMatrix().e(1, 1)))); + row.put("cascadeBothConstrainedVtxZErr/D", Math.sqrt(Math.abs(bothVtx.getCovMatrix().e(2, 2)))); + row.put("cascadeBothConstrainedChi2/D", bothVtx.getChi2()); + row.put("cascadeBothConstrainedNdf/I", bothNdf != null ? bothNdf : -9999.0); + row.put("cascadeBothConstrainedMass/D", bothVtx.getInvMass()); + row.put("cascadeBothConstrainedV0PX/D", bothV0P.x()); + row.put("cascadeBothConstrainedV0PY/D", bothV0P.y()); + row.put("cascadeBothConstrainedV0PZ/D", bothV0P.z()); + row.put("cascadeBothConstrainedV0PXErr/D", bothV0PErr != null ? bothV0PErr.x() : Double.NaN); + row.put("cascadeBothConstrainedV0PYErr/D", bothV0PErr != null ? bothV0PErr.y() : Double.NaN); + row.put("cascadeBothConstrainedV0PZErr/D", bothV0PErr != null ? bothV0PErr.z() : Double.NaN); + row.put("cascadeBothConstrainedRecoilPX/D", bothRecoilP.x()); + row.put("cascadeBothConstrainedRecoilPY/D", bothRecoilP.y()); + row.put("cascadeBothConstrainedRecoilPZ/D", bothRecoilP.z()); + row.put("cascadeBothConstrainedRecoilPXErr/D", bothRecoilPErr != null ? bothRecoilPErr.x() : Double.NaN); + row.put("cascadeBothConstrainedRecoilPYErr/D", bothRecoilPErr != null ? bothRecoilPErr.y() : Double.NaN); + row.put("cascadeBothConstrainedRecoilPZErr/D", bothRecoilPErr != null ? bothRecoilPErr.z() : Double.NaN); + row.put("cascadeBothConstrainedElePX/D", bothEleFitP != null ? bothEleFitP.x() : Double.NaN); + row.put("cascadeBothConstrainedElePY/D", bothEleFitP != null ? bothEleFitP.y() : Double.NaN); + row.put("cascadeBothConstrainedElePZ/D", bothEleFitP != null ? bothEleFitP.z() : Double.NaN); + row.put("cascadeBothConstrainedElePXErr/D", bothEleFitPErr != null ? bothEleFitPErr.x() : Double.NaN); + row.put("cascadeBothConstrainedElePYErr/D", bothEleFitPErr != null ? bothEleFitPErr.y() : Double.NaN); + row.put("cascadeBothConstrainedElePZErr/D", bothEleFitPErr != null ? bothEleFitPErr.z() : Double.NaN); + row.put("cascadeBothConstrainedPosPX/D", bothPosFitP != null ? bothPosFitP.x() : Double.NaN); + row.put("cascadeBothConstrainedPosPY/D", bothPosFitP != null ? bothPosFitP.y() : Double.NaN); + row.put("cascadeBothConstrainedPosPZ/D", bothPosFitP != null ? bothPosFitP.z() : Double.NaN); + row.put("cascadeBothConstrainedPosPXErr/D", bothPosFitPErr != null ? bothPosFitPErr.x() : Double.NaN); + row.put("cascadeBothConstrainedPosPYErr/D", bothPosFitPErr != null ? bothPosFitPErr.y() : Double.NaN); + row.put("cascadeBothConstrainedPosPZErr/D", bothPosFitPErr != null ? bothPosFitPErr.z() : Double.NaN); + row.put("cascadeBothConstrainedV0VtxX/D", bothV0Pos.x()); + row.put("cascadeBothConstrainedV0VtxY/D", bothV0Pos.y()); + row.put("cascadeBothConstrainedV0VtxZ/D", bothV0Pos.z()); + row.put("cascadeBothConstrainedV0VtxXErr/D", Math.sqrt(Math.abs(bothV0Vtx.getCovMatrix().e(0, 0)))); + row.put("cascadeBothConstrainedV0VtxYErr/D", Math.sqrt(Math.abs(bothV0Vtx.getCovMatrix().e(1, 1)))); + row.put("cascadeBothConstrainedV0VtxZErr/D", Math.sqrt(Math.abs(bothV0Vtx.getCovMatrix().e(2, 2)))); + row.put("cascadeBothConstrainedV0Mass/D", bothV0Vtx.getInvMass()); + row.put("cascadeBothConstrainedV0Chi2/D", bothV0Vtx.getChi2()); + } + } + + if (ntrackCandidates != null) { + BilliorVertex ntrackVtx = (BilliorVertex) ntrackCandidates.get(candidateIndex); + Double ntrackNdfCheck = ntrackVtx.getCustomParameters().get("ndf"); + boolean ntrackIsPlaceholder = ntrackNdfCheck == null || ntrackNdfCheck == -9999.0; + if (!ntrackIsPlaceholder) { + Hep3Vector ntrackPos = ntrackVtx.getPosition(); + Hep3Vector ntrackEleP = ntrackVtx.getFittedMomentum(0); + Hep3Vector ntrackPosP = ntrackVtx.getFittedMomentum(1); + Hep3Vector ntrackRecoilP = ntrackVtx.getFittedMomentum(2); + Hep3Vector ntrackElePErr = ntrackVtx.getFittedMomentumError(0); + Hep3Vector ntrackPosPErr = ntrackVtx.getFittedMomentumError(1); + Hep3Vector ntrackRecoilPErr = ntrackVtx.getFittedMomentumError(2); + row.put("ntrackVtxX/D", ntrackPos.x()); + row.put("ntrackVtxY/D", ntrackPos.y()); + row.put("ntrackVtxZ/D", ntrackPos.z()); + row.put("ntrackVtxXErr/D", Math.sqrt(Math.abs(ntrackVtx.getCovMatrix().e(0, 0)))); + row.put("ntrackVtxYErr/D", Math.sqrt(Math.abs(ntrackVtx.getCovMatrix().e(1, 1)))); + row.put("ntrackVtxZErr/D", Math.sqrt(Math.abs(ntrackVtx.getCovMatrix().e(2, 2)))); + row.put("ntrackChi2/D", ntrackVtx.getChi2()); + row.put("ntrackNdf/I", ntrackNdfCheck); + row.put("ntrackMass/D", ntrackVtx.getInvMass()); + row.put("ntrackElePX/D", ntrackEleP.x()); + row.put("ntrackElePY/D", ntrackEleP.y()); + row.put("ntrackElePZ/D", ntrackEleP.z()); + row.put("ntrackElePXErr/D", ntrackElePErr != null ? ntrackElePErr.x() : Double.NaN); + row.put("ntrackElePYErr/D", ntrackElePErr != null ? ntrackElePErr.y() : Double.NaN); + row.put("ntrackElePZErr/D", ntrackElePErr != null ? ntrackElePErr.z() : Double.NaN); + row.put("ntrackPosPX/D", ntrackPosP.x()); + row.put("ntrackPosPY/D", ntrackPosP.y()); + row.put("ntrackPosPZ/D", ntrackPosP.z()); + row.put("ntrackPosPXErr/D", ntrackPosPErr != null ? ntrackPosPErr.x() : Double.NaN); + row.put("ntrackPosPYErr/D", ntrackPosPErr != null ? ntrackPosPErr.y() : Double.NaN); + row.put("ntrackPosPZErr/D", ntrackPosPErr != null ? ntrackPosPErr.z() : Double.NaN); + row.put("ntrackRecoilPX/D", ntrackRecoilP.x()); + row.put("ntrackRecoilPY/D", ntrackRecoilP.y()); + row.put("ntrackRecoilPZ/D", ntrackRecoilP.z()); + row.put("ntrackRecoilPXErr/D", ntrackRecoilPErr != null ? ntrackRecoilPErr.x() : Double.NaN); + row.put("ntrackRecoilPYErr/D", ntrackRecoilPErr != null ? ntrackRecoilPErr.y() : Double.NaN); + row.put("ntrackRecoilPZErr/D", ntrackRecoilPErr != null ? ntrackRecoilPErr.z() : Double.NaN); + } + } + + if (ntrackBcCandidates != null) { + BilliorVertex ntrackBcVtx = (BilliorVertex) ntrackBcCandidates.get(candidateIndex); + Double ntrackBcNdfCheck = ntrackBcVtx.getCustomParameters().get("ndf"); + boolean ntrackBcIsPlaceholder = ntrackBcNdfCheck == null || ntrackBcNdfCheck == -9999.0; + if (!ntrackBcIsPlaceholder) { + Hep3Vector ntrackBcPos = ntrackBcVtx.getPosition(); + Hep3Vector ntrackBcEleP = ntrackBcVtx.getFittedMomentum(0); + Hep3Vector ntrackBcPosP = ntrackBcVtx.getFittedMomentum(1); + Hep3Vector ntrackBcRecoilP = ntrackBcVtx.getFittedMomentum(2); + Hep3Vector ntrackBcElePErr = ntrackBcVtx.getFittedMomentumError(0); + Hep3Vector ntrackBcPosPErr = ntrackBcVtx.getFittedMomentumError(1); + Hep3Vector ntrackBcRecoilPErr = ntrackBcVtx.getFittedMomentumError(2); + row.put("ntrackBeamMomConstrainedVtxX/D", ntrackBcPos.x()); + row.put("ntrackBeamMomConstrainedVtxY/D", ntrackBcPos.y()); + row.put("ntrackBeamMomConstrainedVtxZ/D", ntrackBcPos.z()); + row.put("ntrackBeamMomConstrainedVtxXErr/D", Math.sqrt(Math.abs(ntrackBcVtx.getCovMatrix().e(0, 0)))); + row.put("ntrackBeamMomConstrainedVtxYErr/D", Math.sqrt(Math.abs(ntrackBcVtx.getCovMatrix().e(1, 1)))); + row.put("ntrackBeamMomConstrainedVtxZErr/D", Math.sqrt(Math.abs(ntrackBcVtx.getCovMatrix().e(2, 2)))); + row.put("ntrackBeamMomConstrainedChi2/D", ntrackBcVtx.getChi2()); + row.put("ntrackBeamMomConstrainedNdf/I", ntrackBcNdfCheck); + row.put("ntrackBeamMomConstrainedMass/D", ntrackBcVtx.getInvMass()); + row.put("ntrackBeamMomConstrainedElePX/D", ntrackBcEleP.x()); + row.put("ntrackBeamMomConstrainedElePY/D", ntrackBcEleP.y()); + row.put("ntrackBeamMomConstrainedElePZ/D", ntrackBcEleP.z()); + row.put("ntrackBeamMomConstrainedElePXErr/D", ntrackBcElePErr != null ? ntrackBcElePErr.x() : Double.NaN); + row.put("ntrackBeamMomConstrainedElePYErr/D", ntrackBcElePErr != null ? ntrackBcElePErr.y() : Double.NaN); + row.put("ntrackBeamMomConstrainedElePZErr/D", ntrackBcElePErr != null ? ntrackBcElePErr.z() : Double.NaN); + row.put("ntrackBeamMomConstrainedPosPX/D", ntrackBcPosP.x()); + row.put("ntrackBeamMomConstrainedPosPY/D", ntrackBcPosP.y()); + row.put("ntrackBeamMomConstrainedPosPZ/D", ntrackBcPosP.z()); + row.put("ntrackBeamMomConstrainedPosPXErr/D", ntrackBcPosPErr != null ? ntrackBcPosPErr.x() : Double.NaN); + row.put("ntrackBeamMomConstrainedPosPYErr/D", ntrackBcPosPErr != null ? ntrackBcPosPErr.y() : Double.NaN); + row.put("ntrackBeamMomConstrainedPosPZErr/D", ntrackBcPosPErr != null ? ntrackBcPosPErr.z() : Double.NaN); + row.put("ntrackBeamMomConstrainedRecoilPX/D", ntrackBcRecoilP.x()); + row.put("ntrackBeamMomConstrainedRecoilPY/D", ntrackBcRecoilP.y()); + row.put("ntrackBeamMomConstrainedRecoilPZ/D", ntrackBcRecoilP.z()); + row.put("ntrackBeamMomConstrainedRecoilPXErr/D", ntrackBcRecoilPErr != null ? ntrackBcRecoilPErr.x() : Double.NaN); + row.put("ntrackBeamMomConstrainedRecoilPYErr/D", ntrackBcRecoilPErr != null ? ntrackBcRecoilPErr.y() : Double.NaN); + row.put("ntrackBeamMomConstrainedRecoilPZErr/D", ntrackBcRecoilPErr != null ? ntrackBcRecoilPErr.z() : Double.NaN); + } + } + if (ntrackBscCandidates != null) { + BilliorVertex ntrackBscVtx = (BilliorVertex) ntrackBscCandidates.get(candidateIndex); + Double ntrackBscNdfCheck = ntrackBscVtx.getCustomParameters().get("ndf"); + if (ntrackBscNdfCheck != null && ntrackBscNdfCheck >= 0) { + Hep3Vector ntrackBscPos = ntrackBscVtx.getPosition(); + Hep3Vector ntrackBscEleP = ntrackBscVtx.getFittedMomentum(0); + Hep3Vector ntrackBscPosP = ntrackBscVtx.getFittedMomentum(1); + Hep3Vector ntrackBscRecoilP = ntrackBscVtx.getFittedMomentum(2); + Hep3Vector ntrackBscElePErr = ntrackBscVtx.getFittedMomentumError(0); + Hep3Vector ntrackBscPosPErr = ntrackBscVtx.getFittedMomentumError(1); + Hep3Vector ntrackBscRecoilPErr = ntrackBscVtx.getFittedMomentumError(2); + row.put("ntrackBeamspotConstrainedVtxX/D", ntrackBscPos.x()); + row.put("ntrackBeamspotConstrainedVtxY/D", ntrackBscPos.y()); + row.put("ntrackBeamspotConstrainedVtxZ/D", ntrackBscPos.z()); + row.put("ntrackBeamspotConstrainedVtxXErr/D", Math.sqrt(Math.abs(ntrackBscVtx.getCovMatrix().e(0, 0)))); + row.put("ntrackBeamspotConstrainedVtxYErr/D", Math.sqrt(Math.abs(ntrackBscVtx.getCovMatrix().e(1, 1)))); + row.put("ntrackBeamspotConstrainedVtxZErr/D", Math.sqrt(Math.abs(ntrackBscVtx.getCovMatrix().e(2, 2)))); + row.put("ntrackBeamspotConstrainedChi2/D", ntrackBscVtx.getChi2()); + row.put("ntrackBeamspotConstrainedNdf/I", ntrackBscNdfCheck); + row.put("ntrackBeamspotConstrainedMass/D", ntrackBscVtx.getInvMass()); + row.put("ntrackBeamspotConstrainedElePX/D", ntrackBscEleP.x()); + row.put("ntrackBeamspotConstrainedElePY/D", ntrackBscEleP.y()); + row.put("ntrackBeamspotConstrainedElePZ/D", ntrackBscEleP.z()); + row.put("ntrackBeamspotConstrainedElePXErr/D", ntrackBscElePErr != null ? ntrackBscElePErr.x() : Double.NaN); + row.put("ntrackBeamspotConstrainedElePYErr/D", ntrackBscElePErr != null ? ntrackBscElePErr.y() : Double.NaN); + row.put("ntrackBeamspotConstrainedElePZErr/D", ntrackBscElePErr != null ? ntrackBscElePErr.z() : Double.NaN); + row.put("ntrackBeamspotConstrainedPosPX/D", ntrackBscPosP.x()); + row.put("ntrackBeamspotConstrainedPosPY/D", ntrackBscPosP.y()); + row.put("ntrackBeamspotConstrainedPosPZ/D", ntrackBscPosP.z()); + row.put("ntrackBeamspotConstrainedPosPXErr/D", ntrackBscPosPErr != null ? ntrackBscPosPErr.x() : Double.NaN); + row.put("ntrackBeamspotConstrainedPosPYErr/D", ntrackBscPosPErr != null ? ntrackBscPosPErr.y() : Double.NaN); + row.put("ntrackBeamspotConstrainedPosPZErr/D", ntrackBscPosPErr != null ? ntrackBscPosPErr.z() : Double.NaN); + row.put("ntrackBeamspotConstrainedRecoilPX/D", ntrackBscRecoilP.x()); + row.put("ntrackBeamspotConstrainedRecoilPY/D", ntrackBscRecoilP.y()); + row.put("ntrackBeamspotConstrainedRecoilPZ/D", ntrackBscRecoilP.z()); + row.put("ntrackBeamspotConstrainedRecoilPXErr/D", ntrackBscRecoilPErr != null ? ntrackBscRecoilPErr.x() : Double.NaN); + row.put("ntrackBeamspotConstrainedRecoilPYErr/D", ntrackBscRecoilPErr != null ? ntrackBscRecoilPErr.y() : Double.NaN); + row.put("ntrackBeamspotConstrainedRecoilPZErr/D", ntrackBscRecoilPErr != null ? ntrackBscRecoilPErr.z() : Double.NaN); + } + } + + if (ntrackBothCandidates != null) { + BilliorVertex ntrackBothVtx = (BilliorVertex) ntrackBothCandidates.get(candidateIndex); + Double ntrackBothNdfCheck = ntrackBothVtx.getCustomParameters().get("ndf"); + if (ntrackBothNdfCheck != null && ntrackBothNdfCheck >= 0) { + Hep3Vector ntrackBothPos = ntrackBothVtx.getPosition(); + Hep3Vector ntrackBothEleP = ntrackBothVtx.getFittedMomentum(0); + Hep3Vector ntrackBothPosP = ntrackBothVtx.getFittedMomentum(1); + Hep3Vector ntrackBothRecoilP = ntrackBothVtx.getFittedMomentum(2); + Hep3Vector ntrackBothElePErr = ntrackBothVtx.getFittedMomentumError(0); + Hep3Vector ntrackBothPosPErr = ntrackBothVtx.getFittedMomentumError(1); + Hep3Vector ntrackBothRecoilPErr = ntrackBothVtx.getFittedMomentumError(2); + row.put("ntrackBothConstrainedVtxX/D", ntrackBothPos.x()); + row.put("ntrackBothConstrainedVtxY/D", ntrackBothPos.y()); + row.put("ntrackBothConstrainedVtxZ/D", ntrackBothPos.z()); + row.put("ntrackBothConstrainedVtxXErr/D", Math.sqrt(Math.abs(ntrackBothVtx.getCovMatrix().e(0, 0)))); + row.put("ntrackBothConstrainedVtxYErr/D", Math.sqrt(Math.abs(ntrackBothVtx.getCovMatrix().e(1, 1)))); + row.put("ntrackBothConstrainedVtxZErr/D", Math.sqrt(Math.abs(ntrackBothVtx.getCovMatrix().e(2, 2)))); + row.put("ntrackBothConstrainedChi2/D", ntrackBothVtx.getChi2()); + row.put("ntrackBothConstrainedNdf/I", ntrackBothNdfCheck); + row.put("ntrackBothConstrainedMass/D", ntrackBothVtx.getInvMass()); + row.put("ntrackBothConstrainedElePX/D", ntrackBothEleP.x()); + row.put("ntrackBothConstrainedElePY/D", ntrackBothEleP.y()); + row.put("ntrackBothConstrainedElePZ/D", ntrackBothEleP.z()); + row.put("ntrackBothConstrainedElePXErr/D", ntrackBothElePErr != null ? ntrackBothElePErr.x() : Double.NaN); + row.put("ntrackBothConstrainedElePYErr/D", ntrackBothElePErr != null ? ntrackBothElePErr.y() : Double.NaN); + row.put("ntrackBothConstrainedElePZErr/D", ntrackBothElePErr != null ? ntrackBothElePErr.z() : Double.NaN); + row.put("ntrackBothConstrainedPosPX/D", ntrackBothPosP.x()); + row.put("ntrackBothConstrainedPosPY/D", ntrackBothPosP.y()); + row.put("ntrackBothConstrainedPosPZ/D", ntrackBothPosP.z()); + row.put("ntrackBothConstrainedPosPXErr/D", ntrackBothPosPErr != null ? ntrackBothPosPErr.x() : Double.NaN); + row.put("ntrackBothConstrainedPosPYErr/D", ntrackBothPosPErr != null ? ntrackBothPosPErr.y() : Double.NaN); + row.put("ntrackBothConstrainedPosPZErr/D", ntrackBothPosPErr != null ? ntrackBothPosPErr.z() : Double.NaN); + row.put("ntrackBothConstrainedRecoilPX/D", ntrackBothRecoilP.x()); + row.put("ntrackBothConstrainedRecoilPY/D", ntrackBothRecoilP.y()); + row.put("ntrackBothConstrainedRecoilPZ/D", ntrackBothRecoilP.z()); + row.put("ntrackBothConstrainedRecoilPXErr/D", ntrackBothRecoilPErr != null ? ntrackBothRecoilPErr.x() : Double.NaN); + row.put("ntrackBothConstrainedRecoilPYErr/D", ntrackBothRecoilPErr != null ? ntrackBothRecoilPErr.y() : Double.NaN); + row.put("ntrackBothConstrainedRecoilPZErr/D", ntrackBothRecoilPErr != null ? ntrackBothRecoilPErr.z() : Double.NaN); + } + } + + writeRow(row); + } + } + + private void writeRow(Map row) { + for (String variable : VARIABLES) { + Double value = row.get(variable); + if (value == null || Double.isNaN(value)) { + value = -9999.0; + } + if (variable.endsWith("/I") || variable.endsWith("/B")) { + tupleWriter.format("%d\t", Math.round(value)); + } else { + tupleWriter.format("%g\t", value); + } + } + tupleWriter.println(); + } + + @Override + public void endOfData() { + if (tupleWriter != null) { + tupleWriter.close(); + } + } +} diff --git a/analysis/src/main/java/org/hps/analysis/tuple/NTrackVertexComparisonTupleDriver.java b/analysis/src/main/java/org/hps/analysis/tuple/NTrackVertexComparisonTupleDriver.java new file mode 100644 index 0000000000..c22139e3d8 --- /dev/null +++ b/analysis/src/main/java/org/hps/analysis/tuple/NTrackVertexComparisonTupleDriver.java @@ -0,0 +1,504 @@ +package org.hps.analysis.tuple; + +import java.io.FileNotFoundException; +import java.io.PrintWriter; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +import hep.physics.vec.BasicHep3Vector; +import hep.physics.vec.Hep3Vector; +import hep.physics.vec.VecOp; + +import org.hps.recon.tracking.CoordinateTransformations; +import org.hps.recon.tracking.TrackStateUtils; +import org.hps.recon.tracking.TrackUtils; +import org.hps.recon.vertexing.BilliorTrack; +import org.hps.recon.vertexing.BilliorVertex; +import org.hps.recon.vertexing.BilliorVertexer; +import org.hps.recon.vertexing.NTrackVertexer; +import org.hps.recon.vertexing.TrackConstraintVertexFitter; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.TrackMomentum; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.TrackParams; +import org.lcsim.event.EventHeader; +import org.lcsim.event.LCRelation; +import org.lcsim.event.MCParticle; +import org.lcsim.event.Track; +import org.lcsim.event.TrackState; +import org.lcsim.event.base.BaseTrackState; +import org.lcsim.geometry.Detector; +import org.lcsim.util.Driver; + +import hep.physics.matrix.SymmetricMatrix; + +import org.apache.commons.math3.linear.MatrixUtils; +import org.apache.commons.math3.linear.RealMatrix; + +/** + * Writes a flat ASCII ntuple comparing, per event, the legacy Billoir N-track common-vertex + * fit against the Billoir-batch N-track common-vertex fit + * ({@code NTrackVertexer}), for the 3 truth-matched daughters of either a trident MC + * event (2 e- + 1 e+ from a single common production vertex) or an A' signal MC event (the + * A' e-/e+ decay pair plus the separate recoil electron) -- see the PDGID-622/623 truth + * identification in {@code process} for the exact per-sample topology, kept in sync with the + * same logic in {@link CascadeVertexTupleDriver}. Unlike {@code CascadeVertexTupleDriver}, + * this driver does its own track selection and fitting inline -- it reads raw + * {@code KalmanFullTracks} plus MC truth directly, rather than + * pre-built V0/cascade candidate collections from {@code HpsReconParticleDriver} -- since no + * existing driver builds a true all-tracks-to-one-point common vertex. One row per event + * (only events where all 3 truth daughters are truth-matched to a reconstructed track). + * Header line is variable names joined by ":"; data rows are tab-separated, matching the + * convention used by {@link CascadeVertexTupleDriver}. + */ +public class NTrackVertexComparisonTupleDriver extends Driver { + + private static final List VARIABLES = Arrays.asList( + "run/I", "event/I", + "vtxX/D", "vtxY/D", "vtxZ/D", + "vtxXErr/D", "vtxYErr/D", "vtxZErr/D", + "chi2/D", "mass/D", + "kalUncEle1Px/D", "kalUncEle1Py/D", "kalUncEle1Pz/D", + "kalUncEle1PxErr/D", "kalUncEle1PyErr/D", "kalUncEle1PzErr/D", + "kalUncEle2Px/D", "kalUncEle2Py/D", "kalUncEle2Pz/D", + "kalUncEle2PxErr/D", "kalUncEle2PyErr/D", "kalUncEle2PzErr/D", + "kalUncPosPx/D", "kalUncPosPy/D", "kalUncPosPz/D", + "kalUncPosPxErr/D", "kalUncPosPyErr/D", "kalUncPosPzErr/D", + "kalSoftVtxX/D", "kalSoftVtxY/D", "kalSoftVtxZ/D", + "kalSoftVtxXErr/D", "kalSoftVtxYErr/D", "kalSoftVtxZErr/D", + "kalSoftChi2/D", "kalSoftNdf/I", "kalSoftMass/D", + "kalSoftPx/D", "kalSoftPy/D", "kalSoftPz/D", + "kalSoftPxErr/D", "kalSoftPyErr/D", "kalSoftPzErr/D", + "kalSoftEle1Px/D", "kalSoftEle1Py/D", "kalSoftEle1Pz/D", + "kalSoftEle1PxErr/D", "kalSoftEle1PyErr/D", "kalSoftEle1PzErr/D", + "kalSoftEle2Px/D", "kalSoftEle2Py/D", "kalSoftEle2Pz/D", + "kalSoftEle2PxErr/D", "kalSoftEle2PyErr/D", "kalSoftEle2PzErr/D", + "kalSoftPosPx/D", "kalSoftPosPy/D", "kalSoftPosPz/D", + "kalSoftPosPxErr/D", "kalSoftPosPyErr/D", "kalSoftPosPzErr/D", + "ele1TruthMatched/I", "ele2TruthMatched/I", "posTruthMatched/I", "allTruthMatched/I", + "apVtxXMC/D", "apVtxYMC/D", "apVtxZMC/D", + "mcTotalPx/D", "mcTotalPy/D", "mcTotalPz/D", + "mcEle1Px/D", "mcEle1Py/D", "mcEle1Pz/D", + "mcEle2Px/D", "mcEle2Py/D", "mcEle2Pz/D", + "mcPosPx/D", "mcPosPy/D", "mcPosPz/D", + "recoEle1Px/D", "recoEle1Py/D", "recoEle1Pz/D", + "recoEle1PxErr/D", "recoEle1PyErr/D", "recoEle1PzErr/D", + "recoEle2Px/D", "recoEle2Py/D", "recoEle2Pz/D", + "recoEle2PxErr/D", "recoEle2PyErr/D", "recoEle2PzErr/D", + "recoPosPx/D", "recoPosPy/D", "recoPosPz/D", + "recoPosPxErr/D", "recoPosPyErr/D", "recoPosPzErr/D"); + + private String trackCollectionName = "KalmanFullTracks"; + private String mcParticlesColName = "MCParticle"; + private String trackToMCParticleRelationsColName = "KalmanFullTracksToMCParticleRelations"; + private String tupleFile = null; + private PrintWriter tupleWriter = null; + private double bField; + private double beamEnergy = 3.74; + private double beamRotAngle = -0.0305; + private double minTruthMatchPurity = 0.9; + + public void setTrackCollectionName(String trackCollectionName) { + this.trackCollectionName = trackCollectionName; + } + + public void setMinTruthMatchPurity(double minTruthMatchPurity) { + this.minTruthMatchPurity = minTruthMatchPurity; + } + + public void setBeamEnergy(double beamEnergy) { + this.beamEnergy = beamEnergy; + } + + public void setBeamRotAngle(double beamRotAngle) { + this.beamRotAngle = beamRotAngle; + } + + public void setMcParticlesColName(String mcParticlesColName) { + this.mcParticlesColName = mcParticlesColName; + } + + public void setTrackToMCParticleRelationsColName(String trackToMCParticleRelationsColName) { + this.trackToMCParticleRelationsColName = trackToMCParticleRelationsColName; + } + + public void setTupleFile(String tupleFile) { + this.tupleFile = tupleFile; + } + + @Override + protected void detectorChanged(Detector detector) { + bField = detector.getFieldMap().getField(new BasicHep3Vector(0., 0., -4.3)).y(); + if (tupleFile == null) { + return; + } + try { + tupleWriter = new PrintWriter(tupleFile); + } catch (FileNotFoundException e) { + throw new RuntimeException("Could not open N-track vertex comparison tuple file " + tupleFile, e); + } + tupleWriter.println(String.join(":", VARIABLES)); + } + + @Override + public void process(EventHeader event) { + if (tupleWriter == null || !event.hasCollection(Track.class, trackCollectionName) + || !event.hasCollection(MCParticle.class, mcParticlesColName) + || !event.hasCollection(LCRelation.class, trackToMCParticleRelationsColName)) { + return; + } + + // Truth identification, handling both generator topologies (same conventions/PDGID + // scheme as CascadeVertexTupleDriver, kept in sync with it): the A' sample's 622 is + // the A' signal record (2 daughters: signal e+/e-) with a separate, unrelated + // single-daughter PDGID-623 recoil-electron record; the trident sample has no PDGID + // 622 at all, and PDGID 623 is instead the trident "reaction" pseudo-particle itself, + // with 3 direct daughters (2 e- + 1 e+ from a single common production vertex). + List mcParticles = event.get(MCParticle.class, mcParticlesColName); + MCParticle apMC = null; + MCParticle eleMC = null; + MCParticle posMC = null; + MCParticle recoilMC = null; + for (MCParticle mcp : mcParticles) { + if (mcp.getPDGID() == 622 && mcp.getDaughters().size() == 2) { + apMC = mcp; + for (MCParticle daughter : mcp.getDaughters()) { + if (daughter.getPDGID() == 11) { + eleMC = daughter; + } else if (daughter.getPDGID() == -11) { + posMC = daughter; + } + } + break; + } + } + + MCParticle ele1MC = null; + MCParticle ele2MC = null; + if (apMC != null) { + // A' sample: find the recoil electron as the single PDGID-11 daughter of the + // separate top-level PDGID-623 "reaction" record (same convention as + // CascadeVertexTupleDriver's recoilMC lookup). + for (MCParticle mcp : mcParticles) { + if (mcp.getPDGID() == 623) { + for (MCParticle daughter : mcp.getDaughters()) { + if (daughter.getPDGID() == 11) { + recoilMC = daughter; + break; + } + } + break; + } + } + if (eleMC != null && recoilMC != null) { + // ele1/ele2 have no fixed physical role here (unlike Cascade's ele/recoil + // split) -- just rank by energy for a stable, arbitrary-but-consistent + // column assignment, same as the trident branch below. + if (recoilMC.getEnergy() > eleMC.getEnergy()) { + ele1MC = recoilMC; + ele2MC = eleMC; + } else { + ele1MC = eleMC; + ele2MC = recoilMC; + } + } + } else { + for (MCParticle mcp : mcParticles) { + if (mcp.getPDGID() == 623) { + for (MCParticle daughter : mcp.getDaughters()) { + switch (daughter.getPDGID()) { + case -11: + if (posMC == null || daughter.getEnergy() > posMC.getEnergy()) { + posMC = daughter; + } + break; + case 11: + if (ele1MC == null || daughter.getEnergy() > ele1MC.getEnergy()) { + ele2MC = ele1MC; + ele1MC = daughter; + } else if (ele2MC == null || daughter.getEnergy() > ele2MC.getEnergy()) { + ele2MC = daughter; + } + break; + } + } + break; + } + } + } + if (ele1MC == null || ele2MC == null || posMC == null) { + return; + } + + // Truth vertex: for A', eleMC's own getOrigin() is not contaminated by the apMC + // decay-length-shift bug (see CascadeVertexTupleDriver's apVtxMC/apOriginMC + // comments); for trident, all 3 daughters share one common, uncontaminated + // production vertex, so ele1MC.getOrigin() is equally valid there. + Hep3Vector apVtxMC = eleMC != null ? eleMC.getOrigin() : ele1MC.getOrigin(); + + // Truth total momentum of the 3 truth-matched daughters, used as the pull + // denominator's truth term for the fitted total-momentum pull plots -- same "no + // frame-conversion workaround needed" situation as apVtxMC above (MCParticle.getMomentum() + // is already in detector frame). + Hep3Vector ele1MCMom = ele1MC.getMomentum(); + Hep3Vector ele2MCMom = ele2MC.getMomentum(); + Hep3Vector posMCMom = posMC.getMomentum(); + Hep3Vector mcTotalP = VecOp.add(VecOp.add(ele1MCMom, ele2MCMom), posMCMom); + + // A single MCParticle can receive relations from more than one Track (e.g. a + // ghost/duplicate track sharing hits with the true particle), so pick the + // highest-purity (rel.getWeight()) match rather than whichever relation the + // collection happens to iterate last -- same disambiguation signal used by + // CascadeVertexTupleDriver's trackPurity maps. + Map mcToTrack = new HashMap(); + Map mcToWeight = new HashMap(); + for (LCRelation rel : event.get(LCRelation.class, trackToMCParticleRelationsColName)) { + MCParticle mcp = (MCParticle) rel.getTo(); + if (mcp == ele1MC || mcp == ele2MC || mcp == posMC) { + double weight = rel.getWeight(); + if (weight < minTruthMatchPurity) { + continue; + } + Double best = mcToWeight.get(mcp); + if (best == null || weight > best) { + mcToWeight.put(mcp, weight); + mcToTrack.put(mcp, (Track) rel.getFrom()); + } + } + } + + Track ele1Track = mcToTrack.get(ele1MC); + Track ele2Track = mcToTrack.get(ele2MC); + Track posTrack = mcToTrack.get(posMC); + boolean ele1Matched = ele1Track != null; + boolean ele2Matched = ele2Track != null; + boolean posMatched = posTrack != null; + if (!ele1Matched || !ele2Matched || !posMatched) { + return; + } + + List tracks = Arrays.asList(ele1Track, ele2Track, posTrack); + + // Raw reconstructed momentum (+ its own, no-vertex-constraint error) at each + // track's own point of closest approach, before any vertex fit is applied -- the + // pre-fit baseline the vertex-fit momenta (and MC truth) are compared against. + // Computed in the tracking frame (same convention as TrackDataDriver.java) then + // converted to detector frame to match apVtxMC/mcEle1Px etc. + Hep3Vector[] recoEle1 = getRawMomentumAndError(ele1Track); + Hep3Vector[] recoEle2 = getRawMomentumAndError(ele2Track); + Hep3Vector[] recoPos = getRawMomentumAndError(posTrack); + Hep3Vector recoEle1Mom = recoEle1[0], recoEle1MomErr = recoEle1[1]; + Hep3Vector recoEle2Mom = recoEle2[0], recoEle2MomErr = recoEle2[1]; + Hep3Vector recoPosMom = recoPos[0], recoPosMomErr = recoPos[1]; + + List billTracks = new ArrayList(); + for (Track track : tracks) { + billTracks.add(new BilliorTrack(track)); + } + + // BilliorTrack(Track) copies the AtPerigee track parameters verbatim but drops the + // TrackState's own (fixed, non-zero) reference point, and BilliorVertexer's linear + // approximation is only accurate near its assumed reference point -- exactly the gap + // HpsReconParticleDriver.fitVertex()/shiftTracksToVertex() (production V0 fitting) + // works around: fit once from the naive (uncorrected) tracks, then re-derive each + // track's helix parameters at that first-pass vertex via + // TrackUtils.getParametersAtNewRefPoint/getCovarianceAtNewRefPoint, and refit from + // there. Mirrored here verbatim (generalized to N=3 tracks) rather than just adding + // the reference point back once, for full consistency with the production pattern. + BilliorVertexer firstPassVertexer = new BilliorVertexer(bField); + BilliorVertex firstPassVtx = firstPassVertexer.fitVertex(billTracks); + + double[] newRef = {firstPassVtx.getPosition().z(), firstPassVtx.getPosition().x(), 0.0}; + List shiftedTracks = new ArrayList(); + for (Track track : tracks) { + BaseTrackState oldTs = (BaseTrackState) TrackStateUtils.getTrackStatesAtLocation(track, TrackState.AtPerigee).get(0); + double[] newParams = TrackUtils.getParametersAtNewRefPoint(newRef, oldTs); + SymmetricMatrix newCov = TrackUtils.getCovarianceAtNewRefPoint(newRef, oldTs.getReferencePoint(), oldTs.getParameters(), + new SymmetricMatrix(5, oldTs.getCovMatrix(), true)); + BaseTrackState newTs = new BaseTrackState(newParams, newRef, newCov.asPackedArray(true), TrackState.AtPerigee, oldTs.getBLocal()); + shiftedTracks.add(new BilliorTrack(newTs, 0, 0)); + } + + BilliorVertexer billiorVertexer = new BilliorVertexer(bField); + billiorVertexer.setReferencePosition(newRef); + BilliorVertex billoirVtx = billiorVertexer.fitVertex(shiftedTracks); + BilliorVertex kalmanVtxUnconstrained = new NTrackVertexer(bField).fitVertexNoBeamConstraint(tracks); + // Hard (Lagrange-multiplier/exact) beam-momentum-constrained mode is deprecated (see + // TrackConstraintVertexFitter.fitLagrangeMultiplier) and no longer computed here. + // sigmaTNuclearRecoil accounts for momentum carried away by the target nuclear recoil + // in trident production, not modeled by the beam-divergence-only covariance alone (see + // TrackConstraintVertexFitter.setBeamMomentumTransverseNuclearRecoilSigma). Value is + // 18.6 MeV, measured from std(mcTotalPx/Py) truth spread on a single 10-file real-MC + // pilot; TODO: retune on a larger/full sample. + double sigmaTNuclearRecoil = 0.0186; // GeV + BilliorVertex kalmanVtxSoft = new NTrackVertexer(bField).fitVertexBeamConstrained( + tracks, beamEnergy, beamRotAngle, false, sigmaTNuclearRecoil); + + Hep3Vector vtxPos = billoirVtx.getPosition(); + Hep3Vector vtxPosErr = billoirVtx.getPositionError(); + Hep3Vector kalSoftVtxPos = kalmanVtxSoft.getPosition(); + Hep3Vector kalSoftVtxPosErr = kalmanVtxSoft.getPositionError(); + Double kalSoftNdf = kalmanVtxSoft.getCustomParameters().get("ndf"); + Hep3Vector kalSoftP = kalmanVtxSoft.getV0Momentum(); + Hep3Vector kalSoftPErr = kalmanVtxSoft.getV0MomentumError(); + + Map row = new HashMap(); + row.put("run/I", (double) event.getRunNumber()); + row.put("event/I", (double) event.getEventNumber()); + row.put("vtxX/D", vtxPos.x()); + row.put("vtxY/D", vtxPos.y()); + row.put("vtxZ/D", vtxPos.z()); + row.put("vtxXErr/D", vtxPosErr.x()); + row.put("vtxYErr/D", vtxPosErr.y()); + row.put("vtxZErr/D", vtxPosErr.z()); + row.put("chi2/D", billoirVtx.getChi2()); + row.put("mass/D", billoirVtx.getInvMass()); + row.put("kalSoftVtxX/D", kalSoftVtxPos.x()); + row.put("kalSoftVtxY/D", kalSoftVtxPos.y()); + row.put("kalSoftVtxZ/D", kalSoftVtxPos.z()); + row.put("kalSoftVtxXErr/D", kalSoftVtxPosErr.x()); + row.put("kalSoftVtxYErr/D", kalSoftVtxPosErr.y()); + row.put("kalSoftVtxZErr/D", kalSoftVtxPosErr.z()); + row.put("kalSoftChi2/D", kalmanVtxSoft.getChi2()); + row.put("kalSoftNdf/I", kalSoftNdf != null ? kalSoftNdf : -9999.0); + row.put("kalSoftMass/D", kalmanVtxSoft.getInvMass()); + if (kalSoftP != null && kalSoftPErr != null) { + row.put("kalSoftPx/D", kalSoftP.x()); + row.put("kalSoftPy/D", kalSoftP.y()); + row.put("kalSoftPz/D", kalSoftP.z()); + row.put("kalSoftPxErr/D", kalSoftPErr.x()); + row.put("kalSoftPyErr/D", kalSoftPErr.y()); + row.put("kalSoftPzErr/D", kalSoftPErr.z()); + } + putTrackMomentum(row, kalmanVtxUnconstrained, 0, "kalUncEle1"); + putTrackMomentum(row, kalmanVtxUnconstrained, 1, "kalUncEle2"); + putTrackMomentum(row, kalmanVtxUnconstrained, 2, "kalUncPos"); + putTrackMomentum(row, kalmanVtxSoft, 0, "kalSoftEle1"); + putTrackMomentum(row, kalmanVtxSoft, 1, "kalSoftEle2"); + putTrackMomentum(row, kalmanVtxSoft, 2, "kalSoftPos"); + row.put("ele1TruthMatched/I", ele1Matched ? 1.0 : 0.0); + row.put("ele2TruthMatched/I", ele2Matched ? 1.0 : 0.0); + row.put("posTruthMatched/I", posMatched ? 1.0 : 0.0); + row.put("allTruthMatched/I", (ele1Matched && ele2Matched && posMatched) ? 1.0 : 0.0); + row.put("apVtxXMC/D", apVtxMC.x()); + row.put("apVtxYMC/D", apVtxMC.y()); + row.put("apVtxZMC/D", apVtxMC.z()); + row.put("mcTotalPx/D", mcTotalP.x()); + row.put("mcTotalPy/D", mcTotalP.y()); + row.put("mcTotalPz/D", mcTotalP.z()); + row.put("mcEle1Px/D", ele1MCMom.x()); + row.put("mcEle1Py/D", ele1MCMom.y()); + row.put("mcEle1Pz/D", ele1MCMom.z()); + row.put("mcEle2Px/D", ele2MCMom.x()); + row.put("mcEle2Py/D", ele2MCMom.y()); + row.put("mcEle2Pz/D", ele2MCMom.z()); + row.put("mcPosPx/D", posMCMom.x()); + row.put("mcPosPy/D", posMCMom.y()); + row.put("mcPosPz/D", posMCMom.z()); + row.put("recoEle1Px/D", recoEle1Mom.x()); + row.put("recoEle1Py/D", recoEle1Mom.y()); + row.put("recoEle1Pz/D", recoEle1Mom.z()); + row.put("recoEle1PxErr/D", recoEle1MomErr.x()); + row.put("recoEle1PyErr/D", recoEle1MomErr.y()); + row.put("recoEle1PzErr/D", recoEle1MomErr.z()); + row.put("recoEle2Px/D", recoEle2Mom.x()); + row.put("recoEle2Py/D", recoEle2Mom.y()); + row.put("recoEle2Pz/D", recoEle2Mom.z()); + row.put("recoEle2PxErr/D", recoEle2MomErr.x()); + row.put("recoEle2PyErr/D", recoEle2MomErr.y()); + row.put("recoEle2PzErr/D", recoEle2MomErr.z()); + row.put("recoPosPx/D", recoPosMom.x()); + row.put("recoPosPy/D", recoPosMom.y()); + row.put("recoPosPz/D", recoPosMom.z()); + row.put("recoPosPxErr/D", recoPosMomErr.x()); + row.put("recoPosPyErr/D", recoPosMomErr.y()); + row.put("recoPosPzErr/D", recoPosMomErr.z()); + + writeRow(row); + } + + /** + * Reads track {@code trackIndex}'s fitted momentum + diagonal error off {@code bv} (set + * unconditionally by {@code TrackConstraintVertexFitter.fitVertex}'s dispatcher for every + * track, via {@code getFittedMomentum(i)} and the {@code fitMom{i}_pxErr} custom + * parameters) and writes them into {@code row} under {@code prefix}. No-ops (leaving the + * sentinel fill in {@code writeRow} to apply) if {@code bv} is a failed-fit placeholder, + * signaled by the custom-parameter errors being absent. + */ + private static void putTrackMomentum(Map row, BilliorVertex bv, int trackIndex, String prefix) { + Hep3Vector p = bv.getFittedMomentum(trackIndex); + Double pxErr = bv.getCustomParameters().get("fitMom" + trackIndex + "_pxErr"); + Double pyErr = bv.getCustomParameters().get("fitMom" + trackIndex + "_pyErr"); + Double pzErr = bv.getCustomParameters().get("fitMom" + trackIndex + "_pzErr"); + if (p == null || pxErr == null || pyErr == null || pzErr == null) { + return; + } + row.put(prefix + "Px/D", p.x()); + row.put(prefix + "Py/D", p.y()); + row.put(prefix + "Pz/D", p.z()); + row.put(prefix + "PxErr/D", pxErr); + row.put(prefix + "PyErr/D", pyErr); + row.put(prefix + "PzErr/D", pzErr); + } + + /** + * Raw pre-vertex-fit momentum and its diagonal error of {@code track}, evaluated at + * its own point of closest approach (no vertex constraint applied), converted to + * detector frame. The error comes from propagating the track's own helix-parameter + * covariance through the same momentum Jacobian the vertex fitters use + * ({@link TrackConstraintVertexFitter#computeRawMomentum}), so it is directly + * comparable to the kalSoftXPxErr columns -- the "before any vertex fit" + * baseline for momentum pull plots. Returns {momentum, momentumError}. + */ + private Hep3Vector[] getRawMomentumAndError(Track track) { + TrackState ts = TrackStateUtils.getTrackStatesAtLocation(track, TrackState.AtPerigee).get(0); + double[] par = ts.getParameters(); + SymmetricMatrix sm = new SymmetricMatrix(5, ts.getCovMatrix(), true); + RealMatrix cov = MatrixUtils.createRealMatrix(5, 5); + for (int i = 0; i < 5; i++) { + for (int j = 0; j < 5; j++) { + cov.setEntry(i, j, sm.e(i, j)); + } + } + TrackParams tp = new TrackParams(par[0], par[1], par[2], par[3], par[4], cov); + TrackMomentum tm = new TrackConstraintVertexFitter(bField).computeRawMomentum(tp); + + Hep3Vector pDet = CoordinateTransformations.transformVectorToDetector( + new BasicHep3Vector(tm.p.getEntry(0), tm.p.getEntry(1), tm.p.getEntry(2))); + SymmetricMatrix pCovTrk = new SymmetricMatrix(3); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + pCovTrk.setElement(i, j, tm.pCov.getEntry(i, j)); + } + } + SymmetricMatrix pCovDet = CoordinateTransformations.transformCovarianceToDetector(pCovTrk); + Hep3Vector pErrDet = new BasicHep3Vector( + Math.sqrt(Math.abs(pCovDet.e(0, 0))), + Math.sqrt(Math.abs(pCovDet.e(1, 1))), + Math.sqrt(Math.abs(pCovDet.e(2, 2)))); + return new Hep3Vector[]{pDet, pErrDet}; + } + + private void writeRow(Map row) { + for (String variable : VARIABLES) { + Double value = row.get(variable); + if (value == null || Double.isNaN(value)) { + value = -9999.0; + } + if (variable.endsWith("/I") || variable.endsWith("/B")) { + tupleWriter.format("%d\t", Math.round(value)); + } else { + tupleWriter.format("%g\t", value); + } + } + tupleWriter.println(); + } + + @Override + public void endOfData() { + if (tupleWriter != null) { + tupleWriter.close(); + } + } +} diff --git a/analysis/src/main/java/org/hps/analysis/tuple/V0VertexComparisonTupleDriver.java b/analysis/src/main/java/org/hps/analysis/tuple/V0VertexComparisonTupleDriver.java new file mode 100644 index 0000000000..0c786a5532 --- /dev/null +++ b/analysis/src/main/java/org/hps/analysis/tuple/V0VertexComparisonTupleDriver.java @@ -0,0 +1,351 @@ +package org.hps.analysis.tuple; + +import java.io.FileNotFoundException; +import java.io.PrintWriter; +import java.util.ArrayList; +import java.util.Arrays; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +import hep.physics.vec.BasicHep3Vector; +import hep.physics.vec.Hep3Vector; + +import org.hps.recon.tracking.CoordinateTransformations; +import org.hps.recon.tracking.TrackStateUtils; +import org.hps.recon.tracking.TrackUtils; +import org.hps.recon.vertexing.BilliorTrack; +import org.hps.recon.vertexing.BilliorVertex; +import org.hps.recon.vertexing.BilliorVertexer; +import org.hps.recon.vertexing.NTrackVertexer; +import org.hps.recon.vertexing.TrackConstraintVertexFitter; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.TrackMomentum; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.TrackParams; +import org.lcsim.event.EventHeader; +import org.lcsim.event.LCRelation; +import org.lcsim.event.MCParticle; +import org.lcsim.event.Track; +import org.lcsim.event.TrackState; +import org.lcsim.event.base.BaseTrackState; +import org.lcsim.geometry.Detector; +import org.lcsim.util.Driver; + +import hep.physics.matrix.SymmetricMatrix; + +import org.apache.commons.math3.linear.MatrixUtils; +import org.apache.commons.math3.linear.RealMatrix; + +/** + * Writes a flat ASCII ntuple comparing, per event, the legacy Billoir two-track V0 vertex + * fit against the Billoir-batch two-track vertex fit + * ({@code NTrackVertexer}, N=2), for the truth-matched e-/e+ decay pair of an A' + * signal MC event (PDGID 622 with exactly 2 daughters). Unlike {@code + * NTrackVertexComparisonTupleDriver} (which also handles the trident topology and its + * 3-daughter ambiguity), this driver is scoped to the unambiguous A'-pair case only -- events + * with no such PDGID-622 record (e.g. a trident-only sample) are skipped. As with {@code + * NTrackVertexComparisonTupleDriver}, this driver does its own track selection and fitting + * inline -- it reads raw {@code KalmanFullTracks} plus MC truth directly, rather than a + * pre-built V0 candidate collection from {@code HpsReconParticleDriver} -- so the comparison + * is independent of any production driver wiring. One row per event (only events where both + * truth daughters are truth-matched to a reconstructed track). Header line is variable names + * joined by ":"; data rows are tab-separated, matching the convention used by {@link + * CascadeVertexTupleDriver}. + */ +public class V0VertexComparisonTupleDriver extends Driver { + + private static final List VARIABLES = Arrays.asList( + "run/I", "event/I", + "vtxX/D", "vtxY/D", "vtxZ/D", + "vtxXErr/D", "vtxYErr/D", "vtxZErr/D", + "chi2/D", "mass/D", + "kalUncElePx/D", "kalUncElePy/D", "kalUncElePz/D", + "kalUncElePxErr/D", "kalUncElePyErr/D", "kalUncElePzErr/D", + "kalUncPosPx/D", "kalUncPosPy/D", "kalUncPosPz/D", + "kalUncPosPxErr/D", "kalUncPosPyErr/D", "kalUncPosPzErr/D", + "eleTruthMatched/I", "posTruthMatched/I", "allTruthMatched/I", + "apVtxXMC/D", "apVtxYMC/D", "apVtxZMC/D", + "mcElePx/D", "mcElePy/D", "mcElePz/D", + "mcPosPx/D", "mcPosPy/D", "mcPosPz/D", + "recoElePx/D", "recoElePy/D", "recoElePz/D", + "recoElePxErr/D", "recoElePyErr/D", "recoElePzErr/D", + "recoPosPx/D", "recoPosPy/D", "recoPosPz/D", + "recoPosPxErr/D", "recoPosPyErr/D", "recoPosPzErr/D"); + + private String trackCollectionName = "KalmanFullTracks"; + private String mcParticlesColName = "MCParticle"; + private String trackToMCParticleRelationsColName = "KalmanFullTracksToMCParticleRelations"; + private String tupleFile = null; + private PrintWriter tupleWriter = null; + private double bField; + private double minTruthMatchPurity = 0.9; + + public void setTrackCollectionName(String trackCollectionName) { + this.trackCollectionName = trackCollectionName; + } + + public void setMinTruthMatchPurity(double minTruthMatchPurity) { + this.minTruthMatchPurity = minTruthMatchPurity; + } + + public void setMcParticlesColName(String mcParticlesColName) { + this.mcParticlesColName = mcParticlesColName; + } + + public void setTrackToMCParticleRelationsColName(String trackToMCParticleRelationsColName) { + this.trackToMCParticleRelationsColName = trackToMCParticleRelationsColName; + } + + public void setTupleFile(String tupleFile) { + this.tupleFile = tupleFile; + } + + @Override + protected void detectorChanged(Detector detector) { + bField = detector.getFieldMap().getField(new BasicHep3Vector(0., 0., -4.3)).y(); + if (tupleFile == null) { + return; + } + try { + tupleWriter = new PrintWriter(tupleFile); + } catch (FileNotFoundException e) { + throw new RuntimeException("Could not open V0 vertex comparison tuple file " + tupleFile, e); + } + tupleWriter.println(String.join(":", VARIABLES)); + } + + @Override + public void process(EventHeader event) { + if (tupleWriter == null || !event.hasCollection(Track.class, trackCollectionName) + || !event.hasCollection(MCParticle.class, mcParticlesColName) + || !event.hasCollection(LCRelation.class, trackToMCParticleRelationsColName)) { + return; + } + + // Truth identification: the A' signal record is PDGID 622 with exactly 2 daughters + // (the signal e-/e+ pair), sharing one common, uncontaminated production vertex. + // Scoped to this topology only -- skip events with no such record (e.g. a + // trident-only sample), unlike NTrackVertexComparisonTupleDriver which also handles + // the trident 3-daughter case. + List mcParticles = event.get(MCParticle.class, mcParticlesColName); + MCParticle eleMC = null; + MCParticle posMC = null; + for (MCParticle mcp : mcParticles) { + if (mcp.getPDGID() == 622 && mcp.getDaughters().size() == 2) { + for (MCParticle daughter : mcp.getDaughters()) { + if (daughter.getPDGID() == 11) { + eleMC = daughter; + } else if (daughter.getPDGID() == -11) { + posMC = daughter; + } + } + break; + } + } + if (eleMC == null || posMC == null) { + return; + } + + // Truth vertex: eleMC's own getOrigin() is not contaminated by the apMC + // decay-length-shift bug (see NTrackVertexComparisonTupleDriver's apVtxMC comment). + Hep3Vector apVtxMC = eleMC.getOrigin(); + Hep3Vector eleMCMom = eleMC.getMomentum(); + Hep3Vector posMCMom = posMC.getMomentum(); + + // A single MCParticle can receive relations from more than one Track (e.g. a + // ghost/duplicate track sharing hits with the true particle), so pick the + // highest-purity (rel.getWeight()) match rather than whichever relation the + // collection happens to iterate last. + Map mcToTrack = new HashMap(); + Map mcToWeight = new HashMap(); + for (LCRelation rel : event.get(LCRelation.class, trackToMCParticleRelationsColName)) { + MCParticle mcp = (MCParticle) rel.getTo(); + if (mcp == eleMC || mcp == posMC) { + double weight = rel.getWeight(); + if (weight < minTruthMatchPurity) { + continue; + } + Double best = mcToWeight.get(mcp); + if (best == null || weight > best) { + mcToWeight.put(mcp, weight); + mcToTrack.put(mcp, (Track) rel.getFrom()); + } + } + } + + Track eleTrack = mcToTrack.get(eleMC); + Track posTrack = mcToTrack.get(posMC); + boolean eleMatched = eleTrack != null; + boolean posMatched = posTrack != null; + if (!eleMatched || !posMatched) { + return; + } + + List tracks = Arrays.asList(eleTrack, posTrack); + + // Raw reconstructed momentum (+ its own, no-vertex-constraint error) at each + // track's own point of closest approach, before any vertex fit is applied. + Hep3Vector[] recoEle = getRawMomentumAndError(eleTrack); + Hep3Vector[] recoPos = getRawMomentumAndError(posTrack); + Hep3Vector recoEleMom = recoEle[0], recoEleMomErr = recoEle[1]; + Hep3Vector recoPosMom = recoPos[0], recoPosMomErr = recoPos[1]; + + List billTracks = new ArrayList(); + for (Track track : tracks) { + billTracks.add(new BilliorTrack(track)); + } + + // BilliorTrack(Track) copies the AtPerigee track parameters verbatim but drops the + // TrackState's own (fixed, non-zero) reference point, and BilliorVertexer's linear + // approximation is only accurate near its assumed reference point -- exactly the gap + // HpsReconParticleDriver.fitVertex()/shiftTracksToVertex() (production V0 fitting) + // works around: fit once from the naive (uncorrected) tracks, then re-derive each + // track's helix parameters at that first-pass vertex via + // TrackUtils.getParametersAtNewRefPoint/getCovarianceAtNewRefPoint, and refit from + // there. Mirrored here verbatim for full consistency with the production pattern. + BilliorVertexer firstPassVertexer = new BilliorVertexer(bField); + BilliorVertex firstPassVtx = firstPassVertexer.fitVertex(billTracks); + + double[] newRef = {firstPassVtx.getPosition().z(), firstPassVtx.getPosition().x(), 0.0}; + List shiftedTracks = new ArrayList(); + for (Track track : tracks) { + BaseTrackState oldTs = (BaseTrackState) TrackStateUtils.getTrackStatesAtLocation(track, TrackState.AtPerigee).get(0); + double[] newParams = TrackUtils.getParametersAtNewRefPoint(newRef, oldTs); + SymmetricMatrix newCov = TrackUtils.getCovarianceAtNewRefPoint(newRef, oldTs.getReferencePoint(), oldTs.getParameters(), + new SymmetricMatrix(5, oldTs.getCovMatrix(), true)); + BaseTrackState newTs = new BaseTrackState(newParams, newRef, newCov.asPackedArray(true), TrackState.AtPerigee, oldTs.getBLocal()); + shiftedTracks.add(new BilliorTrack(newTs, 0, 0)); + } + + BilliorVertexer billiorVertexer = new BilliorVertexer(bField); + billiorVertexer.setReferencePosition(newRef); + BilliorVertex billoirVtx = billiorVertexer.fitVertex(shiftedTracks); + BilliorVertex kalmanVtxUnconstrained = new NTrackVertexer(bField).fitVertexNoBeamConstraint(tracks); + + Hep3Vector vtxPos = billoirVtx.getPosition(); + Hep3Vector vtxPosErr = billoirVtx.getPositionError(); + + Map row = new HashMap(); + row.put("run/I", (double) event.getRunNumber()); + row.put("event/I", (double) event.getEventNumber()); + row.put("vtxX/D", vtxPos.x()); + row.put("vtxY/D", vtxPos.y()); + row.put("vtxZ/D", vtxPos.z()); + row.put("vtxXErr/D", vtxPosErr.x()); + row.put("vtxYErr/D", vtxPosErr.y()); + row.put("vtxZErr/D", vtxPosErr.z()); + row.put("chi2/D", billoirVtx.getChi2()); + row.put("mass/D", billoirVtx.getInvMass()); + putTrackMomentum(row, kalmanVtxUnconstrained, 0, "kalUncEle"); + putTrackMomentum(row, kalmanVtxUnconstrained, 1, "kalUncPos"); + row.put("eleTruthMatched/I", eleMatched ? 1.0 : 0.0); + row.put("posTruthMatched/I", posMatched ? 1.0 : 0.0); + row.put("allTruthMatched/I", (eleMatched && posMatched) ? 1.0 : 0.0); + row.put("apVtxXMC/D", apVtxMC.x()); + row.put("apVtxYMC/D", apVtxMC.y()); + row.put("apVtxZMC/D", apVtxMC.z()); + row.put("mcElePx/D", eleMCMom.x()); + row.put("mcElePy/D", eleMCMom.y()); + row.put("mcElePz/D", eleMCMom.z()); + row.put("mcPosPx/D", posMCMom.x()); + row.put("mcPosPy/D", posMCMom.y()); + row.put("mcPosPz/D", posMCMom.z()); + row.put("recoElePx/D", recoEleMom.x()); + row.put("recoElePy/D", recoEleMom.y()); + row.put("recoElePz/D", recoEleMom.z()); + row.put("recoElePxErr/D", recoEleMomErr.x()); + row.put("recoElePyErr/D", recoEleMomErr.y()); + row.put("recoElePzErr/D", recoEleMomErr.z()); + row.put("recoPosPx/D", recoPosMom.x()); + row.put("recoPosPy/D", recoPosMom.y()); + row.put("recoPosPz/D", recoPosMom.z()); + row.put("recoPosPxErr/D", recoPosMomErr.x()); + row.put("recoPosPyErr/D", recoPosMomErr.y()); + row.put("recoPosPzErr/D", recoPosMomErr.z()); + + writeRow(row); + } + + /** + * Reads track {@code trackIndex}'s fitted momentum + diagonal error off {@code bv} (set + * unconditionally by {@code TrackConstraintVertexFitter.fitVertex}'s dispatcher for every + * track, via {@code getFittedMomentum(i)} and the {@code fitMom{i}_pxErr} custom + * parameters) and writes them into {@code row} under {@code prefix}. No-ops (leaving the + * sentinel fill in {@code writeRow} to apply) if {@code bv} is a failed-fit placeholder, + * signaled by the custom-parameter errors being absent. + */ + private static void putTrackMomentum(Map row, BilliorVertex bv, int trackIndex, String prefix) { + Hep3Vector p = bv.getFittedMomentum(trackIndex); + Double pxErr = bv.getCustomParameters().get("fitMom" + trackIndex + "_pxErr"); + Double pyErr = bv.getCustomParameters().get("fitMom" + trackIndex + "_pyErr"); + Double pzErr = bv.getCustomParameters().get("fitMom" + trackIndex + "_pzErr"); + if (p == null || pxErr == null || pyErr == null || pzErr == null) { + return; + } + row.put(prefix + "Px/D", p.x()); + row.put(prefix + "Py/D", p.y()); + row.put(prefix + "Pz/D", p.z()); + row.put(prefix + "PxErr/D", pxErr); + row.put(prefix + "PyErr/D", pyErr); + row.put(prefix + "PzErr/D", pzErr); + } + + /** + * Raw pre-vertex-fit momentum and its diagonal error of {@code track}, evaluated at + * its own point of closest approach (no vertex constraint applied), converted to + * detector frame. The error comes from propagating the track's own helix-parameter + * covariance through the same momentum Jacobian the vertex fitters use + * ({@link TrackConstraintVertexFitter#computeRawMomentum}), so it is directly + * comparable to the kalUncXPxErr columns -- the "before any vertex fit" + * baseline. Returns {momentum, momentumError}. + */ + private Hep3Vector[] getRawMomentumAndError(Track track) { + TrackState ts = TrackStateUtils.getTrackStatesAtLocation(track, TrackState.AtPerigee).get(0); + double[] par = ts.getParameters(); + SymmetricMatrix sm = new SymmetricMatrix(5, ts.getCovMatrix(), true); + RealMatrix cov = MatrixUtils.createRealMatrix(5, 5); + for (int i = 0; i < 5; i++) { + for (int j = 0; j < 5; j++) { + cov.setEntry(i, j, sm.e(i, j)); + } + } + TrackParams tp = new TrackParams(par[0], par[1], par[2], par[3], par[4], cov); + TrackMomentum tm = new TrackConstraintVertexFitter(bField).computeRawMomentum(tp); + + Hep3Vector pDet = CoordinateTransformations.transformVectorToDetector( + new BasicHep3Vector(tm.p.getEntry(0), tm.p.getEntry(1), tm.p.getEntry(2))); + SymmetricMatrix pCovTrk = new SymmetricMatrix(3); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + pCovTrk.setElement(i, j, tm.pCov.getEntry(i, j)); + } + } + SymmetricMatrix pCovDet = CoordinateTransformations.transformCovarianceToDetector(pCovTrk); + Hep3Vector pErrDet = new BasicHep3Vector( + Math.sqrt(Math.abs(pCovDet.e(0, 0))), + Math.sqrt(Math.abs(pCovDet.e(1, 1))), + Math.sqrt(Math.abs(pCovDet.e(2, 2)))); + return new Hep3Vector[]{pDet, pErrDet}; + } + + private void writeRow(Map row) { + for (String variable : VARIABLES) { + Double value = row.get(variable); + if (value == null || Double.isNaN(value)) { + value = -9999.0; + } + if (variable.endsWith("/I") || variable.endsWith("/B")) { + tupleWriter.format("%d\t", Math.round(value)); + } else { + tupleWriter.format("%g\t", value); + } + } + tupleWriter.println(); + } + + @Override + public void endOfData() { + if (tupleWriter != null) { + tupleWriter.close(); + } + } +} diff --git a/recon/src/main/java/org/hps/recon/filtering/ThreeElectronTrackingFilter.java b/recon/src/main/java/org/hps/recon/filtering/ThreeElectronTrackingFilter.java new file mode 100644 index 0000000000..0c36e0f04e --- /dev/null +++ b/recon/src/main/java/org/hps/recon/filtering/ThreeElectronTrackingFilter.java @@ -0,0 +1,133 @@ +package org.hps.recon.filtering; + +import java.io.IOException; +import java.util.HashSet; +import java.util.List; +import java.util.Set; + +import org.lcsim.event.EventHeader; +import org.lcsim.event.LCRelation; +import org.lcsim.event.MCParticle; +import org.lcsim.lcio.LCIOWriter; +import org.lcsim.util.Driver; + +/** + * Writes a separate, small skim LCIO file containing only events where all three signal + * electrons -- the e-/e+ daughters of an A' (MCParticle PDGID 622 with two daughters) and a + * separate recoil e- (the PDGID-11 daughter of a top-level PDGID-623 "reaction" particle) -- + * are each matched to a reconstructed Track, using the same MC-truth conventions as + * CascadeVertexTupleDriver. Unlike filtering on CascadeVertexCandidates, + * this only requires that tracking found all three tracks; it doesn't require that any + * V0/vertex candidate was successfully built from them, so the skim stays valid regardless of + * changes to the downstream vertex-fitting code. + * + * This driver does not skip events: every event flows through the rest of the driver chain + * unaffected, and passing events are additionally written to {@link #outputFilePath} here. + */ +public class ThreeElectronTrackingFilter extends Driver { + + private String mcParticlesColName = "MCParticle"; + private String trackToMCParticleRelationsColName = "KalmanFullTracksToMCParticleRelations"; + private String outputFilePath; + private LCIOWriter writer; + private int nprocessed = 0; + private int npassed = 0; + + public void setMcParticlesColName(String mcParticlesColName) { + this.mcParticlesColName = mcParticlesColName; + } + + public void setTrackToMCParticleRelationsColName(String trackToMCParticleRelationsColName) { + this.trackToMCParticleRelationsColName = trackToMCParticleRelationsColName; + } + + public void setOutputFilePath(String outputFilePath) { + this.outputFilePath = outputFilePath; + } + + @Override + protected void startOfData() { + if (outputFilePath == null) { + throw new RuntimeException("outputFilePath must be set"); + } + try { + writer = new LCIOWriter(outputFilePath); + writer.reOpen(); + } catch (IOException x) { + throw new RuntimeException("Error creating skim LCIO writer", x); + } + } + + @Override + protected void endOfData() { + try { + writer.close(); + } catch (IOException x) { + throw new RuntimeException("Error closing skim LCIO writer", x); + } + System.out.println(this.getClass().getSimpleName() + ": processed " + nprocessed + ", passed " + npassed); + } + + @Override + protected void process(EventHeader event) { + nprocessed++; + + if (passesThreeElectronTrackingCut(event)) { + npassed++; + try { + writer.write(event); + } catch (IOException x) { + throw new RuntimeException("Error writing skim LCIO file", x); + } + } + } + + private boolean passesThreeElectronTrackingCut(EventHeader event) { + if (!event.hasCollection(MCParticle.class, mcParticlesColName) + || !event.hasCollection(LCRelation.class, trackToMCParticleRelationsColName)) { + return false; + } + + List mcParticles = event.get(MCParticle.class, mcParticlesColName); + + MCParticle eleMC = null; + MCParticle posMC = null; + MCParticle recoilMC = null; + for (MCParticle mcp : mcParticles) { + if (mcp.getPDGID() == 622 && mcp.getDaughters().size() == 2) { + for (MCParticle daughter : mcp.getDaughters()) { + if (daughter.getPDGID() == 11) { + eleMC = daughter; + } else if (daughter.getPDGID() == -11) { + posMC = daughter; + } + } + break; + } + } + if (eleMC != null || posMC != null) { + for (MCParticle mcp : mcParticles) { + if (mcp.getPDGID() == 623) { + for (MCParticle daughter : mcp.getDaughters()) { + if (daughter.getPDGID() == 11) { + recoilMC = daughter; + break; + } + } + break; + } + } + } + + if (eleMC == null || posMC == null || recoilMC == null) { + return false; + } + + Set trackedMC = new HashSet(); + for (LCRelation rel : event.get(LCRelation.class, trackToMCParticleRelationsColName)) { + trackedMC.add((MCParticle) rel.getTo()); + } + + return trackedMC.contains(eleMC) && trackedMC.contains(posMC) && trackedMC.contains(recoilMC); + } +} diff --git a/recon/src/main/java/org/hps/recon/particle/HpsReconParticleDriver.java b/recon/src/main/java/org/hps/recon/particle/HpsReconParticleDriver.java index 7e90b78e37..a89b2e654d 100644 --- a/recon/src/main/java/org/hps/recon/particle/HpsReconParticleDriver.java +++ b/recon/src/main/java/org/hps/recon/particle/HpsReconParticleDriver.java @@ -212,6 +212,11 @@ protected void detectorChanged(Detector detector) { } LOGGER.config("Using beam position [ Z, X, Y ]: " + String.format("[ %f, %f, %f ]", beamPositionToUse[0], beamPositionToUse[1], beamPositionToUse[2])); + + // Keep the base class's beamPosition field (read directly by the cascade/N-track + // beamspot-position-constraint toggles in ReconParticleDriver.findCascadeVertices) + // in sync with the conditions-resolved value used by the existing V0 constraint above. + this.beamPosition = beamPositionToUse; } public void setMaxMollerP(double input) { @@ -256,6 +261,13 @@ public void setMinVertexChisqProb(double input) { cuts.setMinVertexChisqProb(input); } + public void setMaxVertexChisq(double input) { + if (cuts == null) { + cuts = new StandardCuts(beamEnergy); + } + cuts.setMaxVertexChisq(input); + } + public void setIncludeUnmatchedTracksInFSP(boolean setUMTrks) { includeUnmatchedTracksInFSP = setUMTrks; } @@ -694,6 +706,10 @@ private void makeV0Candidates(ReconstructedParticle electron, ReconstructedParti return; } + if (vtxFit.getChi2() > cuts.getMaxVertexChisq()) { + return; + } + // patch the track parameters at the found vertex if (_patchVertexTrackParameters) { patchVertex(vtxFit); diff --git a/recon/src/main/java/org/hps/recon/particle/ReconParticleDriver.java b/recon/src/main/java/org/hps/recon/particle/ReconParticleDriver.java index b6341bab55..50a41dd300 100644 --- a/recon/src/main/java/org/hps/recon/particle/ReconParticleDriver.java +++ b/recon/src/main/java/org/hps/recon/particle/ReconParticleDriver.java @@ -7,22 +7,29 @@ import hep.physics.vec.VecOp; import java.util.ArrayList; +import java.util.Arrays; import java.util.HashMap; import java.util.HashSet; import java.util.List; +import java.util.Map; import java.util.Set; import org.hps.conditions.beam.BeamEnergy.BeamEnergyCollection; import org.hps.recon.tracking.CoordinateTransformations; +import org.hps.recon.vertexing.CascadeVertexer; +import org.hps.recon.vertexing.NTrackVertexer; import org.hps.record.StandardCuts; import org.hps.recon.utils.TrackClusterMatcher; import org.hps.recon.utils.TrackClusterMatcherFactory; +import org.hps.recon.tracking.TrackStateUtils; + import org.lcsim.event.Cluster; import org.lcsim.event.EventHeader; import org.lcsim.event.ReconstructedParticle; import org.lcsim.event.Track; +import org.lcsim.event.TrackState; import org.lcsim.event.Vertex; import org.lcsim.event.base.BaseCluster; import org.lcsim.event.base.BaseReconstructedParticle; @@ -54,6 +61,8 @@ public abstract class ReconParticleDriver extends Driver { protected boolean isMC = false; private boolean disablePID = false; + private boolean fixV2BeamCoordinate = false; + private boolean useBeamspotConstraintForV2 = false; protected StandardCuts cuts = new StandardCuts(); // RelationalTable hitToRotated = null; // RelationalTable hitToStrips = null; @@ -143,6 +152,54 @@ public void setApplyClusterCorrections(boolean val) { * constraints. */ protected List targetConV0Vertices; + /** + * Stores reconstructed 3-track (V0 e-/e+ + recoil electron) simultaneous + * vertex candidate particles. + */ + protected List cascadeVertexCandidates; + /** + * Stores the beam-momentum-constrained refit of each entry in {@link + * #cascadeVertexCandidates}, index-aligned with it (a failed refit is + * represented by {@link CascadeVertexer#placeholderCascade} rather than + * being omitted). See {@link #cascadeVertexCandidatesBeamConstrainedColName}. + */ + protected List cascadeVertexCandidatesBeamConstrained; + /** + * Stores the beamspot-position-constrained refit of each entry in {@link + * #cascadeVertexCandidates} (V2 pulled toward the beamspot, no beam-momentum constraint), + * index-aligned with it. See {@link #cascadeVertexCandidatesBeamspotConstrainedColName}. + */ + protected List cascadeVertexCandidatesBeamspotConstrained; + /** + * Stores the refit of each entry in {@link #cascadeVertexCandidates} with both the + * beamspot-position and beam-momentum constraints applied together, index-aligned with + * it. See {@link #cascadeVertexCandidatesBothConstrainedColName}. + */ + protected List cascadeVertexCandidatesBothConstrained; + /** + * Stores the single-common-vertex ("N-track") fit of the same three tracks (V0 e-/e+ + + * recoil electron) as each entry in {@link #cascadeVertexCandidates}, index-aligned with + * it. See {@link #ntrackVertexCandidatesColName}. + */ + protected List ntrackVertexCandidates; + /** + * Stores the beam-momentum-constrained refit of each entry in {@link + * #ntrackVertexCandidates}, index-aligned with it. See {@link + * #ntrackVertexCandidatesBeamConstrainedColName}. + */ + protected List ntrackVertexCandidatesBeamConstrained; + /** + * Stores the beamspot-position-constrained refit of each entry in {@link + * #ntrackVertexCandidates}, index-aligned with it. See {@link + * #ntrackVertexCandidatesBeamspotConstrainedColName}. + */ + protected List ntrackVertexCandidatesBeamspotConstrained; + /** + * Stores the refit of each entry in {@link #ntrackVertexCandidates} with both the + * beamspot-position and beam-momentum constraints applied together, index-aligned with + * it. See {@link #ntrackVertexCandidatesBothConstrainedColName}. + */ + protected List ntrackVertexCandidatesBothConstrained; // LCIO Collection Names /** @@ -196,6 +253,87 @@ public void setApplyClusterCorrections(boolean val) { * constraints. */ protected String targetConV0VerticesColName = null; + /** + * LCIO collection name for 3-track (V0 e-/e+ + recoil electron) simultaneous + * vertex candidate particles. Defaults to null, i.e. this fit is off unless + * a collection name is explicitly set. + */ + protected String cascadeVertexCandidatesColName = null; + /** + * LCIO collection name for the beam-momentum-constrained refit of {@link + * #cascadeVertexCandidates} (see {@link #cascadeVertexCandidatesBeamConstrained}). + * Defaults to null, i.e. this refit is off unless a collection name is explicitly + * set; has no effect unless {@link #cascadeVertexCandidatesColName} is also set, + * since the refit is always seeded from the plain fit's already-resolved branch. + */ + protected String cascadeVertexCandidatesBeamConstrainedColName = null; + /** + * LCIO collection name for the beamspot-position-constrained refit of {@link + * #cascadeVertexCandidates} (see {@link #cascadeVertexCandidatesBeamspotConstrained}), + * using {@link CascadeVertexer#setUseBeamspotConstraintForV2} with the driver's own + * {@link #beamPosition}/{@link #beamSize} -- independent of {@link + * #useBeamspotConstraintForV2}, which instead changes what {@link + * #cascadeVertexCandidatesColName} itself contains. Defaults to null, i.e. this refit is + * off unless a collection name is explicitly set; has no effect unless {@link + * #cascadeVertexCandidatesColName} is also set. + */ + protected String cascadeVertexCandidatesBeamspotConstrainedColName = null; + /** + * LCIO collection name for the refit of {@link #cascadeVertexCandidates} with both the + * beamspot-position and beam-momentum constraints applied together (see {@link + * #cascadeVertexCandidatesBothConstrained}). Defaults to null, i.e. this refit is off + * unless a collection name is explicitly set; has no effect unless {@link + * #cascadeVertexCandidatesColName} is also set. + */ + protected String cascadeVertexCandidatesBothConstrainedColName = null; + /** + * LCIO collection name for the single-common-vertex ("N-track") fit of the same three + * tracks as {@link #cascadeVertexCandidates}, index-aligned with it (see {@link + * #ntrackVertexCandidates}). Defaults to null, i.e. this fit is off unless a collection + * name is explicitly set; has no effect unless {@link #cascadeVertexCandidatesColName} is + * also set, since it only runs for pairs where the plain cascade fit already succeeded. + */ + protected String ntrackVertexCandidatesColName = null; + /** + * LCIO collection name for the beam-momentum-constrained refit of {@link + * #ntrackVertexCandidates} (see {@link #ntrackVertexCandidatesBeamConstrained}). Defaults + * to null, i.e. this refit is off unless a collection name is explicitly set; has no + * effect unless {@link #ntrackVertexCandidatesColName} is also set. + */ + protected String ntrackVertexCandidatesBeamConstrainedColName = null; + /** + * LCIO collection name for the beamspot-position-constrained refit of {@link + * #ntrackVertexCandidates} (see {@link #ntrackVertexCandidatesBeamspotConstrained}), + * using {@link NTrackVertexer#fitVertexBeamspotConstrained} with the driver's + * own {@link #beamPosition}/{@link #beamSize}. Defaults to null, i.e. this refit is + * off unless a collection name is explicitly set; has no effect unless {@link + * #ntrackVertexCandidatesColName} is also set. + */ + protected String ntrackVertexCandidatesBeamspotConstrainedColName = null; + /** + * LCIO collection name for the refit of {@link #ntrackVertexCandidates} with both the + * beamspot-position and beam-momentum constraints applied together (see {@link + * #ntrackVertexCandidatesBothConstrained}), using {@link + * NTrackVertexer#fitVertexBothConstrained}. Defaults to null, i.e. this refit is + * off unless a collection name is explicitly set; has no effect unless {@link + * #ntrackVertexCandidatesColName} is also set. + */ + protected String ntrackVertexCandidatesBothConstrainedColName = null; + + // Accumulated wall-clock time (ns) and count of calls spent in the cascade + // simultaneous vertex fit, reported in endOfData(). + private long cascadeVertexFitTimeNs = 0L; + private int cascadeVertexFitCount = 0; + + // Shared parameters for the beam-momentum-constrained refits (see + // cascadeVertexCandidatesBeamConstrainedColName and + // ntrackVertexCandidatesBeamConstrainedColName). Named distinctly from the + // unrelated beamEnergy field below (the real per-event beam energy from + // conditions data, used elsewhere for track/cluster matching). Defaults match + // CascadeVertexTupleDriver's previously-hardcoded values. + private double beamMomConstraintEnergy = 3.74; + private double beamMomConstraintRotAngle = -0.0305; + private double beamMomConstraintSigmaTNuclearRecoil = 0.0186; // Beam size variables. // The beamsize array is in the tracking frame @@ -237,6 +375,131 @@ public void setBeamConV0CandidatesColName(String beamConV0CandidatesColName) { this.beamConV0CandidatesColName = beamConV0CandidatesColName; } + /** + * Sets the name of the LCIO collection for 3-track (V0 e-/e+ + recoil + * electron) simultaneous vertex candidate particles. Setting this enables + * this fit, which is off by default. + * + * @param cascadeVertexCandidatesColName - The LCIO collection name. + */ + public void setCascadeVertexCandidatesColName(String cascadeVertexCandidatesColName) { + this.cascadeVertexCandidatesColName = cascadeVertexCandidatesColName; + } + + /** + * Sets the name of the LCIO collection for the beam-momentum-constrained refit + * of the cascade vertex candidates. Setting this enables this refit, which is + * off by default and has no effect unless {@link #cascadeVertexCandidatesColName} + * is also set. + * + * @param cascadeVertexCandidatesBeamConstrainedColName - The LCIO collection name. + */ + public void setCascadeVertexCandidatesBeamConstrainedColName(String cascadeVertexCandidatesBeamConstrainedColName) { + this.cascadeVertexCandidatesBeamConstrainedColName = cascadeVertexCandidatesBeamConstrainedColName; + } + + /** + * Sets the name of the LCIO collection for the beamspot-position-constrained refit of + * the cascade vertex candidates. Setting this enables this refit, which is off by + * default and has no effect unless {@link #cascadeVertexCandidatesColName} is also set. + * + * @param cascadeVertexCandidatesBeamspotConstrainedColName - The LCIO collection name. + */ + public void setCascadeVertexCandidatesBeamspotConstrainedColName( + String cascadeVertexCandidatesBeamspotConstrainedColName) { + this.cascadeVertexCandidatesBeamspotConstrainedColName = cascadeVertexCandidatesBeamspotConstrainedColName; + } + + /** + * Sets the name of the LCIO collection for the refit of the cascade vertex candidates + * with both the beamspot-position and beam-momentum constraints applied together. + * Setting this enables this refit, which is off by default and has no effect unless + * {@link #cascadeVertexCandidatesColName} is also set. + * + * @param cascadeVertexCandidatesBothConstrainedColName - The LCIO collection name. + */ + public void setCascadeVertexCandidatesBothConstrainedColName( + String cascadeVertexCandidatesBothConstrainedColName) { + this.cascadeVertexCandidatesBothConstrainedColName = cascadeVertexCandidatesBothConstrainedColName; + } + + /** + * Sets the name of the LCIO collection for the single-common-vertex ("N-track") fit of + * the same three tracks as the cascade vertex candidates. Setting this enables this fit, + * which is off by default and has no effect unless {@link #cascadeVertexCandidatesColName} + * is also set. + * + * @param ntrackVertexCandidatesColName - The LCIO collection name. + */ + public void setNtrackVertexCandidatesColName(String ntrackVertexCandidatesColName) { + this.ntrackVertexCandidatesColName = ntrackVertexCandidatesColName; + } + + /** + * Sets the name of the LCIO collection for the beam-momentum-constrained refit of the + * N-track vertex candidates. Setting this enables this refit, which is off by default and + * has no effect unless {@link #ntrackVertexCandidatesColName} is also set. + * + * @param ntrackVertexCandidatesBeamConstrainedColName - The LCIO collection name. + */ + public void setNtrackVertexCandidatesBeamConstrainedColName(String ntrackVertexCandidatesBeamConstrainedColName) { + this.ntrackVertexCandidatesBeamConstrainedColName = ntrackVertexCandidatesBeamConstrainedColName; + } + + /** + * Sets the name of the LCIO collection for the beamspot-position-constrained refit of + * the N-track vertex candidates. Setting this enables this refit, which is off by + * default and has no effect unless {@link #ntrackVertexCandidatesColName} is also set. + * + * @param ntrackVertexCandidatesBeamspotConstrainedColName - The LCIO collection name. + */ + public void setNtrackVertexCandidatesBeamspotConstrainedColName( + String ntrackVertexCandidatesBeamspotConstrainedColName) { + this.ntrackVertexCandidatesBeamspotConstrainedColName = ntrackVertexCandidatesBeamspotConstrainedColName; + } + + /** + * Sets the name of the LCIO collection for the refit of the N-track vertex candidates + * with both the beamspot-position and beam-momentum constraints applied together. + * Setting this enables this refit, which is off by default and has no effect unless + * {@link #ntrackVertexCandidatesColName} is also set. + * + * @param ntrackVertexCandidatesBothConstrainedColName - The LCIO collection name. + */ + public void setNtrackVertexCandidatesBothConstrainedColName( + String ntrackVertexCandidatesBothConstrainedColName) { + this.ntrackVertexCandidatesBothConstrainedColName = ntrackVertexCandidatesBothConstrainedColName; + } + + /** + * Sets the beam energy (GeV) used by the cascade and N-track beam-momentum-constrained + * refits. Has no effect unless {@link #cascadeVertexCandidatesBeamConstrainedColName} or + * {@link #ntrackVertexCandidatesBeamConstrainedColName} is set. + */ + public void setBeamMomConstraintEnergy(double beamMomConstraintEnergy) { + this.beamMomConstraintEnergy = beamMomConstraintEnergy; + } + + /** + * Sets the beam crossing angle (rad) about the tracking-frame Z axis used by the + * cascade and N-track beam-momentum-constrained refits. Has no effect unless {@link + * #cascadeVertexCandidatesBeamConstrainedColName} or {@link + * #ntrackVertexCandidatesBeamConstrainedColName} is set. + */ + public void setBeamMomConstraintRotAngle(double beamMomConstraintRotAngle) { + this.beamMomConstraintRotAngle = beamMomConstraintRotAngle; + } + + /** + * Sets the additional transverse beam-momentum-constraint width (GeV) accounting + * for target nuclear recoil, used by the cascade and N-track beam-momentum-constrained + * refits. Has no effect unless {@link #cascadeVertexCandidatesBeamConstrainedColName} or + * {@link #ntrackVertexCandidatesBeamConstrainedColName} is set. + */ + public void setBeamMomConstraintSigmaTNuclearRecoil(double beamMomConstraintSigmaTNuclearRecoil) { + this.beamMomConstraintSigmaTNuclearRecoil = beamMomConstraintSigmaTNuclearRecoil; + } + /** * Sets the name of the LCIO collection for beam spot constrained V0 * candidate vertices. @@ -430,6 +693,28 @@ public void setDisablePID(boolean disablePID) { this.disablePID = disablePID; } + /** + * When true, {@link #findCascadeVertices}'s {@link CascadeVertexer} holds V2's + * beam-direction coordinate fixed at the target position instead of fitting it freely -- + * see {@link CascadeVertexer#setFixV2BeamCoordinate}. Default false keeps the + * original fully-free-V2 joint fit. + */ + public void setFixV2BeamCoordinate(boolean fixV2BeamCoordinate) { + this.fixV2BeamCoordinate = fixV2BeamCoordinate; + } + + /** + * When true, {@link #findCascadeVertices}'s {@link CascadeVertexer} replaces V2's + * V0-flight-line prior with a direct Gaussian prior toward the driver's own {@link + * #beamPosition}/{@link #beamSize} -- see {@link + * CascadeVertexer#setUseBeamspotConstraintForV2} and {@link + * CascadeVertexer#setBeamspotConstraintForV2Params}. Default false keeps the original + * V0-flight-line-projection prior. + */ + public void setUseBeamspotConstraintForV2(boolean useBeamspotConstraintForV2) { + this.useBeamspotConstraintForV2 = useBeamspotConstraintForV2; + } + public void setClusterParamFileName(String input) { clusterParamFileName = input; } @@ -746,6 +1031,14 @@ protected void process(EventHeader event) { unconstrainedV0Vertices = new ArrayList(); beamConV0Vertices = new ArrayList(); targetConV0Vertices = new ArrayList(); + cascadeVertexCandidates = new ArrayList(); + cascadeVertexCandidatesBeamConstrained = new ArrayList(); + cascadeVertexCandidatesBeamspotConstrained = new ArrayList(); + cascadeVertexCandidatesBothConstrained = new ArrayList(); + ntrackVertexCandidates = new ArrayList(); + ntrackVertexCandidatesBeamConstrained = new ArrayList(); + ntrackVertexCandidatesBeamspotConstrained = new ArrayList(); + ntrackVertexCandidatesBothConstrained = new ArrayList(); // Loop through all of the track collections present in the event and // create final state particles. @@ -778,6 +1071,15 @@ else if (finalStateParticle.getCharge() < 0) { List goodFinalStateParticles = particleCuts(finalStateParticles); // VERBOSE :: Output the number of reconstructed particles. printDebug("Final State Particles :: " + goodFinalStateParticles.size()); + + // Form 3-track (V0 e-/e+ + recoil electron) simultaneous vertex candidates, + // pairing each unconstrained V0 with every final-state electron that is not + // already one of its daughters. Off by default; only runs if a collection + // name has been set. + if (cascadeVertexCandidatesColName != null) { + findCascadeVertices(unconstrainedV0Candidates, goodFinalStateParticles); + printDebug("[ReconParticleDriver] findCascadeVertices() finished"); + } // Add the final state ReconstructedParticles to the event event.put(finalStateParticlesColName, goodFinalStateParticles, ReconstructedParticle.class, 0); for (ReconstructedParticle ele : goodFinalStateParticles) { @@ -814,7 +1116,238 @@ else if (finalStateParticle.getCharge() < 0) { printDebug("Target-Constrained V0 Vertices: " + targetConV0Vertices.size()); event.put(targetConV0VerticesColName, targetConV0Vertices, Vertex.class, 0); } + if (cascadeVertexCandidatesColName != null) { + printDebug("Cascade Vertex Candidates: " + cascadeVertexCandidates.size()); + event.put(cascadeVertexCandidatesColName, cascadeVertexCandidates, ReconstructedParticle.class, 0); + } + if (cascadeVertexCandidatesBeamConstrainedColName != null) { + printDebug("Cascade Vertex Candidates (beam-momentum-constrained): " + + cascadeVertexCandidatesBeamConstrained.size()); + event.put(cascadeVertexCandidatesBeamConstrainedColName, cascadeVertexCandidatesBeamConstrained, + ReconstructedParticle.class, 0); + } + if (cascadeVertexCandidatesBeamspotConstrainedColName != null) { + printDebug("Cascade Vertex Candidates (beamspot-position-constrained): " + + cascadeVertexCandidatesBeamspotConstrained.size()); + event.put(cascadeVertexCandidatesBeamspotConstrainedColName, cascadeVertexCandidatesBeamspotConstrained, + ReconstructedParticle.class, 0); + } + if (cascadeVertexCandidatesBothConstrainedColName != null) { + printDebug("Cascade Vertex Candidates (both-constrained): " + + cascadeVertexCandidatesBothConstrained.size()); + event.put(cascadeVertexCandidatesBothConstrainedColName, cascadeVertexCandidatesBothConstrained, + ReconstructedParticle.class, 0); + } + if (ntrackVertexCandidatesColName != null) { + printDebug("N-track Vertex Candidates: " + ntrackVertexCandidates.size()); + event.put(ntrackVertexCandidatesColName, ntrackVertexCandidates, Vertex.class, 0); + } + if (ntrackVertexCandidatesBeamConstrainedColName != null) { + printDebug("N-track Vertex Candidates (beam-momentum-constrained): " + + ntrackVertexCandidatesBeamConstrained.size()); + event.put(ntrackVertexCandidatesBeamConstrainedColName, ntrackVertexCandidatesBeamConstrained, + Vertex.class, 0); + } + if (ntrackVertexCandidatesBeamspotConstrainedColName != null) { + printDebug("N-track Vertex Candidates (beamspot-position-constrained): " + + ntrackVertexCandidatesBeamspotConstrained.size()); + event.put(ntrackVertexCandidatesBeamspotConstrainedColName, ntrackVertexCandidatesBeamspotConstrained, + Vertex.class, 0); + } + if (ntrackVertexCandidatesBothConstrainedColName != null) { + printDebug("N-track Vertex Candidates (both-constrained): " + + ntrackVertexCandidatesBothConstrained.size()); + event.put(ntrackVertexCandidatesBothConstrainedColName, ntrackVertexCandidatesBothConstrained, + Vertex.class, 0); + } + + } + + /** + * Returns the field to use for {@code CascadeVertexer} + * fits involving the given track: the local field at that track's AtPerigee state + * (i.e. near the target, where the fringe field is weaker than at the SVT center), + * matching the {@code bLocal} correction already applied in + * {@link HpsReconParticleDriver#fitVertex} for the ordinary V0 fit. For GBL tracks + * (trackType==0), which don't carry a per-track local-field value, falls back to + * {@link #bField} (the SVT-center field), same as fitVertex does. + * + * @param track a track whose AtPerigee state is near the vertex being fit. + */ + protected double bLocalForTrack(Track track) { + if (trackType == 0) { + return bField; + } + return TrackStateUtils.getTrackStatesAtLocation(track, TrackState.AtPerigee).get(0).getBLocal(); + } + /** + * Forms cascade simultaneous vertex candidates: for each unconstrained V0 candidate, + * pair it with every final-state electron that is not already one of its daughters, + * fit a single common vertex for the V0's e-/e+ daughters and the recoil electron + * together, and add the result to {@link #cascadeVertexCandidates}. Mirrors the + * try/catch-per-pair pattern used by {@code HpsReconParticleDriver#findV0s}: a failed + * fit for one pair (e.g. singular covariance, non-convergent geometry) is skipped + * without aborting the rest of the event. Uses {@link CascadeVertexer} for the fit. + * When {@link #cascadeVertexCandidatesBeamConstrainedColName} is set, also refits each + * successful candidate with the beam-momentum constraint applied and adds it to {@link + * #cascadeVertexCandidatesBeamConstrained}, using {@link CascadeVertexer#placeholderCascade} + * in place of a failed refit so the two lists stay index-aligned. When {@link + * #ntrackVertexCandidatesColName} is set, also fits the same three tracks to a single + * common vertex (the alternative N-track topology hypothesis, via {@link + * NTrackVertexer}) and adds the result to {@link #ntrackVertexCandidates}, plus its + * own beam-momentum-constrained refit to {@link #ntrackVertexCandidatesBeamConstrained} + * when {@link #ntrackVertexCandidatesBeamConstrainedColName} is set -- both + * {@code NTrackVertexer} fit methods already return an internal placeholder rather + * than null on failure, so these two lists stay index-aligned with {@link + * #cascadeVertexCandidates} automatically. + * + * @param v0Candidates Unconstrained V0 candidates for this event. + * @param finalStateElectrons Final-state electrons for this event. + */ + protected void findCascadeVertices(List v0Candidates, + List finalStateElectrons) { + for (ReconstructedParticle v0 : v0Candidates) { + List v0Daughters = v0.getParticles(); + ReconstructedParticle v0EleDaughter = v0Daughters.get(0).getCharge() < 0 ? v0Daughters.get(0) : v0Daughters.get(1); + ReconstructedParticle v0PosDaughter = v0Daughters.get(0).getCharge() < 0 ? v0Daughters.get(1) : v0Daughters.get(0); + CascadeVertexer cascadeVertexer = new CascadeVertexer(bLocalForTrack(v0EleDaughter.getTracks().get(0))); + cascadeVertexer.setFixV2BeamCoordinate(fixV2BeamCoordinate); + cascadeVertexer.setUseBeamspotConstraintForV2(useBeamspotConstraintForV2); + if (useBeamspotConstraintForV2) { + cascadeVertexer.setBeamspotConstraintForV2Params(beamPosition, beamSize); + } + for (ReconstructedParticle electron : finalStateElectrons) { + if (electron.getCharge() >= 0 || v0Daughters.contains(electron)) { + continue; + } + long fitStartTime = System.nanoTime(); + try { + ReconstructedParticle cascadeVertex = cascadeVertexer.fit(v0, electron); + if (cascadeVertex != null) { + cascadeVertexCandidates.add(cascadeVertex); + if (cascadeVertexCandidatesBeamConstrainedColName != null) { + ReconstructedParticle cascadeVertexBC; + try { + cascadeVertexBC = cascadeVertexer.fit(v0, electron, true, + beamMomConstraintEnergy, beamMomConstraintRotAngle, + beamMomConstraintSigmaTNuclearRecoil); + } catch (RuntimeException e) { + printDebug("[ReconParticleDriver] findCascadeVertices: beam-momentum-constrained " + + "refit failed with RuntimeException: " + e.getMessage()); + cascadeVertexBC = null; + } + cascadeVertexCandidatesBeamConstrained.add(cascadeVertexBC != null + ? cascadeVertexBC : CascadeVertexer.placeholderCascade(cascadeVertex)); + } + // useBeamspotConstraintForV2 is instance state on this cascadeVertexer, shared by + // the default/BC calls above (for every electron paired with this v0) -- flip it on + // just for these two extra refits, then restore the driver-level setting immediately + // after so subsequent electron iterations' default/BC calls are unaffected. + if (cascadeVertexCandidatesBeamspotConstrainedColName != null) { + cascadeVertexer.setUseBeamspotConstraintForV2(true); + cascadeVertexer.setBeamspotConstraintForV2Params(beamPosition, beamSize); + ReconstructedParticle cascadeVertexBSC; + try { + cascadeVertexBSC = cascadeVertexer.fit(v0, electron); + } catch (RuntimeException e) { + printDebug("[ReconParticleDriver] findCascadeVertices: beamspot-position-constrained " + + "refit failed with RuntimeException: " + e.getMessage()); + cascadeVertexBSC = null; + } + cascadeVertexCandidatesBeamspotConstrained.add(cascadeVertexBSC != null + ? cascadeVertexBSC : CascadeVertexer.placeholderCascade(cascadeVertex)); + cascadeVertexer.setUseBeamspotConstraintForV2(useBeamspotConstraintForV2); + if (useBeamspotConstraintForV2) { + cascadeVertexer.setBeamspotConstraintForV2Params(beamPosition, beamSize); + } + } + if (cascadeVertexCandidatesBothConstrainedColName != null) { + cascadeVertexer.setUseBeamspotConstraintForV2(true); + cascadeVertexer.setBeamspotConstraintForV2Params(beamPosition, beamSize); + ReconstructedParticle cascadeVertexBoth; + try { + cascadeVertexBoth = cascadeVertexer.fit(v0, electron, true, + beamMomConstraintEnergy, beamMomConstraintRotAngle, + beamMomConstraintSigmaTNuclearRecoil); + } catch (RuntimeException e) { + printDebug("[ReconParticleDriver] findCascadeVertices: both-constrained " + + "refit failed with RuntimeException: " + e.getMessage()); + cascadeVertexBoth = null; + } + cascadeVertexCandidatesBothConstrained.add(cascadeVertexBoth != null + ? cascadeVertexBoth : CascadeVertexer.placeholderCascade(cascadeVertex)); + cascadeVertexer.setUseBeamspotConstraintForV2(useBeamspotConstraintForV2); + if (useBeamspotConstraintForV2) { + cascadeVertexer.setBeamspotConstraintForV2Params(beamPosition, beamSize); + } + } + if (ntrackVertexCandidatesColName != null) { + List ntrackTracks = Arrays.asList(v0EleDaughter.getTracks().get(0), + v0PosDaughter.getTracks().get(0), electron.getTracks().get(0)); + NTrackVertexer ntrackVertexer = new NTrackVertexer( + bLocalForTrack(v0EleDaughter.getTracks().get(0))); + Vertex ntrackVertex; + try { + ntrackVertex = ntrackVertexer.fitVertexNoBeamConstraint(ntrackTracks); + } catch (RuntimeException e) { + printDebug("[ReconParticleDriver] findCascadeVertices: N-track fit failed with " + + "RuntimeException: " + e.getMessage()); + ntrackVertex = NTrackVertexer.placeholderVertex(3); + } + ntrackVertexCandidates.add(ntrackVertex); + if (ntrackVertexCandidatesBeamConstrainedColName != null) { + Vertex ntrackVertexBC; + try { + ntrackVertexBC = ntrackVertexer.fitVertexBeamConstrained(ntrackTracks, + beamMomConstraintEnergy, beamMomConstraintRotAngle, false, + beamMomConstraintSigmaTNuclearRecoil); + } catch (RuntimeException e) { + printDebug("[ReconParticleDriver] findCascadeVertices: N-track " + + "beam-momentum-constrained refit failed with RuntimeException: " + + e.getMessage()); + ntrackVertexBC = NTrackVertexer.placeholderVertex(3); + } + ntrackVertexCandidatesBeamConstrained.add(ntrackVertexBC); + } + if (ntrackVertexCandidatesBeamspotConstrainedColName != null) { + Vertex ntrackVertexBSC; + try { + ntrackVertexBSC = ntrackVertexer.fitVertexBeamspotConstrained(ntrackTracks, + beamPosition, beamSize); + } catch (RuntimeException e) { + printDebug("[ReconParticleDriver] findCascadeVertices: N-track " + + "beamspot-position-constrained refit failed with RuntimeException: " + + e.getMessage()); + ntrackVertexBSC = NTrackVertexer.placeholderVertex(3); + } + ntrackVertexCandidatesBeamspotConstrained.add(ntrackVertexBSC); + } + if (ntrackVertexCandidatesBothConstrainedColName != null) { + Vertex ntrackVertexBoth; + try { + ntrackVertexBoth = ntrackVertexer.fitVertexBothConstrained(ntrackTracks, + beamPosition, beamSize, beamMomConstraintEnergy, + beamMomConstraintRotAngle, beamMomConstraintSigmaTNuclearRecoil); + } catch (RuntimeException e) { + printDebug("[ReconParticleDriver] findCascadeVertices: N-track " + + "both-constrained refit failed with RuntimeException: " + + e.getMessage()); + ntrackVertexBoth = NTrackVertexer.placeholderVertex(3); + } + ntrackVertexCandidatesBothConstrained.add(ntrackVertexBoth); + } + } + } + } catch (RuntimeException e) { + printDebug("[ReconParticleDriver] findCascadeVertices: skipping pair after RuntimeException: " + e.getMessage()); + continue; + } finally { + cascadeVertexFitTimeNs += System.nanoTime() - fitStartTime; + cascadeVertexFitCount++; + } + } + } } /** @@ -855,6 +1388,14 @@ protected void endOfData() { if (enableTrackClusterMatchPlots) { matcher.saveHistograms(); } + if (cascadeVertexCandidatesColName != null && cascadeVertexFitCount > 0) { + double totalTimeMs = cascadeVertexFitTimeNs / 1e6; + double timePerFitMs = totalTimeMs / cascadeVertexFitCount; + System.out.format("ReconParticleDriver.endOfData: total cascade simultaneous vertex fit " + + "execution time=%12.4f ms for %d fits.\n", totalTimeMs, cascadeVertexFitCount); + System.out.format(" cascade vertex fit time per fit = %9.4f ms\n", + timePerFitMs); + } } public void setSnapToEdge(boolean val) { diff --git a/recon/src/main/java/org/hps/recon/utils/TrackClusterMatcherMinDistance.java b/recon/src/main/java/org/hps/recon/utils/TrackClusterMatcherMinDistance.java index 2b0883c32e..121864318a 100644 --- a/recon/src/main/java/org/hps/recon/utils/TrackClusterMatcherMinDistance.java +++ b/recon/src/main/java/org/hps/recon/utils/TrackClusterMatcherMinDistance.java @@ -27,6 +27,7 @@ import org.lcsim.event.EventHeader; import org.lcsim.event.base.BaseRelationalTable; import org.lcsim.event.LCRelation; +import org.lcsim.event.GenericObject; import org.lcsim.event.base.BaseCluster; import org.lcsim.geometry.subdetector.HPSEcal3; import org.lcsim.event.ReconstructedParticle; @@ -326,7 +327,6 @@ private List getTrackPositionAtEcal(Track track){ trackz = TrackUtils.getTrackStateAtECal(track).getReferencePoint()[0]; } else { - TrackData trackdata = (TrackData) trackToData.from(track); TrackState ts_ecal = TrackUtils.getTrackStateAtECal(track); //If trackstate is null, upstream extrapolation error. Skip this //Track @@ -355,8 +355,7 @@ private double getTrackTime(Track track){ //KF else{ - TrackData trackdata = (TrackData) trackToData.from(track); - trackt = trackdata.getTrackTime(); + trackt = TrackData.getTrackTime((GenericObject) trackToData.from(track)); } return trackt; } diff --git a/recon/src/main/java/org/hps/recon/utils/TrackTruthRelationsDriver.java b/recon/src/main/java/org/hps/recon/utils/TrackTruthRelationsDriver.java index d95f4a8c5d..f039f590c5 100644 --- a/recon/src/main/java/org/hps/recon/utils/TrackTruthRelationsDriver.java +++ b/recon/src/main/java/org/hps/recon/utils/TrackTruthRelationsDriver.java @@ -830,8 +830,10 @@ else if(wrongL2) //Add track<->truth relations for all Tracks matched to a MCP //with purity >= 0.5 if(purity >= purityCut){ - //Add track to mcp relations - trackToMCParticleRelations.add(new BaseLCRelation(track,mcp)); + //Add track to mcp relations; purity is carried in the relation's + //weight so downstream consumers (e.g. CascadeVertexTupleDriver) can + //inspect match quality instead of just the pass/fail purityCut result. + trackToMCParticleRelations.add(new BaseLCRelation(track,mcp,purity)); //Transform MCP into helical track HelicalTrackFit mcp_htf = TrackUtils.getHTF(mcp,bfield); diff --git a/recon/src/main/java/org/hps/recon/vertexing/BilliorVertex.java b/recon/src/main/java/org/hps/recon/vertexing/BilliorVertex.java index 880cff348d..eb7edd10ca 100644 --- a/recon/src/main/java/org/hps/recon/vertexing/BilliorVertex.java +++ b/recon/src/main/java/org/hps/recon/vertexing/BilliorVertex.java @@ -6,6 +6,7 @@ import java.util.Map; import hep.physics.matrix.Matrix; +import hep.physics.matrix.MatrixOp; import hep.physics.matrix.SymmetricMatrix; import hep.physics.vec.BasicHep3Vector; import hep.physics.vec.Hep3Vector; @@ -44,8 +45,14 @@ public class BilliorVertex implements Vertex { private double[] _v0TargetProjectionXY; private double[] _v0TargetProjectionXYErr; - private List _fitTrkParsList=null;//fitted track parameters (theta,phiv,rho) + private List _fitTrkParsList=null;//fitted track parameters (theta,phiv,rho) private List _fitTrkCovList=null; //list of trk covariances (theta,phiv,rho) + + // Cov(vertex position, daughter momentum) for each daughter, i.e. [Cov(V,p1), Cov(V,p2)] + private List _covVtxMomList = null; + + // Custom parameters map for storing arbitrary key-value pairs (e.g. cascade-fit chi2/ndf) + private Map _customParameters = new HashMap(); /** * Dflt Ctor */ @@ -149,7 +156,10 @@ public BilliorVertex(Vertex lcioVtx) { } public void setProbability(int dof) { - _probability = ChisqProb.gammq(dof, _chiSq); + // Chi2 should be non-negative; a fitted chi2 can come out as a tiny negative value + // from floating-point cancellation in near-degenerate fits, which otherwise trips + // ChisqProb.gammq's "Invalid arguments"/"x less than 0" warnings. + _probability = ChisqProb.gammq(dof, Math.max(0.0, _chiSq)); } public void setStoreCovTrkMomList(boolean input) { @@ -211,6 +221,13 @@ public void setFittedTrackCovariance(List covs){ _fitTrkCovList=covs; } + /** + * Set Cov(vertex position, daughter momentum) for each daughter: [Cov(V,p1), Cov(V,p2)]. + */ + public void setVertexMomentumCovariance(List covs) { + _covVtxMomList = covs; + } + @Override public boolean isPrimary() { return _isPrimary; @@ -264,6 +281,12 @@ public Map getParameters() { pars.put("p2X", p2Fit.x()); pars.put("p2Y", p2Fit.y()); pars.put("p2Z", p2Fit.z()); + if (_fittedMomentum.containsKey(2)) { + Hep3Vector p3Fit = _fittedMomentum.get(2); + pars.put("p3X", p3Fit.x()); + pars.put("p3Y", p3Fit.y()); + pars.put("p3Z", p3Fit.z()); + } } if (_vertexPositionError !=null){ pars.put("vXErr", _vertexPositionError.x()); @@ -330,10 +353,30 @@ public Map getParameters() { pars.put("V0TargProjXErr", projErr[0]); pars.put("V0TargProjYErr", projErr[1]); } - + + // Add any custom parameters + pars.putAll(_customParameters); + return pars; } + /** + * Set a custom parameter value + * @param key Parameter name + * @param value Parameter value + */ + public void setParameter(String key, Double value) { + _customParameters.put(key, value); + } + + /** + * Get the custom parameters map (for direct modification) + * @return The custom parameters map + */ + public Map getCustomParameters() { + return _customParameters; + } + public void setLayerCode(String s) { @@ -378,16 +421,36 @@ public Map getFittedMomentum() { * note: only the diagional terms of covariance */ public Hep3Vector getFittedMomentumError(int index) { + if (_covTrkMomList == null || index >= _covTrkMomList.size()) { + return null; + } return new BasicHep3Vector(Math.sqrt(_covTrkMomList.get(index).e(0, 0)), Math.sqrt(_covTrkMomList.get(index).e(1, 1)), Math.sqrt(_covTrkMomList.get(index).e(2, 2))); } - /* - * Return the entire track momentum covariance list for all tracks + /* + * Return the entire track momentum covariance list for all tracks */ public List getFittedMomentumCovariance() { return _covTrkMomList; } + /* + * Return Cov(vertex position, daughter[index] momentum) + */ + public Matrix getVertexMomentumCovariance(int index) { + return _covVtxMomList.get(index); + } + + /* + * Return Cov(vertex position, V0 momentum) = Cov(V,p1) + Cov(V,p2), + * valid by linearity since P_V0 = p1 + p2. This is the full cross-covariance + * block needed to treat the V0 as a straight line with proper position-momentum + * correlations (no block-diagonal approximation). + */ + public Matrix getVertexV0MomentumCovariance() { + return MatrixOp.add(_covVtxMomList.get(0), _covVtxMomList.get(1)); + } + /* * Return the recon particle associated with this vertex */ diff --git a/recon/src/main/java/org/hps/recon/vertexing/BilliorVertexer.java b/recon/src/main/java/org/hps/recon/vertexing/BilliorVertexer.java index 843c30735c..08e910e3f9 100644 --- a/recon/src/main/java/org/hps/recon/vertexing/BilliorVertexer.java +++ b/recon/src/main/java/org/hps/recon/vertexing/BilliorVertexer.java @@ -98,6 +98,10 @@ else if (_targetConstraint) pcov.add(CoordinateTransformations.transformCovarianceToDetector(new SymmetricMatrix(this.getFittedTrk1Trk2MomCovariance(0, 1)))); vertex.setTrackMomentumCovariances(pcov); vertex.setStoreCovTrkMomList(storeCovTrkMomList); + List vtxMomCov = new ArrayList(); + vtxMomCov.add(CoordinateTransformations.transformMatrixToDetector(this.getVertexMomentumCovariance(0))); + vtxMomCov.add(CoordinateTransformations.transformMatrixToDetector(this.getVertexMomentumCovariance(1))); + vertex.setVertexMomentumCovariance(vtxMomCov); vertex.setV0Momentum(CoordinateTransformations.transformVectorToDetector(getV0Momentum()), CoordinateTransformations.transformVectorToDetector(getV0MomentumError())); vertex.setV0TargetXY(getV0Projection(), getV0ProjectionError()); tpars.add(getFittedTrackParameters(0)); @@ -596,6 +600,21 @@ public Matrix getFittedTrk1Trk2MomCovariance(int ind1, int ind2) { } + /* + * Cov(vertex position, track[index] momentum) in Cartesian (px,py,pz), track frame. + * covVtxMomList holds Cov(vertex_xyz, track_(theta,phiv,rho)); the vertex side is already + * Cartesian so only the momentum side needs the Jacobian, applied on the right. + */ + public Matrix getVertexMomentumCovariance(int index) { + BasicMatrix pi = (BasicMatrix) _pFit.get(index); + double theta = pi.e(0, 0); + double phiv = pi.e(1, 0); + double rho = pi.e(2, 0); + BasicMatrix Jac = (BasicMatrix) getJacobianThetaPhiRhoToPxPyPz(theta, phiv, rho); + BasicMatrix JacT = (BasicMatrix) MatrixOp.transposed(Jac); + return MatrixOp.mult(covVtxMomList.get(index), JacT); + } + public Matrix getFittedVertexCovariance() { return _covVtx; } diff --git a/recon/src/main/java/org/hps/recon/vertexing/CascadeVertexer.java b/recon/src/main/java/org/hps/recon/vertexing/CascadeVertexer.java new file mode 100644 index 0000000000..a0d517927f --- /dev/null +++ b/recon/src/main/java/org/hps/recon/vertexing/CascadeVertexer.java @@ -0,0 +1,570 @@ +package org.hps.recon.vertexing; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +import org.apache.commons.math3.linear.MatrixUtils; +import org.apache.commons.math3.linear.RealMatrix; +import org.apache.commons.math3.linear.RealVector; +import org.apache.commons.math3.util.FastMath; + +import hep.physics.matrix.Matrix; +import hep.physics.matrix.SymmetricMatrix; +import hep.physics.vec.BasicHep3Vector; +import hep.physics.vec.BasicHepLorentzVector; +import hep.physics.vec.Hep3Vector; +import hep.physics.vec.HepLorentzVector; +import hep.physics.vec.VecOp; + +import org.lcsim.event.ReconstructedParticle; +import org.lcsim.event.Track; +import org.lcsim.event.TrackState; +import org.lcsim.event.base.BaseReconstructedParticle; + +import org.hps.recon.tracking.TrackStateUtils; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.LineParams; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.LinePlaneProjection; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.TrackParams; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.TwoVertexFitResult; + +/** + * Fits a hierarchical two-vertex cascade: V1, the e-/e+ decay vertex, and V2, the + * production vertex where the neutral V0 (e-+e+) meets the recoil electron, linked by a + * straight-line-flight collinearity constraint. Unlike a common-vertex fit of all three + * tracks, this lets V1, V2, and all three track momenta move jointly within their + * covariances. Wraps {@link TrackConstraintVertexFitter#fitCascadeVertexJoint} and + * packages the result as a 2-daughter cascade ReconstructedParticle (inner V0 particle at + * V1, recoil electron), matching the output shape expected by existing consumers such as + * {@code CascadeVertexTupleDriver}. + */ +public class CascadeVertexer extends Vertexer { + + // When true, V2's tracking-index-0 (beam-direction/target-z) coordinate is held fixed + // at the target position instead of fit freely -- see fit() below. Default false keeps + // today's fully-free-V2 behavior unchanged. + private boolean fixV2BeamCoordinate = false; + + // When true, V2's prior (init position + covariance) is taken from beamPositionOverride/ + // beamSizeOverride (or the fitter's own defaults, if those are null) instead of the V0 + // flight-line's target-plane projection -- a direct Gaussian prior pulling V2 itself + // toward the beamspot, appropriate when V2 (the production vertex) is physically expected + // to sit at/near the target. Default false keeps today's line-projection-prior behavior + // unchanged -- see fit() below. + private boolean useBeamspotConstraintForV2 = false; + private double[] beamPositionOverride = null; + private double[] beamSizeOverride = null; + + public CascadeVertexer(double bField) { + super(bField); + } + + public void setFixV2BeamCoordinate(boolean fixV2BeamCoordinate) { + this.fixV2BeamCoordinate = fixV2BeamCoordinate; + } + + public void setUseBeamspotConstraintForV2(boolean useBeamspotConstraintForV2) { + this.useBeamspotConstraintForV2 = useBeamspotConstraintForV2; + } + + /** + * Override the beamspot position/size used by {@link #setUseBeamspotConstraintForV2} + * (tracking frame, absolute). Has no effect unless that toggle is also set. Pass + * {@code null} for either argument to fall back to {@link TrackConstraintVertexFitter}'s + * own default for that argument. + */ + public void setBeamspotConstraintForV2Params(double[] beamPositionOverride, double[] beamSizeOverride) { + this.beamPositionOverride = beamPositionOverride; + this.beamSizeOverride = beamSizeOverride; + } + + /** + * Fit the joint two-vertex cascade for the V0's two daughters and a recoil electron. + * + * @param v0Particle an already-fitted V0 whose daughters are the e-/e+ tracks and + * whose {@code getStartVertex()} is a {@link BilliorVertex} + * @param recoilElectron a final-state electron not already a daughter of v0Particle + * @return the cascade ReconstructedParticle (V0 particle + recoil electron), or null + * if the fit fails + */ + public ReconstructedParticle fit(ReconstructedParticle v0Particle, ReconstructedParticle recoilElectron) { + return fit(v0Particle, recoilElectron, false, 0.0, 0.0, 0.0); + } + + /** + * Same as {@link #fit(ReconstructedParticle, ReconstructedParticle)}, but + * optionally also constrains the three daughters' total 3-momentum (softly, weighted by + * the beam-momentum uncertainty) to the beam value, via {@link + * TrackConstraintVertexFitter#fitCascadeVertexJointBeamConstrained}. Branch + * disambiguation (the {@code roots}/forced-branch trial below) always runs first using + * only the plain, unconstrained fit -- {@code fitCascadeVertexJointBeamConstrained} only + * does a weaker, single-seed branch check internally, so reusing the already-resolved + * unconstrained vertex as the seed for the constrained refit avoids duplicating that + * more robust disambiguation logic for the constrained case. When {@code beamConstrained} + * is false, all three tracks' own 5 perigee parameters are instead left free (rather than + * held fixed), with no external momentum constraint imposed, via {@link + * TrackConstraintVertexFitter#fitCascadeVertexJointFreeTrack} -- the decay-chain fit + * faithful to Hulsbergen's formalism (NIM A552 (2005) 566-575), and the sole reported + * result outside of the beam-constrained case. + * + * @param beamConstrained if true, also apply the beam-momentum constraint; if + * false, refit with all three tracks' perigee parameters + * free instead + * @param beamEnergy beam energy (GeV); unused when {@code beamConstrained} + * is false + * @param beamRotAngle beam crossing angle about the tracking-frame Z axis + * (rad); unused when {@code beamConstrained} is false + * @param sigmaTNuclearRecoil additional transverse beam-momentum-constraint width + * (GeV) accounting for target nuclear recoil; unused when + * {@code beamConstrained} is false + */ + public ReconstructedParticle fit(ReconstructedParticle v0Particle, ReconstructedParticle recoilElectron, + boolean beamConstrained, double beamEnergy, double beamRotAngle, + double sigmaTNuclearRecoil) { + List v0Daughters = v0Particle.getParticles(); + ReconstructedParticle eleDaughter = v0Daughters.get(0).getCharge() < 0 ? v0Daughters.get(0) : v0Daughters.get(1); + ReconstructedParticle posDaughter = v0Daughters.get(0).getCharge() < 0 ? v0Daughters.get(1) : v0Daughters.get(0); + + TrackParams eleParams = trackParamsFromTrack(eleDaughter.getTracks().get(0)); + TrackParams posParams = trackParamsFromTrack(posDaughter.getTracks().get(0)); + TrackParams recoilParams = trackParamsFromTrack(recoilElectron.getTracks().get(0)); + + // V1's initial guess and prior covariance come directly from the V0's own already- + // fitted vertex (lineFromV0 does the detector -> tracking frame transform and + // assembles the full 6x6 position-momentum joint covariance). V2's initial guess + // and prior covariance come from + // propagating that same line to the target plane, so a correctly-reconstructed + // candidate starts the fit right next to the true answer instead of needing the + // Newton iteration to discover the V1/V2 branch from a flat/beamspot-only prior. + BilliorVertex v0Vertex = (BilliorVertex) v0Particle.getStartVertex(); + LineParams v0Line = TrackConstraintVertexFitter.lineFromV0(v0Vertex); + + // eleParams/posParams/recoilParams above are raw perigee params, i.e. relative to + // their tracks' own reference point (assumed shared across all three, true for + // AtPerigee states from the tracking reconstruction), whereas v0Line (built from an + // already-fit, absolute-frame BilliorVertex) is not -- shift v0Line into that same + // local frame before deriving any fit inputs from it, then shift the fitted V1/V2 + // back to the absolute frame below. The original v0Line is kept, unshifted, for the + // diagnostic target-plane projection stored in the output (v0Proj below). + double[] refPoint = TrackStateUtils.getTrackStatesAtLocation( + eleDaughter.getTracks().get(0), TrackState.AtPerigee).get(0).getReferencePoint(); + LineParams v0LineLocal = shiftLine(v0Line, refPoint, -1.0); + + RealVector v1Init = MatrixUtils.createRealVector(new double[]{v0LineLocal.x0, v0LineLocal.y0, v0LineLocal.z0}); + RealMatrix v1Cov = v0LineLocal.cov.getSubMatrix(0, 2, 0, 2); + + // sigmaXFloor is deliberately weak (matches the flat variance=100 -> sigma=10 prior + // used for V1/theta elsewhere in this class), NOT fitter.getBeamSize()[0] (a + // disconnected, hardcoded 1-micron default never intended for this purpose): the + // along-beam coordinate here is fixed to exactly xPlane by construction (see + // propagateLineToPlane), and priorC is rebuilt from this covariance every outer + // iteration, so a tight floor would permanently pin V2 to the target plane regardless + // of what the recoil track and geometric constraint say -- only a weak floor lets + // those actually determine where along the beam direction V2 ends up. + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(bField); + LinePlaneProjection v0Proj = TrackConstraintVertexFitter.propagateLineToPlane( + v0Line, fitter.getBeamPosition()[0], 10.0); + LinePlaneProjection v2Proj = TrackConstraintVertexFitter.propagateLineToPlane( + v0LineLocal, fitter.getBeamPosition()[0] - refPoint[0], 10.0); + + // When useBeamspotConstraintForV2 is set, replace the V0-flight-line target-plane + // projection with a direct Gaussian prior toward beamPositionOverride/beamSizeOverride + // (or the fitter's own default, if either override is null) -- appropriate when V2 is + // physically expected to sit at/near the target rather than be determined by the + // (possibly poorly-constrained) V0 flight line. Shifted into the same local + // (refPoint-offset) frame as v2Proj.position, since the fit's internal state lives + // there, not the absolute tracking frame. Left equal to v2Proj (unchanged) when the + // toggle is off, matching today's default behavior exactly. + RealVector v2InitForFit = v2Proj.position; + RealMatrix v2CovForFit = v2Proj.cov; + if (useBeamspotConstraintForV2) { + double[] beamPos = (beamPositionOverride != null) ? beamPositionOverride : fitter.getBeamPosition(); + double[] beamSz = (beamSizeOverride != null) ? beamSizeOverride : fitter.getBeamSize(); + v2InitForFit = MatrixUtils.createRealVector(new double[]{ + beamPos[0] - refPoint[0], beamPos[1] - refPoint[1], beamPos[2] - refPoint[2]}); + v2CovForFit = MatrixUtils.createRealDiagonalMatrix(new double[]{ + beamSz[0] * beamSz[0], beamSz[1] * beamSz[1], beamSz[2] * beamSz[2]}); + } + + // Diagnostic-only (not used by the fit itself): where the recoil track's own helix, + // independent of the V0, crosses the same target plane -- lets the tuple compare the + // fitted V2 against each daughter's unconstrained target-plane projection. recoilParams + // is local-frame, so the plane must be evaluated at its position in that same local + // frame, then the result shifted back to absolute. + LinePlaneProjection recoilProjLocal = TrackConstraintVertexFitter.propagateTrackToPlane( + recoilParams, fitter.getBeamPosition()[0] - refPoint[0], 10.0); + LinePlaneProjection recoilProj = new LinePlaneProjection( + recoilProjLocal.position.add(MatrixUtils.createRealVector(refPoint)), recoilProjLocal.cov); + + RealVector pV0Init = fitter.computeMomentumAtVertex(eleParams, v1Init) + .add(fitter.computeMomentumAtVertex(posParams, v1Init)); + double[] roots = TrackConstraintVertexFitter.transverseCircleRoots(v1Init, pV0Init, recoilParams); + + // v2FixedX is only used when fixV2BeamCoordinate is set; it's exactly the same + // target-plane value v2Proj was itself propagated to (in the same local frame as + // v1Init/v2Proj.position, since it's compared directly against the tracks' own raw + // params inside the fitter), so the fixed coordinate is self-consistent with the + // free-V2 fit's own target-plane prior. + double v2FixedX = fitter.getBeamPosition()[0] - refPoint[0]; + + TwoVertexFitResult result; + if (roots == null) { + result = fixV2BeamCoordinate + ? fitter.fitCascadeVertexJointFixedV2X( + eleParams, posParams, recoilParams, v1Init, v2InitForFit, v2FixedX, v1Cov, v2CovForFit) + : fitter.fitCascadeVertexJoint( + eleParams, posParams, recoilParams, v1Init, v2InitForFit, v1Cov, v2CovForFit); + } else { + // The recoil track's transverse (bending-plane) circle generically crosses the + // V0's flight line at two points -- a genuine branch ambiguity. Neither chi2 nor + // fitCascadeVertexJoint's own selectPhysicalThetaSeed (a vertical/dip-consistency + // check) can tell the branches apart: both are computed entirely from this same + // line/circle system, so both inherit its bias toward whichever branch has the + // shorter theta -- a shorter arc length along the recoil helix structurally + // yields a smaller residual in any metric evaluated on this system, regardless + // of which branch is geometrically correct. Measured on real signal MC (task #27, + // 1795 truth-matched double-root candidates): chi2 picks the truth-closer branch + // only 26.5% of the time, and selectPhysicalThetaSeed only 34.7% -- both worse + // than a coin flip. Break the tie with a discriminator that's genuinely external + // to the ambiguity instead: fit both branches to convergence and keep whichever + // one's V1 lands closest to the original two-track V0 vertex fit's own z. That + // fit only ever involves the e-/e+ pair, never the recoil track, so it can't + // inherit this bias -- measured to pick the truth-closer branch 99.1% of the time. + // v0Vertex.getPosition() is absolute-frame; branchResult.v1 (below) is still + // local-frame at this point (shifted back to absolute only after the branch is + // picked), so shift the comparison target into that same local frame instead. + double v0InputDetZLocal = v0Vertex.getPosition().z() - refPoint[0]; + TwoVertexFitResult best = null; + double bestZDiff = Double.POSITIVE_INFINITY; + for (double theta : roots) { + RealVector v2Forced = v1Init.subtract(pV0Init.mapMultiply(theta)); + TwoVertexFitResult branchResult = fixV2BeamCoordinate + ? fitter.fitCascadeVertexJointFixedV2XForcedBranch(eleParams, posParams, recoilParams, + v1Init, theta, v2Forced, v2FixedX, v1Cov, v2CovForFit, 60, 1.0e-8) + : fitter.fitCascadeVertexJointForcedBranch( + eleParams, posParams, recoilParams, v1Init, theta, v2Forced, v1Cov, v2CovForFit, 60, 1.0e-8); + if (branchResult == null) { + continue; + } + double v1DetZ = branchResult.v1.getEntry(0); + double zDiff = FastMath.abs(v1DetZ - v0InputDetZLocal); + if (zDiff < bestZDiff) { + bestZDiff = zDiff; + best = branchResult; + } + } + result = best; + } + if (result == null) { + return null; + } + + // Branch disambiguation above always runs on the plain, unconstrained fit only. + // When a beam-momentum constraint is also requested, seed the constrained refit + // with this already-converged, already-branch-resolved local-frame vertex -- + // fitCascadeVertexJointBeamConstrained only does a weaker, single-seed branch + // check internally (per its own javadoc), so seeding it correctly avoids needing + // to duplicate the roots/forced-branch trial above for the constrained case too. + // When useBeamspotConstraintForV2 is set, the final refit's V2 prior mean must be the + // beamspot position (v2InitForFit), not the stage-1 branch-resolved result.v2 -- but + // fitCascadeVertexJointBeamConstrained/fitCascadeVertexJointFreeTrack's public wrappers + // unconditionally recompute selectPhysicalThetaSeed and silently substitute its own + // (purely geometric) V2 whenever that returns non-null, which would otherwise discard + // v2InitForFit right back to the geometric answer regardless of what's passed in here. + // Route through the ...ForcedBranch siblings instead, which skip that override, using a + // theta seed consistent with v2InitForFit (least-squares projection, same formula used + // for thetaInit elsewhere in this class) so the Newton iteration starts from a + // self-consistent (v1, theta, v2) triple rather than an inconsistent mix. Only taken + // when the toggle is actually on; the toggle-off branch below is untouched, calling the + // exact same public methods with the exact same arguments as before this change. + TwoVertexFitResult finalResult = result; + if (beamConstrained) { + fitter.setBeamEnergy(beamEnergy); + fitter.setBeamRotAngle(beamRotAngle); + fitter.setBeamMomentumTransverseNuclearRecoilSigma(sigmaTNuclearRecoil); + TwoVertexFitResult resultBC; + if (useBeamspotConstraintForV2) { + RealVector pV0AtResult = fitter.computeMomentumAtVertex(eleParams, result.v1) + .add(fitter.computeMomentumAtVertex(posParams, result.v1)); + double pV0AtResultNormSq = pV0AtResult.dotProduct(pV0AtResult); + double thetaFinal = (pV0AtResultNormSq > 0) + ? result.v1.subtract(v2InitForFit).dotProduct(pV0AtResult) / pV0AtResultNormSq + : 0.0; + resultBC = fitter.fitCascadeVertexJointBeamConstrainedForcedBranch( + eleParams, posParams, recoilParams, result.v1, thetaFinal, v2InitForFit, + v1Cov, v2CovForFit, 60, 1.0e-8); + } else { + resultBC = fitter.fitCascadeVertexJointBeamConstrained( + eleParams, posParams, recoilParams, result.v1, result.v2, v1Cov, v2CovForFit); + } + if (resultBC == null) { + return null; + } + finalResult = resultBC; + } else { + TwoVertexFitResult resultFT; + if (useBeamspotConstraintForV2) { + RealVector pV0AtResult = fitter.computeMomentumAtVertex(eleParams, result.v1) + .add(fitter.computeMomentumAtVertex(posParams, result.v1)); + double pV0AtResultNormSq = pV0AtResult.dotProduct(pV0AtResult); + double thetaFinal = (pV0AtResultNormSq > 0) + ? result.v1.subtract(v2InitForFit).dotProduct(pV0AtResult) / pV0AtResultNormSq + : 0.0; + resultFT = fitter.fitCascadeVertexJointFreeTrackForcedBranch( + eleParams, posParams, recoilParams, result.v1, thetaFinal, v2InitForFit, + v1Cov, v2CovForFit, 60, 1.0e-8); + } else { + resultFT = fitter.fitCascadeVertexJointFreeTrack( + eleParams, posParams, recoilParams, result.v1, result.v2, v1Cov, v2CovForFit); + } + if (resultFT == null) { + return null; + } + finalResult = resultFT; + } + + RealVector refPointVec = MatrixUtils.createRealVector(refPoint); + finalResult.v1 = finalResult.v1.add(refPointVec); + finalResult.v2 = finalResult.v2.add(refPointVec); + + return makeReconstructedParticle(finalResult, eleDaughter, posDaughter, recoilElectron, v0Proj, recoilProj, v0Vertex); + } + + /** + * A sentinel cascade ReconstructedParticle (chi2/mass/ndf = -9999, zero position/momenta) + * used in place of a null result when the beam-momentum-constrained refit ({@link + * #fit(ReconstructedParticle, ReconstructedParticle, boolean, boolean, double, double, + * double)} with {@code beamConstrained=true}) fails, so that callers pairing that refit's + * output list index-for-index against the plain-fit candidate list it was seeded from never + * see the two lists fall out of sync. Mirrors {@link + * NTrackVertexer#placeholderVertex(int)}'s sentinel convention, but shaped to match this + * class's nested cascade/V0/recoil output: reuses {@code unconstrainedCascade}'s own + * e-/e+/recoil daughters (real, valid particles) rather than fabricating placeholders for + * them too. + * + * @param unconstrainedCascade the already-succeeded plain cascade candidate the failed + * refit was seeded from + */ + public static ReconstructedParticle placeholderCascade(ReconstructedParticle unconstrainedCascade) { + ReconstructedParticle v0Particle = unconstrainedCascade.getParticles().get(0); + ReconstructedParticle recoilElectron = unconstrainedCascade.getParticles().get(1); + ReconstructedParticle eleDaughter = v0Particle.getParticles().get(0); + ReconstructedParticle posDaughter = v0Particle.getParticles().get(1); + + Hep3Vector zeroPos = new BasicHep3Vector(0, 0, 0); + + Map v0FitMap = new HashMap(); + v0FitMap.put(0, zeroPos); + v0FitMap.put(1, zeroPos); + BilliorVertex v1Vtx = new BilliorVertex(zeroPos, new SymmetricMatrix(3), -9999.0, -9999.0, + v0FitMap, "CASCADE_BEAM_CONSTRAINED_FAILED"); + v1Vtx.setPositionError(zeroPos); + v1Vtx.setParameter("ndf", -9999.0); + + ReconstructedParticle v0PlaceholderParticle = new BaseReconstructedParticle(); + ((BaseReconstructedParticle) v0PlaceholderParticle).setStartVertex(v1Vtx); + v0PlaceholderParticle.addParticle(eleDaughter); + v0PlaceholderParticle.addParticle(posDaughter); + ((BaseReconstructedParticle) v0PlaceholderParticle).setType(eleDaughter.getType()); + v1Vtx.setAssociatedParticle(v0PlaceholderParticle); + + Map cascadeFitMap = new HashMap(); + cascadeFitMap.put(0, zeroPos); + cascadeFitMap.put(1, zeroPos); + BilliorVertex v2Vtx = new BilliorVertex(zeroPos, new SymmetricMatrix(3), -9999.0, -9999.0, + cascadeFitMap, "CASCADE_BEAM_CONSTRAINED_FAILED"); + v2Vtx.setPositionError(zeroPos); + v2Vtx.setParameter("ndf", -9999.0); + + ReconstructedParticle cascade = new BaseReconstructedParticle(); + ((BaseReconstructedParticle) cascade).setStartVertex(v2Vtx); + cascade.addParticle(v0PlaceholderParticle); + cascade.addParticle(recoilElectron); + ((BaseReconstructedParticle) cascade).setType(recoilElectron.getType()); + v2Vtx.setAssociatedParticle(cascade); + + return cascade; + } + + /** + * Shift a line's point (x0,y0,z0) by {@code sign * refPoint} (direction unchanged; a pure + * translation does not affect the line's covariance). + */ + private static LineParams shiftLine(LineParams line, double[] refPoint, double sign) { + return new LineParams( + line.x0 + sign * refPoint[0], line.y0 + sign * refPoint[1], line.z0 + sign * refPoint[2], + line.dx, line.dy, line.dz, line.cov); + } + + private static ReconstructedParticle makeReconstructedParticle(TwoVertexFitResult result, + ReconstructedParticle eleDaughter, ReconstructedParticle posDaughter, ReconstructedParticle recoilElectron, + LinePlaneProjection v0Proj, LinePlaneProjection recoilProj, BilliorVertex v0InputVertex) { + + // Tracking frame -> detector frame: det(x,y,z) = trk(y,z,x), same convention used + // throughout TrackConstraintVertexFitter.fitVertex(...). + Hep3Vector v1PosDet = new BasicHep3Vector( + result.v1.getEntry(1), result.v1.getEntry(2), result.v1.getEntry(0)); + Hep3Vector v2PosDet = new BasicHep3Vector( + result.v2.getEntry(1), result.v2.getEntry(2), result.v2.getEntry(0)); + + SymmetricMatrix v1CovDet = packCov(result.v1Cov); + SymmetricMatrix v2CovDet = packCov(result.v2Cov); + + Hep3Vector v1PosErr = new BasicHep3Vector( + FastMath.sqrt(FastMath.abs(result.v1Cov.getEntry(1, 1))), + FastMath.sqrt(FastMath.abs(result.v1Cov.getEntry(2, 2))), + FastMath.sqrt(FastMath.abs(result.v1Cov.getEntry(0, 0)))); + Hep3Vector v2PosErr = new BasicHep3Vector( + FastMath.sqrt(FastMath.abs(result.v2Cov.getEntry(1, 1))), + FastMath.sqrt(FastMath.abs(result.v2Cov.getEntry(2, 2))), + FastMath.sqrt(FastMath.abs(result.v2Cov.getEntry(0, 0)))); + + Hep3Vector pEle = toDetFrame(result.eMinusMomentum.p); + Hep3Vector pPos = toDetFrame(result.ePlusMomentum.p); + Hep3Vector pRecoil = toDetFrame(result.recoilMomentum.p); + Hep3Vector pV0 = VecOp.add(pEle, pPos); + + // Inner V0 particle at V1: e- and e+ daughters, mirroring HpsReconParticleDriver's + // V0-particle construction style. + double eEleMag = pEle.magnitude(); + double ePosMag = pPos.magnitude(); + double eEle = FastMath.sqrt(eEleMag * eEleMag + ELECTRON_MASS * ELECTRON_MASS); + double ePos = FastMath.sqrt(ePosMag * ePosMag + ELECTRON_MASS * ELECTRON_MASS); + double eV0 = eEle + ePos; + double v0PMag = pV0.magnitude(); + double v0MassSq = eV0 * eV0 - v0PMag * v0PMag; + double v0Mass = v0MassSq > 0 ? FastMath.sqrt(v0MassSq) : -99.0; + + Map v0FitMap = new HashMap(); + v0FitMap.put(0, pEle); + v0FitMap.put(1, pPos); + + // The fit is fully coupled (V1, V2, and all three track momenta adjust jointly), + // so there is no clean way to partition chi2/ndf between the two vertices -- + // report the same joint values on both, as CascadeVertexTupleDriver only reads + // v0Vtx.getInvMass()/getChi2() (not its ndf) from the inner vertex. + BilliorVertex v1Vtx = new BilliorVertex(v1PosDet, v1CovDet, result.chi2, v0Mass, v0FitMap, "TWOVERTEX"); + v1Vtx.setPositionError(v1PosErr); + v1Vtx.setProbability(result.ndf); + v1Vtx.setParameter("ndf", (double) result.ndf); + + // Populate v1Vtx's per-daughter momentum covariances (needed by lineFromV0, which + // any downstream re-fit of this cascade's V0 -- e.g. a second, beam-constrained + // CascadeVertexer.fit() call seeded from this candidate -- relies on to build its + // 6x6 position+momentum line prior). Cross terms (Cov(p_ele,p_pos) and Cov(V1,p_i)) + // aren't tracked by TwoVertexFitResult (only per-track pCov and v1Cov are), so they're + // approximated as zero; this only weakens the resulting prior's off-diagonal terms, + // consistent with this class's existing sigmaXFloor-style deliberately-weak-prior + // philosophy elsewhere (priors here only seed a re-fit, never a final reported value). + Matrix zero3x3 = new SymmetricMatrix(3, new double[6], true); + List covTrkMomList = new ArrayList(); + covTrkMomList.add(packCov(result.eMinusMomentum.pCov)); + covTrkMomList.add(packCov(result.ePlusMomentum.pCov)); + covTrkMomList.add(zero3x3); + v1Vtx.setTrackMomentumCovariances(covTrkMomList); + List covVtxMomList = new ArrayList(); + covVtxMomList.add(zero3x3); + covVtxMomList.add(zero3x3); + v1Vtx.setVertexMomentumCovariance(covVtxMomList); + + ReconstructedParticle v0Particle = new BaseReconstructedParticle(); + ((BaseReconstructedParticle) v0Particle).setStartVertex(v1Vtx); + v0Particle.addParticle(eleDaughter); + v0Particle.addParticle(posDaughter); + ((BaseReconstructedParticle) v0Particle).setType(eleDaughter.getType()); + ((BaseReconstructedParticle) v0Particle).setMass(v0Mass); + HepLorentzVector v0FourVector = new BasicHepLorentzVector(eV0, pV0); + ((BaseReconstructedParticle) v0Particle).set4Vector(v0FourVector); + double v0Charge = eleDaughter.getCharge() + posDaughter.getCharge(); + ((BaseReconstructedParticle) v0Particle).setCharge(v0Charge); + v1Vtx.setAssociatedParticle(v0Particle); + ((BaseReconstructedParticle) v0Particle).setReferencePoint(v1Vtx.getPosition()); + + // Outer cascade particle at V2: V0 particle + recoil electron. + double recoilPMag = pRecoil.magnitude(); + double eRecoil = FastMath.sqrt(recoilPMag * recoilPMag + ELECTRON_MASS * ELECTRON_MASS); + double totalE = eV0 + eRecoil; + Hep3Vector totalP = VecOp.add(pV0, pRecoil); + double totalPMag = totalP.magnitude(); + double massSq = totalE * totalE - totalPMag * totalPMag; + double invMass = massSq > 0 ? FastMath.sqrt(massSq) : -99.0; + + Map cascadeFitMap = new HashMap(); + cascadeFitMap.put(0, pV0); + cascadeFitMap.put(1, pRecoil); + + BilliorVertex v2Vtx = new BilliorVertex(v2PosDet, v2CovDet, result.chi2, invMass, cascadeFitMap, "TWOVERTEX"); + v2Vtx.setPositionError(v2PosErr); + v2Vtx.setProbability(result.ndf); + v2Vtx.setParameter("ndf", (double) result.ndf); + + // Populate v2Vtx's per-daughter (V0, recoil) momentum covariances, needed for + // v0P/recoilP momentum-pull denominators downstream (CascadeVertexTupleDriver). + // V0's covariance is approximated as cov(p_ele)+cov(p_pos) (the cross term + // Cov(p_ele,p_pos) isn't tracked by TwoVertexFitResult, same zero-cross-term + // approximation already used for v1Vtx above). + RealMatrix v0PCov = result.eMinusMomentum.pCov.add(result.ePlusMomentum.pCov); + List cascadeCovTrkMomList = new ArrayList(); + cascadeCovTrkMomList.add(packCov(v0PCov)); + cascadeCovTrkMomList.add(packCov(result.recoilMomentum.pCov)); + v2Vtx.setTrackMomentumCovariances(cascadeCovTrkMomList); + + // Diagnostic target-plane projections (tracking-frame y,z -> detector-frame x,y, same + // det(x,y,z)=trk(y,z,x) convention as everywhere else in this method), independent of + // the fit result itself: where the V0's own flight line and the recoil track's own + // helix separately cross the target plane, for comparing against the fitted V2. + v2Vtx.setParameter("v0ProjX", v0Proj.position.getEntry(1)); + v2Vtx.setParameter("v0ProjY", v0Proj.position.getEntry(2)); + v2Vtx.setParameter("v0ProjXErr", FastMath.sqrt(FastMath.abs(v0Proj.cov.getEntry(1, 1)))); + v2Vtx.setParameter("v0ProjYErr", FastMath.sqrt(FastMath.abs(v0Proj.cov.getEntry(2, 2)))); + v2Vtx.setParameter("recoilProjX", recoilProj.position.getEntry(1)); + v2Vtx.setParameter("recoilProjY", recoilProj.position.getEntry(2)); + v2Vtx.setParameter("recoilProjXErr", FastMath.sqrt(FastMath.abs(recoilProj.cov.getEntry(1, 1)))); + v2Vtx.setParameter("recoilProjYErr", FastMath.sqrt(FastMath.abs(recoilProj.cov.getEntry(2, 2)))); + + // The original two-body Billior V0 fit (position, chi2, mass), from before the joint + // V1/V2 fit above replaced it -- kept here since v0Particle's own startVertex is now + // the joint-fit V1, not this original fit, so it would otherwise be lost. + Hep3Vector v0InputPos = v0InputVertex.getPosition(); + Hep3Vector v0InputPosErr = v0InputVertex.getPositionError(); + v2Vtx.setParameter("v0InputVtxX", v0InputPos.x()); + v2Vtx.setParameter("v0InputVtxY", v0InputPos.y()); + v2Vtx.setParameter("v0InputVtxZ", v0InputPos.z()); + v2Vtx.setParameter("v0InputVtxXErr", v0InputPosErr.x()); + v2Vtx.setParameter("v0InputVtxYErr", v0InputPosErr.y()); + v2Vtx.setParameter("v0InputVtxZErr", v0InputPosErr.z()); + v2Vtx.setParameter("v0InputChi2", v0InputVertex.getChi2()); + v2Vtx.setParameter("v0InputMass", v0InputVertex.getInvMass()); + + ReconstructedParticle cascade = new BaseReconstructedParticle(); + ((BaseReconstructedParticle) cascade).setStartVertex(v2Vtx); + cascade.addParticle(v0Particle); + cascade.addParticle(recoilElectron); + ((BaseReconstructedParticle) cascade).setType(recoilElectron.getType()); + ((BaseReconstructedParticle) cascade).setMass(invMass); + HepLorentzVector fourVector = new BasicHepLorentzVector(totalE, totalP); + ((BaseReconstructedParticle) cascade).set4Vector(fourVector); + double particleCharge = v0Charge + recoilElectron.getCharge(); + ((BaseReconstructedParticle) cascade).setCharge(particleCharge); + v2Vtx.setAssociatedParticle(cascade); + ((BaseReconstructedParticle) cascade).setReferencePoint(v2Vtx.getPosition()); + + return cascade; + } + + private static Hep3Vector toDetFrame(RealVector pTrk) { + return new BasicHep3Vector(pTrk.getEntry(1), pTrk.getEntry(2), pTrk.getEntry(0)); + } + + private static SymmetricMatrix packCov(RealMatrix covTrk) { + double[] covPacked = new double[6]; + covPacked[0] = covTrk.getEntry(1, 1); + covPacked[1] = covTrk.getEntry(2, 1); + covPacked[2] = covTrk.getEntry(2, 2); + covPacked[3] = covTrk.getEntry(0, 1); + covPacked[4] = covTrk.getEntry(0, 2); + covPacked[5] = covTrk.getEntry(0, 0); + return new SymmetricMatrix(3, covPacked, true); + } +} diff --git a/recon/src/main/java/org/hps/recon/vertexing/NTrackVertexer.java b/recon/src/main/java/org/hps/recon/vertexing/NTrackVertexer.java new file mode 100644 index 0000000000..6f9988424a --- /dev/null +++ b/recon/src/main/java/org/hps/recon/vertexing/NTrackVertexer.java @@ -0,0 +1,256 @@ +package org.hps.recon.vertexing; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +import hep.physics.matrix.SymmetricMatrix; +import hep.physics.vec.BasicHep3Vector; +import hep.physics.vec.Hep3Vector; + +import org.lcsim.event.Track; +import org.lcsim.event.TrackState; + +import org.hps.recon.tracking.TrackStateUtils; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.TrackParams; + +/** + * Fits a plain N-track common vertex (all input tracks constrained to the same (x,y,z) + * point, all assumed to be electron-mass), using the Billoir-batch algorithm in {@link + * TrackConstraintVertexFitter}, as an alternative to {@link BilliorVertexer} for the same + * problem. Handles arbitrary N (N >= 2) uniformly -- e.g. the two-track V0 case as well + * as validating against trident MC (2 e- + 1 e+ from a single common production vertex). + * Packages the result as a {@link BilliorVertex} (used here purely as a generic {@code + * Vertex}-implementing data container, not as a Billoir-algorithm result) so it plugs + * directly into existing tuple-reading code. + */ +public class NTrackVertexer extends Vertexer { + + public NTrackVertexer(double bField) { + super(bField); + } + + /** + * Fit the common vertex of N tracks with the total 3-momentum constrained to the beam + * value, using {@link TrackConstraintVertexFitter#fitVertex(List, boolean, boolean, + * boolean)} directly (no beamspot-position constraint). That richer method already + * performs the tracking-to-detector frame conversion, invariant mass calculation, and + * ndf storage, so no separate post-processing is needed here. + * + * @param tracks the input tracks (any N >= 2) + * @param beamEnergy beam energy (GeV) + * @param beamRotAngle beam crossing angle about the tracking-frame Z axis (rad) + * @param hardMomentumConstraint if true, enforce the momentum constraint exactly + * (Lagrange multiplier); if false, apply it softly, weighted + * by the beam momentum uncertainty. + * Deprecated: {@code true} (hard mode) is not physically + * correct -- the target nuclear recoil carries real momentum + * away from the tracked leptons -- and, unlike soft mode, + * cannot be corrected via {@code transverseNuclearRecoilSigma} + * below (see {@link TrackConstraintVertexFitter#fitLagrangeMultiplier}). + * Prefer {@code false} for new production use. + * @return the fitted vertex, or a sentinel placeholder vertex if the fit fails + */ + public BilliorVertex fitVertexBeamConstrained(List tracks, double beamEnergy, + double beamRotAngle, boolean hardMomentumConstraint) { + return fitVertexBeamConstrained(tracks, beamEnergy, beamRotAngle, hardMomentumConstraint, 0.0); + } + + /** + * Same as {@link #fitVertexBeamConstrained(List, double, double, boolean)}, but with an + * additional transverse beam-momentum-constraint width, combined in quadrature with the + * beam-divergence term, to account for event-to-event transverse momentum not carried by + * the tracked leptons -- primarily momentum transferred to the target nucleus during + * production (nuclear recoil; distinct from a recoil electron from radiative/A' events) -- + * see {@link TrackConstraintVertexFitter#setBeamMomentumTransverseNuclearRecoilSigma(double)}. + * Kept as a separate overload rather than changing the 4-argument method in place, so that + * method's existing (recoil-free) behavior remains available unchanged for any other caller. + * + * @param hardMomentumConstraint see {@link #fitVertexBeamConstrained(List, double, double, boolean)}; + * deprecated (hard mode), prefer {@code false} + * @param transverseNuclearRecoilSigma additional transverse momentum width (GeV); 0.0 + * reproduces the original divergence-only covariance exactly. + * Has no effect when {@code hardMomentumConstraint} is true. + */ + public BilliorVertex fitVertexBeamConstrained(List tracks, double beamEnergy, + double beamRotAngle, boolean hardMomentumConstraint, double transverseNuclearRecoilSigma) { + List trackParams = new ArrayList(); + double[] referencePoint = null; + for (Track track : tracks) { + TrackState ts = TrackStateUtils.getTrackStatesAtLocation(track, TrackState.AtPerigee).get(0); + if (referencePoint == null) { + referencePoint = ts.getReferencePoint(); + } + trackParams.add(trackParamsFromTrack(ts)); + } + + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(bField); + fitter.setBeamEnergy(beamEnergy); + fitter.setBeamRotAngle(beamRotAngle); + fitter.setBeamMomentumTransverseNuclearRecoilSigma(transverseNuclearRecoilSigma); + fitter.setReferencePosition(referencePoint); + // storeCovTrkMomList defaults to false in TrackConstraintVertexFitter (only CascadeVertexer's + // own hand-built BilliorVertex path sets per-track momentum covariances unconditionally) -- + // without this, fitVertex() silently drops the per-track momentum covariance it already + // computes internally, and getFittedMomentumError() on the result always returns null. + fitter.setStoreCovTrkMomList(true); + BilliorVertex bv = fitter.fitVertex(trackParams, false, true, hardMomentumConstraint); + return (bv != null) ? bv : placeholderVertex(tracks.size()); + } + + /** + * Fit the common vertex of N tracks with no beamspot-position or beam-momentum + * constraint, using {@link TrackConstraintVertexFitter#fitVertex(List, boolean, + * boolean, boolean)} directly (dispatches internally to {@code fitBillior1985}, which + * computes a real per-track momentum covariance and, via {@code setStoreCovTrkMomList(true)}, + * attaches it to the returned {@code BilliorVertex} so {@code getFittedMomentumError()} is + * populated). + * + * @param tracks the input tracks (any N >= 2) + * @return the fitted vertex, or a sentinel placeholder vertex if the fit fails + */ + public BilliorVertex fitVertexNoBeamConstraint(List tracks) { + List trackParams = new ArrayList(); + double[] referencePoint = null; + for (Track track : tracks) { + TrackState ts = TrackStateUtils.getTrackStatesAtLocation(track, TrackState.AtPerigee).get(0); + if (referencePoint == null) { + referencePoint = ts.getReferencePoint(); + } + trackParams.add(trackParamsFromTrack(ts)); + } + + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(bField); + fitter.setReferencePosition(referencePoint); + // See the comment in fitVertexBeamConstrained -- without this, fitVertex() never attaches + // the per-track momentum covariance it already computes to the returned BilliorVertex. + fitter.setStoreCovTrkMomList(true); + BilliorVertex bv = fitter.fitVertex(trackParams, false, false, false); + return (bv != null) ? bv : placeholderVertex(tracks.size()); + } + + /** + * Fit the common vertex of N tracks with a beamspot-position constraint (no beam-momentum + * constraint), using {@link TrackConstraintVertexFitter#fitVertex(List, boolean, boolean, + * boolean)} with {@code beamspotConstraint=true} -- a direct 3D Gaussian prior pulling the + * fitted vertex position itself toward {@code beamPosition} with weight {@code 1/beamSize^2}, + * appropriate for a vertex physically expected to sit at/near the target (unlike {@link + * BilliorVertexer}'s {@code applyBSconstraint}, which instead projects the fitted momentum + * direction back through the beamspot -- appropriate for a displaced decay vertex). Kept as + * a separate method rather than changing {@link #fitVertexNoBeamConstraint} or {@link + * #fitVertexBeamConstrained} in place, so their existing (unconstrained-position) behavior + * remains available unchanged for any other caller. + * + * @param tracks the input tracks (any N >= 2) + * @param beamPosition beamspot/target position (tracking frame, absolute), or {@code null} + * to use the fitter's own default. Internally shifted by the tracks' + * own reference point before use, since the fit's internal vertex state + * lives in that local frame, not the absolute tracking frame. + * @param beamSize beamspot size (tracking frame, x/y/z sigmas), or {@code null} to use + * the fitter's own default + * @return the fitted vertex, or a sentinel placeholder vertex if the fit fails + */ + public BilliorVertex fitVertexBeamspotConstrained(List tracks, double[] beamPosition, double[] beamSize) { + List trackParams = new ArrayList(); + double[] referencePoint = null; + for (Track track : tracks) { + TrackState ts = TrackStateUtils.getTrackStatesAtLocation(track, TrackState.AtPerigee).get(0); + if (referencePoint == null) { + referencePoint = ts.getReferencePoint(); + } + trackParams.add(trackParamsFromTrack(ts)); + } + + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(bField); + if (beamPosition != null) { + double[] beamPositionLocal = new double[3]; + for (int i = 0; i < 3; i++) { + beamPositionLocal[i] = beamPosition[i] - referencePoint[i]; + } + fitter.setBeamPosition(beamPositionLocal); + } + if (beamSize != null) { + fitter.setBeamSize(beamSize); + } + fitter.setReferencePosition(referencePoint); + // See the comment in fitVertexBeamConstrained -- without this, fitVertex() never attaches + // the per-track momentum covariance it already computes to the returned BilliorVertex. + fitter.setStoreCovTrkMomList(true); + BilliorVertex bv = fitter.fitVertex(trackParams, true, false, false); + return (bv != null) ? bv : placeholderVertex(tracks.size()); + } + + /** + * Fit the common vertex of N tracks with both the beamspot-position constraint ({@link + * #fitVertexBeamspotConstrained}) and the beam-momentum constraint ({@link + * #fitVertexBeamConstrained}) applied together, using {@link + * TrackConstraintVertexFitter#fitVertex(List, boolean, boolean, boolean)} with both + * {@code beamspotConstraint} and {@code beamMomentumConstraint} true. Kept as a separate + * method rather than changing either of those two in place, so their existing (single- + * constraint) behavior remains available unchanged for any other caller. + * + * @param tracks the input tracks (any N >= 2) + * @param beamPosition beamspot/target position (tracking frame, absolute), + * or {@code null} to use the fitter's own default + * @param beamSize beamspot size (tracking frame, x/y/z sigmas), or + * {@code null} to use the fitter's own default + * @param beamEnergy beam energy (GeV) + * @param beamRotAngle beam crossing angle about the tracking-frame Z axis (rad) + * @param transverseNuclearRecoilSigma additional transverse beam-momentum-constraint width + * (GeV); see {@link #fitVertexBeamConstrained(List, + * double, double, boolean, double)} + * @return the fitted vertex, or a sentinel placeholder vertex if the fit fails + */ + public BilliorVertex fitVertexBothConstrained(List tracks, double[] beamPosition, double[] beamSize, + double beamEnergy, double beamRotAngle, double transverseNuclearRecoilSigma) { + List trackParams = new ArrayList(); + double[] referencePoint = null; + for (Track track : tracks) { + TrackState ts = TrackStateUtils.getTrackStatesAtLocation(track, TrackState.AtPerigee).get(0); + if (referencePoint == null) { + referencePoint = ts.getReferencePoint(); + } + trackParams.add(trackParamsFromTrack(ts)); + } + + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(bField); + if (beamPosition != null) { + double[] beamPositionLocal = new double[3]; + for (int i = 0; i < 3; i++) { + beamPositionLocal[i] = beamPosition[i] - referencePoint[i]; + } + fitter.setBeamPosition(beamPositionLocal); + } + if (beamSize != null) { + fitter.setBeamSize(beamSize); + } + fitter.setBeamEnergy(beamEnergy); + fitter.setBeamRotAngle(beamRotAngle); + fitter.setBeamMomentumTransverseNuclearRecoilSigma(transverseNuclearRecoilSigma); + fitter.setReferencePosition(referencePoint); + // See the comment in fitVertexBeamConstrained -- without this, fitVertex() never attaches + // the per-track momentum covariance it already computes to the returned BilliorVertex. + fitter.setStoreCovTrkMomList(true); + BilliorVertex bv = fitter.fitVertex(trackParams, true, true, false); + return (bv != null) ? bv : placeholderVertex(tracks.size()); + } + + /** + * A sentinel vertex (chi2/mass = -9999, zero position/momenta) used in place of a null + * result when the fit fails, so that callers pairing this fit's output index-for-index + * against another collection (e.g. the Billoir N-track fit, for comparison) never see + * the two lists fall out of sync. + */ + public static BilliorVertex placeholderVertex(int nTracks) { + Map pFitMap = new HashMap(); + for (int i = 0; i < nTracks; i++) { + pFitMap.put(i, new BasicHep3Vector(0, 0, 0)); + } + BilliorVertex vtxFit = new BilliorVertex(new BasicHep3Vector(0, 0, 0), + new SymmetricMatrix(3), -9999.0, -9999.0, pFitMap, "KALMAN_NTRACK_FAILED"); + vtxFit.setPositionError(new BasicHep3Vector(0, 0, 0)); + vtxFit.setParameter("ndf", -9999.0); + return vtxFit; + } +} diff --git a/recon/src/main/java/org/hps/recon/vertexing/TrackConstraintVertexFitter.java b/recon/src/main/java/org/hps/recon/vertexing/TrackConstraintVertexFitter.java new file mode 100644 index 0000000000..1ac1f38e8b --- /dev/null +++ b/recon/src/main/java/org/hps/recon/vertexing/TrackConstraintVertexFitter.java @@ -0,0 +1,3542 @@ +package org.hps.recon.vertexing; + +import org.apache.commons.math3.linear.*; +import org.apache.commons.math3.util.FastMath; +import java.util.ArrayList; +import java.util.List; +import hep.physics.matrix.BasicMatrix; +import hep.physics.matrix.Matrix; +import hep.physics.matrix.MatrixOp; +import hep.physics.vec.BasicHep3Vector; +import hep.physics.vec.Hep3Vector; +import org.hps.recon.tracking.CoordinateTransformations; + + +/** + * Kalman Filter vertex fitter using Gain Matrix formalism + * Follows the approach of Frühwirth and Billoir for vertex fitting + */ +public class TrackConstraintVertexFitter { + + private double bField; + private RealVector vertex; + private RealMatrix vertexCov; + private double chi2; + private int ndf; + private List trackMomenta; + + /** + * Track parameters in perigee representation + */ + public static class TrackParams { + public double d0, phi0, omega, z0, tanLambda; + public RealMatrix cov; + + public TrackParams(double d0, double phi0, double omega, double z0, + double tanLambda, RealMatrix cov) { + this.d0 = d0; + this.phi0 = phi0; + this.omega = omega; + this.z0 = z0; + this.tanLambda = tanLambda; + this.cov = cov; + } + + /** Create a copy of this TrackParams */ + public TrackParams copy() { + return new TrackParams(d0, phi0, omega, z0, tanLambda, cov.copy()); + } + + /** Get parameters as array [d0, phi0, omega, z0, tanLambda] */ + public double[] toArray() { + return new double[]{d0, phi0, omega, z0, tanLambda}; + } + + /** Set parameters from array [d0, phi0, omega, z0, tanLambda] */ + public void fromArray(double[] params) { + this.d0 = params[0]; + this.phi0 = params[1]; + this.omega = params[2]; + this.z0 = params[3]; + this.tanLambda = params[4]; + } + } + + /** + * A straight line (zero-curvature "pseudo-track"), used to treat an already-fitted + * neutral V0's momentum direction as a track with no curvature in the cascade + * (V0 + recoil-track) vertex fit. Parameterized by a point on the line (x0,y0,z0) + * and a direction vector (dx,dy,dz) (need not be unit-normalized), plus the full + * 6x6 joint covariance of (x0,y0,z0,dx,dy,dz) -- e.g. the V0 vertex-position/momentum + * joint covariance, with no block-diagonal approximation. + */ + public static class LineParams { + public double x0, y0, z0; + public double dx, dy, dz; + public RealMatrix cov; + + public LineParams(double x0, double y0, double z0, double dx, double dy, double dz, RealMatrix cov) { + this.x0 = x0; + this.y0 = y0; + this.z0 = z0; + this.dx = dx; + this.dy = dy; + this.dz = dz; + this.cov = cov; + } + + /** Create a copy of this LineParams */ + public LineParams copy() { + return new LineParams(x0, y0, z0, dx, dy, dz, cov.copy()); + } + + /** Get parameters as array [x0, y0, z0, dx, dy, dz] */ + public double[] toArray() { + return new double[]{x0, y0, z0, dx, dy, dz}; + } + } + + /** + * Track momentum at vertex + */ + public static class TrackMomentum { + public RealVector p; + public RealMatrix pCov; + public double pt, pMag, theta, phi; + + public TrackMomentum(RealVector p, RealMatrix pCov) { + this.p = p; + this.pCov = pCov; + this.pt = FastMath.sqrt(p.getEntry(0) * p.getEntry(0) + + p.getEntry(1) * p.getEntry(1)); + this.pMag = p.getNorm(); + this.theta = FastMath.atan2(this.pt, p.getEntry(2)); + this.phi = FastMath.atan2(p.getEntry(1), p.getEntry(0)); + } + } + + /** + * Fit results + */ + public static class FitResult { + public RealVector vertex; + public RealMatrix vertexCov; + public double chi2; + public int ndf; + public List trackMomenta; + public List fittedTracks; // Fitted track parameters (for kinematic fit) + // Total (summed over all tracks) fitted 3-momentum and its covariance, correctly + // propagated through the FULL post-fit state covariance (including cross-track and + // vertex-momentum correlation blocks, not just each track's own diagonal block) -- + // only populated by fitSoftConstrained/fitLagrangeMultiplier; null otherwise. + public RealVector totalMomentum; + public RealMatrix totalMomentumCov; + + public FitResult(RealVector vertex, RealMatrix vertexCov, double chi2, + int ndf, List trackMomenta) { + this.vertex = vertex; + this.vertexCov = vertexCov; + this.chi2 = chi2; + this.ndf = ndf; + this.trackMomenta = trackMomenta; + this.fittedTracks = null; + } + + public FitResult(RealVector vertex, RealMatrix vertexCov, double chi2, + int ndf, List trackMomenta, List fittedTracks) { + this.vertex = vertex; + this.vertexCov = vertexCov; + this.chi2 = chi2; + this.ndf = ndf; + this.trackMomenta = trackMomenta; + this.fittedTracks = fittedTracks; + } + } + + /** + * Constraint representation: c = constraint residual, H = constraint matrix, V = covariance + */ + static class Constraint { + RealVector c; + RealMatrix H; + RealMatrix V; + + Constraint(RealVector c, RealMatrix H, RealMatrix V) { + this.c = c; + this.H = H; + this.V = V; + } + } + + public TrackConstraintVertexFitter(double bField) { + this.bField = bField; + } + + public TrackConstraintVertexFitter() { + this(2.0); + } + + /** + * Convert perigee to vertex parameters + */ + static class VertexParams { + double phiV, zV; + VertexParams(double phiV, double zV) { + this.phiV = phiV; + this.zV = zV; + } + } + + VertexParams perigeeToVertexParams(TrackParams track, double xV, double yV) { + double R = 1.0 / FastMath.abs(track.omega); + double sign = FastMath.signum(track.omega); + + double xc = sign * R * FastMath.sin(track.phi0) - track.d0 * FastMath.sin(track.phi0); + double yc = -sign * R * FastMath.cos(track.phi0) + track.d0 * FastMath.cos(track.phi0); + + double dx = xV - xc; + double dy = yV - yc; + + double phiV = FastMath.atan2(-dx * sign, dy * sign); + double dphi = phiV - track.phi0; + // Normalize dphi to (-π, π] to handle the atan2 branch cut. + // Positrons (phi0 ≈ π) have phiV wrap from just above π to just below -π, + // giving dphi ≈ -2π instead of the correct small positive value. + while (dphi > FastMath.PI) dphi -= 2.0 * FastMath.PI; + while (dphi < -FastMath.PI) dphi += 2.0 * FastMath.PI; + // Arc length along the helix from the perigee to (xV,yV): matches the canonical + // phi(s) = phi0 - s/R convention (org.lcsim TrackUtils/HelixUtils), i.e. + // s = -sign(omega)*R*dphi, NOT s = R*dphi -- the missing -sign flips the sign of + // z_v for every track with omega>0 relative to one with omega<0. + double s = -sign * R * dphi; + double zV = track.z0 + s * track.tanLambda; + return new VertexParams(phiV, zV); + } + + /** + * Compute track constraint using Gain Matrix formalism + * The track provides a constraint on where the vertex should be + */ + Constraint computeTrackConstraint(TrackParams track, RealVector vertex) { + double xV = vertex.getEntry(0); + double yV = vertex.getEntry(1); + double zV = vertex.getEntry(2); + + VertexParams vp = perigeeToVertexParams(track, xV, yV); + + // Compute H = dc/dvertex + double R = 1.0 / FastMath.abs(track.omega); + double sign = FastMath.signum(track.omega); + + double xc = sign * R * FastMath.sin(track.phi0) - track.d0 * FastMath.sin(track.phi0); + double yc = -sign * R * FastMath.cos(track.phi0) + track.d0 * FastMath.cos(track.phi0); + + double dx = xV - xc; + double dy = yV - yc; + double r2 = dx * dx + dy * dy; + double r = FastMath.sqrt(r2); + + // Constraint residuals: + // [0] transverse: distance from vertex to helix circle in XY plane + // [1] longitudinal: z at vertex vs z predicted from track + RealVector c = MatrixUtils.createRealVector(new double[]{r - R, zV - vp.zV}); + + // Derivatives for longitudinal constraint (via phi at vertex) + double dphiDx = -dy / r2; + double dphiDy = dx / r2; + double dzDx = -sign * R * track.tanLambda * dphiDx; + double dzDy = -sign * R * track.tanLambda * dphiDy; + + RealMatrix H = MatrixUtils.createRealMatrix(2, 3); + // Transverse: d(r-R)/d(xV,yV) = (dx/r, dy/r, 0) + H.setEntry(0, 0, dx / r); + H.setEntry(0, 1, dy / r); + H.setEntry(0, 2, 0.0); + // Longitudinal: unchanged + H.setEntry(1, 0, -dzDx); + H.setEntry(1, 1, -dzDy); + H.setEntry(1, 2, 1.0); + + // Propagate covariance + RealMatrix V = propagateTrackCovariance(track, xV, yV); + + return new Constraint(c, H, V); + } + + /** + * Propagate track covariance to constraint space + */ + private RealMatrix propagateTrackCovariance(TrackParams track, double xV, double yV) { + double R = 1.0 / FastMath.abs(track.omega); + double sign = FastMath.signum(track.omega); + + double xc = sign * R * FastMath.sin(track.phi0) - track.d0 * FastMath.sin(track.phi0); + double yc = -sign * R * FastMath.cos(track.phi0) + track.d0 * FastMath.cos(track.phi0); + + double dx = xV - xc; + double dy = yV - yc; + double r2 = dx * dx + dy * dy; + + double r = FastMath.sqrt(r2); + + // Derivatives of transverse constraint f_t = sqrt((xV-xc)^2+(yV-yc)^2) - R + // w.r.t. perigee parameters (xc, yc, and R all depend on d0/phi0/omega) + double dftDd0 = (dx * FastMath.sin(track.phi0) - dy * FastMath.cos(track.phi0)) / r; + double dftDphi0 = -(sign * R - track.d0) * (dx * FastMath.cos(track.phi0) + dy * FastMath.sin(track.phi0)) / r; + double dftDomega = (dx * FastMath.sin(track.phi0) - dy * FastMath.cos(track.phi0)) / (r * track.omega * track.omega) + + sign / (track.omega * track.omega); + + // Derivatives of phi_v w.r.t. perigee (still needed for z derivatives below) + double dphiDd0 = -(FastMath.cos(track.phi0) * dx + FastMath.sin(track.phi0) * dy) / r2; + double dphiDphi0 = (sign * R - track.d0) * (dy * FastMath.cos(track.phi0) - dx * FastMath.sin(track.phi0)) / r2; + double dphiDomega = -R * R * (FastMath.cos(track.phi0) * dx + FastMath.sin(track.phi0) * dy) / r2; + + double phiV = FastMath.atan2(-dx * sign, dy * sign); + double dphi = phiV - track.phi0; + while (dphi > FastMath.PI) dphi -= 2.0 * FastMath.PI; + while (dphi < -FastMath.PI) dphi += 2.0 * FastMath.PI; + double s = -sign * R * dphi; + + // Derivatives of z_v w.r.t. perigee. z_v = z0 + s*tanLambda with + // s = -sign(omega)*R*dphi, so each R*(dphi-derivative) term below picks up the + // same -sign factor; the explicit s/omega term (from d/domega of R itself) does not. + double dzDd0 = -sign * track.tanLambda * R * dphiDd0; + double dzDphi0 = -sign * (-track.tanLambda * R + track.tanLambda * R * dphiDphi0); + double dzDomega = -s * track.tanLambda / track.omega - sign * track.tanLambda * R * dphiDomega; + double dzDz0 = 1.0; + double dzDtl = s; + + RealMatrix J = MatrixUtils.createRealMatrix(2, 5); + J.setRow(0, new double[]{dftDd0, dftDphi0, dftDomega, 0.0, 0.0}); + J.setRow(1, new double[]{dzDd0, dzDphi0, dzDomega, dzDz0, dzDtl}); + + return J.multiply(track.cov).multiply(J.transpose()); + } + + /** + * Jacobian of {@link #computeTrackConstraint}'s 2-row residual (transverse {@code r-R}, + * longitudinal {@code zV-zPredicted}) with respect to the track's own 5 perigee + * parameters, at a fixed vertex {@code (xV,yV)} -- needed by + * {@link #fitCascadeVertexJointBeamConstrainedCore}, where each track's own parameters + * are free state variables rather than fixed inputs. Duplicates (rather than extracts) + * the algebra {@link #propagateTrackCovariance} already computes internally as its own + * local {@code J} before propagating it through the track covariance and discarding J + * itself, so that method and its existing callers stay untouched; the longitudinal row + * is negated relative to that internal {@code J} (matching the same negation + * {@link #fitSoftConstrained} applies to its own inline {@code rowZ} block), since the + * residual here is {@code zV-zPredicted}, not {@code zPredicted} itself. + */ + private RealMatrix trackResidualJacobianWrtTrackParams(TrackParams track, double xV, double yV) { + double R = 1.0 / FastMath.abs(track.omega); + double sign = FastMath.signum(track.omega); + + double xc = sign * R * FastMath.sin(track.phi0) - track.d0 * FastMath.sin(track.phi0); + double yc = -sign * R * FastMath.cos(track.phi0) + track.d0 * FastMath.cos(track.phi0); + + double dx = xV - xc; + double dy = yV - yc; + double r2 = dx * dx + dy * dy; + double r = FastMath.sqrt(r2); + + double dftDd0 = (dx * FastMath.sin(track.phi0) - dy * FastMath.cos(track.phi0)) / r; + double dftDphi0 = -(sign * R - track.d0) * (dx * FastMath.cos(track.phi0) + dy * FastMath.sin(track.phi0)) / r; + double dftDomega = (dx * FastMath.sin(track.phi0) - dy * FastMath.cos(track.phi0)) / (r * track.omega * track.omega) + + sign / (track.omega * track.omega); + + double dphiDd0 = -(FastMath.cos(track.phi0) * dx + FastMath.sin(track.phi0) * dy) / r2; + double dphiDphi0 = (sign * R - track.d0) * (dy * FastMath.cos(track.phi0) - dx * FastMath.sin(track.phi0)) / r2; + double dphiDomega = -R * R * (FastMath.cos(track.phi0) * dx + FastMath.sin(track.phi0) * dy) / r2; + + double phiV = FastMath.atan2(-dx * sign, dy * sign); + double dphi = phiV - track.phi0; + while (dphi > FastMath.PI) dphi -= 2.0 * FastMath.PI; + while (dphi < -FastMath.PI) dphi += 2.0 * FastMath.PI; + double s = -sign * R * dphi; + + double dzPredDd0 = -sign * track.tanLambda * R * dphiDd0; + double dzPredDphi0 = -sign * (-track.tanLambda * R + track.tanLambda * R * dphiDphi0); + double dzPredDomega = -s * track.tanLambda / track.omega - sign * track.tanLambda * R * dphiDomega; + + RealMatrix J = MatrixUtils.createRealMatrix(2, 5); + J.setRow(0, new double[]{dftDd0, dftDphi0, dftDomega, 0.0, 0.0}); + J.setRow(1, new double[]{-dzPredDd0, -dzPredDphi0, -dzPredDomega, -1.0, -s}); + return J; + } + + /** + * Deterministic orthonormal basis {u,v} spanning the plane perpendicular to unit + * vector n. Picks whichever of e_z/e_x is less parallel to n as the seed for + * Gram-Schmidt, so the construction is smooth except very near that switch point. + */ + private static RealVector[] perpendicularBasis(RealVector n) { + RealVector ref = (FastMath.abs(n.getEntry(2)) < 0.9) + ? MatrixUtils.createRealVector(new double[]{0, 0, 1}) + : MatrixUtils.createRealVector(new double[]{1, 0, 0}); + RealVector u = ref.subtract(n.mapMultiply(ref.dotProduct(n))); + u = u.mapDivide(u.getNorm()); + RealVector v = crossProduct(n, u); + return new RealVector[]{u, v}; + } + + private static RealVector crossProduct(RealVector a, RealVector b) { + return MatrixUtils.createRealVector(new double[]{ + a.getEntry(1) * b.getEntry(2) - a.getEntry(2) * b.getEntry(1), + a.getEntry(2) * b.getEntry(0) - a.getEntry(0) * b.getEntry(2), + a.getEntry(0) * b.getEntry(1) - a.getEntry(1) * b.getEntry(0) + }); + } + + /** + * Residual of a vertex position w.r.t. a line, expressed as the 2 components of + * (vertexGuess - x0) transverse to the line's direction. Zero iff vertexGuess lies + * exactly on the line. Written to take a raw 6-parameter array (rather than a + * LineParams) so it can be reused both for the analytic residual and for finite- + * differencing the Jacobian w.r.t. the line's own parameters below. + */ + private static RealVector lineResidual(double[] lineParams, RealVector vertexGuess) { + RealVector x0 = MatrixUtils.createRealVector(new double[]{lineParams[0], lineParams[1], lineParams[2]}); + RealVector d = MatrixUtils.createRealVector(new double[]{lineParams[3], lineParams[4], lineParams[5]}); + RealVector n = d.mapDivide(d.getNorm()); + RealVector[] basis = perpendicularBasis(n); + RealVector delta = vertexGuess.subtract(x0); + return MatrixUtils.createRealVector(new double[]{ + delta.dotProduct(basis[0]), delta.dotProduct(basis[1]) + }); + } + + /** + * Jacobian of lineResidual w.r.t. the line's own 6 parameters, at fixed vertexGuess, + * by central finite differences. An analytic Jacobian would need d(basis)/d(direction), + * which is gauge-dependent on the (deterministic but branch-y) perpendicular-basis + * construction above; finite-differencing the actual residual function sidesteps that + * while still correctly capturing how the chosen basis responds to changes in direction. + */ + private static RealMatrix lineResidualJacobianWrtLineParams(double[] lineParams, RealVector vertexGuess) { + RealMatrix J = MatrixUtils.createRealMatrix(2, 6); + for (int k = 0; k < 6; k++) { + double h = 1.0e-6 * FastMath.max(1.0, FastMath.abs(lineParams[k])); + double[] pPlus = lineParams.clone(); + double[] pMinus = lineParams.clone(); + pPlus[k] += h; + pMinus[k] -= h; + RealVector cPlus = lineResidual(pPlus, vertexGuess); + RealVector cMinus = lineResidual(pMinus, vertexGuess); + RealVector dcdp = cPlus.subtract(cMinus).mapDivide(2.0 * h); + J.setColumn(k, dcdp.toArray()); + } + return J; + } + + /** + * Compute the constraint that a vertex must lie on a given line (e.g. a V0's fitted + * momentum direction, treated as a zero-curvature pseudo-track). Mirrors + * computeTrackConstraint: H is the residual's Jacobian w.r.t. the vertex position, + * V is the line's own parameter covariance propagated into residual space. + */ + private Constraint computeLineConstraint(LineParams line, RealVector vertex) { + double[] lineParams = line.toArray(); + + RealVector c = lineResidual(lineParams, vertex); + + RealVector d = MatrixUtils.createRealVector(new double[]{line.dx, line.dy, line.dz}); + RealVector n = d.mapDivide(d.getNorm()); + RealVector[] basis = perpendicularBasis(n); + + RealMatrix H = MatrixUtils.createRealMatrix(2, 3); + H.setRow(0, basis[0].toArray()); + H.setRow(1, basis[1].toArray()); + + RealMatrix J = lineResidualJacobianWrtLineParams(lineParams, vertex); + RealMatrix V = J.multiply(line.cov).multiply(J.transpose()); + + return new Constraint(c, H, V); + } + + /** + * Compute momentum at vertex + */ + RealVector computeMomentumAtVertex(TrackParams track, RealVector vertex) { + VertexParams vp = perigeeToVertexParams(track, vertex.getEntry(0), vertex.getEntry(1)); + + // pT is a magnitude (R = pT/(C*|B|)), so it must use |bField|, not the signed field -- + // direction comes entirely from phiV/tanLambda. Using signed bField here silently + // flips every reconstructed momentum's direction whenever bField<0 (the real HPS field). + double pT = 2.99792458e-4 * FastMath.abs(bField) / FastMath.abs(track.omega); + double px = pT * FastMath.cos(vp.phiV); + double py = pT * FastMath.sin(vp.phiV); + double pz = pT * track.tanLambda; + + return MatrixUtils.createRealVector(new double[]{px, py, pz}); + } + + /** + * Compute d(momentum)/d(vertex) + */ + RealMatrix computeMomentumVertexDerivatives(TrackParams track, RealVector vertex) { + double xV = vertex.getEntry(0); + double yV = vertex.getEntry(1); + + double R = 1.0 / FastMath.abs(track.omega); + double sign = FastMath.signum(track.omega); + double pT = 2.99792458e-4 * FastMath.abs(bField) / FastMath.abs(track.omega); + + double xc = sign * R * FastMath.sin(track.phi0) - track.d0 * FastMath.sin(track.phi0); + double yc = -sign * R * FastMath.cos(track.phi0) + track.d0 * FastMath.cos(track.phi0); + + double dx = xV - xc; + double dy = yV - yc; + double r2 = dx * dx + dy * dy; + + VertexParams vp = perigeeToVertexParams(track, xV, yV); + + double dphiDx = -dy / r2; + double dphiDy = dx / r2; + + RealMatrix dpDv = MatrixUtils.createRealMatrix(3, 3); + dpDv.setEntry(0, 0, -pT * FastMath.sin(vp.phiV) * dphiDx); + dpDv.setEntry(0, 1, -pT * FastMath.sin(vp.phiV) * dphiDy); + dpDv.setEntry(0, 2, 0.0); + dpDv.setEntry(1, 0, pT * FastMath.cos(vp.phiV) * dphiDx); + dpDv.setEntry(1, 1, pT * FastMath.cos(vp.phiV) * dphiDy); + dpDv.setEntry(1, 2, 0.0); + dpDv.setEntry(2, 0, 0.0); + dpDv.setEntry(2, 1, 0.0); + dpDv.setEntry(2, 2, 0.0); + + return dpDv; + } + + /** + * Compute momentum covariance + */ + RealMatrix computeMomentumCovariance(TrackParams track, RealVector vertex) { + RealMatrix Jp = computeMomentumTrackJacobian(track, vertex); + return Jp.multiply(track.cov).multiply(Jp.transpose()); + } + + /** + * Raw momentum and covariance for a single track, with no vertex constraint applied + * -- evaluated at the track's own point of closest approach (arc length zero), which + * makes {@link #computeMomentumAtVertex} reduce to the direct px=pT*cos(phi0), + * py=pT*sin(phi0), pz=pT*tanLambda formula with no extrapolation. This is the "before + * any vertex fit" baseline used to compare against constrained/unconstrained + * vertex-fit momenta and their reported errors. Tracking frame. + */ + public TrackMomentum computeRawMomentum(TrackParams track) { + RealVector poca = MatrixUtils.createRealVector(new double[]{ + -track.d0 * FastMath.sin(track.phi0), track.d0 * FastMath.cos(track.phi0), track.z0}); + return new TrackMomentum(computeMomentumAtVertex(track, poca), computeMomentumCovariance(track, poca)); + } + + /** + * Jacobian d(momentum)/d(track parameters) [d0, phi0, omega, z0, tanLambda] at the given + * vertex, as a 3x5 matrix. Extracted out of {@link #computeMomentumCovariance} so the same + * per-track momentum Jacobian can be reused (alongside {@link #computeMomentumVertexDerivatives}) + * to build the full state-vector Jacobian needed for a correctly-correlated TOTAL momentum + * covariance (see the total-momentum computation in {@link #fitSoftConstrained}). + */ + RealMatrix computeMomentumTrackJacobian(TrackParams track, RealVector vertex) { + double xV = vertex.getEntry(0); + double yV = vertex.getEntry(1); + + VertexParams vp = perigeeToVertexParams(track, xV, yV); + + double R = 1.0 / FastMath.abs(track.omega); + double sign = FastMath.signum(track.omega); + double pT = 2.99792458e-4 * FastMath.abs(bField) / FastMath.abs(track.omega); + + double xc = sign * R * FastMath.sin(track.phi0) - track.d0 * FastMath.sin(track.phi0); + double yc = -sign * R * FastMath.cos(track.phi0) + track.d0 * FastMath.cos(track.phi0); + double dx = xV - xc; + double dy = yV - yc; + double r2 = dx * dx + dy * dy; + + double dphiDd0 = -(FastMath.cos(track.phi0) * dx + FastMath.sin(track.phi0) * dy) / r2; + double dphiDphi0 = (sign * R - track.d0) * (dy * FastMath.cos(track.phi0) - dx * FastMath.sin(track.phi0)) / r2; + double dphiDomega = -R * R * (FastMath.cos(track.phi0) * dx + FastMath.sin(track.phi0) * dy) / r2; + + double dpTDomega = -2.99792458e-4 * FastMath.abs(bField) * sign / (track.omega * track.omega); + + double dpxDd0 = -pT * FastMath.sin(vp.phiV) * dphiDd0; + double dpxDphi0 = -pT * FastMath.sin(vp.phiV) * dphiDphi0; + double dpxDomega = FastMath.cos(vp.phiV) * dpTDomega - pT * FastMath.sin(vp.phiV) * dphiDomega; + + double dpyDd0 = pT * FastMath.cos(vp.phiV) * dphiDd0; + double dpyDphi0 = pT * FastMath.cos(vp.phiV) * dphiDphi0; + double dpyDomega = FastMath.sin(vp.phiV) * dpTDomega + pT * FastMath.cos(vp.phiV) * dphiDomega; + + double dpzDomega = track.tanLambda * dpTDomega; + double dpzDtl = pT; + + RealMatrix Jp = MatrixUtils.createRealMatrix(3, 5); + Jp.setRow(0, new double[]{dpxDd0, dpxDphi0, dpxDomega, 0.0, 0.0}); + Jp.setRow(1, new double[]{dpyDd0, dpyDphi0, dpyDomega, 0.0, 0.0}); + Jp.setRow(2, new double[]{0.0, 0.0, dpzDomega, 0.0, dpzDtl}); + + return Jp; + } + + /** + * Full momentum covariance for a track evaluated at its associated (fitted) vertex, + * including the vertex-position uncertainty and its cross-covariance with the track's + * own parameters -- unlike {@link #computeMomentumCovariance}, which uses only the + * track's own diagonal covariance block and is a good approximation when the vertex's + * momentum-Jacobian ({@link #computeMomentumVertexDerivatives}) is negligible (true for + * high-pT tracks close to their reference vertex, but not necessarily true once the + * vertex itself carries significant, theta-lever-arm-inflated uncertainty -- e.g. the + * recoil track at V2 in {@link #fitCascadeVertexJointBeamConstrainedCore}, at large + * flight length). {@code vertexOffset}/{@code trackOffset} index into {@code cFitted}, + * the full post-fit state covariance, to pull out the associated 3x3/5x5/cross blocks. + */ + private RealMatrix computeMomentumCovarianceFull(TrackParams track, RealVector vertex, + RealMatrix cFitted, int vertexOffset, int trackOffset) { + RealMatrix Jv = computeMomentumVertexDerivatives(track, vertex); + RealMatrix Jp = computeMomentumTrackJacobian(track, vertex); + RealMatrix J = MatrixUtils.createRealMatrix(3, 8); + J.setSubMatrix(Jv.getData(), 0, 0); + J.setSubMatrix(Jp.getData(), 0, 3); + + RealMatrix cFull = MatrixUtils.createRealMatrix(8, 8); + cFull.setSubMatrix(cFitted.getSubMatrix(vertexOffset, vertexOffset + 2, vertexOffset, vertexOffset + 2).getData(), 0, 0); + cFull.setSubMatrix(cFitted.getSubMatrix(vertexOffset, vertexOffset + 2, trackOffset, trackOffset + 4).getData(), 0, 3); + cFull.setSubMatrix(cFitted.getSubMatrix(trackOffset, trackOffset + 4, vertexOffset, vertexOffset + 2).getData(), 3, 0); + cFull.setSubMatrix(cFitted.getSubMatrix(trackOffset, trackOffset + 4, trackOffset, trackOffset + 4).getData(), 3, 3); + + return J.multiply(cFull).multiply(J.transpose()); + } + + /** + * Result of {@link #fitCascadeVertexJoint}: a joint fit of an e-/e+ decay vertex (V1) + * and a separate V0(=e-+e+)/recoil production vertex (V2), linked by requiring the + * neutral V0 to fly in a straight line from V1 to V2. Unlike {@link #fitCascadeVertex}, + * which freezes the V0 as a fixed line, this lets V1, V2, and all three track momenta + * move jointly within their covariances. + */ + public static class TwoVertexFitResult { + public RealVector v1, v2; + public RealMatrix v1Cov, v2Cov; + public double chi2; + public int ndf; + public TrackMomentum eMinusMomentum, ePlusMomentum, recoilMomentum; + public List fittedTracks; // [eMinus, ePlus, recoil] + + public TwoVertexFitResult(RealVector v1, RealMatrix v1Cov, RealVector v2, RealMatrix v2Cov, + double chi2, int ndf, TrackMomentum eMinusMomentum, TrackMomentum ePlusMomentum, + TrackMomentum recoilMomentum, List fittedTracks) { + this.v1 = v1; + this.v1Cov = v1Cov; + this.v2 = v2; + this.v2Cov = v2Cov; + this.chi2 = chi2; + this.ndf = ndf; + this.eMinusMomentum = eMinusMomentum; + this.ePlusMomentum = ePlusMomentum; + this.recoilMomentum = recoilMomentum; + this.fittedTracks = fittedTracks; + } + } + + private static final int TVJ_OFF_V1 = 0; + private static final int TVJ_OFF_THETA = 3; + private static final int TVJ_OFF_V2 = 4; + private static final int TVJ_STATE_SIZE = 7; + private static final double MAX_STEP_NORM = 5.0; + + // State offsets for fitCascadeVertexJointBeamConstrainedCore: [V1(3), theta(1), V2(3), + // eMinus(5), ePlus(5), recoil(5)] -- the same V1/theta/V2 layout as TVJ_OFF_* above, + // extended with each track's own 5 perigee parameters as free state variables (see + // fitCascadeVertexJointBeamConstrainedCore javadoc for why that's needed). + private static final int CVJBC_OFF_V1 = 0; + private static final int CVJBC_OFF_THETA = 3; + private static final int CVJBC_OFF_V2 = 4; + private static final int CVJBC_OFF_EM = 7; + private static final int CVJBC_OFF_EP = 12; + private static final int CVJBC_OFF_RC = 17; + private static final int CVJBC_STATE_SIZE = 22; + + public static boolean DEBUG_JOINT_FIT = false; + + // The trust-region step cap (MAX_STEP_NORM) trades per-iteration progress for damping a + // badly-linearized first step, so it needs more outer iterations than the old, uncapped + // update to fully converge on real events -- confirmed empirically (the real-data + // regression case below needs ~40, not 20) -- hence 60 as this convenience overload's + // default rather than the smaller default used before the cap existed. + private static final int DEFAULT_TVJ_MAX_ITERATIONS = 60; + + /** A candidate (theta, V2) pair picked out of the transverse near/far branch ambiguity. */ + static class ThetaSeed { + final double theta; + final RealVector v2; + + ThetaSeed(double theta, RealVector v2) { + this.theta = theta; + this.v2 = v2; + } + } + + /** + * The (up to two) theta values where the line through {@code v1} in direction + * {@code pV0} (tracking-frame x,y bending plane) crosses the recoil track's own + * transverse helix circle (center xc,yc, radius R). Substituting the line + * {@code (xV,yV) = (v1x - theta*px, v1y - theta*py)} into the circle equation + * {@code (xV-xc)^2+(yV-yc)^2 = R^2} gives a quadratic in theta with discriminant + * {@code Δ/4 = |p_xy|^2 * (R^2 - d_perp^2)}, where {@code d_perp} is the + * perpendicular distance from the circle's center to the line -- so this returns + * {@code null} whenever that distance exceeds R (line misses the circle; no branch + * ambiguity to resolve) or the line has no transverse component at all. + */ + static double[] transverseCircleRoots(RealVector v1, RealVector pV0, TrackParams recoilTrack) { + double R = 1.0 / FastMath.abs(recoilTrack.omega); + double sign = FastMath.signum(recoilTrack.omega); + double xc = sign * R * FastMath.sin(recoilTrack.phi0) - recoilTrack.d0 * FastMath.sin(recoilTrack.phi0); + double yc = -sign * R * FastMath.cos(recoilTrack.phi0) + recoilTrack.d0 * FastMath.cos(recoilTrack.phi0); + + double px = pV0.getEntry(0); + double py = pV0.getEntry(1); + double a = px * px + py * py; + if (a < 1.0e-12) { + return null; + } + double ax = v1.getEntry(0) - xc; + double ay = v1.getEntry(1) - yc; + double b = -2.0 * (ax * px + ay * py); + double c = ax * ax + ay * ay - R * R; + + double disc = b * b - 4.0 * a * c; + if (disc < 0.0) { + return null; + } + double sq = FastMath.sqrt(disc); + return new double[]{(-b - sq) / (2.0 * a), (-b + sq) / (2.0 * a)}; + } + + /** + * Of the (up to two) theta roots where the V0 line through {@code v1} crosses the + * recoil track's own transverse circle ({@link #transverseCircleRoots}), pick + * whichever also matches the recoil track's own longitudinal (z) prediction -- + * see the class Javadoc on {@link #fitCascadeVertexJoint} for the near/far branch + * ambiguity this resolves. Returns {@code null} (defer to the caller's fallback + * theta) when the line misses the circle entirely. + */ + ThetaSeed selectPhysicalThetaSeed(RealVector v1, RealVector pV0, TrackParams recoilTrack) { + double[] roots = transverseCircleRoots(v1, pV0, recoilTrack); + if (roots == null) { + return null; + } + ThetaSeed best = null; + double bestResidual = Double.POSITIVE_INFINITY; + for (double theta : roots) { + RealVector v2 = v1.subtract(pV0.mapMultiply(theta)); + VertexParams vp = perigeeToVertexParams(recoilTrack, v2.getEntry(0), v2.getEntry(1)); + double residual = FastMath.abs(v2.getEntry(2) - vp.zV); + if (residual < bestResidual) { + bestResidual = residual; + best = new ThetaSeed(theta, v2); + } + } + return best; + } + + public TwoVertexFitResult fitCascadeVertexJoint(TrackParams eMinusIn, TrackParams ePlusIn, TrackParams recoilIn, + RealVector v1Init, RealVector v2Init) { + return fitCascadeVertexJoint(eMinusIn, ePlusIn, recoilIn, v1Init, v2Init, null, null, DEFAULT_TVJ_MAX_ITERATIONS, 1.0e-8); + } + + /** + * As above, but with explicit V1/V2 prior covariances -- e.g. the already-fitted V0's own + * vertex covariance for V1, and that V0's line propagated to the target plane + * ({@link #propagateLineToPlane}) for V2, instead of the flat/beamspot defaults below. Pass + * {@code null} for either to fall back to this class's original default for that vertex. + */ + public TwoVertexFitResult fitCascadeVertexJoint(TrackParams eMinusIn, TrackParams ePlusIn, TrackParams recoilIn, + RealVector v1Init, RealVector v2Init, RealMatrix v1Cov, RealMatrix v2Cov) { + return fitCascadeVertexJoint(eMinusIn, ePlusIn, recoilIn, v1Init, v2Init, v1Cov, v2Cov, DEFAULT_TVJ_MAX_ITERATIONS, 1.0e-8); + } + + /** + * Joint two-vertex fit: e-/e+ decay vertex V1 and V0/recoil production vertex V2, + * linked by a straight-line-flight collinearity constraint. Unlike the earlier + * Newton-Raphson/KKT formulation (which represented that constraint as a symmetric, + * direction-blind projection of (V2-V1) onto the plane perpendicular to + * p_v0(V1) = p_e-(V1) + p_e+(V1), and consequently let the solver converge to a + * physically impossible branch -- V1 on the wrong side of the production point -- in + * roughly half of truth-matched candidates), this follows Hulsbergen's decay-chain + * Kalman filter (NIM A552 (2005) 566-575, Eq. 33): the flight from V1 to V2 gets an + * explicit, signed decay-length parameter theta = l/|p|, related to the two vertices + * by the *exact* constraint g(V1,theta,V2) = V2 - V1 + theta*p_v0(V1) = 0. Because + * theta carries its own sign, prior, and row in the covariance, the fit never has to + * implicitly "discover" which side of V1 the production vertex sits on; a good initial + * guess plus a weak prior on theta is enough to pin the correct branch. + * + * State is the 7-vector [V1(0-2), theta(3), V2(4-6)]; the eMinus/ePlus/recoil track + * parameters are held fixed at their input values throughout (momenta are read off + * those fixed tracks at the fitted vertices via {@link #computeMomentumAtVertex}, same + * convention as {@link #fitCascadeVertex}) -- there is no + * 21-dim track sub-state to fit. Each outer iteration applies four constraints in + * sequence via a single Kalman-gain update shared by measurement-type constraints + * (track-at-vertex, Eq. 7-19) and the exact geometric constraint (Eq. 24-31, which the + * paper notes -- after Eq. 32 -- is simply the V->0 limit of the measurement update): + * eMinus-at-V1, ePlus-at-V1, recoil-at-V2, then the geometric decay-length constraint. + * As in {@link #fitCascadeVertex}, the covariance C is reset to its prior each outer + * pass (the four constraints are the same fixed measurements being re-linearized + * around a better expansion point, not new independent data), and the update uses the + * numerically-stable Joseph-form covariance update throughout. + * + * @param v2Init initial guess for the production vertex; if null, defaults to this + * fitter's own {@code beamPosition} field (tracking frame), matching the + * convention used as the initial vertex guess in {@link #fitVertex}. + */ + public TwoVertexFitResult fitCascadeVertexJoint(TrackParams eMinusIn, TrackParams ePlusIn, TrackParams recoilIn, + RealVector v1Init, RealVector v2Init, int maxIterations, double tolerance) { + return fitCascadeVertexJoint(eMinusIn, ePlusIn, recoilIn, v1Init, v2Init, null, null, maxIterations, tolerance); + } + + /** + * As above, but with explicit V1/V2 prior covariances (nullable -- {@code null} falls back + * to this class's original flat/beamspot default for that vertex). + */ + public TwoVertexFitResult fitCascadeVertexJoint(TrackParams eMinusIn, TrackParams ePlusIn, TrackParams recoilIn, + RealVector v1Init, RealVector v2Init, RealMatrix v1CovIn, RealMatrix v2CovIn, + int maxIterations, double tolerance) { + + TrackParams eMinus = eMinusIn.copy(); + TrackParams ePlus = ePlusIn.copy(); + TrackParams recoil = recoilIn.copy(); + + // V2 -- the production vertex -- is genuinely known to sit at the beamspot when the + // caller doesn't supply its own guess (the case used by CascadeVertexer); V1 -- + // the decay vertex -- is not otherwise constrained and is purely determined by the + // eMinus-/ePlus-at-V1 track constraints, so it always gets a weak/flat prior. + boolean useBeamspotPriorForV2 = (v2Init == null); + RealVector v2InitVec = useBeamspotPriorForV2 ? MatrixUtils.createRealVector(beamPosition) : v2Init; + + // Seed theta as the least-squares projection of the initial vertex separation onto + // the initial flight direction -- this gives the fit the right *sign* immediately + // from the existing v1Init/v2Init guesses, which is what actually fixes the branch + // ambiguity that the old symmetric collinearity residual couldn't resolve. Sign + // matches the constraint g = V2 - V1 + theta*p_v0(V1) = 0 solved for theta at the + // initial guess: theta = (V1Init-V2Init).pV0Init / |pV0Init|^2. + RealVector pV0Init = computeMomentumAtVertex(eMinus, v1Init).add(computeMomentumAtVertex(ePlus, v1Init)); + double pV0InitNormSq = pV0Init.dotProduct(pV0Init); + double thetaInit = (pV0InitNormSq > 0) + ? v1Init.subtract(v2InitVec).dotProduct(pV0Init) / pV0InitNormSq + : 0.0; + + // The V0 flight line through v1Init (direction pV0Init) generically crosses the + // recoil track's own transverse (bending-plane) circle at two points, not one -- + // a true near/far branch ambiguity, distinct from the least-squares projection + // above (which only fixes the sign relative to the *caller's* v2Init guess, and + // has no information about the recoil track at all). Newton can walk to either + // branch once iterating, self-consistently converging to a wrong-but-stable fixed + // point (large chi2, but no signal in the iteration itself to avoid it). Break the + // tie analytically before iterating: of the two transverse roots, keep whichever + // also matches the recoil track's own longitudinal (z) prediction -- the two + // branches generically disagree on that sharply, since z is fixed only by arc + // length along the recoil's own helix, independent of which transverse root was + // used to get there. When the line misses the circle entirely (no real root), + // there's no branch ambiguity to resolve -- the recoil-at-V2 constraint is a soft + // measurement, not exact, so the least-squares theta above already sits at the + // unique, well-posed closest-approach minimum. + ThetaSeed branchSeed = selectPhysicalThetaSeed(v1Init, pV0Init, recoil); + if (branchSeed != null) { + thetaInit = branchSeed.theta; + v2InitVec = branchSeed.v2; + } + + return fitCascadeVertexJointCore(eMinus, ePlus, recoil, v1Init, thetaInit, v2InitVec, + v1CovIn, v2CovIn, useBeamspotPriorForV2, maxIterations, tolerance); + } + + /** + * Run the joint fit from an explicitly forced (theta, V2) seed, bypassing + * {@link #selectPhysicalThetaSeed}'s own branch choice entirely -- lets a caller fit both + * {@link #transverseCircleRoots} candidates to convergence and choose between them using + * its own external discriminator, since neither chi2 nor selectPhysicalThetaSeed can tell + * the two branches apart (see {@link org.hps.recon.vertexing.CascadeVertexer#fit}, which + * picks between them by comparing each branch's V1 to the independently-fitted V0 vertex). + * Package-private: not part of the general-purpose API, only used alongside + * {@link org.hps.recon.vertexing.CascadeVertexer}. + */ + TwoVertexFitResult fitCascadeVertexJointForcedBranch(TrackParams eMinusIn, TrackParams ePlusIn, TrackParams recoilIn, + RealVector v1Init, double thetaInit, RealVector v2InitVec, RealMatrix v1CovIn, RealMatrix v2CovIn, + int maxIterations, double tolerance) { + TrackParams eMinus = eMinusIn.copy(); + TrackParams ePlus = ePlusIn.copy(); + TrackParams recoil = recoilIn.copy(); + return fitCascadeVertexJointCore(eMinus, ePlus, recoil, v1Init, thetaInit, v2InitVec, + v1CovIn, v2CovIn, false, maxIterations, tolerance); + } + + /** + * Free-track counterpart of {@link #fitCascadeVertexJointForcedBranch}: same explicitly- + * forced (theta, V2) seed, bypassing {@link #selectPhysicalThetaSeed}'s own branch choice, + * but dispatching to {@link #fitCascadeVertexJointFreeTrackCore} (all three tracks' own + * perigee parameters float under their own prior) instead of the fixed-track core. Needed + * because {@link #fitCascadeVertexJointFreeTrack}'s public wrapper unconditionally + * recomputes and substitutes {@code selectPhysicalThetaSeed}'s own (purely geometric) V2 + * whenever it returns non-null -- which silently discards any caller-supplied V2 prior mean + * distinct from that geometric branch point (e.g. {@link + * org.hps.recon.vertexing.CascadeVertexer}'s beamspot-position-constraint override). Since + * {@code fitCascadeVertexJointFreeTrackCore} anchors its Newton iteration's prior term at + * the literal {@code v2InitVec} passed in (a genuine persistent Bayesian prior, unlike the + * fixed-track core's {@code fitCascadeVertexJointCore} -- see the class-level discussion), + * this is the only way to actually deliver a caller-chosen V2 prior mean to that fit. + * Package-private: not part of the general-purpose API, only used alongside {@link + * org.hps.recon.vertexing.CascadeVertexer}. + */ + TwoVertexFitResult fitCascadeVertexJointFreeTrackForcedBranch(TrackParams eMinusIn, TrackParams ePlusIn, + TrackParams recoilIn, RealVector v1Init, double thetaInit, RealVector v2InitVec, + RealMatrix v1CovIn, RealMatrix v2CovIn, int maxIterations, double tolerance) { + TrackParams eMinus = eMinusIn.copy(); + TrackParams ePlus = ePlusIn.copy(); + TrackParams recoil = recoilIn.copy(); + return fitCascadeVertexJointFreeTrackCore(eMinus, ePlus, recoil, v1Init, thetaInit, v2InitVec, + v1CovIn, v2CovIn, false, maxIterations, tolerance); + } + + /** + * Beam-momentum-constrained counterpart of {@link #fitCascadeVertexJointFreeTrackForcedBranch} + * -- same rationale (bypass {@code selectPhysicalThetaSeed}'s silent V2-mean override so a + * caller-supplied V2 prior mean, e.g. a beamspot-position constraint, actually reaches the + * fit), but dispatching to {@link #fitCascadeVertexJointBeamConstrainedCore}. Package-private: + * not part of the general-purpose API, only used alongside {@link + * org.hps.recon.vertexing.CascadeVertexer}. + */ + TwoVertexFitResult fitCascadeVertexJointBeamConstrainedForcedBranch(TrackParams eMinusIn, TrackParams ePlusIn, + TrackParams recoilIn, RealVector v1Init, double thetaInit, RealVector v2InitVec, + RealMatrix v1CovIn, RealMatrix v2CovIn, int maxIterations, double tolerance) { + TrackParams eMinus = eMinusIn.copy(); + TrackParams ePlus = ePlusIn.copy(); + TrackParams recoil = recoilIn.copy(); + return fitCascadeVertexJointBeamConstrainedCore(eMinus, ePlus, recoil, v1Init, thetaInit, v2InitVec, + v1CovIn, v2CovIn, false, maxIterations, tolerance); + } + + private TwoVertexFitResult fitCascadeVertexJointCore(TrackParams eMinus, TrackParams ePlus, TrackParams recoil, + RealVector v1Init, double thetaInit, RealVector v2InitVec, RealMatrix v1CovIn, RealMatrix v2CovIn, + boolean useBeamspotPriorForV2, int maxIterations, double tolerance) { + + RealVector y0 = MatrixUtils.createRealVector(new double[TVJ_STATE_SIZE]); + y0.setSubVector(TVJ_OFF_V1, v1Init); + y0.setEntry(TVJ_OFF_THETA, thetaInit); + y0.setSubVector(TVJ_OFF_V2, v2InitVec); + + RealMatrix priorC = MatrixUtils.createRealMatrix(TVJ_STATE_SIZE, TVJ_STATE_SIZE); + if (v1CovIn != null) { + priorC.setSubMatrix(v1CovIn.getData(), TVJ_OFF_V1, TVJ_OFF_V1); + } else { + for (int i = 0; i < 3; i++) { + priorC.setEntry(TVJ_OFF_V1 + i, TVJ_OFF_V1 + i, 100.0); + } + } + priorC.setEntry(TVJ_OFF_THETA, TVJ_OFF_THETA, 100.0); + if (v2CovIn != null) { + priorC.setSubMatrix(v2CovIn.getData(), TVJ_OFF_V2, TVJ_OFF_V2); + } else { + for (int i = 0; i < 3; i++) { + double var = useBeamspotPriorForV2 ? beamSize[i] * beamSize[i] : 100.0; + priorC.setEntry(TVJ_OFF_V2 + i, TVJ_OFF_V2 + i, var); + } + } + + RealVector y = y0.copy(); + RealMatrix C = priorC.copy(); + + if (DEBUG_JOINT_FIT) { + System.err.println("JFDEBUG start y0=" + y0 + " thetaInit=" + thetaInit); + } + + // All four constraint blocks (eMinus-at-V1, ePlus-at-V1, recoil-at-V2, geometric) are + // stacked into a single simultaneous Kalman update per outer iteration, all evaluated + // at the same linearization point y. Applying them as separate sequential sub-steps + // (as tried previously) relinearizes the later sub-steps around whatever wildly wrong + // point the earlier, nearly-degenerate eMinus/ePlus-only V1 update jumped to -- since + // that 2-track-only sub-step is poorly conditioned for low-opening-angle pairs, it can + // walk V1 tens of mm away before the geometric constraint gets a chance to pull it + // back, and repeating that per iteration produces either a stable wrong fixed point or + // an undamped 2-cycle oscillation (both observed on real bad events). A single joint + // update conditions the near-degenerate track direction on the geometric constraint + // from the start of each pass, removing that failure mode. + final int TVJ_N_ROWS = 9; + for (int iteration = 0; iteration < maxIterations; iteration++) { + RealVector yOld = y.copy(); + C = priorC.copy(); + try { + RealVector v1 = y.getSubVector(TVJ_OFF_V1, 3); + double theta = y.getEntry(TVJ_OFF_THETA); + RealVector v2 = y.getSubVector(TVJ_OFF_V2, 3); + + Constraint cEm = computeTrackConstraint(eMinus, v1); + Constraint cEp = computeTrackConstraint(ePlus, v1); + Constraint cRc = computeTrackConstraint(recoil, v2); + GeometricConstraint gc = computeGeometricConstraint(v1, theta, v2, eMinus, ePlus); + + RealVector cStack = MatrixUtils.createRealVector(new double[TVJ_N_ROWS]); + cStack.setSubVector(0, cEm.c); + cStack.setSubVector(2, cEp.c); + cStack.setSubVector(4, cRc.c); + cStack.setSubVector(6, gc.g); + + RealMatrix HStack = MatrixUtils.createRealMatrix(TVJ_N_ROWS, TVJ_STATE_SIZE); + HStack.setSubMatrix(embed(cEm.H, TVJ_OFF_V1).getData(), 0, 0); + HStack.setSubMatrix(embed(cEp.H, TVJ_OFF_V1).getData(), 2, 0); + HStack.setSubMatrix(embed(cRc.H, TVJ_OFF_V2).getData(), 4, 0); + HStack.setSubMatrix(gc.H.getData(), 6, 0); + + // Geometric block (rows 6-8): carries gc.V, the eMinus/ePlus momentum-direction + // uncertainty propagated through the theta-scaled Jacobian (see + // computeGeometricConstraint) -- not an exact (V=0) constraint, since pV0(V1) + // has real uncertainty that must be reflected here or the update over-trusts + // this block and underestimates V1/V2's reported covariance. + RealMatrix VStack = MatrixUtils.createRealMatrix(TVJ_N_ROWS, TVJ_N_ROWS); + VStack.setSubMatrix(cEm.V.getData(), 0, 0); + VStack.setSubMatrix(cEp.V.getData(), 2, 2); + VStack.setSubMatrix(cRc.V.getData(), 4, 4); + VStack.setSubMatrix(gc.V.getData(), 6, 6); + + SequentialUpdate u = sequentialUpdate(y, C, cStack, HStack, VStack); + + // Trust-region cap: the geometric block is an exact (V=0) constraint, so + // nothing in the linear update damps a bad first step when the initial + // linearization is poor (near-degenerate eMinus/ePlus opening angle) -- + // observed concretely on real data as V1 leaping ~100 mm and flipping to + // the unphysical branch within iteration 0, then just self-consistently + // refining around that wrong point (large, non-vanishing track-at-vertex + // residuals alongside a near-zero geometric residual). Capping the full + // state step's norm forces the fit to re-linearize closer to its starting + // point instead of leaping on the first, least-trustworthy iteration. + RealVector step = u.y.subtract(y); + double stepNorm = step.getNorm(); + if (stepNorm > MAX_STEP_NORM) { + step = step.mapMultiply(MAX_STEP_NORM / stepNorm); + } + y = y.add(step); + C = u.C; + + if (DEBUG_JOINT_FIT) { + System.err.println("JFDEBUG it=" + iteration + " v1=" + y.getSubVector(TVJ_OFF_V1, 3) + + " theta=" + y.getEntry(TVJ_OFF_THETA) + " v2=" + y.getSubVector(TVJ_OFF_V2, 3) + + " |cEm|=" + cEm.c.getNorm() + " |cEp|=" + cEp.c.getNorm() + + " |cRc|=" + cRc.c.getNorm() + " |g|=" + gc.g.getNorm()); + } + + if (!Double.isFinite(y.getNorm())) { + y = yOld; + break; + } + if (y.subtract(yOld).getNorm() < tolerance) { + break; + } + } catch (Exception e) { + if (DEBUG_JOINT_FIT) { + System.err.println("JFDEBUG it=" + iteration + " EXCEPTION " + e); + } + y = yOld; + break; + } + } + + RealVector v1Final = y.getSubVector(TVJ_OFF_V1, 3); + double thetaFinal = y.getEntry(TVJ_OFF_THETA); + RealVector v2Final = y.getSubVector(TVJ_OFF_V2, 3); + + RealMatrix v1Cov = C.getSubMatrix(TVJ_OFF_V1, TVJ_OFF_V1 + 2, TVJ_OFF_V1, TVJ_OFF_V1 + 2); + RealMatrix v2Cov = C.getSubMatrix(TVJ_OFF_V2, TVJ_OFF_V2 + 2, TVJ_OFF_V2, TVJ_OFF_V2 + 2); + + // chi2/ndf: recompute each of the 4 constraint blocks at the converged state and + // sum r^T*Vblock^-1*r (Hulsbergen Eq. 19/31); ndf follows the standard + // measurements-minus-free-parameters convention used elsewhere in this class + // (fit(): ndf=2*nTracks-3; fitCascadeVertex(): ndf=1) -- 3 track constraints x 2 + // rows + 1 geometric constraint x 3 rows = 9 measurements, minus 7 free + // parameters = 2. + Constraint cEmFinal = computeTrackConstraint(eMinus, v1Final); + Constraint cEpFinal = computeTrackConstraint(ePlus, v1Final); + Constraint cRcFinal = computeTrackConstraint(recoil, v2Final); + GeometricConstraint gcFinal = computeGeometricConstraint(v1Final, thetaFinal, v2Final, eMinus, ePlus); + + double chi2 = 0.0; + chi2 += chi2Contribution(cEmFinal.c, cEmFinal.V); + chi2 += chi2Contribution(cEpFinal.c, cEpFinal.V); + chi2 += chi2Contribution(cRcFinal.c, cRcFinal.V); + chi2 += chi2Contribution(gcFinal.g, gcFinal.V); + + int ndf = 2; + + TrackMomentum eMinusMomentum = new TrackMomentum( + computeMomentumAtVertex(eMinus, v1Final), computeMomentumCovariance(eMinus, v1Final)); + TrackMomentum ePlusMomentum = new TrackMomentum( + computeMomentumAtVertex(ePlus, v1Final), computeMomentumCovariance(ePlus, v1Final)); + TrackMomentum recoilMomentum = new TrackMomentum( + computeMomentumAtVertex(recoil, v2Final), computeMomentumCovariance(recoil, v2Final)); + + List fittedTracks = new ArrayList<>(); + fittedTracks.add(eMinus); + fittedTracks.add(ePlus); + fittedTracks.add(recoil); + + return new TwoVertexFitResult(v1Final, v1Cov, v2Final, v2Cov, chi2, ndf, + eMinusMomentum, ePlusMomentum, recoilMomentum, fittedTracks); + } + + public TwoVertexFitResult fitCascadeVertexJointBeamConstrained(TrackParams eMinusIn, TrackParams ePlusIn, + TrackParams recoilIn, RealVector v1Init, RealVector v2Init) { + return fitCascadeVertexJointBeamConstrained(eMinusIn, ePlusIn, recoilIn, v1Init, v2Init, + null, null, DEFAULT_TVJ_MAX_ITERATIONS, 1.0e-8); + } + + /** As above, but with explicit V1/V2 prior covariances (nullable, same convention as {@link #fitCascadeVertexJoint}). */ + public TwoVertexFitResult fitCascadeVertexJointBeamConstrained(TrackParams eMinusIn, TrackParams ePlusIn, + TrackParams recoilIn, RealVector v1Init, RealVector v2Init, RealMatrix v1Cov, RealMatrix v2Cov) { + return fitCascadeVertexJointBeamConstrained(eMinusIn, ePlusIn, recoilIn, v1Init, v2Init, + v1Cov, v2Cov, DEFAULT_TVJ_MAX_ITERATIONS, 1.0e-8); + } + + /** + * Joint V1(e-/e+ decay)+V2(V0/recoil production) vertex fit with the daughters' total + * 3-momentum constrained (softly, weighted by the beam-momentum uncertainty from + * {@link #beamMomentumCovarianceMatrix}) to the beam value from + * {@link #beamMomentumVector} -- see {@link #fitCascadeVertexJointBeamConstrainedCore} + * for the fit itself. Seeding (theta least-squares projection, then the + * {@link #transverseCircleRoots}/{@link #selectPhysicalThetaSeed} branch override) is + * identical to, and duplicated (not shared) from, {@link #fitCascadeVertexJoint} so + * that method's body stays untouched. + */ + public TwoVertexFitResult fitCascadeVertexJointBeamConstrained(TrackParams eMinusIn, TrackParams ePlusIn, + TrackParams recoilIn, RealVector v1Init, RealVector v2Init, RealMatrix v1CovIn, RealMatrix v2CovIn, + int maxIterations, double tolerance) { + + TrackParams eMinus = eMinusIn.copy(); + TrackParams ePlus = ePlusIn.copy(); + TrackParams recoil = recoilIn.copy(); + + boolean useBeamspotPriorForV2 = (v2Init == null); + RealVector v2InitVec = useBeamspotPriorForV2 ? MatrixUtils.createRealVector(beamPosition) : v2Init; + + RealVector pV0Init = computeMomentumAtVertex(eMinus, v1Init).add(computeMomentumAtVertex(ePlus, v1Init)); + double pV0InitNormSq = pV0Init.dotProduct(pV0Init); + double thetaInit = (pV0InitNormSq > 0) + ? v1Init.subtract(v2InitVec).dotProduct(pV0Init) / pV0InitNormSq + : 0.0; + + ThetaSeed branchSeed = selectPhysicalThetaSeed(v1Init, pV0Init, recoil); + if (branchSeed != null) { + thetaInit = branchSeed.theta; + v2InitVec = branchSeed.v2; + } + + return fitCascadeVertexJointBeamConstrainedCore(eMinus, ePlus, recoil, v1Init, thetaInit, v2InitVec, + v1CovIn, v2CovIn, useBeamspotPriorForV2, maxIterations, tolerance); + } + + /** + * As {@link #fitCascadeVertexJointCore}, but lets each of the three tracks' own 5 + * perigee parameters float as free state variables (weighted by that track's own input + * covariance), rather than holding them fixed at their input values -- required for a + * beam-momentum constraint to have any real effect on this fit: momentum magnitude + * depends only on {@code omega}, which the fixed-track formalism never adjusts (only + * momentum *direction* varies, via the moving vertex), so a momentum constraint bolted + * onto that state could never actually pull momentum toward the beam value. Mirrors + * {@link #fitSoftConstrained}'s approach for the flat N-track case. + * + *

State is the 22-vector {@code [V1(0-2), theta(3), V2(4-6), eMinus(7-11), + * ePlus(12-16), recoil(17-21)]} (see {@code CVJBC_OFF_*}). Solved via the same + * KKT/Newton-Raphson block-elimination {@link #fitSoftConstrained} uses (not the + * sequential Joseph-form Kalman update {@link #fitCascadeVertexJointCore} uses), since + * that formalism has no mechanism for a measurement's own parameters to be free state + * variables with a prior. Beam momentum/covariance come from this fitter's own + * {@code pBeam}/{@code rotAngle}/{@code sigmaTNuclearRecoil} fields via + * {@link #beamMomentumVector}/{@link #beamMomentumCovarianceMatrix}, matching how + * {@link NTrackVertexer#fitVertexBeamConstrained} already works. + * + *

Twelve constraint rows: eMinus-at-V1 (2), ePlus-at-V1 (2), recoil-at-V2 (2) -- + * {@link #computeTrackConstraint}'s residual/vertex-Jacobian plus a track-parameter + * Jacobian block ({@link #trackResidualJacobianWrtTrackParams}); the geometric + * decay-length constraint (3, {@link #computeGeometricConstraint}'s residual/V1-theta-V2 + * columns -- which happen to line up exactly with {@code CVJBC_OFF_V1/THETA/V2} since + * both use the same 0/3/4 offsets -- plus new eMinus/ePlus track-parameter columns + * {@code theta * }{@link #computeMomentumTrackJacobian}, since {@code p_v0(V1)} now also + * depends on those tracks' own free parameters); and the beam-momentum constraint (3, + * {@code pEm(V1)+pEp(V1)+pRc(V2)-pBeam=0}, with V1/V2 columns via + * {@link #computeMomentumVertexDerivatives} and per-track columns via + * {@link #computeMomentumTrackJacobian}, each evaluated at that track's own vertex). + * + *

Track-at-vertex and geometric blocks are regularized only by a tiny fixed epsilon, + * not by {@code computeTrackConstraint}'s/{@code computeGeometricConstraint}'s own + * {@code V} -- since each track's own parameters now carry a real prior via {@code W}, + * softening these blocks with a track-covariance-derived {@code V} would double-count + * that same covariance a second time (identical reasoning already documented in + * {@link #fitSoftConstrained}). The beam-momentum block gets the real + * {@link #beamMomentumCovarianceMatrix}. {@code ndf = nConstraints(12) - 7} (7 = + * V1+theta+V2, the only state components with a free/weak prior; track params have a + * real prior via {@code W} so don't count) {@code = 5}. + */ + private TwoVertexFitResult fitCascadeVertexJointBeamConstrainedCore( + TrackParams eMinus, TrackParams ePlus, TrackParams recoil, + RealVector v1Init, double thetaInit, RealVector v2InitVec, + RealMatrix v1CovIn, RealMatrix v2CovIn, boolean useBeamspotPriorForV2, + int maxIterations, double tolerance) { + + final int N_TRACK_ROWS = 6; + final int N_GEOM_ROWS = 3; + final int N_MOM_ROWS = 3; + final int nConstraints = N_TRACK_ROWS + N_GEOM_ROWS + N_MOM_ROWS; + + RealVector x0 = MatrixUtils.createRealVector(new double[CVJBC_STATE_SIZE]); + x0.setSubVector(CVJBC_OFF_V1, v1Init); + x0.setEntry(CVJBC_OFF_THETA, thetaInit); + x0.setSubVector(CVJBC_OFF_V2, v2InitVec); + x0.setSubVector(CVJBC_OFF_EM, MatrixUtils.createRealVector(eMinus.toArray())); + x0.setSubVector(CVJBC_OFF_EP, MatrixUtils.createRealVector(ePlus.toArray())); + x0.setSubVector(CVJBC_OFF_RC, MatrixUtils.createRealVector(recoil.toArray())); + + RealMatrix W = MatrixUtils.createRealMatrix(CVJBC_STATE_SIZE, CVJBC_STATE_SIZE); + RealMatrix WInv = MatrixUtils.createRealMatrix(CVJBC_STATE_SIZE, CVJBC_STATE_SIZE); + if (v1CovIn != null) { + RealMatrix v1CovInv = new LUDecomposition(v1CovIn).getSolver().getInverse(); + W.setSubMatrix(v1CovInv.getData(), CVJBC_OFF_V1, CVJBC_OFF_V1); + WInv.setSubMatrix(v1CovIn.getData(), CVJBC_OFF_V1, CVJBC_OFF_V1); + } else { + for (int i = 0; i < 3; i++) { + W.setEntry(CVJBC_OFF_V1 + i, CVJBC_OFF_V1 + i, 0.01); + WInv.setEntry(CVJBC_OFF_V1 + i, CVJBC_OFF_V1 + i, 100.0); + } + } + W.setEntry(CVJBC_OFF_THETA, CVJBC_OFF_THETA, 0.01); + WInv.setEntry(CVJBC_OFF_THETA, CVJBC_OFF_THETA, 100.0); + if (v2CovIn != null) { + RealMatrix v2CovInv = new LUDecomposition(v2CovIn).getSolver().getInverse(); + W.setSubMatrix(v2CovInv.getData(), CVJBC_OFF_V2, CVJBC_OFF_V2); + WInv.setSubMatrix(v2CovIn.getData(), CVJBC_OFF_V2, CVJBC_OFF_V2); + } else { + for (int i = 0; i < 3; i++) { + double var = useBeamspotPriorForV2 ? beamSize[i] * beamSize[i] : 100.0; + W.setEntry(CVJBC_OFF_V2 + i, CVJBC_OFF_V2 + i, 1.0 / var); + WInv.setEntry(CVJBC_OFF_V2 + i, CVJBC_OFF_V2 + i, var); + } + } + + RealMatrix[] trackCovs = {eMinus.cov, ePlus.cov, recoil.cov}; + int[] trackOffsets = {CVJBC_OFF_EM, CVJBC_OFF_EP, CVJBC_OFF_RC}; + for (int t = 0; t < 3; t++) { + RealMatrix covInv; + try { + covInv = new LUDecomposition(trackCovs[t]).getSolver().getInverse(); + } catch (SingularMatrixException e) { + return null; // Cannot build weight matrix; skip this event silently + } + W.setSubMatrix(covInv.getData(), trackOffsets[t], trackOffsets[t]); + WInv.setSubMatrix(trackCovs[t].getData(), trackOffsets[t], trackOffsets[t]); + } + + RealVector pBeamVec = beamMomentumVector(); + RealMatrix beamCov = beamMomentumCovarianceMatrix(); + + RealVector x = x0.copy(); + RealMatrix finalH = null; + RealMatrix finalV = null; + RealVector finalHvec = null; + + for (int iteration = 0; iteration < maxIterations; iteration++) { + RealVector xOld = x.copy(); + + RealVector v1 = x.getSubVector(CVJBC_OFF_V1, 3); + double theta = x.getEntry(CVJBC_OFF_THETA); + RealVector v2 = x.getSubVector(CVJBC_OFF_V2, 3); + eMinus.fromArray(x.getSubVector(CVJBC_OFF_EM, 5).toArray()); + ePlus.fromArray(x.getSubVector(CVJBC_OFF_EP, 5).toArray()); + recoil.fromArray(x.getSubVector(CVJBC_OFF_RC, 5).toArray()); + + try { + RealVector h = MatrixUtils.createRealVector(new double[nConstraints]); + RealMatrix H = MatrixUtils.createRealMatrix(nConstraints, CVJBC_STATE_SIZE); + + Constraint cEm = computeTrackConstraint(eMinus, v1); + Constraint cEp = computeTrackConstraint(ePlus, v1); + Constraint cRc = computeTrackConstraint(recoil, v2); + h.setSubVector(0, cEm.c); + h.setSubVector(2, cEp.c); + h.setSubVector(4, cRc.c); + H.setSubMatrix(cEm.H.getData(), 0, CVJBC_OFF_V1); + H.setSubMatrix(trackResidualJacobianWrtTrackParams(eMinus, v1.getEntry(0), v1.getEntry(1)).getData(), + 0, CVJBC_OFF_EM); + H.setSubMatrix(cEp.H.getData(), 2, CVJBC_OFF_V1); + H.setSubMatrix(trackResidualJacobianWrtTrackParams(ePlus, v1.getEntry(0), v1.getEntry(1)).getData(), + 2, CVJBC_OFF_EP); + H.setSubMatrix(cRc.H.getData(), 4, CVJBC_OFF_V2); + H.setSubMatrix(trackResidualJacobianWrtTrackParams(recoil, v2.getEntry(0), v2.getEntry(1)).getData(), + 4, CVJBC_OFF_RC); + + GeometricConstraint gc = computeGeometricConstraint(v1, theta, v2, eMinus, ePlus); + h.setSubVector(6, gc.g); + // gc.H's columns are laid out [V1(0-2), theta(3), V2(4-6)] (TVJ_OFF_*), which + // is bit-for-bit the same layout as CVJBC_OFF_V1/THETA/V2 -- copy directly. + H.setSubMatrix(gc.H.getData(), 6, 0); + RealMatrix dgdEm = computeMomentumTrackJacobian(eMinus, v1).scalarMultiply(theta); + RealMatrix dgdEp = computeMomentumTrackJacobian(ePlus, v1).scalarMultiply(theta); + H.setSubMatrix(dgdEm.getData(), 6, CVJBC_OFF_EM); + H.setSubMatrix(dgdEp.getData(), 6, CVJBC_OFF_EP); + + RealVector pEmV1 = computeMomentumAtVertex(eMinus, v1); + RealVector pEpV1 = computeMomentumAtVertex(ePlus, v1); + RealVector pRcV2 = computeMomentumAtVertex(recoil, v2); + RealVector momResidual = pEmV1.add(pEpV1).add(pRcV2).subtract(pBeamVec); + h.setSubVector(9, momResidual); + RealMatrix dPdV1 = computeMomentumVertexDerivatives(eMinus, v1).add(computeMomentumVertexDerivatives(ePlus, v1)); + RealMatrix dPdV2 = computeMomentumVertexDerivatives(recoil, v2); + H.setSubMatrix(dPdV1.getData(), 9, CVJBC_OFF_V1); + H.setSubMatrix(dPdV2.getData(), 9, CVJBC_OFF_V2); + H.setSubMatrix(computeMomentumTrackJacobian(eMinus, v1).getData(), 9, CVJBC_OFF_EM); + H.setSubMatrix(computeMomentumTrackJacobian(ePlus, v1).getData(), 9, CVJBC_OFF_EP); + H.setSubMatrix(computeMomentumTrackJacobian(recoil, v2).getData(), 9, CVJBC_OFF_RC); + + RealMatrix HWInvHT = H.multiply(WInv).multiply(H.transpose()); + // Regularization for the track-at-vertex/geometric rows, which are otherwise + // exact (V=0) equality constraints once each track's own parameters carry a + // real prior via W: must be several orders of magnitude smaller than each row's + // own H*WInv*H^T diagonal entry, or it materially over-softens that row and + // inflates the reported posterior covariance -- empirically, 1e-6*diag (the + // naive choice) already inflates sigma(V1x) by ~35% relative to the true + // (toy-MC) scatter; 1e-9 reproduces the correct, well-calibrated covariance. + // Scaled PER-ROW (not by a single averaged diagScale across all 9 rows) because + // the geometric row's diagonal grows with theta^2 (dgdEm/dgdEp scale with theta) + // while the track-at-vertex rows' diagonal does not -- pooling into one average + // let the geometric row's growth drag up the track rows' epsilon at large theta, + // over-softening the recoil-at-V2 row and inflating its reported momentum + // covariance (caught via the recoil-track Py pull std degrading from ~0.97 to + // ~0.79 as flight length grew from 5 to 150 mm in the toy-MC pull test). + RealMatrix constraintCov = MatrixUtils.createRealMatrix(nConstraints, nConstraints); + for (int i = 0; i < N_TRACK_ROWS + N_GEOM_ROWS; i++) { + double diagI = HWInvHT.getEntry(i, i); + double epsilonI = (diagI > 0) ? diagI * 1e-9 : 1e-12; + constraintCov.setEntry(i, i, epsilonI); + } + constraintCov.setSubMatrix(beamCov.getData(), N_TRACK_ROWS + N_GEOM_ROWS, N_TRACK_ROWS + N_GEOM_ROWS); + + finalH = H; + finalV = constraintCov; + finalHvec = h; + + RealMatrix S = HWInvHT.add(constraintCov); + RealVector rhs = h.add(H.operate(x0.subtract(x))); + RealVector lambda = new LUDecomposition(S).getSolver().solve(rhs); + RealVector deltaX = x0.subtract(x).subtract(WInv.multiply(H.transpose()).operate(lambda)); + + double stepNorm = deltaX.getNorm(); + if (stepNorm > MAX_STEP_NORM) { + deltaX = deltaX.mapMultiply(MAX_STEP_NORM / stepNorm); + } + x = x.add(deltaX); + + if (!Double.isFinite(x.getNorm())) { + x = xOld; + break; + } + if (deltaX.getNorm() < tolerance && h.getNorm() < tolerance * 10) { + break; + } + } catch (Exception e) { + x = xOld; + break; + } + } + + RealVector v1Final = x.getSubVector(CVJBC_OFF_V1, 3); + RealVector v2Final = x.getSubVector(CVJBC_OFF_V2, 3); + eMinus.fromArray(x.getSubVector(CVJBC_OFF_EM, 5).toArray()); + ePlus.fromArray(x.getSubVector(CVJBC_OFF_EP, 5).toArray()); + recoil.fromArray(x.getSubVector(CVJBC_OFF_RC, 5).toArray()); + + RealMatrix C_fitted = WInv.copy(); + if (finalH != null) { + try { + RealMatrix S = finalH.multiply(WInv).multiply(finalH.transpose()).add(finalV); + RealMatrix K = WInv.multiply(finalH.transpose()) + .multiply(new LUDecomposition(S).getSolver().getInverse()); + C_fitted = WInv.subtract(K.multiply(finalH).multiply(WInv)); + } catch (Exception e) { + // keep prior WInv as a fallback post-fit covariance + } + } + + eMinus.cov = C_fitted.getSubMatrix(CVJBC_OFF_EM, CVJBC_OFF_EM + 4, CVJBC_OFF_EM, CVJBC_OFF_EM + 4); + ePlus.cov = C_fitted.getSubMatrix(CVJBC_OFF_EP, CVJBC_OFF_EP + 4, CVJBC_OFF_EP, CVJBC_OFF_EP + 4); + recoil.cov = C_fitted.getSubMatrix(CVJBC_OFF_RC, CVJBC_OFF_RC + 4, CVJBC_OFF_RC, CVJBC_OFF_RC + 4); + + RealMatrix v1Cov = C_fitted.getSubMatrix(CVJBC_OFF_V1, CVJBC_OFF_V1 + 2, CVJBC_OFF_V1, CVJBC_OFF_V1 + 2); + RealMatrix v2Cov = C_fitted.getSubMatrix(CVJBC_OFF_V2, CVJBC_OFF_V2 + 2, CVJBC_OFF_V2, CVJBC_OFF_V2 + 2); + + RealVector dx = x.subtract(x0); + double chi2 = dx.dotProduct(W.operate(dx)); + if (finalHvec != null && finalV != null) { + chi2 += chi2Contribution(finalHvec.getSubVector(0, 2), finalV.getSubMatrix(0, 1, 0, 1)); + chi2 += chi2Contribution(finalHvec.getSubVector(2, 2), finalV.getSubMatrix(2, 3, 2, 3)); + chi2 += chi2Contribution(finalHvec.getSubVector(4, 2), finalV.getSubMatrix(4, 5, 4, 5)); + chi2 += chi2Contribution(finalHvec.getSubVector(6, 3), finalV.getSubMatrix(6, 8, 6, 8)); + chi2 += chi2Contribution(finalHvec.getSubVector(9, 3), finalV.getSubMatrix(9, 11, 9, 11)); + } + + int ndf = nConstraints - 7; + + // Momentum covariance must be computed from a "measurement-only" sandwich + // C_fitted*Wtrk*C_fitted, NOT C_fitted's own [V2,recoil] sub-block directly: C_fitted + // is the Bayesian posterior covariance of the full state under the model "x0 ~ + // N(x0, WInv)" for EVERY state component, including V1/theta/V2's weak/uninformative + // prior (var=100) -- but that prior's x0 (the caller's seed) is never actually + // randomized; only the 3 tracks' own parameters carry real (measurement) noise. Using + // C_fitted directly therefore double-counts the fake vertex/theta prior as if it were + // real randomness, which is propagated (via the geometric constraint's lever arm) + // into an over-estimated recoil-momentum covariance that grows with flight length + // (verified: at flightLength=150mm this inflated the reported recoil-Py sigma by + // ~21% relative to both a finite-difference sensitivity check and the toy-MC's + // empirical residual scatter). Sandwiching with Wtrk -- W with the V1/theta/V2 block + // zeroed, keeping only the 3 tracks' own (real) weight blocks -- exactly reproduces + // the finite-difference ground truth (confirmed to 3 significant figures). + RealMatrix Wtrk = MatrixUtils.createRealMatrix(CVJBC_STATE_SIZE, CVJBC_STATE_SIZE); + Wtrk.setSubMatrix(W.getSubMatrix(CVJBC_OFF_EM, CVJBC_OFF_EM + 4, CVJBC_OFF_EM, CVJBC_OFF_EM + 4).getData(), + CVJBC_OFF_EM, CVJBC_OFF_EM); + Wtrk.setSubMatrix(W.getSubMatrix(CVJBC_OFF_EP, CVJBC_OFF_EP + 4, CVJBC_OFF_EP, CVJBC_OFF_EP + 4).getData(), + CVJBC_OFF_EP, CVJBC_OFF_EP); + Wtrk.setSubMatrix(W.getSubMatrix(CVJBC_OFF_RC, CVJBC_OFF_RC + 4, CVJBC_OFF_RC, CVJBC_OFF_RC + 4).getData(), + CVJBC_OFF_RC, CVJBC_OFF_RC); + RealMatrix C_meas = C_fitted.multiply(Wtrk).multiply(C_fitted); + + TrackMomentum eMinusMomentum = new TrackMomentum(computeMomentumAtVertex(eMinus, v1Final), + computeMomentumCovarianceFull(eMinus, v1Final, C_meas, CVJBC_OFF_V1, CVJBC_OFF_EM)); + TrackMomentum ePlusMomentum = new TrackMomentum(computeMomentumAtVertex(ePlus, v1Final), + computeMomentumCovarianceFull(ePlus, v1Final, C_meas, CVJBC_OFF_V1, CVJBC_OFF_EP)); + TrackMomentum recoilMomentum = new TrackMomentum(computeMomentumAtVertex(recoil, v2Final), + computeMomentumCovarianceFull(recoil, v2Final, C_meas, CVJBC_OFF_V2, CVJBC_OFF_RC)); + + List fittedTracks = new ArrayList<>(); + fittedTracks.add(eMinus); + fittedTracks.add(ePlus); + fittedTracks.add(recoil); + + return new TwoVertexFitResult(v1Final, v1Cov, v2Final, v2Cov, chi2, ndf, + eMinusMomentum, ePlusMomentum, recoilMomentum, fittedTracks); + } + + public TwoVertexFitResult fitCascadeVertexJointFreeTrack(TrackParams eMinusIn, TrackParams ePlusIn, + TrackParams recoilIn, RealVector v1Init, RealVector v2Init) { + return fitCascadeVertexJointFreeTrack(eMinusIn, ePlusIn, recoilIn, v1Init, v2Init, + null, null, DEFAULT_TVJ_MAX_ITERATIONS, 1.0e-8); + } + + /** As above, but with explicit V1/V2 prior covariances (nullable, same convention as {@link #fitCascadeVertexJoint}). */ + public TwoVertexFitResult fitCascadeVertexJointFreeTrack(TrackParams eMinusIn, TrackParams ePlusIn, + TrackParams recoilIn, RealVector v1Init, RealVector v2Init, RealMatrix v1Cov, RealMatrix v2Cov) { + return fitCascadeVertexJointFreeTrack(eMinusIn, ePlusIn, recoilIn, v1Init, v2Init, + v1Cov, v2Cov, DEFAULT_TVJ_MAX_ITERATIONS, 1.0e-8); + } + + /** + * As {@link #fitCascadeVertexJointBeamConstrainedCore}, but with the beam-momentum + * constraint dropped entirely -- isolates the effect of letting each of the three + * tracks' own 5 perigee parameters float as free state (weighted by that track's own + * input covariance) from the effect of also imposing an external momentum constraint. + * Same 22-dim state layout ({@code CVJBC_OFF_*}) and KKT/Newton-Raphson solve as {@link + * #fitCascadeVertexJointBeamConstrainedCore}; see that method's Javadoc for the shared + * per-row regularization and measurement-only momentum-covariance-sandwich reasoning, + * both of which apply here unchanged. + * + *

Only nine constraint rows (no beam-momentum block): eMinus-at-V1 (2), ePlus-at-V1 + * (2), recoil-at-V2 (2), and the geometric decay-length constraint (3). {@code ndf = + * nConstraints(9) - 7 = 2} (7 = V1+theta+V2, the only state components with a free/weak + * prior; track params have a real prior via {@code W} so don't count) -- the same ndf as + * the fixed-track {@link #fitCascadeVertexJointCore}, since freeing track params under + * their own real prior doesn't consume additional ndf. + */ + public TwoVertexFitResult fitCascadeVertexJointFreeTrack(TrackParams eMinusIn, TrackParams ePlusIn, + TrackParams recoilIn, RealVector v1Init, RealVector v2Init, RealMatrix v1CovIn, RealMatrix v2CovIn, + int maxIterations, double tolerance) { + + TrackParams eMinus = eMinusIn.copy(); + TrackParams ePlus = ePlusIn.copy(); + TrackParams recoil = recoilIn.copy(); + + boolean useBeamspotPriorForV2 = (v2Init == null); + RealVector v2InitVec = useBeamspotPriorForV2 ? MatrixUtils.createRealVector(beamPosition) : v2Init; + + RealVector pV0Init = computeMomentumAtVertex(eMinus, v1Init).add(computeMomentumAtVertex(ePlus, v1Init)); + double pV0InitNormSq = pV0Init.dotProduct(pV0Init); + double thetaInit = (pV0InitNormSq > 0) + ? v1Init.subtract(v2InitVec).dotProduct(pV0Init) / pV0InitNormSq + : 0.0; + + ThetaSeed branchSeed = selectPhysicalThetaSeed(v1Init, pV0Init, recoil); + if (branchSeed != null) { + thetaInit = branchSeed.theta; + v2InitVec = branchSeed.v2; + } + + return fitCascadeVertexJointFreeTrackCore(eMinus, ePlus, recoil, v1Init, thetaInit, v2InitVec, + v1CovIn, v2CovIn, useBeamspotPriorForV2, maxIterations, tolerance); + } + + private TwoVertexFitResult fitCascadeVertexJointFreeTrackCore( + TrackParams eMinus, TrackParams ePlus, TrackParams recoil, + RealVector v1Init, double thetaInit, RealVector v2InitVec, + RealMatrix v1CovIn, RealMatrix v2CovIn, boolean useBeamspotPriorForV2, + int maxIterations, double tolerance) { + + final int N_TRACK_ROWS = 6; + final int N_GEOM_ROWS = 3; + final int nConstraints = N_TRACK_ROWS + N_GEOM_ROWS; + + RealVector x0 = MatrixUtils.createRealVector(new double[CVJBC_STATE_SIZE]); + x0.setSubVector(CVJBC_OFF_V1, v1Init); + x0.setEntry(CVJBC_OFF_THETA, thetaInit); + x0.setSubVector(CVJBC_OFF_V2, v2InitVec); + x0.setSubVector(CVJBC_OFF_EM, MatrixUtils.createRealVector(eMinus.toArray())); + x0.setSubVector(CVJBC_OFF_EP, MatrixUtils.createRealVector(ePlus.toArray())); + x0.setSubVector(CVJBC_OFF_RC, MatrixUtils.createRealVector(recoil.toArray())); + + RealMatrix W = MatrixUtils.createRealMatrix(CVJBC_STATE_SIZE, CVJBC_STATE_SIZE); + RealMatrix WInv = MatrixUtils.createRealMatrix(CVJBC_STATE_SIZE, CVJBC_STATE_SIZE); + if (v1CovIn != null) { + RealMatrix v1CovInv = new LUDecomposition(v1CovIn).getSolver().getInverse(); + W.setSubMatrix(v1CovInv.getData(), CVJBC_OFF_V1, CVJBC_OFF_V1); + WInv.setSubMatrix(v1CovIn.getData(), CVJBC_OFF_V1, CVJBC_OFF_V1); + } else { + for (int i = 0; i < 3; i++) { + W.setEntry(CVJBC_OFF_V1 + i, CVJBC_OFF_V1 + i, 0.01); + WInv.setEntry(CVJBC_OFF_V1 + i, CVJBC_OFF_V1 + i, 100.0); + } + } + W.setEntry(CVJBC_OFF_THETA, CVJBC_OFF_THETA, 0.01); + WInv.setEntry(CVJBC_OFF_THETA, CVJBC_OFF_THETA, 100.0); + if (v2CovIn != null) { + RealMatrix v2CovInv = new LUDecomposition(v2CovIn).getSolver().getInverse(); + W.setSubMatrix(v2CovInv.getData(), CVJBC_OFF_V2, CVJBC_OFF_V2); + WInv.setSubMatrix(v2CovIn.getData(), CVJBC_OFF_V2, CVJBC_OFF_V2); + } else { + for (int i = 0; i < 3; i++) { + double var = useBeamspotPriorForV2 ? beamSize[i] * beamSize[i] : 100.0; + W.setEntry(CVJBC_OFF_V2 + i, CVJBC_OFF_V2 + i, 1.0 / var); + WInv.setEntry(CVJBC_OFF_V2 + i, CVJBC_OFF_V2 + i, var); + } + } + + RealMatrix[] trackCovs = {eMinus.cov, ePlus.cov, recoil.cov}; + int[] trackOffsets = {CVJBC_OFF_EM, CVJBC_OFF_EP, CVJBC_OFF_RC}; + for (int t = 0; t < 3; t++) { + RealMatrix covInv; + try { + covInv = new LUDecomposition(trackCovs[t]).getSolver().getInverse(); + } catch (SingularMatrixException e) { + return null; // Cannot build weight matrix; skip this event silently + } + W.setSubMatrix(covInv.getData(), trackOffsets[t], trackOffsets[t]); + WInv.setSubMatrix(trackCovs[t].getData(), trackOffsets[t], trackOffsets[t]); + } + + RealVector x = x0.copy(); + RealMatrix finalH = null; + RealMatrix finalV = null; + RealVector finalHvec = null; + + for (int iteration = 0; iteration < maxIterations; iteration++) { + RealVector xOld = x.copy(); + + RealVector v1 = x.getSubVector(CVJBC_OFF_V1, 3); + double theta = x.getEntry(CVJBC_OFF_THETA); + RealVector v2 = x.getSubVector(CVJBC_OFF_V2, 3); + eMinus.fromArray(x.getSubVector(CVJBC_OFF_EM, 5).toArray()); + ePlus.fromArray(x.getSubVector(CVJBC_OFF_EP, 5).toArray()); + recoil.fromArray(x.getSubVector(CVJBC_OFF_RC, 5).toArray()); + + try { + RealVector h = MatrixUtils.createRealVector(new double[nConstraints]); + RealMatrix H = MatrixUtils.createRealMatrix(nConstraints, CVJBC_STATE_SIZE); + + Constraint cEm = computeTrackConstraint(eMinus, v1); + Constraint cEp = computeTrackConstraint(ePlus, v1); + Constraint cRc = computeTrackConstraint(recoil, v2); + h.setSubVector(0, cEm.c); + h.setSubVector(2, cEp.c); + h.setSubVector(4, cRc.c); + H.setSubMatrix(cEm.H.getData(), 0, CVJBC_OFF_V1); + H.setSubMatrix(trackResidualJacobianWrtTrackParams(eMinus, v1.getEntry(0), v1.getEntry(1)).getData(), + 0, CVJBC_OFF_EM); + H.setSubMatrix(cEp.H.getData(), 2, CVJBC_OFF_V1); + H.setSubMatrix(trackResidualJacobianWrtTrackParams(ePlus, v1.getEntry(0), v1.getEntry(1)).getData(), + 2, CVJBC_OFF_EP); + H.setSubMatrix(cRc.H.getData(), 4, CVJBC_OFF_V2); + H.setSubMatrix(trackResidualJacobianWrtTrackParams(recoil, v2.getEntry(0), v2.getEntry(1)).getData(), + 4, CVJBC_OFF_RC); + + GeometricConstraint gc = computeGeometricConstraint(v1, theta, v2, eMinus, ePlus); + h.setSubVector(6, gc.g); + // gc.H's columns are laid out [V1(0-2), theta(3), V2(4-6)] (TVJ_OFF_*), which + // is bit-for-bit the same layout as CVJBC_OFF_V1/THETA/V2 -- copy directly. + H.setSubMatrix(gc.H.getData(), 6, 0); + RealMatrix dgdEm = computeMomentumTrackJacobian(eMinus, v1).scalarMultiply(theta); + RealMatrix dgdEp = computeMomentumTrackJacobian(ePlus, v1).scalarMultiply(theta); + H.setSubMatrix(dgdEm.getData(), 6, CVJBC_OFF_EM); + H.setSubMatrix(dgdEp.getData(), 6, CVJBC_OFF_EP); + + RealMatrix HWInvHT = H.multiply(WInv).multiply(H.transpose()); + // Regularization: see fitCascadeVertexJointBeamConstrainedCore's Javadoc/inline + // comment for why a tiny per-row epsilon (not 0) is needed here, and why it must + // be scaled per-row rather than by one averaged scale. With no momentum block, + // all nConstraints(9) rows get this same treatment. + RealMatrix constraintCov = MatrixUtils.createRealMatrix(nConstraints, nConstraints); + for (int i = 0; i < nConstraints; i++) { + double diagI = HWInvHT.getEntry(i, i); + double epsilonI = (diagI > 0) ? diagI * 1e-9 : 1e-12; + constraintCov.setEntry(i, i, epsilonI); + } + + finalH = H; + finalV = constraintCov; + finalHvec = h; + + RealMatrix S = HWInvHT.add(constraintCov); + RealVector rhs = h.add(H.operate(x0.subtract(x))); + RealVector lambda = new LUDecomposition(S).getSolver().solve(rhs); + RealVector deltaX = x0.subtract(x).subtract(WInv.multiply(H.transpose()).operate(lambda)); + + double stepNorm = deltaX.getNorm(); + if (stepNorm > MAX_STEP_NORM) { + deltaX = deltaX.mapMultiply(MAX_STEP_NORM / stepNorm); + } + x = x.add(deltaX); + + if (!Double.isFinite(x.getNorm())) { + x = xOld; + break; + } + if (deltaX.getNorm() < tolerance && h.getNorm() < tolerance * 10) { + break; + } + } catch (Exception e) { + x = xOld; + break; + } + } + + RealVector v1Final = x.getSubVector(CVJBC_OFF_V1, 3); + RealVector v2Final = x.getSubVector(CVJBC_OFF_V2, 3); + eMinus.fromArray(x.getSubVector(CVJBC_OFF_EM, 5).toArray()); + ePlus.fromArray(x.getSubVector(CVJBC_OFF_EP, 5).toArray()); + recoil.fromArray(x.getSubVector(CVJBC_OFF_RC, 5).toArray()); + + RealMatrix C_fitted = WInv.copy(); + if (finalH != null) { + try { + RealMatrix S = finalH.multiply(WInv).multiply(finalH.transpose()).add(finalV); + RealMatrix K = WInv.multiply(finalH.transpose()) + .multiply(new LUDecomposition(S).getSolver().getInverse()); + C_fitted = WInv.subtract(K.multiply(finalH).multiply(WInv)); + } catch (Exception e) { + // keep prior WInv as a fallback post-fit covariance + } + } + + eMinus.cov = C_fitted.getSubMatrix(CVJBC_OFF_EM, CVJBC_OFF_EM + 4, CVJBC_OFF_EM, CVJBC_OFF_EM + 4); + ePlus.cov = C_fitted.getSubMatrix(CVJBC_OFF_EP, CVJBC_OFF_EP + 4, CVJBC_OFF_EP, CVJBC_OFF_EP + 4); + recoil.cov = C_fitted.getSubMatrix(CVJBC_OFF_RC, CVJBC_OFF_RC + 4, CVJBC_OFF_RC, CVJBC_OFF_RC + 4); + + RealMatrix v1Cov = C_fitted.getSubMatrix(CVJBC_OFF_V1, CVJBC_OFF_V1 + 2, CVJBC_OFF_V1, CVJBC_OFF_V1 + 2); + RealMatrix v2Cov = C_fitted.getSubMatrix(CVJBC_OFF_V2, CVJBC_OFF_V2 + 2, CVJBC_OFF_V2, CVJBC_OFF_V2 + 2); + + RealVector dx = x.subtract(x0); + double chi2 = dx.dotProduct(W.operate(dx)); + if (finalHvec != null && finalV != null) { + chi2 += chi2Contribution(finalHvec.getSubVector(0, 2), finalV.getSubMatrix(0, 1, 0, 1)); + chi2 += chi2Contribution(finalHvec.getSubVector(2, 2), finalV.getSubMatrix(2, 3, 2, 3)); + chi2 += chi2Contribution(finalHvec.getSubVector(4, 2), finalV.getSubMatrix(4, 5, 4, 5)); + chi2 += chi2Contribution(finalHvec.getSubVector(6, 3), finalV.getSubMatrix(6, 8, 6, 8)); + } + + int ndf = nConstraints - 7; + + // Measurement-only momentum-covariance sandwich (Wtrk zeroes the weak V1/theta/V2 + // prior block) -- same double-counting reasoning as + // fitCascadeVertexJointBeamConstrainedCore's Javadoc. + RealMatrix Wtrk = MatrixUtils.createRealMatrix(CVJBC_STATE_SIZE, CVJBC_STATE_SIZE); + Wtrk.setSubMatrix(W.getSubMatrix(CVJBC_OFF_EM, CVJBC_OFF_EM + 4, CVJBC_OFF_EM, CVJBC_OFF_EM + 4).getData(), + CVJBC_OFF_EM, CVJBC_OFF_EM); + Wtrk.setSubMatrix(W.getSubMatrix(CVJBC_OFF_EP, CVJBC_OFF_EP + 4, CVJBC_OFF_EP, CVJBC_OFF_EP + 4).getData(), + CVJBC_OFF_EP, CVJBC_OFF_EP); + Wtrk.setSubMatrix(W.getSubMatrix(CVJBC_OFF_RC, CVJBC_OFF_RC + 4, CVJBC_OFF_RC, CVJBC_OFF_RC + 4).getData(), + CVJBC_OFF_RC, CVJBC_OFF_RC); + RealMatrix C_meas = C_fitted.multiply(Wtrk).multiply(C_fitted); + + TrackMomentum eMinusMomentum = new TrackMomentum(computeMomentumAtVertex(eMinus, v1Final), + computeMomentumCovarianceFull(eMinus, v1Final, C_meas, CVJBC_OFF_V1, CVJBC_OFF_EM)); + TrackMomentum ePlusMomentum = new TrackMomentum(computeMomentumAtVertex(ePlus, v1Final), + computeMomentumCovarianceFull(ePlus, v1Final, C_meas, CVJBC_OFF_V1, CVJBC_OFF_EP)); + TrackMomentum recoilMomentum = new TrackMomentum(computeMomentumAtVertex(recoil, v2Final), + computeMomentumCovarianceFull(recoil, v2Final, C_meas, CVJBC_OFF_V2, CVJBC_OFF_RC)); + + List fittedTracks = new ArrayList<>(); + fittedTracks.add(eMinus); + fittedTracks.add(ePlus); + fittedTracks.add(recoil); + + return new TwoVertexFitResult(v1Final, v1Cov, v2Final, v2Cov, chi2, ndf, + eMinusMomentum, ePlusMomentum, recoilMomentum, fittedTracks); + } + + private static final int TVJX_OFF_V1 = 0; + private static final int TVJX_OFF_THETA = 3; + private static final int TVJX_OFF_V2FREE = 4; // holds only v2[1], v2[2] (the free transverse coords) + private static final int TVJX_STATE_SIZE = 6; + + /** + * As {@link #fitCascadeVertexJoint}, but V2's tracking-index-0 (beam-direction/target-z) + * coordinate is held fixed at {@code v2FixedX} instead of being a free fit parameter -- + * for topologies (e.g. near-collinear V0+recoil, which lack the ~30mrad minimum opening + * angle that ordinary top/bottom-paired 2-track vertexing enforces) where that coordinate + * is only very weakly constrained by the track geometry, fixing it to the known target + * position removes the poorly-determined degree of freedom entirely rather than merely + * down-weighting it via a soft prior. V1 stays fully free in all 3 dimensions, and V2's + * other two (transverse) coordinates keep fitting freely. State is the 6-vector + * [V1(0-2), theta(3), v2free(4-5)] where v2free=[v2.y,v2.z]; the returned + * {@link TwoVertexFitResult} still carries a full 3-vector/3x3-cov V2 (fixed + * coordinate reported as exactly {@code v2FixedX} with exactly-zero variance/covariance + * in that row/column), so downstream consumers need no changes. + */ + public TwoVertexFitResult fitCascadeVertexJointFixedV2X(TrackParams eMinusIn, TrackParams ePlusIn, TrackParams recoilIn, + RealVector v1Init, RealVector v2Init, double v2FixedX, RealMatrix v1Cov, RealMatrix v2Cov) { + return fitCascadeVertexJointFixedV2X(eMinusIn, ePlusIn, recoilIn, v1Init, v2Init, v2FixedX, v1Cov, v2Cov, + DEFAULT_TVJ_MAX_ITERATIONS, 1.0e-8); + } + + public TwoVertexFitResult fitCascadeVertexJointFixedV2X(TrackParams eMinusIn, TrackParams ePlusIn, TrackParams recoilIn, + RealVector v1Init, RealVector v2Init, double v2FixedX, RealMatrix v1CovIn, RealMatrix v2CovIn, + int maxIterations, double tolerance) { + + TrackParams eMinus = eMinusIn.copy(); + TrackParams ePlus = ePlusIn.copy(); + TrackParams recoil = recoilIn.copy(); + + boolean useBeamspotPriorForV2 = (v2Init == null); + RealVector v2InitVec = (useBeamspotPriorForV2 ? MatrixUtils.createRealVector(beamPosition) : v2Init).copy(); + v2InitVec.setEntry(0, v2FixedX); + + // Same theta seeding as fitCascadeVertexJoint (least-squares projection, then the + // transverseCircleRoots/selectPhysicalThetaSeed branch override) -- both operate on + // full 3-vectors and are unaffected by fixing V2's index-0 coordinate afterward. + RealVector pV0Init = computeMomentumAtVertex(eMinus, v1Init).add(computeMomentumAtVertex(ePlus, v1Init)); + double pV0InitNormSq = pV0Init.dotProduct(pV0Init); + double thetaInit = (pV0InitNormSq > 0) + ? v1Init.subtract(v2InitVec).dotProduct(pV0Init) / pV0InitNormSq + : 0.0; + + ThetaSeed branchSeed = selectPhysicalThetaSeed(v1Init, pV0Init, recoil); + if (branchSeed != null) { + thetaInit = branchSeed.theta; + v2InitVec = branchSeed.v2.copy(); + v2InitVec.setEntry(0, v2FixedX); + } + + return fitCascadeVertexJointFixedV2XCore(eMinus, ePlus, recoil, v1Init, thetaInit, v2InitVec, v2FixedX, + v1CovIn, v2CovIn, useBeamspotPriorForV2, maxIterations, tolerance); + } + + /** + * Run the fixed-V2-coordinate joint fit from an explicitly forced (theta, V2) seed, + * bypassing {@link #selectPhysicalThetaSeed}'s own branch choice -- the fixed-V2X + * counterpart of {@link #fitCascadeVertexJointForcedBranch}, used the same way by + * {@link CascadeVertexer#fit} to fit both {@link #transverseCircleRoots} candidates + * and choose between them externally. Package-private, not part of the general-purpose + * API. + */ + TwoVertexFitResult fitCascadeVertexJointFixedV2XForcedBranch(TrackParams eMinusIn, TrackParams ePlusIn, TrackParams recoilIn, + RealVector v1Init, double thetaInit, RealVector v2InitVec, double v2FixedX, + RealMatrix v1CovIn, RealMatrix v2CovIn, int maxIterations, double tolerance) { + TrackParams eMinus = eMinusIn.copy(); + TrackParams ePlus = ePlusIn.copy(); + TrackParams recoil = recoilIn.copy(); + RealVector v2Fixed = v2InitVec.copy(); + v2Fixed.setEntry(0, v2FixedX); + return fitCascadeVertexJointFixedV2XCore(eMinus, ePlus, recoil, v1Init, thetaInit, v2Fixed, v2FixedX, + v1CovIn, v2CovIn, false, maxIterations, tolerance); + } + + private TwoVertexFitResult fitCascadeVertexJointFixedV2XCore(TrackParams eMinus, TrackParams ePlus, TrackParams recoil, + RealVector v1Init, double thetaInit, RealVector v2InitVec, double v2FixedX, RealMatrix v1CovIn, RealMatrix v2CovIn, + boolean useBeamspotPriorForV2, int maxIterations, double tolerance) { + + RealVector y0 = MatrixUtils.createRealVector(new double[TVJX_STATE_SIZE]); + y0.setSubVector(TVJX_OFF_V1, v1Init); + y0.setEntry(TVJX_OFF_THETA, thetaInit); + y0.setEntry(TVJX_OFF_V2FREE, v2InitVec.getEntry(1)); + y0.setEntry(TVJX_OFF_V2FREE + 1, v2InitVec.getEntry(2)); + + RealMatrix priorC = MatrixUtils.createRealMatrix(TVJX_STATE_SIZE, TVJX_STATE_SIZE); + if (v1CovIn != null) { + priorC.setSubMatrix(v1CovIn.getData(), TVJX_OFF_V1, TVJX_OFF_V1); + } else { + for (int i = 0; i < 3; i++) { + priorC.setEntry(TVJX_OFF_V1 + i, TVJX_OFF_V1 + i, 100.0); + } + } + priorC.setEntry(TVJX_OFF_THETA, TVJX_OFF_THETA, 100.0); + if (v2CovIn != null) { + priorC.setSubMatrix(v2CovIn.getSubMatrix(1, 2, 1, 2).getData(), TVJX_OFF_V2FREE, TVJX_OFF_V2FREE); + } else { + for (int i = 0; i < 2; i++) { + double var = useBeamspotPriorForV2 ? beamSize[i + 1] * beamSize[i + 1] : 100.0; + priorC.setEntry(TVJX_OFF_V2FREE + i, TVJX_OFF_V2FREE + i, var); + } + } + + RealVector y = y0.copy(); + RealMatrix C = priorC.copy(); + + if (DEBUG_JOINT_FIT) { + System.err.println("JFXDEBUG start y0=" + y0 + " thetaInit=" + thetaInit + " v2FixedX=" + v2FixedX); + } + + final int TVJX_N_ROWS = 9; + for (int iteration = 0; iteration < maxIterations; iteration++) { + RealVector yOld = y.copy(); + C = priorC.copy(); + try { + RealVector v1 = y.getSubVector(TVJX_OFF_V1, 3); + double theta = y.getEntry(TVJX_OFF_THETA); + RealVector v2Full = MatrixUtils.createRealVector(new double[]{ + v2FixedX, y.getEntry(TVJX_OFF_V2FREE), y.getEntry(TVJX_OFF_V2FREE + 1)}); + + Constraint cEm = computeTrackConstraint(eMinus, v1); + Constraint cEp = computeTrackConstraint(ePlus, v1); + Constraint cRc = computeTrackConstraint(recoil, v2Full); + GeometricConstraint gc = computeGeometricConstraint(v1, theta, v2Full, eMinus, ePlus); + + RealVector cStack = MatrixUtils.createRealVector(new double[TVJX_N_ROWS]); + cStack.setSubVector(0, cEm.c); + cStack.setSubVector(2, cEp.c); + cStack.setSubVector(4, cRc.c); + cStack.setSubVector(6, gc.g); + + // cRc.H (2x3, columns = v2.x,y,z) has its column 0 (the now-fixed coordinate) + // dropped before embedding; gc.H (3x7, in the original TVJ_OFF_* column + // convention) has its single V2-x column (index TVJ_OFF_V2) dropped, which + // shifts the V2-y/z columns left to land exactly at TVJX_OFF_V2FREE. + RealMatrix HStack = MatrixUtils.createRealMatrix(TVJX_N_ROWS, TVJX_STATE_SIZE); + HStack.setSubMatrix(embedFixedV2X(cEm.H, TVJX_OFF_V1).getData(), 0, 0); + HStack.setSubMatrix(embedFixedV2X(cEp.H, TVJX_OFF_V1).getData(), 2, 0); + HStack.setSubMatrix(embedFixedV2X(dropColumn(cRc.H, 0), TVJX_OFF_V2FREE).getData(), 4, 0); + HStack.setSubMatrix(dropColumn(gc.H, TVJ_OFF_V2).getData(), 6, 0); + + RealMatrix VStack = MatrixUtils.createRealMatrix(TVJX_N_ROWS, TVJX_N_ROWS); + VStack.setSubMatrix(cEm.V.getData(), 0, 0); + VStack.setSubMatrix(cEp.V.getData(), 2, 2); + VStack.setSubMatrix(cRc.V.getData(), 4, 4); + VStack.setSubMatrix(gc.V.getData(), 6, 6); + + SequentialUpdate u = sequentialUpdate(y, C, cStack, HStack, VStack); + + RealVector step = u.y.subtract(y); + double stepNorm = step.getNorm(); + if (stepNorm > MAX_STEP_NORM) { + step = step.mapMultiply(MAX_STEP_NORM / stepNorm); + } + y = y.add(step); + C = u.C; + + if (DEBUG_JOINT_FIT) { + System.err.println("JFXDEBUG it=" + iteration + " v1=" + y.getSubVector(TVJX_OFF_V1, 3) + + " theta=" + y.getEntry(TVJX_OFF_THETA) + " v2free=" + y.getSubVector(TVJX_OFF_V2FREE, 2) + + " |cEm|=" + cEm.c.getNorm() + " |cEp|=" + cEp.c.getNorm() + + " |cRc|=" + cRc.c.getNorm() + " |g|=" + gc.g.getNorm()); + } + + if (!Double.isFinite(y.getNorm())) { + y = yOld; + break; + } + if (y.subtract(yOld).getNorm() < tolerance) { + break; + } + } catch (Exception e) { + if (DEBUG_JOINT_FIT) { + System.err.println("JFXDEBUG it=" + iteration + " EXCEPTION " + e); + } + y = yOld; + break; + } + } + + RealVector v1Final = y.getSubVector(TVJX_OFF_V1, 3); + double thetaFinal = y.getEntry(TVJX_OFF_THETA); + RealVector v2Final = MatrixUtils.createRealVector(new double[]{ + v2FixedX, y.getEntry(TVJX_OFF_V2FREE), y.getEntry(TVJX_OFF_V2FREE + 1)}); + + RealMatrix v1Cov = C.getSubMatrix(TVJX_OFF_V1, TVJX_OFF_V1 + 2, TVJX_OFF_V1, TVJX_OFF_V1 + 2); + // Fixed coordinate's row/col (index 0) stays exactly 0, per the confirmed reporting + // convention -- only the free (y,z) 2x2 sub-block comes from the converged fit. + RealMatrix v2Cov = MatrixUtils.createRealMatrix(3, 3); + v2Cov.setSubMatrix( + C.getSubMatrix(TVJX_OFF_V2FREE, TVJX_OFF_V2FREE + 1, TVJX_OFF_V2FREE, TVJX_OFF_V2FREE + 1).getData(), + 1, 1); + + Constraint cEmFinal = computeTrackConstraint(eMinus, v1Final); + Constraint cEpFinal = computeTrackConstraint(ePlus, v1Final); + Constraint cRcFinal = computeTrackConstraint(recoil, v2Final); + GeometricConstraint gcFinal = computeGeometricConstraint(v1Final, thetaFinal, v2Final, eMinus, ePlus); + + double chi2 = 0.0; + chi2 += chi2Contribution(cEmFinal.c, cEmFinal.V); + chi2 += chi2Contribution(cEpFinal.c, cEpFinal.V); + chi2 += chi2Contribution(cRcFinal.c, cRcFinal.V); + chi2 += chi2Contribution(gcFinal.g, gcFinal.V); + + // ndf: 3 track constraints x 2 rows + 1 geometric constraint x 3 rows = 9 + // measurements, minus 6 free parameters (V1 fully free, theta, V2's 2 free + // transverse coords) = 3. + int ndf = 3; + + TrackMomentum eMinusMomentum = new TrackMomentum( + computeMomentumAtVertex(eMinus, v1Final), computeMomentumCovariance(eMinus, v1Final)); + TrackMomentum ePlusMomentum = new TrackMomentum( + computeMomentumAtVertex(ePlus, v1Final), computeMomentumCovariance(ePlus, v1Final)); + TrackMomentum recoilMomentum = new TrackMomentum( + computeMomentumAtVertex(recoil, v2Final), computeMomentumCovariance(recoil, v2Final)); + + List fittedTracks = new ArrayList<>(); + fittedTracks.add(eMinus); + fittedTracks.add(ePlus); + fittedTracks.add(recoil); + + return new TwoVertexFitResult(v1Final, v1Cov, v2Final, v2Cov, chi2, ndf, + eMinusMomentum, ePlusMomentum, recoilMomentum, fittedTracks); + } + + /** Zero-pad a Jacobian's columns into a {@code TVJX_STATE_SIZE}-wide state, starting at colOffset. */ + private static RealMatrix embedFixedV2X(RealMatrix H, int colOffset) { + RealMatrix full = MatrixUtils.createRealMatrix(H.getRowDimension(), TVJX_STATE_SIZE); + full.setSubMatrix(H.getData(), 0, colOffset); + return full; + } + + /** Drop column {@code dropCol} from H, shifting later columns left by one. */ + private static RealMatrix dropColumn(RealMatrix H, int dropCol) { + int rows = H.getRowDimension(); + int cols = H.getColumnDimension(); + RealMatrix reduced = MatrixUtils.createRealMatrix(rows, cols - 1); + int destCol = 0; + for (int c = 0; c < cols; c++) { + if (c == dropCol) { + continue; + } + reduced.setColumnVector(destCol++, H.getColumnVector(c)); + } + return reduced; + } + + /** r^T*V^-1*r, or 0.0 if V is singular (mirrors the defensive pattern used elsewhere in this class). */ + private static double chi2Contribution(RealVector c, RealMatrix V) { + try { + RealMatrix VInv = new LUDecomposition(V).getSolver().getInverse(); + return c.dotProduct(VInv.operate(c)); + } catch (Exception e) { + return 0.0; + } + } + + /** Zero-pad a Jacobian's columns into the full {@code TVJ_STATE_SIZE}-wide state, starting at colOffset. */ + private static RealMatrix embed(RealMatrix H3, int colOffset) { + RealMatrix full = MatrixUtils.createRealMatrix(H3.getRowDimension(), TVJ_STATE_SIZE); + full.setSubMatrix(H3.getData(), 0, colOffset); + return full; + } + + private static class SequentialUpdate { + final RealVector y; + final RealMatrix C; + + SequentialUpdate(RealVector y, RealMatrix C) { + this.y = y; + this.C = C; + } + } + + /** + * One step of Hulsbergen's sequential Kalman-gain recursion, shared by + * measurement-type constraints (Eq. 7-19, V = real measurement covariance) and exact + * constraints (Eq. 24-31, V = 0) -- the paper notes after Eq. 32 that the two are + * identical once V (for a measurement) or 0 (for an exact constraint) is substituted + * in, so a single implementation covers both. Uses the same numerically-stable + * Joseph-form covariance update as {@link #fitCascadeVertex}. + */ + private static SequentialUpdate sequentialUpdate(RealVector y, RealMatrix C, RealVector c, RealMatrix H, RealMatrix V) { + RealMatrix S = H.multiply(C).multiply(H.transpose()).add(V); + RealMatrix K = C.multiply(H.transpose()).multiply(new LUDecomposition(S).getSolver().getInverse()); + RealVector yNew = y.subtract(K.operate(c)); + RealMatrix I = MatrixUtils.createRealIdentityMatrix(y.getDimension()); + RealMatrix ImKH = I.subtract(K.multiply(H)); + RealMatrix Cnew = ImKH.multiply(C).multiply(ImKH.transpose()).add(K.multiply(V).multiply(K.transpose())); + return new SequentialUpdate(yNew, Cnew); + } + + private static class GeometricConstraint { + final RealVector g; + final RealMatrix H; + final RealMatrix V; + + GeometricConstraint(RealVector g, RealMatrix H, RealMatrix V) { + this.g = g; + this.H = H; + this.V = V; + } + } + + /** + * Hulsbergen's Eq. 33 decay-length constraint g(V1,theta,V2) = V2 - V1 + theta*p_v0(V1) + * = 0, with a fully analytic Jacobian (no finite differences): dg/dV2 = I, + * dg/dtheta = p_v0(V1), dg/dV1 = -I + theta * d(p_v0)/dV1, where d(p_v0)/dV1 is the sum + * of the two daughters' {@link #computeMomentumVertexDerivatives}. + * + *

Unlike a true Eq. 32 "exact" constraint (V=0), g depends on p_v0(V1), which carries + * real uncertainty from the eMinus/ePlus track parameters (direction/curvature), amplified + * by theta -- for a near-collinear V0+recoil topology theta can be large, so this term is + * not negligible. dg/dpEm = dg/dpEp = theta*I (each daughter's momentum enters g solely + * through pV0 = pEm+pEp, linearly scaled by theta), so the propagated covariance is + * theta^2 * (covariance of pEm + covariance of pEp), reusing the same per-track momentum + * covariance already computed for reporting fitted momenta ({@link #computeMomentumCovariance}). + * Leaving V=0 here (the prior behavior) causes the Kalman update to over-trust this + * constraint, underestimating the reported V1/V2 position covariances. + */ + private GeometricConstraint computeGeometricConstraint(RealVector v1, double theta, RealVector v2, + TrackParams eMinus, TrackParams ePlus) { + RealVector pEm = computeMomentumAtVertex(eMinus, v1); + RealVector pEp = computeMomentumAtVertex(ePlus, v1); + RealVector pV0 = pEm.add(pEp); + RealVector g = v2.subtract(v1).add(pV0.mapMultiply(theta)); + + RealMatrix dpV0dV1 = computeMomentumVertexDerivatives(eMinus, v1).add(computeMomentumVertexDerivatives(ePlus, v1)); + RealMatrix dGdV1 = MatrixUtils.createRealIdentityMatrix(3).scalarMultiply(-1.0).add(dpV0dV1.scalarMultiply(theta)); + + RealMatrix H = MatrixUtils.createRealMatrix(3, TVJ_STATE_SIZE); + H.setSubMatrix(dGdV1.getData(), 0, TVJ_OFF_V1); + H.setColumnVector(TVJ_OFF_THETA, pV0); + H.setSubMatrix(MatrixUtils.createRealIdentityMatrix(3).getData(), 0, TVJ_OFF_V2); + + RealMatrix pCov = computeMomentumCovariance(eMinus, v1).add(computeMomentumCovariance(ePlus, v1)); + RealMatrix V = pCov.scalarMultiply(theta * theta); + + return new GeometricConstraint(g, H, V); + } + + /** + * Build a LineParams representing an already-fitted V0's momentum direction, carrying + * the full 6x6 position-momentum joint covariance (no block-diagonal approximation), + * for use as the "line" input to {@link #fitCascadeVertex}. BilliorVertex stores its + * position/momentum/covariances in the detector frame; this fitter's helix/line math + * operates in the tracking frame (same convention as BilliorVertexer's internal fit), + * so everything is rotated to tracking frame here. + */ + public static LineParams lineFromV0(BilliorVertex v0Vertex) { + Hep3Vector vDet = v0Vertex.getPosition(); + Hep3Vector p1Det = v0Vertex.getFittedMomentum(0); + Hep3Vector p2Det = v0Vertex.getFittedMomentum(1); + Hep3Vector pDet = new BasicHep3Vector(p1Det.x() + p2Det.x(), p1Det.y() + p2Det.y(), p1Det.z() + p2Det.z()); + + Hep3Vector vTrk = CoordinateTransformations.transformVectorToTracking(vDet); + Hep3Vector pTrk = CoordinateTransformations.transformVectorToTracking(pDet); + + Matrix covVVDet = v0Vertex.getCovMatrix(); + List covTrkMom = v0Vertex.getFittedMomentumCovariance(); + Matrix covPPDet = MatrixOp.add(MatrixOp.add(covTrkMom.get(0), covTrkMom.get(1)), + MatrixOp.add(covTrkMom.get(2), MatrixOp.transposed(covTrkMom.get(2)))); + Matrix covVPDet = v0Vertex.getVertexV0MomentumCovariance(); + + Matrix covVVTrk = CoordinateTransformations.transformMatrixToTracking(covVVDet); + Matrix covPPTrk = CoordinateTransformations.transformMatrixToTracking(covPPDet); + Matrix covVPTrk = CoordinateTransformations.transformMatrixToTracking(covVPDet); + + RealMatrix cov6 = MatrixUtils.createRealMatrix(6, 6); + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + cov6.setEntry(i, j, covVVTrk.e(i, j)); + cov6.setEntry(3 + i, 3 + j, covPPTrk.e(i, j)); + cov6.setEntry(i, 3 + j, covVPTrk.e(i, j)); + cov6.setEntry(3 + j, i, covVPTrk.e(i, j)); + } + } + + return new LineParams(vTrk.x(), vTrk.y(), vTrk.z(), pTrk.x(), pTrk.y(), pTrk.z(), cov6); + } + + /** Position and covariance where a {@link LineParams} line crosses a fixed plane. */ + public static class LinePlaneProjection { + public final RealVector position; + public final RealMatrix cov; + + public LinePlaneProjection(RealVector position, RealMatrix cov) { + this.position = position; + this.cov = cov; + } + } + + /** + * Propagate a line (e.g. an already-fitted V0's flight line from {@link #lineFromV0}) to + * the plane x = xPlane (tracking frame), carrying its covariance along. The line crosses + * the plane at a fixed, deterministic x = xPlane by construction, so the propagated + * covariance's x-row/column is exactly zero from the Jacobian alone -- {@code sigmaXFloor} + * is substituted in for that entry so a caller using this as a Kalman prior doesn't rigidly + * pin x forever. When used as a Kalman prior, this must be a weak/uninformative value + * (e.g. the variance=100 -> sigma=10 convention used elsewhere in this class for V1/theta), + * NOT {@link #beamSize}[0] -- that field is a disconnected, hardcoded ~1-micron default + * never intended as a covariance floor, and using it here rigidly pins the propagated x to + * xPlane on every outer iteration (priorC is rebuilt from this covariance each time), + * preventing the fit from ever moving away from the target plane. + */ + public static LinePlaneProjection propagateLineToPlane(LineParams line, double xPlane, double sigmaXFloor) { + double s = (xPlane - line.x0) / line.dx; + double yProp = line.y0 + s * line.dy; + double zProp = line.z0 + s * line.dz; + + RealVector position = MatrixUtils.createRealVector(new double[]{xPlane, yProp, zProp}); + + RealMatrix J = MatrixUtils.createRealMatrix(3, 6); + // Row 0 (x): always exactly xPlane, independent of the line's parameters -> all zero. + // Row 1 (y): yProp = y0 + s*dy, s = (xPlane-x0)/dx + J.setEntry(1, 0, -line.dy / line.dx); // d(yProp)/d(x0) + J.setEntry(1, 1, 1.0); // d(yProp)/d(y0) + J.setEntry(1, 3, -s * line.dy / line.dx); // d(yProp)/d(dx) + J.setEntry(1, 4, s); // d(yProp)/d(dy) + // Row 2 (z): zProp = z0 + s*dz + J.setEntry(2, 0, -line.dz / line.dx); // d(zProp)/d(x0) + J.setEntry(2, 2, 1.0); // d(zProp)/d(z0) + J.setEntry(2, 3, -s * line.dz / line.dx); // d(zProp)/d(dx) + J.setEntry(2, 5, s); // d(zProp)/d(dz) + + RealMatrix cov = J.multiply(line.cov).multiply(J.transpose()); + cov.setEntry(0, 0, sigmaXFloor * sigmaXFloor); + + return new LinePlaneProjection(position, cov); + } + + /** + * Propagate a curved track (perigee helix, e.g. the recoil electron) to the plane + * x = xPlane (tracking frame), carrying its covariance along via the same + * turning-angle parameterization used by {@link #perigeeToVertexParams}/ + * {@link #propagateTrackCovariance} -- but solving for the turning angle phiV that + * puts the helix at a given x, instead of the one nearest a given (x,y) vertex guess. + * As with {@link #propagateLineToPlane}, x is fixed by construction so the propagated + * covariance's x-row/column is set from {@code sigmaXFloor} rather than the (otherwise + * exactly zero) Jacobian entry. + */ + public static LinePlaneProjection propagateTrackToPlane(TrackParams track, double xPlane, double sigmaXFloor) { + double omega = track.omega; + double R = 1.0 / FastMath.abs(omega); + double sign = FastMath.signum(omega); + double d0 = track.d0; + double phi0 = track.phi0; + double tanLambda = track.tanLambda; + double z0 = track.z0; + + // xc,yc: helix center; xV = xc - sin(phiV)/omega, yV = yc + cos(phiV)/omega (inverting + // the same relations used in perigeeToVertexParams, using the identity sign*R = 1/omega). + double xc = FastMath.sin(phi0) * (1.0 / omega - d0); + double yc = -FastMath.cos(phi0) * (1.0 / omega - d0); + + double sinPhiV = omega * (xc - xPlane); + sinPhiV = FastMath.min(1.0, FastMath.max(-1.0, sinPhiV)); + double cand1 = FastMath.asin(sinPhiV); + double cand2 = FastMath.PI - cand1; + double dphi1 = normalizeAngle(cand1 - phi0); + double dphi2 = normalizeAngle(cand2 - phi0); + // Two candidate crossings of the plane exist (helix circle meets the line twice); + // pick the one reached by the smaller turning angle from the perigee, i.e. the first + // crossing along the track's flight path. + boolean useCand1 = FastMath.abs(dphi1) <= FastMath.abs(dphi2); + double phiV = useCand1 ? cand1 : cand2; + double dphi = useCand1 ? dphi1 : dphi2; + + // See perigeeToVertexParams: s = -sign(omega)*R*dphi, not R*dphi. + double s = -sign * R * dphi; + double cosPhiV = FastMath.cos(phiV); + double sinPhiV2 = FastMath.sin(phiV); + double yProp = yc + cosPhiV / omega; + double zProp = z0 + s * tanLambda; + + RealVector position = MatrixUtils.createRealVector(new double[]{xPlane, yProp, zProp}); + + double dPhiVDd0 = -omega * FastMath.sin(phi0) / cosPhiV; + double dPhiVDphi0 = FastMath.cos(phi0) * (1.0 - omega * d0) / cosPhiV; + double dPhiVDomega = (sinPhiV2 - FastMath.sin(phi0)) / (omega * cosPhiV); + + double dyDd0 = FastMath.cos(phi0) - (sinPhiV2 / omega) * dPhiVDd0; + double dyDphi0 = FastMath.sin(phi0) * (1.0 / omega - d0) - (sinPhiV2 / omega) * dPhiVDphi0; + double dyDomega = (FastMath.cos(phi0) - cosPhiV) / (omega * omega) - (sinPhiV2 / omega) * dPhiVDomega; + + // z = z0 + s*tanLambda with s = -sign(omega)*R*dphi: each R*(phiV-derivative) term + // below picks up the same -sign factor; the explicit s/omega term does not. + double dzDd0 = -sign * tanLambda * R * dPhiVDd0; + double dzDphi0 = -sign * (-tanLambda * R + tanLambda * R * dPhiVDphi0); + double dzDomega = -s * tanLambda / omega - sign * tanLambda * R * dPhiVDomega; + double dzDz0 = 1.0; + double dzDtl = s; + + RealMatrix J = MatrixUtils.createRealMatrix(2, 5); + J.setRow(0, new double[]{dyDd0, dyDphi0, dyDomega, 0.0, 0.0}); + J.setRow(1, new double[]{dzDd0, dzDphi0, dzDomega, dzDz0, dzDtl}); + + RealMatrix covYZ = J.multiply(track.cov).multiply(J.transpose()); + + RealMatrix cov = MatrixUtils.createRealMatrix(3, 3); + cov.setEntry(0, 0, sigmaXFloor * sigmaXFloor); + cov.setEntry(1, 1, covYZ.getEntry(0, 0)); + cov.setEntry(1, 2, covYZ.getEntry(0, 1)); + cov.setEntry(2, 1, covYZ.getEntry(1, 0)); + cov.setEntry(2, 2, covYZ.getEntry(1, 1)); + + return new LinePlaneProjection(position, cov); + } + + private static double normalizeAngle(double a) { + while (a > FastMath.PI) { + a -= 2.0 * FastMath.PI; + } + while (a < -FastMath.PI) { + a += 2.0 * FastMath.PI; + } + return a; + } + + /** + * Joint kinematic fit using soft (penalized) constraints via the Gain Matrix formalism. + * All constraints (track + 4-momentum) are satisfied simultaneously. + * + * This solves: minimize (x - x0)^T W (x - x0) + h(x)^T V^{-1} h(x) + * where x = [vertex, track1_params, track2_params, ...] is the full state vector, + * x0 is the initial/measured values, W is the weight matrix (inverse covariance), + * h(x) are the constraint residuals, and V is the constraint covariance. + * + * Because V > 0, constraints are soft (approximately satisfied, weighted by their + * uncertainty). This is equivalent to a Kalman filter update treating h(x)=0 as + * a measurement with noise V. True Lagrange multipliers (hard constraints) are + * recovered only in the limit V -> 0. + * + * Constraints: + * - Track constraints: each track must pass through the vertex (z matching) + * - 4-momentum constraint: sum of track momenta = beam momentum + */ + public FitResult fitSoftConstrained(List inputTracks, + RealVector vertexConstraint, + RealMatrix vertexConstraintCov, + RealVector fourMomentumConstraint, + RealMatrix fourMomentumConstraintCov, + int maxIterations, + double tolerance) { + + int nTracks = inputTracks.size(); + int nTrackParams = 5; + int nVertexParams = 3; + int stateSize = nVertexParams + nTracks * nTrackParams; + + // Number of constraints: 2 per track (transverse + longitudinal) + 3 momentum constraints + // Note: We only constrain 3-momentum, not energy, because for non-collinear tracks + // sum(E_i) > E_beam due to triangle inequality, making energy constraint unphysical + // + // Track constraints are SOFT (covariance propagated from track errors): + // transverse: r - R = 0 (vertex must lie on the helix circle in XY) + // longitudinal: zV - z_predicted = 0 + int nTrackConstraints = 2 * nTracks; + int nMomConstraints = (fourMomentumConstraint != null) ? 3 : 0; + int nConstraints = nTrackConstraints + nMomConstraints; + + // Make working copies of tracks + List tracks = new ArrayList<>(); + for (TrackParams t : inputTracks) { + tracks.add(t.copy()); + } + + // Build initial state vector x0 = [vertex, track1, track2, ...] + RealVector x0 = MatrixUtils.createRealVector(new double[stateSize]); + + // Initial vertex from constraint or average of track perigees + if (vertexConstraint != null) { + x0.setEntry(0, vertexConstraint.getEntry(0)); + x0.setEntry(1, vertexConstraint.getEntry(1)); + x0.setEntry(2, vertexConstraint.getEntry(2)); + } else { + double xInit = 0, yInit = 0, zInit = 0; + for (TrackParams t : tracks) { + xInit += -t.d0 * FastMath.sin(t.phi0); + yInit += t.d0 * FastMath.cos(t.phi0); + zInit += t.z0; + } + x0.setEntry(0, xInit / nTracks); + x0.setEntry(1, yInit / nTracks); + x0.setEntry(2, zInit / nTracks); + } + + // Initial track parameters + for (int i = 0; i < nTracks; i++) { + TrackParams t = tracks.get(i); + int offset = nVertexParams + i * nTrackParams; + x0.setEntry(offset + 0, t.d0); + x0.setEntry(offset + 1, t.phi0); + x0.setEntry(offset + 2, t.omega); + x0.setEntry(offset + 3, t.z0); + x0.setEntry(offset + 4, t.tanLambda); + } + + // Build weight matrix W = inverse of initial covariance (block diagonal) + RealMatrix W = MatrixUtils.createRealMatrix(stateSize, stateSize); + + // Vertex part + if (vertexConstraintCov != null) { + // Threshold 0.0: a tight-but-well-conditioned diagonal beamspot prior (e.g. a + // few-micron beamSize, variance ~1e-12) is not actually singular, but LUDecomposition's + // default 1e-11 threshold flags small-magnitude (not small-rank) pivots as singular -- + // unlike fitBillior1985's inversion (hep.physics.matrix.MatrixOp), which has no such + // absolute-scale threshold and handles the same tight priors fine. + RealMatrix vertexCovInv; + try { + vertexCovInv = new LUDecomposition(vertexConstraintCov, 0.0).getSolver().getInverse(); + } catch (SingularMatrixException e) { + return null; // Genuinely singular vertex constraint covariance; skip this event silently + } + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + W.setEntry(i, j, vertexCovInv.getEntry(i, j)); + } else { + // Weak constraint if no beamspot + for (int i = 0; i < 3; i++) + W.setEntry(i, i, 0.01); + } + + // Track parts + for (int i = 0; i < nTracks; i++) { + int offset = nVertexParams + i * nTrackParams; + RealMatrix trackCovInv; + try { + trackCovInv = new LUDecomposition(tracks.get(i).cov).getSolver().getInverse(); + } catch (SingularMatrixException e) { + return null; // Cannot build weight matrix; skip this event silently + } + for (int a = 0; a < nTrackParams; a++) + for (int b = 0; b < nTrackParams; b++) + W.setEntry(offset + a, offset + b, trackCovInv.getEntry(a, b)); + } + + // Also need W^-1 for the solution + RealMatrix WInv = MatrixUtils.createRealMatrix(stateSize, stateSize); + if (vertexConstraintCov != null) { + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + WInv.setEntry(i, j, vertexConstraintCov.getEntry(i, j)); + } else { + for (int i = 0; i < 3; i++) + WInv.setEntry(i, i, 100.0); + } + for (int i = 0; i < nTracks; i++) { + int offset = nVertexParams + i * nTrackParams; + RealMatrix tCov = tracks.get(i).cov; + for (int a = 0; a < nTrackParams; a++) + for (int b = 0; b < nTrackParams; b++) + WInv.setEntry(offset + a, offset + b, tCov.getEntry(a, b)); + } + + // Current state (start at x0) + RealVector x = x0.copy(); + + // Storage for final-iteration constraint system (used for post-fit covariance and chi2) + RealMatrix finalH = null; + RealMatrix finalV = null; + RealVector finalHvec = null; + + if (debugFlag) { + System.out.println("=== fitSoftConstrained ==="); + System.out.println(" State size: " + stateSize + ", Constraints: " + nConstraints); + } + + // Iterative solution using Newton-Raphson + for (int iteration = 0; iteration < maxIterations; iteration++) { + RealVector xOld = x.copy(); + + // Extract vertex from state + RealVector vertex = x.getSubVector(0, 3); + + // Update track parameters from state + for (int i = 0; i < nTracks; i++) { + int offset = nVertexParams + i * nTrackParams; + tracks.get(i).d0 = x.getEntry(offset + 0); + tracks.get(i).phi0 = x.getEntry(offset + 1); + tracks.get(i).omega = x.getEntry(offset + 2); + tracks.get(i).z0 = x.getEntry(offset + 3); + tracks.get(i).tanLambda = x.getEntry(offset + 4); + } + + // Compute constraint residuals h(x) and Jacobian H = dh/dx + RealVector h = MatrixUtils.createRealVector(new double[nConstraints]); + RealMatrix H = MatrixUtils.createRealMatrix(nConstraints, stateSize); + + // Track constraints: 2 per track + // row 2*i: transverse h_t = r - R (vertex on helix circle in XY) + // row 2*i+1: longitudinal h_z = zV - z_predicted + for (int i = 0; i < nTracks; i++) { + TrackParams track = tracks.get(i); + int offset = nVertexParams + i * nTrackParams; + int rowT = 2 * i; + int rowZ = 2 * i + 1; + + double xV = vertex.getEntry(0); + double yV = vertex.getEntry(1); + double zV = vertex.getEntry(2); + + VertexParams vp = perigeeToVertexParams(track, xV, yV); + + double R = 1.0 / FastMath.abs(track.omega); + double sign = FastMath.signum(track.omega); + + double xc = sign * R * FastMath.sin(track.phi0) - track.d0 * FastMath.sin(track.phi0); + double yc = -sign * R * FastMath.cos(track.phi0) + track.d0 * FastMath.cos(track.phi0); + double dx = xV - xc; + double dy = yV - yc; + double r2 = dx * dx + dy * dy; + double r = FastMath.sqrt(r2); + + // Constraint residuals + h.setEntry(rowT, r - R); + h.setEntry(rowZ, zV - vp.zV); + + // --- Transverse constraint Jacobian: d(r-R)/d(state) --- + // w.r.t. vertex + H.setEntry(rowT, 0, dx / r); + H.setEntry(rowT, 1, dy / r); + H.setEntry(rowT, 2, 0.0); + // w.r.t. track params + double dftDd0 = (dx * FastMath.sin(track.phi0) - dy * FastMath.cos(track.phi0)) / r; + double dftDphi0 = -(sign * R - track.d0) * (dx * FastMath.cos(track.phi0) + dy * FastMath.sin(track.phi0)) / r; + double dftDomega = (dx * FastMath.sin(track.phi0) - dy * FastMath.cos(track.phi0)) / (r * track.omega * track.omega) + + sign / (track.omega * track.omega); + H.setEntry(rowT, offset + 0, dftDd0); + H.setEntry(rowT, offset + 1, dftDphi0); + H.setEntry(rowT, offset + 2, dftDomega); + H.setEntry(rowT, offset + 3, 0.0); + H.setEntry(rowT, offset + 4, 0.0); + + // --- Longitudinal constraint Jacobian: d(zV - zPred)/d(state) --- + // w.r.t. vertex + double dphiDx = -dy / r2; + double dphiDy = dx / r2; + double dzPredDx = -sign * R * track.tanLambda * dphiDx; + double dzPredDy = -sign * R * track.tanLambda * dphiDy; + H.setEntry(rowZ, 0, -dzPredDx); + H.setEntry(rowZ, 1, -dzPredDy); + H.setEntry(rowZ, 2, 1.0); + // w.r.t. track params + double dphiDd0 = -(FastMath.cos(track.phi0) * dx + FastMath.sin(track.phi0) * dy) / r2; + double dphiDphi0 = (sign * R - track.d0) * (dy * FastMath.cos(track.phi0) - dx * FastMath.sin(track.phi0)) / r2; + double dphiDomega = -R * R * (FastMath.cos(track.phi0) * dx + FastMath.sin(track.phi0) * dy) / r2; + double phiV = FastMath.atan2(-dx * sign, dy * sign); + double dphi_s = phiV - track.phi0; + while (dphi_s > FastMath.PI) dphi_s -= 2.0 * FastMath.PI; + while (dphi_s < -FastMath.PI) dphi_s += 2.0 * FastMath.PI; + double s = -sign * R * dphi_s; + // z_pred = z0 + s*tanLambda with s = -sign(omega)*R*dphi: each R*(dphi-derivative) + // term below picks up the same -sign factor; the explicit s/omega term does not. + double dzPredDd0 = -sign * track.tanLambda * R * dphiDd0; + double dzPredDphi0 = -sign * (-track.tanLambda * R + track.tanLambda * R * dphiDphi0); + double dzPredDomega = -s * track.tanLambda / track.omega - sign * track.tanLambda * R * dphiDomega; + H.setEntry(rowZ, offset + 0, -dzPredDd0); + H.setEntry(rowZ, offset + 1, -dzPredDphi0); + H.setEntry(rowZ, offset + 2, -dzPredDomega); + H.setEntry(rowZ, offset + 3, -1.0); + H.setEntry(rowZ, offset + 4, -s); + } + + // 3-momentum constraints: totalP - beamP = 0 (no energy constraint) + if (fourMomentumConstraint != null) { + RealVector totalP = MatrixUtils.createRealVector(new double[3]); + + // First pass: compute total 3-momentum + for (int itrk = 0; itrk < nTracks; itrk++) { + TrackParams track = tracks.get(itrk); + RealVector p = computeMomentumAtVertex(track, vertex); + + totalP.addToEntry(0, p.getEntry(0)); + totalP.addToEntry(1, p.getEntry(1)); + totalP.addToEntry(2, p.getEntry(2)); + } + + // Constraint residuals (3-momentum only) + for (int j = 0; j < 3; j++) { + h.setEntry(nTrackConstraints + j, totalP.getEntry(j) - fourMomentumConstraint.getEntry(j)); + } + + // Second pass: compute Jacobian for 3-momentum constraints + for (int itrk = 0; itrk < nTracks; itrk++) { + TrackParams track = tracks.get(itrk); + int offset = nVertexParams + itrk * nTrackParams; + + double xV = vertex.getEntry(0); + double yV = vertex.getEntry(1); + VertexParams vp = perigeeToVertexParams(track, xV, yV); + double R = 1.0 / FastMath.abs(track.omega); + double sign = FastMath.signum(track.omega); + double pT = 2.99792458e-4 * FastMath.abs(bField) / FastMath.abs(track.omega); + + double xc = sign * R * FastMath.sin(track.phi0) - track.d0 * FastMath.sin(track.phi0); + double yc = -sign * R * FastMath.cos(track.phi0) + track.d0 * FastMath.cos(track.phi0); + double dx = xV - xc; + double dy = yV - yc; + double r2 = dx * dx + dy * dy; + + // dp/dvertex + double dphiDx = -dy / r2; + double dphiDy = dx / r2; + + double dpxDxV = -pT * FastMath.sin(vp.phiV) * dphiDx; + double dpxDyV = -pT * FastMath.sin(vp.phiV) * dphiDy; + double dpyDxV = pT * FastMath.cos(vp.phiV) * dphiDx; + double dpyDyV = pT * FastMath.cos(vp.phiV) * dphiDy; + + H.addToEntry(nTrackConstraints + 0, 0, dpxDxV); + H.addToEntry(nTrackConstraints + 0, 1, dpxDyV); + H.addToEntry(nTrackConstraints + 1, 0, dpyDxV); + H.addToEntry(nTrackConstraints + 1, 1, dpyDyV); + // pz doesn't depend on vertex position + + // dp/dtrack (phi0, omega, tanLambda) + double dphiDphi0 = (sign * R - track.d0) * (dy * FastMath.cos(track.phi0) - dx * FastMath.sin(track.phi0)) / r2; + double dphiDomega = -R * R * (FastMath.cos(track.phi0) * dx + FastMath.sin(track.phi0) * dy) / r2; + double dpTDomega = -2.99792458e-4 * FastMath.abs(bField) * sign / (track.omega * track.omega); + + double dpxDphi0 = -pT * FastMath.sin(vp.phiV) * dphiDphi0; + double dpxDomega = FastMath.cos(vp.phiV) * dpTDomega - pT * FastMath.sin(vp.phiV) * dphiDomega; + double dpyDphi0 = pT * FastMath.cos(vp.phiV) * dphiDphi0; + double dpyDomega = FastMath.sin(vp.phiV) * dpTDomega + pT * FastMath.cos(vp.phiV) * dphiDomega; + double dpzDomega = track.tanLambda * dpTDomega; + double dpzDtl = pT; + + H.addToEntry(nTrackConstraints + 0, offset + 1, dpxDphi0); + H.addToEntry(nTrackConstraints + 0, offset + 2, dpxDomega); + H.addToEntry(nTrackConstraints + 1, offset + 1, dpyDphi0); + H.addToEntry(nTrackConstraints + 1, offset + 2, dpyDomega); + H.addToEntry(nTrackConstraints + 2, offset + 2, dpzDomega); + H.addToEntry(nTrackConstraints + 2, offset + 4, dpzDtl); + } + } + + if (debugFlag) { + System.out.printf(" Iteration %d: |h| = %.6f%n", iteration, h.getNorm()); + for (int i = 0; i < nConstraints; i++) { + System.out.printf(" h[%d] = %.6f%n", i, h.getEntry(i)); + } + } + + // Build constraint covariance matrix V for soft constraints. + // The geometric track constraints (h_t = r-R, h_z = zV-zPred) are exact + // functions of the *same* state x whose track-parameter block is already + // weighted by W via the (x-x0) measurement term. Softening them with V = + // J_h*trackCov*J_h^T (as this used to do) double-counts that same track + // covariance a second time -- J_h here is exactly the track-parameter block + // of H above, so V was just H_trk*trackCov*H_trk^T, the same information + // already present in W^-1. That inflated the reported posterior vertex + // covariance (confirmed via toy-MC pulls: std ~0.7 instead of 1, for both + // hard and soft momentum-constraint modes, since this track-level V was + // added unconditionally regardless of the momentum-constraint softness). + // These constraints are therefore treated as effectively hard instead, + // regularized only by a tiny fixed epsilon (a small fraction of H*WInv*H^T's + // own diagonal scale) purely to avoid the near-singular case the original + // code was guarding against: two tracks with nearly identical |tanLambda| + // make the two longitudinal constraints nearly degenerate. Only the momentum + // constraint, when a real fourMomentumConstraintCov is supplied, represents + // genuinely independent information (the beam momentum uncertainty) and + // keeps its own physical covariance below. + RealMatrix constraintCov = MatrixUtils.createRealMatrix(nConstraints, nConstraints); + + RealMatrix HWInvHTforEps = H.multiply(WInv).multiply(H.transpose()); + double diagScale = 0.0; + for (int i = 0; i < nConstraints; i++) { + diagScale += HWInvHTforEps.getEntry(i, i); + } + diagScale = (nConstraints > 0) ? diagScale / nConstraints : 1.0; + // Scale factor confirmed via a fitBillior1985-vs-fitSoftConstrained (momentum + // constraint off) toy-MC agreement test: at 1e-6 the leftover geometric-constraint + // residual h was not actually driven near zero (|h| ~ 1e-3-1e-2), so the h^T V^-1 h + // term below priced that residual as if epsilon were a real measurement covariance, + // producing a consistent ~10-19% chi2 bias relative to fitBillior1985's exact + // (hard-constraint) elimination -- worse, not better, since the *smaller* term + // (the missing chi2 relative to Billior) was mostly hidden in an under-converged + // pull term, not in this block. Shrinking to 1e-10 drives |h| far closer to zero, + // reducing the max chi2 disagreement to sub-percent across 200 toys, without + // reintroducing the singularity this regularization guards against (the full + // TrackConstraintVertexFitterTest suite -- 22 tests, all fitSoftConstrained/ + // fitLagrangeMultiplier call sites in this file -- still passes at this scale). + double epsilon = (diagScale > 0) ? diagScale * 1e-10 : 1e-12; + + for (int i = 0; i < nTrackConstraints; i++) { + constraintCov.setEntry(i, i, epsilon); + } + + if (debugFlag && iteration == 0) { + System.out.printf(" Track constraint regularization epsilon = %.3e%n", epsilon); + } + + // Momentum constraint covariances (if applicable) + if (fourMomentumConstraint != null && fourMomentumConstraintCov != null) { + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + constraintCov.setEntry(nTrackConstraints + i, nTrackConstraints + j, + fourMomentumConstraintCov.getEntry(i, j)); + } + } + } + + // Save constraint system at current x for post-fit covariance and chi2 + finalH = H; + finalV = constraintCov; + finalHvec = h; + + // Solve the KKT system using block elimination with SOFT CONSTRAINTS: + // For soft constraints with covariance V, we solve: + // min (x-x0)^T W (x-x0) + h^T V^-1 h + // + // This modifies the standard Lagrange multiplier solution by adding V to H W^-1 H^T: + // λ = (H W^-1 H^T + V)^-1 (h + H(x0 - x)) + // dx = (x0 - x) - W^-1 H^T λ + // + // V = J·Cov·J^T is needed for numerical regularization: for a V0 where both tracks + // have similar |tanλ|, the two longitudinal vertex-Z constraints are nearly degenerate + // and H W^-1 H^T becomes singular without V on the diagonal. + + try { + RealMatrix HWInvHT = H.multiply(WInv).multiply(H.transpose()); + // Add constraint covariance for regularization + RealMatrix HWInvHT_plus_V = HWInvHT.add(constraintCov); + RealVector rhs = h.add(H.operate(x0.subtract(x))); + + RealVector lambda = new LUDecomposition(HWInvHT_plus_V).getSolver().solve(rhs); + RealVector deltaX = x0.subtract(x).subtract(WInv.multiply(H.transpose()).operate(lambda)); + + x = x.add(deltaX); + + if (debugFlag) { + System.out.printf(" |deltaX| = %.6f%n", deltaX.getNorm()); + } + + // Check convergence + if (deltaX.getNorm() < tolerance && h.getNorm() < tolerance * 10) { + if (debugFlag) { + System.out.println(" Converged at iteration " + iteration); + } + break; + } + } catch (Exception e) { + if (debugFlag) { + System.out.println(" Matrix inversion failed: " + e.getMessage()); + } + break; + } + } + + // Extract final results + RealVector vertex = x.getSubVector(0, 3); + + // Post-fit covariance: C = W^{-1} - W^{-1} H^T (H W^{-1} H^T + V)^{-1} H W^{-1} + // This is the full stateSize x stateSize covariance after all constraints are applied. + // K = W^{-1} H^T S^{-1} where S = H W^{-1} H^T + V + RealMatrix C_fitted = WInv.copy(); + if (finalH != null) { + try { + RealMatrix S = finalH.multiply(WInv).multiply(finalH.transpose()).add(finalV); + RealMatrix K = WInv.multiply(finalH.transpose()) + .multiply(new LUDecomposition(S).getSolver().getInverse()); + C_fitted = WInv.subtract(K.multiply(finalH).multiply(WInv)); + } catch (Exception e) { + if (debugFlag) System.out.println(" Post-fit covariance failed: " + e.getMessage()); + } + } + + // Extract fitted track parameters with post-fit covariances + List fittedTracks = new ArrayList<>(); + for (int i = 0; i < nTracks; i++) { + int offset = nVertexParams + i * nTrackParams; + double d0 = x.getEntry(offset + 0); + double phi0 = x.getEntry(offset + 1); + double omega = x.getEntry(offset + 2); + double z0 = x.getEntry(offset + 3); + double tanLambda = x.getEntry(offset + 4); + RealMatrix trackCovFitted = C_fitted.getSubMatrix( + offset, offset + nTrackParams - 1, + offset, offset + nTrackParams - 1); + fittedTracks.add(new TrackParams(d0, phi0, omega, z0, tanLambda, trackCovFitted)); + } + + // Full chi2 = parameter pulls + constraint residuals + // (x-x0)^T W (x-x0) + h^T V^{-1} h + // The second term is computed per block for numerical stability. + RealVector dx = x.subtract(x0); + double chi2 = dx.dotProduct(W.operate(dx)); + if (finalHvec != null && finalV != null) { + for (int i = 0; i < nTracks; i++) { + int rowT = 2 * i, rowZ = 2 * i + 1; + RealMatrix Vblock = finalV.getSubMatrix(rowT, rowZ, rowT, rowZ); + RealVector hblock = MatrixUtils.createRealVector(new double[]{ + finalHvec.getEntry(rowT), finalHvec.getEntry(rowZ)}); + try { + chi2 += hblock.dotProduct(new LUDecomposition(Vblock).getSolver().solve(hblock)); + } catch (Exception e) { /* skip singular block */ } + } + if (fourMomentumConstraint != null && fourMomentumConstraintCov != null) { + RealMatrix Vblock = finalV.getSubMatrix( + nTrackConstraints, nTrackConstraints + 2, + nTrackConstraints, nTrackConstraints + 2); + RealVector hblock = finalHvec.getSubVector(nTrackConstraints, 3); + try { + chi2 += hblock.dotProduct(new LUDecomposition(Vblock).getSolver().solve(hblock)); + } catch (Exception e) { /* skip singular block */ } + } + } + + // NDF = number of constraints minus the vertex-position dof they determine + // (vertex has only a weak/free prior in W, so all 3 of its dof are absorbed + // by the constraints rather than by a measurement -- matches the ndf convention + // used elsewhere in this file, e.g. fitCascadeVertex's `2*nTracks-3`). + int ndf = nConstraints - nVertexParams; + + // Compute final track momenta using fitted track parameters and their post-fit covariances + List trackMomenta = new ArrayList<>(); + double me = 0.000511; + for (TrackParams track : fittedTracks) { + RealVector p = computeMomentumAtVertex(track, vertex); + RealMatrix pCov = computeMomentumCovariance(track, vertex); + trackMomenta.add(new TrackMomentum(p, pCov)); + } + + // Total (summed) fitted momentum and its covariance, correctly propagated through the + // FULL post-fit state covariance C_fitted -- unlike each track's own pCov above (which + // only uses that track's diagonal block), this includes the cross-track and + // vertex-momentum correlation blocks induced by the shared vertex and (for the + // momentum-constrained fits) shared momentum-sum constraint. Built via + // Cov(totalP) = J^T C_fitted J, where J (stateSize x 3) stacks the vertex-block + // Jacobian (dP_total/d(vertex), summed over tracks) and each track's own + // dP_track/d(track params) block (computeMomentumTrackJacobian). + RealVector totalPTracking = MatrixUtils.createRealVector(new double[3]); + RealMatrix vertexPBlock = MatrixUtils.createRealMatrix(3, 3); + RealMatrix Jtotal = MatrixUtils.createRealMatrix(stateSize, 3); + for (int i = 0; i < nTracks; i++) { + TrackParams track = fittedTracks.get(i); + totalPTracking = totalPTracking.add(computeMomentumAtVertex(track, vertex)); + vertexPBlock = vertexPBlock.add(computeMomentumVertexDerivatives(track, vertex)); + RealMatrix Jp = computeMomentumTrackJacobian(track, vertex); + int offset = nVertexParams + i * nTrackParams; + Jtotal.setSubMatrix(Jp.transpose().getData(), offset, 0); + } + Jtotal.setSubMatrix(vertexPBlock.transpose().getData(), 0, 0); + RealMatrix totalPCovTracking = Jtotal.transpose().multiply(C_fitted).multiply(Jtotal); + + // Vertex covariance: upper-left 3x3 block of the post-fit covariance + RealMatrix vertexCov = C_fitted.getSubMatrix(0, 2, 0, 2); + + if (debugFlag) { + System.out.printf(" Final vertex: [%.4f, %.4f, %.4f]%n", + vertex.getEntry(0), vertex.getEntry(1), vertex.getEntry(2)); + System.out.printf(" Chi2: %.4f, NDF: %d, Chi2/NDF: %.2f%n", chi2, ndf, chi2/ndf); + + // Show track parameter changes vs uncertainties + String[] paramNames = {"d0", "phi0", "omega", "z0", "tanL"}; + for (int i = 0; i < nTracks; i++) { + int offset = nVertexParams + i * nTrackParams; + System.out.printf(" Track %d parameter pulls (change/sigma):%n", i); + double trackChi2 = 0; + for (int p = 0; p < nTrackParams; p++) { + double change = x.getEntry(offset + p) - x0.getEntry(offset + p); + double sigma = FastMath.sqrt(inputTracks.get(i).cov.getEntry(p, p)); + double pull = change / sigma; + trackChi2 += pull * pull; + System.out.printf(" %5s: change=%12.6f, sigma=%12.6f, pull=%8.2f%n", + paramNames[p], change, sigma, pull); + } + System.out.printf(" Track %d chi2 contribution (diagonal only): %.2f%n", i, trackChi2); + } + + // Show vertex change + System.out.printf(" Vertex change: [%.4f, %.4f, %.4f]%n", + x.getEntry(0) - x0.getEntry(0), + x.getEntry(1) - x0.getEntry(1), + x.getEntry(2) - x0.getEntry(2)); + + RealVector totalP = MatrixUtils.createRealVector(new double[3]); + double totalE = 0; + for (TrackMomentum tm : trackMomenta) { + totalP = totalP.add(tm.p); + totalE += FastMath.sqrt(tm.pMag * tm.pMag + me * me); + } + System.out.printf(" Total fitted 4-momentum: [%.4f, %.4f, %.4f, %.4f]%n", + totalP.getEntry(0), totalP.getEntry(1), totalP.getEntry(2), totalE); + if (fourMomentumConstraint != null) { + System.out.printf(" Beam 4-momentum: [%.4f, %.4f, %.4f, %.4f]%n", + fourMomentumConstraint.getEntry(0), fourMomentumConstraint.getEntry(1), + fourMomentumConstraint.getEntry(2), fourMomentumConstraint.getEntry(3)); + } + } + + FitResult fitResult = new FitResult(vertex, vertexCov, chi2, ndf, trackMomenta, fittedTracks); + fitResult.totalMomentum = totalPTracking; + fitResult.totalMomentumCov = totalPCovTracking; + return fitResult; + } + + /** + * Joint kinematic fit with an EXACT (hard) beam momentum constraint via the + * Lagrange multiplier method, combined with soft track-helix constraints. + * + *

Solves the mixed constrained optimisation problem: + * 

+     *   minimise  (x - x0)^T W (x - x0) + sum_i h_track_i^T V_track_i^{-1} h_track_i
+     *   subject to  h_mom(x) = total_p(x) - beamMomentum = 0  (exactly)
+     *
+ * where x = [vertex, track_1, ..., track_N] is the full state vector. + * + *

Implemented via the same unified KKT system as {@link #fitSoftConstrained}: + * 

+     *   (H W^{-1} H^T + V_mixed) lambda = rhs
+     *
+ * but with V_mixed = diag(V_track_1, ..., V_track_N, 0): the zero block on the + * momentum rows enforces that constraint exactly rather than softly weighting it + * by a beam momentum uncertainty. This recovers the classical Lagrange multiplier + * solution for the momentum constraint while keeping the track-helix constraints + * soft (as is physically appropriate given measurement errors). + * + * @param inputTracks List of track parameters + * @param vertexConstraint Beamspot position prior (null for weak 100 mm prior) + * @param vertexConstraintCov Beamspot position covariance (null for weak prior) + * @param beamMomentum Exact beam 3-momentum [px, py, pz] in GeV (4-vector accepted; only first 3 used) + * @param maxIterations Maximum Newton-Raphson iterations + * @param tolerance Convergence tolerance + * @return FitResult with vertex, track parameters, chi2, and momenta + * + * @deprecated The beam momentum is not actually conserved exactly by the tracked + * leptons alone -- some momentum (~18.6 MeV transverse, see + * {@link #setBeamMomentumTransverseNuclearRecoilSigma(double)}) is carried away by + * the target nuclear recoil. Enforcing V_mom=0 exactly therefore fits an equality + * that is not physically true, which structurally cannot be fixed by covariance + * tuning (see {@code ntrack_beam_momentum_constraint_result.md}: n=123 real + * candidates showed hard-mode chi2/ndf completely unchanged by the nuclear-recoil + * covariance widening that fixes soft mode, since that widening has no effect when + * V_mom is hardcoded to zero). Use {@link #fitSoftConstrained} (or + * {@link NTrackVertexer#fitVertexBeamConstrained} with + * {@code hardMomentumConstraint=false}) with a tuned + * {@code setBeamMomentumTransverseNuclearRecoilSigma} instead. Kept for + * reference/regression comparison, not recommended for new production use. + */ + @Deprecated + public FitResult fitLagrangeMultiplier(List inputTracks, + RealVector vertexConstraint, + RealMatrix vertexConstraintCov, + RealVector beamMomentum, + int maxIterations, + double tolerance) { + // Passing null for fourMomentumConstraintCov leaves the momentum block of the + // KKT constraint covariance matrix as zero (V_mom = 0), which enforces the + // momentum constraint exactly as a hard Lagrange multiplier constraint, in + // contrast to fitSoftConstrained() which fills that block with the beam + // momentum uncertainty and satisfies the constraint only approximately. + return fitSoftConstrained(inputTracks, vertexConstraint, vertexConstraintCov, + beamMomentum, null, maxIterations, tolerance); + } + + // Configurable fields for BilliorVertexer-style interface + private double[] beamSize = {0.001, 0.150, 0.050}; + private double[] beamPosition = {-1.1, 0, 0}; + private double[] referencePosition = {0.0, 0.0, 0.0}; // tracking frame offset added to output vertex + private double pBeam = 3.74; + private double rotAngle = -0.030; + private boolean debugFlag = false; + private boolean storeCovTrkMomList = false; + // Additional transverse beam-momentum-constraint width (GeV), combined in quadrature with + // the beam-divergence term below. Default 0 reproduces the original divergence-only + // covariance exactly. Non-zero values represent event-to-event transverse momentum not + // carried by the tracked leptons -- primarily momentum transferred to the target nucleus + // during production (nuclear recoil; distinct from a recoil electron from radiative/A' + // events) -- measured directly from trident MC truth (std(mcTotalPx), std(mcTotalPy)) at + // ~18.5-18.8 MeV, vs. the ~0.37 MeV implied by 100 urad beam divergence alone. + private double sigmaTNuclearRecoil = 0.0; + + public void setBeamSize(double[] bs) { this.beamSize = bs; } + public void setBeamPosition(double[] bp) { this.beamPosition = bp; } + public double[] getBeamSize() { return beamSize; } + public double[] getBeamPosition() { return beamPosition; } + public void setReferencePosition(double[] rp) { this.referencePosition = rp.clone(); } + public void setBeamEnergy(double energy) { this.pBeam = energy; } + public void setBeamRotAngle(double angle) { this.rotAngle = angle; } + public void setBeamMomentumTransverseNuclearRecoilSigma(double sigma) { this.sigmaTNuclearRecoil = sigma; } + public void setDebug(boolean debug) { this.debugFlag = debug; } + public void setStoreCovTrkMomList(boolean value) { this.storeCovTrkMomList = value; } + + /** + * Beam 3-momentum in the tracking frame (x=beam, y=horiz, z=vert), from this fitter's + * {@code pBeam}/{@code rotAngle} fields. Extracted out of {@link #fitVertex}'s inline + * beam-momentum-constraint setup so the same vector can be reused by + * {@link #fitCascadeVertexJointBeamConstrainedCore}. + */ + private RealVector beamMomentumVector() { + double pxBeam = pBeam * FastMath.cos(rotAngle); // tracking X = HPS Z + double pyBeam = -pBeam * FastMath.sin(rotAngle); // tracking Y = HPS X + double pzBeam = 0.0; // tracking Z = HPS Y + return MatrixUtils.createRealVector(new double[]{pxBeam, pyBeam, pzBeam}); + } + + /** + * Covariance (3x3, tracking frame) of {@link #beamMomentumVector}, from 1% dp/p + * (longitudinal) combined in quadrature with the beam angular divergence and + * {@code sigmaTNuclearRecoil} (transverse). Extracted out of {@link #fitVertex}'s inline + * beam-momentum-constraint setup, dropping the energy row/column that method also builds + * (not needed here, or by {@link #fitCascadeVertexJointBeamConstrainedCore} -- like + * {@link #fitSoftConstrained}, both constrain only 3-momentum, not energy). + */ + private RealMatrix beamMomentumCovarianceMatrix() { + double dpOverP = 1e-2; + double sigmaTheta = 100e-6; // beam angular divergence (rad) + double sigmaL = dpOverP * pBeam; + double sigmaT = FastMath.hypot(sigmaTheta * pBeam, sigmaTNuclearRecoil); + double cosR = FastMath.cos(rotAngle); + double sinR = FastMath.sin(rotAngle); + double sL2 = sigmaL * sigmaL; + double sT2 = sigmaT * sigmaT; + RealMatrix cov = MatrixUtils.createRealMatrix(3, 3); + cov.setEntry(0, 0, sL2 * cosR * cosR + sT2 * sinR * sinR); + cov.setEntry(0, 1, (sT2 - sL2) * sinR * cosR); + cov.setEntry(1, 0, (sT2 - sL2) * sinR * cosR); + cov.setEntry(1, 1, sL2 * sinR * sinR + sT2 * cosR * cosR); + cov.setEntry(2, 2, sT2); + return cov; + } + + /** + * Vertex-only fit following Billior (NIM A225, 1984) / Billior & Qian (NIM A311, 1992). + * Direct translation of BilliorVertexer.follow1985Paper, using TrackParams (LCIO helix). + * + * TrackParams (LCIO: d0, phi0, omega, z0, tanL) are converted internally to Billior + * parameterization (eps=-d0, z0, theta=PI/2-atan(tanL), phi0, rho=omega). The vertex + * is solved in the tracking frame (x=beam, y=horiz, z=vert) by marginalising the three + * momentum parameters (theta, phiV, rho) analytically via the Schur complement of the + * full 5x5 track weight matrix. + * + * @param tracks Track parameters (LCIO helix, reference near vertex). + * @param vertexConstraintVec Beamspot centre in tracking frame, or null. + * @param vertexConstraintCov Beamspot covariance, or null. + * @return FitResult, or null on numerical failure. + */ + public FitResult fitBillior1985(List tracks, + RealVector vertexConstraintVec, + RealMatrix vertexConstraintCov) { + int nTracks = tracks.size(); + if (nTracks < 2) return null; + + // fieldConversion: same as org.lcsim.constants.Constants.fieldConversion + final double fieldConv = 2.99792458e-4; + + // ----------------------------------------------------------------------- + // Step 1: Convert LCIO TrackParams → Billior parameterization per track + // + // LCIO order: (d0, phi0, omega, z0, tanL) indices 0,1,2,3,4 + // Billior order: (eps=-d0, z0, theta, phi0, rho=omega) indices 0,1,2,3,4 + // where theta = PI/2 - atan(tanL) + // + // Covariance transforms via diagonal Jacobian J: + // J[0][0]=-1 J[1][3]=1 J[2][4]=-1/(1+tanL^2) J[3][1]=1 J[4][2]=1 + // ----------------------------------------------------------------------- + double[][] bPar = new double[nTracks][5]; + BasicMatrix[] bCov = new BasicMatrix[nTracks]; + + for (int t = 0; t < nTracks; t++) { + TrackParams tp = tracks.get(t); + double tanL = tp.tanLambda; + bPar[t][0] = -tp.d0; + bPar[t][1] = tp.z0; + bPar[t][2] = Math.PI / 2.0 - Math.atan(tanL); + bPar[t][3] = tp.phi0; + bPar[t][4] = tp.omega; + + // Build Jacobian J (Billior row, LCIO column) + BasicMatrix J = new BasicMatrix(5, 5); + J.setElement(0, 0, -1.0); + J.setElement(1, 3, 1.0); + J.setElement(2, 4, -1.0 / (1.0 + tanL * tanL)); + J.setElement(3, 1, 1.0); + J.setElement(4, 2, 1.0); + + // Copy LCIO covariance into BasicMatrix + BasicMatrix lcioC = new BasicMatrix(5, 5); + for (int r = 0; r < 5; r++) + for (int c = 0; c < 5; c++) + lcioC.setElement(r, c, tp.cov.getEntry(r, c)); + + // Billior covariance = J * lcioC * J^T + bCov[t] = (BasicMatrix) MatrixOp.mult(J, MatrixOp.mult(lcioC, MatrixOp.transposed(J))); + } + + // ----------------------------------------------------------------------- + // Step 2: Build per-track matrices at linearisation point v0 = (0,0,0) + // following Billior & Qian NIM A311 (1992) eqs. 3-9. + // + // At v0=(0,0,0): uu=vv=0, phiVert=phi0, ci=0, pis=measured Billior params. + // + // A (5x3): d(helix params)/d(vertex position) + // B (5x3): d(helix params)/d(momentum params theta,phiV,rho) + // G (5x5): inverse of Billior covariance + // Di (3x3) = A^T G B, Ei (3x3) = B^T G B + // ----------------------------------------------------------------------- + List As = new ArrayList<>(); + List Bs = new ArrayList<>(); + List Gs = new ArrayList<>(); + List pis = new ArrayList<>(); // measured Billior params as 5x1 + List Ds = new ArrayList<>(); // Di = A^T G B + List Es = new ArrayList<>(); // Ei = B^T G B + + BasicMatrix D0 = new BasicMatrix(3, 3); // D0 = sum A_i^T G_i A_i + + for (int t = 0; t < nTracks; t++) { + double theta = bPar[t][2]; + double phi0 = bPar[t][3]; // == phiVert at v0=0 + double rho = bPar[t][4]; + double cotth = 1.0 / Math.tan(theta); + double cosf = Math.cos(phi0); + double sinf = Math.sin(phi0); + + // A matrix (5x3): partial derivatives of helix params w.r.t. vertex (x,y,z) + // At v0=0, phiVert=phi0. Non-zero entries: + // eps row (0): d(eps)/dx = sin(f), d(eps)/dy = -cos(f) + // z0 row (1): d(z0)/dx = -cot*cos(f), d(z0)/dy = -cot*sin(f), d(z0)/dz=1 + // phi row (3): d(phi)/dx = -rho*cos(f), d(phi)/dy = -rho*sin(f) + BasicMatrix A = new BasicMatrix(5, 3); + A.setElement(0, 0, sinf); + A.setElement(0, 1, -cosf); + A.setElement(1, 0, -cotth * cosf); + A.setElement(1, 1, -cotth * sinf); + A.setElement(1, 2, 1.0); + A.setElement(3, 0, -rho * cosf); + A.setElement(3, 1, -rho * sinf); + + // B matrix (5x3): partial derivatives of helix params w.r.t. (theta, phiV, rho) + // At v0=0 (uu=vv=0) all uu/vv terms vanish. + BasicMatrix B = new BasicMatrix(5, 3); + B.setElement(2, 0, 1.0); // d(theta)/d(theta) + B.setElement(3, 1, 1.0); // d(phiV)/d(phiV) + B.setElement(4, 2, 1.0); // d(rho)/d(rho) + // B[0,1]=uu=0, B[0,2]=-uu^2/2=0, B[1,0]=uu*(1+cot^2)=0, + // B[1,1]=-vv*cot=0, B[1,2]=uu*vv*cot=0, B[3,2]=-uu=0 + + // G = inverse of Billior covariance + BasicMatrix G; + try { G = (BasicMatrix) MatrixOp.inverse(bCov[t]); } + catch (Exception e) { return null; } + + // Measured Billior params as 5x1 column + BasicMatrix pi = new BasicMatrix(5, 1); + for (int k = 0; k < 5; k++) pi.setElement(k, 0, bPar[t][k]); + + As.add(A); Bs.add(B); Gs.add(G); pis.add(pi); + + // Di = A^T G B (3x3), Ei = B^T G B (3x3) + BasicMatrix Di = (BasicMatrix) MatrixOp.mult(MatrixOp.transposed(A), MatrixOp.mult(G, B)); + BasicMatrix Ei = (BasicMatrix) MatrixOp.mult(MatrixOp.transposed(B), MatrixOp.mult(G, B)); + Ds.add(Di); Es.add(Ei); + + // Accumulate D0 = sum A^T G A (3x3) + BasicMatrix contrib = (BasicMatrix) MatrixOp.mult(MatrixOp.transposed(A), MatrixOp.mult(G, A)); + D0 = (BasicMatrix) MatrixOp.add(D0, contrib); + } + + // ----------------------------------------------------------------------- + // Step 3: Solve for vertex position + // + // Vertex information matrix = D0 - sum Di Ei^{-1} Di^T (Schur complement) + // Plus optional Gaussian vertex prior (beamspot constraint). + // + // bigsum (RHS) = sum (A^T G - A^T G B Ei^{-1} B^T G) p + // ----------------------------------------------------------------------- + BasicMatrix tmpInfoVtx = D0; + BasicMatrix bigsum = new BasicMatrix(3, 1); + + // Beamspot prior: add to vertex information matrix and RHS + if (vertexConstraintVec != null && vertexConstraintCov != null) { + BasicMatrix priorCov = new BasicMatrix(3, 3); + for (int r = 0; r < 3; r++) + for (int c = 0; c < 3; c++) + priorCov.setElement(r, c, vertexConstraintCov.getEntry(r, c)); + BasicMatrix priorInv; + try { priorInv = (BasicMatrix) MatrixOp.inverse(priorCov); } + catch (Exception e) { return null; } + tmpInfoVtx = (BasicMatrix) MatrixOp.add(tmpInfoVtx, priorInv); + // RHS contribution: priorInv * priorPos + BasicMatrix priorPos = new BasicMatrix(3, 1); + for (int r = 0; r < 3; r++) priorPos.setElement(r, 0, vertexConstraintVec.getEntry(r)); + bigsum = (BasicMatrix) MatrixOp.add(bigsum, MatrixOp.mult(priorInv, priorPos)); + } + + for (int i = 0; i < nTracks; i++) { + BasicMatrix A = As.get(i); + BasicMatrix B = Bs.get(i); + BasicMatrix G = Gs.get(i); + BasicMatrix p = pis.get(i); + BasicMatrix Di = Ds.get(i); + BasicMatrix Ei = Es.get(i); + BasicMatrix EiInv; + try { EiInv = (BasicMatrix) MatrixOp.inverse(Ei); } + catch (Exception e) { return null; } + + // Subtract Schur complement contribution: Di Ei^{-1} Di^T + tmpInfoVtx = (BasicMatrix) MatrixOp.add(tmpInfoVtx, + MatrixOp.mult(-1, MatrixOp.mult(Di, MatrixOp.mult(EiInv, MatrixOp.transposed(Di))))); + + // RHS: (A^T G - A^T G B Ei^{-1} B^T G) * p + BasicMatrix ATG = (BasicMatrix) MatrixOp.mult(MatrixOp.transposed(A), G); + BasicMatrix BEIBtG = (BasicMatrix) MatrixOp.mult(B, + MatrixOp.mult(EiInv, MatrixOp.mult(MatrixOp.transposed(B), G))); + BasicMatrix coeff = (BasicMatrix) MatrixOp.add(ATG, MatrixOp.mult(-1, + MatrixOp.mult(ATG, BEIBtG))); + bigsum = (BasicMatrix) MatrixOp.add(bigsum, MatrixOp.mult(coeff, p)); + } + + BasicMatrix covVtx; + try { covVtx = (BasicMatrix) MatrixOp.inverse(tmpInfoVtx); } + catch (Exception e) { return null; } + + BasicMatrix xtilde = (BasicMatrix) MatrixOp.mult(covVtx, bigsum); + + // ----------------------------------------------------------------------- + // Step 4: Compute fitted momenta and chi2 + // Following BilliorVertexer.follow1985Paper lines 1130-1188. + // + // qtilde (3x1) = -Ei^{-1} Di^T xtilde + Ei^{-1} B^T G p [eqs 22b,d] + // ptilde (5x1) = A xtilde + B qtilde [fitted helix params] + // chi2 += (p - ptilde)^T G (p - ptilde) + // pfit = (theta_fit, phiV_fit, rho_fit) = qtilde (since ci=0 at v0=0) + // ----------------------------------------------------------------------- + double chi2 = 0.0; + List trackMomenta = new ArrayList<>(); + + for (int j = 0; j < nTracks; j++) { + BasicMatrix A = As.get(j); + BasicMatrix B = Bs.get(j); + BasicMatrix G = Gs.get(j); + BasicMatrix p = pis.get(j); + BasicMatrix Di = Ds.get(j); + BasicMatrix Ei = Es.get(j); + BasicMatrix EiInv; + try { EiInv = (BasicMatrix) MatrixOp.inverse(Ei); } + catch (Exception e) { return null; } + + // qtilde: fitted momentum params (theta, phiV, rho) + BasicMatrix qtilde = (BasicMatrix) MatrixOp.add( + MatrixOp.mult(-1, MatrixOp.mult(EiInv, MatrixOp.mult(MatrixOp.transposed(Di), xtilde))), + MatrixOp.mult(EiInv, MatrixOp.mult(MatrixOp.transposed(B), MatrixOp.mult(G, p)))); + + // ptilde: predicted 5-parameter helix at fitted vertex + BasicMatrix ptilde = (BasicMatrix) MatrixOp.add( + MatrixOp.mult(A, xtilde), MatrixOp.mult(B, qtilde)); + + // Chi2 contribution: (p - ptilde)^T G (p - ptilde) + BasicMatrix residual = (BasicMatrix) MatrixOp.add(p, MatrixOp.mult(-1, ptilde)); + chi2 += MatrixOp.mult(MatrixOp.transposed(residual), + MatrixOp.mult(G, residual)).e(0, 0); + + // pfit = qtilde (ci=0 at v0=0, so qtilde_j + ci_j[2..4] = qtilde_j) + double thetaFit = qtilde.e(0, 0); + double phiVFit = qtilde.e(1, 0); + double rhoFit = qtilde.e(2, 0); + + // Convert (theta, phiV, rho) → (px, py, pz) in tracking frame + // Pt = |fieldConv * B / rho|, px = Pt*cos(phiV), py = Pt*sin(phiV), pz = Pt/tan(theta) + double Pt = Math.abs(fieldConv * bField / rhoFit); + double px = Pt * Math.cos(phiVFit); + double py = Pt * Math.sin(phiVFit); + double pz = Pt / Math.tan(thetaFit); + + // Fitted momentum covariance: propagate Cij[j][j] (theta,phiV,rho) → (px,py,pz) + // Cij[j][j] = Ei^{-1} + Ei^{-1} Di^T covVtx Di Ei^{-1} (from eq 22c) + BasicMatrix C0j = (BasicMatrix) MatrixOp.mult(-1, + MatrixOp.mult(covVtx, MatrixOp.mult(Di, EiInv))); + BasicMatrix CjjTmp = (BasicMatrix) MatrixOp.mult(-1, + MatrixOp.mult(EiInv, MatrixOp.mult(MatrixOp.transposed(Di), C0j))); + BasicMatrix Cjj = (BasicMatrix) MatrixOp.add(EiInv, CjjTmp); + + // Jacobian d(px,py,pz)/d(theta,phiV,rho) + double Bsig = fieldConv * bField; // signed B + BasicMatrix Jmom = new BasicMatrix(3, 3); + Jmom.setElement(0, 0, 0.0); + Jmom.setElement(0, 1, -Pt * Math.sin(phiVFit)); + Jmom.setElement(0, 2, -(Bsig * Math.cos(phiVFit)) / (rhoFit * rhoFit)); + Jmom.setElement(1, 0, 0.0); + Jmom.setElement(1, 1, Pt * Math.cos(phiVFit)); + Jmom.setElement(1, 2, -(Bsig * Math.sin(phiVFit)) / (rhoFit * rhoFit)); + Jmom.setElement(2, 0, -Pt * Math.pow(1.0 / Math.sin(thetaFit), 2)); + Jmom.setElement(2, 1, 0.0); + Jmom.setElement(2, 2, -(Bsig / Math.tan(thetaFit)) / (rhoFit * rhoFit)); + + BasicMatrix pCovBillior = (BasicMatrix) MatrixOp.mult(Jmom, + MatrixOp.mult(Cjj, MatrixOp.transposed(Jmom))); + + RealMatrix pCovRM = MatrixUtils.createRealMatrix(3, 3); + for (int r = 0; r < 3; r++) + for (int c = 0; c < 3; c++) + pCovRM.setEntry(r, c, pCovBillior.e(r, c)); + + RealVector pVec = MatrixUtils.createRealVector(new double[]{px, py, pz}); + trackMomenta.add(new TrackMomentum(pVec, pCovRM)); + } + + // Wrap vertex covariance into RealMatrix + RealMatrix covVtxRM = MatrixUtils.createRealMatrix(3, 3); + for (int r = 0; r < 3; r++) + for (int c = 0; c < 3; c++) + covVtxRM.setEntry(r, c, covVtx.e(r, c)); + RealVector xVtx = MatrixUtils.createRealVector(new double[]{ + xtilde.e(0, 0), xtilde.e(1, 0), xtilde.e(2, 0)}); + + int ndf = 2 * nTracks - 3; + return new FitResult(xVtx, covVtxRM, chi2, ndf, trackMomenta); + } + + /** + * Fit vertex and return a BilliorVertex for compatibility with existing code. + * Applies beamspot position constraint always; optionally applies beam momentum constraint. + * + * @param tracks List of track parameters + * @param beamConstraint If true, apply beam 4-momentum constraint in addition to beamspot + * @return BilliorVertex with fitted results + */ + public BilliorVertex fitVertex(List tracks, boolean beamConstraint) { + return fitVertex(tracks, true, beamConstraint, false); + } + + /** + * Fit vertex with independent control over beamspot position and beam momentum constraints. + * + *

Four modes are supported: + *

    + *
  • (false, false) – unconstrained: only track-helix constraints on vertex position
    • + *
  • (true, false) – beamspot only: vertex position constrained to beam spot
    • + *
  • (false, true) – beam momentum only: total 3-momentum constrained to beam value, + * no position constraint beyond the track helices
    • + *
  • (true, true) – full: beamspot position + beam 4-momentum constraints
    • + *
+ * + * @param tracks List of track parameters + * @param beamspotConstraint If true, constrain vertex position to beam spot + * @param beamMomentumConstraint If true, constrain total 3-momentum to beam value + * @return BilliorVertex with fitted results + */ + public BilliorVertex fitVertex(List tracks, boolean beamspotConstraint, boolean beamMomentumConstraint) { + return fitVertex(tracks, beamspotConstraint, beamMomentumConstraint, false); + } + + /** + * Fit vertex with independent control over beamspot, beam momentum, and whether + * the momentum constraint is applied exactly (hard/Lagrange multiplier) or softly + * (weighted by beam momentum uncertainty). + * + *

When {@code hardMomentumConstraint} is true and {@code beamMomentumConstraint} + * is true, {@link #fitLagrangeMultiplier} is called so that total 3-momentum equals + * the beam value exactly. Otherwise {@link #fitSoftConstrained} is called and the + * beam momentum uncertainty is folded into the constraint weight. + * + * @param tracks List of track parameters + * @param beamspotConstraint If true, constrain vertex position to beam spot + * @param beamMomentumConstraint If true, constrain total 3-momentum to beam value + * @param hardMomentumConstraint If true (and beamMomentumConstraint is true), enforce + * the momentum constraint exactly via Lagrange multipliers. + * Deprecated: see {@link #fitLagrangeMultiplier} -- + * the exact constraint is not physically correct (target + * nuclear recoil carries real momentum away) and this mode + * cannot be fixed by {@link #setBeamMomentumTransverseNuclearRecoilSigma}. + * Prefer {@code false} (soft mode) for new production use. + * @return BilliorVertex with fitted results + */ + public BilliorVertex fitVertex(List tracks, boolean beamspotConstraint, boolean beamMomentumConstraint, boolean hardMomentumConstraint) { + if (debugFlag) + System.out.println(" ********* starting new fitVertex ********* "); + + // Print input track 4-momenta + if (debugFlag) { + double me = 0.000511; + RealVector initVertex = MatrixUtils.createRealVector(beamPosition); + RealVector totalP = MatrixUtils.createRealVector(new double[3]); + double totalE = 0; + for (int i = 0; i < tracks.size(); i++) { + TrackParams t = tracks.get(i); + RealVector p = computeMomentumAtVertex(t, initVertex); + double pMag = p.getNorm(); + double E = FastMath.sqrt(pMag * pMag + me * me); + totalP = totalP.add(p); + totalE += E; + System.out.printf(" Input track %d: p=[%.4f, %.4f, %.4f] |p|=%.4f E=%.4f%n", + i, p.getEntry(0), p.getEntry(1), p.getEntry(2), pMag, E); + System.out.printf(" params: d0=%.4f phi0=%.4f omega=%.6f z0=%.4f tanL=%.4f%n", + t.d0, t.phi0, t.omega, t.z0, t.tanLambda); + } + System.out.printf(" Input total 4-momentum: [%.4f, %.4f, %.4f, %.4f]%n", + totalP.getEntry(0), totalP.getEntry(1), totalP.getEntry(2), totalE); + } + + // Set up vertex (beamspot) constraint + RealVector vertexConstraintVec = null; + RealMatrix vertexConstraintCovMat = null; + if (beamspotConstraint) { + vertexConstraintVec = MatrixUtils.createRealVector(beamPosition); + vertexConstraintCovMat = MatrixUtils.createRealMatrix(3, 3); + vertexConstraintCovMat.setEntry(0, 0, beamSize[0] * beamSize[0]); + vertexConstraintCovMat.setEntry(1, 1, beamSize[1] * beamSize[1]); + vertexConstraintCovMat.setEntry(2, 2, beamSize[2] * beamSize[2]); + } + + // Set up beam 4-momentum constraint + RealVector fourMomentumConstraintVec = null; + RealMatrix fourMomentumConstraintCovMat = null; + if (beamMomentumConstraint) { + // Beam momentum vector in tracking frame (X=HPS_Z, Y=HPS_X, Z=HPS_Y), detector + // frame beam along HPS Z rotated by rotAngle in the HPS X-Z plane -- see + // beamMomentumVector()/beamMomentumCovarianceMatrix() for the (extracted, + // bit-identical) formulas, also reused by fitCascadeVertexJointBeamConstrainedCore. + RealVector pBeamVec = beamMomentumVector(); + double me = 0.000511; // electron mass in GeV + double eBeam = FastMath.sqrt(pBeam * pBeam + me * me); + fourMomentumConstraintVec = MatrixUtils.createRealVector(new double[]{ + pBeamVec.getEntry(0), pBeamVec.getEntry(1), pBeamVec.getEntry(2), eBeam}); + + RealMatrix pCovMat = beamMomentumCovarianceMatrix(); + double dpOverP = 1e-2; + double sigmaE = dpOverP * eBeam; + fourMomentumConstraintCovMat = MatrixUtils.createRealMatrix(4, 4); + fourMomentumConstraintCovMat.setSubMatrix(pCovMat.getData(), 0, 0); + fourMomentumConstraintCovMat.setEntry(3, 3, sigmaE * sigmaE); + } + + // Dispatch to the appropriate fitting method. + // fitBillior1985: Billior (NIM A225, 1984) / Billior & Qian (NIM A311, 1992) — + // default for position-only fits. Full 5-param helix linearisation + // marginalising the 3 momentum DOF via Schur complement. + // fitLagrangeMultiplier: hard (exact) momentum constraint, V_mom = 0. + // fitSoftConstrained: full state-vector NR; used when a beam-momentum constraint + // is needed (adds momentum rows to the constraint system). + FitResult result; + if (beamMomentumConstraint && hardMomentumConstraint) { + result = fitLagrangeMultiplier(tracks, vertexConstraintVec, vertexConstraintCovMat, + fourMomentumConstraintVec, 10, 1e-6); + } else if (beamMomentumConstraint) { + result = fitSoftConstrained(tracks, vertexConstraintVec, vertexConstraintCovMat, + fourMomentumConstraintVec, fourMomentumConstraintCovMat, + 10, 1e-6); + } else { + // Position-only fit: Billior 1985 algorithm. + result = fitBillior1985(tracks, vertexConstraintVec, vertexConstraintCovMat); + } + + // Determine label for BilliorVertex + String label; + if (beamspotConstraint && beamMomentumConstraint && hardMomentumConstraint) { + label = "ThreeProngBSBeamHardConstrained"; + } else if (beamspotConstraint && beamMomentumConstraint) { + label = "ThreeProngBSBeamConstrained"; + } else if (beamspotConstraint) { + label = "ThreeProngBSConstrained"; + } else if (beamMomentumConstraint && hardMomentumConstraint) { + label = "ThreeProngMomHardConstrained"; + } else if (beamMomentumConstraint) { + label = "ThreeProngMomConstrained"; + } else { + label = "ThreeProngUnconstrained"; + } + + // If the fit failed (e.g. singular track covariance), return null so callers can skip. + if (result == null) return null; + + // Convert FitResult to BilliorVertex + // Tracking frame to detector frame: HPS X = TRACK Y, HPS Y = TRACK Z, HPS Z = TRACK X + // Add referencePosition offset (tracking frame) before converting — mirrors BilliorVertexer + double vtxX = result.vertex.getEntry(1) + referencePosition[1]; // tracking Y -> HPS X + double vtxY = result.vertex.getEntry(2) + referencePosition[2]; // tracking Z -> HPS Y + double vtxZ = result.vertex.getEntry(0) + referencePosition[0]; // tracking X -> HPS Z + hep.physics.vec.Hep3Vector vtxPos = new hep.physics.vec.BasicHep3Vector(vtxX, vtxY, vtxZ); + + // Convert covariance matrix (tracking -> detector frame) + // Reorder: (0,1,2) tracking -> (1,2,0) detector + double[] covPacked = new double[6]; + // Symmetric matrix packed: (0,0), (1,0), (1,1), (2,0), (2,1), (2,2) + // In detector frame: x=trk_y(1), y=trk_z(2), z=trk_x(0) + covPacked[0] = result.vertexCov.getEntry(1, 1); // xx = trk(1,1) + covPacked[1] = result.vertexCov.getEntry(2, 1); // yx = trk(2,1) + covPacked[2] = result.vertexCov.getEntry(2, 2); // yy = trk(2,2) + covPacked[3] = result.vertexCov.getEntry(0, 1); // zx = trk(0,1) + covPacked[4] = result.vertexCov.getEntry(0, 2); // zy = trk(0,2) + covPacked[5] = result.vertexCov.getEntry(0, 0); // zz = trk(0,0) + hep.physics.matrix.SymmetricMatrix covVtx = new hep.physics.matrix.SymmetricMatrix(3, covPacked, true); + + // Vertex position error + hep.physics.vec.Hep3Vector vtxPosErr = new hep.physics.vec.BasicHep3Vector( + FastMath.sqrt(result.vertexCov.getEntry(1, 1)), + FastMath.sqrt(result.vertexCov.getEntry(2, 2)), + FastMath.sqrt(result.vertexCov.getEntry(0, 0)) + ); + + // Fitted momenta (convert tracking -> detector frame) + java.util.Map pFitMap = new java.util.HashMap<>(); + double me = 0.000511; + double totalE = 0.0; + double totalPx = 0.0, totalPy = 0.0, totalPz = 0.0; + + for (int i = 0; i < result.trackMomenta.size(); i++) { + RealVector p = result.trackMomenta.get(i).p; + // tracking (px,py,pz) -> detector (py, pz, px) + double detPx = p.getEntry(1); + double detPy = p.getEntry(2); + double detPz = p.getEntry(0); + pFitMap.put(i, new hep.physics.vec.BasicHep3Vector(detPx, detPy, detPz)); + double pMag = p.getNorm(); + totalE += FastMath.sqrt(pMag * pMag + me * me); + totalPx += detPx; + totalPy += detPy; + totalPz += detPz; + } + + double pSumSq = totalPx * totalPx + totalPy * totalPy + totalPz * totalPz; + double massSq = totalE * totalE - pSumSq; + double invMass = massSq > 0 ? FastMath.sqrt(massSq) : -99.0; + + BilliorVertex bv = new BilliorVertex(vtxPos, covVtx, result.chi2, invMass, pFitMap, label); + bv.setPositionError(vtxPosErr); + bv.setProbability(result.ndf); + bv.setParameter("ndf", (double) result.ndf); + + // Total (summed) fitted momentum + diagonal error, converted tracking -> detector frame + // with the same {1,2,0} reindex used for the per-track momenta above. Reuses the + // existing (Kalman-unused until now) BilliorVertex V0-momentum slot rather than adding + // new custom-parameter keys -- this is exactly the "total momentum + error" slot it was + // designed for, and it is already wired into getParameters() (V0Px/y/z, V0PxErr/etc, + // V0PErr). Only the diagonal error is stored, matching the diagonal-only convention + // already used for fitMom{i}_pxErr below. + if (result.totalMomentum != null && result.totalMomentumCov != null) { + int[] map = {1, 2, 0}; // detector index -> tracking index + double detTotalPx = result.totalMomentum.getEntry(map[0]); + double detTotalPy = result.totalMomentum.getEntry(map[1]); + double detTotalPz = result.totalMomentum.getEntry(map[2]); + hep.physics.vec.Hep3Vector detTotalP = + new hep.physics.vec.BasicHep3Vector(detTotalPx, detTotalPy, detTotalPz); + hep.physics.vec.Hep3Vector detTotalPErr = new hep.physics.vec.BasicHep3Vector( + FastMath.sqrt(FastMath.abs(result.totalMomentumCov.getEntry(map[0], map[0]))), + FastMath.sqrt(FastMath.abs(result.totalMomentumCov.getEntry(map[1], map[1]))), + FastMath.sqrt(FastMath.abs(result.totalMomentumCov.getEntry(map[2], map[2])))); + bv.setV0Momentum(detTotalP, detTotalPErr); + } + + // Store momentum covariances in detector frame (for the _covTrkMomList accessor path) + if (storeCovTrkMomList) { + java.util.List covTrkMomList = new java.util.ArrayList<>(); + for (int i = 0; i < result.trackMomenta.size(); i++) { + RealMatrix pCov = result.trackMomenta.get(i).pCov; + // Reorder tracking -> detector frame: det (x,y,z) = trk (y,z,x) + double[][] detCov = new double[3][3]; + int[] map = {1, 2, 0}; // detector index -> tracking index + for (int a = 0; a < 3; a++) + for (int b = 0; b < 3; b++) + detCov[a][b] = pCov.getEntry(map[a], map[b]); + double[] packed = new double[6]; + packed[0] = detCov[0][0]; + packed[1] = detCov[1][0]; + packed[2] = detCov[1][1]; + packed[3] = detCov[2][0]; + packed[4] = detCov[2][1]; + packed[5] = detCov[2][2]; + covTrkMomList.add(new hep.physics.matrix.SymmetricMatrix(3, packed, true)); + } + bv.setTrackMomentumCovariances(covTrkMomList); + } + + // Always store fitted momentum errors and fitted track parameters + errors as named + // custom parameters. This uses the same proven getParameters()/setParameter() path + // as vXErr, invMass, and the predicted-track quantities, so they are accessible to + // any downstream analyser without relying on the _covTrkMomList / _fitTrkParsList fields. + for (int i = 0; i < result.trackMomenta.size(); i++) { + RealMatrix pCov = result.trackMomenta.get(i).pCov; + // Tracking -> detector: det x = trk y (index 1), det y = trk z (index 2), det z = trk x (index 0) + String mpfx = "fitMom" + i + "_"; + bv.setParameter(mpfx + "pxErr", FastMath.sqrt(FastMath.abs(pCov.getEntry(1, 1)))); + bv.setParameter(mpfx + "pyErr", FastMath.sqrt(FastMath.abs(pCov.getEntry(2, 2)))); + bv.setParameter(mpfx + "pzErr", FastMath.sqrt(FastMath.abs(pCov.getEntry(0, 0)))); + } + // fitSoftConstrained() always populates result.fittedTracks. + // The fallback to input tracks guards against any future code path that returns null. + List tracksForOutput = (result.fittedTracks != null) ? result.fittedTracks : tracks; + for (int i = 0; i < tracksForOutput.size(); i++) { + TrackParams ft = tracksForOutput.get(i); + String tpfx = "fitTrk" + i + "_"; + bv.setParameter(tpfx + "d0", ft.d0); + bv.setParameter(tpfx + "phi0", ft.phi0); + bv.setParameter(tpfx + "omega", ft.omega); + bv.setParameter(tpfx + "z0", ft.z0); + bv.setParameter(tpfx + "tanL", ft.tanLambda); + bv.setParameter(tpfx + "d0Err", FastMath.sqrt(FastMath.abs(ft.cov.getEntry(0, 0)))); + bv.setParameter(tpfx + "phi0Err", FastMath.sqrt(FastMath.abs(ft.cov.getEntry(1, 1)))); + bv.setParameter(tpfx + "omegaErr", FastMath.sqrt(FastMath.abs(ft.cov.getEntry(2, 2)))); + bv.setParameter(tpfx + "z0Err", FastMath.sqrt(FastMath.abs(ft.cov.getEntry(3, 3)))); + bv.setParameter(tpfx + "tanLErr", FastMath.sqrt(FastMath.abs(ft.cov.getEntry(4, 4)))); + } + + if (debugFlag) { + System.out.println("=== TrackConstraintVertexFitter::fitVertex ==="); + System.out.println(" B field: " + bField); + System.out.println(" Beamspot constraint: " + beamspotConstraint); + System.out.println(" Beam momentum constraint: " + beamMomentumConstraint); + System.out.println(" Hard momentum constraint: " + hardMomentumConstraint); + System.out.println(" Label: " + label); + System.out.println(" Number of tracks: " + tracks.size()); + for (int i = 0; i < tracks.size(); i++) { + TrackParams t = tracks.get(i); + System.out.printf(" Input Track %d: d0=%.4f phi0=%.4f omega=%.6f z0=%.4f tanLambda=%.4f%n", + i, t.d0, t.phi0, t.omega, t.z0, t.tanLambda); + } + if (fourMomentumConstraintVec != null) { + System.out.printf(" Beam 4-momentum constraint: [%.4f, %.4f, %.4f, %.4f]%n", + fourMomentumConstraintVec.getEntry(0), + fourMomentumConstraintVec.getEntry(1), + fourMomentumConstraintVec.getEntry(2), + fourMomentumConstraintVec.getEntry(3)); + } + if (vertexConstraintVec != null) { + System.out.printf(" Vertex constraint: [%.4f, %.4f, %.4f]%n", + vertexConstraintVec.getEntry(0), + vertexConstraintVec.getEntry(1), + vertexConstraintVec.getEntry(2)); + } + System.out.println(" --- Fit Results ---"); + System.out.printf(" Vertex (tracking frame): [%.4f, %.4f, %.4f]%n", + result.vertex.getEntry(0), result.vertex.getEntry(1), result.vertex.getEntry(2)); + System.out.println(" Vertex (det frame): " + vtxPos); + System.out.println(" Vertex error (det frame): " + vtxPosErr); + System.out.printf(" Vertex covariance (det frame): [%.6f, %.6f, %.6f; %.6f, %.6f; %.6f]%n", + covPacked[0], covPacked[1], covPacked[2], + covPacked[3], covPacked[4], covPacked[5]); + System.out.printf(" Chi2: %.4f NDF: %d Chi2/NDF: %.4f%n", + result.chi2, result.ndf, + result.ndf > 0 ? result.chi2 / result.ndf : -1.0); + System.out.println(" InvMass: " + invMass); + for (int i = 0; i < result.trackMomenta.size(); i++) { + RealVector pTrk = result.trackMomenta.get(i).p; + hep.physics.vec.Hep3Vector pDet = pFitMap.get(i); + double pMagI = pTrk.getNorm(); + System.out.printf(" Track %d momentum (trk frame): [%.4f, %.4f, %.4f] |p|=%.4f%n", + i, pTrk.getEntry(0), pTrk.getEntry(1), pTrk.getEntry(2), pMagI); + System.out.printf(" Track %d momentum (det frame): [%.4f, %.4f, %.4f] |p|=%.4f%n", + i, pDet.x(), pDet.y(), pDet.z(), pDet.magnitude()); + } + double totalPMag = FastMath.sqrt(totalPx * totalPx + totalPy * totalPy + totalPz * totalPz); + System.out.printf(" Total momentum (det frame): [%.4f, %.4f, %.4f] |p|=%.4f E=%.4f%n", + totalPx, totalPy, totalPz, totalPMag, totalE); + System.out.println("=== End TrackConstraintVertexFitter::fitVertex ==="); + } + + return bv; + } + + // Getters + public RealVector getVertex() { return vertex; } + public RealMatrix getVertexCov() { return vertexCov; } + public double getChi2() { return chi2; } + public int getNdf() { return ndf; } + public List getTrackMomenta() { return trackMomenta; } + +} diff --git a/recon/src/main/java/org/hps/recon/vertexing/Vertexer.java b/recon/src/main/java/org/hps/recon/vertexing/Vertexer.java new file mode 100644 index 0000000000..8bd6fff716 --- /dev/null +++ b/recon/src/main/java/org/hps/recon/vertexing/Vertexer.java @@ -0,0 +1,54 @@ +package org.hps.recon.vertexing; + +import org.apache.commons.math3.linear.MatrixUtils; +import org.apache.commons.math3.linear.RealMatrix; + +import hep.physics.matrix.SymmetricMatrix; + +import org.lcsim.event.Track; +import org.lcsim.event.TrackState; + +import org.hps.recon.tracking.TrackStateUtils; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.TrackParams; + +/** + * Common base class for {@link NTrackVertexer} and {@link CascadeVertexer}: shared magnetic + * field, electron-mass constant, and perigee-parameter extraction, both of which wrap + * {@link TrackConstraintVertexFitter} but return different result shapes (a plain N-track + * {@link BilliorVertex} vs. a two-vertex cascade {@code ReconstructedParticle}), so no common + * {@code fit(...)} method is imposed here. + */ +public abstract class Vertexer { + + protected static final double ELECTRON_MASS = 0.000511; + + protected final double bField; + + protected Vertexer(double bField) { + this.bField = bField; + } + + /** + * Extract perigee TrackParams (d0, phi0, omega, z0, tanLambda) directly from an + * AtPerigee TrackState, with no reparametrization. + */ + protected static TrackParams trackParamsFromTrack(TrackState ts) { + double[] par = ts.getParameters(); + SymmetricMatrix sm = new SymmetricMatrix(5, ts.getCovMatrix(), true); + RealMatrix cov = MatrixUtils.createRealMatrix(5, 5); + for (int i = 0; i < 5; i++) { + for (int j = 0; j < 5; j++) { + cov.setEntry(i, j, sm.e(i, j)); + } + } + return new TrackParams(par[0], par[1], par[2], par[3], par[4], cov); + } + + /** + * Extract perigee TrackParams (d0, phi0, omega, z0, tanLambda) from a Track's + * AtPerigee TrackState. + */ + protected static TrackParams trackParamsFromTrack(Track track) { + return trackParamsFromTrack(TrackStateUtils.getTrackStatesAtLocation(track, TrackState.AtPerigee).get(0)); + } +} diff --git a/recon/src/test/java/org/hps/recon/vertexing/BilliorVertexerTest.java b/recon/src/test/java/org/hps/recon/vertexing/BilliorVertexerTest.java new file mode 100644 index 0000000000..3ef63e993b --- /dev/null +++ b/recon/src/test/java/org/hps/recon/vertexing/BilliorVertexerTest.java @@ -0,0 +1,106 @@ +package org.hps.recon.vertexing; + +import java.util.ArrayList; +import java.util.List; + +import hep.physics.matrix.BasicMatrix; +import hep.physics.matrix.Matrix; +import hep.physics.matrix.SymmetricMatrix; + +import junit.framework.TestCase; + +import org.lcsim.fit.helicaltrack.HelicalTrackFit; + +/** + * Sanity checks on the vertex-momentum cross-covariance exposed by BilliorVertexer/BilliorVertex, + * used to build a full (non-block-diagonal) 6x6 joint covariance for a V0 candidate. + */ +public class BilliorVertexerTest extends TestCase { + + private static final double B_FIELD = 0.5; + + public void testVertexMomentumJointCovarianceIsSymmetricPSD() { + BilliorTrack track1 = makeTrack(0.05, 0.15, 0.0012, 0.02, 0.06); + BilliorTrack track2 = makeTrack(-0.04, -0.12, -0.0011, -0.01, -0.05); + + List tracks = new ArrayList(); + tracks.add(track1); + tracks.add(track2); + + BilliorVertexer vertexer = new BilliorVertexer(B_FIELD); + vertexer.doBeamSpotConstraint(false); + BilliorVertex vertex = vertexer.fitVertex(tracks); + + Matrix covVV = vertex.getCovMatrix(); + List covTrkMom = vertex.getFittedMomentumCovariance(); + Matrix covPP = MatrixOpAdd(MatrixOpAdd(covTrkMom.get(0), covTrkMom.get(1)), + MatrixOpAdd(covTrkMom.get(2), transposed(covTrkMom.get(2)))); + Matrix covVP = vertex.getVertexV0MomentumCovariance(); + + assertNotNull(covVV); + assertNotNull(covVP); + assertEquals(3, covVP.getNRows()); + assertEquals(3, covVP.getNColumns()); + + double[][] joint = new double[6][6]; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + joint[i][j] = covVV.e(i, j); + joint[3 + i][3 + j] = covPP.e(i, j); + joint[i][3 + j] = covVP.e(i, j); + joint[3 + j][i] = covVP.e(i, j); + } + } + + // symmetric by construction; check numerically anyway + for (int i = 0; i < 6; i++) + for (int j = 0; j < 6; j++) + assertEquals("joint covariance not symmetric at (" + i + "," + j + ")", + joint[i][j], joint[j][i], 1e-12 * (1 + Math.abs(joint[i][j]))); + + assertTrue("assembled 6x6 vertex-momentum joint covariance is not PSD", isPositiveSemiDefinite(joint, 1e-9)); + } + + private static BilliorTrack makeTrack(double dca, double phi0, double curvature, double z0, double slope) { + double[] par = {dca, phi0, curvature, z0, slope}; + SymmetricMatrix cov = new SymmetricMatrix(5); + cov.setElement(HelicalTrackFit.dcaIndex, HelicalTrackFit.dcaIndex, 1e-6); + cov.setElement(HelicalTrackFit.phi0Index, HelicalTrackFit.phi0Index, 1e-7); + cov.setElement(HelicalTrackFit.curvatureIndex, HelicalTrackFit.curvatureIndex, 1e-10); + cov.setElement(HelicalTrackFit.z0Index, HelicalTrackFit.z0Index, 1e-6); + cov.setElement(HelicalTrackFit.slopeIndex, HelicalTrackFit.slopeIndex, 1e-7); + HelicalTrackFit htf = new HelicalTrackFit(par, cov, new double[2], new int[2], null, null); + return new BilliorTrack(htf); + } + + private static Matrix MatrixOpAdd(Matrix a, Matrix b) { + return hep.physics.matrix.MatrixOp.add(a, b); + } + + private static Matrix transposed(Matrix a) { + return hep.physics.matrix.MatrixOp.transposed(a); + } + + /** + * Attempts an LDL^T (Cholesky-like) decomposition of a symmetric matrix; the matrix is PSD + * iff no pivot is negative beyond tolerance. + */ + private static boolean isPositiveSemiDefinite(double[][] m, double tol) { + int n = m.length; + double[][] a = new double[n][n]; + for (int i = 0; i < n; i++) + a[i] = m[i].clone(); + for (int k = 0; k < n; k++) { + if (a[k][k] < -tol) + return false; + if (a[k][k] < tol) + continue; + for (int i = k + 1; i < n; i++) { + double factor = a[i][k] / a[k][k]; + for (int j = k; j < n; j++) + a[i][j] -= factor * a[k][j]; + } + } + return true; + } +} diff --git a/recon/src/test/java/org/hps/recon/vertexing/CascadeVertexerTest.java b/recon/src/test/java/org/hps/recon/vertexing/CascadeVertexerTest.java new file mode 100644 index 0000000000..a3389198b3 --- /dev/null +++ b/recon/src/test/java/org/hps/recon/vertexing/CascadeVertexerTest.java @@ -0,0 +1,451 @@ +package org.hps.recon.vertexing; + +import java.util.ArrayList; +import java.util.HashMap; +import java.util.List; +import java.util.Map; + +import hep.physics.matrix.Matrix; +import hep.physics.matrix.SymmetricMatrix; +import hep.physics.vec.BasicHep3Vector; +import hep.physics.vec.Hep3Vector; + +import junit.framework.TestCase; + +import org.apache.commons.math.util.FastMath; + +import org.lcsim.event.ReconstructedParticle; +import org.lcsim.event.Track; +import org.lcsim.event.TrackState; +import org.lcsim.event.base.BaseReconstructedParticle; +import org.lcsim.event.base.BaseTrack; +import org.lcsim.event.base.BaseTrackState; + +/** + * Exact-geometry sanity check for {@link CascadeVertexer}: build an e-/e+ pair whose + * tracks (and already-fitted V0 BilliorVertex) exactly meet at a known V1 point, and a + * recoil-electron track whose helix + * passes exactly through a second known point V2 lying on the V0's flight line, with all + * three tracks' AtPerigee reference point set away from the origin -- verifies the fitted + * V1/V2 positions correctly land on those points in the absolute tracking/detector frame. + */ +public class CascadeVertexerTest extends TestCase { + + private static final double B_FIELD = 0.5; // Tesla + private static final double C = 2.99792458e-4; + + public void testFitExactGeometryNonOriginReferencePoint() { + double[] refPoint = {-1.1, 0.0, 0.0}; + + // V2 (production vertex, tracking frame), near the target. + double x2V = 0.5, y2V = 0.05, z2V = -0.05; + // V1 (e-/e+ decay vertex, tracking frame), downstream of V2. + double x1V = 5.0, y1V = 0.3, z1V = -0.2; + + // e-/e+ momenta (tracking frame) chosen so p1+p2 is parallel to (V1 - V2), i.e. the + // V0 flies from V2 to V1 along its own total momentum direction. + double[] p1 = {0.25, 0.02, -0.01}; + double[] p2 = {0.20, 0.005, -0.005}; + + Track eleTrack = makeTrackThroughPoint(x1V, y1V, z1V, p1[0], p1[1], p1[2], -1, refPoint); + Track posTrack = makeTrackThroughPoint(x1V, y1V, z1V, p2[0], p2[1], p2[2], 1, refPoint); + + Hep3Vector v1PosDet = new BasicHep3Vector(y1V, z1V, x1V); + Hep3Vector p1Det = new BasicHep3Vector(p1[1], p1[2], p1[0]); + Hep3Vector p2Det = new BasicHep3Vector(p2[1], p2[2], p2[0]); + + ReconstructedParticle v0Particle = makeV0Particle(v1PosDet, p1Det, p2Det, eleTrack, posTrack); + + double[] pRecoil = {0.15, -0.02, 0.01}; + Track recoilTrack = makeTrackThroughPoint(x2V, y2V, z2V, pRecoil[0], pRecoil[1], pRecoil[2], -1, refPoint); + ReconstructedParticle recoilElectron = makeElectronParticle(recoilTrack); + + CascadeVertexer vertexer = new CascadeVertexer(B_FIELD); + ReconstructedParticle cascade = vertexer.fit(v0Particle, recoilElectron); + + assertNotNull("three-track fit should not be null", cascade); + + BilliorVertex v2Vtx = (BilliorVertex) cascade.getStartVertex(); + Hep3Vector v2PosDet = v2Vtx.getPosition(); + ReconstructedParticle v0Out = cascade.getParticles().get(0); + BilliorVertex v1Vtx = (BilliorVertex) v0Out.getStartVertex(); + Hep3Vector v1PosDetFit = v1Vtx.getPosition(); + + System.out.printf("Fitted V1 (det frame): [%.6f, %.6f, %.6f] (truth trk frame: [%.6f, %.6f, %.6f])%n", + v1PosDetFit.x(), v1PosDetFit.y(), v1PosDetFit.z(), x1V, y1V, z1V); + System.out.printf("Fitted V2 (det frame): [%.6f, %.6f, %.6f] (truth trk frame: [%.6f, %.6f, %.6f])%n", + v2PosDet.x(), v2PosDet.y(), v2PosDet.z(), x2V, y2V, z2V); + System.out.printf("Chi2: %.6f%n", v2Vtx.getChi2()); + + // Detector frame: det(x,y,z) = trk(y,z,x) + assertEquals(y1V, v1PosDetFit.x(), 1e-3); + assertEquals(z1V, v1PosDetFit.y(), 1e-3); + assertEquals(x1V, v1PosDetFit.z(), 1e-3); + + assertEquals(y2V, v2PosDet.x(), 1e-3); + assertEquals(z2V, v2PosDet.y(), 1e-3); + assertEquals(x2V, v2PosDet.z(), 1e-3); + + assertTrue("chi2 should be small for exactly-consistent geometry, got " + v2Vtx.getChi2(), + v2Vtx.getChi2() < 1e-2); + } + + /** + * Verify {@link CascadeVertexer#setUseBeamspotConstraintForV2} actually pulls the fitted V2 + * toward a supplied beamspot position/size (via {@link + * CascadeVertexer#setBeamspotConstraintForV2Params}), and that the default (toggle off) + * behavior is unaffected -- proving the new opt-in toggle causes zero regression to existing + * behavior when left at its default. + */ + public void testFitWithBeamspotConstraintForV2PullsTowardBeamPosition() { + double[] refPoint = {-1.1, 0.0, 0.0}; + + // V2 (production vertex, tracking frame), near the target. + double x2V = 0.5, y2V = 0.05, z2V = -0.05; + // V1 (e-/e+ decay vertex, tracking frame), downstream of V2. + double x1V = 5.0, y1V = 0.3, z1V = -0.2; + + double[] p1 = {0.25, 0.02, -0.01}; + double[] p2 = {0.20, 0.005, -0.005}; + double[] pRecoil = {0.15, -0.02, 0.01}; + + Track eleTrack = makeTrackThroughPoint(x1V, y1V, z1V, p1[0], p1[1], p1[2], -1, refPoint); + Track posTrack = makeTrackThroughPoint(x1V, y1V, z1V, p2[0], p2[1], p2[2], 1, refPoint); + Hep3Vector v1PosDet = new BasicHep3Vector(y1V, z1V, x1V); + Hep3Vector p1Det = new BasicHep3Vector(p1[1], p1[2], p1[0]); + Hep3Vector p2Det = new BasicHep3Vector(p2[1], p2[2], p2[0]); + ReconstructedParticle v0Particle = makeV0Particle(v1PosDet, p1Det, p2Det, eleTrack, posTrack); + Track recoilTrack = makeTrackThroughPoint(x2V, y2V, z2V, pRecoil[0], pRecoil[1], pRecoil[2], -1, refPoint); + ReconstructedParticle recoilElectron = makeElectronParticle(recoilTrack); + + CascadeVertexer defaultVertexer = new CascadeVertexer(B_FIELD); + ReconstructedParticle cascadeDefault = defaultVertexer.fit(v0Particle, recoilElectron); + assertNotNull("default (unconstrained-V2) fit should not be null", cascadeDefault); + Hep3Vector v2PosDetDefault = ((BilliorVertex) cascadeDefault.getStartVertex()).getPosition(); + + // Well-separated from both refPoint and V2's own truth position, so the pull is + // unambiguous. beamSizeOverride must be tight not just relative to the tracks' + // individual d0/z0 sigmas, but relative to the recoil track's own (potentially + // anisotropic) vertex information -- 1e-4 (information ~1e8) safely dominates that, + // unlike an initially-tried 0.001 (information ~1e6, only comparable to the track's best- + // constrained direction, which pulled V2 to neither truth nor the beam position; see + // NTrackVertexerTest's analogous comment). An even tighter 1e-6 (information ~1e12) + // was also tried but made fitCascadeVertexJointFreeTrackCore's internal matrix inversion + // numerically singular -- this joint two-vertex fit is more sensitive to ill-conditioning + // from an extreme prior/track information mismatch than the simpler N-track fit. + double[] beamPositionOverride = {0.0, 0.0, 0.0}; + double[] beamSizeOverride = {1e-4, 1e-4, 1e-4}; + CascadeVertexer beamspotVertexer = new CascadeVertexer(B_FIELD); + beamspotVertexer.setUseBeamspotConstraintForV2(true); + beamspotVertexer.setBeamspotConstraintForV2Params(beamPositionOverride, beamSizeOverride); + ReconstructedParticle cascadeBS = beamspotVertexer.fit(v0Particle, recoilElectron); + assertNotNull("beamspot-constrained-V2 fit should not be null", cascadeBS); + Hep3Vector v2PosDetBS = ((BilliorVertex) cascadeBS.getStartVertex()).getPosition(); + + System.out.printf("Default V2 (det frame): [%.6f, %.6f, %.6f] (truth trk frame: [%.6f, %.6f, %.6f])%n", + v2PosDetDefault.x(), v2PosDetDefault.y(), v2PosDetDefault.z(), x2V, y2V, z2V); + System.out.printf("Beamspot-constrained V2 (det frame): [%.6f, %.6f, %.6f] " + + "(beam position trk frame: [%.6f, %.6f, %.6f], beam size trk frame: [%.6f, %.6f, %.6f])%n", + v2PosDetBS.x(), v2PosDetBS.y(), v2PosDetBS.z(), + beamPositionOverride[0], beamPositionOverride[1], beamPositionOverride[2], + beamSizeOverride[0], beamSizeOverride[1], beamSizeOverride[2]); + + // The default (toggle off) fit should reproduce the existing exact-geometry result, + // unaffected by the new toggle's existence. + assertEquals(y2V, v2PosDetDefault.x(), 1e-3); + assertEquals(z2V, v2PosDetDefault.y(), 1e-3); + assertEquals(x2V, v2PosDetDefault.z(), 1e-3); + + // The beamspot-constrained fit should land close to the (tight) beamspot prior instead + // of near truth (det frame: det(x,y,z) = trk(y,z,x), beam position trk (0,0,0) -> det + // (0,0,0)). + assertEquals(0.0, v2PosDetBS.x(), 0.01); + assertEquals(0.0, v2PosDetBS.y(), 0.01); + assertEquals(0.0, v2PosDetBS.z(), 0.01); + + double pullDistance = distance(v2PosDetBS, v2PosDetDefault); + assertTrue("beamspot constraint should measurably pull V2 away from the default result, " + + "got pull distance " + pullDistance, pullDistance > 0.3); + } + + private static double distance(Hep3Vector a, Hep3Vector b) { + double dx = a.x() - b.x(), dy = a.y() - b.y(), dz = a.z() - b.z(); + return FastMath.sqrt(dx * dx + dy * dy + dz * dz); + } + + /** + * Verify {@link CascadeVertexer#fit(ReconstructedParticle, ReconstructedParticle, boolean, + * boolean, double, double, double)} with {@code beamConstrained=true} AND {@link + * CascadeVertexer#setUseBeamspotConstraintForV2} both set simultaneously pulls V2 toward + * the beamspot position AND the total (V0 + recoil) 3-momentum toward the beam-momentum + * target at the same time, and that the pre-existing beamspot-only and beam-momentum-only + * paths (each exercised here on fresh {@code CascadeVertexer} instances, since the toggle + * is instance state) are completely unaffected by the new combined call. + */ + public void testFitBothConstrainedPullsTowardBeamspotAndBeamMomentum() { + double[] refPoint = {-1.1, 0.0, 0.0}; + + // V2 (production vertex, tracking frame), near the target. + double x2V = 0.5, y2V = 0.05, z2V = -0.05; + // V1 (e-/e+ decay vertex, tracking frame), downstream of V2. + double x1V = 5.0, y1V = 0.3, z1V = -0.2; + + // Same track geometry as testFitExactGeometryNonOriginReferencePoint -- total + // (e-/e+/recoil) momentum [0.60, 0.005, -0.005] (trk frame) is already dominantly + // along tracking-X, matching the beam-momentum convention below, so no direction + // mismatch (see NTrackVertexerTest's analogous comment) is introduced by adding + // the beam-momentum constraint on top of these tracks. + double[] p1 = {0.25, 0.02, -0.01}; + double[] p2 = {0.20, 0.005, -0.005}; + double[] pRecoil = {0.15, -0.02, 0.01}; + + Track eleTrack = makeTrackThroughPoint(x1V, y1V, z1V, p1[0], p1[1], p1[2], -1, refPoint); + Track posTrack = makeTrackThroughPoint(x1V, y1V, z1V, p2[0], p2[1], p2[2], 1, refPoint); + Hep3Vector v1PosDet = new BasicHep3Vector(y1V, z1V, x1V); + Hep3Vector p1Det = new BasicHep3Vector(p1[1], p1[2], p1[0]); + Hep3Vector p2Det = new BasicHep3Vector(p2[1], p2[2], p2[0]); + ReconstructedParticle v0Particle = makeV0Particle(v1PosDet, p1Det, p2Det, eleTrack, posTrack); + Track recoilTrack = makeTrackThroughPoint(x2V, y2V, z2V, pRecoil[0], pRecoil[1], pRecoil[2], -1, refPoint); + ReconstructedParticle recoilElectron = makeElectronParticle(recoilTrack); + + CascadeVertexer defaultVertexer = new CascadeVertexer(B_FIELD); + ReconstructedParticle cascadeDefault = defaultVertexer.fit(v0Particle, recoilElectron); + assertNotNull("default (unconstrained) fit should not be null", cascadeDefault); + Hep3Vector v2PosDetDefault = ((BilliorVertex) cascadeDefault.getStartVertex()).getPosition(); + Hep3Vector totalPDefault = cascadeDefault.getMomentum(); + + // Beam momentum target chosen far from the tracks' own (unconstrained) momentum sum + // [0.60, 0.005, -0.005] (trk frame), so a real pull is unambiguous. + double beamEnergy = 1.0; + double beamRotAngle = 0.02; + double beamPxTrk = beamEnergy * FastMath.cos(beamRotAngle); + double beamPyTrk = -beamEnergy * FastMath.sin(beamRotAngle); + // getMomentum() (used throughout below) returns detector frame, det(x,y,z) = trk(y,z,x) + // -- same convention CascadeVertexer.makeReconstructedParticle itself uses -- so the + // comparison target must be built in that frame too, not tracking frame (see + // NTrackVertexerTest's analogous fix for the same pitfall). + Hep3Vector beamPDet = new BasicHep3Vector(beamPyTrk, 0.0, beamPxTrk); + + // Same beamspot prior as testFitWithBeamspotConstraintForV2PullsTowardBeamPosition, far + // from the tracks' own (unconstrained) V2 position. + double[] beamPositionOverride = {0.0, 0.0, 0.0}; + double[] beamSizeOverride = {1e-4, 1e-4, 1e-4}; + + CascadeVertexer bothVertexer = new CascadeVertexer(B_FIELD); + bothVertexer.setUseBeamspotConstraintForV2(true); + bothVertexer.setBeamspotConstraintForV2Params(beamPositionOverride, beamSizeOverride); + ReconstructedParticle cascadeBoth = bothVertexer.fit( + v0Particle, recoilElectron, true, beamEnergy, beamRotAngle, 0.0); + assertNotNull("both-constrained fit should not be null", cascadeBoth); + Hep3Vector v2PosDetBoth = ((BilliorVertex) cascadeBoth.getStartVertex()).getPosition(); + Hep3Vector totalPBoth = cascadeBoth.getMomentum(); + + System.out.printf("Default V2 (det frame): [%.6f, %.6f, %.6f], total P: [%.6f, %.6f, %.6f]%n", + v2PosDetDefault.x(), v2PosDetDefault.y(), v2PosDetDefault.z(), + totalPDefault.x(), totalPDefault.y(), totalPDefault.z()); + System.out.printf("Both-constrained V2 (det frame): [%.6f, %.6f, %.6f], total P: [%.6f, %.6f, %.6f] " + + "(beam P det frame: [%.6f, %.6f, %.6f])%n", + v2PosDetBoth.x(), v2PosDetBoth.y(), v2PosDetBoth.z(), + totalPBoth.x(), totalPBoth.y(), totalPBoth.z(), + beamPDet.x(), beamPDet.y(), beamPDet.z()); + + // Position: the beamspot prior is far tighter than the tracks' own position + // sensitivity, so the constrained fit's V2 should land very close to the beam position + // (det frame: det(x,y,z) = trk(y,z,x), beam position trk (0,0,0) -> det (0,0,0)). + assertEquals(0.0, v2PosDetBoth.x(), 0.01); + assertEquals(0.0, v2PosDetBoth.y(), 0.01); + assertEquals(0.0, v2PosDetBoth.z(), 0.01); + + // Momentum: the both-constrained total momentum should land measurably closer to the + // beam momentum target than the default (unconstrained) fit's total momentum did. + double distDefaultFromBeam = distance(totalPDefault, beamPDet); + double distBothFromBeam = distance(totalPBoth, beamPDet); + assertTrue("both-constrained total momentum should be pulled measurably closer to the " + + "beam momentum target than the default fit (default dist=" + distDefaultFromBeam + + ", both dist=" + distBothFromBeam + ")", distBothFromBeam < 0.5 * distDefaultFromBeam); + + // Calling the pre-existing single-constraint paths on the same tracks (fresh instances, + // since the beamspot toggle is instance state) must be completely unaffected by having + // exercised the new combined path above. + CascadeVertexer beamspotOnlyVertexer = new CascadeVertexer(B_FIELD); + beamspotOnlyVertexer.setUseBeamspotConstraintForV2(true); + beamspotOnlyVertexer.setBeamspotConstraintForV2Params(beamPositionOverride, beamSizeOverride); + ReconstructedParticle cascadeBS = beamspotOnlyVertexer.fit(v0Particle, recoilElectron); + assertNotNull("beamspot-only fit should not be null", cascadeBS); + Hep3Vector v2PosDetBS = ((BilliorVertex) cascadeBS.getStartVertex()).getPosition(); + assertEquals(0.0, v2PosDetBS.x(), 0.01); + assertEquals(0.0, v2PosDetBS.y(), 0.01); + assertEquals(0.0, v2PosDetBS.z(), 0.01); + + CascadeVertexer beamMomOnlyVertexer = new CascadeVertexer(B_FIELD); + ReconstructedParticle cascadeBM = beamMomOnlyVertexer.fit( + v0Particle, recoilElectron, true, beamEnergy, beamRotAngle, 0.0); + assertNotNull("beam-momentum-only fit should not be null", cascadeBM); + double distBMFromBeam = distance(cascadeBM.getMomentum(), beamPDet); + assertTrue("beam-momentum-only total momentum should also be pulled measurably closer " + + "to the beam momentum target than the default fit (default dist=" + + distDefaultFromBeam + ", beam-mom-only dist=" + distBMFromBeam + ")", + distBMFromBeam < 0.5 * distDefaultFromBeam); + } + + /** + * Shape-only check for {@link CascadeVertexer#placeholderCascade}: given a real, + * successful cascade candidate, the placeholder should mirror its nested structure + * (cascade -> [v0Particle, recoilElectron], v0Particle -> [eleDaughter, posDaughter]) + * with the same daughter objects, but sentinel (-9999) chi2/mass/ndf on both vertices. + */ + public void testPlaceholderCascade() { + double[] refPoint = {-1.1, 0.0, 0.0}; + double x1V = 5.0, y1V = 0.3, z1V = -0.2; + double x2V = 0.5, y2V = 0.05, z2V = -0.05; + double[] p1 = {0.25, 0.02, -0.01}; + double[] p2 = {0.20, 0.005, -0.005}; + + Track eleTrack = makeTrackThroughPoint(x1V, y1V, z1V, p1[0], p1[1], p1[2], -1, refPoint); + Track posTrack = makeTrackThroughPoint(x1V, y1V, z1V, p2[0], p2[1], p2[2], 1, refPoint); + Hep3Vector v1PosDet = new BasicHep3Vector(y1V, z1V, x1V); + Hep3Vector p1Det = new BasicHep3Vector(p1[1], p1[2], p1[0]); + Hep3Vector p2Det = new BasicHep3Vector(p2[1], p2[2], p2[0]); + ReconstructedParticle v0Particle = makeV0Particle(v1PosDet, p1Det, p2Det, eleTrack, posTrack); + + double[] pRecoil = {0.15, -0.02, 0.01}; + Track recoilTrack = makeTrackThroughPoint(x2V, y2V, z2V, pRecoil[0], pRecoil[1], pRecoil[2], -1, refPoint); + ReconstructedParticle recoilElectron = makeElectronParticle(recoilTrack); + + CascadeVertexer vertexer = new CascadeVertexer(B_FIELD); + ReconstructedParticle cascade = vertexer.fit(v0Particle, recoilElectron); + assertNotNull("cascade fit should not be null", cascade); + + ReconstructedParticle placeholder = CascadeVertexer.placeholderCascade(cascade); + assertNotNull("placeholder cascade should not be null", placeholder); + assertEquals(2, placeholder.getParticles().size()); + assertSame(recoilElectron.getTracks().get(0), placeholder.getParticles().get(1).getTracks().get(0)); + + ReconstructedParticle placeholderV0 = placeholder.getParticles().get(0); + assertEquals(2, placeholderV0.getParticles().size()); + assertSame(eleTrack, placeholderV0.getParticles().get(0).getTracks().get(0)); + assertSame(posTrack, placeholderV0.getParticles().get(1).getTracks().get(0)); + + BilliorVertex placeholderV2Vtx = (BilliorVertex) placeholder.getStartVertex(); + assertEquals(-9999.0, placeholderV2Vtx.getChi2(), 1e-9); + assertEquals(-9999.0, placeholderV2Vtx.getInvMass(), 1e-9); + assertEquals(-9999.0, placeholderV2Vtx.getCustomParameters().get("ndf"), 1e-9); + + BilliorVertex placeholderV1Vtx = (BilliorVertex) placeholderV0.getStartVertex(); + assertEquals(-9999.0, placeholderV1Vtx.getChi2(), 1e-9); + assertEquals(-9999.0, placeholderV1Vtx.getInvMass(), 1e-9); + assertEquals(-9999.0, placeholderV1Vtx.getCustomParameters().get("ndf"), 1e-9); + } + + private static ReconstructedParticle makeV0Particle(Hep3Vector vtxPosDet, Hep3Vector p1Det, Hep3Vector p2Det, + Track eleTrack, Track posTrack) { + SymmetricMatrix covVtx = new SymmetricMatrix(3); + covVtx.setElement(0, 0, 1e-6); + covVtx.setElement(1, 1, 1e-6); + covVtx.setElement(2, 2, 1e-6); + + Map pFitMap = new HashMap(); + pFitMap.put(0, p1Det); + pFitMap.put(1, p2Det); + + BilliorVertex v0Vertex = new BilliorVertex(vtxPosDet, covVtx, 0.0, 0.05, pFitMap, "TEST_V0"); + v0Vertex.setPositionError(new BasicHep3Vector(1e-3, 1e-3, 1e-3)); + + SymmetricMatrix covP1 = new SymmetricMatrix(3); + covP1.setElement(0, 0, 1e-6); + covP1.setElement(1, 1, 1e-6); + covP1.setElement(2, 2, 1e-6); + SymmetricMatrix covP2 = new SymmetricMatrix(3); + covP2.setElement(0, 0, 1e-6); + covP2.setElement(1, 1, 1e-6); + covP2.setElement(2, 2, 1e-6); + SymmetricMatrix covP12 = new SymmetricMatrix(3); + List covTrkMomList = new ArrayList(); + covTrkMomList.add(covP1); + covTrkMomList.add(covP2); + covTrkMomList.add(covP12); + v0Vertex.setTrackMomentumCovariances(covTrkMomList); + + SymmetricMatrix covVp1 = new SymmetricMatrix(3); + SymmetricMatrix covVp2 = new SymmetricMatrix(3); + List covVtxMomList = new ArrayList(); + covVtxMomList.add(covVp1); + covVtxMomList.add(covVp2); + v0Vertex.setVertexMomentumCovariance(covVtxMomList); + + BaseReconstructedParticle eleDaughter = new BaseReconstructedParticle(); + eleDaughter.addTrack(eleTrack); + eleDaughter.setCharge(-1); + + BaseReconstructedParticle posDaughter = new BaseReconstructedParticle(); + posDaughter.addTrack(posTrack); + posDaughter.setCharge(1); + + BaseReconstructedParticle v0Particle = new BaseReconstructedParticle(); + v0Particle.setStartVertex(v0Vertex); + v0Particle.setMass(0.05); + v0Particle.setCharge(0); + v0Particle.addParticle(eleDaughter); + v0Particle.addParticle(posDaughter); + return v0Particle; + } + + private static ReconstructedParticle makeElectronParticle(Track track) { + BaseReconstructedParticle particle = new BaseReconstructedParticle(); + particle.addTrack(track); + particle.setCharge(-1); + return particle; + } + + /** + * Build a Track with a single AtPerigee TrackState whose helix passes exactly through + * (xV,yV,zV) with the given momentum and reference point, mirroring + * {@code NTrackVertexerTest#makeTrackThroughPoint}. + */ + private static Track makeTrackThroughPoint(double xV, double yV, double zV, + double px, double py, double pz, int charge, double[] refPoint) { + double xVLocal = xV - refPoint[0]; + double yVLocal = yV - refPoint[1]; + double zVLocal = zV - refPoint[2]; + double pT = FastMath.sqrt(px * px + py * py); + double omega = charge * C * B_FIELD / pT; + double R = 1.0 / FastMath.abs(omega); + double sign = FastMath.signum(omega); + double phiV = FastMath.atan2(py, px); + double tanLambda = pz / pT; + + double xc = xVLocal + R * sign * FastMath.sin(phiV); + double yc = yVLocal - R * sign * FastMath.cos(phiV); + + double A = FastMath.sqrt(xc * xc + yc * yc); + double phi0 = FastMath.atan2(sign * xc, -sign * yc); + double d0 = sign * (R - A); + + double dphi = phiV - phi0; + while (dphi > FastMath.PI) dphi -= 2.0 * FastMath.PI; + while (dphi < -FastMath.PI) dphi += 2.0 * FastMath.PI; + double s = -sign * R * dphi; + double z0 = zVLocal - s * tanLambda; + + double[] params = new double[5]; + params[BaseTrack.D0] = d0; + params[BaseTrack.PHI] = phi0; + params[BaseTrack.OMEGA] = omega; + params[BaseTrack.Z0] = z0; + params[BaseTrack.TANLAMBDA] = tanLambda; + + SymmetricMatrix cov = new SymmetricMatrix(5); + cov.setElement(0, 0, 1e-4); + cov.setElement(1, 1, 1e-5); + cov.setElement(2, 2, 1e-8); + cov.setElement(3, 3, 1e-4); + cov.setElement(4, 4, 1e-5); + + BaseTrackState ts = new BaseTrackState(params, refPoint, + cov.asPackedArray(true), TrackState.AtPerigee, B_FIELD); + + BaseTrack track = new BaseTrack(); + track.getTrackStates().add(ts); + return track; + } +} diff --git a/recon/src/test/java/org/hps/recon/vertexing/GainMatrixVertexer.java b/recon/src/test/java/org/hps/recon/vertexing/GainMatrixVertexer.java new file mode 100644 index 0000000000..e3d111204b --- /dev/null +++ b/recon/src/test/java/org/hps/recon/vertexing/GainMatrixVertexer.java @@ -0,0 +1,353 @@ +package org.hps.recon.vertexing; + +import java.util.ArrayList; +import java.util.List; + +import org.apache.commons.math3.linear.LUDecomposition; +import org.apache.commons.math3.linear.MatrixUtils; +import org.apache.commons.math3.linear.RealMatrix; +import org.apache.commons.math3.linear.RealVector; +import org.apache.commons.math3.util.FastMath; + +import org.hps.recon.vertexing.TrackConstraintVertexFitter.Constraint; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.FitResult; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.TrackMomentum; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.TrackParams; + +/** + * The original Kalman gain-matrix vertex fit (iterative sequential updates, Joseph-form + * covariance), superseded in all production/analysis code by the Billoir-batch linear-algebra + * path ({@code TrackConstraintVertexFitter.fitBillior1985}/{@code fitVertex}). Kept here for + * reference/regression testing only -- it lives under {@code src/test}, so it cannot be + * referenced from any {@code src/main} code, structurally enforcing that nothing in production + * or analysis code depends on it. + * + *

Holds a {@link TrackConstraintVertexFitter} instance and calls its package-private + * per-track constraint helpers ({@code computeTrackConstraint}, {@code + * computeMomentumVertexDerivatives}, {@code computeMomentumCovariance}, {@code + * computeMomentumTrackJacobian}, {@code perigeeToVertexParams}), which are shared with that + * class's own {@code fitCascadeVertexJoint*} family. + */ +public class GainMatrixVertexer { + + private final TrackConstraintVertexFitter fitter; + private boolean debugFlag = false; + + public GainMatrixVertexer(double bField) { + this.fitter = new TrackConstraintVertexFitter(bField); + } + + public void setDebugFlag(boolean debugFlag) { + this.debugFlag = debugFlag; + } + + private Constraint computeVertexConstraint(RealVector vertex, + RealVector vertexConstraint, + RealMatrix vertexConstraintCov) { + RealVector c = vertex.subtract(vertexConstraint); + RealMatrix H = MatrixUtils.createRealIdentityMatrix(3); + RealMatrix V = vertexConstraintCov; + + return new Constraint(c, H, V); + } + + /** + * Compute momentum constraint. The covariance V includes both beam momentum uncertainty + * AND track momentum uncertainties. + */ + private Constraint computeMomentumConstraint(List tracks, + RealVector vertex, + RealVector momentumConstraint, + RealMatrix momentumConstraintCov) { + RealVector totalP = MatrixUtils.createRealVector(new double[3]); + RealMatrix dpDvertex = MatrixUtils.createRealMatrix(3, 3); + RealMatrix totalPCov = MatrixUtils.createRealMatrix(3, 3); + + for (TrackParams track : tracks) { + RealVector p = fitter.computeMomentumAtVertex(track, vertex); + totalP = totalP.add(p); + + RealMatrix dpDv = fitter.computeMomentumVertexDerivatives(track, vertex); + dpDvertex = dpDvertex.add(dpDv); + + RealMatrix pCov = fitter.computeMomentumCovariance(track, vertex); + totalPCov = totalPCov.add(pCov); + } + + RealVector c = totalP.subtract(momentumConstraint); + RealMatrix H = dpDvertex; + RealMatrix V = momentumConstraintCov.add(totalPCov); + + return new Constraint(c, H, V); + } + + /** + * Compute mass constraint. Invariant mass: M^2 = (Sum E)^2 - (Sum p)^2. + */ + private Constraint computeMassConstraint(List tracks, + RealVector vertex, + double massConstraint, + double massConstraintSigma) { + double mPi = 0.13957; // GeV/c^2 + + double totalE = 0.0; + RealVector totalP = MatrixUtils.createRealVector(new double[3]); + RealVector dEDvertex = MatrixUtils.createRealVector(new double[3]); + RealMatrix dpDvertex = MatrixUtils.createRealMatrix(3, 3); + + for (TrackParams track : tracks) { + RealVector p = fitter.computeMomentumAtVertex(track, vertex); + double pMag = p.getNorm(); + + double E = FastMath.sqrt(pMag * pMag + mPi * mPi); + totalE += E; + totalP = totalP.add(p); + + RealMatrix dpDv = fitter.computeMomentumVertexDerivatives(track, vertex); + + for (int i = 0; i < 3; i++) { + double dEDv = 0.0; + for (int j = 0; j < 3; j++) { + dEDv += p.getEntry(j) * dpDv.getEntry(j, i); + } + dEDvertex.setEntry(i, dEDvertex.getEntry(i) + dEDv / E); + } + + dpDvertex = dpDvertex.add(dpDv); + } + + double totalPmag = totalP.getNorm(); + double M = FastMath.sqrt(totalE * totalE - totalPmag * totalPmag); + + RealVector c = MatrixUtils.createRealVector(new double[]{M - massConstraint}); + + RealVector dMDvertex = MatrixUtils.createRealVector(new double[3]); + for (int i = 0; i < 3; i++) { + double dpDotDv = 0.0; + for (int j = 0; j < 3; j++) { + dpDotDv += totalP.getEntry(j) * dpDvertex.getEntry(j, i); + } + dMDvertex.setEntry(i, (totalE * dEDvertex.getEntry(i) - dpDotDv) / M); + } + + RealMatrix H = MatrixUtils.createRealMatrix(1, 3); + H.setRowVector(0, dMDvertex); + + RealMatrix V = MatrixUtils.createRealMatrix(1, 1); + V.setEntry(0, 0, massConstraintSigma * massConstraintSigma); + + return new Constraint(c, H, V); + } + + public FitResult fit(List tracks, + RealVector initialVertex, + RealVector vertexConstraint, + RealMatrix vertexConstraintCov, + RealVector momentumConstraint, + RealMatrix momentumConstraintCov, + Double massConstraint, + Double massConstraintSigma, + int maxIterations, + double tolerance) { + + int nTracks = tracks.size(); + + // Initial vertex + RealVector vertex; + if (initialVertex != null) { + vertex = initialVertex.copy(); + } else if (vertexConstraint != null) { + vertex = vertexConstraint.copy(); + } else { + double xInit = 0.0, yInit = 0.0, zInit = 0.0; + for (TrackParams track : tracks) { + xInit += -track.d0 * FastMath.sin(track.phi0); + yInit += track.d0 * FastMath.cos(track.phi0); + zInit += track.z0; + } + vertex = MatrixUtils.createRealVector(new double[]{ + xInit / nTracks, yInit / nTracks, zInit / nTracks + }); + } + + // Prior covariance, re-applied at the top of every iteration below (see comment + // there for why) rather than the initial value of a running C. + RealMatrix priorC; + if (vertexConstraint != null && vertexConstraintCov != null) { + priorC = vertexConstraintCov.copy(); + } else { + priorC = MatrixUtils.createRealIdentityMatrix(3).scalarMultiply(100.0); + } + RealMatrix I = MatrixUtils.createRealIdentityMatrix(3); + RealMatrix C = priorC; + + // Iterative Gain Matrix updates + for (int iteration = 0; iteration < maxIterations; iteration++) { + RealVector vertexOld = vertex.copy(); + // Reset to the vague prior each iteration: the vertex/track/momentum/mass + // constraints below are the same fixed measurements being re-linearized at + // successive vertex guesses, not new independent data arriving sequentially. + // Carrying C forward across iterations would reprocess the same information + // repeatedly, shrinking the covariance by roughly a factor of (iterations to + // converge) -- same bug as originally found and fixed in fitCascadeVertex(). + C = priorC; + + // Apply vertex constraint + if (vertexConstraint != null && vertexConstraintCov != null) { + Constraint constraint = computeVertexConstraint(vertex, vertexConstraint, + vertexConstraintCov); + + RealMatrix S = constraint.H.multiply(C).multiply(constraint.H.transpose()).add(constraint.V); + RealMatrix K = C.multiply(constraint.H.transpose()).multiply( + new LUDecomposition(S).getSolver().getInverse() + ); + + vertex = vertex.subtract(K.operate(constraint.c)); + + RealMatrix ImKH = I.subtract(K.multiply(constraint.H)); + C = ImKH.multiply(C).multiply(ImKH.transpose()) + .add(K.multiply(constraint.V).multiply(K.transpose())); + } + + // Apply track constraints + for (TrackParams track : tracks) { + Constraint constraint = fitter.computeTrackConstraint(track, vertex); + + RealMatrix S = constraint.H.multiply(C).multiply(constraint.H.transpose()).add(constraint.V); + RealMatrix K = C.multiply(constraint.H.transpose()).multiply( + new LUDecomposition(S).getSolver().getInverse() + ); + + vertex = vertex.subtract(K.operate(constraint.c)); + RealMatrix ImKH = I.subtract(K.multiply(constraint.H)); + C = ImKH.multiply(C).multiply(ImKH.transpose()) + .add(K.multiply(constraint.V).multiply(K.transpose())); + } + + // Apply momentum constraint + if (momentumConstraint != null && momentumConstraintCov != null) { + try { + Constraint constraint = computeMomentumConstraint(tracks, vertex, + momentumConstraint, + momentumConstraintCov); + + RealMatrix S = constraint.H.multiply(C).multiply(constraint.H.transpose()).add(constraint.V); + RealMatrix K = C.multiply(constraint.H.transpose()).multiply( + new LUDecomposition(S).getSolver().getInverse() + ); + + vertex = vertex.subtract(K.operate(constraint.c)); + RealMatrix ImKH = I.subtract(K.multiply(constraint.H)); + C = ImKH.multiply(C).multiply(ImKH.transpose()) + .add(K.multiply(constraint.V).multiply(K.transpose())); + } catch (Exception e) { + // Skip if singular + } + } + + // Apply mass constraint + if (massConstraint != null && massConstraintSigma != null) { + try { + Constraint constraint = computeMassConstraint(tracks, vertex, + massConstraint, + massConstraintSigma); + + RealMatrix S = constraint.H.multiply(C).multiply(constraint.H.transpose()).add(constraint.V); + RealMatrix K = C.multiply(constraint.H.transpose()).multiply( + new LUDecomposition(S).getSolver().getInverse() + ); + + vertex = vertex.subtract(K.operate(constraint.c)); + RealMatrix ImKH = I.subtract(K.multiply(constraint.H)); + C = ImKH.multiply(C).multiply(ImKH.transpose()) + .add(K.multiply(constraint.V).multiply(K.transpose())); + } catch (Exception e) { + // Skip if singular + } + } + + // Check convergence + if (vertex.subtract(vertexOld).getNorm() < tolerance) { + break; + } + } + + // Calculate chi-squared with individual contributions + double chi2 = 0.0; + double chi2Vertex = 0.0; + double chi2Momentum = 0.0; + double[] chi2Tracks = new double[nTracks]; + + if (vertexConstraint != null && vertexConstraintCov != null) { + Constraint constraint = computeVertexConstraint(vertex, vertexConstraint, + vertexConstraintCov); + RealMatrix VInv = new LUDecomposition(constraint.V).getSolver().getInverse(); + chi2Vertex = constraint.c.dotProduct(VInv.operate(constraint.c)); + chi2 += chi2Vertex; + } + + for (int itrk = 0; itrk < nTracks; itrk++) { + Constraint constraint = fitter.computeTrackConstraint(tracks.get(itrk), vertex); + RealMatrix VInv = new LUDecomposition(constraint.V).getSolver().getInverse(); + chi2Tracks[itrk] = constraint.c.dotProduct(VInv.operate(constraint.c)); + chi2 += chi2Tracks[itrk]; + } + + if (momentumConstraint != null && momentumConstraintCov != null) { + try { + Constraint constraint = computeMomentumConstraint(tracks, vertex, + momentumConstraint, + momentumConstraintCov); + RealMatrix VInv = new LUDecomposition(constraint.V).getSolver().getInverse(); + chi2Momentum = constraint.c.dotProduct(VInv.operate(constraint.c)); + chi2 += chi2Momentum; + } catch (Exception e) { + // Skip if singular + } + } + + if (massConstraint != null && massConstraintSigma != null) { + try { + Constraint constraint = computeMassConstraint(tracks, vertex, + massConstraint, + massConstraintSigma); + RealMatrix VInv = new LUDecomposition(constraint.V).getSolver().getInverse(); + chi2 += constraint.c.dotProduct(VInv.operate(constraint.c)); + } catch (Exception e) { + // Skip if singular + } + } + + if (debugFlag) { + System.out.printf(" Chi2 contributions: vertex=%.4f", chi2Vertex); + for (int itrk = 0; itrk < nTracks; itrk++) { + System.out.printf(" track%d=%.4f", itrk, chi2Tracks[itrk]); + } + System.out.printf(" momentum=%.4f total=%.4f%n", chi2Momentum, chi2); + } + + // NDF + int ndf = 2 * nTracks - 3; + if (vertexConstraint != null) ndf += 3; + if (momentumConstraint != null) ndf += 3; + if (massConstraint != null) ndf += 1; + + // Track momenta + List trackMomenta = new ArrayList(); + for (TrackParams track : tracks) { + RealVector p = fitter.computeMomentumAtVertex(track, vertex); + RealMatrix pCov = fitter.computeMomentumCovariance(track, vertex); + trackMomenta.add(new TrackMomentum(p, pCov)); + } + + return new FitResult(vertex, C, chi2, ndf, trackMomenta); + } + + public FitResult fit(List tracks) { + return fit(tracks, null, null, null, null, null, null, null, 10, 1e-6); + } + + public FitResult fit(List tracks, int maxIterations, double tolerance) { + return fit(tracks, null, null, null, null, null, null, null, maxIterations, tolerance); + } +} diff --git a/recon/src/test/java/org/hps/recon/vertexing/GainMatrixVertexerTest.java b/recon/src/test/java/org/hps/recon/vertexing/GainMatrixVertexerTest.java new file mode 100644 index 0000000000..0391e3be12 --- /dev/null +++ b/recon/src/test/java/org/hps/recon/vertexing/GainMatrixVertexerTest.java @@ -0,0 +1,117 @@ +package org.hps.recon.vertexing; + +import java.util.Arrays; +import java.util.List; + +import org.apache.commons.math3.linear.MatrixUtils; +import org.apache.commons.math3.linear.RealMatrix; +import org.apache.commons.math3.linear.RealVector; +import org.apache.commons.math3.util.FastMath; + +import junit.framework.TestCase; + +import org.hps.recon.vertexing.TrackConstraintVertexFitter.FitResult; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.TrackParams; + +/** + * Exact-geometry sanity check for the extracted gain-matrix {@link GainMatrixVertexer}: build + * an electron and a positron track that are both constructed to pass exactly through the same + * known point, with small (non-singular) measurement covariances, and verify {@code fit} + * recovers that point with near-zero chi2. Kept alongside {@link NTrackVertexerTest}'s + * equivalent Billoir-batch check so the superseded gain-matrix algorithm still has a working + * regression test even though nothing in {@code src/main} calls it anymore. + */ +public class GainMatrixVertexerTest extends TestCase { + + private static final double B_FIELD = 0.5; // Tesla + private static final double C = 2.99792458e-4; + + public void testFitVertexExactGeometry() { + double xV = 0.3, yV = -0.2, zV = 5.0; + double pxEle = 0.30, pyEle = 0.10, pzEle = 1.5; + double pxPos = 0.15, pyPos = -0.05, pzPos = 0.8; + + TrackParams eleTrack = makeTrackParamsThroughPoint(xV, yV, zV, pxEle, pyEle, pzEle, -1); + TrackParams posTrack = makeTrackParamsThroughPoint(xV, yV, zV, pxPos, pyPos, pzPos, 1); + + GainMatrixVertexer vertexer = new GainMatrixVertexer(B_FIELD); + FitResult result = vertexer.fit(Arrays.asList(eleTrack, posTrack)); + + assertNotNull("fit result should not be null", result); + + RealVector vertex = result.vertex; + System.out.printf("Fitted vertex (trk frame): [%.6f, %.6f, %.6f] (truth: [%.6f, %.6f, %.6f])%n", + vertex.getEntry(0), vertex.getEntry(1), vertex.getEntry(2), xV, yV, zV); + System.out.printf("Chi2: %.6f%n", result.chi2); + + assertEquals(xV, vertex.getEntry(0), 1e-3); + assertEquals(yV, vertex.getEntry(1), 1e-3); + assertEquals(zV, vertex.getEntry(2), 1e-3); + assertTrue("chi2 should be small for exactly-consistent geometry, got " + result.chi2, + result.chi2 < 1e-2); + } + + /** + * Same exact-geometry check, but with an explicit non-origin initial vertex guess, to + * verify {@code fit} converges to the true point regardless of starting point. + */ + public void testFitVertexExactGeometryWithInitialVertex() { + double xV = 0.3, yV = -0.2, zV = 5.0; + double pxEle = 0.30, pyEle = 0.10, pzEle = 1.5; + double pxPos = 0.15, pyPos = -0.05, pzPos = 0.8; + + TrackParams eleTrack = makeTrackParamsThroughPoint(xV, yV, zV, pxEle, pyEle, pzEle, -1); + TrackParams posTrack = makeTrackParamsThroughPoint(xV, yV, zV, pxPos, pyPos, pzPos, 1); + + GainMatrixVertexer vertexer = new GainMatrixVertexer(B_FIELD); + RealVector initialVertex = MatrixUtils.createRealVector(new double[]{0.0, 0.0, 0.0}); + FitResult result = vertexer.fit(Arrays.asList(eleTrack, posTrack), initialVertex, + null, null, null, null, null, null, 20, 1e-10); + + assertNotNull("fit result should not be null", result); + + RealVector vertex = result.vertex; + assertEquals(xV, vertex.getEntry(0), 1e-3); + assertEquals(yV, vertex.getEntry(1), 1e-3); + assertEquals(zV, vertex.getEntry(2), 1e-3); + assertTrue("chi2 should be small for exactly-consistent geometry, got " + result.chi2, + result.chi2 < 1e-2); + } + + /** + * Build track parameters passing exactly through (xV,yV,zV) with the given momentum, by + * inverting the same center/perigee formulas as + * {@code TrackConstraintVertexFitterTest#createExactTrackThroughPoint}. + */ + private static TrackParams makeTrackParamsThroughPoint(double xV, double yV, double zV, + double px, double py, double pz, int charge) { + double pT = FastMath.sqrt(px * px + py * py); + double omega = charge * C * B_FIELD / pT; + double R = 1.0 / FastMath.abs(omega); + double sign = FastMath.signum(omega); + double phiV = FastMath.atan2(py, px); + double tanLambda = pz / pT; + + double xc = xV + R * sign * FastMath.sin(phiV); + double yc = yV - R * sign * FastMath.cos(phiV); + + double A = FastMath.sqrt(xc * xc + yc * yc); + double phi0 = FastMath.atan2(sign * xc, -sign * yc); + double d0 = sign * (R - A); + + double dphi = phiV - phi0; + while (dphi > FastMath.PI) dphi -= 2.0 * FastMath.PI; + while (dphi < -FastMath.PI) dphi += 2.0 * FastMath.PI; + double s = -sign * R * dphi; + double z0 = zV - s * tanLambda; + + RealMatrix cov = MatrixUtils.createRealMatrix(5, 5); + cov.setEntry(0, 0, 1e-4); + cov.setEntry(1, 1, 1e-5); + cov.setEntry(2, 2, 1e-8); + cov.setEntry(3, 3, 1e-4); + cov.setEntry(4, 4, 1e-5); + + return new TrackParams(d0, phi0, omega, z0, tanLambda, cov); + } +} diff --git a/recon/src/test/java/org/hps/recon/vertexing/NTrackVertexerTest.java b/recon/src/test/java/org/hps/recon/vertexing/NTrackVertexerTest.java new file mode 100644 index 0000000000..c2eaeb9fd7 --- /dev/null +++ b/recon/src/test/java/org/hps/recon/vertexing/NTrackVertexerTest.java @@ -0,0 +1,328 @@ +package org.hps.recon.vertexing; + +import java.util.Arrays; + +import hep.physics.matrix.SymmetricMatrix; +import hep.physics.vec.Hep3Vector; +import hep.physics.vec.VecOp; + +import junit.framework.TestCase; + +import org.apache.commons.math.util.FastMath; + +import org.lcsim.event.Track; +import org.lcsim.event.TrackState; +import org.lcsim.event.base.BaseTrack; +import org.lcsim.event.base.BaseTrackState; + +/** + * Exact-geometry sanity check for {@link NTrackVertexer}: build an electron and + * a positron track that are both constructed to pass exactly through the same known point, with + * small (non-singular) measurement covariances, and verify {@code fitVertexNoBeamConstraint} + * recovers that point with near-zero chi2. Covers the 2-track (V0) case, formerly exercised via + * the now-removed {@code KalmanV0Vertexer}. + */ +public class NTrackVertexerTest extends TestCase { + + private static final double B_FIELD = 0.5; // Tesla + private static final double ELECTRON_MASS = 0.000511; // GeV + private static final double C = 2.99792458e-4; + + public void testFitVertexExactGeometry() { + double xV = 0.3, yV = -0.2, zV = 5.0; + double pxEle = 0.30, pyEle = 0.10, pzEle = 1.5; + double pxPos = 0.15, pyPos = -0.05, pzPos = 0.8; + + Track eleTrack = makeTrackThroughPoint(xV, yV, zV, pxEle, pyEle, pzEle, -1); + Track posTrack = makeTrackThroughPoint(xV, yV, zV, pxPos, pyPos, pzPos, 1); + + NTrackVertexer vertexer = new NTrackVertexer(B_FIELD); + BilliorVertex vtx = vertexer.fitVertexNoBeamConstraint(Arrays.asList(eleTrack, posTrack)); + + assertNotNull("fit result should not be null", vtx); + + Hep3Vector pos = vtx.getPosition(); + System.out.printf("Fitted vertex (det frame): [%.6f, %.6f, %.6f] (truth trk frame: [%.6f, %.6f, %.6f])%n", + pos.x(), pos.y(), pos.z(), xV, yV, zV); + System.out.printf("Chi2: %.6f%n", vtx.getChi2()); + + // Detector frame: det(x,y,z) = trk(y,z,x) + assertEquals(yV, pos.x(), 1e-3); + assertEquals(zV, pos.y(), 1e-3); + assertEquals(xV, pos.z(), 1e-3); + assertTrue("chi2 should be small for exactly-consistent geometry, got " + vtx.getChi2(), + vtx.getChi2() < 1e-2); + + double eEle = FastMath.sqrt(pxEle * pxEle + pyEle * pyEle + pzEle * pzEle + + ELECTRON_MASS * ELECTRON_MASS); + double ePos = FastMath.sqrt(pxPos * pxPos + pyPos * pyPos + pzPos * pzPos + + ELECTRON_MASS * ELECTRON_MASS); + double pxSum = pxEle + pxPos, pySum = pyEle + pyPos, pzSum = pzEle + pzPos; + double massSqTruth = (eEle + ePos) * (eEle + ePos) - (pxSum * pxSum + pySum * pySum + pzSum * pzSum); + double massTruth = massSqTruth > 0 ? FastMath.sqrt(massSqTruth) : -1.0; + + assertEquals(massTruth, vtx.getInvMass(), 1e-3); + } + + /** + * Same exact-geometry check as {@link #testFitVertexExactGeometry}, but with both tracks' + * AtPerigee reference point set to the real target position (tracking-frame x = -1.1mm, as + * used by production reconstruction) instead of the origin, to verify the fitted vertex is + * correctly shifted back into the absolute tracking/detector frame rather than being left + * relative to the tracks' reference point. + */ + public void testFitVertexExactGeometryNonOriginReferencePoint() { + double xV = 0.3, yV = -0.2, zV = 5.0; + double pxEle = 0.30, pyEle = 0.10, pzEle = 1.5; + double pxPos = 0.15, pyPos = -0.05, pzPos = 0.8; + double[] refPoint = {-1.1, 0.0, 0.0}; + + Track eleTrack = makeTrackThroughPoint(xV, yV, zV, pxEle, pyEle, pzEle, -1, refPoint); + Track posTrack = makeTrackThroughPoint(xV, yV, zV, pxPos, pyPos, pzPos, 1, refPoint); + + NTrackVertexer vertexer = new NTrackVertexer(B_FIELD); + BilliorVertex vtx = vertexer.fitVertexNoBeamConstraint(Arrays.asList(eleTrack, posTrack)); + + assertNotNull("fit result should not be null", vtx); + + Hep3Vector pos = vtx.getPosition(); + System.out.printf("Fitted vertex (det frame, non-origin ref): [%.6f, %.6f, %.6f] " + + "(truth trk frame: [%.6f, %.6f, %.6f])%n", + pos.x(), pos.y(), pos.z(), xV, yV, zV); + + // Detector frame: det(x,y,z) = trk(y,z,x). fitVertexNoBeamConstraint dispatches to + // fitBillior1985, a single-shot linearization around v0=(0,0,0) in the tracks' local + // (reference-point-relative) frame -- unlike the iterative gain-matrix path it + // replaces, it does not re-linearize at successive vertex guesses. With refPoint + // x=-1.1mm, the true vertex sits 1.4mm from v0 in local x (vs 0.3mm in the + // origin-reference-point test), large enough for the resulting O(offset^2) residual + // to exceed a 1um tolerance on the tracking-x/z (detector-z/y) axes -- hence the + // looser tolerance on those two only. + assertEquals(yV, pos.x(), 1e-3); + assertEquals(zV, pos.y(), 0.02); + assertEquals(xV, pos.z(), 0.01); + assertTrue("chi2 should be small for exactly-consistent geometry, got " + vtx.getChi2(), + vtx.getChi2() < 1e-2); + } + + /** + * Verify {@code fitVertexBeamspotConstrained} actually pulls the fitted vertex toward a + * supplied beamspot position, and that calling the pre-existing {@code + * fitVertexNoBeamConstraint} is completely unaffected (same tracks, called before and after + * the new method) -- proving the new opt-in method causes zero regression to existing + * behavior. + */ + public void testFitVertexBeamspotConstrainedPullsTowardBeamPosition() { + double xV = 0.3, yV = -0.2, zV = 5.0; + double pxEle = 0.30, pyEle = 0.10, pzEle = 1.5; + double pxPos = 0.15, pyPos = -0.05, pzPos = 0.8; + + Track eleTrack = makeTrackThroughPoint(xV, yV, zV, pxEle, pyEle, pzEle, -1); + Track posTrack = makeTrackThroughPoint(xV, yV, zV, pxPos, pyPos, pzPos, 1); + + NTrackVertexer vertexer = new NTrackVertexer(B_FIELD); + + BilliorVertex vtxUnconstrainedBefore = vertexer.fitVertexNoBeamConstraint(Arrays.asList(eleTrack, posTrack)); + Hep3Vector posUnc = vtxUnconstrainedBefore.getPosition(); + + // beamSize must be tight not just relative to the tracks' individual d0/z0 sigmas, but + // relative to the *track pair's own vertex information matrix*, which for two tracks + // crossing at a long lever arm from the reference point is highly anisotropic and can + // have an eigenvalue (best-constrained direction) far tighter than any single sigma + // would suggest -- 1e-6 (information ~1e12) safely dominates that too, unlike an + // initially-tried 0.001 (information ~1e6, only comparable to the tracks' best + // direction, which pulled the fit to neither the truth nor the beam position). + double[] beamPosition = {0.0, 0.0, 0.0}; + double[] beamSize = {1e-6, 1e-6, 1e-6}; + BilliorVertex vtxBS = vertexer.fitVertexBeamspotConstrained( + Arrays.asList(eleTrack, posTrack), beamPosition, beamSize); + assertNotNull("beamspot-constrained fit result should not be null", vtxBS); + Hep3Vector posBS = vtxBS.getPosition(); + + System.out.printf("Unconstrained vertex (det frame): [%.6f, %.6f, %.6f]%n", + posUnc.x(), posUnc.y(), posUnc.z()); + System.out.printf("Beamspot-constrained vertex (det frame): [%.6f, %.6f, %.6f] " + + "(beam position trk frame: [%.6f, %.6f, %.6f], beam size trk frame: [%.6f, %.6f, %.6f])%n", + posBS.x(), posBS.y(), posBS.z(), + beamPosition[0], beamPosition[1], beamPosition[2], + beamSize[0], beamSize[1], beamSize[2]); + + // The beamspot prior is far tighter than the tracks' own position sensitivity, and + // centered far from the true (and unconstrained-fit) vertex, so the constrained fit + // should land very close to the beam position (det frame: det(x,y,z) = trk(y,z,x)). + assertEquals(0.0, posBS.x(), 0.01); + assertEquals(0.0, posBS.y(), 0.01); + assertEquals(0.0, posBS.z(), 0.01); + + double pullDistance = VecOp.sub(posBS, posUnc).magnitude(); + assertTrue("beamspot constraint should measurably pull the vertex away from the " + + "unconstrained result, got pull distance " + pullDistance, pullDistance > 1.0); + + // Calling the pre-existing unconstrained method again on the same tracks must give a + // bit-identical result to the first call above -- the new method must not have mutated + // any shared state or otherwise perturbed existing behavior. + BilliorVertex vtxUnconstrainedAfter = vertexer.fitVertexNoBeamConstraint(Arrays.asList(eleTrack, posTrack)); + Hep3Vector posUncAfter = vtxUnconstrainedAfter.getPosition(); + assertEquals(posUnc.x(), posUncAfter.x(), 1e-12); + assertEquals(posUnc.y(), posUncAfter.y(), 1e-12); + assertEquals(posUnc.z(), posUncAfter.z(), 1e-12); + } + + /** + * Verify {@code fitVertexBothConstrained} pulls the fitted vertex toward a supplied + * beamspot position AND the total 2-track momentum toward a supplied beam momentum, + * simultaneously -- and that the pre-existing single-constraint methods on the same + * tracks are completely unaffected by having called the new method. + */ + public void testFitVertexBothConstrainedPullsTowardBeamspotAndBeamMomentum() { + double xV = 0.3, yV = -0.2, zV = 5.0; + // Unlike the other tests in this file (which only need a non-degenerate momentum to + // check vertex-position recovery), this test also applies a beam-momentum constraint, + // so the tracks' total momentum must be dominantly along tracking-X to match the beam + // direction convention below (beamPx=beamEnergy*cos(rotAngle) dominant) -- tracks whose + // total momentum instead points dominantly along tracking-Z (as in the other tests' + // pzEle=1.5-style values) would require fitSoftConstrained's Newton-Raphson solve to + // rotate the total momentum by nearly 90 degrees in a few linearized steps, which does + // not converge to a sensible answer (confirmed by direct fitSoftConstrained probing). + double pxEle = 1.9, pyEle = 0.15, pzEle = 0.05; + double pxPos = 1.3, pyPos = -0.20, pzPos = -0.03; + + Track eleTrack = makeTrackThroughPoint(xV, yV, zV, pxEle, pyEle, pzEle, -1); + Track posTrack = makeTrackThroughPoint(xV, yV, zV, pxPos, pyPos, pzPos, 1); + + NTrackVertexer vertexer = new NTrackVertexer(B_FIELD); + + BilliorVertex vtxUnconstrainedBefore = vertexer.fitVertexNoBeamConstraint(Arrays.asList(eleTrack, posTrack)); + Hep3Vector posUnc = vtxUnconstrainedBefore.getPosition(); + Hep3Vector totalPUnc = VecOp.add(vtxUnconstrainedBefore.getFittedMomentum(0), + vtxUnconstrainedBefore.getFittedMomentum(1)); + + // Beam momentum target chosen far from the tracks' own (unconstrained) momentum sum + // [3.2, -0.05, 0.02] (trk frame), so a real pull is unambiguous. + double beamEnergy = 3.7; + double beamRotAngle = 0.0305; + double beamPx = beamEnergy * FastMath.cos(beamRotAngle); + double beamPy = -beamEnergy * FastMath.sin(beamRotAngle); + // getFittedMomentum() (used below) returns detector frame, det(x,y,z) = trk(y,z,x), + // same convention as vertex position -- so the comparison target must be built in + // that frame too, not tracking frame. + Hep3Vector beamP = new hep.physics.vec.BasicHep3Vector(beamPy, 0.0, beamPx); + + // Same tight beamspot prior as testFitVertexBeamspotConstrainedPullsTowardBeamPosition, + // far from the tracks' own (unconstrained) vertex position. + double[] beamPosition = {0.0, 0.0, 0.0}; + double[] beamSize = {1e-6, 1e-6, 1e-6}; + + BilliorVertex vtxBoth = vertexer.fitVertexBothConstrained( + Arrays.asList(eleTrack, posTrack), beamPosition, beamSize, beamEnergy, beamRotAngle, 0.0); + assertNotNull("both-constrained fit result should not be null", vtxBoth); + Hep3Vector posBoth = vtxBoth.getPosition(); + Hep3Vector totalPBoth = VecOp.add(vtxBoth.getFittedMomentum(0), vtxBoth.getFittedMomentum(1)); + + System.out.printf("Unconstrained vertex (det frame): [%.6f, %.6f, %.6f], total P: [%.6f, %.6f, %.6f]%n", + posUnc.x(), posUnc.y(), posUnc.z(), totalPUnc.x(), totalPUnc.y(), totalPUnc.z()); + System.out.printf("Both-constrained vertex (det frame): [%.6f, %.6f, %.6f], total P: [%.6f, %.6f, %.6f] " + + "(beam P det frame: [%.6f, %.6f, %.6f])%n", + posBoth.x(), posBoth.y(), posBoth.z(), totalPBoth.x(), totalPBoth.y(), totalPBoth.z(), + beamP.x(), beamP.y(), beamP.z()); + + // Position: the beamspot prior is far tighter than the tracks' own position + // sensitivity, so the constrained fit should land very close to the beam position + // (det frame: det(x,y,z) = trk(y,z,x)). + assertEquals(0.0, posBoth.x(), 0.01); + assertEquals(0.0, posBoth.y(), 0.01); + assertEquals(0.0, posBoth.z(), 0.01); + + // Momentum: the both-constrained total momentum should land measurably closer to the + // beam momentum target than the unconstrained fit's total momentum did. + double distUncFromBeam = VecOp.sub(totalPUnc, beamP).magnitude(); + double distBothFromBeam = VecOp.sub(totalPBoth, beamP).magnitude(); + assertTrue("both-constrained total momentum should be pulled measurably closer to the " + + "beam momentum target than the unconstrained fit (unc dist=" + distUncFromBeam + + ", both dist=" + distBothFromBeam + ")", distBothFromBeam < 0.5 * distUncFromBeam); + + // Calling the pre-existing single-constraint methods again on the same tracks must be + // completely unaffected by having called fitVertexBothConstrained above. + BilliorVertex vtxUnconstrainedAfter = vertexer.fitVertexNoBeamConstraint(Arrays.asList(eleTrack, posTrack)); + Hep3Vector posUncAfter = vtxUnconstrainedAfter.getPosition(); + assertEquals(posUnc.x(), posUncAfter.x(), 1e-12); + assertEquals(posUnc.y(), posUncAfter.y(), 1e-12); + assertEquals(posUnc.z(), posUncAfter.z(), 1e-12); + + BilliorVertex vtxBS = vertexer.fitVertexBeamspotConstrained( + Arrays.asList(eleTrack, posTrack), beamPosition, beamSize); + Hep3Vector posBS = vtxBS.getPosition(); + assertEquals(0.0, posBS.x(), 0.01); + assertEquals(0.0, posBS.y(), 0.01); + assertEquals(0.0, posBS.z(), 0.01); + } + + public void testPlaceholderVertexOnFailedFit() { + BilliorVertex placeholder = NTrackVertexer.placeholderVertex(2); + assertNotNull(placeholder); + assertEquals(-9999.0, placeholder.getChi2(), 1e-9); + assertEquals(-9999.0, placeholder.getInvMass(), 1e-9); + assertEquals(-9999.0, placeholder.getCustomParameters().get("ndf"), 1e-9); + } + + /** + * Build a Track with a single AtPerigee TrackState whose helix passes exactly through + * (xV,yV,zV) with the given momentum, by inverting the same center/perigee formulas as + * {@code TrackConstraintVertexFitterTest#createExactTrackThroughPoint}. + */ + private static Track makeTrackThroughPoint(double xV, double yV, double zV, + double px, double py, double pz, int charge) { + return makeTrackThroughPoint(xV, yV, zV, px, py, pz, charge, new double[]{0.0, 0.0, 0.0}); + } + + /** + * As above, but with the perigee params expressed relative to {@code refPoint} (tracking + * frame) instead of the origin, and the TrackState's reference point set accordingly. + */ + private static Track makeTrackThroughPoint(double xV, double yV, double zV, + double px, double py, double pz, int charge, double[] refPoint) { + double xVLocal = xV - refPoint[0]; + double yVLocal = yV - refPoint[1]; + double zVLocal = zV - refPoint[2]; + double pT = FastMath.sqrt(px * px + py * py); + double omega = charge * C * B_FIELD / pT; + double R = 1.0 / FastMath.abs(omega); + double sign = FastMath.signum(omega); + double phiV = FastMath.atan2(py, px); + double tanLambda = pz / pT; + + double xc = xVLocal + R * sign * FastMath.sin(phiV); + double yc = yVLocal - R * sign * FastMath.cos(phiV); + + double A = FastMath.sqrt(xc * xc + yc * yc); + double phi0 = FastMath.atan2(sign * xc, -sign * yc); + double d0 = sign * (R - A); + + double dphi = phiV - phi0; + while (dphi > FastMath.PI) dphi -= 2.0 * FastMath.PI; + while (dphi < -FastMath.PI) dphi += 2.0 * FastMath.PI; + double s = -sign * R * dphi; + double z0 = zVLocal - s * tanLambda; + + double[] params = new double[5]; + params[BaseTrack.D0] = d0; + params[BaseTrack.PHI] = phi0; + params[BaseTrack.OMEGA] = omega; + params[BaseTrack.Z0] = z0; + params[BaseTrack.TANLAMBDA] = tanLambda; + + SymmetricMatrix cov = new SymmetricMatrix(5); + cov.setElement(0, 0, 1e-4); + cov.setElement(1, 1, 1e-5); + cov.setElement(2, 2, 1e-8); + cov.setElement(3, 3, 1e-4); + cov.setElement(4, 4, 1e-5); + + BaseTrackState ts = new BaseTrackState(params, refPoint, + cov.asPackedArray(true), TrackState.AtPerigee, B_FIELD); + + BaseTrack track = new BaseTrack(); + track.getTrackStates().add(ts); + return track; + } +} diff --git a/recon/src/test/java/org/hps/recon/vertexing/TrackConstraintVertexFitterTest.java b/recon/src/test/java/org/hps/recon/vertexing/TrackConstraintVertexFitterTest.java new file mode 100644 index 0000000000..4b8c0d5902 --- /dev/null +++ b/recon/src/test/java/org/hps/recon/vertexing/TrackConstraintVertexFitterTest.java @@ -0,0 +1,2417 @@ +package org.hps.recon.vertexing; + +import junit.framework.TestCase; + +import org.apache.commons.math3.linear.CholeskyDecomposition; +import org.apache.commons.math3.linear.MatrixUtils; +import org.apache.commons.math3.linear.RealMatrix; +import org.apache.commons.math3.linear.RealVector; +import org.apache.commons.math.util.FastMath; + +import org.hps.recon.vertexing.TrackConstraintVertexFitter; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.TrackParams; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.FitResult; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.TwoVertexFitResult; +import org.hps.recon.vertexing.TrackConstraintVertexFitter.TrackMomentum; + +import java.util.ArrayList; +import java.util.List; + +/** + * Test cases for TrackConstraintVertexFitter, particularly the Lagrange multiplier + * constrained fitting for three-prong vertices with 4-momentum conservation. + */ +public class TrackConstraintVertexFitterTest extends TestCase { + + private static final double B_FIELD = 1.0; // Tesla + private static final double ELECTRON_MASS = 0.000511; // GeV + + // Field conversion constant: pT [GeV] = C * B [T] / |omega| [1/mm] + private static final double C = 2.99792458e-4; + + /** + * Create a track covariance matrix with reasonable uncertainties + */ + private RealMatrix createTrackCovariance(double d0Err, double phi0Err, double omegaErr, + double z0Err, double tanLErr) { + RealMatrix cov = MatrixUtils.createRealMatrix(5, 5); + cov.setEntry(0, 0, d0Err * d0Err); + cov.setEntry(1, 1, phi0Err * phi0Err); + cov.setEntry(2, 2, omegaErr * omegaErr); + cov.setEntry(3, 3, z0Err * z0Err); + cov.setEntry(4, 4, tanLErr * tanLErr); + return cov; + } + + /** + * Create TrackParams for a track with given momentum at a vertex position. + * This is a simplified model - in reality the track parameters depend on the + * full helix geometry. + */ + private TrackParams createTrackAtVertex(double px, double py, double pz, + double vx, double vy, double vz, + int charge, double bField) { + double pT = FastMath.sqrt(px * px + py * py); + double p = FastMath.sqrt(px * px + py * py + pz * pz); + + // omega = C * B / pT, with sign from charge + double omega = charge * C * bField / pT; + double R = 1.0 / FastMath.abs(omega); + double sign = FastMath.signum(omega); + + // phi0 is the momentum direction at perigee + // For a track at the vertex, phi at vertex is atan2(py, px) + // phi0 = phiV - dphi where dphi depends on path from perigee to vertex + // For simplicity, assume vertex is near origin so phi0 ≈ atan2(py, px) + double phiV = FastMath.atan2(py, px); + + // tanLambda = pz / pT + double tanLambda = pz / pT; + + // For a track passing through vertex (vx, vy, vz) with momentum direction phiV, + // we need to find the perigee parameters (d0, phi0, z0) + // + // The helix center is perpendicular to momentum direction: + // xc = vx + sign * R * sin(phiV) ... wait, this isn't quite right + // Let me use a simpler approximation for testing: + // Assume vertex is at origin, so d0 ≈ 0, z0 ≈ vz, phi0 ≈ phiV + + // More accurate: compute helix center from vertex position + // The momentum at vertex points in direction (cos(phiV), sin(phiV)) in our convention + // (where px = pT * cos(phiV), py = pT * sin(phiV)) + // + // For the helix center formula: xc = (sign*R - d0)*sin(phi0), yc = (d0 - sign*R)*cos(phi0) + // At the vertex: xV = xc + R * something... + // + // For simplicity in this test, let's place the vertex at the origin + // Then d0 = 0, z0 = 0, and phi0 = phiV + + double d0 = 0.0; + double phi0 = phiV; + double z0 = vz; + + // Add small random perturbations to simulate measurement + // (In a real test we might want deterministic values) + + RealMatrix cov = createTrackCovariance(0.1, 0.01, 1e-5, 0.1, 0.01); + + return new TrackParams(d0, phi0, omega, z0, tanLambda, cov); + } + + /** + * Test basic unconstrained vertex fit with three tracks + */ + public void testUnconstrainedFit() { + System.out.println("\n=== testUnconstrainedFit ===\n"); + + GainMatrixVertexer fitter = new GainMatrixVertexer(B_FIELD); + fitter.setDebugFlag(true); + + // Create three tracks that should meet at the origin + // Track 1: electron going forward-right-up + // Track 2: electron going forward-left-down + // Track 3: positron going forward (to balance charge for momentum) + + List tracks = new ArrayList<>(); + + // Electron 1: pT ~ 1 GeV, going in +x direction with small py, pz + double omega1 = -C * B_FIELD / 1.0; // negative for electron + tracks.add(new TrackParams(0.0, 0.1, omega1, 0.0, 0.05, + createTrackCovariance(0.1, 0.001, 1e-6, 0.1, 0.001))); + + // Electron 2: pT ~ 1.5 GeV + double omega2 = -C * B_FIELD / 1.5; + tracks.add(new TrackParams(0.0, -0.1, omega2, 0.0, -0.03, + createTrackCovariance(0.1, 0.001, 1e-6, 0.1, 0.001))); + + // Positron: pT ~ 1.2 GeV + double omega3 = C * B_FIELD / 1.2; // positive for positron + tracks.add(new TrackParams(0.0, 0.0, omega3, 0.0, 0.01, + createTrackCovariance(0.1, 0.001, 1e-6, 0.1, 0.001))); + + // Fit without constraints, using the extracted gain-matrix algorithm (fit() + // itself was moved out of TrackConstraintVertexFitter into GainMatrixVertexer). + FitResult result = fitter.fit(tracks); + + assertNotNull("Fit result should not be null", result); + System.out.println("Vertex: " + result.vertex); + System.out.println("Chi2: " + result.chi2 + ", NDF: " + result.ndf); + + // Vertex should be near origin since all tracks have d0=0, z0=0 + assertTrue("Vertex x should be near 0", FastMath.abs(result.vertex.getEntry(0)) < 1.0); + assertTrue("Vertex y should be near 0", FastMath.abs(result.vertex.getEntry(1)) < 1.0); + assertTrue("Vertex z should be near 0", FastMath.abs(result.vertex.getEntry(2)) < 1.0); + } + + /** + * Test 4-momentum constrained fit using Lagrange multiplier method via fitVertex() + */ + public void testThreeMomentumConstraint() { + System.out.println("\n=== testThreeMomentumConstraint ===\n"); + + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(B_FIELD); + fitter.setDebug(true); + + // Beam parameters with realistic HPS rotation + // The beam is rotated by ~30.5 mrad around the global Y-axis (vertical) + // In tracking frame: X = detector Z (beamline), Y = detector X (horizontal), Z = detector Y (vertical) + // So the rotation is around tracking Z, giving beam momentum: + // px_trk = pBeam * cos(rotAngle) + // py_trk = -pBeam * sin(rotAngle) + // pz_trk = 0 + double beamEnergy = 3.7; // GeV + double beamRotAngle = 0.0305; // 30.5 mrad - realistic HPS value + + // Compute beam momentum in tracking frame + double beamPx = beamEnergy * FastMath.cos(beamRotAngle); + double beamPy = -beamEnergy * FastMath.sin(beamRotAngle); + double beamPz = 0.0; + + System.out.printf("Beam rotation angle: %.4f rad (%.2f mrad)%n", beamRotAngle, beamRotAngle * 1000); + System.out.printf("Beam 3-momentum (tracking frame): [%.6f, %.6f, %.6f]%n", beamPx, beamPy, beamPz); + + // Design NON-COLLINEAR tracks whose 3-momentum sums to the beam momentum + // We only constrain 3-momentum now (not energy), so non-collinear tracks are fine + // + // Track 1 (electron): p1 going mostly forward with some transverse + // Track 2 (electron): p2 going forward-left + // Track 3 (positron): p3 going forward-right (to balance py) + // + // Design: sum(px) = beamPx, sum(py) = beamPy, sum(pz) = 0 + + double px1 = 1.5, py1 = 0.2, pz1 = 0.1; + double px2 = 1.0, py2 = -0.15, pz2 = -0.05; + // Track 3 chosen to make sum = beam momentum + double px3 = beamPx - px1 - px2; + double py3 = beamPy - py1 - py2; + double pz3 = -pz1 - pz2; + + double totalPxIn = px1 + px2 + px3; + double totalPyIn = py1 + py2 + py3; + double totalPzIn = pz1 + pz2 + pz3; + + System.out.printf("Track momenta (designed):%n"); + System.out.printf(" Track 1: [%.6f, %.6f, %.6f]%n", px1, py1, pz1); + System.out.printf(" Track 2: [%.6f, %.6f, %.6f]%n", px2, py2, pz2); + System.out.printf(" Track 3: [%.6f, %.6f, %.6f]%n", px3, py3, pz3); + System.out.printf(" Sum: [%.6f, %.6f, %.6f]%n", totalPxIn, totalPyIn, totalPzIn); + System.out.printf(" Beam: [%.6f, %.6f, %.6f]%n", beamPx, beamPy, beamPz); + System.out.printf(" Diff: [%.2e, %.2e, %.2e]%n", + totalPxIn - beamPx, totalPyIn - beamPy, totalPzIn - beamPz); + + // Set fitter beam parameters + fitter.setBeamEnergy(beamEnergy); + fitter.setBeamRotAngle(beamRotAngle); + fitter.setBeamPosition(new double[]{0.0, 0.0, 0.0}); // Origin + fitter.setBeamSize(new double[]{0.1, 0.1, 1.0}); // Reasonable beam spot + + List tracks = new ArrayList<>(); + + // Create track parameters from momenta + // For track at origin: d0=0, z0=0, phi0=atan2(py,px), omega=q*C*B/pT, tanL=pz/pT + double pT1 = FastMath.sqrt(px1*px1 + py1*py1); + double phi1 = FastMath.atan2(py1, px1); + double omega1 = -C * B_FIELD / pT1; // electron (negative charge) + double tanL1 = pz1 / pT1; + + double pT2 = FastMath.sqrt(px2*px2 + py2*py2); + double phi2 = FastMath.atan2(py2, px2); + double omega2 = -C * B_FIELD / pT2; // electron + double tanL2 = pz2 / pT2; + + double pT3 = FastMath.sqrt(px3*px3 + py3*py3); + double phi3 = FastMath.atan2(py3, px3); + double omega3 = C * B_FIELD / pT3; // positron (positive charge) + double tanL3 = pz3 / pT3; + + // All tracks at origin (d0=0, z0=0) + // Use reasonable covariance matrix + RealMatrix cov = createTrackCovariance(0.1, 0.01, 1e-5, 0.1, 0.01); + + tracks.add(new TrackParams(0.0, phi1, omega1, 0.0, tanL1, cov)); + tracks.add(new TrackParams(0.0, phi2, omega2, 0.0, tanL2, cov)); + tracks.add(new TrackParams(0.0, phi3, omega3, 0.0, tanL3, cov)); + + // Print input track parameters + System.out.println("\nInput track parameters:"); + for (int i = 0; i < tracks.size(); i++) { + TrackParams t = tracks.get(i); + double pT = C * B_FIELD / FastMath.abs(t.omega); + double px = pT * FastMath.cos(t.phi0); + double py = pT * FastMath.sin(t.phi0); + double pz = pT * t.tanLambda; + System.out.printf(" Track %d: pT=%.4f, phi0=%.4f, omega=%.6f, tanL=%.4f -> p=[%.4f, %.4f, %.4f]%n", + i, pT, t.phi0, t.omega, t.tanLambda, px, py, pz); + } + + // Fit with 3-momentum constraint using fitVertex + BilliorVertex vtx = fitter.fitVertex(tracks, true); + + assertNotNull("Vertex should not be null", vtx); + + System.out.println("\nFit results:"); + System.out.printf(" Vertex (det frame): [%.4f, %.4f, %.4f]%n", + vtx.getPosition().x(), vtx.getPosition().y(), vtx.getPosition().z()); + System.out.printf(" Chi2: %.4f, NDF: %d%n", vtx.getChi2(), 6); // 3 track z-constraints + 3 momentum constraints + + // Get fitted momenta from the vertex + // Note: fitVertex returns momenta in DETECTOR frame + // Detector frame: X = tracking Y, Y = tracking Z, Z = tracking X + // So to convert back to tracking frame: trk_x = det_z, trk_y = det_x, trk_z = det_y + double totalPx_det = 0, totalPy_det = 0, totalPz_det = 0; + for (int i = 0; i < 3; i++) { + double pxi = vtx.getFittedMomentum(i).x(); + double pyi = vtx.getFittedMomentum(i).y(); + double pzi = vtx.getFittedMomentum(i).z(); + double pMag = FastMath.sqrt(pxi*pxi + pyi*pyi + pzi*pzi); + totalPx_det += pxi; + totalPy_det += pyi; + totalPz_det += pzi; + System.out.printf(" Track %d fitted (det frame): p=[%.4f, %.4f, %.4f], |p|=%.4f%n", + i, pxi, pyi, pzi, pMag); + } + + // Convert total momentum to tracking frame for comparison with beam constraint + // tracking X = detector Z, tracking Y = detector X, tracking Z = detector Y + double totalPx_trk = totalPz_det; + double totalPy_trk = totalPx_det; + double totalPz_trk = totalPy_det; + + System.out.printf(" Total fitted 3-mom (det frame): [%.6f, %.6f, %.6f]%n", + totalPx_det, totalPy_det, totalPz_det); + System.out.printf(" Total fitted 3-mom (trk frame): [%.6f, %.6f, %.6f]%n", + totalPx_trk, totalPy_trk, totalPz_trk); + System.out.printf(" Beam 3-momentum (trk frame): [%.6f, %.6f, %.6f]%n", beamPx, beamPy, beamPz); + + // Check that 3-momentum constraint is satisfied (in tracking frame) + double dpx = totalPx_trk - beamPx; + double dpy = totalPy_trk - beamPy; + double dpz = totalPz_trk - beamPz; + + System.out.printf(" 3-momentum residuals (trk frame): [%.2e, %.2e, %.2e]%n", dpx, dpy, dpz); + + // For this test, the input tracks exactly satisfy the constraint + // So the fit should converge with chi2 ~ 0 and residuals ~ 0 + System.out.printf(" Chi2/NDF: %.4f%n", vtx.getChi2() / 6.0); + + // The constraint should be satisfied to very tight tolerance since input exactly satisfies it + assertTrue("px constraint should be satisfied", FastMath.abs(dpx) < 0.001); + assertTrue("py constraint should be satisfied", FastMath.abs(dpy) < 0.001); + assertTrue("pz constraint should be satisfied", FastMath.abs(dpz) < 0.001); + + // Chi2 should be very small since input exactly satisfies all constraints + assertTrue("Chi2 should be small for exact input", vtx.getChi2() < 1.0); + } + + /** + * Test that constraint Jacobians are computed correctly by numerical differentiation + */ + public void testConstraintJacobians() { + System.out.println("\n=== testConstraintJacobians ===\n"); + + // This test verifies the analytical Jacobian matches numerical derivatives + // We'll compute d(momentum)/d(track_params) numerically and compare + + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(B_FIELD); + + // Create a single track + double d0 = 0.5; + double phi0 = 0.2; + double omega = -C * B_FIELD / 1.5; // electron with pT = 1.5 GeV + double z0 = 0.1; + double tanL = 0.05; + + RealMatrix cov = createTrackCovariance(0.1, 0.01, 1e-5, 0.1, 0.01); + TrackParams track = new TrackParams(d0, phi0, omega, z0, tanL, cov); + + // Vertex position + double xV = 0.0, yV = 0.0, zV = 0.0; + RealVector vertex = MatrixUtils.createRealVector(new double[]{xV, yV, zV}); + + // Compute momentum at vertex + // We need to access the private method, so we'll use reflection or + // compute it ourselves using the same formula + + double R = 1.0 / FastMath.abs(omega); + double sign = FastMath.signum(omega); + + // Helix center + double xc = sign * R * FastMath.sin(phi0) - d0 * FastMath.sin(phi0); + double yc = -sign * R * FastMath.cos(phi0) + d0 * FastMath.cos(phi0); + + double dx = xV - xc; + double dy = yV - yc; + + double phiV = FastMath.atan2(-dx * sign, dy * sign); + + double pT = C * B_FIELD / FastMath.abs(omega); + double px = pT * FastMath.cos(phiV); + double py = pT * FastMath.sin(phiV); + double pz = pT * tanL; + + System.out.printf("Track params: d0=%.4f, phi0=%.4f, omega=%.6f, z0=%.4f, tanL=%.4f%n", + d0, phi0, omega, z0, tanL); + System.out.printf("Helix center: xc=%.4f, yc=%.4f%n", xc, yc); + System.out.printf("phiV=%.4f, pT=%.4f%n", phiV, pT); + System.out.printf("Momentum at vertex: [%.4f, %.4f, %.4f]%n", px, py, pz); + + // Numerical derivatives + double eps = 1e-6; + + // d(px)/d(omega) numerically + double omega_plus = omega + eps; + double R_plus = 1.0 / FastMath.abs(omega_plus); + double sign_plus = FastMath.signum(omega_plus); + double xc_plus = sign_plus * R_plus * FastMath.sin(phi0) - d0 * FastMath.sin(phi0); + double yc_plus = -sign_plus * R_plus * FastMath.cos(phi0) + d0 * FastMath.cos(phi0); + double dx_plus = xV - xc_plus; + double dy_plus = yV - yc_plus; + double phiV_plus = FastMath.atan2(-dx_plus * sign_plus, dy_plus * sign_plus); + double pT_plus = C * B_FIELD / FastMath.abs(omega_plus); + double px_plus = pT_plus * FastMath.cos(phiV_plus); + + double dpx_domega_numerical = (px_plus - px) / eps; + + System.out.printf("d(px)/d(omega) numerical: %.4f%n", dpx_domega_numerical); + + // The test passes if we can compute these - detailed validation would require + // exposing the Jacobian computation methods or computing them analytically + assertTrue("Numerical derivative should be finite", Double.isFinite(dpx_domega_numerical)); + } + + /** + * Build TrackParams for a helix that passes exactly through (xV,yV,zV) with the given + * momentum, by inverting the same center/perigee formulas used internally by + * computeTrackConstraint/perigeeToVertexParams (xc = (sign*R-d0)*sin(phi0), + * yc = -(sign*R-d0)*cos(phi0), zV = z0 - sign*R*dphi*tanLambda). + */ + private TrackParams createExactTrackThroughPoint(double xV, double yV, double zV, + double px, double py, double pz, + int charge, double bField, RealMatrix cov) { + double pT = FastMath.sqrt(px * px + py * py); + double omega = charge * C * bField / pT; + double R = 1.0 / FastMath.abs(omega); + double sign = FastMath.signum(omega); + double phiV = FastMath.atan2(py, px); + double tanLambda = pz / pT; + + double xc = xV + R * sign * FastMath.sin(phiV); + double yc = yV - R * sign * FastMath.cos(phiV); + + // (xc,yc) and (d0,phi0) are related by (xc,yc) = ((sign*R-d0)*sin(phi0), -(sign*R-d0)*cos(phi0)), + // which has a two-fold ambiguity: (phi0, sign*R-A) and (phi0+pi, sign*R+A) both reproduce the + // same (xc,yc). Only the sign*(R-A) branch gives the physically-correct small |d0| (distance of + // closest approach); the other branch spuriously gives |d0| ~ R+A and pushes phi0 off by pi. + double A = FastMath.sqrt(xc * xc + yc * yc); + double phi0 = FastMath.atan2(sign * xc, -sign * yc); + double d0 = sign * (R - A); + + double dphi = phiV - phi0; + while (dphi > FastMath.PI) dphi -= 2.0 * FastMath.PI; + while (dphi < -FastMath.PI) dphi += 2.0 * FastMath.PI; + double s = -sign * R * dphi; + double z0 = zV - s * tanLambda; + + return new TrackParams(d0, phi0, omega, z0, tanLambda, cov); + } + + /** + * Exact-geometry sanity check for the NEW joint two-vertex fit (fitCascadeVertexJoint): + * construct eMinus/ePlus tracks passing exactly through a chosen V1, sum their momenta + * there to get the V0 flight direction, place V2 exactly along that direction from V1, + * and construct a recoil track passing exactly through V2. Verify the fit recovers V1, + * V2, and all three momenta to tight tolerance with near-zero chi2 -- isolating whether + * a reported bug (bad V1 position / inflated V1 mass on real data) is in the core fit + * math here vs. elsewhere (CascadeVertexer's packaging, or real-data specifics). + */ + public void testJointTwoVertexExactGeometry() { + System.out.println("\n=== testJointTwoVertexExactGeometry ===\n"); + + double xV1 = 0.2, yV1 = -0.1, zV1 = 3.0; + double pxEm = 0.30, pyEm = 0.10, pzEm = 1.5; + double pxEp = 0.15, pyEp = -0.05, pzEp = 0.8; + + double pxV0 = pxEm + pxEp, pyV0 = pyEm + pyEp, pzV0 = pzEm + pzEp; + double pV0Mag = FastMath.sqrt(pxV0 * pxV0 + pyV0 * pyV0 + pzV0 * pzV0); + double nx = pxV0 / pV0Mag, ny = pyV0 / pV0Mag, nz = pzV0 / pV0Mag; + + double flightLength = 40.0; // mm + double xV2 = xV1 + flightLength * nx; + double yV2 = yV1 + flightLength * ny; + double zV2 = zV1 + flightLength * nz; + + double pxRc = 0.20, pyRc = -0.10, pzRc = 3.0; + + RealMatrix trackCov = createTrackCovariance(0.03, 0.003, 5e-6, 0.03, 0.003); + TrackParams eMinusTrack = createExactTrackThroughPoint(xV1, yV1, zV1, pxEm, pyEm, pzEm, -1, B_FIELD, trackCov); + TrackParams ePlusTrack = createExactTrackThroughPoint(xV1, yV1, zV1, pxEp, pyEp, pzEp, 1, B_FIELD, trackCov); + TrackParams recoilTrack = createExactTrackThroughPoint(xV2, yV2, zV2, pxRc, pyRc, pzRc, -1, B_FIELD, trackCov); + + RealVector v1Init = MatrixUtils.createRealVector(new double[]{xV1, yV1, zV1}); + // Deliberately poor initial guess for V2 (mimicking CascadeVertexer's real usage, + // where v2Init defaults to the beamspot near the origin rather than the truth). + RealVector v2Init = MatrixUtils.createRealVector(new double[]{0.0, 0.0, 0.0}); + + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(B_FIELD); + TwoVertexFitResult result = fitter.fitCascadeVertexJoint(eMinusTrack, ePlusTrack, recoilTrack, v1Init, v2Init, 500, 1.0e-10); + + assertNotNull("Joint two-vertex fit result should not be null", result); + + System.out.printf("Fitted V1: [%.6f, %.6f, %.6f] (truth: [%.6f, %.6f, %.6f])%n", + result.v1.getEntry(0), result.v1.getEntry(1), result.v1.getEntry(2), xV1, yV1, zV1); + System.out.printf("Fitted V2: [%.6f, %.6f, %.6f] (truth: [%.6f, %.6f, %.6f])%n", + result.v2.getEntry(0), result.v2.getEntry(1), result.v2.getEntry(2), xV2, yV2, zV2); + System.out.printf("Fitted eMinus p: [%.6f, %.6f, %.6f] (truth: [%.6f, %.6f, %.6f])%n", + result.eMinusMomentum.p.getEntry(0), result.eMinusMomentum.p.getEntry(1), result.eMinusMomentum.p.getEntry(2), + pxEm, pyEm, pzEm); + System.out.printf("Fitted ePlus p: [%.6f, %.6f, %.6f] (truth: [%.6f, %.6f, %.6f])%n", + result.ePlusMomentum.p.getEntry(0), result.ePlusMomentum.p.getEntry(1), result.ePlusMomentum.p.getEntry(2), + pxEp, pyEp, pzEp); + System.out.printf("Fitted recoil p: [%.6f, %.6f, %.6f] (truth: [%.6f, %.6f, %.6f])%n", + result.recoilMomentum.p.getEntry(0), result.recoilMomentum.p.getEntry(1), result.recoilMomentum.p.getEntry(2), + pxRc, pyRc, pzRc); + System.out.printf("Chi2/NDF: %.6f / %d%n", result.chi2, result.ndf); + + assertEquals(xV1, result.v1.getEntry(0), 1e-3); + assertEquals(yV1, result.v1.getEntry(1), 1e-3); + assertEquals(zV1, result.v1.getEntry(2), 1e-3); + assertEquals(xV2, result.v2.getEntry(0), 1e-3); + assertEquals(yV2, result.v2.getEntry(1), 1e-3); + assertEquals(zV2, result.v2.getEntry(2), 1e-3); + assertEquals(pxEm, result.eMinusMomentum.p.getEntry(0), 1e-3); + assertEquals(pyEm, result.eMinusMomentum.p.getEntry(1), 1e-3); + assertEquals(pzEm, result.eMinusMomentum.p.getEntry(2), 1e-3); + assertEquals(pxEp, result.ePlusMomentum.p.getEntry(0), 1e-3); + assertEquals(pyEp, result.ePlusMomentum.p.getEntry(1), 1e-3); + assertEquals(pzEp, result.ePlusMomentum.p.getEntry(2), 1e-3); + assertTrue("chi2 should be small for exactly-consistent geometry, got " + result.chi2, result.chi2 < 1e-2); + } + + /** + * As {@link #testJointTwoVertexExactGeometry}, but for {@code fitCascadeVertexJointFixedV2X}: + * V2's tracking-index-0 coordinate is passed in as {@code v2FixedX} equal to the true xV2 + * (isolating the reduced-state fit mechanics from any target-plane-vs-truth systematic, + * which is instead checked separately against real data), and must come back reported as + * exactly that value with exactly-zero variance -- V1 and V2's other two coordinates should + * still recover truth as tightly as the unconstrained fit. + */ + public void testJointTwoVertexFixedV2XExactGeometry() { + System.out.println("\n=== testJointTwoVertexFixedV2XExactGeometry ===\n"); + + double xV1 = 0.2, yV1 = -0.1, zV1 = 3.0; + double pxEm = 0.30, pyEm = 0.10, pzEm = 1.5; + double pxEp = 0.15, pyEp = -0.05, pzEp = 0.8; + + double pxV0 = pxEm + pxEp, pyV0 = pyEm + pyEp, pzV0 = pzEm + pzEp; + double pV0Mag = FastMath.sqrt(pxV0 * pxV0 + pyV0 * pyV0 + pzV0 * pzV0); + double nx = pxV0 / pV0Mag, ny = pyV0 / pV0Mag, nz = pzV0 / pV0Mag; + + double flightLength = 40.0; // mm + double xV2 = xV1 + flightLength * nx; + double yV2 = yV1 + flightLength * ny; + double zV2 = zV1 + flightLength * nz; + + double pxRc = 0.20, pyRc = -0.10, pzRc = 3.0; + + RealMatrix trackCov = createTrackCovariance(0.03, 0.003, 5e-6, 0.03, 0.003); + TrackParams eMinusTrack = createExactTrackThroughPoint(xV1, yV1, zV1, pxEm, pyEm, pzEm, -1, B_FIELD, trackCov); + TrackParams ePlusTrack = createExactTrackThroughPoint(xV1, yV1, zV1, pxEp, pyEp, pzEp, 1, B_FIELD, trackCov); + TrackParams recoilTrack = createExactTrackThroughPoint(xV2, yV2, zV2, pxRc, pyRc, pzRc, -1, B_FIELD, trackCov); + + RealVector v1Init = MatrixUtils.createRealVector(new double[]{xV1, yV1, zV1}); + // Deliberately poor initial guess for V2 (mimicking CascadeVertexer's real usage). + RealVector v2Init = MatrixUtils.createRealVector(new double[]{0.0, 0.0, 0.0}); + + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(B_FIELD); + TwoVertexFitResult result = fitter.fitCascadeVertexJointFixedV2X( + eMinusTrack, ePlusTrack, recoilTrack, v1Init, v2Init, xV2, null, null, 500, 1.0e-10); + + assertNotNull("Fixed-V2X joint two-vertex fit result should not be null", result); + + System.out.printf("Fitted V1: [%.6f, %.6f, %.6f] (truth: [%.6f, %.6f, %.6f])%n", + result.v1.getEntry(0), result.v1.getEntry(1), result.v1.getEntry(2), xV1, yV1, zV1); + System.out.printf("Fitted V2: [%.6f, %.6f, %.6f] (truth: [%.6f, %.6f, %.6f])%n", + result.v2.getEntry(0), result.v2.getEntry(1), result.v2.getEntry(2), xV2, yV2, zV2); + System.out.printf("Chi2/NDF: %.6f / %d%n", result.chi2, result.ndf); + + assertEquals(xV1, result.v1.getEntry(0), 1e-3); + assertEquals(yV1, result.v1.getEntry(1), 1e-3); + assertEquals(zV1, result.v1.getEntry(2), 1e-3); + assertEquals("fixed V2 coordinate should be reported as exactly the input value", + xV2, result.v2.getEntry(0), 1e-12); + assertEquals(yV2, result.v2.getEntry(1), 1e-3); + assertEquals(zV2, result.v2.getEntry(2), 1e-3); + assertEquals("fixed V2 coordinate's variance should be exactly zero", + 0.0, result.v2Cov.getEntry(0, 0), 1e-12); + assertEquals(0.0, result.v2Cov.getEntry(0, 1), 1e-12); + assertEquals(0.0, result.v2Cov.getEntry(0, 2), 1e-12); + assertEquals(0.0, result.v2Cov.getEntry(1, 0), 1e-12); + assertEquals(0.0, result.v2Cov.getEntry(2, 0), 1e-12); + assertEquals(3, result.ndf); + assertTrue("chi2 should be small for exactly-consistent geometry, got " + result.chi2, result.chi2 < 1e-2); + } + + /** + * Pull-distribution check for {@code fitCascadeVertexJointFixedV2X}, mirroring + * {@link #testJointTwoVertexSmearedPulls}: V1's full 3 coordinates and V2's 2 free + * (transverse) coordinates should have pull mean ~0, std ~1 across many smeared toys. + * V2's fixed coordinate has no pull to check -- it isn't a fitted quantity. + */ + public void testJointTwoVertexFixedV2XSmearedPulls() { + for (double flightLength : new double[]{5.0, 20.0, 50.0, 90.0, 120.0, 150.0}) { + runJointTwoVertexFixedV2XSmearedPulls(flightLength, 500); + } + } + + private void runJointTwoVertexFixedV2XSmearedPulls(double flightLength, int nToys) { + System.out.println("\n=== testJointTwoVertexFixedV2XSmearedPulls (flightLength=" + flightLength + " mm) ===\n"); + + double xV1 = 0.2, yV1 = -0.1, zV1 = 3.0; + double pxEm = 0.30, pyEm = 0.10, pzEm = 1.5; + double pxEp = 0.15, pyEp = -0.05, pzEp = 0.8; + + double pxV0 = pxEm + pxEp, pyV0 = pyEm + pyEp, pzV0 = pzEm + pzEp; + double pV0Mag = FastMath.sqrt(pxV0 * pxV0 + pyV0 * pyV0 + pzV0 * pzV0); + double nx = pxV0 / pV0Mag, ny = pyV0 / pV0Mag, nz = pzV0 / pV0Mag; + + double xV2 = xV1 + flightLength * nx; + double yV2 = yV1 + flightLength * ny; + double zV2 = zV1 + flightLength * nz; + + double pxRc = 0.20, pyRc = -0.10, pzRc = 3.0; + + double d0Err = 0.03, phi0Err = 0.003, omegaErr = 5e-6, z0Err = 0.03, tanLErr = 0.003; + RealMatrix trackCov = createTrackCovariance(d0Err, phi0Err, omegaErr, z0Err, tanLErr); + double[] trackSigma = {d0Err, phi0Err, omegaErr, z0Err, tanLErr}; + + RealVector v1Init = MatrixUtils.createRealVector(new double[]{xV1, yV1, zV1}); + RealVector v2Init = MatrixUtils.createRealVector(new double[]{0.0, 0.0, -1.1}); + + java.util.Random rng = new java.util.Random(6789); + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(B_FIELD); + + int nNull = 0; + int nOk = 0; + double[] pullV1X = new double[nToys], pullV1Y = new double[nToys], pullV1Z = new double[nToys]; + double[] pullV2Y = new double[nToys], pullV2Z = new double[nToys]; + + for (int toy = 0; toy < nToys; toy++) { + TrackParams eMinusTruth = createExactTrackThroughPoint(xV1, yV1, zV1, pxEm, pyEm, pzEm, -1, B_FIELD, trackCov); + TrackParams ePlusTruth = createExactTrackThroughPoint(xV1, yV1, zV1, pxEp, pyEp, pzEp, 1, B_FIELD, trackCov); + TrackParams recoilTruth = createExactTrackThroughPoint(xV2, yV2, zV2, pxRc, pyRc, pzRc, -1, B_FIELD, trackCov); + + TrackParams eMinusTrack = smearTrack(eMinusTruth, trackSigma, trackCov, rng); + TrackParams ePlusTrack = smearTrack(ePlusTruth, trackSigma, trackCov, rng); + TrackParams recoilTrack = smearTrack(recoilTruth, trackSigma, trackCov, rng); + + TwoVertexFitResult result = fitter.fitCascadeVertexJointFixedV2X( + eMinusTrack, ePlusTrack, recoilTrack, v1Init, v2Init, xV2, null, null, 60, 1.0e-8); + if (result == null) { + nNull++; + continue; + } + + pullV1X[nOk] = (result.v1.getEntry(0) - xV1) / FastMath.sqrt(result.v1Cov.getEntry(0, 0)); + pullV1Y[nOk] = (result.v1.getEntry(1) - yV1) / FastMath.sqrt(result.v1Cov.getEntry(1, 1)); + pullV1Z[nOk] = (result.v1.getEntry(2) - zV1) / FastMath.sqrt(result.v1Cov.getEntry(2, 2)); + pullV2Y[nOk] = (result.v2.getEntry(1) - yV2) / FastMath.sqrt(result.v2Cov.getEntry(1, 1)); + pullV2Z[nOk] = (result.v2.getEntry(2) - zV2) / FastMath.sqrt(result.v2Cov.getEntry(2, 2)); + assertEquals("fixed V2 coordinate should never move from the input value", + xV2, result.v2.getEntry(0), 1e-12); + nOk++; + } + + double[] pullXTrim = java.util.Arrays.copyOf(pullV1X, nOk); + double[] pullYTrim = java.util.Arrays.copyOf(pullV1Y, nOk); + double[] pullZTrim = java.util.Arrays.copyOf(pullV1Z, nOk); + double[] pullV2YTrim = java.util.Arrays.copyOf(pullV2Y, nOk); + double[] pullV2ZTrim = java.util.Arrays.copyOf(pullV2Z, nOk); + + System.out.printf("nToys=%d nNull=%d nOk=%d%n", nToys, nNull, nOk); + System.out.printf("V1 pull X: mean=%.3f std=%.3f%n", mean(pullXTrim), std(pullXTrim, mean(pullXTrim))); + System.out.printf("V1 pull Y: mean=%.3f std=%.3f%n", mean(pullYTrim), std(pullYTrim, mean(pullYTrim))); + System.out.printf("V1 pull Z: mean=%.3f std=%.3f%n", mean(pullZTrim), std(pullZTrim, mean(pullZTrim))); + System.out.printf("V2 pull Y: mean=%.3f std=%.3f%n", mean(pullV2YTrim), std(pullV2YTrim, mean(pullV2YTrim))); + System.out.printf("V2 pull Z: mean=%.3f std=%.3f%n", mean(pullV2ZTrim), std(pullV2ZTrim, mean(pullV2ZTrim))); + } + + /** + * Toy-MC pull study for {@link TrackConstraintVertexFitter#fitCascadeVertexJointBeamConstrained}, + * mirroring {@link #testJointTwoVertexSmearedPulls} (same V1/track geometry construction) + * but with the recoil momentum chosen so the three daughters' total 3-momentum exactly + * equals a beam value (same beam construction as + * {@link #testNTrackBeamMomentumConstraintSmearedPulls}), and with + * {@code sigmaTNuclearRecoil=0} so the toy isolates the fitter math with no extra recoil + * smearing (matching how the N-track beam-constraint toy test isolates its own fitter + * math). Checks V1(x,y,z), V2(x,y,z), and per-track (eMinus/ePlus/recoil) momentum pulls + * all have mean~0/std~1 and mean chi2/ndf~1 (ndf=5: 12 constraints - 7 free V1/theta/V2 + * parameters). Also runs the existing unconstrained {@link + * TrackConstraintVertexFitter#fitCascadeVertexJoint} on the same smeared toys, printing + * (not asserting) its chi2/ndf alongside the beam-constrained result for comparison. + */ + public void testJointTwoVertexBeamConstrainedSmearedPulls() { + for (double flightLength : new double[]{5.0, 20.0, 50.0, 90.0, 120.0, 150.0}) { + runJointTwoVertexBeamConstrainedSmearedPulls(flightLength, 500); + } + } + + private void runJointTwoVertexBeamConstrainedSmearedPulls(double flightLength, int nToys) { + System.out.println("\n=== testJointTwoVertexBeamConstrainedSmearedPulls (flightLength=" + flightLength + " mm) ===\n"); + + double pBeamMag = 3.74; + double rotAngle = -0.0305; + double beamPx = pBeamMag * FastMath.cos(rotAngle); + double beamPy = -pBeamMag * FastMath.sin(rotAngle); + double beamPz = 0.0; + + double xV1 = 0.2, yV1 = -0.1, zV1 = 3.0; + double pxEm = 0.30, pyEm = 0.10, pzEm = 1.5; + double pxEp = 0.15, pyEp = -0.05, pzEp = 0.8; + + double pxV0 = pxEm + pxEp, pyV0 = pyEm + pyEp, pzV0 = pzEm + pzEp; + double pV0Mag = FastMath.sqrt(pxV0 * pxV0 + pyV0 * pyV0 + pzV0 * pzV0); + double nx = pxV0 / pV0Mag, ny = pyV0 / pV0Mag, nz = pzV0 / pV0Mag; + + double xV2 = xV1 + flightLength * nx; + double yV2 = yV1 + flightLength * ny; + double zV2 = zV1 + flightLength * nz; + + // Recoil momentum fixed by exact beam-momentum conservation, so the three daughters' + // total momentum equals the beam value by construction (same idea as the N-track + // beam-constraint toy's px3/py3/pz3). + double pxRc = beamPx - pxV0, pyRc = beamPy - pyV0, pzRc = beamPz - pzV0; + + double d0Err = 0.03, phi0Err = 0.003, omegaErr = 5e-6, z0Err = 0.03, tanLErr = 0.003; + RealMatrix trackCov = createTrackCovariance(d0Err, phi0Err, omegaErr, z0Err, tanLErr); + double[] trackSigma = {d0Err, phi0Err, omegaErr, z0Err, tanLErr}; + + RealVector v1Init = MatrixUtils.createRealVector(new double[]{xV1, yV1, zV1}); + RealVector v2Init = MatrixUtils.createRealVector(new double[]{0.0, 0.0, -1.1}); + + java.util.Random rng = new java.util.Random(24601); + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(B_FIELD); + fitter.setBeamEnergy(pBeamMag); + fitter.setBeamRotAngle(rotAngle); + fitter.setBeamMomentumTransverseNuclearRecoilSigma(0.0); + + int nNull = 0, nOk = 0; + double[] pullV1X = new double[nToys], pullV1Y = new double[nToys], pullV1Z = new double[nToys]; + double[] pullV2X = new double[nToys], pullV2Y = new double[nToys], pullV2Z = new double[nToys]; + double[] chi2NdfArr = new double[nToys]; + double[] chi2NdfUncArr = new double[nToys]; + int nOkUnc = 0; + double[][] trackPullPx = new double[3][nToys]; + double[][] trackPullPy = new double[3][nToys]; + double[][] trackPullPz = new double[3][nToys]; + double[] truthPx = {pxEm, pxEp, pxRc}; + double[] truthPy = {pyEm, pyEp, pyRc}; + double[] truthPz = {pzEm, pzEp, pzRc}; + + for (int toy = 0; toy < nToys; toy++) { + TrackParams eMinusTruth = createExactTrackThroughPoint(xV1, yV1, zV1, pxEm, pyEm, pzEm, -1, B_FIELD, trackCov); + TrackParams ePlusTruth = createExactTrackThroughPoint(xV1, yV1, zV1, pxEp, pyEp, pzEp, 1, B_FIELD, trackCov); + TrackParams recoilTruth = createExactTrackThroughPoint(xV2, yV2, zV2, pxRc, pyRc, pzRc, -1, B_FIELD, trackCov); + + TrackParams eMinusTrack = smearTrack(eMinusTruth, trackSigma, trackCov, rng); + TrackParams ePlusTrack = smearTrack(ePlusTruth, trackSigma, trackCov, rng); + TrackParams recoilTrack = smearTrack(recoilTruth, trackSigma, trackCov, rng); + + TwoVertexFitResult resultUnc = fitter.fitCascadeVertexJoint(eMinusTrack, ePlusTrack, recoilTrack, v1Init, v2Init); + if (resultUnc != null) { + chi2NdfUncArr[nOkUnc] = resultUnc.ndf > 0 ? resultUnc.chi2 / resultUnc.ndf : 0.0; + nOkUnc++; + } + + TwoVertexFitResult result = fitter.fitCascadeVertexJointBeamConstrained( + eMinusTrack, ePlusTrack, recoilTrack, v1Init, v2Init); + if (result == null) { + nNull++; + continue; + } + + pullV1X[nOk] = (result.v1.getEntry(0) - xV1) / FastMath.sqrt(result.v1Cov.getEntry(0, 0)); + pullV1Y[nOk] = (result.v1.getEntry(1) - yV1) / FastMath.sqrt(result.v1Cov.getEntry(1, 1)); + pullV1Z[nOk] = (result.v1.getEntry(2) - zV1) / FastMath.sqrt(result.v1Cov.getEntry(2, 2)); + pullV2X[nOk] = (result.v2.getEntry(0) - xV2) / FastMath.sqrt(result.v2Cov.getEntry(0, 0)); + pullV2Y[nOk] = (result.v2.getEntry(1) - yV2) / FastMath.sqrt(result.v2Cov.getEntry(1, 1)); + pullV2Z[nOk] = (result.v2.getEntry(2) - zV2) / FastMath.sqrt(result.v2Cov.getEntry(2, 2)); + chi2NdfArr[nOk] = result.ndf > 0 ? result.chi2 / result.ndf : 0.0; + + for (int i = 0; i < 3; i++) { + TrackMomentum tm = i == 0 ? result.eMinusMomentum : (i == 1 ? result.ePlusMomentum : result.recoilMomentum); + trackPullPx[i][nOk] = (tm.p.getEntry(0) - truthPx[i]) / FastMath.sqrt(tm.pCov.getEntry(0, 0)); + trackPullPy[i][nOk] = (tm.p.getEntry(1) - truthPy[i]) / FastMath.sqrt(tm.pCov.getEntry(1, 1)); + trackPullPz[i][nOk] = (tm.p.getEntry(2) - truthPz[i]) / FastMath.sqrt(tm.pCov.getEntry(2, 2)); + } + nOk++; + } + + double[] pullV1XTrim = java.util.Arrays.copyOf(pullV1X, nOk); + double[] pullV1YTrim = java.util.Arrays.copyOf(pullV1Y, nOk); + double[] pullV1ZTrim = java.util.Arrays.copyOf(pullV1Z, nOk); + double[] pullV2XTrim = java.util.Arrays.copyOf(pullV2X, nOk); + double[] pullV2YTrim = java.util.Arrays.copyOf(pullV2Y, nOk); + double[] pullV2ZTrim = java.util.Arrays.copyOf(pullV2Z, nOk); + double[] chi2NdfTrim = java.util.Arrays.copyOf(chi2NdfArr, nOk); + double[] chi2NdfUncTrim = java.util.Arrays.copyOf(chi2NdfUncArr, nOkUnc); + + double meanV1X = mean(pullV1XTrim), stdV1X = std(pullV1XTrim, meanV1X); + double meanV1Y = mean(pullV1YTrim), stdV1Y = std(pullV1YTrim, meanV1Y); + double meanV1Z = mean(pullV1ZTrim), stdV1Z = std(pullV1ZTrim, meanV1Z); + double meanV2X = mean(pullV2XTrim), stdV2X = std(pullV2XTrim, meanV2X); + double meanV2Y = mean(pullV2YTrim), stdV2Y = std(pullV2YTrim, meanV2Y); + double meanV2Z = mean(pullV2ZTrim), stdV2Z = std(pullV2ZTrim, meanV2Z); + double meanChi2Ndf = mean(chi2NdfTrim); + double meanChi2NdfUnc = mean(chi2NdfUncTrim); + + System.out.printf("nToys=%d nNull=%d nOk=%d%n", nToys, nNull, nOk); + System.out.printf("V1 pull X: mean=%.3f std=%.3f%n", meanV1X, stdV1X); + System.out.printf("V1 pull Y: mean=%.3f std=%.3f%n", meanV1Y, stdV1Y); + System.out.printf("V1 pull Z: mean=%.3f std=%.3f%n", meanV1Z, stdV1Z); + System.out.printf("V2 pull X: mean=%.3f std=%.3f%n", meanV2X, stdV2X); + System.out.printf("V2 pull Y: mean=%.3f std=%.3f%n", meanV2Y, stdV2Y); + System.out.printf("V2 pull Z: mean=%.3f std=%.3f%n", meanV2Z, stdV2Z); + System.out.printf("[BEAM-CONSTRAINED] mean chi2/ndf: %.3f%n", meanChi2Ndf); + System.out.printf("[UNCONSTRAINED, same toys] mean chi2/ndf: %.3f (for comparison only, not asserted)%n", meanChi2NdfUnc); + + for (int i = 0; i < 3; i++) { + double[] tPx = java.util.Arrays.copyOf(trackPullPx[i], nOk); + double[] tPy = java.util.Arrays.copyOf(trackPullPy[i], nOk); + double[] tPz = java.util.Arrays.copyOf(trackPullPz[i], nOk); + double mPx = mean(tPx), sPx = std(tPx, mPx); + double mPy = mean(tPy), sPy = std(tPy, mPy); + double mPz = mean(tPz), sPz = std(tPz, mPz); + String trackName = i == 0 ? "eMinus" : (i == 1 ? "ePlus" : "recoil"); + System.out.printf("track %s momentum pull: Px mean=%.3f std=%.3f | Py mean=%.3f std=%.3f | Pz mean=%.3f std=%.3f%n", + trackName, mPx, sPx, mPy, sPy, mPz, sPz); + assertTrue(trackName + " Px pull mean should be near 0, got " + mPx, FastMath.abs(mPx) < 0.15); + assertTrue(trackName + " Py pull mean should be near 0, got " + mPy, FastMath.abs(mPy) < 0.15); + assertTrue(trackName + " Pz pull mean should be near 0, got " + mPz, FastMath.abs(mPz) < 0.15); + assertTrue(trackName + " Px pull std should be near 1, got " + sPx, sPx > 0.8 && sPx < 1.2); + assertTrue(trackName + " Py pull std should be near 1, got " + sPy, sPy > 0.8 && sPy < 1.2); + assertTrue(trackName + " Pz pull std should be near 1, got " + sPz, sPz > 0.8 && sPz < 1.2); + } + + assertTrue("V1 pull X mean should be near 0, got " + meanV1X, FastMath.abs(meanV1X) < 0.15); + assertTrue("V1 pull Y mean should be near 0, got " + meanV1Y, FastMath.abs(meanV1Y) < 0.15); + assertTrue("V1 pull Z mean should be near 0, got " + meanV1Z, FastMath.abs(meanV1Z) < 0.15); + assertTrue("V1 pull X std should be near 1, got " + stdV1X, stdV1X > 0.8 && stdV1X < 1.2); + assertTrue("V1 pull Y std should be near 1, got " + stdV1Y, stdV1Y > 0.8 && stdV1Y < 1.2); + assertTrue("V1 pull Z std should be near 1, got " + stdV1Z, stdV1Z > 0.8 && stdV1Z < 1.2); + assertTrue("V2 pull X mean should be near 0, got " + meanV2X, FastMath.abs(meanV2X) < 0.15); + assertTrue("V2 pull Y mean should be near 0, got " + meanV2Y, FastMath.abs(meanV2Y) < 0.15); + assertTrue("V2 pull Z mean should be near 0, got " + meanV2Z, FastMath.abs(meanV2Z) < 0.15); + assertTrue("V2 pull X std should be near 1, got " + stdV2X, stdV2X > 0.8 && stdV2X < 1.2); + assertTrue("V2 pull Y std should be near 1, got " + stdV2Y, stdV2Y > 0.8 && stdV2Y < 1.2); + assertTrue("V2 pull Z std should be near 1, got " + stdV2Z, stdV2Z > 0.8 && stdV2Z < 1.2); + assertTrue("mean chi2/ndf should be near 1, got " + meanChi2Ndf, meanChi2Ndf > 0.5 && meanChi2Ndf < 1.5); + } + + /** + * Toy-MC pull study for {@link TrackConstraintVertexFitter#fitCascadeVertexJointFreeTrack}, + * mirroring {@link #testJointTwoVertexBeamConstrainedSmearedPulls} exactly (same V1/track + * geometry construction) but with no beam-momentum constraint at all -- isolates the + * effect of freeing the three tracks' perigee parameters from the effect of imposing an + * external momentum constraint. Checks V1(x,y,z), V2(x,y,z), and per-track + * (eMinus/ePlus/recoil) momentum pulls all have mean~0/std~1 and mean chi2/ndf~1 (ndf=2: + * 9 constraints - 7 free V1/theta/V2 parameters, same as the fixed-track {@link + * TrackConstraintVertexFitter#fitCascadeVertexJoint}). + */ + public void testJointTwoVertexFreeTrackSmearedPulls() { + for (double flightLength : new double[]{5.0, 20.0, 50.0, 90.0, 120.0, 150.0}) { + runJointTwoVertexFreeTrackSmearedPulls(flightLength, 500); + } + } + + private void runJointTwoVertexFreeTrackSmearedPulls(double flightLength, int nToys) { + System.out.println("\n=== testJointTwoVertexFreeTrackSmearedPulls (flightLength=" + flightLength + " mm) ===\n"); + + double xV1 = 0.2, yV1 = -0.1, zV1 = 3.0; + double pxEm = 0.30, pyEm = 0.10, pzEm = 1.5; + double pxEp = 0.15, pyEp = -0.05, pzEp = 0.8; + double pxRc = 0.20, pyRc = -0.08, pzRc = 1.2; + + double pxV0 = pxEm + pxEp, pyV0 = pyEm + pyEp, pzV0 = pzEm + pzEp; + double pV0Mag = FastMath.sqrt(pxV0 * pxV0 + pyV0 * pyV0 + pzV0 * pzV0); + double nx = pxV0 / pV0Mag, ny = pyV0 / pV0Mag, nz = pzV0 / pV0Mag; + + double xV2 = xV1 + flightLength * nx; + double yV2 = yV1 + flightLength * ny; + double zV2 = zV1 + flightLength * nz; + + double d0Err = 0.03, phi0Err = 0.003, omegaErr = 5e-6, z0Err = 0.03, tanLErr = 0.003; + RealMatrix trackCov = createTrackCovariance(d0Err, phi0Err, omegaErr, z0Err, tanLErr); + double[] trackSigma = {d0Err, phi0Err, omegaErr, z0Err, tanLErr}; + + RealVector v1Init = MatrixUtils.createRealVector(new double[]{xV1, yV1, zV1}); + RealVector v2Init = MatrixUtils.createRealVector(new double[]{0.0, 0.0, -1.1}); + + java.util.Random rng = new java.util.Random(24601); + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(B_FIELD); + + int nNull = 0, nOk = 0; + double[] pullV1X = new double[nToys], pullV1Y = new double[nToys], pullV1Z = new double[nToys]; + double[] pullV2X = new double[nToys], pullV2Y = new double[nToys], pullV2Z = new double[nToys]; + double[] chi2NdfArr = new double[nToys]; + double[][] trackPullPx = new double[3][nToys]; + double[][] trackPullPy = new double[3][nToys]; + double[][] trackPullPz = new double[3][nToys]; + double[] truthPx = {pxEm, pxEp, pxRc}; + double[] truthPy = {pyEm, pyEp, pyRc}; + double[] truthPz = {pzEm, pzEp, pzRc}; + + for (int toy = 0; toy < nToys; toy++) { + TrackParams eMinusTruth = createExactTrackThroughPoint(xV1, yV1, zV1, pxEm, pyEm, pzEm, -1, B_FIELD, trackCov); + TrackParams ePlusTruth = createExactTrackThroughPoint(xV1, yV1, zV1, pxEp, pyEp, pzEp, 1, B_FIELD, trackCov); + TrackParams recoilTruth = createExactTrackThroughPoint(xV2, yV2, zV2, pxRc, pyRc, pzRc, -1, B_FIELD, trackCov); + + TrackParams eMinusTrack = smearTrack(eMinusTruth, trackSigma, trackCov, rng); + TrackParams ePlusTrack = smearTrack(ePlusTruth, trackSigma, trackCov, rng); + TrackParams recoilTrack = smearTrack(recoilTruth, trackSigma, trackCov, rng); + + TwoVertexFitResult result = fitter.fitCascadeVertexJointFreeTrack( + eMinusTrack, ePlusTrack, recoilTrack, v1Init, v2Init); + if (result == null) { + nNull++; + continue; + } + + pullV1X[nOk] = (result.v1.getEntry(0) - xV1) / FastMath.sqrt(result.v1Cov.getEntry(0, 0)); + pullV1Y[nOk] = (result.v1.getEntry(1) - yV1) / FastMath.sqrt(result.v1Cov.getEntry(1, 1)); + pullV1Z[nOk] = (result.v1.getEntry(2) - zV1) / FastMath.sqrt(result.v1Cov.getEntry(2, 2)); + pullV2X[nOk] = (result.v2.getEntry(0) - xV2) / FastMath.sqrt(result.v2Cov.getEntry(0, 0)); + pullV2Y[nOk] = (result.v2.getEntry(1) - yV2) / FastMath.sqrt(result.v2Cov.getEntry(1, 1)); + pullV2Z[nOk] = (result.v2.getEntry(2) - zV2) / FastMath.sqrt(result.v2Cov.getEntry(2, 2)); + chi2NdfArr[nOk] = result.ndf > 0 ? result.chi2 / result.ndf : 0.0; + + for (int i = 0; i < 3; i++) { + TrackMomentum tm = i == 0 ? result.eMinusMomentum : (i == 1 ? result.ePlusMomentum : result.recoilMomentum); + trackPullPx[i][nOk] = (tm.p.getEntry(0) - truthPx[i]) / FastMath.sqrt(tm.pCov.getEntry(0, 0)); + trackPullPy[i][nOk] = (tm.p.getEntry(1) - truthPy[i]) / FastMath.sqrt(tm.pCov.getEntry(1, 1)); + trackPullPz[i][nOk] = (tm.p.getEntry(2) - truthPz[i]) / FastMath.sqrt(tm.pCov.getEntry(2, 2)); + } + nOk++; + } + + double[] pullV1XTrim = java.util.Arrays.copyOf(pullV1X, nOk); + double[] pullV1YTrim = java.util.Arrays.copyOf(pullV1Y, nOk); + double[] pullV1ZTrim = java.util.Arrays.copyOf(pullV1Z, nOk); + double[] pullV2XTrim = java.util.Arrays.copyOf(pullV2X, nOk); + double[] pullV2YTrim = java.util.Arrays.copyOf(pullV2Y, nOk); + double[] pullV2ZTrim = java.util.Arrays.copyOf(pullV2Z, nOk); + double[] chi2NdfTrim = java.util.Arrays.copyOf(chi2NdfArr, nOk); + + double meanV1X = mean(pullV1XTrim), stdV1X = std(pullV1XTrim, meanV1X); + double meanV1Y = mean(pullV1YTrim), stdV1Y = std(pullV1YTrim, meanV1Y); + double meanV1Z = mean(pullV1ZTrim), stdV1Z = std(pullV1ZTrim, meanV1Z); + double meanV2X = mean(pullV2XTrim), stdV2X = std(pullV2XTrim, meanV2X); + double meanV2Y = mean(pullV2YTrim), stdV2Y = std(pullV2YTrim, meanV2Y); + double meanV2Z = mean(pullV2ZTrim), stdV2Z = std(pullV2ZTrim, meanV2Z); + double meanChi2Ndf = mean(chi2NdfTrim); + + System.out.printf("nToys=%d nNull=%d nOk=%d%n", nToys, nNull, nOk); + System.out.printf("V1 pull X: mean=%.3f std=%.3f%n", meanV1X, stdV1X); + System.out.printf("V1 pull Y: mean=%.3f std=%.3f%n", meanV1Y, stdV1Y); + System.out.printf("V1 pull Z: mean=%.3f std=%.3f%n", meanV1Z, stdV1Z); + System.out.printf("V2 pull X: mean=%.3f std=%.3f%n", meanV2X, stdV2X); + System.out.printf("V2 pull Y: mean=%.3f std=%.3f%n", meanV2Y, stdV2Y); + System.out.printf("V2 pull Z: mean=%.3f std=%.3f%n", meanV2Z, stdV2Z); + System.out.printf("[FREE-TRACK] mean chi2/ndf: %.3f%n", meanChi2Ndf); + + for (int i = 0; i < 3; i++) { + double[] tPx = java.util.Arrays.copyOf(trackPullPx[i], nOk); + double[] tPy = java.util.Arrays.copyOf(trackPullPy[i], nOk); + double[] tPz = java.util.Arrays.copyOf(trackPullPz[i], nOk); + double mPx = mean(tPx), sPx = std(tPx, mPx); + double mPy = mean(tPy), sPy = std(tPy, mPy); + double mPz = mean(tPz), sPz = std(tPz, mPz); + String trackName = i == 0 ? "eMinus" : (i == 1 ? "ePlus" : "recoil"); + System.out.printf("track %s momentum pull: Px mean=%.3f std=%.3f | Py mean=%.3f std=%.3f | Pz mean=%.3f std=%.3f%n", + trackName, mPx, sPx, mPy, sPy, mPz, sPz); + assertTrue(trackName + " Px pull mean should be near 0, got " + mPx, FastMath.abs(mPx) < 0.15); + assertTrue(trackName + " Py pull mean should be near 0, got " + mPy, FastMath.abs(mPy) < 0.15); + assertTrue(trackName + " Pz pull mean should be near 0, got " + mPz, FastMath.abs(mPz) < 0.15); + assertTrue(trackName + " Px pull std should be near 1, got " + sPx, sPx > 0.8 && sPx < 1.2); + assertTrue(trackName + " Py pull std should be near 1, got " + sPy, sPy > 0.8 && sPy < 1.2); + assertTrue(trackName + " Pz pull std should be near 1, got " + sPz, sPz > 0.8 && sPz < 1.2); + } + + assertTrue("V1 pull X mean should be near 0, got " + meanV1X, FastMath.abs(meanV1X) < 0.15); + assertTrue("V1 pull Y mean should be near 0, got " + meanV1Y, FastMath.abs(meanV1Y) < 0.15); + assertTrue("V1 pull Z mean should be near 0, got " + meanV1Z, FastMath.abs(meanV1Z) < 0.15); + assertTrue("V1 pull X std should be near 1, got " + stdV1X, stdV1X > 0.8 && stdV1X < 1.2); + assertTrue("V1 pull Y std should be near 1, got " + stdV1Y, stdV1Y > 0.8 && stdV1Y < 1.2); + assertTrue("V1 pull Z std should be near 1, got " + stdV1Z, stdV1Z > 0.8 && stdV1Z < 1.2); + assertTrue("V2 pull X mean should be near 0, got " + meanV2X, FastMath.abs(meanV2X) < 0.15); + assertTrue("V2 pull Y mean should be near 0, got " + meanV2Y, FastMath.abs(meanV2Y) < 0.15); + assertTrue("V2 pull Z mean should be near 0, got " + meanV2Z, FastMath.abs(meanV2Z) < 0.15); + assertTrue("V2 pull X std should be near 1, got " + stdV2X, stdV2X > 0.8 && stdV2X < 1.2); + assertTrue("V2 pull Y std should be near 1, got " + stdV2Y, stdV2Y > 0.8 && stdV2Y < 1.2); + assertTrue("V2 pull Z std should be near 1, got " + stdV2Z, stdV2Z > 0.8 && stdV2Z < 1.2); + assertTrue("mean chi2/ndf should be near 1, got " + meanChi2Ndf, meanChi2Ndf > 0.5 && meanChi2Ndf < 1.5); + } + + /** + * The V0 flight line through V1 (direction pV0) generically crosses the recoil track's + * own transverse (bending-plane) circle at two points -- a genuine near/far branch + * ambiguity that plain least-squares projection of the caller's v2Init guess cannot + * resolve, since it knows nothing about the recoil track. Build a geometry with a large, + * deliberately-unphysical truth theta (15.0, i.e. V2 is 15x further along -pV0 than V1) + * so the two transverse roots are far apart, and verify: (1) transverseCircleRoots finds + * both roots and they satisfy the recoil track's own circle equation; (2) one root + * matches truth closely while the other is a genuinely distinct, spurious branch; (3) + * selectPhysicalThetaSeed's z-consistency discriminant (comparing V2.z against the + * recoil track's own zV prediction at each candidate) picks the truth-matching root, not + * the spurious one. + */ + public void testTransverseCircleRootsAndBranchSelection() { + System.out.println("\n=== testTransverseCircleRootsAndBranchSelection ===\n"); + + double xV1 = 0.2, yV1 = -0.1, zV1 = 3.0; + double pxEm = 0.30, pyEm = 0.10, pzEm = 1.5; + double pxEp = 0.15, pyEp = -0.05, pzEp = 0.8; + + double pxV0 = pxEm + pxEp, pyV0 = pyEm + pyEp, pzV0 = pzEm + pzEp; + + double thetaTrue = 15.0; + double xV2 = xV1 - thetaTrue * pxV0; + double yV2 = yV1 - thetaTrue * pyV0; + double zV2 = zV1 - thetaTrue * pzV0; + + double pxRc = 0.20, pyRc = -0.10, pzRc = 3.0; + RealMatrix trackCov = createTrackCovariance(0.03, 0.003, 5e-6, 0.03, 0.003); + TrackParams recoilTrack = createExactTrackThroughPoint(xV2, yV2, zV2, pxRc, pyRc, pzRc, -1, B_FIELD, trackCov); + + RealVector v1 = MatrixUtils.createRealVector(new double[]{xV1, yV1, zV1}); + RealVector pV0 = MatrixUtils.createRealVector(new double[]{pxV0, pyV0, pzV0}); + + double[] roots = TrackConstraintVertexFitter.transverseCircleRoots(v1, pV0, recoilTrack); + assertNotNull("expected two real transverse-circle roots for this geometry", roots); + assertEquals(2, roots.length); + + double R = 1.0 / FastMath.abs(recoilTrack.omega); + double sign = FastMath.signum(recoilTrack.omega); + double xc = R * sign * FastMath.sin(recoilTrack.phi0) - recoilTrack.d0 * FastMath.sin(recoilTrack.phi0); + double yc = -R * sign * FastMath.cos(recoilTrack.phi0) + recoilTrack.d0 * FastMath.cos(recoilTrack.phi0); + for (double theta : roots) { + double xOnLine = v1.getEntry(0) - theta * pV0.getEntry(0); + double yOnLine = v1.getEntry(1) - theta * pV0.getEntry(1); + double r = FastMath.sqrt((xOnLine - xc) * (xOnLine - xc) + (yOnLine - yc) * (yOnLine - yc)); + assertEquals("root theta=" + theta + " should lie on the recoil circle", R, r, 1e-6); + } + + double res0 = FastMath.abs(roots[0] - thetaTrue); + double res1 = FastMath.abs(roots[1] - thetaTrue); + double bestResidual = FastMath.min(res0, res1); + double worstResidual = FastMath.max(res0, res1); + System.out.printf("roots=[%.6f, %.6f] thetaTrue=%.6f%n", roots[0], roots[1], thetaTrue); + assertTrue("one root should closely match thetaTrue, got best residual " + bestResidual, + bestResidual < 1e-6); + assertTrue("the other root should be a genuinely distinct, spurious branch, got separation " + + FastMath.abs(roots[1] - roots[0]), FastMath.abs(roots[1] - roots[0]) > 1.0); + assertTrue("spurious root should not itself be near truth, got residual " + worstResidual, + worstResidual > 1.0); + + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(B_FIELD); + TrackConstraintVertexFitter.ThetaSeed seed = fitter.selectPhysicalThetaSeed(v1, pV0, recoilTrack); + assertNotNull("selectPhysicalThetaSeed should resolve the branch ambiguity", seed); + System.out.printf("selected theta=%.6f v2=[%.6f, %.6f, %.6f] (truth: %.6f, [%.6f, %.6f, %.6f])%n", + seed.theta, seed.v2.getEntry(0), seed.v2.getEntry(1), seed.v2.getEntry(2), + thetaTrue, xV2, yV2, zV2); + + assertEquals("selected theta should match the truth branch, not the spurious one", + thetaTrue, seed.theta, 1e-6); + assertEquals(xV2, seed.v2.getEntry(0), 1e-4); + assertEquals(yV2, seed.v2.getEntry(1), 1e-4); + assertEquals(zV2, seed.v2.getEntry(2), 1e-4); + } + + /** + * End-to-end check that the branch-selection fix ({@link #testTransverseCircleRootsAndBranchSelection}) + * actually changes the full Newton iteration's outcome, not just the seed in isolation. + * Reuses that test's exact geometry -- same V1/tracks/thetaTrue=15, so the same genuinely + * separated near/far roots (-1774.86 and 15.0) exist -- but drives the full + * {@code fitCascadeVertexJoint} call with a deliberately poor {@code v2Init} at the + * origin, matching {@code CascadeVertexer}'s real usage (defaults to the beamspot, not + * truth, when the caller has no better guess). Without the branch-selection override, the + * least-squares theta seeded from (v1Init, v2Init=origin) has no information about the + * recoil track at all and can leave Newton free to walk to the spurious branch and + * self-consistently settle there (large chi2, wrong V1/V2) -- see the bug description on + * {@link #fitCascadeVertexJoint} referenced in the production-code comment. With the fix, + * {@code selectPhysicalThetaSeed} overrides the seed before iteration starts regardless of + * {@code v2Init}, so the fit should converge tightly to truth even from this adversarial + * starting point. + */ + public void testJointTwoVertexAdversarialSeedConvergesToTruthBranch() { + System.out.println("\n=== testJointTwoVertexAdversarialSeedConvergesToTruthBranch ===\n"); + + double xV1 = 0.2, yV1 = -0.1, zV1 = 3.0; + double pxEm = 0.30, pyEm = 0.10, pzEm = 1.5; + double pxEp = 0.15, pyEp = -0.05, pzEp = 0.8; + + double pxV0 = pxEm + pxEp, pyV0 = pyEm + pyEp, pzV0 = pzEm + pzEp; + + double thetaTrue = 15.0; + double xV2 = xV1 - thetaTrue * pxV0; + double yV2 = yV1 - thetaTrue * pyV0; + double zV2 = zV1 - thetaTrue * pzV0; + + double pxRc = 0.20, pyRc = -0.10, pzRc = 3.0; + + RealMatrix trackCov = createTrackCovariance(0.03, 0.003, 5e-6, 0.03, 0.003); + TrackParams eMinusTrack = createExactTrackThroughPoint(xV1, yV1, zV1, pxEm, pyEm, pzEm, -1, B_FIELD, trackCov); + TrackParams ePlusTrack = createExactTrackThroughPoint(xV1, yV1, zV1, pxEp, pyEp, pzEp, 1, B_FIELD, trackCov); + TrackParams recoilTrack = createExactTrackThroughPoint(xV2, yV2, zV2, pxRc, pyRc, pzRc, -1, B_FIELD, trackCov); + + RealVector v1Init = MatrixUtils.createRealVector(new double[]{xV1, yV1, zV1}); + // Deliberately adversarial: far from both truth V2 and the spurious branch's V2, and + // carrying no information at all about which branch the recoil track prefers -- the + // same "beamspot near origin" guess CascadeVertexer falls back to in practice. + RealVector v2Init = MatrixUtils.createRealVector(new double[]{0.0, 0.0, 0.0}); + + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(B_FIELD); + TwoVertexFitResult result = fitter.fitCascadeVertexJoint( + eMinusTrack, ePlusTrack, recoilTrack, v1Init, v2Init, 500, 1.0e-10); + + assertNotNull("Joint two-vertex fit result should not be null", result); + + System.out.printf("Fitted V1: [%.6f, %.6f, %.6f] (truth: [%.6f, %.6f, %.6f])%n", + result.v1.getEntry(0), result.v1.getEntry(1), result.v1.getEntry(2), xV1, yV1, zV1); + System.out.printf("Fitted V2: [%.6f, %.6f, %.6f] (truth: [%.6f, %.6f, %.6f])%n", + result.v2.getEntry(0), result.v2.getEntry(1), result.v2.getEntry(2), xV2, yV2, zV2); + System.out.printf("Chi2/NDF: %.6f / %d%n", result.chi2, result.ndf); + + assertEquals(xV1, result.v1.getEntry(0), 1e-3); + assertEquals(yV1, result.v1.getEntry(1), 1e-3); + assertEquals(zV1, result.v1.getEntry(2), 1e-3); + assertEquals(xV2, result.v2.getEntry(0), 1e-3); + assertEquals(yV2, result.v2.getEntry(1), 1e-3); + assertEquals(zV2, result.v2.getEntry(2), 1e-3); + assertEquals(pxEm, result.eMinusMomentum.p.getEntry(0), 1e-3); + assertEquals(pyEm, result.eMinusMomentum.p.getEntry(1), 1e-3); + assertEquals(pzEm, result.eMinusMomentum.p.getEntry(2), 1e-3); + assertTrue("chi2 should be small for exactly-consistent geometry, got " + result.chi2, result.chi2 < 1e-2); + } + + private static RealMatrix cov5x5(double[][] rows) { + RealMatrix m = MatrixUtils.createRealMatrix(5, 5); + for (int i = 0; i < 5; i++) { + for (int j = 0; j < 5; j++) { + m.setEntry(i, j, rows[i][j]); + } + } + return m; + } + + /** + * Regression test using exact real-data inputs (run 14272, one of the truth-matched + * candidates from mcReconApPulser180MeV/ap_pulser_100 with apVtxZMC~37mm and v1Init~39mm) + * that, prior to fixing the backtracking line search's merit function to include the + * parameter-pull term dx^T W dx (not just the constraint-residual norm ||h||) and adding + * a trust-region cap on the per-iteration vertex step, converged to a wildly wrong V1 + * (~[-55,-3,-1] mm) with chi2/ndf~1267. This particular candidate's v1Init turns out to + * have a genuine ~7mm self-inconsistency against the eMinus track's own longitudinal + * (z0/tanLambda) prediction even before any fit adjustment (verified by hand from the + * raw track parameters) -- i.e. not every truth-matched candidate has a v1Init that is + * itself a perfect local optimum, so this test only checks that the fit no longer blows + * up catastrophically (chi2/ndf << 1267), not that it lands exactly on v1Init. + * + *

Uses the explicit-v2Init overload (an arbitrary non-null guess, giving V2 a flat/ + * weak prior) rather than the {@code v2Init=null} convenience overload, which defaults + * to a beamspot-size-tied V2 prior ({@code useBeamspotPriorForV2=true}). Production + * ({@link CascadeVertexer}) never takes the null-v2Init path -- it always supplies + * its own explicit v2Init/v2Cov from the already-fitted V0 line -- precisely because + * pairing a tight beamspot prior with {@code selectPhysicalThetaSeed}'s own + * documented-unreliable branch choice (worse than a coin flip; see + * {@code CascadeVertexer}'s Javadoc on {@code v0InputVtxZ}) can pin V2 near a seed on + * the wrong branch and blow up chi2, independent of any track-level correctness. With a + * flat V2 prior, both {@code transverseCircleRoots} branches converge to the same + * well-behaved V1 for this event, which is what this test actually checks. + */ + public void testJointTwoVertexRealDataRegression() { + System.out.println("\n=== testJointTwoVertexRealDataRegression ===\n"); + + double bField = -0.8595999999999999; + + TrackParams eMinusTrack = new TrackParams( + -0.4138278812633871, 0.05818441086284123, 2.2774348286756926E-4, + 3.4280614931007167, -0.08920239911869288, + cov5x5(new double[][]{ + {0.15615547160986665, -0.0016108269680988323, -6.136407470045745E-6, 0.00905529155838811, -8.625784313471315E-5}, + {-0.0016108269680988323, 1.8920753232229165E-5, 7.679882358313323E-8, -8.499857689815913E-5, 8.411670918677625E-7}, + {-6.136407470045745E-6, 7.679882358313323E-8, 3.436488183137914E-10, -3.103531812967638E-7, 3.1355922839603002E-9}, + {0.00905529155838811, -8.499857689815913E-5, -3.103531812967638E-7, 0.0028035017716515386, -4.008046330392784E-5}, + {-8.625784313471315E-5, 8.411670918677625E-7, 3.1355922839603002E-9, -4.008046330392784E-5, 6.201734967043375E-7}})); + + TrackParams ePlusTrack = new TrackParams( + -1.4158622787508648, 0.026232654124577415, -7.340327909292423E-5, + -1.395029157958812, 0.03055412093977258, + cov5x5(new double[][]{ + {0.06907944877016554, -3.707516890884016E-4, -5.570557539621614E-7, -0.0027419045884956397, 1.0944381852808471E-5}, + {-3.707516890884016E-4, 2.3555616481904708E-6, 3.679013133079066E-9, 1.5548720728112388E-5, -8.038460884907129E-8}, + {-5.570557539621614E-7, 3.679013133079066E-9, 6.949082309771353E-12, 2.3959185608354854E-8, -1.3153402748976983E-10}, + {-0.0027419045884956397, 1.5548720728112388E-5, 2.3959185608354854E-8, 0.001142758650098608, -1.0696167027024057E-5}, + {1.0944381852808471E-5, -8.038460884907129E-8, -1.3153402748976983E-10, -1.0696167027024057E-5, 1.332734727465295E-7}})); + + TrackParams recoilTrack = new TrackParams( + 1.7534739233699383, 0.0028232486700802044, 6.790166806190062E-4, + -0.10407287685488854, -0.03300521957923669, + cov5x5(new double[][]{ + {0.24563231182724743, -0.0029459600581028204, -1.1294416141395693E-5, 0.022629210781667644, -3.005455288274632E-4}, + {-0.0029459600581028204, 4.00251403930356E-5, 1.6327180350031857E-7, -2.3970366994327392E-4, 3.2538808883958757E-6}, + {-1.1294416141395693E-5, 1.6327180350031857E-7, 8.643410712643231E-10, -9.189655156689234E-7, 1.2613796957457733E-8}, + {0.022629210781667644, -2.3970366994327392E-4, -9.189655156689234E-7, 0.015358330058350982, -2.5466221420599583E-4}, + {-3.005455288274632E-4, 3.2538808883958757E-6, 1.2613796957457733E-8, -2.5466221420599583E-4, 4.300732964474161E-6}})); + + RealVector v1Init = MatrixUtils.createRealVector(new double[]{38.9028909318, 0.5285581997, -0.2061714304}); + + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(bField); + RealVector v2Init = MatrixUtils.createRealVector(fitter.getBeamPosition()); + TwoVertexFitResult result = fitter.fitCascadeVertexJoint(eMinusTrack, ePlusTrack, recoilTrack, v1Init, v2Init); + + assertNotNull("Joint two-vertex fit result should not be null", result); + System.out.printf("Fitted V1: [%.6f, %.6f, %.6f] (v1Init: [%.6f, %.6f, %.6f])%n", + result.v1.getEntry(0), result.v1.getEntry(1), result.v1.getEntry(2), + v1Init.getEntry(0), v1Init.getEntry(1), v1Init.getEntry(2)); + System.out.printf("Chi2/NDF: %.6f / %d = %.6f%n", result.chi2, result.ndf, result.chi2 / result.ndf); + + assertTrue("chi2/ndf should be far below the pre-fix catastrophic value of ~1267, got " + + (result.chi2 / result.ndf), result.chi2 / result.ndf < 300.0); + } + + /** + * Toy-MC pull study for the joint two-vertex fit under realistic smearing and the same + * (v1Init=truth-ish, v2Init=beamspot, maxIterations=20, tolerance=1e-8) conditions + * {@link org.hps.recon.vertexing.CascadeVertexer} actually uses in production -- + * isolating whether the bad V1 positions / inflated V1 mass seen on real reconstructed + * data are reproducible from the core fit math alone under realistic noise, as opposed + * to something specific to real detector data. + */ + public void testJointTwoVertexSmearedPulls() { + for (double flightLength : new double[]{5.0, 20.0, 50.0, 90.0, 120.0, 150.0}) { + runJointTwoVertexSmearedPulls(flightLength, 500); + } + } + + private void runJointTwoVertexSmearedPulls(double flightLength, int nToys) { + System.out.println("\n=== testJointTwoVertexSmearedPulls (flightLength=" + flightLength + " mm) ===\n"); + + double xV1 = 0.2, yV1 = -0.1, zV1 = 3.0; + double pxEm = 0.30, pyEm = 0.10, pzEm = 1.5; + double pxEp = 0.15, pyEp = -0.05, pzEp = 0.8; + + double pxV0 = pxEm + pxEp, pyV0 = pyEm + pyEp, pzV0 = pzEm + pzEp; + double pV0Mag = FastMath.sqrt(pxV0 * pxV0 + pyV0 * pyV0 + pzV0 * pzV0); + double nx = pxV0 / pV0Mag, ny = pyV0 / pV0Mag, nz = pzV0 / pV0Mag; + + double xV2 = xV1 + flightLength * nx; + double yV2 = yV1 + flightLength * ny; + double zV2 = zV1 + flightLength * nz; + + double pxRc = 0.20, pyRc = -0.10, pzRc = 3.0; + + double d0Err = 0.03, phi0Err = 0.003, omegaErr = 5e-6, z0Err = 0.03, tanLErr = 0.003; + RealMatrix trackCov = createTrackCovariance(d0Err, phi0Err, omegaErr, z0Err, tanLErr); + double[] trackSigma = {d0Err, phi0Err, omegaErr, z0Err, tanLErr}; + + RealVector v1Init = MatrixUtils.createRealVector(new double[]{xV1, yV1, zV1}); + RealVector v2Init = MatrixUtils.createRealVector(new double[]{0.0, 0.0, -1.1}); + + java.util.Random rng = new java.util.Random(6789); + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(B_FIELD); + + int nNull = 0; + int nHighChi2 = 0; + int nMassOver200 = 0; + double[] pullV1X = new double[nToys], pullV1Y = new double[nToys], pullV1Z = new double[nToys]; + double[] chi2Arr = new double[nToys]; + double[] massArr = new double[nToys]; + int nOk = 0; + + for (int toy = 0; toy < nToys; toy++) { + TrackParams eMinusTruth = createExactTrackThroughPoint(xV1, yV1, zV1, pxEm, pyEm, pzEm, -1, B_FIELD, trackCov); + TrackParams ePlusTruth = createExactTrackThroughPoint(xV1, yV1, zV1, pxEp, pyEp, pzEp, 1, B_FIELD, trackCov); + TrackParams recoilTruth = createExactTrackThroughPoint(xV2, yV2, zV2, pxRc, pyRc, pzRc, -1, B_FIELD, trackCov); + + TrackParams eMinusTrack = smearTrack(eMinusTruth, trackSigma, trackCov, rng); + TrackParams ePlusTrack = smearTrack(ePlusTruth, trackSigma, trackCov, rng); + TrackParams recoilTrack = smearTrack(recoilTruth, trackSigma, trackCov, rng); + + TwoVertexFitResult result = fitter.fitCascadeVertexJoint(eMinusTrack, ePlusTrack, recoilTrack, v1Init, v2Init); + if (result == null) { + nNull++; + continue; + } + + double chi2Ndf = result.chi2 / result.ndf; + if (chi2Ndf > 20) { + nHighChi2++; + } + + double eEm = FastMath.sqrt(result.eMinusMomentum.p.dotProduct(result.eMinusMomentum.p) + ELECTRON_MASS * ELECTRON_MASS); + double eEp = FastMath.sqrt(result.ePlusMomentum.p.dotProduct(result.ePlusMomentum.p) + ELECTRON_MASS * ELECTRON_MASS); + RealVector pV0 = result.eMinusMomentum.p.add(result.ePlusMomentum.p); + double massSq = (eEm + eEp) * (eEm + eEp) - pV0.dotProduct(pV0); + double mass = massSq > 0 ? FastMath.sqrt(massSq) : -1.0; + if (mass > 0.2) { + nMassOver200++; + } + + chi2Arr[nOk] = result.chi2; + massArr[nOk] = mass; + pullV1X[nOk] = (result.v1.getEntry(0) - xV1) / FastMath.sqrt(result.v1Cov.getEntry(0, 0)); + pullV1Y[nOk] = (result.v1.getEntry(1) - yV1) / FastMath.sqrt(result.v1Cov.getEntry(1, 1)); + pullV1Z[nOk] = (result.v1.getEntry(2) - zV1) / FastMath.sqrt(result.v1Cov.getEntry(2, 2)); + nOk++; + } + + double[] chi2Trim = java.util.Arrays.copyOf(chi2Arr, nOk); + double[] massTrim = java.util.Arrays.copyOf(massArr, nOk); + double[] pullXTrim = java.util.Arrays.copyOf(pullV1X, nOk); + double[] pullYTrim = java.util.Arrays.copyOf(pullV1Y, nOk); + double[] pullZTrim = java.util.Arrays.copyOf(pullV1Z, nOk); + + System.out.printf("nToys=%d nNull=%d nHighChi2(>20)=%d nMassOver200MeV=%d nOk=%d%n", + nToys, nNull, nHighChi2, nMassOver200, nOk); + System.out.printf("chi2: mean=%.3f max=%.3f%n", mean(chi2Trim), java.util.Arrays.stream(chi2Trim).max().orElse(0)); + System.out.printf("V1 mass: mean=%.4f max=%.4f%n", mean(massTrim), java.util.Arrays.stream(massTrim).max().orElse(0)); + System.out.printf("V1 pull X: mean=%.3f std=%.3f%n", mean(pullXTrim), std(pullXTrim, mean(pullXTrim))); + System.out.printf("V1 pull Y: mean=%.3f std=%.3f%n", mean(pullYTrim), std(pullYTrim, mean(pullYTrim))); + System.out.printf("V1 pull Z: mean=%.3f std=%.3f%n", mean(pullZTrim), std(pullZTrim, mean(pullZTrim))); + } + + /** + * Toy-MC pull study for the N-track (trident) common-vertex fit with the total 3-momentum + * constrained to the beam value -- the actual physics goal of this repo. Builds a common + * production vertex with three tracks (2 e- + 1 e+) whose momenta sum exactly to the beam + * 3-momentum (same construction as {@code testThreeMomentumConstraint}, but now at a + * displaced vertex point rather than the origin, and smeared/looped as toys), then checks + * both the soft ({@link #fitSoftConstrained}) and hard/exact + * ({@link #fitLagrangeMultiplier}) momentum-constraint modes side by side: fitted-vertex + * pulls should have mean~0 / width~1 and chi2/ndf should average to ~1 (ndf=6: 2 constraints + * per track x 3 tracks + 3 momentum constraints - 3 vertex parameters), for both modes, + * before trusting the same machinery on real trident MC. + */ + public void testNTrackBeamMomentumConstraintSmearedPulls() { + System.out.println("\n=== testNTrackBeamMomentumConstraintSmearedPulls ===\n"); + + double pBeam = 3.74; + double rotAngle = -0.0305; + + double beamPx = pBeam * FastMath.cos(rotAngle); + double beamPy = -pBeam * FastMath.sin(rotAngle); + double beamPz = 0.0; + + // Same beam-momentum-covariance construction as fitVertex()'s beamMomentumConstraint + // block: 1% longitudinal, 100 urad transverse divergence, rotated into tracking frame. + double dpOverP = 1e-2; + double sigmaTheta = 100e-6; + double sigmaL = dpOverP * pBeam; + double sigmaT = sigmaTheta * pBeam; + double cosR = FastMath.cos(rotAngle); + double sinR = FastMath.sin(rotAngle); + double sL2 = sigmaL * sigmaL; + double sT2 = sigmaT * sigmaT; + RealMatrix beamPCov = MatrixUtils.createRealMatrix(3, 3); + beamPCov.setEntry(0, 0, sL2 * cosR * cosR + sT2 * sinR * sinR); + beamPCov.setEntry(0, 1, (sT2 - sL2) * sinR * cosR); + beamPCov.setEntry(1, 0, (sT2 - sL2) * sinR * cosR); + beamPCov.setEntry(1, 1, sL2 * sinR * sinR + sT2 * cosR * cosR); + beamPCov.setEntry(2, 2, sT2); + + RealVector beamP = MatrixUtils.createRealVector(new double[]{beamPx, beamPy, beamPz}); + + // Common production vertex (tracking frame), displaced from the origin. + double xV = 0.2, yV = -0.1, zV = 3.0; + + // Three track momenta (2 e-, 1 e+) summing exactly to the beam 3-momentum. + double px1 = 1.8, py1 = 0.25, pz1 = 0.15; + double px2 = 1.2, py2 = -0.20, pz2 = -0.10; + double px3 = beamPx - px1 - px2; + double py3 = beamPy - py1 - py2; + double pz3 = -pz1 - pz2; + + double d0Err = 0.03, phi0Err = 0.003, omegaErr = 5e-6, z0Err = 0.03, tanLErr = 0.003; + RealMatrix trackCov = createTrackCovariance(d0Err, phi0Err, omegaErr, z0Err, tanLErr); + double[] trackSigma = {d0Err, phi0Err, omegaErr, z0Err, tanLErr}; + + java.util.Random rng = new java.util.Random(31415); + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(B_FIELD); + + // Cholesky factor of beamPCov, used to draw a per-toy smeared beam-momentum + // constraint input for SOFT mode (see momentum-pull comment below): the fit is told + // the constraint has uncertainty beamPCov, so a meaningful toy-MC pull test must + // actually realize that uncertainty toy-by-toy, not feed the exact truth every time. + RealMatrix beamPCovChol = new CholeskyDecomposition(beamPCov).getL(); + + int nToys = 500; + for (boolean hard : new boolean[]{false, true}) { + int nNull = 0, nOk = 0; + double[] pullX = new double[nToys], pullY = new double[nToys], pullZ = new double[nToys]; + double[] chi2NdfArr = new double[nToys]; + // Momentum-pull arrays are only meaningful in SOFT mode. In HARD + // (Lagrange-multiplier/exact) mode, totalP is forced to equal beamP exactly for + // every toy (fourMomentumConstraintCov=null gives V_mom=0 in the KKT system), so + // Cov(totalP) = J^T*C_fitted*J collapses to ~0 by construction -- dividing by its + // near-zero sqrt would produce meaningless/unstable pulls, not a real statistical + // test. Instead, HARD mode is checked via the raw residual + sqrt(cov) below, + // confirming both are consistently near zero (i.e. the constraint really is exact). + double[] pullPx = new double[nToys], pullPy = new double[nToys], pullPz = new double[nToys]; + double[] residPx = new double[nToys], residPy = new double[nToys], residPz = new double[nToys]; + double[] sigPx = new double[nToys], sigPy = new double[nToys], sigPz = new double[nToys]; + + // Per-track momentum pulls, in BOTH modes: unlike totalMomentumCov (which collapses + // to ~0 under the hard total-momentum constraint, since that constraint pins exactly + // the sum), each track's own pCov comes from its own diagonal block of C_fitted and + // is not directly collapsed by a constraint on a different (summed) linear + // combination -- individual track momenta should remain meaningfully uncertain even + // in HARD mode. + double[][] trackPullPx = new double[3][nToys]; + double[][] trackPullPy = new double[3][nToys]; + double[][] trackPullPz = new double[3][nToys]; + double[] truthPx = {px1, px2, px3}; + double[] truthPy = {py1, py2, py3}; + double[] truthPz = {pz1, pz2, pz3}; + + for (int toy = 0; toy < nToys; toy++) { + TrackParams e1Truth = createExactTrackThroughPoint(xV, yV, zV, px1, py1, pz1, -1, B_FIELD, trackCov); + TrackParams e2Truth = createExactTrackThroughPoint(xV, yV, zV, px2, py2, pz2, -1, B_FIELD, trackCov); + TrackParams posTruth = createExactTrackThroughPoint(xV, yV, zV, px3, py3, pz3, 1, B_FIELD, trackCov); + + List tracks = new ArrayList<>(); + tracks.add(smearTrack(e1Truth, trackSigma, trackCov, rng)); + tracks.add(smearTrack(e2Truth, trackSigma, trackCov, rng)); + tracks.add(smearTrack(posTruth, trackSigma, trackCov, rng)); + + // SOFT mode is fed a per-toy smeared beam constraint (see beamPCovChol comment + // above); HARD mode keeps the exact truth value, since that mode's own test + // below is specifically about verifying the constraint is enforced exactly. + RealVector beamNoise = MatrixUtils.createRealVector( + new double[]{rng.nextGaussian(), rng.nextGaussian(), rng.nextGaussian()}); + RealVector smearedBeamP = beamP.add(beamPCovChol.operate(beamNoise)); + + FitResult result = hard + ? fitter.fitLagrangeMultiplier(tracks, null, null, beamP, 10, 1e-6) + : fitter.fitSoftConstrained(tracks, null, null, smearedBeamP, beamPCov, 10, 1e-6); + if (result == null) { + nNull++; + continue; + } + + pullX[nOk] = (result.vertex.getEntry(0) - xV) / FastMath.sqrt(result.vertexCov.getEntry(0, 0)); + pullY[nOk] = (result.vertex.getEntry(1) - yV) / FastMath.sqrt(result.vertexCov.getEntry(1, 1)); + pullZ[nOk] = (result.vertex.getEntry(2) - zV) / FastMath.sqrt(result.vertexCov.getEntry(2, 2)); + chi2NdfArr[nOk] = result.ndf > 0 ? result.chi2 / result.ndf : 0.0; + + // Total (summed) fitted momentum, in tracking frame -- truth total P is + // exactly beamP by construction (px3/py3/pz3 above are defined as beamP minus + // the other two tracks), so this checks the new Cov(totalP) = J^T*C_fitted*J + // propagation (task #33/#34) independently of the vertex-position check above. + residPx[nOk] = result.totalMomentum.getEntry(0) - beamPx; + residPy[nOk] = result.totalMomentum.getEntry(1) - beamPy; + residPz[nOk] = result.totalMomentum.getEntry(2) - beamPz; + sigPx[nOk] = FastMath.sqrt(FastMath.abs(result.totalMomentumCov.getEntry(0, 0))); + sigPy[nOk] = FastMath.sqrt(FastMath.abs(result.totalMomentumCov.getEntry(1, 1))); + sigPz[nOk] = FastMath.sqrt(FastMath.abs(result.totalMomentumCov.getEntry(2, 2))); + if (!hard) { + pullPx[nOk] = residPx[nOk] / sigPx[nOk]; + pullPy[nOk] = residPy[nOk] / sigPy[nOk]; + pullPz[nOk] = residPz[nOk] / sigPz[nOk]; + } + + for (int i = 0; i < 3; i++) { + RealVector pTrk = result.trackMomenta.get(i).p; + RealMatrix pCovTrk = result.trackMomenta.get(i).pCov; + trackPullPx[i][nOk] = (pTrk.getEntry(0) - truthPx[i]) / FastMath.sqrt(pCovTrk.getEntry(0, 0)); + trackPullPy[i][nOk] = (pTrk.getEntry(1) - truthPy[i]) / FastMath.sqrt(pCovTrk.getEntry(1, 1)); + trackPullPz[i][nOk] = (pTrk.getEntry(2) - truthPz[i]) / FastMath.sqrt(pCovTrk.getEntry(2, 2)); + } + nOk++; + } + + double[] pullXTrim = java.util.Arrays.copyOf(pullX, nOk); + double[] pullYTrim = java.util.Arrays.copyOf(pullY, nOk); + double[] pullZTrim = java.util.Arrays.copyOf(pullZ, nOk); + double[] chi2NdfTrim = java.util.Arrays.copyOf(chi2NdfArr, nOk); + + double meanX = mean(pullXTrim), stdX = std(pullXTrim, meanX); + double meanY = mean(pullYTrim), stdY = std(pullYTrim, meanY); + double meanZ = mean(pullZTrim), stdZ = std(pullZTrim, meanZ); + double meanChi2Ndf = mean(chi2NdfTrim); + + String mode = hard ? "HARD" : "SOFT"; + System.out.printf("[%s] nToys=%d nNull=%d nOk=%d%n", mode, nToys, nNull, nOk); + System.out.printf("[%s] pull X: mean=%.3f std=%.3f%n", mode, meanX, stdX); + System.out.printf("[%s] pull Y: mean=%.3f std=%.3f%n", mode, meanY, stdY); + System.out.printf("[%s] pull Z: mean=%.3f std=%.3f%n", mode, meanZ, stdZ); + System.out.printf("[%s] mean chi2/ndf: %.3f%n", mode, meanChi2Ndf); + + if (!hard) { + double[] pullPxTrim = java.util.Arrays.copyOf(pullPx, nOk); + double[] pullPyTrim = java.util.Arrays.copyOf(pullPy, nOk); + double[] pullPzTrim = java.util.Arrays.copyOf(pullPz, nOk); + double meanPx = mean(pullPxTrim), stdPx = std(pullPxTrim, meanPx); + double meanPy = mean(pullPyTrim), stdPy = std(pullPyTrim, meanPy); + double meanPz = mean(pullPzTrim), stdPz = std(pullPzTrim, meanPz); + System.out.printf("[%s] pull totalPx: mean=%.3f std=%.3f%n", mode, meanPx, stdPx); + System.out.printf("[%s] pull totalPy: mean=%.3f std=%.3f%n", mode, meanPy, stdPy); + System.out.printf("[%s] pull totalPz: mean=%.3f std=%.3f%n", mode, meanPz, stdPz); + assertTrue(mode + " pull totalPx mean should be near 0, got " + meanPx, FastMath.abs(meanPx) < 0.15); + assertTrue(mode + " pull totalPy mean should be near 0, got " + meanPy, FastMath.abs(meanPy) < 0.15); + assertTrue(mode + " pull totalPz mean should be near 0, got " + meanPz, FastMath.abs(meanPz) < 0.15); + assertTrue(mode + " pull totalPx std should be near 1, got " + stdPx, stdPx > 0.8 && stdPx < 1.2); + assertTrue(mode + " pull totalPy std should be near 1, got " + stdPy, stdPy > 0.8 && stdPy < 1.2); + assertTrue(mode + " pull totalPz std should be near 1, got " + stdPz, stdPz > 0.8 && stdPz < 1.2); + } else { + // HARD mode: confirm the constraint really is exact -- both the residual + // (fitted totalP vs beamP) and the propagated sqrt(Cov(totalP)) should be + // consistently tiny (numerical-precision level), not just individually small. + double meanAbsResidPx = mean(absArr(java.util.Arrays.copyOf(residPx, nOk))); + double meanAbsResidPy = mean(absArr(java.util.Arrays.copyOf(residPy, nOk))); + double meanAbsResidPz = mean(absArr(java.util.Arrays.copyOf(residPz, nOk))); + double meanSigPx = mean(java.util.Arrays.copyOf(sigPx, nOk)); + double meanSigPy = mean(java.util.Arrays.copyOf(sigPy, nOk)); + double meanSigPz = mean(java.util.Arrays.copyOf(sigPz, nOk)); + System.out.printf("[%s] totalP residual (fit-beam): |Px|=%.3e |Py|=%.3e |Pz|=%.3e%n", + mode, meanAbsResidPx, meanAbsResidPy, meanAbsResidPz); + System.out.printf("[%s] sqrt(Cov(totalP)): Px=%.3e Py=%.3e Pz=%.3e%n", + mode, meanSigPx, meanSigPy, meanSigPz); + assertTrue(mode + " totalPx residual should be ~0 (exact constraint), got " + meanAbsResidPx, + meanAbsResidPx < 1e-4); + assertTrue(mode + " totalPy residual should be ~0 (exact constraint), got " + meanAbsResidPy, + meanAbsResidPy < 1e-4); + assertTrue(mode + " totalPz residual should be ~0 (exact constraint), got " + meanAbsResidPz, + meanAbsResidPz < 1e-4); + assertTrue(mode + " sqrt(Cov(totalPx)) should be ~0 (exact constraint), got " + meanSigPx, + meanSigPx < 1e-4); + assertTrue(mode + " sqrt(Cov(totalPy)) should be ~0 (exact constraint), got " + meanSigPy, + meanSigPy < 1e-4); + assertTrue(mode + " sqrt(Cov(totalPz)) should be ~0 (exact constraint), got " + meanSigPz, + meanSigPz < 1e-4); + } + + for (int i = 0; i < 3; i++) { + double[] tPx = java.util.Arrays.copyOf(trackPullPx[i], nOk); + double[] tPy = java.util.Arrays.copyOf(trackPullPy[i], nOk); + double[] tPz = java.util.Arrays.copyOf(trackPullPz[i], nOk); + double mPx = mean(tPx), sPx = std(tPx, mPx); + double mPy = mean(tPy), sPy = std(tPy, mPy); + double mPz = mean(tPz), sPz = std(tPz, mPz); + System.out.printf("[%s] track%d momentum pull: Px mean=%.3f std=%.3f | Py mean=%.3f std=%.3f | Pz mean=%.3f std=%.3f%n", + mode, i + 1, mPx, sPx, mPy, sPy, mPz, sPz); + assertTrue(mode + " track" + (i + 1) + " Px pull mean should be near 0, got " + mPx, FastMath.abs(mPx) < 0.15); + assertTrue(mode + " track" + (i + 1) + " Py pull mean should be near 0, got " + mPy, FastMath.abs(mPy) < 0.15); + assertTrue(mode + " track" + (i + 1) + " Pz pull mean should be near 0, got " + mPz, FastMath.abs(mPz) < 0.15); + assertTrue(mode + " track" + (i + 1) + " Px pull std should be near 1, got " + sPx, sPx > 0.8 && sPx < 1.2); + assertTrue(mode + " track" + (i + 1) + " Py pull std should be near 1, got " + sPy, sPy > 0.8 && sPy < 1.2); + assertTrue(mode + " track" + (i + 1) + " Pz pull std should be near 1, got " + sPz, sPz > 0.8 && sPz < 1.2); + } + + assertTrue(mode + " pull X mean should be near 0, got " + meanX, FastMath.abs(meanX) < 0.15); + assertTrue(mode + " pull Y mean should be near 0, got " + meanY, FastMath.abs(meanY) < 0.15); + assertTrue(mode + " pull Z mean should be near 0, got " + meanZ, FastMath.abs(meanZ) < 0.15); + assertTrue(mode + " pull X std should be near 1, got " + stdX, stdX > 0.8 && stdX < 1.2); + assertTrue(mode + " pull Y std should be near 1, got " + stdY, stdY > 0.8 && stdY < 1.2); + assertTrue(mode + " pull Z std should be near 1, got " + stdZ, stdZ > 0.8 && stdZ < 1.2); + assertTrue(mode + " mean chi2/ndf should be near 1, got " + meanChi2Ndf, + meanChi2Ndf > 0.5 && meanChi2Ndf < 1.5); + } + } + + /** + * Toy-MC test of the "upstream track-covariance underestimate" hypothesis for why the + * real-MC beam-momentum-constrained N-track fit shows a much worse chi2/ndf and broader + * per-track momentum pulls than the unconstrained fit, despite + * {@link #testNTrackBeamMomentumConstraintSmearedPulls} showing the fitter itself is + * statistically correct given accurately-covaried inputs. Unlike that test, this one + * deliberately mis-calibrates the input: toy tracks are smeared by + * {@code miscalFactor * trackSigma} (the TRUE scatter) while the fitter is still told the + * covariance is {@code trackCov} (the un-scaled, too-small covariance) -- simulating a + * track fit that under-reports its own parameter uncertainty by a fixed factor, identically + * for every parameter and every track. + *

+ * Result (2026-09-06): this UNIFORM mis-calibration does NOT reproduce the real-data + * pattern -- chi2/ndf inflates by essentially the same factor for the unconstrained, soft-, + * and hard-constrained fits (all ~3.7-3.9x at miscalFactor=2.0), and per-track momentum + * pulls do not broaden unconstrained -> soft -> hard the way they do on real data. This + * falsifies the simplest form of the hypothesis: a globally-uniform covariance underestimate + * inflates chi2 = r^T*C^-1*r by the same factor regardless of how many constraints are + * stacked on top, so it cannot by itself explain why the real beam-momentum-constrained fit + * is so much worse than the unconstrained one. See the assertions below and the printed + * per-{@code miscalFactor} diagnostics for the actual numbers; a real explanation likely + * needs a NON-uniform effect (specific parameters/tracks under-covaried more than others, a + * missing off-diagonal/correlation term, or a systematic bias rather than pure extra + * variance) rather than a single global scale factor. + */ + public void testNTrackBeamMomentumConstraintMiscalibratedCovariancePulls() { + System.out.println("\n=== testNTrackBeamMomentumConstraintMiscalibratedCovariancePulls ===\n"); + + double pBeam = 3.74; + double rotAngle = -0.0305; + double beamPx = pBeam * FastMath.cos(rotAngle); + double beamPy = -pBeam * FastMath.sin(rotAngle); + double beamPz = 0.0; + + double dpOverP = 1e-2; + double sigmaTheta = 100e-6; + double sigmaL = dpOverP * pBeam; + double sigmaT = sigmaTheta * pBeam; + double cosR = FastMath.cos(rotAngle); + double sinR = FastMath.sin(rotAngle); + double sL2 = sigmaL * sigmaL; + double sT2 = sigmaT * sigmaT; + RealMatrix beamPCov = MatrixUtils.createRealMatrix(3, 3); + beamPCov.setEntry(0, 0, sL2 * cosR * cosR + sT2 * sinR * sinR); + beamPCov.setEntry(0, 1, (sT2 - sL2) * sinR * cosR); + beamPCov.setEntry(1, 0, (sT2 - sL2) * sinR * cosR); + beamPCov.setEntry(1, 1, sL2 * sinR * sinR + sT2 * cosR * cosR); + beamPCov.setEntry(2, 2, sT2); + RealVector beamP = MatrixUtils.createRealVector(new double[]{beamPx, beamPy, beamPz}); + RealMatrix beamPCovChol = new CholeskyDecomposition(beamPCov).getL(); + + double xV = 0.2, yV = -0.1, zV = 3.0; + double px1 = 1.8, py1 = 0.25, pz1 = 0.15; + double px2 = 1.2, py2 = -0.20, pz2 = -0.10; + double px3 = beamPx - px1 - px2; + double py3 = beamPy - py1 - py2; + double pz3 = -pz1 - pz2; + double[] truthPx = {px1, px2, px3}; + double[] truthPy = {py1, py2, py3}; + double[] truthPz = {pz1, pz2, pz3}; + + // "Reported" covariance -- what the fitter is told, unchanged by miscalFactor below. + double d0Err = 0.03, phi0Err = 0.003, omegaErr = 5e-6, z0Err = 0.03, tanLErr = 0.003; + RealMatrix trackCov = createTrackCovariance(d0Err, phi0Err, omegaErr, z0Err, tanLErr); + double[] trackSigma = {d0Err, phi0Err, omegaErr, z0Err, tanLErr}; + + java.util.Random rng = new java.util.Random(271828); + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(B_FIELD); + int nToys = 500; + + // miscalFactor = (TRUE smearing sigma) / (reported sigma); 1.0 is the calibrated + // baseline sanity check (should give ~1 chi2/ndf and ~1 pull width for ALL THREE modes, + // including the unconstrained fit, which testNTrackBeamMomentumConstraintSmearedPulls + // doesn't cover); values above 1.0 simulate an under-estimated track covariance. + double[] miscalFactors = {1.0, 1.3, 1.6, 2.0}; + double baselineChi2Unc = 1.0, baselineChi2Soft = 1.0, baselineChi2Hard = 1.0; + + for (double miscalFactor : miscalFactors) { + double[] trueSigma = new double[5]; + for (int i = 0; i < 5; i++) trueSigma[i] = trackSigma[i] * miscalFactor; + + double[] chi2NdfUnc = new double[nToys], chi2NdfSoft = new double[nToys], chi2NdfHard = new double[nToys]; + int nOkUnc = 0, nOkSoft = 0, nOkHard = 0; + double[][] trackPullUnc = new double[9][nToys]; + double[][] trackPullSoft = new double[9][nToys]; + double[][] trackPullHard = new double[9][nToys]; + + for (int toy = 0; toy < nToys; toy++) { + TrackParams e1Truth = createExactTrackThroughPoint(xV, yV, zV, px1, py1, pz1, -1, B_FIELD, trackCov); + TrackParams e2Truth = createExactTrackThroughPoint(xV, yV, zV, px2, py2, pz2, -1, B_FIELD, trackCov); + TrackParams posTruth = createExactTrackThroughPoint(xV, yV, zV, px3, py3, pz3, 1, B_FIELD, trackCov); + + // Smear by the TRUE (larger) sigma, but hand the fitter the ORIGINAL (smaller) + // trackCov as the reported uncertainty -- the deliberate mis-calibration. + List tracks = new ArrayList<>(); + tracks.add(smearTrack(e1Truth, trueSigma, trackCov, rng)); + tracks.add(smearTrack(e2Truth, trueSigma, trackCov, rng)); + tracks.add(smearTrack(posTruth, trueSigma, trackCov, rng)); + + RealVector beamNoise = MatrixUtils.createRealVector( + new double[]{rng.nextGaussian(), rng.nextGaussian(), rng.nextGaussian()}); + RealVector smearedBeamP = beamP.add(beamPCovChol.operate(beamNoise)); + + FitResult resUnc = fitter.fitBillior1985(tracks, null, null); + FitResult resSoft = fitter.fitSoftConstrained(tracks, null, null, smearedBeamP, beamPCov, 10, 1e-6); + FitResult resHard = fitter.fitLagrangeMultiplier(tracks, null, null, beamP, 10, 1e-6); + + if (resUnc != null && resUnc.ndf > 0) { + chi2NdfUnc[nOkUnc] = resUnc.chi2 / resUnc.ndf; + fillTrackPulls(trackPullUnc, nOkUnc, resUnc, truthPx, truthPy, truthPz); + nOkUnc++; + } + if (resSoft != null && resSoft.ndf > 0) { + chi2NdfSoft[nOkSoft] = resSoft.chi2 / resSoft.ndf; + fillTrackPulls(trackPullSoft, nOkSoft, resSoft, truthPx, truthPy, truthPz); + nOkSoft++; + } + if (resHard != null && resHard.ndf > 0) { + chi2NdfHard[nOkHard] = resHard.chi2 / resHard.ndf; + fillTrackPulls(trackPullHard, nOkHard, resHard, truthPx, truthPy, truthPz); + nOkHard++; + } + } + + double meanChi2Unc = mean(java.util.Arrays.copyOf(chi2NdfUnc, nOkUnc)); + double meanChi2Soft = mean(java.util.Arrays.copyOf(chi2NdfSoft, nOkSoft)); + double meanChi2Hard = mean(java.util.Arrays.copyOf(chi2NdfHard, nOkHard)); + double meanPullStdUnc = meanTrackPullStd(trackPullUnc, nOkUnc); + double meanPullStdSoft = meanTrackPullStd(trackPullSoft, nOkSoft); + double meanPullStdHard = meanTrackPullStd(trackPullHard, nOkHard); + + System.out.printf("[miscalFactor=%.1f] chi2/ndf: unconstrained=%.3f soft=%.3f hard=%.3f%n", + miscalFactor, meanChi2Unc, meanChi2Soft, meanChi2Hard); + System.out.printf("[miscalFactor=%.1f] mean per-track momentum pull std: unconstrained=%.3f soft=%.3f hard=%.3f%n", + miscalFactor, meanPullStdUnc, meanPullStdSoft, meanPullStdHard); + + if (miscalFactor == 1.0) { + baselineChi2Unc = meanChi2Unc; + baselineChi2Soft = meanChi2Soft; + baselineChi2Hard = meanChi2Hard; + assertTrue("calibrated-baseline unconstrained chi2/ndf should be near 1, got " + meanChi2Unc, + meanChi2Unc > 0.7 && meanChi2Unc < 1.3); + assertTrue("calibrated-baseline soft chi2/ndf should be near 1, got " + meanChi2Soft, + meanChi2Soft > 0.7 && meanChi2Soft < 1.3); + assertTrue("calibrated-baseline hard chi2/ndf should be near 1, got " + meanChi2Hard, + meanChi2Hard > 0.7 && meanChi2Hard < 1.3); + assertTrue("calibrated-baseline mean per-track pull std should be near 1, got " + meanPullStdUnc, + meanPullStdUnc > 0.8 && meanPullStdUnc < 1.2); + assertTrue("calibrated-baseline mean per-track pull std should be near 1, got " + meanPullStdSoft, + meanPullStdSoft > 0.8 && meanPullStdSoft < 1.2); + assertTrue("calibrated-baseline mean per-track pull std should be near 1, got " + meanPullStdHard, + meanPullStdHard > 0.8 && meanPullStdHard < 1.2); + } + + if (miscalFactor == miscalFactors[miscalFactors.length - 1]) { + // Result (2026-09-06): a UNIFORM covariance underestimate -- the same factor + // applied to all 5 track parameters, identically for all 3 tracks -- does NOT + // reproduce the differential amplification seen on real trident MC (there, + // unconstrained chi2/ndf~2 but soft/hard~10-15, i.e. constrained >> unconstrained). + // Here all three modes inflate by essentially the SAME factor, and per-track + // pulls do not broaden going unconstrained -> soft -> hard as they do on real + // data. This falsifies the simplest version of the "upstream covariance is just + // globally too small" hypothesis: chi2 = r^T*C^-1*r scales with the mismatch + // factor the same way regardless of how many constraints are stacked on top, so + // a real, non-uniform effect (e.g. a covariance underestimate concentrated in + // specific parameters/tracks, a missing correlation/off-diagonal term, or a + // systematic bias rather than pure extra variance) is needed to explain the real + // data -- not just "the same covariance underestimate everywhere, scaled up". + System.out.printf("[miscalFactor=%.1f] chi2/ndf inflation vs calibrated baseline: " + + "unconstrained x%.2f soft x%.2f hard x%.2f (uniform mis-calibration " + + "inflates all three modes comparably -- does not reproduce real-data " + + "differential amplification)%n", miscalFactor, + meanChi2Unc / baselineChi2Unc, meanChi2Soft / baselineChi2Soft, meanChi2Hard / baselineChi2Hard); + double ratioUnc = meanChi2Unc / baselineChi2Unc; + double ratioSoft = meanChi2Soft / baselineChi2Soft; + double ratioHard = meanChi2Hard / baselineChi2Hard; + assertTrue("uniform mis-calibration should inflate soft chi2/ndf comparably to " + + "(within 30% of) the unconstrained fit, got soft x" + ratioSoft + " unconstrained x" + ratioUnc, + FastMath.abs(ratioSoft - ratioUnc) / ratioUnc < 0.3); + assertTrue("uniform mis-calibration should inflate hard chi2/ndf comparably to " + + "(within 30% of) the unconstrained fit, got hard x" + ratioHard + " unconstrained x" + ratioUnc, + FastMath.abs(ratioHard - ratioUnc) / ratioUnc < 0.3); + } + } + } + + /** + * {@code fitBillior1985} (one-shot analytic Schur-complement elimination of each track's + * own 5 perigee parameters, no outer Newton loop) and {@code fitSoftConstrained} with the + * momentum constraint disabled ({@code fourMomentumConstraint == null}, so + * {@code nMomConstraints == 0} per its own guard) are two different linear-algebra routes + * to the *same* unconstrained vertex chi2 -- one-shot elimination vs. iterative + * Newton-Raphson on the full {@code [vertex, track1, track2, ...]} joint state. On + * identical input tracks they should therefore agree on vertex position, chi2, ndf, and + * vertex covariance, up to whatever residual nonlinearity a single linearization pass + * (Billior) misses relative to a fully re-linearized Newton solve (free-track). This is a + * fitter-vs-fitter numerical agreement check, not a physics validation -- both methods see + * literally the same smeared tracks each toy, so there is no notion of a "truth" pull here. + */ + public void testBilliorVsSoftConstrainedUnconstrainedAgreement() { + System.out.println("\n=== testBilliorVsSoftConstrainedUnconstrainedAgreement ===\n"); + + double xV = 0.2, yV = -0.1, zV = 3.0; + double px1 = 1.8, py1 = 0.25, pz1 = 0.15; + double px2 = 1.2, py2 = -0.20, pz2 = -0.10; + double px3 = 1.0 - px1 - px2; + double py3 = -0.05 - py1 - py2; + double pz3 = -pz1 - pz2; + + double d0Err = 0.03, phi0Err = 0.003, omegaErr = 5e-6, z0Err = 0.03, tanLErr = 0.003; + RealMatrix trackCov = createTrackCovariance(d0Err, phi0Err, omegaErr, z0Err, tanLErr); + double[] trackSigma = {d0Err, phi0Err, omegaErr, z0Err, tanLErr}; + + java.util.Random rng = new java.util.Random(90210); + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(B_FIELD); + int nToys = 200; + + double maxVertexDiff = 0, maxChi2RelDiff = 0, maxCovDiff = 0; + int nBothOk = 0; + + for (int toy = 0; toy < nToys; toy++) { + TrackParams e1Truth = createExactTrackThroughPoint(xV, yV, zV, px1, py1, pz1, -1, B_FIELD, trackCov); + TrackParams e2Truth = createExactTrackThroughPoint(xV, yV, zV, px2, py2, pz2, -1, B_FIELD, trackCov); + TrackParams posTruth = createExactTrackThroughPoint(xV, yV, zV, px3, py3, pz3, 1, B_FIELD, trackCov); + + List tracks = new ArrayList<>(); + tracks.add(smearTrack(e1Truth, trackSigma, trackCov, rng)); + tracks.add(smearTrack(e2Truth, trackSigma, trackCov, rng)); + tracks.add(smearTrack(posTruth, trackSigma, trackCov, rng)); + + FitResult resBillior = fitter.fitBillior1985(tracks, null, null); + FitResult resSoft = fitter.fitSoftConstrained(tracks, null, null, null, null, 20, 1e-10); + + if (resBillior == null || resSoft == null || resBillior.ndf <= 0 || resSoft.ndf <= 0) { + continue; + } + nBothOk++; + + assertEquals("ndf should match exactly (both 2*nTracks-3 with no momentum constraint)", + resBillior.ndf, resSoft.ndf); + + for (int i = 0; i < 3; i++) { + double diff = FastMath.abs(resBillior.vertex.getEntry(i) - resSoft.vertex.getEntry(i)); + maxVertexDiff = FastMath.max(maxVertexDiff, diff); + for (int j = 0; j < 3; j++) { + double covDiff = FastMath.abs(resBillior.vertexCov.getEntry(i, j) - resSoft.vertexCov.getEntry(i, j)); + maxCovDiff = FastMath.max(maxCovDiff, covDiff); + } + } + double chi2RelDiff = FastMath.abs(resBillior.chi2 - resSoft.chi2) + / FastMath.max(1e-12, FastMath.abs(resBillior.chi2)); + if (chi2RelDiff > maxChi2RelDiff) { + System.out.printf(" toy=%d chi2Billior=%.6f chi2Soft=%.6f relDiff=%.4f vBillior=%s vSoft=%s%n", + toy, resBillior.chi2, resSoft.chi2, chi2RelDiff, resBillior.vertex, resSoft.vertex); + } + maxChi2RelDiff = FastMath.max(maxChi2RelDiff, chi2RelDiff); + } + + System.out.printf("nBothOk=%d/%d maxVertexDiff=%.3e mm maxChi2RelDiff=%.3e maxVertexCovDiff=%.3e%n", + nBothOk, nToys, maxVertexDiff, maxChi2RelDiff, maxCovDiff); + + assertTrue("expect most toys to converge for both methods, got " + nBothOk + "/" + nToys, + nBothOk > nToys * 0.9); + assertTrue("Billior and free-track vertex position should agree closely, got max diff " + + maxVertexDiff + " mm", maxVertexDiff < 1e-3); + assertTrue("Billior and free-track chi2 should agree closely, got max relative diff " + maxChi2RelDiff, + maxChi2RelDiff < 1e-2); + assertTrue("Billior and free-track vertex covariance should agree closely, got max diff " + maxCovDiff, + maxCovDiff < 1e-3); + } + + /** + * Tests the hypothesis that the broad (but unbiased) Kalman-minus-Billoir V0 residuals seen + * in real data come from Billoir's fit being only a fixed 1-or-2-linearization procedure + * (single Newton step, plus the driver's one {@code shiftTracksToVertex} reprojection+refit + * pass -- see {@code HpsReconParticleDriver.fitVertex}), while Kalman's {@code fit()} + * re-linearizes the same exact nonlinear track-vertex geometry every iteration until + * {@code tolerance}. Both algorithms solve the same underlying nonlinear geometric + * constraint (a helix's distance-of-closest-approach to a vertex point), just in different + * parametrizations, so truncating {@code fit()}'s own iteration count at 1 or 2 is a faithful + * stand-in for "linearize once" / "linearize, correct once" without needing to reimplement + * Billoir's theta/phiv/rho algebra -- AS LONG AS the linearization point matches Billoir's + * actual one. {@code BilliorVertexer}'s {@code _v0} initial guess is a hardcoded {0,0,0} + * (never updated from the tracks -- the one line that would update it, + * {@code BilliorVertexer.java:470}, is commented out), so the 1-/2-pass fits below are + * forced to start from a fixed {@code initialVertex=(0,0,0)} rather than {@code fit()}'s own + * (much better, track-averaged) default initial guess. + *

+ * Result (2026-09-15): DISPROVEN, quantitatively, in two stages. + *

    + *
  1. With {@code fit()}'s own smart default initial guess (track-averaged d0/z0, already + * close to the true vertex), 1-pass vs. fully-converged agree to sub-micron precision even + * at zV=60mm -- "one linearization" is already exact enough given a good starting point.
    2. + *
  2. Forcing the linearization point to Billoir's actual fixed {@code (0,0,0)} (the + * like-for-like comparison, since {@code BilliorVertexer._v0} is a hardcoded constant, never + * updated from the tracks -- the line that would update it, {@code + * BilliorVertexer.java:470}, is commented out) does produce decay-length-dependent, unbiased + * (mean always << std) scatter in the 1-pass-only vertex, as expected. But even pushed to + * an extreme regime (125-260 MeV tracks -- much softer/more-curved than typical, 200mm decay + * length -- far beyond a typical prompt/short-lived V0), the 1-pass z scatter only reaches + * ~10 microns (std 0.0103mm at zV=200mm, growing smoothly from ~0 at zV=0). The 2-pass + * correction -- what {@code HpsReconParticleDriver.fitVertex}'s {@code shiftTracksToVertex} + * reprojection-and-refit actually does in production -- brings this back down to sub-micron + * (std < 0.0005mm) at every decay length tested, because reprojecting to Billoir's own + * pass-1 vertex (already close to truth) makes the second linearization point good enough.
    3. + *
+ * Conclusion: real Billoir's actual 2-pass procedure predicts a linearization-driven + * Kalman-minus-Billoir scatter at most ~1 micron even in this exaggerated regime -- two to + * three orders of magnitude below the ~1.6mm z scatter seen in real V0 data. The + * "single/double linearization vs. fully-converged nonlinear iteration" mechanism, while a + * real and confirmed structural difference between the two fitters, is NOT large enough to + * explain the observed real-data residual broadening. The cause must lie elsewhere (e.g. a + * genuine difference in how track parameters/covariances are consumed or converted between + * the two paths, not in how many times the same geometry is linearized). + */ + public void testBilliorLinearizationVsKalmanConvergedVsDecayLength() { + System.out.println("\n=== testBilliorLinearizationVsKalmanConvergedVsDecayLength ===\n"); + + double xV = 0.1, yV = -0.05; + double px1 = 0.25, py1 = 0.05, pz1 = 0.02; // e-, soft (~260 MeV) -- more curvature/mm decay length + double px2 = 0.12, py2 = -0.03, pz2 = -0.01; // e+, soft (~125 MeV) + + double d0Err = 0.03, phi0Err = 0.003, omegaErr = 5e-6, z0Err = 0.03, tanLErr = 0.003; + RealMatrix trackCov = createTrackCovariance(d0Err, phi0Err, omegaErr, z0Err, tanLErr); + double[] trackSigma = {d0Err, phi0Err, omegaErr, z0Err, tanLErr}; + + double[] decayLengths = {0.0, 20.0, 50.0, 100.0, 150.0, 200.0}; + int nToys = 300; + + GainMatrixVertexer fitter = new GainMatrixVertexer(B_FIELD); + java.util.Random rng = new java.util.Random(20260915); + + System.out.printf("%8s %8s %10s %10s %10s | %10s %10s %10s%n", + "zV(mm)", "nOk", "meanDz_1p", "stdDz_1p", "stdDx_1p", "meanDz_2p", "stdDz_2p", "stdDx_2p"); + + double prevStdDz1p = -1, prevStdDz2p = -1; + for (double zV : decayLengths) { + double[] dx1p = new double[nToys], dy1p = new double[nToys], dz1p = new double[nToys]; + double[] dx2p = new double[nToys], dy2p = new double[nToys], dz2p = new double[nToys]; + int nOk = 0; + + for (int toy = 0; toy < nToys; toy++) { + TrackParams eTruth = createExactTrackThroughPoint(xV, yV, zV, px1, py1, pz1, -1, B_FIELD, trackCov); + TrackParams pTruth = createExactTrackThroughPoint(xV, yV, zV, px2, py2, pz2, 1, B_FIELD, trackCov); + + List tracks = new ArrayList<>(); + tracks.add(smearTrack(eTruth, trackSigma, trackCov, rng)); + tracks.add(smearTrack(pTruth, trackSigma, trackCov, rng)); + + RealVector zeroInit = MatrixUtils.createRealVector(new double[]{0.0, 0.0, 0.0}); + FitResult resFull = fitter.fit(tracks, zeroInit, null, null, null, null, null, null, 20, 1e-10); + FitResult res1p = fitter.fit(tracks, zeroInit, null, null, null, null, null, null, 1, 1e-10); + FitResult res2p = fitter.fit(tracks, zeroInit, null, null, null, null, null, null, 2, 1e-10); + + if (resFull == null || res1p == null || res2p == null + || resFull.ndf <= 0 || res1p.ndf <= 0 || res2p.ndf <= 0) { + continue; + } + + dx1p[nOk] = res1p.vertex.getEntry(0) - resFull.vertex.getEntry(0); + dy1p[nOk] = res1p.vertex.getEntry(1) - resFull.vertex.getEntry(1); + dz1p[nOk] = res1p.vertex.getEntry(2) - resFull.vertex.getEntry(2); + dx2p[nOk] = res2p.vertex.getEntry(0) - resFull.vertex.getEntry(0); + dy2p[nOk] = res2p.vertex.getEntry(1) - resFull.vertex.getEntry(1); + dz2p[nOk] = res2p.vertex.getEntry(2) - resFull.vertex.getEntry(2); + nOk++; + } + + dx1p = java.util.Arrays.copyOf(dx1p, nOk); + dy1p = java.util.Arrays.copyOf(dy1p, nOk); + dz1p = java.util.Arrays.copyOf(dz1p, nOk); + dx2p = java.util.Arrays.copyOf(dx2p, nOk); + dy2p = java.util.Arrays.copyOf(dy2p, nOk); + dz2p = java.util.Arrays.copyOf(dz2p, nOk); + + double meanDz1p = mean(dz1p), stdDz1p = std(dz1p, meanDz1p), stdDx1p = std(dx1p, mean(dx1p)); + double meanDz2p = mean(dz2p), stdDz2p = std(dz2p, meanDz2p), stdDx2p = std(dx2p, mean(dx2p)); + + System.out.printf("%8.1f %8d %10.5f %10.5f %10.5f | %10.5f %10.5f %10.5f%n", + zV, nOk, meanDz1p, stdDz1p, stdDx1p, meanDz2p, stdDz2p, stdDx2p); + + assertTrue("expect most toys to converge for all three fits at zV=" + zV + + ", got " + nOk + "/" + nToys, nOk > nToys * 0.9); + // (The 1-pass-only residual has a genuine deterministic Newton-truncation bias on + // top of any smearing-driven scatter, so no unbiasedness check is applied to it -- + // only the production-realistic 2-pass number below matters for the real comparison.) + // The production-realistic 2-pass correction stays negligible (sub-micron) even in + // this exaggerated regime -- confirms the linearization-count mechanism cannot + // explain a mm-scale real-data residual. + assertTrue("2-pass z scatter should stay sub-micron at zV=" + zV + ", got stdDz2p=" + stdDz2p, + stdDz2p < 1e-3); + if (zV == 0.0) { + assertTrue("at zV=0 all three fits should agree to sub-micron precision, got stdDz1p=" + stdDz1p, + stdDz1p < 1e-3); + } + prevStdDz1p = stdDz1p; + prevStdDz2p = stdDz2p; + } + + // Even in this deliberately exaggerated regime (soft, high-curvature tracks; 200mm decay + // length), the 1-pass-only linearization error should stay far below the ~1.6mm z scatter + // seen in real data -- confirming the mechanism this test targets is not the explanation. + assertTrue("expect final 1-pass z scatter to stay far below the real-data 1.6mm scale, got " + + prevStdDz1p, prevStdDz1p < 0.1); + } + + /** + * Follow-up to {@link #testNTrackBeamMomentumConstraintMiscalibratedCovariancePulls}: that + * test showed a UNIFORM covariance underestimate (all 5 track params, all 3 tracks) does + * NOT reproduce the real-data differential amplification (constrained chi2/ndf >> + * unconstrained). This test targets a NON-uniform mis-calibration instead: only {@code + * omega} (curvature, the parameter that directly sets pT and therefore the beam-momentum + * sum) is under-covaried by {@code miscalFactor}; {@code d0/phi0/z0/tanLambda} keep their + * correctly-reported uncertainty. + *

+ * Result (2026-09-06): unlike the uniform case, this DOES reproduce the real-data + * differential amplification in chi2/ndf. Across {@code miscalFactor} 1-5, the unconstrained + * fit's chi2/ndf stays flat (~0.93-1.03x baseline -- it barely depends on omega's absolute + * scale, only its ratio between tracks for the vertex position), while soft inflates up to + * 6.3x and hard up to 7.5x baseline at miscalFactor=5, with hard consistently inflating + * faster than soft at every factor tested. This matches the qualitative (and roughly + * quantitative) real-data pattern well: omega specifically being under-covaried, not a + * uniform covariance scale-down, is a much better candidate mechanism. + *

+ * However, the per-track momentum PULL WIDTH pattern does NOT match real data here: pull + * std grows fastest for the UNCONSTRAINED fit (0.99 -> 3.58 by miscalFactor=5) and + * LEAST for soft/hard (0.99 -> ~2.4-2.5) -- backwards from real data, where pull width + * grows going unconstrained -> soft -> hard. Plausible reason: the beam-momentum + * constraint pulls each track's fitted momentum toward values consistent with the (correctly + * modeled) total-momentum conservation, partially correcting for a single track's noisy + * omega -- a regularization effect that the real upstream mechanism evidently does NOT + * share. So omega-only under-covariance explains the chi2/ndf escalation but not the + * per-track pull broadening; the real effect is likely omega-under-covariance PLUS + * something else (e.g. a missing correlation between tracks' omega, which would defeat this + * regularization effect) rather than omega-only in isolation. + */ + public void testNTrackBeamMomentumConstraintOmegaOnlyMiscalibratedCovariancePulls() { + System.out.println("\n=== testNTrackBeamMomentumConstraintOmegaOnlyMiscalibratedCovariancePulls ===\n"); + + double pBeam = 3.74; + double rotAngle = -0.0305; + double beamPx = pBeam * FastMath.cos(rotAngle); + double beamPy = -pBeam * FastMath.sin(rotAngle); + double beamPz = 0.0; + + double dpOverP = 1e-2; + double sigmaTheta = 100e-6; + double sigmaL = dpOverP * pBeam; + double sigmaT = sigmaTheta * pBeam; + double cosR = FastMath.cos(rotAngle); + double sinR = FastMath.sin(rotAngle); + double sL2 = sigmaL * sigmaL; + double sT2 = sigmaT * sigmaT; + RealMatrix beamPCov = MatrixUtils.createRealMatrix(3, 3); + beamPCov.setEntry(0, 0, sL2 * cosR * cosR + sT2 * sinR * sinR); + beamPCov.setEntry(0, 1, (sT2 - sL2) * sinR * cosR); + beamPCov.setEntry(1, 0, (sT2 - sL2) * sinR * cosR); + beamPCov.setEntry(1, 1, sL2 * sinR * sinR + sT2 * cosR * cosR); + beamPCov.setEntry(2, 2, sT2); + RealVector beamP = MatrixUtils.createRealVector(new double[]{beamPx, beamPy, beamPz}); + RealMatrix beamPCovChol = new CholeskyDecomposition(beamPCov).getL(); + + double xV = 0.2, yV = -0.1, zV = 3.0; + double px1 = 1.8, py1 = 0.25, pz1 = 0.15; + double px2 = 1.2, py2 = -0.20, pz2 = -0.10; + double px3 = beamPx - px1 - px2; + double py3 = beamPy - py1 - py2; + double pz3 = -pz1 - pz2; + double[] truthPx = {px1, px2, px3}; + double[] truthPy = {py1, py2, py3}; + double[] truthPz = {pz1, pz2, pz3}; + + // "Reported" covariance -- what the fitter is told, unchanged by miscalFactor below. + double d0Err = 0.03, phi0Err = 0.003, omegaErr = 5e-6, z0Err = 0.03, tanLErr = 0.003; + RealMatrix trackCov = createTrackCovariance(d0Err, phi0Err, omegaErr, z0Err, tanLErr); + double[] trackSigma = {d0Err, phi0Err, omegaErr, z0Err, tanLErr}; + final int OMEGA_IDX = 2; + + java.util.Random rng = new java.util.Random(161803); + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(B_FIELD); + int nToys = 500; + + // Only omega's TRUE smearing sigma is scaled by miscalFactor; d0/phi0/z0/tanLambda are + // smeared at their correctly-reported scale (factor 1) in every case. + double[] miscalFactors = {1.0, 1.3, 1.6, 2.0, 3.0, 5.0}; + double baselineChi2Unc = 1.0, baselineChi2Soft = 1.0, baselineChi2Hard = 1.0; + + for (double miscalFactor : miscalFactors) { + double[] trueSigma = java.util.Arrays.copyOf(trackSigma, 5); + trueSigma[OMEGA_IDX] = trackSigma[OMEGA_IDX] * miscalFactor; + + double[] chi2NdfUnc = new double[nToys], chi2NdfSoft = new double[nToys], chi2NdfHard = new double[nToys]; + int nOkUnc = 0, nOkSoft = 0, nOkHard = 0; + double[][] trackPullUnc = new double[9][nToys]; + double[][] trackPullSoft = new double[9][nToys]; + double[][] trackPullHard = new double[9][nToys]; + + for (int toy = 0; toy < nToys; toy++) { + TrackParams e1Truth = createExactTrackThroughPoint(xV, yV, zV, px1, py1, pz1, -1, B_FIELD, trackCov); + TrackParams e2Truth = createExactTrackThroughPoint(xV, yV, zV, px2, py2, pz2, -1, B_FIELD, trackCov); + TrackParams posTruth = createExactTrackThroughPoint(xV, yV, zV, px3, py3, pz3, 1, B_FIELD, trackCov); + + List tracks = new ArrayList<>(); + tracks.add(smearTrack(e1Truth, trueSigma, trackCov, rng)); + tracks.add(smearTrack(e2Truth, trueSigma, trackCov, rng)); + tracks.add(smearTrack(posTruth, trueSigma, trackCov, rng)); + + RealVector beamNoise = MatrixUtils.createRealVector( + new double[]{rng.nextGaussian(), rng.nextGaussian(), rng.nextGaussian()}); + RealVector smearedBeamP = beamP.add(beamPCovChol.operate(beamNoise)); + + FitResult resUnc = fitter.fitBillior1985(tracks, null, null); + FitResult resSoft = fitter.fitSoftConstrained(tracks, null, null, smearedBeamP, beamPCov, 10, 1e-6); + FitResult resHard = fitter.fitLagrangeMultiplier(tracks, null, null, beamP, 10, 1e-6); + + if (resUnc != null && resUnc.ndf > 0) { + chi2NdfUnc[nOkUnc] = resUnc.chi2 / resUnc.ndf; + fillTrackPulls(trackPullUnc, nOkUnc, resUnc, truthPx, truthPy, truthPz); + nOkUnc++; + } + if (resSoft != null && resSoft.ndf > 0) { + chi2NdfSoft[nOkSoft] = resSoft.chi2 / resSoft.ndf; + fillTrackPulls(trackPullSoft, nOkSoft, resSoft, truthPx, truthPy, truthPz); + nOkSoft++; + } + if (resHard != null && resHard.ndf > 0) { + chi2NdfHard[nOkHard] = resHard.chi2 / resHard.ndf; + fillTrackPulls(trackPullHard, nOkHard, resHard, truthPx, truthPy, truthPz); + nOkHard++; + } + } + + double meanChi2Unc = mean(java.util.Arrays.copyOf(chi2NdfUnc, nOkUnc)); + double meanChi2Soft = mean(java.util.Arrays.copyOf(chi2NdfSoft, nOkSoft)); + double meanChi2Hard = mean(java.util.Arrays.copyOf(chi2NdfHard, nOkHard)); + double meanPullStdUnc = meanTrackPullStd(trackPullUnc, nOkUnc); + double meanPullStdSoft = meanTrackPullStd(trackPullSoft, nOkSoft); + double meanPullStdHard = meanTrackPullStd(trackPullHard, nOkHard); + + System.out.printf("[omega miscalFactor=%.1f] chi2/ndf: unconstrained=%.3f soft=%.3f hard=%.3f%n", + miscalFactor, meanChi2Unc, meanChi2Soft, meanChi2Hard); + System.out.printf("[omega miscalFactor=%.1f] mean per-track momentum pull std: unconstrained=%.3f soft=%.3f hard=%.3f%n", + miscalFactor, meanPullStdUnc, meanPullStdSoft, meanPullStdHard); + + if (miscalFactor == 1.0) { + baselineChi2Unc = meanChi2Unc; + baselineChi2Soft = meanChi2Soft; + baselineChi2Hard = meanChi2Hard; + assertTrue("calibrated-baseline unconstrained chi2/ndf should be near 1, got " + meanChi2Unc, + meanChi2Unc > 0.7 && meanChi2Unc < 1.3); + assertTrue("calibrated-baseline soft chi2/ndf should be near 1, got " + meanChi2Soft, + meanChi2Soft > 0.7 && meanChi2Soft < 1.3); + assertTrue("calibrated-baseline hard chi2/ndf should be near 1, got " + meanChi2Hard, + meanChi2Hard > 0.7 && meanChi2Hard < 1.3); + } + + double ratioUnc = meanChi2Unc / baselineChi2Unc; + double ratioSoft = meanChi2Soft / baselineChi2Soft; + double ratioHard = meanChi2Hard / baselineChi2Hard; + System.out.printf("[omega miscalFactor=%.1f] chi2/ndf inflation vs calibrated baseline: " + + "unconstrained x%.2f soft x%.2f hard x%.2f%n", miscalFactor, + ratioUnc, ratioSoft, ratioHard); + + if (miscalFactor == miscalFactors[miscalFactors.length - 1]) { + // Core positive result: unlike the uniform mis-calibration in + // testNTrackBeamMomentumConstraintMiscalibratedCovariancePulls, under-covaried + // omega ALONE differentially amplifies the momentum-constrained fits' chi2/ndf + // well beyond the unconstrained fit, and hard inflates more than soft -- both + // qualitatively matching the real-data pattern. + assertTrue("omega-only mis-calibration should leave the unconstrained fit's " + + "chi2/ndf inflation near 1 (it barely depends on omega's absolute " + + "scale), got x" + ratioUnc, ratioUnc < 1.3); + assertTrue("omega-only mis-calibration should inflate soft chi2/ndf well beyond " + + "the unconstrained fit, got soft x" + ratioSoft + " unconstrained x" + ratioUnc, + ratioSoft > 3 * ratioUnc); + assertTrue("omega-only mis-calibration should inflate hard chi2/ndf even more " + + "than soft, got hard x" + ratioHard + " soft x" + ratioSoft, + ratioHard > ratioSoft); + } + } + } + + private static void fillTrackPulls(double[][] trackPulls, int idx, FitResult res, + double[] truthPx, double[] truthPy, double[] truthPz) { + for (int i = 0; i < 3; i++) { + RealVector p = res.trackMomenta.get(i).p; + RealMatrix pCov = res.trackMomenta.get(i).pCov; + trackPulls[3 * i][idx] = (p.getEntry(0) - truthPx[i]) / FastMath.sqrt(pCov.getEntry(0, 0)); + trackPulls[3 * i + 1][idx] = (p.getEntry(1) - truthPy[i]) / FastMath.sqrt(pCov.getEntry(1, 1)); + trackPulls[3 * i + 2][idx] = (p.getEntry(2) - truthPz[i]) / FastMath.sqrt(pCov.getEntry(2, 2)); + } + } + + private static double meanTrackPullStd(double[][] trackPulls, int nOk) { + double sum = 0; + for (double[] arr : trackPulls) { + double[] trimmed = java.util.Arrays.copyOf(arr, nOk); + sum += std(trimmed, mean(trimmed)); + } + return sum / trackPulls.length; + } + + private static TrackParams smearTrack(TrackParams truth, double[] trackSigma, RealMatrix trackCov, java.util.Random rng) { + double[] tp = truth.toArray(); + double[] smearedTp = new double[5]; + for (int i = 0; i < 5; i++) { + smearedTp[i] = tp[i] + trackSigma[i] * rng.nextGaussian(); + } + return new TrackParams(smearedTp[0], smearedTp[1], smearedTp[2], smearedTp[3], smearedTp[4], trackCov); + } + + private static double mean(double[] x) { + double sum = 0; + for (double v : x) sum += v; + return sum / x.length; + } + + private static double std(double[] x, double mean) { + double sum = 0; + for (double v : x) sum += (v - mean) * (v - mean); + return FastMath.sqrt(sum / x.length); + } + + private static double[] absArr(double[] x) { + double[] out = new double[x.length]; + for (int i = 0; i < x.length; i++) out[i] = FastMath.abs(x[i]); + return out; + } + + /** + * Reproduces one specific real-MC event (run 14272, first BADFIT_DEBUG block in + * /tmp/vo_run2.log) directly from its captured perigee parameters/covariances, with + * DEBUG_JOINT_FIT enabled, to inspect the per-iteration JFDEBUG trace. + */ + public void testDebugRealBadEvent() { + double bField = -0.8595999999999999; + RealVector v1Init = MatrixUtils.createRealVector(new double[]{44.2652212072, 1.4093367997, 0.170271548}); + + RealMatrix eleCov = MatrixUtils.createRealMatrix(new double[][]{ + {0.09099249541759491, -4.65152581455186E-4, -6.321477599158243E-7, 0.004269929137080908, -1.8205431842943653E-5}, + {-4.65152581455186E-4, 2.659913434399641E-6, 3.725346697791565E-9, -2.0947405573679134E-5, 9.69749862633762E-8}, + {-6.321477599158243E-7, 3.725346697791565E-9, 7.499869648930346E-12, -2.8541577989926736E-8, 1.3351865446598055E-10}, + {0.004269929137080908, -2.0947405573679134E-5, -2.8541577989926736E-8, 0.0029748319648206234, -2.6774487196234986E-5}, + {-1.8205431842943653E-5, 9.69749862633762E-8, 1.3351865446598055E-10, -2.6774487196234986E-5, 2.78031990319505E-7} + }); + TrackParams eleParams = new TrackParams(0.629338800907135, 0.016672732308506966, 1.2636852625291795E-4, + 2.378530979156494, -0.04880968853831291, eleCov); + + RealMatrix posCov = MatrixUtils.createRealMatrix(new double[][]{ + {0.06338092684745789, -3.4044793574139476E-4, -4.6726222535653505E-7, -0.002420209813863039, 1.005577087198617E-5}, + {-3.4044793574139476E-4, 2.1097148419357836E-6, 2.87417623034969E-9, 1.2600367881532293E-5, -5.876536235405183E-8}, + {-4.6726222535653505E-7, 2.87417623034969E-9, 5.894043499793389E-12, 1.7731609958104855E-8, -8.565938930393813E-11}, + {-0.002420209813863039, 1.2600367881532293E-5, 1.7731609958104855E-8, 0.0019167944556102157, -2.1231198843452148E-5}, + {1.005577087198617E-5, -5.876536235405183E-8, -8.565938930393813E-11, -2.1231198843452148E-5, 2.9569127946160734E-7} + }); + TrackParams posParams = new TrackParams(-0.06583922356367111, 0.03222518786787987, -1.1622635793173686E-4, + -2.1310274600982666, 0.050598885864019394, posCov); + + RealMatrix recoilCov = MatrixUtils.createRealMatrix(new double[][]{ + {0.3589296340942383, -0.005202689673751593, -2.7695679818862118E-5, 0.022159304469823837, -2.742857614066452E-4}, + {-0.005202689673751593, 8.316571620525792E-5, 4.6634096406705794E-7, -2.9148461180739105E-4, 3.763552740565501E-6}, + {-2.7695679818862118E-5, 4.6634096406705794E-7, 2.805736754041277E-9, -1.4865117918816395E-6, 1.9633365155868887E-8}, + {0.022159304469823837, -2.9148461180739105E-4, -1.4865117918816395E-6, 0.008457320742309093, -1.352451363345608E-4}, + {-2.742857614066452E-4, 3.763552740565501E-6, 1.9633365155868887E-8, -1.352451363345608E-4, 2.2341159819916356E-6} + }); + TrackParams recoilParams = new TrackParams(-0.2463390827178955, 0.05816323310136795, 4.706922627519816E-4, + -0.10475388169288635, -0.03560171276330948, recoilCov); + + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(bField); + TrackConstraintVertexFitter.DEBUG_JOINT_FIT = true; + try { + TwoVertexFitResult result = fitter.fitCascadeVertexJoint(eleParams, posParams, recoilParams, v1Init, null); + System.err.println("JFDEBUG FINAL v1=" + result.v1 + " v2=" + result.v2 + + " chi2=" + result.chi2 + " ndf=" + result.ndf); + } finally { + TrackConstraintVertexFitter.DEBUG_JOINT_FIT = false; + } + } + + /** + * Second bad event from the same log, same eMinus/ePlus/v1Init/bField but a different + * recoil-track hypothesis (second BADFIT_DEBUG block in /tmp/vo_run2.log) -- checks + * whether the carried-forward-covariance fix behaves consistently across events. + */ + public void testDebugRealBadEvent2() { + double bField = -0.8595999999999999; + RealVector v1Init = MatrixUtils.createRealVector(new double[]{44.2652212072, 1.4093367997, 0.170271548}); + + RealMatrix eleCov = MatrixUtils.createRealMatrix(new double[][]{ + {0.09099249541759491, -4.65152581455186E-4, -6.321477599158243E-7, 0.004269929137080908, -1.8205431842943653E-5}, + {-4.65152581455186E-4, 2.659913434399641E-6, 3.725346697791565E-9, -2.0947405573679134E-5, 9.69749862633762E-8}, + {-6.321477599158243E-7, 3.725346697791565E-9, 7.499869648930346E-12, -2.8541577989926736E-8, 1.3351865446598055E-10}, + {0.004269929137080908, -2.0947405573679134E-5, -2.8541577989926736E-8, 0.0029748319648206234, -2.6774487196234986E-5}, + {-1.8205431842943653E-5, 9.69749862633762E-8, 1.3351865446598055E-10, -2.6774487196234986E-5, 2.78031990319505E-7} + }); + TrackParams eleParams = new TrackParams(0.629338800907135, 0.016672732308506966, 1.2636852625291795E-4, + 2.378530979156494, -0.04880968853831291, eleCov); + + RealMatrix posCov = MatrixUtils.createRealMatrix(new double[][]{ + {0.06338092684745789, -3.4044793574139476E-4, -4.6726222535653505E-7, -0.002420209813863039, 1.005577087198617E-5}, + {-3.4044793574139476E-4, 2.1097148419357836E-6, 2.87417623034969E-9, 1.2600367881532293E-5, -5.876536235405183E-8}, + {-4.6726222535653505E-7, 2.87417623034969E-9, 5.894043499793389E-12, 1.7731609958104855E-8, -8.565938930393813E-11}, + {-0.002420209813863039, 1.2600367881532293E-5, 1.7731609958104855E-8, 0.0019167944556102157, -2.1231198843452148E-5}, + {1.005577087198617E-5, -5.876536235405183E-8, -8.565938930393813E-11, -2.1231198843452148E-5, 2.9569127946160734E-7} + }); + TrackParams posParams = new TrackParams(-0.06583922356367111, 0.03222518786787987, -1.1622635793173686E-4, + -2.1310274600982666, 0.050598885864019394, posCov); + + RealMatrix recoilCov = MatrixUtils.createRealMatrix(new double[][]{ + {0.22654478251934052, -0.0031680979300290346, -1.2684452485700604E-5, 0.007989047095179558, -3.1170796137303114E-4}, + {-0.0031680979300290346, 5.3549021686194465E-5, 2.268006795702604E-7, -7.062631630105898E-5, 2.9667264698218787E-6}, + {-1.2684452485700604E-5, 2.268006795702604E-7, 1.4096562805931967E-9, -2.4915863150454243E-7, 1.0956304308251674E-8}, + {0.007989047095179558, -7.062631630105898E-5, -2.4915863150454243E-7, 0.012361372821033001, -3.0974321998655796E-4}, + {-3.1170796137303114E-4, 2.9667264698218787E-6, 1.0956304308251674E-8, -3.0974321998655796E-4, 7.928260856715497E-6} + }); + TrackParams recoilParams = new TrackParams(0.680279016494751, 0.14952220022678375, 9.425431489944458E-4, + 0.2652631998062134, 0.04711122438311577, recoilCov); + + TrackConstraintVertexFitter fitter = new TrackConstraintVertexFitter(bField); + TrackConstraintVertexFitter.DEBUG_JOINT_FIT = true; + try { + TwoVertexFitResult result = fitter.fitCascadeVertexJoint(eleParams, posParams, recoilParams, v1Init, null); + System.err.println("JFDEBUG FINAL v1=" + result.v1 + " v2=" + result.v2 + + " chi2=" + result.chi2 + " ndf=" + result.ndf); + } finally { + TrackConstraintVertexFitter.DEBUG_JOINT_FIT = false; + } + } + + /** + * Checks {@code propagateLineToPlane} against a hand-computed crossing point and a + * finite-difference Jacobian, for a line with a non-trivial (correlated, non-diagonal) + * 6x6 covariance. Also checks that the x-row/column of the returned covariance is exactly + * the supplied {@code sigmaXFloor}, not the (exactly-zero) value the Jacobian alone would + * give -- x is fixed to the plane by construction, so the floor is what keeps a caller's + * Kalman prior from permanently pinning that coordinate. + */ + public void testPropagateLineToPlane() { + System.out.println("\n=== testPropagateLineToPlane ===\n"); + + double x0 = 30.0, y0 = 0.4, z0 = -0.2; + double dx = 0.9, dy = 0.15, dz = -0.05; + + RealMatrix cov6 = MatrixUtils.createRealMatrix(6, 6); + double[] sigma = {0.05, 0.03, 0.04, 0.02, 0.015, 0.01}; + java.util.Random rng = new java.util.Random(4242); + // Build a random-but-valid (symmetric positive-definite) covariance: A*A^T scaled by sigmas. + RealMatrix A = MatrixUtils.createRealMatrix(6, 6); + for (int i = 0; i < 6; i++) { + for (int j = 0; j < 6; j++) { + A.setEntry(i, j, rng.nextGaussian() * 0.3); + } + A.setEntry(i, i, A.getEntry(i, i) + 1.0); + } + RealMatrix base = A.multiply(A.transpose()); + for (int i = 0; i < 6; i++) { + for (int j = 0; j < 6; j++) { + cov6.setEntry(i, j, base.getEntry(i, j) * sigma[i] * sigma[j] / FastMath.sqrt(base.getEntry(i, i) * base.getEntry(j, j))); + } + } + + TrackConstraintVertexFitter.LineParams line = + new TrackConstraintVertexFitter.LineParams(x0, y0, z0, dx, dy, dz, cov6); + + double xPlane = -1.1; + double sigmaXFloor = 0.001; + TrackConstraintVertexFitter.LinePlaneProjection proj = + TrackConstraintVertexFitter.propagateLineToPlane(line, xPlane, sigmaXFloor); + + double sExpected = (xPlane - x0) / dx; + double yExpected = y0 + sExpected * dy; + double zExpected = z0 + sExpected * dz; + + assertEquals(xPlane, proj.position.getEntry(0), 1e-12); + assertEquals(yExpected, proj.position.getEntry(1), 1e-9); + assertEquals(zExpected, proj.position.getEntry(2), 1e-9); + + // Finite-difference Jacobian of [yProp, zProp] w.r.t. the 6 line parameters, compared + // against propagateLineToPlane's covariance via J*cov6*J^T. + double h = 1e-6; + RealMatrix Jyz = MatrixUtils.createRealMatrix(2, 6); + for (int k = 0; k < 6; k++) { + double[] plus = line.toArray(); + double[] minus = line.toArray(); + plus[k] += h; + minus[k] -= h; + double[] yzPlus = crossingYZ(plus, xPlane); + double[] yzMinus = crossingYZ(minus, xPlane); + Jyz.setEntry(0, k, (yzPlus[0] - yzMinus[0]) / (2 * h)); + Jyz.setEntry(1, k, (yzPlus[1] - yzMinus[1]) / (2 * h)); + } + RealMatrix covYZExpected = Jyz.multiply(cov6).multiply(Jyz.transpose()); + + assertEquals(covYZExpected.getEntry(0, 0), proj.cov.getEntry(1, 1), 1e-6); + assertEquals(covYZExpected.getEntry(0, 1), proj.cov.getEntry(1, 2), 1e-6); + assertEquals(covYZExpected.getEntry(1, 1), proj.cov.getEntry(2, 2), 1e-6); + + // x is fixed by construction: the Jacobian-only covariance would be exactly zero there, + // but the floor should be substituted in instead. + assertEquals(sigmaXFloor * sigmaXFloor, proj.cov.getEntry(0, 0), 1e-15); + assertEquals(0.0, proj.cov.getEntry(0, 1), 1e-15); + assertEquals(0.0, proj.cov.getEntry(0, 2), 1e-15); + + System.out.printf("position=%s%n", proj.position); + System.out.printf("cov=%s%n", proj.cov); + } + + /** [yProp, zProp] where a line with parameters [x0,y0,z0,dx,dy,dz] crosses x=xPlane. */ + private static double[] crossingYZ(double[] p, double xPlane) { + double s = (xPlane - p[0]) / p[3]; + return new double[]{p[1] + s * p[4], p[2] + s * p[5]}; + } + + /** + * Checks {@code propagateTrackToPlane} (curved-track analog of + * {@code propagateLineToPlane}) two ways: (1) algebraically, that the returned crossing + * point lies exactly on the track's helix circle (independent of the turning-angle + * machinery used internally to find it), and (2) via a finite-difference Jacobian of + * [yProp, zProp] w.r.t. the 5 perigee parameters (obtained by calling the method itself + * at perturbed inputs -- its closed-form position formula and its analytic Jacobian are + * independent derivations, so this is a real cross-check, not a tautology). + */ + public void testPropagateTrackToPlane() { + System.out.println("\n=== testPropagateTrackToPlane ===\n"); + + double d0 = 1.7534739233699383, phi0 = 0.0028232486700802044, + omega = 6.790166806190062E-4, z0 = -0.10407287685488854, + tanLambda = -0.03300521957923669; + + RealMatrix cov5 = MatrixUtils.createRealMatrix(5, 5); + double[] sigma = {0.03, 0.002, 1e-5, 0.02, 0.003}; + java.util.Random rng = new java.util.Random(1234); + RealMatrix A = MatrixUtils.createRealMatrix(5, 5); + for (int i = 0; i < 5; i++) { + for (int j = 0; j < 5; j++) { + A.setEntry(i, j, rng.nextGaussian() * 0.3); + } + A.setEntry(i, i, A.getEntry(i, i) + 1.0); + } + RealMatrix base = A.multiply(A.transpose()); + for (int i = 0; i < 5; i++) { + for (int j = 0; j < 5; j++) { + cov5.setEntry(i, j, base.getEntry(i, j) * sigma[i] * sigma[j] / FastMath.sqrt(base.getEntry(i, i) * base.getEntry(j, j))); + } + } + + TrackParams track = new TrackParams(d0, phi0, omega, z0, tanLambda, cov5); + + double xPlane = -1.1; + double sigmaXFloor = 10.0; + TrackConstraintVertexFitter.LinePlaneProjection proj = + TrackConstraintVertexFitter.propagateTrackToPlane(track, xPlane, sigmaXFloor); + + double R = 1.0 / FastMath.abs(omega); + double xc = FastMath.sin(phi0) * (1.0 / omega - d0); + double yc = -FastMath.cos(phi0) * (1.0 / omega - d0); + double dx = xPlane - xc; + double dy = proj.position.getEntry(1) - yc; + assertEquals(R * R, dx * dx + dy * dy, 1e-6); + + double h = 1e-6; + RealMatrix Jyz = MatrixUtils.createRealMatrix(2, 5); + for (int k = 0; k < 5; k++) { + double[] plus = track.toArray(); + double[] minus = track.toArray(); + plus[k] += h; + minus[k] -= h; + TrackParams tPlus = new TrackParams(plus[0], plus[1], plus[2], plus[3], plus[4], cov5); + TrackParams tMinus = new TrackParams(minus[0], minus[1], minus[2], minus[3], minus[4], cov5); + TrackConstraintVertexFitter.LinePlaneProjection pPlus = + TrackConstraintVertexFitter.propagateTrackToPlane(tPlus, xPlane, sigmaXFloor); + TrackConstraintVertexFitter.LinePlaneProjection pMinus = + TrackConstraintVertexFitter.propagateTrackToPlane(tMinus, xPlane, sigmaXFloor); + Jyz.setEntry(0, k, (pPlus.position.getEntry(1) - pMinus.position.getEntry(1)) / (2 * h)); + Jyz.setEntry(1, k, (pPlus.position.getEntry(2) - pMinus.position.getEntry(2)) / (2 * h)); + } + RealMatrix covYZExpected = Jyz.multiply(cov5).multiply(Jyz.transpose()); + + assertClose(covYZExpected.getEntry(0, 0), proj.cov.getEntry(1, 1)); + assertClose(covYZExpected.getEntry(0, 1), proj.cov.getEntry(1, 2)); + assertClose(covYZExpected.getEntry(1, 1), proj.cov.getEntry(2, 2)); + + assertEquals(sigmaXFloor * sigmaXFloor, proj.cov.getEntry(0, 0), 1e-15); + assertEquals(0.0, proj.cov.getEntry(0, 1), 1e-15); + assertEquals(0.0, proj.cov.getEntry(0, 2), 1e-15); + + System.out.printf("position=%s%n", proj.position); + System.out.printf("cov=%s%n", proj.cov); + } + + /** Relative-tolerance comparison (1e-4), robust across the widely varying magnitudes of + * the covariance entries checked in {@code testPropagateTrackToPlane}. */ + private static void assertClose(double expected, double actual) { + double scale = FastMath.max(1.0, FastMath.abs(expected)); + assertTrue("expected=" + expected + " actual=" + actual, FastMath.abs(expected - actual) <= 1e-4 * scale); + } +} diff --git a/record-util/src/main/java/org/hps/record/StandardCuts.java b/record-util/src/main/java/org/hps/record/StandardCuts.java index bd9a73a1f2..bef96d4f4f 100644 --- a/record-util/src/main/java/org/hps/record/StandardCuts.java +++ b/record-util/src/main/java/org/hps/record/StandardCuts.java @@ -24,6 +24,8 @@ public class StandardCuts { private double maxVertexP; // max chisq prob for V0 vertex fit private double minVertexChisqProb; + // max raw chisq for V0 vertex fit + private double maxVertexChisq; // max chisq prob for vertex fit private double minMollerChisqProb; // max time diff [ns] between the two recon particle clusters in vertex @@ -104,6 +106,9 @@ public double getMaxVertexP() { public void setMinVertexChisqProb(double input) { minVertexChisqProb = input; } + public void setMaxVertexChisq(double input) { + maxVertexChisq = input; + } public void setMinMollerChisqProb(double input) { minMollerChisqProb = input; } @@ -111,6 +116,9 @@ public void setMinMollerChisqProb(double input) { public double getMinVertexChisqProb() { return minVertexChisqProb; } + public double getMaxVertexChisq() { + return maxVertexChisq; + } public double getMinMollerChisqProb() { return minMollerChisqProb; } @@ -193,6 +201,7 @@ public StandardCuts(double ebeam) { maxMatchDy = 20.0; maxVertexClusterDt = 2.0; minVertexChisqProb = 0.00001; + maxVertexChisq = Double.MAX_VALUE; minMollerChisqProb = 0.00001; maxTrackChisqProb = 0.00001; diff --git a/steering-files/src/main/resources/org/hps/steering/recon/PhysicsRun2021MCRecon_KF_WithSpacing_pass5_physics_HitSmear_HitKill_14272.lcsim b/steering-files/src/main/resources/org/hps/steering/recon/PhysicsRun2021MCRecon_KF_WithSpacing_pass5_physics_HitSmear_HitKill_14272.lcsim index 64bb5aaa0a..7e51ce825a 100644 --- a/steering-files/src/main/resources/org/hps/steering/recon/PhysicsRun2021MCRecon_KF_WithSpacing_pass5_physics_HitSmear_HitKill_14272.lcsim +++ b/steering-files/src/main/resources/org/hps/steering/recon/PhysicsRun2021MCRecon_KF_WithSpacing_pass5_physics_HitSmear_HitKill_14272.lcsim @@ -34,6 +34,7 @@ + @@ -206,6 +207,19 @@ BeamspotConstrainedMollerVertices_KF TargetConstrainedMollerCandidates_KF TargetConstrainedMollerVertices_KF + CascadeVertexCandidates + CascadeVertexCandidatesBeamConstrained + NTrackVertexCandidates + NTrackVertexCandidatesBeamConstrained + + + CascadeVertexCandidates + CascadeVertexCandidatesBeamConstrained + NTrackVertexCandidates + NTrackVertexCandidatesBeamConstrained + MCParticle + KalmanFullTracksToMCParticleRelations + ${outputFile}_cascade.txt ${outputFile}_hit_eff.root diff --git a/tracking/src/main/java/org/hps/recon/tracking/CoordinateTransformations.java b/tracking/src/main/java/org/hps/recon/tracking/CoordinateTransformations.java index 8b5955d607..b47fe4334c 100644 --- a/tracking/src/main/java/org/hps/recon/tracking/CoordinateTransformations.java +++ b/tracking/src/main/java/org/hps/recon/tracking/CoordinateTransformations.java @@ -1,5 +1,8 @@ package org.hps.recon.tracking; +import hep.physics.matrix.BasicMatrix; +import hep.physics.matrix.Matrix; +import hep.physics.matrix.MatrixOp; import hep.physics.matrix.SymmetricMatrix; import hep.physics.vec.BasicHep3Matrix; import hep.physics.vec.Hep3Matrix; @@ -79,4 +82,29 @@ public static Hep3Matrix getMatrixInverse() { return _trkToDet.getRotation().getRotationMatrix(); } + /** + * Rotate a general (possibly non-symmetric) 3x3 matrix from tracking frame to detector frame + * via R*m*R^T. Needed for cross-covariance blocks (e.g. Cov(vertex position, momentum)) which + * are not instances of SymmetricMatrix and so cannot use {@link #transformCovarianceToDetector}. + */ + public static Matrix transformMatrixToDetector(Matrix m) { + return rotate(m, getMatrixInverse()); + } + + /** + * Rotate a general (possibly non-symmetric) 3x3 matrix from detector frame to tracking frame + * via R*m*R^T. See {@link #transformMatrixToDetector(Matrix)}. + */ + public static Matrix transformMatrixToTracking(Matrix m) { + return rotate(m, getMatrix()); + } + + private static Matrix rotate(Matrix m, Hep3Matrix rot) { + BasicMatrix r = new BasicMatrix(3, 3); + for (int i = 0; i < 3; i++) + for (int j = 0; j < 3; j++) + r.setElement(i, j, rot.e(i, j)); + return MatrixOp.mult(r, MatrixOp.mult(m, MatrixOp.transposed(r))); + } + }