From 1fb3d0ed64d9ba09d4d25ac91e6a37e6c73d5bcb Mon Sep 17 00:00:00 2001 From: Matt Graham Date: Thu, 3 Sep 2026 09:32:31 -0700 Subject: [PATCH 1/4] Add Kalman gain-matrix vertex fitting (V0, cascade, 3-track, N-track) with comparison tuples Introduces standalone Kalman gain-matrix vertexers (KalmanV0Vertexer, CascadeVertexer, ThreeTrackVertexer, KalmanNTrackVertexer) alongside the legacy Billoir fitter for displaced 3-prong vertex studies, plus tuple drivers to compare the two on truth-matched V0/cascade/trident MC. Fixes several bugs found along the way in KalmanVertexFitterGainMatrix (arc-length sign, |B| in pT, Joseph-form covariance update, per-iteration prior reset) and BilliorVertex (negative chi2 guard), and adds purity-weighted track-to-MCParticle relations so downstream tuple drivers can pick the best-match track. Also adds a two-track Kalman V0 fit option to HpsReconParticleDriver for production-side comparison, and new steering files for the three-electron skim and N-track vertex studies. --- .../MC/TrackToMCParticleRelationsDriver.java | 5 +- .../tuple/CascadeVertexTupleDriver.java | 268 ++ .../NTrackVertexComparisonTupleDriver.java | 268 ++ .../TwoTrackVertexComparisonTupleDriver.java | 239 ++ .../tuple/CascadeVertexTupleDriverTest.java | 102 + .../ThreeElectronTrackingFilter.java | 133 + .../particle/HpsReconParticleDriver.java | 31 + .../recon/particle/ReconParticleDriver.java | 351 ++ .../utils/TrackClusterMatcherMinDistance.java | 5 +- .../utils/TrackTruthRelationsDriver.java | 6 +- .../hps/recon/vertexing/BilliorVertex.java | 70 +- .../hps/recon/vertexing/BilliorVertexer.java | 19 + .../hps/recon/vertexing/CascadeVertexer.java | 224 ++ .../recon/vertexing/KalmanNTrackVertexer.java | 159 + .../hps/recon/vertexing/KalmanV0Vertexer.java | 155 + .../KalmanVertexFitterGainMatrix.java | 3481 +++++++++++++++++ .../recon/vertexing/ThreeTrackVertexer.java | 400 ++ .../recon/vertexing/BilliorVertexerTest.java | 106 + .../recon/vertexing/CascadeVertexerTest.java | 172 + .../recon/vertexing/KalmanV0VertexerTest.java | 166 + .../KalmanVertexFitterGainMatrixTest.java | 1384 +++++++ .../vertexing/ThreeTrackVertexerTest.java | 204 + .../java/org/hps/record/StandardCuts.java | 9 + .../tracking/CoordinateTransformations.java | 28 + 24 files changed, 7973 insertions(+), 12 deletions(-) create mode 100644 analysis/src/main/java/org/hps/analysis/tuple/CascadeVertexTupleDriver.java create mode 100644 analysis/src/main/java/org/hps/analysis/tuple/NTrackVertexComparisonTupleDriver.java create mode 100644 analysis/src/main/java/org/hps/analysis/tuple/TwoTrackVertexComparisonTupleDriver.java create mode 100644 analysis/src/test/java/org/hps/analysis/tuple/CascadeVertexTupleDriverTest.java create mode 100644 recon/src/main/java/org/hps/recon/filtering/ThreeElectronTrackingFilter.java create mode 100644 recon/src/main/java/org/hps/recon/vertexing/CascadeVertexer.java create mode 100644 recon/src/main/java/org/hps/recon/vertexing/KalmanNTrackVertexer.java create mode 100644 recon/src/main/java/org/hps/recon/vertexing/KalmanV0Vertexer.java create mode 100644 recon/src/main/java/org/hps/recon/vertexing/KalmanVertexFitterGainMatrix.java create mode 100644 recon/src/main/java/org/hps/recon/vertexing/ThreeTrackVertexer.java create mode 100644 recon/src/test/java/org/hps/recon/vertexing/BilliorVertexerTest.java create mode 100644 recon/src/test/java/org/hps/recon/vertexing/CascadeVertexerTest.java create mode 100644 recon/src/test/java/org/hps/recon/vertexing/KalmanV0VertexerTest.java create mode 100644 recon/src/test/java/org/hps/recon/vertexing/KalmanVertexFitterGainMatrixTest.java create mode 100644 recon/src/test/java/org/hps/recon/vertexing/ThreeTrackVertexerTest.java 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..f80abadf6f --- /dev/null +++ b/analysis/src/main/java/org/hps/analysis/tuple/CascadeVertexTupleDriver.java @@ -0,0 +1,268 @@ +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.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.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 the {@code 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 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", + "v0UncPX/D", "v0UncPY/D", "v0UncPZ/D", + "recoilPX/D", "recoilPY/D", "recoilPZ/D", + "recoilUncPX/D", "recoilUncPY/D", "recoilUncPZ/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", + "v0EleTruthMatched/I", "v0PosTruthMatched/I", "v0BothTruthMatchedToAp/I", + "recoilTruthMatched/I", + "v0ElePurity/D", "v0PosPurity/D", "recoilPurity/D"); + + private String cascadeVertexCandidatesColName = "CascadeVertexCandidates"; + 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 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; + 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; + } + } + } + } + + List cascadeCandidates = event.get(ReconstructedParticle.class, cascadeVertexCandidatesColName); + for (ReconstructedParticle cascade : cascadeCandidates) { + 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(); + + 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); + 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). + 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("v0UncPX/D", pV0Unc.x()); + row.put("v0UncPY/D", pV0Unc.y()); + row.put("v0UncPZ/D", pV0Unc.z()); + row.put("recoilPX/D", pRecoil.x()); + row.put("recoilPY/D", pRecoil.y()); + row.put("recoilPZ/D", pRecoil.z()); + row.put("recoilUncPX/D", pRecoilUnc.x()); + row.put("recoilUncPY/D", pRecoilUnc.y()); + row.put("recoilUncPZ/D", pRecoilUnc.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("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))); + + 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..2f1d5e9977 --- /dev/null +++ b/analysis/src/main/java/org/hps/analysis/tuple/NTrackVertexComparisonTupleDriver.java @@ -0,0 +1,268 @@ +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.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.KalmanNTrackVertexer; +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; + +/** + * Writes a flat ASCII ntuple comparing, per trident MC event, the legacy Billoir N-track + * common-vertex fit against the standalone Kalman gain-matrix N-track common-vertex fit + * ({@code KalmanNTrackVertexer}), for the 3 truth-matched trident tracks (2 e- + 1 e+ from a + * single common production vertex). Unlike {@code TwoTrackVertexComparisonTupleDriver} and + * {@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 trident 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", + "kalVtxX/D", "kalVtxY/D", "kalVtxZ/D", + "kalVtxXErr/D", "kalVtxYErr/D", "kalVtxZErr/D", + "kalChi2/D", "kalNdf/I", "kalMass/D", + "ele1TruthMatched/I", "ele2TruthMatched/I", "posTruthMatched/I", "allTruthMatched/I", + "apVtxXMC/D", "apVtxYMC/D", "apVtxZMC/D"); + + private String trackCollectionName = "KalmanFullTracks"; + private String mcParticlesColName = "MCParticle"; + private String trackToMCParticleRelationsColName = "KalmanFullTracksToMCParticleRelations"; + private String tupleFile = null; + private PrintWriter tupleWriter = null; + private double bField; + + public void setTrackCollectionName(String trackCollectionName) { + this.trackCollectionName = trackCollectionName; + } + + 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; + } + + // Trident truth identification: PDGID 623 is the trident/"reaction" pseudo-particle + // and has 3 direct daughters (2 e- + 1 e+ from a single common production vertex) -- + // architecturally different from the A' sample, where 622 is the A' signal record + // (2 daughters: signal e+e-) and 623 is a separate, unrelated single-daughter recoil + // electron record (confirmed via direct LCIO inspection of tritrig_pulser_100.slcio: + // zero PDGID-622 particles present; every event has exactly one PDGID-623 top-level + // particle with the expected 2x11 + 1x(-11) daughter topology). Ranks electron + // daughters by energy, same convention as MCTupleMaker.fillMCTridentVariables (which + // has this same PDGID mixup for tritrig-only samples). + MCParticle ele1MC = null; + MCParticle ele2MC = null; + MCParticle posMC = null; + for (MCParticle mcp : event.get(MCParticle.class, mcParticlesColName)) { + 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: unlike the A' sample, this generator's PDGID-622 daughters' own + // getOrigin() is not contaminated by any upstream converter bug (confirmed via + // MCTupleMaker.fillMCParticleVariables, which uses particle.getOrigin() directly), + // so no daughter-production-point workaround is needed here. + Hep3Vector apVtxMC = ele1MC.getOrigin(); + + // 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/TwoTrackVertexComparisonTupleDriver'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(); + 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); + + 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 kalmanVtx = new KalmanNTrackVertexer(bField).fitVertex(tracks); + + Hep3Vector vtxPos = billoirVtx.getPosition(); + Hep3Vector vtxPosErr = billoirVtx.getPositionError(); + Hep3Vector kalVtxPos = kalmanVtx.getPosition(); + Hep3Vector kalVtxPosErr = kalmanVtx.getPositionError(); + Double kalNdf = kalmanVtx.getCustomParameters().get("ndf"); + + 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("kalVtxX/D", kalVtxPos.x()); + row.put("kalVtxY/D", kalVtxPos.y()); + row.put("kalVtxZ/D", kalVtxPos.z()); + row.put("kalVtxXErr/D", kalVtxPosErr.x()); + row.put("kalVtxYErr/D", kalVtxPosErr.y()); + row.put("kalVtxZErr/D", kalVtxPosErr.z()); + row.put("kalChi2/D", kalmanVtx.getChi2()); + row.put("kalNdf/I", kalNdf != null ? kalNdf : -9999.0); + row.put("kalMass/D", kalmanVtx.getInvMass()); + 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()); + + 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/TwoTrackVertexComparisonTupleDriver.java b/analysis/src/main/java/org/hps/analysis/tuple/TwoTrackVertexComparisonTupleDriver.java new file mode 100644 index 0000000000..4f388f5bab --- /dev/null +++ b/analysis/src/main/java/org/hps/analysis/tuple/TwoTrackVertexComparisonTupleDriver.java @@ -0,0 +1,239 @@ +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 java.util.logging.Logger; + +import hep.physics.vec.Hep3Vector; + +import org.hps.recon.tracking.TrackStateUtils; +import org.hps.recon.vertexing.BilliorVertex; +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.TrackState; +import org.lcsim.geometry.Detector; +import org.lcsim.util.Driver; + +/** + * Writes a flat ASCII ntuple pairing, per unconstrained V0 (e-/e+) candidate, the + * existing Billoir vertex fit against the standalone unconstrained two-track Kalman + * vertex fit ({@code KalmanV0Vertexer}, wired through {@code HpsReconParticleDriver}), + * for offline comparison of the two algorithms. One row per V0 candidate. Header line is + * variable names joined by ":"; data rows are tab-separated, matching the convention + * used by {@link CascadeVertexTupleDriver}. + */ +public class TwoTrackVertexComparisonTupleDriver extends Driver { + + private static final Logger LOGGER = Logger.getLogger(TwoTrackVertexComparisonTupleDriver.class.getPackage().getName()); + + 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", + "kalVtxX/D", "kalVtxY/D", "kalVtxZ/D", + "kalVtxXErr/D", "kalVtxYErr/D", "kalVtxZErr/D", + "kalChi2/D", "kalNdf/I", "kalMass/D", + "eleTruthMatched/I", "posTruthMatched/I", "bothTruthMatchedToAp/I", + "apVtxXMC/D", "apVtxYMC/D", "apVtxZMC/D"); + + private String unconstrainedV0CandidatesColName = "UnconstrainedV0Candidates"; + private String kalmanV0CandidatesColName = "KalmanUnconstrainedV0Candidates"; + private String mcParticlesColName = null; + private String trackToMCParticleRelationsColName = null; + private String tupleFile = null; + private PrintWriter tupleWriter = null; + + public void setUnconstrainedV0CandidatesColName(String unconstrainedV0CandidatesColName) { + this.unconstrainedV0CandidatesColName = unconstrainedV0CandidatesColName; + } + + public void setKalmanV0CandidatesColName(String kalmanV0CandidatesColName) { + this.kalmanV0CandidatesColName = kalmanV0CandidatesColName; + } + + 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 two-track vertex comparison tuple file " + tupleFile, e); + } + tupleWriter.println(String.join(":", VARIABLES)); + } + + @Override + public void process(EventHeader event) { + if (tupleWriter == null || !event.hasCollection(ReconstructedParticle.class, unconstrainedV0CandidatesColName) + || !event.hasCollection(ReconstructedParticle.class, kalmanV0CandidatesColName)) { + return; + } + + Map trackToMC = 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()); + } + } + + MCParticle eleMC = null; + MCParticle posMC = null; + if (mcParticlesColName != null && event.hasCollection(MCParticle.class, mcParticlesColName)) { + for (MCParticle mcp : event.get(MCParticle.class, mcParticlesColName)) { + 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; + } + } + } + + // apMC.getOrigin()/getEndPoint() are unreliable for this generator sample: the + // .dat/LHE-to-stdhep converters apply the decay-length vertex shift to the A' record + // itself, and BaseMCParticle's endpoint field defaults to (0,0,0) unless set by an + // external writer. Use a daughter's own production point instead -- daughters are + // produced exactly at the parent's decay vertex (same convention as CascadeVertexTupleDriver). + Hep3Vector apVtxMC = eleMC != null ? eleMC.getOrigin() : (posMC != null ? posMC.getOrigin() : null); + + List billoirV0s = event.get(ReconstructedParticle.class, unconstrainedV0CandidatesColName); + List kalmanV0s = event.get(ReconstructedParticle.class, kalmanV0CandidatesColName); + if (billoirV0s.size() != kalmanV0s.size()) { + LOGGER.warning("Billoir/Kalman unconstrained V0 collection size mismatch (" + + billoirV0s.size() + " vs " + kalmanV0s.size() + "), skipping event " + + event.getEventNumber()); + return; + } + + for (int i = 0; i < billoirV0s.size(); i++) { + ReconstructedParticle billoirV0 = billoirV0s.get(i); + ReconstructedParticle kalmanV0 = kalmanV0s.get(i); + BilliorVertex billoirVtx = (BilliorVertex) billoirV0.getStartVertex(); + BilliorVertex kalmanVtx = (BilliorVertex) kalmanV0.getStartVertex(); + + List daughters = billoirV0.getParticles(); + ReconstructedParticle eleDaughter = daughters.get(0).getCharge() < 0 ? daughters.get(0) : daughters.get(1); + ReconstructedParticle posDaughter = daughters.get(0).getCharge() < 0 ? daughters.get(1) : daughters.get(0); + + if (event.getRunNumber() == 14596 && event.getEventNumber() == 734) { + System.out.println("CANDIDATE i=" + i + " billoirVtxZ=" + billoirVtx.getPosition().z() + + " billoirChi2=" + billoirVtx.getChi2() + + " kalmanVtxZ=" + kalmanVtx.getPosition().z() + + " kalmanChi2=" + kalmanVtx.getChi2()); + dumpTrack("ELECTRON", eleDaughter.getTracks().get(0)); + dumpTrack("POSITRON", posDaughter.getTracks().get(0)); + } + boolean eleMatched = eleMC != null && eleMC.equals(trackToMC.get(eleDaughter.getTracks().get(0))); + boolean posMatched = posMC != null && posMC.equals(trackToMC.get(posDaughter.getTracks().get(0))); + + Hep3Vector vtxPos = billoirVtx.getPosition(); + Hep3Vector vtxPosErr = billoirVtx.getPositionError(); + Hep3Vector kalVtxPos = kalmanVtx.getPosition(); + Hep3Vector kalVtxPosErr = kalmanVtx.getPositionError(); + Double kalNdf = kalmanVtx.getCustomParameters().get("ndf"); + + 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("kalVtxX/D", kalVtxPos.x()); + row.put("kalVtxY/D", kalVtxPos.y()); + row.put("kalVtxZ/D", kalVtxPos.z()); + row.put("kalVtxXErr/D", kalVtxPosErr.x()); + row.put("kalVtxYErr/D", kalVtxPosErr.y()); + row.put("kalVtxZErr/D", kalVtxPosErr.z()); + row.put("kalChi2/D", kalmanVtx.getChi2()); + row.put("kalNdf/I", kalNdf != null ? kalNdf : -9999.0); + row.put("kalMass/D", kalmanVtx.getInvMass()); + row.put("eleTruthMatched/I", eleMatched ? 1.0 : 0.0); + row.put("posTruthMatched/I", posMatched ? 1.0 : 0.0); + row.put("bothTruthMatchedToAp/I", eleMatched && posMatched ? 1.0 : 0.0); + 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); + + 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(); + } + } + + private static void dumpTrack(String label, Track track) { + TrackState ts = TrackStateUtils.getTrackStatesAtLocation(track, TrackState.AtPerigee).get(0); + double[] par = ts.getParameters(); + double[] cov = ts.getCovMatrix(); + System.out.println("=== " + label + " AtPerigee ==="); + System.out.println(" d0=" + par[0] + " phi0=" + par[1] + " omega=" + par[2] + + " z0=" + par[3] + " tanLambda=" + par[4] + " bLocal=" + ts.getBLocal()); + System.out.println(" covPacked=" + Arrays.toString(cov)); + System.out.println(" referencePoint=" + Arrays.toString(ts.getReferencePoint())); + + System.out.println(" track.getTrackStates().size()=" + track.getTrackStates().size()); + for (int k = 0; k < track.getTrackStates().size(); k++) { + TrackState tsk = track.getTrackStates().get(k); + System.out.println(" state[" + k + "] location=" + tsk.getLocation() + + " sameObjAsPerigeeLookup=" + (tsk == ts) + + " d0=" + tsk.getParameter(0) + " z0=" + tsk.getParameter(3)); + } + TrackState ts0 = track.getTrackStates().get(0); + System.out.println(" getTrackStates().get(0): d0=" + ts0.getParameter(0) + + " phi0=" + ts0.getParameter(1) + " omega=" + ts0.getParameter(2) + + " z0=" + ts0.getParameter(3) + " tanLambda=" + ts0.getParameter(4) + + " bLocal=" + ts0.getBLocal() + " location=" + ts0.getLocation()); + System.out.println(" ts0==tsPerigee: " + (ts0 == ts)); + } +} diff --git a/analysis/src/test/java/org/hps/analysis/tuple/CascadeVertexTupleDriverTest.java b/analysis/src/test/java/org/hps/analysis/tuple/CascadeVertexTupleDriverTest.java new file mode 100644 index 0000000000..fae683f1c2 --- /dev/null +++ b/analysis/src/test/java/org/hps/analysis/tuple/CascadeVertexTupleDriverTest.java @@ -0,0 +1,102 @@ +package org.hps.analysis.tuple; + +import java.io.BufferedReader; +import java.io.File; +import java.io.FileReader; + +import org.hps.conditions.database.DatabaseConditionsManager; +import org.hps.detector.svt.SvtDetectorSetup; +import org.hps.recon.particle.HpsReconParticleDriver; +import org.hps.util.test.TestUtil; +import org.lcsim.util.loop.LCSimLoop; + +import junit.framework.TestCase; + +/** + * End-to-end smoke test for the cascade-vertex reconstruction chain: runs a fresh + * {@code HpsReconParticleDriver} (so V0 candidates are rebuilt with the current + * full-covariance BilliorVertexer) followed by {@link CascadeVertexTupleDriver} over a + * real reconstructed 2016 physics-run sample, and checks the resulting flat ntuple. + */ +public class CascadeVertexTupleDriverTest extends TestCase { + + private static final String LCIO_INPUT_FILE_NAME = "hps_005772.0_recon_Rv4657-0-10000.slcio"; + private static final String TUPLE_OUTPUT_FILE_NAME = "target/test-output/cascade_vertex_tuple.txt"; + private static final String CASCADE_COL_NAME = "CascadeVertexCandidates"; + + public void testIt() throws Exception { + File lcioInputFile = TestUtil.downloadTestFile(LCIO_INPUT_FILE_NAME); + + LCSimLoop loop = new LCSimLoop(); + loop.setLCIORecordSource(lcioInputFile); + + final DatabaseConditionsManager manager = DatabaseConditionsManager.getInstance(); + manager.addConditionsListener(new SvtDetectorSetup()); + + loop.add(new org.lcsim.recon.tracking.digitization.sisim.config.ReadoutCleanupDriver()); + loop.add(new org.hps.recon.filtering.EventFlagFilter()); + + org.lcsim.recon.tracking.digitization.sisim.config.RawTrackerHitSensorSetup rthss + = new org.lcsim.recon.tracking.digitization.sisim.config.RawTrackerHitSensorSetup(); + rthss.setReadoutCollections(new String[]{"SVTRawTrackerHits"}); + loop.add(rthss); + + HpsReconParticleDriver reconParticleDriver = new HpsReconParticleDriver(); + reconParticleDriver.setEcalClusterCollectionName("EcalClustersCorr"); + reconParticleDriver.setTrackCollectionNames(new String[]{"GBLTracks"}); + reconParticleDriver.setUnconstrainedV0CandidatesColName("UnconstrainedV0CandidatesCascadeTest"); + reconParticleDriver.setFinalStateParticlesColName("FinalStateParticlesCascadeTest"); + reconParticleDriver.setCascadeVertexCandidatesColName(CASCADE_COL_NAME); + loop.add(reconParticleDriver); + + File tupleOutputFile = new File(TUPLE_OUTPUT_FILE_NAME); + tupleOutputFile.getParentFile().mkdirs(); + CascadeVertexTupleDriver tupleDriver = new CascadeVertexTupleDriver(); + tupleDriver.setCascadeVertexCandidatesColName(CASCADE_COL_NAME); + tupleDriver.setTupleFile(TUPLE_OUTPUT_FILE_NAME); + loop.add(tupleDriver); + + loop.loop(5000); + loop.dispose(); + + checkTupleOutput(tupleOutputFile); + } + + private void checkTupleOutput(File tupleOutputFile) throws Exception { + assertTrue("cascade vertex tuple file should exist", tupleOutputFile.exists()); + + BufferedReader reader = new BufferedReader(new FileReader(tupleOutputFile)); + String header = reader.readLine(); + assertNotNull("cascade vertex tuple file should have a header line", header); + String[] headerVars = header.split(":"); + + int nRows = 0; + String line; + while ((line = reader.readLine()) != null) { + if (line.trim().isEmpty()) { + continue; + } + nRows++; + String[] fields = line.trim().split("\\s+"); + assertEquals("row should have one value per header variable", headerVars.length, fields.length); + + double chi2 = -1, ndf = -1; + for (int i = 0; i < headerVars.length; i++) { + double value = Double.parseDouble(fields[i]); + assertTrue("field " + headerVars[i] + " should be finite, got " + value, + !Double.isNaN(value) && !Double.isInfinite(value)); + if (headerVars[i].equals("cascadeChi2/D")) { + chi2 = value; + } + if (headerVars[i].equals("cascadeNdf/I")) { + ndf = value; + } + } + assertTrue("cascadeChi2 should be non-negative, got " + chi2, chi2 >= 0); + assertEquals("cascadeNdf should be 1", 1.0, ndf); + } + reader.close(); + + assertTrue("at least one cascade vertex candidate should have been written", nRows > 0); + } +} 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..28f1df57ab --- /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/ThreeTrackVertexCandidates, + * 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 b10c5170f7..f7eda42945 100644 --- a/recon/src/main/java/org/hps/recon/particle/HpsReconParticleDriver.java +++ b/recon/src/main/java/org/hps/recon/particle/HpsReconParticleDriver.java @@ -15,6 +15,7 @@ import org.hps.recon.vertexing.BilliorTrack; import org.hps.recon.vertexing.BilliorVertex; import org.hps.recon.vertexing.BilliorVertexer; +import org.hps.recon.vertexing.KalmanV0Vertexer; import org.hps.record.StandardCuts; import org.hps.recon.tracking.TrackStateUtils; import org.lcsim.detector.tracker.silicon.HpsSiSensor; @@ -245,6 +246,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; } @@ -656,6 +664,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); @@ -663,6 +675,25 @@ private void makeV0Candidates(ReconstructedParticle electron, ReconstructedParti if (eleIsTop != posIsTop) { unconstrainedV0Vertices.add(vtxFit); unconstrainedV0Candidates.add(candidate); + + // Standalone unconstrained two-track Kalman vertex fit, run in parallel with + // the Billoir fit above (for comparison) on the same track pair. Off unless + // kalmanUnconstrainedV0CandidatesColName is set. Kept in lockstep with + // unconstrainedV0Candidates (same cuts already applied above) so the two + // collections can be paired by index downstream. + if (kalmanUnconstrainedV0CandidatesColName != null) { + BilliorVertex kalmanVtxFit; + try { + kalmanVtxFit = new KalmanV0Vertexer(bLocalForTrack(electron.getTracks().get(0))) + .fitVertex(electron.getTracks().get(0), positron.getTracks().get(0)); + } catch (RuntimeException e) { + LOGGER.warning("KalmanV0Vertexer fit failed, using placeholder: " + e.getMessage()); + kalmanVtxFit = KalmanV0Vertexer.placeholderVertex(); + } + ReconstructedParticle kalmanCandidate = makeReconstructedParticle(electron, positron, kalmanVtxFit); + kalmanUnconstrainedV0Vertices.add(kalmanVtxFit); + kalmanUnconstrainedV0Candidates.add(kalmanCandidate); + } } else { unconstrainedVcVertices.add(vtxFit); unconstrainedVcCandidates.add(candidate); 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..e87cefaaed 100644 --- a/recon/src/main/java/org/hps/recon/particle/ReconParticleDriver.java +++ b/recon/src/main/java/org/hps/recon/particle/ReconParticleDriver.java @@ -10,19 +10,27 @@ 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.ThreeTrackVertexer; 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.LCRelation; +import org.lcsim.event.MCParticle; 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 +62,7 @@ public abstract class ReconParticleDriver extends Driver { protected boolean isMC = false; private boolean disablePID = false; + private boolean fixV2BeamCoordinate = false; protected StandardCuts cuts = new StandardCuts(); // RelationalTable hitToRotated = null; // RelationalTable hitToStrips = null; @@ -129,10 +138,22 @@ public void setApplyClusterCorrections(boolean val) { * constraints. */ protected List targetConV0Candidates; + /** + * Stores reconstructed V0 candidate particles from the standalone unconstrained + * two-track Kalman vertex fit ({@link org.hps.recon.vertexing.KalmanV0Vertexer}), + * run in parallel with the ordinary Billoir unconstrained V0 fit for comparison. + * Off by default; only filled if {@link #kalmanUnconstrainedV0CandidatesColName} is set. + */ + protected List kalmanUnconstrainedV0Candidates; /** * Stores reconstructed V0 candidate vertices generated without constraints. */ protected List unconstrainedV0Vertices; + /** + * Stores reconstructed V0 candidate vertices from the standalone unconstrained + * two-track Kalman vertex fit. See {@link #kalmanUnconstrainedV0Candidates}. + */ + protected List kalmanUnconstrainedV0Vertices; /** * Stores reconstructed V0 candidate vertices generated with beam spot * constraints. @@ -143,6 +164,16 @@ public void setApplyClusterCorrections(boolean val) { * constraints. */ protected List targetConV0Vertices; + /** + * Stores reconstructed cascade (V0 + recoil electron production vertex) + * candidate particles. + */ + protected List cascadeVertexCandidates; + /** + * Stores reconstructed 3-track (V0 e-/e+ + recoil electron) simultaneous + * vertex candidate particles. + */ + protected List threeTrackVertexCandidates; // LCIO Collection Names /** @@ -186,6 +217,17 @@ public void setApplyClusterCorrections(boolean val) { * constraints. */ protected String unconstrainedV0VerticesColName = null; + /** + * LCIO collection name for V0 candidate particles from the standalone unconstrained + * two-track Kalman vertex fit. Defaults to null, i.e. this fit is off unless a + * collection name is explicitly set. See {@link #kalmanUnconstrainedV0Candidates}. + */ + protected String kalmanUnconstrainedV0CandidatesColName = null; + /** + * LCIO collection name for V0 candidate vertices from the standalone unconstrained + * two-track Kalman vertex fit. See {@link #kalmanUnconstrainedV0CandidatesColName}. + */ + protected String kalmanUnconstrainedV0VerticesColName = null; /** * LCIO collection name for V0 candidate vertices generated with beam spot * constraints. @@ -196,6 +238,38 @@ public void setApplyClusterCorrections(boolean val) { * constraints. */ protected String targetConV0VerticesColName = null; + /** + * LCIO collection name for cascade (V0 + recoil electron production vertex) + * candidate particles. Defaults to null, i.e. cascade vertexing is off unless + * a collection name is explicitly set. + */ + protected String cascadeVertexCandidatesColName = 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 threeTrackVertexCandidatesColName = null; + /** + * MC-truth collection names used only to gate the {@code BADFIT_DEBUG} diagnostic + * print in {@link ThreeTrackVertexer#fit} to genuinely truth-matched V0(e-/e+) + + * recoil-electron combinations, rather than every combinatorial pairing tried by + * {@link #findThreeTrackVertices}. Both default to null, i.e. no truth info is + * looked up and the debug print never fires, unless both are explicitly set (MC + * steering files only). + */ + protected String mcParticlesColName = null; + protected String trackToMCParticleRelationsColName = null; + + // Accumulated wall-clock time (ns) and count of calls spent in the cascade + // (helix + straight-line) vertex fit, reported in endOfData(). + private long cascadeVertexFitTimeNs = 0L; + private int cascadeVertexFitCount = 0; + + // Accumulated wall-clock time (ns) and count of calls spent in the 3-track + // simultaneous vertex fit, reported in endOfData(). + private long threeTrackVertexFitTimeNs = 0L; + private int threeTrackVertexFitCount = 0; // Beam size variables. // The beamsize array is in the tracking frame @@ -237,6 +311,50 @@ public void setBeamConV0CandidatesColName(String beamConV0CandidatesColName) { this.beamConV0CandidatesColName = beamConV0CandidatesColName; } + /** + * Sets the name of the LCIO collection for cascade (V0 + recoil electron + * production vertex) candidate particles. Setting this enables cascade + * vertexing, 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 3-track (V0 e-/e+ + recoil + * electron) simultaneous vertex candidate particles. Setting this enables + * this fit, which is off by default. + * + * @param threeTrackVertexCandidatesColName - The LCIO collection name. + */ + public void setThreeTrackVertexCandidatesColName(String threeTrackVertexCandidatesColName) { + this.threeTrackVertexCandidatesColName = threeTrackVertexCandidatesColName; + } + + /** + * Sets the name of the LCIO MCParticle collection used only to gate the + * {@code BADFIT_DEBUG} print in {@link #findThreeTrackVertices} to truth-matched + * candidates. Optional; MC steering files only. + * + * @param mcParticlesColName - The LCIO collection name. + */ + public void setMcParticlesColName(String mcParticlesColName) { + this.mcParticlesColName = mcParticlesColName; + } + + /** + * Sets the name of the LCIO Track-to-MCParticle LCRelation collection used only to + * gate the {@code BADFIT_DEBUG} print in {@link #findThreeTrackVertices} to + * truth-matched candidates. Optional; MC steering files only. + * + * @param trackToMCParticleRelationsColName - The LCIO collection name. + */ + public void setTrackToMCParticleRelationsColName(String trackToMCParticleRelationsColName) { + this.trackToMCParticleRelationsColName = trackToMCParticleRelationsColName; + } + /** * Sets the name of the LCIO collection for beam spot constrained V0 * candidate vertices. @@ -401,6 +519,28 @@ public void setUnconstrainedV0VerticesColName(String unconstrainedV0VerticesColN this.unconstrainedV0VerticesColName = unconstrainedV0VerticesColName; } + /** + * Sets the name of the LCIO collection for V0 candidate particles from the + * standalone unconstrained two-track Kalman vertex fit. Setting this enables that + * fit (run in parallel with, not instead of, the ordinary Billoir unconstrained V0 + * fit), which is off by default. + * + * @param kalmanUnconstrainedV0CandidatesColName - The LCIO collection name. + */ + public void setKalmanUnconstrainedV0CandidatesColName(String kalmanUnconstrainedV0CandidatesColName) { + this.kalmanUnconstrainedV0CandidatesColName = kalmanUnconstrainedV0CandidatesColName; + } + + /** + * Sets the name of the LCIO collection for V0 candidate vertices from the + * standalone unconstrained two-track Kalman vertex fit. + * + * @param kalmanUnconstrainedV0VerticesColName - The LCIO collection name. + */ + public void setKalmanUnconstrainedV0VerticesColName(String kalmanUnconstrainedV0VerticesColName) { + this.kalmanUnconstrainedV0VerticesColName = kalmanUnconstrainedV0VerticesColName; + } + /** * Set the names of the LCIO track collections used as input. * @@ -430,6 +570,16 @@ public void setDisablePID(boolean disablePID) { this.disablePID = disablePID; } + /** + * When true, {@link #findThreeTrackVertices}'s {@link ThreeTrackVertexer} holds V2's + * beam-direction coordinate fixed at the target position instead of fitting it freely -- + * see {@link ThreeTrackVertexer#setFixV2BeamCoordinate}. Default false keeps the + * original fully-free-V2 joint fit. + */ + public void setFixV2BeamCoordinate(boolean fixV2BeamCoordinate) { + this.fixV2BeamCoordinate = fixV2BeamCoordinate; + } + public void setClusterParamFileName(String input) { clusterParamFileName = input; } @@ -744,8 +894,12 @@ protected void process(EventHeader event) { beamConV0Candidates = new ArrayList(); targetConV0Candidates = new ArrayList(); unconstrainedV0Vertices = new ArrayList(); + kalmanUnconstrainedV0Candidates = new ArrayList(); + kalmanUnconstrainedV0Vertices = new ArrayList(); beamConV0Vertices = new ArrayList(); targetConV0Vertices = new ArrayList(); + cascadeVertexCandidates = new ArrayList(); + threeTrackVertexCandidates = new ArrayList(); // Loop through all of the track collections present in the event and // create final state particles. @@ -778,6 +932,22 @@ else if (finalStateParticle.getCharge() < 0) { List goodFinalStateParticles = particleCuts(finalStateParticles); // VERBOSE :: Output the number of reconstructed particles. printDebug("Final State Particles :: " + goodFinalStateParticles.size()); + + // Form cascade (V0 + recoil electron production 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"); + } + // 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 (threeTrackVertexCandidatesColName != null) { + findThreeTrackVertices(event, unconstrainedV0Candidates, goodFinalStateParticles); + printDebug("[ReconParticleDriver] findThreeTrackVertices() finished"); + } // Add the final state ReconstructedParticles to the event event.put(finalStateParticlesColName, goodFinalStateParticles, ReconstructedParticle.class, 0); for (ReconstructedParticle ele : goodFinalStateParticles) { @@ -806,6 +976,14 @@ else if (finalStateParticle.getCharge() < 0) { printDebug("Unconstrained V0 Vertices: " + unconstrainedV0Vertices.size()); event.put(unconstrainedV0VerticesColName, unconstrainedV0Vertices, Vertex.class, 0); } + if (kalmanUnconstrainedV0CandidatesColName != null) { + printDebug("Kalman Unconstrained V0 Candidates: " + kalmanUnconstrainedV0Candidates.size()); + event.put(kalmanUnconstrainedV0CandidatesColName, kalmanUnconstrainedV0Candidates, ReconstructedParticle.class, 0); + } + if (kalmanUnconstrainedV0VerticesColName != null) { + printDebug("Kalman Unconstrained V0 Vertices: " + kalmanUnconstrainedV0Vertices.size()); + event.put(kalmanUnconstrainedV0VerticesColName, kalmanUnconstrainedV0Vertices, Vertex.class, 0); + } if (beamConV0VerticesColName != null) { printDebug("Beam-Constrained V0 Vertices: " + beamConV0Vertices.size()); event.put(beamConV0VerticesColName, beamConV0Vertices, Vertex.class, 0); @@ -814,7 +992,164 @@ 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 (threeTrackVertexCandidatesColName != null) { + printDebug("3-Track Vertex Candidates: " + threeTrackVertexCandidates.size()); + event.put(threeTrackVertexCandidatesColName, threeTrackVertexCandidates, ReconstructedParticle.class, 0); + } + + } + + /** + * Returns the field to use for {@code CascadeVertexer}/{@code ThreeTrackVertexer} + * 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 (production) vertex candidates: for each unconstrained V0 candidate, + * pair it with every final-state electron that is not already one of its daughters, + * fit the production vertex where the V0's momentum line meets the recoil electron's + * helix, 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. + * + * @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); + CascadeVertexer cascadeVertexer = new CascadeVertexer(bLocalForTrack(v0EleDaughter.getTracks().get(0))); + for (ReconstructedParticle electron : finalStateElectrons) { + if (electron.getCharge() >= 0 || v0Daughters.contains(electron)) { + continue; + } + long fitStartTime = System.nanoTime(); + try { + ReconstructedParticle cascade = cascadeVertexer.fit(v0, electron); + if (cascade != null) { + cascadeVertexCandidates.add(cascade); + } + } catch (RuntimeException e) { + printDebug("[ReconParticleDriver] findCascadeVertices: skipping pair after RuntimeException: " + e.getMessage()); + continue; + } finally { + cascadeVertexFitTimeNs += System.nanoTime() - fitStartTime; + cascadeVertexFitCount++; + } + } + } + } + /** + * Forms 3-track 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 #threeTrackVertexCandidates}. Mirrors + * {@link #findCascadeVertices} exactly, using {@link ThreeTrackVertexer} instead of + * {@link CascadeVertexer}. + * + * @param event Current event, used only to look up MC truth (via + * {@link #mcParticlesColName}/{@link #trackToMCParticleRelationsColName}) + * to gate {@link ThreeTrackVertexer}'s BADFIT_DEBUG print + * to truth-matched candidates; unused otherwise. + * @param v0Candidates Unconstrained V0 candidates for this event. + * @param finalStateElectrons Final-state electrons for this event. + */ + protected void findThreeTrackVertices(EventHeader event, List v0Candidates, + List finalStateElectrons) { + Map trackToMC = new HashMap(); + MCParticle eleMC = null; + MCParticle posMC = null; + MCParticle recoilMC = null; + if (mcParticlesColName != null && trackToMCParticleRelationsColName != null + && event.hasCollection(MCParticle.class, mcParticlesColName) + && event.hasCollection(LCRelation.class, trackToMCParticleRelationsColName)) { + for (LCRelation rel : event.get(LCRelation.class, trackToMCParticleRelationsColName)) { + trackToMC.put((Track) rel.getFrom(), (MCParticle) rel.getTo()); + } + MCParticle apMC = null; + 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 (see CascadeVertexTupleDriver): 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 -- it 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; + } + } + } + } + + 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); + boolean v0Matched = eleMC != null && eleMC.equals(trackToMC.get(v0EleDaughter.getTracks().get(0))) + && posMC != null && posMC.equals(trackToMC.get(v0PosDaughter.getTracks().get(0))); + ThreeTrackVertexer threeTrackVertexer = new ThreeTrackVertexer(bLocalForTrack(v0EleDaughter.getTracks().get(0))); + threeTrackVertexer.setFixV2BeamCoordinate(fixV2BeamCoordinate); + for (ReconstructedParticle electron : finalStateElectrons) { + if (electron.getCharge() >= 0 || v0Daughters.contains(electron)) { + continue; + } + boolean truthMatched = v0Matched && recoilMC != null + && recoilMC.equals(trackToMC.get(electron.getTracks().get(0))); + long fitStartTime = System.nanoTime(); + try { + ReconstructedParticle threeTrackVertex = threeTrackVertexer.fit(v0, electron, truthMatched); + if (threeTrackVertex != null) { + threeTrackVertexCandidates.add(threeTrackVertex); + } + } catch (RuntimeException e) { + printDebug("[ReconParticleDriver] findThreeTrackVertices: skipping pair after RuntimeException: " + e.getMessage()); + continue; + } finally { + threeTrackVertexFitTimeNs += System.nanoTime() - fitStartTime; + threeTrackVertexFitCount++; + } + } + } } /** @@ -855,6 +1190,22 @@ 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 (helix+straight) 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); + } + if (threeTrackVertexCandidatesColName != null && threeTrackVertexFitCount > 0) { + double totalTimeMs = threeTrackVertexFitTimeNs / 1e6; + double timePerFitMs = totalTimeMs / threeTrackVertexFitCount; + System.out.format("ReconParticleDriver.endOfData: total 3-track simultaneous vertex fit " + + "execution time=%12.4f ms for %d fits.\n", totalTimeMs, threeTrackVertexFitCount); + System.out.format(" 3-track 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..c49f98b54a 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) { @@ -381,13 +424,30 @@ public Hep3Vector getFittedMomentumError(int index) { 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 ee649f990c..daa6c50b83 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)); @@ -593,6 +597,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..3c1c668741 --- /dev/null +++ b/recon/src/main/java/org/hps/recon/vertexing/CascadeVertexer.java @@ -0,0 +1,224 @@ +package org.hps.recon.vertexing; + +import java.util.HashMap; +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.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.KalmanVertexFitterGainMatrix.FitResult; +import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.LineParams; +import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.LinePlaneProjection; +import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TrackParams; + +/** + * Fits a "cascade" (production) vertex where an already-vertexed neutral V0's momentum + * direction -- treated as a zero-curvature line, carrying its full 6x6 position-momentum + * joint covariance -- meets a third, curving recoil-electron track. Wraps + * {@link KalmanVertexFitterGainMatrix#fitCascadeVertex} and packages the result as a + * 2-daughter ReconstructedParticle (V0, recoil electron) with the production Vertex + * attached via setStartVertex(...), mirroring the conventions used by + * {@code HpsReconParticleDriver#makeReconstructedParticle}. + */ +public class CascadeVertexer { + + private static final double ELECTRON_MASS = 0.000511; + + private final double bField; + + public CascadeVertexer(double bField) { + this.bField = bField; + } + + /** + * Fit the production vertex where {@code v0Particle}'s momentum line meets + * {@code recoilElectron}'s helix. + * + * @param v0Particle an already-fitted V0 whose {@code getStartVertex()} is a + * {@link BilliorVertex} (as produced by BilliorVertexer / + * HpsReconParticleDriver) + * @param recoilElectron a final-state electron not already a daughter of v0Particle + * @return the cascade ReconstructedParticle, or null if the fit fails + */ + public ReconstructedParticle fit(ReconstructedParticle v0Particle, ReconstructedParticle recoilElectron) { + BilliorVertex v0Vertex = (BilliorVertex) v0Particle.getStartVertex(); + LineParams v0Line = KalmanVertexFitterGainMatrix.lineFromV0(v0Vertex); + + Track recoilTrack = recoilElectron.getTracks().get(0); + TrackState recoilTs = TrackStateUtils.getTrackStatesAtLocation(recoilTrack, TrackState.AtPerigee).get(0); + TrackParams recoilParams = trackParamsFromTrack(recoilTrack); + + // recoilParams' raw d0/z0 (and hence the vertex unknown inside fitCascadeVertex) are + // relative to the recoil track's own reference point, whereas v0Line (built from an + // already-fit, absolute-frame BilliorVertex) is not -- shift v0Line into that same local + // frame before fitting, then shift the fitted vertex back to the absolute frame below. + double[] refPoint = recoilTs.getReferencePoint(); + LineParams v0LineLocal = shiftLine(v0Line, refPoint, -1.0); + + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(bField); + FitResult result = fitter.fitCascadeVertex(recoilParams, v0LineLocal); + if (result == null) { + return null; + } + result.vertex = result.vertex.add(MatrixUtils.createRealVector(refPoint)); + + // Diagnostic-only target-plane projections (not used by the fit 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 production vertex -- same convention as the analogous + // diagnostics added to ThreeTrackVertexer. v0Line is already absolute-frame; recoilParams + // is not, so its plane crossing must be evaluated at the plane's position in the recoil + // track's local frame, then shifted back to absolute. + LinePlaneProjection v0Proj = KalmanVertexFitterGainMatrix.propagateLineToPlane( + v0Line, fitter.getBeamPosition()[0], 10.0); + LinePlaneProjection recoilProjLocal = KalmanVertexFitterGainMatrix.propagateTrackToPlane( + recoilParams, fitter.getBeamPosition()[0] - refPoint[0], 10.0); + LinePlaneProjection recoilProj = new LinePlaneProjection( + recoilProjLocal.position.add(MatrixUtils.createRealVector(refPoint)), recoilProjLocal.cov); + + return makeReconstructedParticle(v0Particle, recoilElectron, result, v0Line, v0Proj, recoilProj, v0Vertex); + } + + /** + * 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); + } + + /** + * Extract perigee TrackParams (d0, phi0, omega, z0, tanLambda) from a Track's + * AtPerigee TrackState. Parameter order matches {@code TrackState.getParameters()} + * exactly (same convention as {@code BilliorTrack(Track)}), so no reordering is needed. + */ + private static TrackParams trackParamsFromTrack(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)); + } + } + return new TrackParams(par[0], par[1], par[2], par[3], par[4], cov); + } + + private static ReconstructedParticle makeReconstructedParticle(ReconstructedParticle v0Particle, + ReconstructedParticle recoilElectron, FitResult result, LineParams v0Line, + LinePlaneProjection v0Proj, LinePlaneProjection recoilProj, BilliorVertex v0InputVertex) { + + // Tracking frame -> detector frame: det(x,y,z) = trk(y,z,x), same convention used + // throughout KalmanVertexFitterGainMatrix.fitVertex(...) and BilliorVertexer. + Hep3Vector vtxPos = new BasicHep3Vector( + result.vertex.getEntry(1), + result.vertex.getEntry(2), + result.vertex.getEntry(0)); + + double[] covPacked = new double[6]; + covPacked[0] = result.vertexCov.getEntry(1, 1); + covPacked[1] = result.vertexCov.getEntry(2, 1); + covPacked[2] = result.vertexCov.getEntry(2, 2); + covPacked[3] = result.vertexCov.getEntry(0, 1); + covPacked[4] = result.vertexCov.getEntry(0, 2); + covPacked[5] = result.vertexCov.getEntry(0, 0); + SymmetricMatrix covVtx = new SymmetricMatrix(3, covPacked, true); + + Hep3Vector vtxPosErr = new BasicHep3Vector( + FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(1, 1))), + FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(2, 2))), + FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(0, 0)))); + + // Recoil electron's momentum evaluated at the fitted production vertex. + RealVector pRecoilTrk = result.trackMomenta.get(0).p; + Hep3Vector pRecoil = new BasicHep3Vector( + pRecoilTrk.getEntry(1), pRecoilTrk.getEntry(2), pRecoilTrk.getEntry(0)); + + // The V0's momentum is constant along its (zero-curvature) line, so it is + // unchanged by the fit: it is exactly the direction vector carried by v0Line. + Hep3Vector pV0 = new BasicHep3Vector(v0Line.dy, v0Line.dz, v0Line.dx); + + Map pFitMap = new HashMap(); + pFitMap.put(0, pV0); + pFitMap.put(1, pRecoil); + + double v0Mass = v0Particle.getMass(); + double v0PMag = pV0.magnitude(); + double eV0 = FastMath.sqrt(v0PMag * v0PMag + v0Mass * v0Mass); + 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; + + BilliorVertex vtxFit = new BilliorVertex(vtxPos, covVtx, result.chi2, invMass, pFitMap, "CASCADE"); + vtxFit.setPositionError(vtxPosErr); + vtxFit.setProbability(result.ndf); + vtxFit.setParameter("ndf", (double) result.ndf); + + // 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). + vtxFit.setParameter("v0ProjX", v0Proj.position.getEntry(1)); + vtxFit.setParameter("v0ProjY", v0Proj.position.getEntry(2)); + vtxFit.setParameter("v0ProjXErr", FastMath.sqrt(FastMath.abs(v0Proj.cov.getEntry(1, 1)))); + vtxFit.setParameter("v0ProjYErr", FastMath.sqrt(FastMath.abs(v0Proj.cov.getEntry(2, 2)))); + vtxFit.setParameter("recoilProjX", recoilProj.position.getEntry(1)); + vtxFit.setParameter("recoilProjY", recoilProj.position.getEntry(2)); + vtxFit.setParameter("recoilProjXErr", FastMath.sqrt(FastMath.abs(recoilProj.cov.getEntry(1, 1)))); + vtxFit.setParameter("recoilProjYErr", FastMath.sqrt(FastMath.abs(recoilProj.cov.getEntry(2, 2)))); + + // The original two-body Billior V0 fit (position, chi2, mass). Here v0Particle is + // untouched by this fit, so this duplicates v0Particle.getStartVertex(); stored under + // the same parameter names as ThreeTrackVertexer's cascade (where it does NOT + // duplicate v0Particle's startVertex) so CascadeVertexTupleDriver can read both + // outputs uniformly. + Hep3Vector v0InputPos = v0InputVertex.getPosition(); + Hep3Vector v0InputPosErr = v0InputVertex.getPositionError(); + vtxFit.setParameter("v0InputVtxX", v0InputPos.x()); + vtxFit.setParameter("v0InputVtxY", v0InputPos.y()); + vtxFit.setParameter("v0InputVtxZ", v0InputPos.z()); + vtxFit.setParameter("v0InputVtxXErr", v0InputPosErr.x()); + vtxFit.setParameter("v0InputVtxYErr", v0InputPosErr.y()); + vtxFit.setParameter("v0InputVtxZErr", v0InputPosErr.z()); + vtxFit.setParameter("v0InputChi2", v0InputVertex.getChi2()); + vtxFit.setParameter("v0InputMass", v0InputVertex.getInvMass()); + + ReconstructedParticle candidate = new BaseReconstructedParticle(); + ((BaseReconstructedParticle) candidate).setStartVertex(vtxFit); + candidate.addParticle(v0Particle); + candidate.addParticle(recoilElectron); + ((BaseReconstructedParticle) candidate).setType(recoilElectron.getType()); + ((BaseReconstructedParticle) candidate).setMass(invMass); + + HepLorentzVector fourVector = new BasicHepLorentzVector(totalE, totalP); + ((BaseReconstructedParticle) candidate).set4Vector(fourVector); + + double particleCharge = v0Particle.getCharge() + recoilElectron.getCharge(); + ((BaseReconstructedParticle) candidate).setCharge(particleCharge); + + vtxFit.setAssociatedParticle(candidate); + ((BaseReconstructedParticle) candidate).setReferencePoint(vtxFit.getPosition()); + + return candidate; + } +} diff --git a/recon/src/main/java/org/hps/recon/vertexing/KalmanNTrackVertexer.java b/recon/src/main/java/org/hps/recon/vertexing/KalmanNTrackVertexer.java new file mode 100644 index 0000000000..298cee2f00 --- /dev/null +++ b/recon/src/main/java/org/hps/recon/vertexing/KalmanNTrackVertexer.java @@ -0,0 +1,159 @@ +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.SymmetricMatrix; +import hep.physics.vec.BasicHep3Vector; +import hep.physics.vec.Hep3Vector; +import hep.physics.vec.VecOp; + +import org.lcsim.event.Track; +import org.lcsim.event.TrackState; + +import org.hps.recon.tracking.TrackStateUtils; +import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.FitResult; +import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TrackMomentum; +import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TrackParams; + +/** + * Fits a plain, unconstrained N-track common vertex (all input tracks constrained to the + * same (x,y,z) point, all assumed to be electron-mass) using the Kalman gain-matrix fitter + * ({@link KalmanVertexFitterGainMatrix}), as an alternative to {@link BilliorVertexer} for + * the same problem. This is a generalization of {@link KalmanV0Vertexer} (which is + * hardcoded to exactly 2 tracks) to arbitrary N -- e.g. for 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 KalmanNTrackVertexer { + + private static final double ELECTRON_MASS = 0.000511; + + private final double bField; + + public KalmanNTrackVertexer(double bField) { + this.bField = bField; + } + + /** + * Fit the common vertex of N tracks, with no vertex/momentum/mass constraint. All + * tracks are assumed to share the same reference point (true for AtPerigee states as + * produced by the tracking reconstruction). + * + * @param tracks the input tracks (any N >= 2) + * @return the fitted vertex, or a sentinel placeholder vertex if the fit fails + */ + public BilliorVertex fitVertex(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)); + } + + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(bField); + FitResult result = fitter.fit(trackParams); + if (result == null) { + return placeholderVertex(tracks.size()); + } + + return makeVertex(result, referencePoint); + } + + /** + * 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; + } + + /** + * Extract perigee TrackParams (d0, phi0, omega, z0, tanLambda) directly from an + * AtPerigee TrackState, with no reparametrization -- see {@link KalmanV0Vertexer}'s + * copy of this method for the full rationale (same convention here). + */ + private 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); + } + + /** + * Convert a Kalman {@code FitResult} for N tracks into a {@code BilliorVertex}, using + * the same tracking-frame -> detector-frame convention (det(x,y,z) = trk(y,z,x)) as + * {@link KalmanV0Vertexer#makeVertex}. All tracks are assumed electron-mass (valid for + * the trident use case: 2 e- + 1 e+). + * + * @param referencePoint the (shared) reference point of the input tracks, tracking + * frame; added back to {@code result.vertex} since the fitter's + * raw output is relative to that point, not the absolute frame + */ + private static BilliorVertex makeVertex(FitResult result, double[] referencePoint) { + Hep3Vector vtxPos = new BasicHep3Vector( + result.vertex.getEntry(1) + referencePoint[1], + result.vertex.getEntry(2) + referencePoint[2], + result.vertex.getEntry(0) + referencePoint[0]); + + double[] covPacked = new double[6]; + covPacked[0] = result.vertexCov.getEntry(1, 1); + covPacked[1] = result.vertexCov.getEntry(2, 1); + covPacked[2] = result.vertexCov.getEntry(2, 2); + covPacked[3] = result.vertexCov.getEntry(0, 1); + covPacked[4] = result.vertexCov.getEntry(0, 2); + covPacked[5] = result.vertexCov.getEntry(0, 0); + SymmetricMatrix covVtx = new SymmetricMatrix(3, covPacked, true); + + Hep3Vector vtxPosErr = new BasicHep3Vector( + FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(1, 1))), + FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(2, 2))), + FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(0, 0)))); + + Map pFitMap = new HashMap(); + double totalE = 0.0; + Hep3Vector totalP = new BasicHep3Vector(0, 0, 0); + for (int i = 0; i < result.trackMomenta.size(); i++) { + TrackMomentum tm = result.trackMomenta.get(i); + RealVector pTrk = tm.p; + Hep3Vector p = new BasicHep3Vector(pTrk.getEntry(1), pTrk.getEntry(2), pTrk.getEntry(0)); + pFitMap.put(i, p); + totalE += FastMath.sqrt(p.magnitudeSquared() + ELECTRON_MASS * ELECTRON_MASS); + totalP = VecOp.add(totalP, p); + } + double massSq = totalE * totalE - totalP.magnitudeSquared(); + double invMass = massSq > 0 ? FastMath.sqrt(massSq) : -99.0; + + BilliorVertex vtxFit = new BilliorVertex(vtxPos, covVtx, result.chi2, invMass, pFitMap, "KALMAN_NTRACK"); + vtxFit.setPositionError(vtxPosErr); + vtxFit.setProbability(result.ndf); + vtxFit.setParameter("ndf", (double) result.ndf); + return vtxFit; + } +} diff --git a/recon/src/main/java/org/hps/recon/vertexing/KalmanV0Vertexer.java b/recon/src/main/java/org/hps/recon/vertexing/KalmanV0Vertexer.java new file mode 100644 index 0000000000..2846184731 --- /dev/null +++ b/recon/src/main/java/org/hps/recon/vertexing/KalmanV0Vertexer.java @@ -0,0 +1,155 @@ +package org.hps.recon.vertexing; + +import java.util.Arrays; +import java.util.HashMap; +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.SymmetricMatrix; +import hep.physics.vec.BasicHep3Vector; +import hep.physics.vec.Hep3Vector; +import hep.physics.vec.VecOp; + +import org.lcsim.event.Track; +import org.lcsim.event.TrackState; + +import org.hps.recon.tracking.TrackStateUtils; +import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.FitResult; +import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TrackParams; + +/** + * Fits a plain, unconstrained two-track (e-/e+) V0 vertex using the Kalman gain-matrix + * fitter ({@link KalmanVertexFitterGainMatrix}), as an alternative to {@link + * BilliorVertexer} for the same problem. 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 the existing + * {@code HpsReconParticleDriver#makeReconstructedParticle} and tuple-reading code. + */ +public class KalmanV0Vertexer { + + private static final double ELECTRON_MASS = 0.000511; + + private final double bField; + + public KalmanV0Vertexer(double bField) { + this.bField = bField; + } + + /** + * Fit the vertex of an electron and a positron track, with no vertex/momentum/mass + * constraint. + * + * @param eleTrack the electron track + * @param posTrack the positron track + * @return the fitted vertex, or null if the fit fails + */ + public BilliorVertex fitVertex(Track eleTrack, Track posTrack) { + TrackState eleTs = TrackStateUtils.getTrackStatesAtLocation(eleTrack, TrackState.AtPerigee).get(0); + TrackParams eleParams = trackParamsFromTrack(eleTs); + TrackParams posParams = trackParamsFromTrack( + TrackStateUtils.getTrackStatesAtLocation(posTrack, TrackState.AtPerigee).get(0)); + + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(bField); + FitResult result = fitter.fit(Arrays.asList(eleParams, posParams)); + if (result == null) { + return placeholderVertex(); + } + + return makeVertex(result, eleTs.getReferencePoint()); + } + + /** + * 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 list + * index-for-index against another collection (e.g. the Billoir unconstrained V0 fit, + * for comparison) never see the two lists fall out of sync. + */ + public static BilliorVertex placeholderVertex() { + Map pFitMap = new HashMap(); + pFitMap.put(0, new BasicHep3Vector(0, 0, 0)); + pFitMap.put(1, new BasicHep3Vector(0, 0, 0)); + BilliorVertex vtxFit = new BilliorVertex(new BasicHep3Vector(0, 0, 0), + new SymmetricMatrix(3), -9999.0, -9999.0, pFitMap, "KALMAN_UNCONSTRAINED_FAILED"); + vtxFit.setPositionError(new BasicHep3Vector(0, 0, 0)); + vtxFit.setParameter("ndf", -9999.0); + return vtxFit; + } + + /** + * Extract perigee TrackParams (d0, phi0, omega, z0, tanLambda) directly from an + * AtPerigee TrackState, with no reparametrization. {@link + * KalmanVertexFitterGainMatrix}'s track-constraint math implicitly treats the vertex + * unknown as living in the same frame as the raw d0/phi0/omega/z0, so the fitted + * vertex it returns is relative to the track's reference point, not the absolute + * tracking/detector frame; {@link #makeVertex} adds that reference point back in + * afterward. Electron and positron tracks are assumed to share the same reference + * point (true for AtPerigee states as produced by the tracking reconstruction). + */ + private 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); + } + + /** + * Convert a Kalman {@code FitResult} for the (electron, positron) track pair into a + * {@code BilliorVertex}, using the same tracking-frame -> detector-frame convention + * (det(x,y,z) = trk(y,z,x)) as {@code CascadeVertexer#makeReconstructedParticle}. + * + * @param referencePoint the (shared) reference point of the input tracks, tracking + * frame; added back to {@code result.vertex} since the fitter's + * raw output is relative to that point, not the absolute frame + */ + private static BilliorVertex makeVertex(FitResult result, double[] referencePoint) { + Hep3Vector vtxPos = new BasicHep3Vector( + result.vertex.getEntry(1) + referencePoint[1], + result.vertex.getEntry(2) + referencePoint[2], + result.vertex.getEntry(0) + referencePoint[0]); + + double[] covPacked = new double[6]; + covPacked[0] = result.vertexCov.getEntry(1, 1); + covPacked[1] = result.vertexCov.getEntry(2, 1); + covPacked[2] = result.vertexCov.getEntry(2, 2); + covPacked[3] = result.vertexCov.getEntry(0, 1); + covPacked[4] = result.vertexCov.getEntry(0, 2); + covPacked[5] = result.vertexCov.getEntry(0, 0); + SymmetricMatrix covVtx = new SymmetricMatrix(3, covPacked, true); + + Hep3Vector vtxPosErr = new BasicHep3Vector( + FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(1, 1))), + FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(2, 2))), + FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(0, 0)))); + + RealVector pEleTrk = result.trackMomenta.get(0).p; + Hep3Vector pEle = new BasicHep3Vector(pEleTrk.getEntry(1), pEleTrk.getEntry(2), pEleTrk.getEntry(0)); + RealVector pPosTrk = result.trackMomenta.get(1).p; + Hep3Vector pPos = new BasicHep3Vector(pPosTrk.getEntry(1), pPosTrk.getEntry(2), pPosTrk.getEntry(0)); + + double eEle = FastMath.sqrt(pEle.magnitudeSquared() + ELECTRON_MASS * ELECTRON_MASS); + double ePos = FastMath.sqrt(pPos.magnitudeSquared() + ELECTRON_MASS * ELECTRON_MASS); + double totalE = eEle + ePos; + Hep3Vector totalP = VecOp.add(pEle, pPos); + double massSq = totalE * totalE - totalP.magnitudeSquared(); + double invMass = massSq > 0 ? FastMath.sqrt(massSq) : -99.0; + + Map pFitMap = new HashMap(); + pFitMap.put(0, pEle); + pFitMap.put(1, pPos); + + BilliorVertex vtxFit = new BilliorVertex(vtxPos, covVtx, result.chi2, invMass, pFitMap, "KALMAN_UNCONSTRAINED"); + vtxFit.setPositionError(vtxPosErr); + vtxFit.setProbability(result.ndf); + vtxFit.setParameter("ndf", (double) result.ndf); + return vtxFit; + } +} diff --git a/recon/src/main/java/org/hps/recon/vertexing/KalmanVertexFitterGainMatrix.java b/recon/src/main/java/org/hps/recon/vertexing/KalmanVertexFitterGainMatrix.java new file mode 100644 index 0000000000..b3e1e62c67 --- /dev/null +++ b/recon/src/main/java/org/hps/recon/vertexing/KalmanVertexFitterGainMatrix.java @@ -0,0 +1,3481 @@ +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 KalmanVertexFitterGainMatrix { + + 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) + + 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 + */ + private 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 KalmanVertexFitterGainMatrix(double bField) { + this.bField = bField; + } + + public KalmanVertexFitterGainMatrix() { + this(2.0); + } + + /** + * Convert perigee to vertex parameters + */ + private static class VertexParams { + double phiV, zV; + VertexParams(double phiV, double zV) { + this.phiV = phiV; + this.zV = zV; + } + } + + private 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 + */ + private 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()); + } + + /** + * 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 vertex position constraint (beamspot) + */ + 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) { + // Calculate total momentum and total momentum covariance from tracks + 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 = computeMomentumAtVertex(track, vertex); + totalP = totalP.add(p); + + RealMatrix dpDv = computeMomentumVertexDerivatives(track, vertex); + dpDvertex = dpDvertex.add(dpDv); + + // Add track momentum covariance (propagated from track parameter errors) + RealMatrix pCov = computeMomentumCovariance(track, vertex); + totalPCov = totalPCov.add(pCov); + } + + RealVector c = totalP.subtract(momentumConstraint); + RealMatrix H = dpDvertex; + // Total covariance = beam momentum uncertainty + sum of track momentum uncertainties + RealMatrix V = momentumConstraintCov.add(totalPCov); + + return new Constraint(c, H, V); + } + + /** + * Compute mass constraint + * Invariant mass: M² = (ΣE)² - (Σp)² + * + * @param tracks List of track parameters + * @param vertex Vertex position + * @param massConstraint Constrained mass value (GeV/c²) + * @param massConstraintSigma Uncertainty on mass (GeV/c²) + * @return Constraint object + */ + private Constraint computeMassConstraint(List tracks, + RealVector vertex, + double massConstraint, + double massConstraintSigma) { + // Assume pion mass for all tracks (can be extended) + double mPi = 0.13957; // GeV/c² + + 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 = computeMomentumAtVertex(track, vertex); + double pMag = p.getNorm(); + + // Energy (assuming pion mass) + double E = FastMath.sqrt(pMag * pMag + mPi * mPi); + totalE += E; + totalP = totalP.add(p); + + // Derivatives of E w.r.t. vertex + // E = sqrt(p² + m²), so dE/dvertex = (p · dp/dvertex) / E + RealMatrix dpDv = 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); + } + + // Invariant mass + double totalPmag = totalP.getNorm(); + double M = FastMath.sqrt(totalE * totalE - totalPmag * totalPmag); + + // Constraint residual + RealVector c = MatrixUtils.createRealVector(new double[]{M - massConstraint}); + + // Derivatives of M w.r.t. vertex + // M² = E² - p², so 2M dM = 2E dE - 2p·dp + // dM/dvertex = (E * dE/dvertex - p · dp/dvertex) / M + 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); + + // Covariance (1x1 matrix) + RealMatrix V = MatrixUtils.createRealMatrix(1, 1); + V.setEntry(0, 0, massConstraintSigma * massConstraintSigma); + + 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) + */ + private 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 + */ + private RealMatrix computeMomentumCovariance(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.multiply(track.cov).multiply(Jp.transpose()); + } + + /** + * Fit vertex using Gain Matrix formalism + */ + 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); + + // Gain matrix: K = C * H^T * (H * C * H^T + V)^-1 + 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() + ); + + // Update: vertex = vertex - K * c + vertex = vertex.subtract(K.operate(constraint.c)); + + // Joseph-form covariance update: numerically stable under re-linearization + // across iterations, unlike the simple (I-KH)*C form which can + // systematically underestimate 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 = 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 = 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 + } + } + + // Print chi2 contributions + 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 = computeMomentumAtVertex(track, vertex); + RealMatrix pCov = computeMomentumCovariance(track, vertex); + trackMomenta.add(new TrackMomentum(p, pCov)); + } + + this.vertex = vertex; + this.vertexCov = C; + this.chi2 = chi2; + this.ndf = ndf; + this.trackMomenta = trackMomenta; + + return new FitResult(vertex, C, chi2, ndf, trackMomenta); + } + + /** + * Fit a "cascade" (production) vertex where a curving recoil track meets an + * already-fitted neutral V0's momentum direction, treated as a zero-curvature + * line. Same sequential Kalman gain-matrix update as {@link #fit}, specialized to + * exactly one track constraint and one line constraint (no vertex/momentum/mass + * constraints). NDF = (2 track-constraint dof + 2 line-constraint dof) - 3 (vertex + * position dof) = 1. + */ + public FitResult fitCascadeVertex(TrackParams recoilTrack, LineParams v0Line, + RealVector initialVertex, int maxIterations, double tolerance) { + RealVector vertex = (initialVertex != null) + ? initialVertex.copy() + : MatrixUtils.createRealVector(new double[]{v0Line.x0, v0Line.y0, v0Line.z0}); + + RealMatrix priorC = MatrixUtils.createRealIdentityMatrix(3).scalarMultiply(100.0); + RealMatrix I = MatrixUtils.createRealIdentityMatrix(3); + RealMatrix C = priorC; + + for (int iteration = 0; iteration < maxIterations; iteration++) { + RealVector vertexOld = vertex.copy(); + // Reset to the vague prior each iteration: the track and line constraints are + // the same two 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) -- exactly the + // ~20x variance underestimate (~4.5x pull-std inflation) seen before this fix. + C = priorC; + + Constraint trackConstraint = computeTrackConstraint(recoilTrack, vertex); + RealMatrix St = trackConstraint.H.multiply(C).multiply(trackConstraint.H.transpose()).add(trackConstraint.V); + RealMatrix Kt = C.multiply(trackConstraint.H.transpose()).multiply( + new LUDecomposition(St).getSolver().getInverse()); + vertex = vertex.subtract(Kt.operate(trackConstraint.c)); + // Joseph-form covariance update: numerically stable under re-linearization across + // iterations, unlike the simple (I-KH)*C form which can systematically underestimate C. + RealMatrix ImKHt = I.subtract(Kt.multiply(trackConstraint.H)); + C = ImKHt.multiply(C).multiply(ImKHt.transpose()) + .add(Kt.multiply(trackConstraint.V).multiply(Kt.transpose())); + + Constraint lineConstraint = computeLineConstraint(v0Line, vertex); + RealMatrix Sl = lineConstraint.H.multiply(C).multiply(lineConstraint.H.transpose()).add(lineConstraint.V); + RealMatrix Kl = C.multiply(lineConstraint.H.transpose()).multiply( + new LUDecomposition(Sl).getSolver().getInverse()); + vertex = vertex.subtract(Kl.operate(lineConstraint.c)); + RealMatrix ImKHl = I.subtract(Kl.multiply(lineConstraint.H)); + C = ImKHl.multiply(C).multiply(ImKHl.transpose()) + .add(Kl.multiply(lineConstraint.V).multiply(Kl.transpose())); + + if (vertex.subtract(vertexOld).getNorm() < tolerance) { + break; + } + } + + Constraint trackConstraint = computeTrackConstraint(recoilTrack, vertex); + RealMatrix VInvT = new LUDecomposition(trackConstraint.V).getSolver().getInverse(); + double chi2Track = trackConstraint.c.dotProduct(VInvT.operate(trackConstraint.c)); + + Constraint lineConstraint = computeLineConstraint(v0Line, vertex); + RealMatrix VInvL = new LUDecomposition(lineConstraint.V).getSolver().getInverse(); + double chi2Line = lineConstraint.c.dotProduct(VInvL.operate(lineConstraint.c)); + + double chi2 = chi2Track + chi2Line; + int ndf = 1; + + List trackMomenta = new ArrayList<>(); + RealVector p = computeMomentumAtVertex(recoilTrack, vertex); + RealMatrix pCov = computeMomentumCovariance(recoilTrack, vertex); + trackMomenta.add(new TrackMomentum(p, pCov)); + + this.vertex = vertex; + this.vertexCov = C; + this.chi2 = chi2; + this.ndf = ndf; + this.trackMomenta = trackMomenta; + + return new FitResult(vertex, C, chi2, ndf, trackMomenta); + } + + public FitResult fitCascadeVertex(TrackParams recoilTrack, LineParams v0Line) { + return fitCascadeVertex(recoilTrack, v0Line, null, 20, 1.0e-8); + } + + /** + * 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; + + 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}/{@link CascadeVertexer}) -- 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 ThreeTrackVertexer); 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.ThreeTrackVertexer#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.ThreeTrackVertexer}. + */ + 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); + } + + 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); + } + + 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 ThreeTrackVertexer#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) { + RealMatrix vertexCovInv = new LUDecomposition(vertexConstraintCov).getSolver().getInverse(); + 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. + // For each track, V is a 2x2 block: V = J_h * trackCov * J_h^T + // where J_h has rows [d(h_t)/d(params), d(h_z)/d(params)]: + // h_t = r - R -> d(h_t)/d(params) = +Jft (positive) + // h_z = zV - zPred -> d(h_z)/d(params) = -Jz (negative) + // For momentum constraints: use the provided fourMomentumConstraintCov + RealMatrix constraintCov = MatrixUtils.createRealMatrix(nConstraints, nConstraints); + + // Track constraint covariances: 2x2 block per track + for (int i = 0; i < nTracks; i++) { + TrackParams track = tracks.get(i); + int rowT = 2 * i; + int rowZ = 2 * i + 1; + + 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 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); + + // d(h_t)/d(track params): Jft (positive) + double[] Jht = new double[5]; + Jht[0] = (dx * FastMath.sin(track.phi0) - dy * FastMath.cos(track.phi0)) / r; + Jht[1] = -(sign * R - track.d0) * (dx * FastMath.cos(track.phi0) + dy * FastMath.sin(track.phi0)) / r; + Jht[2] = (dx * FastMath.sin(track.phi0) - dy * FastMath.cos(track.phi0)) / (r * track.omega * track.omega) + + sign / (track.omega * track.omega); + Jht[3] = 0.0; + Jht[4] = 0.0; + + // d(h_z)/d(track params): -Jz (negative, since h_z = zV - zPred) + 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_v = phiV - track.phi0; + while (dphi_v > FastMath.PI) dphi_v -= 2.0 * FastMath.PI; + while (dphi_v < -FastMath.PI) dphi_v += 2.0 * FastMath.PI; + double s = -sign * R * dphi_v; + // 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[] Jhz = new double[5]; + Jhz[0] = -(-sign * track.tanLambda * R * dphiDd0); + Jhz[1] = -(-sign * (-track.tanLambda * R + track.tanLambda * R * dphiDphi0)); + Jhz[2] = -(-s * track.tanLambda / track.omega - sign * track.tanLambda * R * dphiDomega); + Jhz[3] = -1.0; + Jhz[4] = -s; + + // V_2x2 = J_h * trackCov * J_h^T + double V_tt = 0, V_zz = 0, V_tz = 0; + for (int a = 0; a < 5; a++) { + for (int b = 0; b < 5; b++) { + double c = track.cov.getEntry(a, b); + V_tt += Jht[a] * c * Jht[b]; + V_zz += Jhz[a] * c * Jhz[b]; + V_tz += Jht[a] * c * Jhz[b]; + } + } + constraintCov.setEntry(rowT, rowT, V_tt); + constraintCov.setEntry(rowT, rowZ, V_tz); + constraintCov.setEntry(rowZ, rowT, V_tz); + constraintCov.setEntry(rowZ, rowZ, V_zz); + + if (debugFlag && iteration == 0) { + System.out.printf(" Track %d transverse sigma = %.4f mm, z sigma = %.4f mm%n", + i, FastMath.sqrt(V_tt), FastMath.sqrt(V_zz)); + } + } + + // 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 + int ndf = nConstraints; + + // 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)); + } + + // 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)); + } + } + + return new FitResult(vertex, vertexCov, chi2, ndf, trackMomenta, fittedTracks); + } + + /** + * 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 + */ + 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); + } + + // Simplified methods + 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); + } + + // 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; + + 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 setDebug(boolean debug) { this.debugFlag = debug; } + public void setStoreCovTrkMomList(boolean value) { this.storeCovTrkMomList = value; } + + /** + * 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 + * @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 + // Detector frame: beam along HPS Z, rotated by rotAngle in HPS X-Z plane + // Tracking frame: X=HPS_Z, Y=HPS_X, Z=HPS_Y + 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 + double me = 0.000511; // electron mass in GeV + double eBeam = FastMath.sqrt(pBeam * pBeam + me * me); + fourMomentumConstraintVec = MatrixUtils.createRealVector(new double[]{pxBeam, pyBeam, pzBeam, eBeam}); + + double dpOverP = 1e-2; + double sigmaTheta = 100e-6; // beam angular divergence (rad) + 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; + fourMomentumConstraintCovMat = MatrixUtils.createRealMatrix(4, 4); + fourMomentumConstraintCovMat.setEntry(0, 0, sL2 * cosR * cosR + sT2 * sinR * sinR); + fourMomentumConstraintCovMat.setEntry(0, 1, (sT2 - sL2) * sinR * cosR); + fourMomentumConstraintCovMat.setEntry(1, 0, (sT2 - sL2) * sinR * cosR); + fourMomentumConstraintCovMat.setEntry(1, 1, sL2 * sinR * sinR + sT2 * cosR * cosR); + fourMomentumConstraintCovMat.setEntry(2, 2, sT2); + double sigmaE = dpOverP * eBeam; + 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); + + // 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("=== KalmanVertexFitterGainMatrix::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 KalmanVertexFitterGainMatrix::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; } + + /** + * Fit vertex using N-1 tracks and predict the momentum of the Nth track + * + * This method uses vertex and/or total momentum constraints to fit with a subset + * of tracks, then predicts the momentum of the excluded track. + * + * @param tracks All track parameters including the one to predict + * @param predictTrackIdx Index of track to predict (0-based) + * @param vertexConstraint Known vertex position + * @param vertexConstraintCov Vertex constraint covariance + * @param totalMomentumConstraint Known total momentum of ALL tracks + * @param totalMomentumConstraintCov Total momentum constraint covariance + * @param massConstraint Invariant mass constraint for ALL tracks + * @param massConstraintSigma Mass constraint uncertainty + * @param maxIterations Maximum iterations + * @param tolerance Convergence tolerance + * @return PredictedTrackResult containing vertex, predicted momentum, and all track momenta + */ + public PredictedTrackResult fitWithPredictedTrack( + List tracks, + int predictTrackIdx, + RealVector vertexConstraint, + RealMatrix vertexConstraintCov, + RealVector totalMomentumConstraint, + RealMatrix totalMomentumConstraintCov, + Double massConstraint, + Double massConstraintSigma, + int maxIterations, + double tolerance) { + + int nTracks = tracks.size(); + + if (predictTrackIdx < 0 || predictTrackIdx >= nTracks) { + throw new IllegalArgumentException( + "predictTrackIdx must be between 0 and " + (nTracks - 1)); + } + + // Fit all N tracks to get the best vertex - do NOT use momentum constraint here; + // we use momentum conservation to PREDICT the excluded track's momentum + FitResult fitResult = fit( + tracks, null, vertexConstraint, vertexConstraintCov, + null, null, // No momentum constraint for N-track fit + null, null, // No mass constraint either + maxIterations, tolerance + ); + + return fitWithPredictedTrack(fitResult, predictTrackIdx, + totalMomentumConstraint, totalMomentumConstraintCov, + massConstraint, massConstraintSigma); + } + + /** + * Predict the momentum of one track using momentum conservation, given a pre-computed + * vertex fit result. The vertex fit must already include all tracks. + * Use this overload to avoid recomputing the vertex fit for each predicted track. + * + * @param precomputedFit Result of a prior fit to all N tracks + * @param predictTrackIdx Index of the track whose momentum is to be predicted + * @param totalMomentumConstraint Known total momentum (beam momentum) + * @param totalMomentumConstraintCov Covariance on total momentum + * @param massConstraint Invariant mass constraint (null to skip) + * @param massConstraintSigma Mass constraint uncertainty (null to skip) + * @return PredictedTrackResult containing vertex, predicted momentum, and all track momenta + */ + public PredictedTrackResult fitWithPredictedTrack( + FitResult precomputedFit, + int predictTrackIdx, + RealVector totalMomentumConstraint, + RealMatrix totalMomentumConstraintCov, + Double massConstraint, + Double massConstraintSigma) { + + int nTracks = precomputedFit.trackMomenta.size(); + + if (predictTrackIdx < 0 || predictTrackIdx >= nTracks) { + throw new IllegalArgumentException( + "predictTrackIdx must be between 0 and " + (nTracks - 1)); + } + + FitResult fitResult = precomputedFit; + + // Predict momentum of excluded track + RealVector predictedP; + RealMatrix predictedPCov; + + if (totalMomentumConstraint != null) { + // Use momentum conservation: p_predicted = p_total - sum(p_fitted for other tracks) + RealVector fittedTotalP = MatrixUtils.createRealVector(new double[3]); + RealMatrix fittedPCov = MatrixUtils.createRealMatrix(3, 3); + for (int i = 0; i < nTracks; i++) { + if (i == predictTrackIdx) continue; + TrackMomentum mom = fitResult.trackMomenta.get(i); + fittedTotalP = fittedTotalP.add(mom.p); + fittedPCov = fittedPCov.add(mom.pCov); + if(debugFlag) + System.out.printf("DEBUG fitWithPredictedTrack: fitted track %d p (tracking) = [%.4f, %.4f, %.4f] |p|=%.4f%n", + i, mom.p.getEntry(0), mom.p.getEntry(1), mom.p.getEntry(2), mom.p.getNorm()); + } + predictedP = totalMomentumConstraint.subtract(fittedTotalP); + if(debugFlag){ + System.out.printf("DEBUG fitWithPredictedTrack: total fitted p (tracking) = [%.4f, %.4f, %.4f] |p|=%.4f%n", + fittedTotalP.getEntry(0), fittedTotalP.getEntry(1), fittedTotalP.getEntry(2), fittedTotalP.getNorm()); + System.out.printf("DEBUG fitWithPredictedTrack: momentum constraint (tracking) = [%.4f, %.4f, %.4f] |p|=%.4f%n", + totalMomentumConstraint.getEntry(0), totalMomentumConstraint.getEntry(1), totalMomentumConstraint.getEntry(2), + totalMomentumConstraint.getNorm()); + System.out.printf("DEBUG fitWithPredictedTrack: predicted p (tracking) = [%.4f, %.4f, %.4f] |p|=%.4f%n", + predictedP.getEntry(0), predictedP.getEntry(1), predictedP.getEntry(2), predictedP.getNorm()); + } + // Propagate uncertainty: Cov(p_pred) = Cov(p_total) + Cov(sum p_fitted for other tracks) + // Note: This assumes uncorrelated track momenta (ignores vertex correlations) + + if (totalMomentumConstraintCov != null) { + predictedPCov = totalMomentumConstraintCov.add(fittedPCov); + } else { + predictedPCov = fittedPCov; + } + + } else { + // Without momentum constraint, use the fitted momentum of the predicted track + predictedP = fitResult.trackMomenta.get(predictTrackIdx).p; + predictedPCov = fitResult.trackMomenta.get(predictTrackIdx).pCov; + } + + // Create momentum info for predicted track + TrackMomentum predictedMom = new TrackMomentum(predictedP, predictedPCov); + + // All track momenta in original order; replace predicted track entry with + // the conservation-predicted momentum (actual fitted momentum stored in actualP) + List allTrackMomenta = new ArrayList<>(fitResult.trackMomenta); + allTrackMomenta.set(predictTrackIdx, predictedMom); + + // Calculate total chi2: + // Start with chi2 from fitting all N tracks + double chi2Total = fitResult.chi2; + if(debugFlag) + System.out.printf("DEBUG fitWithPredictedTrack: N-track fit chi2=%.2f ndf=%d%n", fitResult.chi2, fitResult.ndf); + + // Add chi2 contribution from comparing predicted momentum to actual fitted track momentum + RealVector actualP = fitResult.trackMomenta.get(predictTrackIdx).p; + RealMatrix actualPCov = fitResult.trackMomenta.get(predictTrackIdx).pCov; + + if(debugFlag) + System.out.printf("DEBUG fitWithPredictedTrack: actual p (tracking) = [%.4f, %.4f, %.4f] |p|=%.4f%n", + actualP.getEntry(0), actualP.getEntry(1), actualP.getEntry(2), actualP.getNorm()); + + // Residual: predicted - actual + RealVector pResidual = predictedP.subtract(actualP); + if(debugFlag) + System.out.printf("DEBUG fitWithPredictedTrack: residual (pred-act) = [%.4f, %.4f, %.4f]%n", + pResidual.getEntry(0), pResidual.getEntry(1), pResidual.getEntry(2)); + + // Combined covariance for the comparison + RealMatrix combinedCov = predictedPCov.add(actualPCov); + + // Chi2 contribution from momentum comparison + double chi2Momentum = 0; + try { + RealMatrix combinedCovInv = new LUDecomposition(combinedCov).getSolver().getInverse(); + chi2Momentum = pResidual.dotProduct(combinedCovInv.operate(pResidual)); + chi2Total += chi2Momentum; + if(debugFlag) + System.out.printf("DEBUG fitWithPredictedTrack: chi2 from momentum comparison = %.2f%n", chi2Momentum); + } catch (SingularMatrixException e) { + // Combined covariance is singular; skip this chi2 contribution silently + } + if(debugFlag) + System.out.printf("DEBUG fitWithPredictedTrack: total chi2 = %.2f%n", chi2Total); + + // ndf: from N-track fit plus 3 for the 3-component momentum comparison + int ndfTotal = fitResult.ndf + 3; + + this.vertex = fitResult.vertex; + this.vertexCov = fitResult.vertexCov; + this.chi2 = chi2Total; + this.ndf = ndfTotal; + this.trackMomenta = allTrackMomenta; + + return new PredictedTrackResult( + fitResult.vertex, fitResult.vertexCov, + predictedP, predictedPCov, + actualP, actualPCov, + pResidual, + chi2Total, ndfTotal, allTrackMomenta + ); + } + + /** + * Convert a PredictedTrackResult to a BilliorVertex + * + * @param result The predicted track result + * @param predictedTrackIdx Index of the predicted track (0=ele, 1=pos, 2=rec) + * @param label Label for the vertex type + * @return BilliorVertex containing the fit results + */ + public BilliorVertex predictedResultToBilliorVertex(PredictedTrackResult result, int predictedTrackIdx, String label) { + // Tracking frame to detector frame: HPS X = TRACK Y, HPS Y = TRACK Z, HPS Z = TRACK X + double vtxX = result.vertex.getEntry(1); // tracking Y -> HPS X + double vtxY = result.vertex.getEntry(2); // tracking Z -> HPS Y + double vtxZ = result.vertex.getEntry(0); // tracking X -> HPS Z + hep.physics.vec.Hep3Vector vtxPos = new hep.physics.vec.BasicHep3Vector(vtxX, vtxY, vtxZ); + + // Convert covariance matrix (tracking -> detector frame) + double[] covPacked = new double[6]; + 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.allTrackMomenta.size(); i++) { + RealVector p = result.allTrackMomenta.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.setProbability(result.ndf); + bv.setParameter("ndf", (double) result.ndf); + bv.setPositionError(vtxPosErr); + + // Store predicted momentum (converted to detector frame) + // Tracking frame: (px_trk, py_trk, pz_trk) -> Detector frame: (py_trk, pz_trk, px_trk) + double predPx = result.predictedMomentum.getEntry(1); // trk Y -> det X + double predPy = result.predictedMomentum.getEntry(2); // trk Z -> det Y + double predPz = result.predictedMomentum.getEntry(0); // trk X -> det Z + double predPtot = Math.sqrt(predPx*predPx + predPy*predPy + predPz*predPz); + if(debugFlag){ + System.out.printf("DEBUG toBilliorVertex: predicted p (tracking) = [%.4f, %.4f, %.4f]%n", + result.predictedMomentum.getEntry(0), result.predictedMomentum.getEntry(1), result.predictedMomentum.getEntry(2)); + System.out.printf("DEBUG toBilliorVertex: predicted p (detector) = [%.4f, %.4f, %.4f] |p|=%.4f%n", + predPx, predPy, predPz, predPtot); + } + bv.setParameter("predictedPx", predPx); + bv.setParameter("predictedPy", predPy); + bv.setParameter("predictedPz", predPz); + + // Store actual momentum (converted to detector frame) + double actPx = result.actualMomentum.getEntry(1); + double actPy = result.actualMomentum.getEntry(2); + double actPz = result.actualMomentum.getEntry(0); + double actPtot = Math.sqrt(actPx*actPx + actPy*actPy + actPz*actPz); + if(debugFlag){ + System.out.printf("DEBUG toBilliorVertex: actual p (tracking) = [%.4f, %.4f, %.4f]%n", + result.actualMomentum.getEntry(0), result.actualMomentum.getEntry(1), result.actualMomentum.getEntry(2)); + System.out.printf("DEBUG toBilliorVertex: actual p (detector) = [%.4f, %.4f, %.4f] |p|=%.4f%n", + actPx, actPy, actPz, actPtot); + } + bv.setParameter("actualPx", actPx); + bv.setParameter("actualPy", actPy); + bv.setParameter("actualPz", actPz); + + // Store residuals (converted to detector frame) + double resPx = result.momentumResidual.getEntry(1); + double resPy = result.momentumResidual.getEntry(2); + double resPz = result.momentumResidual.getEntry(0); + bv.setParameter("residualPx", resPx); + bv.setParameter("residualPy", resPy); + bv.setParameter("residualPz", resPz); + + // Store predicted momentum covariance (converted to detector frame) + { + RealMatrix pCov = result.predictedMomentumCov; + int[] map = {1, 2, 0}; // detector index -> tracking index + bv.setParameter("predictedMomCovXX", pCov.getEntry(map[0], map[0])); + bv.setParameter("predictedMomCovXY", pCov.getEntry(map[0], map[1])); + bv.setParameter("predictedMomCovXZ", pCov.getEntry(map[0], map[2])); + bv.setParameter("predictedMomCovYY", pCov.getEntry(map[1], map[1])); + bv.setParameter("predictedMomCovYZ", pCov.getEntry(map[1], map[2])); + bv.setParameter("predictedMomCovZZ", pCov.getEntry(map[2], map[2])); + } + + // Store actual momentum covariance (converted to detector frame) + { + RealMatrix pCov = result.actualMomentumCov; + int[] map = {1, 2, 0}; // detector index -> tracking index + bv.setParameter("actualMomCovXX", pCov.getEntry(map[0], map[0])); + bv.setParameter("actualMomCovXY", pCov.getEntry(map[0], map[1])); + bv.setParameter("actualMomCovXZ", pCov.getEntry(map[0], map[2])); + bv.setParameter("actualMomCovYY", pCov.getEntry(map[1], map[1])); + bv.setParameter("actualMomCovYZ", pCov.getEntry(map[1], map[2])); + bv.setParameter("actualMomCovZZ", pCov.getEntry(map[2], map[2])); + } + + // Store which track was predicted + bv.setParameter("predictedTrackIdx", (double) predictedTrackIdx); + + // Store momentum covariances if requested + if (storeCovTrkMomList) { + java.util.List covTrkMomList = new java.util.ArrayList<>(); + for (int i = 0; i < result.allTrackMomenta.size(); i++) { + RealMatrix pCov = result.allTrackMomenta.get(i).pCov; + // Reorder tracking -> detector frame + 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); + } + + return bv; + } + + /** + * Result from fitting with predicted track + */ + public static class PredictedTrackResult { + public RealVector vertex; + public RealMatrix vertexCov; + public RealVector predictedMomentum; + public RealMatrix predictedMomentumCov; + public RealVector actualMomentum; // Measured momentum of excluded track + public RealMatrix actualMomentumCov; + public RealVector momentumResidual; // predicted - actual + public double chi2; + public int ndf; + public List allTrackMomenta; + + public PredictedTrackResult(RealVector vertex, RealMatrix vertexCov, + RealVector predictedMomentum, RealMatrix predictedMomentumCov, + RealVector actualMomentum, RealMatrix actualMomentumCov, + RealVector momentumResidual, + double chi2, int ndf, List allTrackMomenta) { + this.vertex = vertex; + this.vertexCov = vertexCov; + this.predictedMomentum = predictedMomentum; + this.predictedMomentumCov = predictedMomentumCov; + this.actualMomentum = actualMomentum; + this.actualMomentumCov = actualMomentumCov; + this.momentumResidual = momentumResidual; + this.chi2 = chi2; + this.ndf = ndf; + this.allTrackMomenta = allTrackMomenta; + } + } +} diff --git a/recon/src/main/java/org/hps/recon/vertexing/ThreeTrackVertexer.java b/recon/src/main/java/org/hps/recon/vertexing/ThreeTrackVertexer.java new file mode 100644 index 0000000000..7a7302f210 --- /dev/null +++ b/recon/src/main/java/org/hps/recon/vertexing/ThreeTrackVertexer.java @@ -0,0 +1,400 @@ +package org.hps.recon.vertexing; + +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.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.KalmanVertexFitterGainMatrix.LineParams; +import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.LinePlaneProjection; +import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TrackParams; +import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.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 KalmanVertexFitterGainMatrix#fitCascadeVertexJoint} and + * packages the result as a 2-daughter cascade ReconstructedParticle (inner V0 particle at + * V1, recoil electron) nested exactly like {@link CascadeVertexer}'s output, so existing + * consumers of that output shape (e.g. {@code CascadeVertexTupleDriver}) work unchanged. + */ +public class ThreeTrackVertexer { + + private static final double ELECTRON_MASS = 0.000511; + + private final double bField; + + // 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; + + public ThreeTrackVertexer(double bField) { + this.bField = bField; + } + + public void setFixV2BeamCoordinate(boolean fixV2BeamCoordinate) { + this.fixV2BeamCoordinate = fixV2BeamCoordinate; + } + + /** + * 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 + * @param truthMatched whether this (v0Particle, recoilElectron) combination is the + * MC-truth-matched signal pairing; gates the BADFIT_DEBUG print + * below so it isn't flooded by wrong-pairing combinatorics that + * are expected to fit badly. Callers without truth info should + * pass {@code true} to preserve the original always-print behavior. + * @return the cascade ReconstructedParticle (V0 particle + recoil electron), or null + * if the fit fails + */ + public ReconstructedParticle fit(ReconstructedParticle v0Particle, ReconstructedParticle recoilElectron, boolean truthMatched) { + 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, same utility already + // used by CascadeVertexer). 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 = KalmanVertexFitterGainMatrix.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. + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(bField); + LinePlaneProjection v0Proj = KalmanVertexFitterGainMatrix.propagateLineToPlane( + v0Line, fitter.getBeamPosition()[0], 10.0); + LinePlaneProjection v2Proj = KalmanVertexFitterGainMatrix.propagateLineToPlane( + v0LineLocal, fitter.getBeamPosition()[0] - refPoint[0], 10.0); + + // 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 = KalmanVertexFitterGainMatrix.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 = KalmanVertexFitterGainMatrix.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, v2Proj.position, v2FixedX, v1Cov, v2Proj.cov) + : fitter.fitCascadeVertexJoint( + eleParams, posParams, recoilParams, v1Init, v2Proj.position, v1Cov, v2Proj.cov); + } 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, v2Proj.cov, 60, 1.0e-8) + : fitter.fitCascadeVertexJointForcedBranch( + eleParams, posParams, recoilParams, v1Init, theta, v2Forced, v1Cov, v2Proj.cov, 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; + } + RealVector refPointVec = MatrixUtils.createRealVector(refPoint); + result.v1 = result.v1.add(refPointVec); + result.v2 = result.v2.add(refPointVec); + if (truthMatched && result.chi2 / result.ndf > 1000) { + System.out.println("BADFIT_DEBUG bField=" + bField); + System.out.println("BADFIT_DEBUG v1Init=" + v1Init); + System.out.println("BADFIT_DEBUG v1Cov=" + v1Cov); + System.out.println("BADFIT_DEBUG v2Init=" + v2Proj.position); + System.out.println("BADFIT_DEBUG v2Cov=" + v2Proj.cov); + System.out.println("BADFIT_DEBUG eleParams=" + dumpTrackParams(eleParams)); + System.out.println("BADFIT_DEBUG posParams=" + dumpTrackParams(posParams)); + System.out.println("BADFIT_DEBUG recoilParams=" + dumpTrackParams(recoilParams)); + System.out.println("BADFIT_DEBUG chi2=" + result.chi2 + " ndf=" + result.ndf); + System.out.println("BADFIT_DEBUG v1=" + result.v1 + " v2=" + result.v2); + } + + return makeReconstructedParticle(result, eleDaughter, posDaughter, recoilElectron, v0Proj, recoilProj, v0Vertex); + } + + /** + * 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 String dumpTrackParams(TrackParams tp) { + double[] a = tp.toArray(); + StringBuilder sb = new StringBuilder(); + sb.append("d0=").append(a[0]).append(" phi0=").append(a[1]).append(" omega=").append(a[2]) + .append(" z0=").append(a[3]).append(" tanLambda=").append(a[4]).append(" cov=["); + for (int i = 0; i < 5; i++) { + for (int j = 0; j < 5; j++) { + sb.append(tp.cov.getEntry(i, j)); + if (j < 4) { + sb.append(","); + } + } + if (i < 4) { + sb.append(";"); + } + } + sb.append("]"); + return sb.toString(); + } + + /** + * Extract perigee TrackParams (d0, phi0, omega, z0, tanLambda) from a Track's + * AtPerigee TrackState. Duplicated from {@link CascadeVertexer}'s private helper of + * the same name rather than shared, matching that class's own convention. + */ + private static TrackParams trackParamsFromTrack(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)); + } + } + return new TrackParams(par[0], par[1], par[2], par[3], par[4], 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 KalmanVertexFitterGainMatrix.fitVertex(...) and CascadeVertexer. + 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); + + 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, identical nesting + // to CascadeVertexer's output. + 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); + + // 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/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..487730e0e2 --- /dev/null +++ b/recon/src/test/java/org/hps/recon/vertexing/CascadeVertexerTest.java @@ -0,0 +1,172 @@ +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 already-fitted V0 + * BilliorVertex whose position exactly equals a known point (so its flight line trivially + * passes through that point regardless of direction) and a recoil-electron track whose helix + * is constructed to pass through the same point, with the recoil track's AtPerigee reference + * point set away from the origin (as in real reconstructed tracks) -- verifies the fitted + * production vertex correctly lands on that point 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() { + // Target point, tracking frame (index0=detZ, index1=detX, index2=detY). + double xV = 5.0, yV = 0.3, zV = -0.2; + double[] refPoint = {-1.1, 0.0, 0.0}; + + Hep3Vector vtxPosDet = new BasicHep3Vector(yV, zV, xV); + Hep3Vector p1Det = new BasicHep3Vector(0.05, 0.02, 0.3); + Hep3Vector p2Det = new BasicHep3Vector(0.03, -0.01, 0.25); + + ReconstructedParticle v0Particle = makeV0Particle(vtxPosDet, p1Det, p2Det); + + double pxRecoil = 0.15, pyRecoil = -0.05, pzRecoil = 0.8; + Track recoilTrack = makeTrackThroughPoint(xV, yV, zV, pxRecoil, pyRecoil, pzRecoil, -1, refPoint); + ReconstructedParticle recoilElectron = makeElectronParticle(recoilTrack); + + CascadeVertexer vertexer = new CascadeVertexer(B_FIELD); + ReconstructedParticle candidate = vertexer.fit(v0Particle, recoilElectron); + + assertNotNull("cascade fit should not be null", candidate); + + BilliorVertex vtx = (BilliorVertex) candidate.getStartVertex(); + Hep3Vector pos = vtx.getPosition(); + System.out.printf("Cascade 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); + } + + private static ReconstructedParticle makeV0Particle(Hep3Vector vtxPosDet, Hep3Vector p1Det, Hep3Vector p2Det) { + 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 v0Particle = new BaseReconstructedParticle(); + v0Particle.setStartVertex(v0Vertex); + v0Particle.setMass(0.05); + v0Particle.setCharge(0); + 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 KalmanV0VertexerTest#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/KalmanV0VertexerTest.java b/recon/src/test/java/org/hps/recon/vertexing/KalmanV0VertexerTest.java new file mode 100644 index 0000000000..1699ab9980 --- /dev/null +++ b/recon/src/test/java/org/hps/recon/vertexing/KalmanV0VertexerTest.java @@ -0,0 +1,166 @@ +package org.hps.recon.vertexing; + +import hep.physics.matrix.SymmetricMatrix; +import hep.physics.vec.Hep3Vector; + +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 KalmanV0Vertexer}, mirroring + * {@code KalmanVertexFitterGainMatrixTest#testCascadeVertexExactGeometry}: 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 fitVertex} recovers that point + * with near-zero chi2. + */ +public class KalmanV0VertexerTest 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); + + KalmanV0Vertexer vertexer = new KalmanV0Vertexer(B_FIELD); + BilliorVertex vtx = vertexer.fitVertex(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); + + KalmanV0Vertexer vertexer = new KalmanV0Vertexer(B_FIELD); + BilliorVertex vtx = vertexer.fitVertex(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) + 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); + } + + public void testPlaceholderVertexOnFailedFit() { + BilliorVertex placeholder = KalmanV0Vertexer.placeholderVertex(); + 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 KalmanVertexFitterGainMatrixTest#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/KalmanVertexFitterGainMatrixTest.java b/recon/src/test/java/org/hps/recon/vertexing/KalmanVertexFitterGainMatrixTest.java new file mode 100644 index 0000000000..52aaeca4d1 --- /dev/null +++ b/recon/src/test/java/org/hps/recon/vertexing/KalmanVertexFitterGainMatrixTest.java @@ -0,0 +1,1384 @@ +package org.hps.recon.vertexing; + +import junit.framework.TestCase; + +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.KalmanVertexFitterGainMatrix; +import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TrackParams; +import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.FitResult; +import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TwoVertexFitResult; + +import java.util.ArrayList; +import java.util.List; + +/** + * Test cases for KalmanVertexFitterGainMatrix, particularly the Lagrange multiplier + * constrained fitting for three-prong vertices with 4-momentum conservation. + */ +public class KalmanVertexFitterGainMatrixTest 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"); + + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(B_FIELD); + fitter.setDebug(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 + FitResult result = fitter.fit(tracks, 10, 1e-6); + + 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"); + + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(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 + + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(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 cascade (V0-line + recoil-track) vertex fit: + * construct a helix and a line that are both built to pass exactly through the same + * point, with small (non-singular) measurement covariances, and verify + * fitCascadeVertex recovers that point. + */ + public void testCascadeVertexExactGeometry() { + System.out.println("\n=== testCascadeVertexExactGeometry ===\n"); + + double xV = 0.3, yV = -0.2, zV = 5.0; + + RealMatrix trackCov = createTrackCovariance(1e-4, 1e-5, 1e-8, 1e-4, 1e-5); + TrackParams recoilTrack = createExactTrackThroughPoint( + xV, yV, zV, 0.15, -0.05, 1.0, -1, B_FIELD, trackCov); + + RealMatrix lineCov = MatrixUtils.createRealIdentityMatrix(6).scalarMultiply(1e-8); + KalmanVertexFitterGainMatrix.LineParams v0Line = + new KalmanVertexFitterGainMatrix.LineParams(xV, yV, zV, 0.10, 0.03, 2.5, lineCov); + + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(B_FIELD); + FitResult result = fitter.fitCascadeVertex(recoilTrack, v0Line); + + System.out.printf("Fitted vertex: [%.6f, %.6f, %.6f] (truth: [%.6f, %.6f, %.6f])%n", + result.vertex.getEntry(0), result.vertex.getEntry(1), result.vertex.getEntry(2), + xV, yV, zV); + System.out.printf("Chi2/NDF: %.6f / %d%n", result.chi2, result.ndf); + + assertEquals(xV, result.vertex.getEntry(0), 1e-4); + assertEquals(yV, result.vertex.getEntry(1), 1e-4); + assertEquals(zV, result.vertex.getEntry(2), 1e-4); + assertEquals(1, result.ndf); + assertTrue("chi2 should be small for an exactly-consistent geometry", result.chi2 < 1e-3); + } + + /** + * Toy-MC pull study for the cascade vertex fit: build a truth production vertex where a + * helix and a line cross, smear both inputs by their (correlated, full 6x6 for the line) + * covariances many times, refit, and check that the fitted-vertex pulls have mean ~0 / + * width ~1 and that chi2/ndf averages to ~1 (ndf=1), consistent with + * {@code testCascadeVertexExactGeometry}'s single noiseless case but now exercising the + * statistical behavior of the fit under realistic measurement uncertainty. + */ + public void testCascadeVertexSmearedPulls() { + System.out.println("\n=== testCascadeVertexSmearedPulls ===\n"); + + double xV = 0.5, yV = -0.3, zV = 8.0; + double px = 0.20, py = -0.06, pz = 1.2; // recoil track truth momentum + double dx = 0.35, dy = 0.12, dz = 3.0; // V0 line truth direction (= its momentum) + + // Line's own reference point need not be the intersection point -- any point on the + // line works. Offset it to exercise that generality. + double s = -2.0; + double refX = xV + s * dx, refY = yV + s * dy, refZ = zV + s * dz; + + 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 sigmaPos = 0.05; // mm, line reference-point uncertainty + double sigmaMom = 0.02; // GeV, line direction (momentum) uncertainty + double rho = 0.3; // correlation between position_i and momentum_i + RealMatrix lineCov = MatrixUtils.createRealMatrix(6, 6); + for (int i = 0; i < 3; i++) { + lineCov.setEntry(i, i, sigmaPos * sigmaPos); + lineCov.setEntry(3 + i, 3 + i, sigmaMom * sigmaMom); + double cross = rho * sigmaPos * sigmaMom; + lineCov.setEntry(i, 3 + i, cross); + lineCov.setEntry(3 + i, i, cross); + } + + java.util.Random rng = new java.util.Random(12345); + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(B_FIELD); + + int nToys = 3000; + double[] pullX = new double[nToys], pullY = new double[nToys], pullZ = new double[nToys]; + double[] chi2 = new double[nToys]; + int nFailed = 0; + + for (int toy = 0; toy < nToys; toy++) { + TrackParams truthTrack = createExactTrackThroughPoint( + xV, yV, zV, px, py, pz, -1, B_FIELD, trackCov); + double[] tp = truthTrack.toArray(); + double[] smearedTp = new double[5]; + double[] trackSigma = {d0Err, phi0Err, omegaErr, z0Err, tanLErr}; + for (int i = 0; i < 5; i++) { + smearedTp[i] = tp[i] + trackSigma[i] * rng.nextGaussian(); + } + TrackParams smearedTrack = new TrackParams( + smearedTp[0], smearedTp[1], smearedTp[2], smearedTp[3], smearedTp[4], trackCov); + + double[] dPos = new double[3], dMom = new double[3]; + for (int i = 0; i < 3; i++) { + double z1 = rng.nextGaussian(); + double z2 = rng.nextGaussian(); + dPos[i] = sigmaPos * z1; + dMom[i] = rho * sigmaMom * z1 + sigmaMom * FastMath.sqrt(1.0 - rho * rho) * z2; + } + KalmanVertexFitterGainMatrix.LineParams smearedLine = new KalmanVertexFitterGainMatrix.LineParams( + refX + dPos[0], refY + dPos[1], refZ + dPos[2], + dx + dMom[0], dy + dMom[1], dz + dMom[2], lineCov); + + FitResult result = fitter.fitCascadeVertex(smearedTrack, smearedLine); + if (result == null) { + nFailed++; + continue; + } + + pullX[toy] = (result.vertex.getEntry(0) - xV) / FastMath.sqrt(result.vertexCov.getEntry(0, 0)); + pullY[toy] = (result.vertex.getEntry(1) - yV) / FastMath.sqrt(result.vertexCov.getEntry(1, 1)); + pullZ[toy] = (result.vertex.getEntry(2) - zV) / FastMath.sqrt(result.vertexCov.getEntry(2, 2)); + chi2[toy] = result.chi2; + } + + assertEquals("no toy fits should fail", 0, nFailed); + + double meanChi2 = mean(chi2); + double[] meanX = {mean(pullX)}, meanY = {mean(pullY)}, meanZ = {mean(pullZ)}; + double stdX = std(pullX, meanX[0]), stdY = std(pullY, meanY[0]), stdZ = std(pullZ, meanZ[0]); + + System.out.printf("Pull X: mean=%.3f std=%.3f%n", meanX[0], stdX); + System.out.printf("Pull Y: mean=%.3f std=%.3f%n", meanY[0], stdY); + System.out.printf("Pull Z: mean=%.3f std=%.3f%n", meanZ[0], stdZ); + System.out.printf("Mean chi2 (ndf=1): %.3f%n", meanChi2); + + assertTrue("pull X mean should be near 0, got " + meanX[0], FastMath.abs(meanX[0]) < 0.15); + assertTrue("pull Y mean should be near 0, got " + meanY[0], FastMath.abs(meanY[0]) < 0.15); + assertTrue("pull Z mean should be near 0, got " + meanZ[0], FastMath.abs(meanZ[0]) < 0.15); + assertTrue("pull X std should be near 1, got " + stdX, stdX > 0.8 && stdX < 1.2); + assertTrue("pull Y std should be near 1, got " + stdY, stdY > 0.8 && stdY < 1.2); + assertTrue("pull Z std should be near 1, got " + stdZ, stdZ > 0.8 && stdZ < 1.2); + assertTrue("mean chi2 for ndf=1 should be near 1, got " + meanChi2, meanChi2 > 0.7 && meanChi2 < 1.4); + } + + /** + * 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 (ThreeTrackVertexer'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 ThreeTrackVertexer'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}); + + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(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 ThreeTrackVertexer's real usage). + RealVector v2Init = MatrixUtils.createRealVector(new double[]{0.0, 0.0, 0.0}); + + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(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); + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(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))); + } + + /** + * 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 = KalmanVertexFitterGainMatrix.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); + + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(B_FIELD); + KalmanVertexFitterGainMatrix.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 ThreeTrackVertexer}'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 ThreeTrackVertexer falls back to in practice. + RealVector v2Init = MatrixUtils.createRealVector(new double[]{0.0, 0.0, 0.0}); + + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(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 ThreeTrackVertexer}) 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 ThreeTrackVertexer}'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}); + + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(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.ThreeTrackVertexer} 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); + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(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))); + } + + 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); + } + + /** + * 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); + + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(bField); + KalmanVertexFitterGainMatrix.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 { + KalmanVertexFitterGainMatrix.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); + + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(bField); + KalmanVertexFitterGainMatrix.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 { + KalmanVertexFitterGainMatrix.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))); + } + } + + KalmanVertexFitterGainMatrix.LineParams line = + new KalmanVertexFitterGainMatrix.LineParams(x0, y0, z0, dx, dy, dz, cov6); + + double xPlane = -1.1; + double sigmaXFloor = 0.001; + KalmanVertexFitterGainMatrix.LinePlaneProjection proj = + KalmanVertexFitterGainMatrix.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; + KalmanVertexFitterGainMatrix.LinePlaneProjection proj = + KalmanVertexFitterGainMatrix.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); + KalmanVertexFitterGainMatrix.LinePlaneProjection pPlus = + KalmanVertexFitterGainMatrix.propagateTrackToPlane(tPlus, xPlane, sigmaXFloor); + KalmanVertexFitterGainMatrix.LinePlaneProjection pMinus = + KalmanVertexFitterGainMatrix.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/recon/src/test/java/org/hps/recon/vertexing/ThreeTrackVertexerTest.java b/recon/src/test/java/org/hps/recon/vertexing/ThreeTrackVertexerTest.java new file mode 100644 index 0000000000..25721f39d6 --- /dev/null +++ b/recon/src/test/java/org/hps/recon/vertexing/ThreeTrackVertexerTest.java @@ -0,0 +1,204 @@ +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 ThreeTrackVertexer}, mirroring {@link + * CascadeVertexerTest}: 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 ThreeTrackVertexerTest 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); + + ThreeTrackVertexer vertexer = new ThreeTrackVertexer(B_FIELD); + ReconstructedParticle cascade = vertexer.fit(v0Particle, recoilElectron, true); + + 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); + } + + 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 KalmanV0VertexerTest#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/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/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))); + } + } From 55d99381b285974fd77e13391f6ae09d2a8a0a54 Mon Sep 17 00:00:00 2001 From: Matt Graham Date: Mon, 7 Sep 2026 09:23:39 -0700 Subject: [PATCH 2/4] Add nuclear-recoil term to N-track beam-momentum constraint, deprecate hard mode The beam-momentum-constrained vertex fit's covariance modeled only beam divergence, missing the transverse momentum carried by the target nuclear recoil in trident production (true spread ~18.6 MeV vs modeled 0.37 MeV). Add a toggleable sigmaTNuclearRecoil term (soft mode only) that closes most of the real-MC chi2/ndf gap, and adopt it as the default for kalSoft*. Hard/exact mode (fitLagrangeMultiplier) hardcodes a null momentum covariance and can never benefit from this fix, so it is marked @Deprecated (kept for regression reference) and its kalHard* tuple columns are no longer computed. Renamed sigmaTRecoil -> sigmaTNuclearRecoil to avoid confusion with a recoil electron from radiative/A' events (unrelated concept). --- .../NTrackVertexComparisonTupleDriver.java | 207 ++++++++++++++- .../recon/vertexing/KalmanNTrackVertexer.java | 92 +++++++ .../KalmanVertexFitterGainMatrix.java | 236 ++++++++++++------ 3 files changed, 453 insertions(+), 82 deletions(-) diff --git a/analysis/src/main/java/org/hps/analysis/tuple/NTrackVertexComparisonTupleDriver.java b/analysis/src/main/java/org/hps/analysis/tuple/NTrackVertexComparisonTupleDriver.java index 2f1d5e9977..9eefd47643 100644 --- a/analysis/src/main/java/org/hps/analysis/tuple/NTrackVertexComparisonTupleDriver.java +++ b/analysis/src/main/java/org/hps/analysis/tuple/NTrackVertexComparisonTupleDriver.java @@ -10,13 +10,18 @@ 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.KalmanNTrackVertexer; +import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix; +import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TrackMomentum; +import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TrackParams; import org.lcsim.event.EventHeader; import org.lcsim.event.LCRelation; import org.lcsim.event.MCParticle; @@ -28,6 +33,9 @@ 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 trident MC event, the legacy Billoir N-track * common-vertex fit against the standalone Kalman gain-matrix N-track common-vertex fit @@ -51,8 +59,35 @@ public class NTrackVertexComparisonTupleDriver extends Driver { "kalVtxX/D", "kalVtxY/D", "kalVtxZ/D", "kalVtxXErr/D", "kalVtxYErr/D", "kalVtxZErr/D", "kalChi2/D", "kalNdf/I", "kalMass/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"); + "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"; @@ -60,11 +95,26 @@ public class NTrackVertexComparisonTupleDriver extends Driver { 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; } @@ -143,6 +193,15 @@ public void process(EventHeader event) { // so no daughter-production-point workaround is needed here. Hep3Vector apVtxMC = ele1MC.getOrigin(); + // Truth total momentum of the 3 trident 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 @@ -154,6 +213,9 @@ public void process(EventHeader event) { 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); @@ -174,6 +236,18 @@ public void process(EventHeader event) { 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)); @@ -206,12 +280,28 @@ public void process(EventHeader event) { billiorVertexer.setReferencePosition(newRef); BilliorVertex billoirVtx = billiorVertexer.fitVertex(shiftedTracks); BilliorVertex kalmanVtx = new KalmanNTrackVertexer(bField).fitVertex(tracks); + BilliorVertex kalmanVtxUnconstrained = new KalmanNTrackVertexer(bField).fitVertexNoBeamConstraint(tracks); + // Hard (Lagrange-multiplier/exact) beam-momentum-constrained mode is deprecated (see + // KalmanVertexFitterGainMatrix.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 + // KalmanVertexFitterGainMatrix.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 KalmanNTrackVertexer(bField).fitVertexBeamConstrained( + tracks, beamEnergy, beamRotAngle, false, sigmaTNuclearRecoil); Hep3Vector vtxPos = billoirVtx.getPosition(); Hep3Vector vtxPosErr = billoirVtx.getPositionError(); Hep3Vector kalVtxPos = kalmanVtx.getPosition(); Hep3Vector kalVtxPosErr = kalmanVtx.getPositionError(); Double kalNdf = kalmanVtx.getCustomParameters().get("ndf"); + 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()); @@ -233,6 +323,29 @@ public void process(EventHeader event) { row.put("kalChi2/D", kalmanVtx.getChi2()); row.put("kalNdf/I", kalNdf != null ? kalNdf : -9999.0); row.put("kalMass/D", kalmanVtx.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); @@ -240,10 +353,102 @@ public void process(EventHeader event) { 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 KalmanVertexFitterGainMatrix.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 KalmanVertexFitterGainMatrix#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 KalmanVertexFitterGainMatrix(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); diff --git a/recon/src/main/java/org/hps/recon/vertexing/KalmanNTrackVertexer.java b/recon/src/main/java/org/hps/recon/vertexing/KalmanNTrackVertexer.java index 298cee2f00..6dc1503df0 100644 --- a/recon/src/main/java/org/hps/recon/vertexing/KalmanNTrackVertexer.java +++ b/recon/src/main/java/org/hps/recon/vertexing/KalmanNTrackVertexer.java @@ -71,6 +71,98 @@ public BilliorVertex fitVertex(List tracks) { return makeVertex(result, referencePoint); } + /** + * Fit the common vertex of N tracks with the total 3-momentum constrained to the beam + * value, using {@link KalmanVertexFitterGainMatrix#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 {@code makeVertex}-style 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 KalmanVertexFitterGainMatrix#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 KalmanVertexFitterGainMatrix#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)); + } + + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(bField); + fitter.setBeamEnergy(beamEnergy); + fitter.setBeamRotAngle(beamRotAngle); + fitter.setBeamMomentumTransverseNuclearRecoilSigma(transverseNuclearRecoilSigma); + fitter.setReferencePosition(referencePoint); + 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 KalmanVertexFitterGainMatrix#fitVertex(List, boolean, + * boolean, boolean)} directly (dispatches internally to {@code fitBillior1985}, which + * -- unlike the simpler {@code fit()} algorithm used by {@link #fitVertex(List)} -- + * already computes a real per-track momentum covariance). Kept as a separate method + * rather than changing {@link #fitVertex(List)} in place, so that method's existing + * (covariance-free) behavior remains available unchanged for any other caller. + * + * @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)); + } + + KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(bField); + fitter.setReferencePosition(referencePoint); + BilliorVertex bv = fitter.fitVertex(trackParams, false, false, 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 diff --git a/recon/src/main/java/org/hps/recon/vertexing/KalmanVertexFitterGainMatrix.java b/recon/src/main/java/org/hps/recon/vertexing/KalmanVertexFitterGainMatrix.java index b3e1e62c67..72d9c377ee 100644 --- a/recon/src/main/java/org/hps/recon/vertexing/KalmanVertexFitterGainMatrix.java +++ b/recon/src/main/java/org/hps/recon/vertexing/KalmanVertexFitterGainMatrix.java @@ -125,6 +125,12 @@ public static class FitResult { 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) { @@ -575,11 +581,37 @@ private RealMatrix computeMomentumVertexDerivatives(TrackParams track, RealVecto * Compute momentum covariance */ private 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}). + */ + private 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); @@ -595,24 +627,24 @@ private RealMatrix computeMomentumCovariance(TrackParams track, RealVector verte 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.multiply(track.cov).multiply(Jp.transpose()); + + return Jp; } /** @@ -2168,78 +2200,40 @@ public FitResult fitSoftConstrained(List inputTracks, } // Build constraint covariance matrix V for soft constraints. - // For each track, V is a 2x2 block: V = J_h * trackCov * J_h^T - // where J_h has rows [d(h_t)/d(params), d(h_z)/d(params)]: - // h_t = r - R -> d(h_t)/d(params) = +Jft (positive) - // h_z = zV - zPred -> d(h_z)/d(params) = -Jz (negative) - // For momentum constraints: use the provided fourMomentumConstraintCov + // 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); - // Track constraint covariances: 2x2 block per track - for (int i = 0; i < nTracks; i++) { - TrackParams track = tracks.get(i); - int rowT = 2 * i; - int rowZ = 2 * i + 1; - - 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 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); + 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; + double epsilon = (diagScale > 0) ? diagScale * 1e-6 : 1e-12; - // d(h_t)/d(track params): Jft (positive) - double[] Jht = new double[5]; - Jht[0] = (dx * FastMath.sin(track.phi0) - dy * FastMath.cos(track.phi0)) / r; - Jht[1] = -(sign * R - track.d0) * (dx * FastMath.cos(track.phi0) + dy * FastMath.sin(track.phi0)) / r; - Jht[2] = (dx * FastMath.sin(track.phi0) - dy * FastMath.cos(track.phi0)) / (r * track.omega * track.omega) - + sign / (track.omega * track.omega); - Jht[3] = 0.0; - Jht[4] = 0.0; + for (int i = 0; i < nTrackConstraints; i++) { + constraintCov.setEntry(i, i, epsilon); + } - // d(h_z)/d(track params): -Jz (negative, since h_z = zV - zPred) - 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_v = phiV - track.phi0; - while (dphi_v > FastMath.PI) dphi_v -= 2.0 * FastMath.PI; - while (dphi_v < -FastMath.PI) dphi_v += 2.0 * FastMath.PI; - double s = -sign * R * dphi_v; - // 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[] Jhz = new double[5]; - Jhz[0] = -(-sign * track.tanLambda * R * dphiDd0); - Jhz[1] = -(-sign * (-track.tanLambda * R + track.tanLambda * R * dphiDphi0)); - Jhz[2] = -(-s * track.tanLambda / track.omega - sign * track.tanLambda * R * dphiDomega); - Jhz[3] = -1.0; - Jhz[4] = -s; - - // V_2x2 = J_h * trackCov * J_h^T - double V_tt = 0, V_zz = 0, V_tz = 0; - for (int a = 0; a < 5; a++) { - for (int b = 0; b < 5; b++) { - double c = track.cov.getEntry(a, b); - V_tt += Jht[a] * c * Jht[b]; - V_zz += Jhz[a] * c * Jhz[b]; - V_tz += Jht[a] * c * Jhz[b]; - } - } - constraintCov.setEntry(rowT, rowT, V_tt); - constraintCov.setEntry(rowT, rowZ, V_tz); - constraintCov.setEntry(rowZ, rowT, V_tz); - constraintCov.setEntry(rowZ, rowZ, V_zz); - - if (debugFlag && iteration == 0) { - System.out.printf(" Track %d transverse sigma = %.4f mm, z sigma = %.4f mm%n", - i, FastMath.sqrt(V_tt), FastMath.sqrt(V_zz)); - } + if (debugFlag && iteration == 0) { + System.out.printf(" Track constraint regularization epsilon = %.3e%n", epsilon); } // Momentum constraint covariances (if applicable) @@ -2358,8 +2352,11 @@ public FitResult fitSoftConstrained(List inputTracks, } } - // NDF = number of constraints - int ndf = nConstraints; + // 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<>(); @@ -2370,6 +2367,28 @@ public FitResult fitSoftConstrained(List inputTracks, 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); @@ -2416,7 +2435,10 @@ public FitResult fitSoftConstrained(List inputTracks, } } - return new FitResult(vertex, vertexCov, chi2, ndf, trackMomenta, fittedTracks); + FitResult fitResult = new FitResult(vertex, vertexCov, chi2, ndf, trackMomenta, fittedTracks); + fitResult.totalMomentum = totalPTracking; + fitResult.totalMomentumCov = totalPCovTracking; + return fitResult; } /** @@ -2447,7 +2469,22 @@ public FitResult fitSoftConstrained(List inputTracks, * @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 KalmanNTrackVertexer#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, @@ -2480,6 +2517,14 @@ public FitResult fit(List tracks, int maxIterations, double toleran 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; } @@ -2488,6 +2533,7 @@ public FitResult fit(List tracks, int maxIterations, double toleran 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; } @@ -2825,7 +2871,12 @@ public BilliorVertex fitVertex(List tracks, boolean beamspotConstra * @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 + * 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) { @@ -2882,7 +2933,7 @@ public BilliorVertex fitVertex(List tracks, boolean beamspotConstra double dpOverP = 1e-2; double sigmaTheta = 100e-6; // beam angular divergence (rad) double sigmaL = dpOverP * pBeam; - double sigmaT = sigmaTheta * pBeam; + double sigmaT = FastMath.hypot(sigmaTheta * pBeam, sigmaTNuclearRecoil); double cosR = FastMath.cos(rotAngle); double sinR = FastMath.sin(rotAngle); double sL2 = sigmaL * sigmaL; @@ -2990,6 +3041,29 @@ public BilliorVertex fitVertex(List tracks, boolean beamspotConstra 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) { From a89884d738e30d8539cc463705dbdf5a52f21e30 Mon Sep 17 00:00:00 2001 From: Matt Graham Date: Wed, 7 Oct 2026 06:29:46 -0700 Subject: [PATCH 3/4] WIP: rename KalmanVertexFitterGainMatrix to TrackConstraintVertexFitter Renames the gain-matrix vertex fitter class to TrackConstraintVertexFitter to reflect that it shares one per-track constraint linearization across several solve strategies (sequential gain update, Schur-complement, joint Newton-Raphson/KKT) rather than being a single gain-matrix algorithm. Removes the standalone V0 Billoir-vs-Kalman cross-check driver (TwoTrackVertexComparisonTupleDriver) and dead test-only overloads, and adds the new Vertexer/NTrackVertexer abstractions and V0VertexComparisonTupleDriver used by the cascade/N-track all-constraint comparison studies. --- .../tuple/CascadeVertexTupleDriver.java | 675 ++++- .../NTrackVertexComparisonTupleDriver.java | 173 +- .../TwoTrackVertexComparisonTupleDriver.java | 239 -- .../tuple/V0VertexComparisonTupleDriver.java | 351 +++ .../tuple/CascadeVertexTupleDriverTest.java | 102 - .../ThreeElectronTrackingFilter.java | 2 +- .../particle/HpsReconParticleDriver.java | 25 +- .../recon/particle/ReconParticleDriver.java | 604 ++-- .../hps/recon/vertexing/BilliorVertex.java | 3 + .../hps/recon/vertexing/CascadeVertexer.java | 665 ++++- .../recon/vertexing/KalmanNTrackVertexer.java | 251 -- .../hps/recon/vertexing/KalmanV0Vertexer.java | 155 -- .../hps/recon/vertexing/NTrackVertexer.java | 256 ++ .../recon/vertexing/ThreeTrackVertexer.java | 400 --- ....java => TrackConstraintVertexFitter.java} | 1827 +++++++------ .../org/hps/recon/vertexing/Vertexer.java | 54 + .../recon/vertexing/CascadeVertexerTest.java | 337 ++- .../recon/vertexing/GainMatrixVertexer.java | 353 +++ .../vertexing/GainMatrixVertexerTest.java | 117 + .../recon/vertexing/KalmanV0VertexerTest.java | 166 -- .../KalmanVertexFitterGainMatrixTest.java | 1384 ---------- .../recon/vertexing/NTrackVertexerTest.java | 328 +++ .../vertexing/ThreeTrackVertexerTest.java | 204 -- .../TrackConstraintVertexFitterTest.java | 2417 +++++++++++++++++ ...pass5_physics_HitSmear_HitKill_14272.lcsim | 14 + 25 files changed, 6848 insertions(+), 4254 deletions(-) delete mode 100644 analysis/src/main/java/org/hps/analysis/tuple/TwoTrackVertexComparisonTupleDriver.java create mode 100644 analysis/src/main/java/org/hps/analysis/tuple/V0VertexComparisonTupleDriver.java delete mode 100644 analysis/src/test/java/org/hps/analysis/tuple/CascadeVertexTupleDriverTest.java delete mode 100644 recon/src/main/java/org/hps/recon/vertexing/KalmanNTrackVertexer.java delete mode 100644 recon/src/main/java/org/hps/recon/vertexing/KalmanV0Vertexer.java create mode 100644 recon/src/main/java/org/hps/recon/vertexing/NTrackVertexer.java delete mode 100644 recon/src/main/java/org/hps/recon/vertexing/ThreeTrackVertexer.java rename recon/src/main/java/org/hps/recon/vertexing/{KalmanVertexFitterGainMatrix.java => TrackConstraintVertexFitter.java} (74%) create mode 100644 recon/src/main/java/org/hps/recon/vertexing/Vertexer.java create mode 100644 recon/src/test/java/org/hps/recon/vertexing/GainMatrixVertexer.java create mode 100644 recon/src/test/java/org/hps/recon/vertexing/GainMatrixVertexerTest.java delete mode 100644 recon/src/test/java/org/hps/recon/vertexing/KalmanV0VertexerTest.java delete mode 100644 recon/src/test/java/org/hps/recon/vertexing/KalmanVertexFitterGainMatrixTest.java create mode 100644 recon/src/test/java/org/hps/recon/vertexing/NTrackVertexerTest.java delete mode 100644 recon/src/test/java/org/hps/recon/vertexing/ThreeTrackVertexerTest.java create mode 100644 recon/src/test/java/org/hps/recon/vertexing/TrackConstraintVertexFitterTest.java diff --git a/analysis/src/main/java/org/hps/analysis/tuple/CascadeVertexTupleDriver.java b/analysis/src/main/java/org/hps/analysis/tuple/CascadeVertexTupleDriver.java index f80abadf6f..d57ad31f0c 100644 --- a/analysis/src/main/java/org/hps/analysis/tuple/CascadeVertexTupleDriver.java +++ b/analysis/src/main/java/org/hps/analysis/tuple/CascadeVertexTupleDriver.java @@ -10,17 +10,19 @@ 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 the {@code CascadeVertexer} output. + * 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 @@ -28,15 +30,20 @@ */ 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", - "v0UncPX/D", "v0UncPY/D", "v0UncPZ/D", - "recoilPX/D", "recoilPY/D", "recoilPZ/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", @@ -45,11 +52,116 @@ public class CascadeVertexTupleDriver extends Driver { "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"); + "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; @@ -59,6 +171,34 @@ public void setCascadeVertexCandidatesColName(String cascadeVertexCandidatesColN 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; } @@ -104,6 +244,13 @@ public void process(EventHeader event) { 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) { @@ -136,11 +283,81 @@ public void process(EventHeader event) { 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); - for (ReconstructedParticle cascade : cascadeCandidates) { + 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); @@ -153,10 +370,60 @@ public void process(EventHeader event) { 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))); @@ -173,7 +440,10 @@ public void process(EventHeader event) { // 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). + // (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(); @@ -191,15 +461,40 @@ public void process(EventHeader event) { 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()); @@ -232,6 +527,15 @@ public void process(EventHeader event) { 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); @@ -240,6 +544,363 @@ public void process(EventHeader event) { 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); } } diff --git a/analysis/src/main/java/org/hps/analysis/tuple/NTrackVertexComparisonTupleDriver.java b/analysis/src/main/java/org/hps/analysis/tuple/NTrackVertexComparisonTupleDriver.java index 9eefd47643..c22139e3d8 100644 --- a/analysis/src/main/java/org/hps/analysis/tuple/NTrackVertexComparisonTupleDriver.java +++ b/analysis/src/main/java/org/hps/analysis/tuple/NTrackVertexComparisonTupleDriver.java @@ -18,10 +18,10 @@ import org.hps.recon.vertexing.BilliorTrack; import org.hps.recon.vertexing.BilliorVertex; import org.hps.recon.vertexing.BilliorVertexer; -import org.hps.recon.vertexing.KalmanNTrackVertexer; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TrackMomentum; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TrackParams; +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; @@ -37,15 +37,18 @@ import org.apache.commons.math3.linear.RealMatrix; /** - * Writes a flat ASCII ntuple comparing, per trident MC event, the legacy Billoir N-track - * common-vertex fit against the standalone Kalman gain-matrix N-track common-vertex fit - * ({@code KalmanNTrackVertexer}), for the 3 truth-matched trident tracks (2 e- + 1 e+ from a - * single common production vertex). Unlike {@code TwoTrackVertexComparisonTupleDriver} and - * {@code CascadeVertexTupleDriver}, this driver does its own track selection and fitting - * inline -- it reads raw {@code KalmanFullTracks} plus MC truth directly, rather than + * 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 trident daughters are truth-matched to a reconstructed track). + * (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}. */ @@ -56,9 +59,6 @@ public class NTrackVertexComparisonTupleDriver extends Driver { "vtxX/D", "vtxY/D", "vtxZ/D", "vtxXErr/D", "vtxYErr/D", "vtxZErr/D", "chi2/D", "mass/D", - "kalVtxX/D", "kalVtxY/D", "kalVtxZ/D", - "kalVtxXErr/D", "kalVtxYErr/D", "kalVtxZErr/D", - "kalChi2/D", "kalNdf/I", "kalMass/D", "kalUncEle1Px/D", "kalUncEle1Py/D", "kalUncEle1Pz/D", "kalUncEle1PxErr/D", "kalUncEle1PyErr/D", "kalUncEle1PzErr/D", "kalUncEle2Px/D", "kalUncEle2Py/D", "kalUncEle2Pz/D", @@ -149,54 +149,98 @@ public void process(EventHeader event) { return; } - // Trident truth identification: PDGID 623 is the trident/"reaction" pseudo-particle - // and has 3 direct daughters (2 e- + 1 e+ from a single common production vertex) -- - // architecturally different from the A' sample, where 622 is the A' signal record - // (2 daughters: signal e+e-) and 623 is a separate, unrelated single-daughter recoil - // electron record (confirmed via direct LCIO inspection of tritrig_pulser_100.slcio: - // zero PDGID-622 particles present; every event has exactly one PDGID-623 top-level - // particle with the expected 2x11 + 1x(-11) daughter topology). Ranks electron - // daughters by energy, same convention as MCTupleMaker.fillMCTridentVariables (which - // has this same PDGID mixup for tritrig-only samples). - MCParticle ele1MC = null; - MCParticle ele2MC = null; + // 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; - for (MCParticle mcp : event.get(MCParticle.class, mcParticlesColName)) { - if (mcp.getPDGID() == 623) { + MCParticle recoilMC = null; + for (MCParticle mcp : mcParticles) { + if (mcp.getPDGID() == 622 && mcp.getDaughters().size() == 2) { + apMC = mcp; 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; + 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: unlike the A' sample, this generator's PDGID-622 daughters' own - // getOrigin() is not contaminated by any upstream converter bug (confirmed via - // MCTupleMaker.fillMCParticleVariables, which uses particle.getOrigin() directly), - // so no daughter-production-point workaround is needed here. - Hep3Vector apVtxMC = ele1MC.getOrigin(); + // 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 trident 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). + // 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(); @@ -206,7 +250,7 @@ public void process(EventHeader event) { // 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/TwoTrackVertexComparisonTupleDriver's trackPurity maps. + // CascadeVertexTupleDriver's trackPurity maps. Map mcToTrack = new HashMap(); Map mcToWeight = new HashMap(); for (LCRelation rel : event.get(LCRelation.class, trackToMCParticleRelationsColName)) { @@ -279,24 +323,20 @@ public void process(EventHeader event) { BilliorVertexer billiorVertexer = new BilliorVertexer(bField); billiorVertexer.setReferencePosition(newRef); BilliorVertex billoirVtx = billiorVertexer.fitVertex(shiftedTracks); - BilliorVertex kalmanVtx = new KalmanNTrackVertexer(bField).fitVertex(tracks); - BilliorVertex kalmanVtxUnconstrained = new KalmanNTrackVertexer(bField).fitVertexNoBeamConstraint(tracks); + BilliorVertex kalmanVtxUnconstrained = new NTrackVertexer(bField).fitVertexNoBeamConstraint(tracks); // Hard (Lagrange-multiplier/exact) beam-momentum-constrained mode is deprecated (see - // KalmanVertexFitterGainMatrix.fitLagrangeMultiplier) and no longer computed here. + // 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 - // KalmanVertexFitterGainMatrix.setBeamMomentumTransverseNuclearRecoilSigma). Value is + // 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 KalmanNTrackVertexer(bField).fitVertexBeamConstrained( + BilliorVertex kalmanVtxSoft = new NTrackVertexer(bField).fitVertexBeamConstrained( tracks, beamEnergy, beamRotAngle, false, sigmaTNuclearRecoil); Hep3Vector vtxPos = billoirVtx.getPosition(); Hep3Vector vtxPosErr = billoirVtx.getPositionError(); - Hep3Vector kalVtxPos = kalmanVtx.getPosition(); - Hep3Vector kalVtxPosErr = kalmanVtx.getPositionError(); - Double kalNdf = kalmanVtx.getCustomParameters().get("ndf"); Hep3Vector kalSoftVtxPos = kalmanVtxSoft.getPosition(); Hep3Vector kalSoftVtxPosErr = kalmanVtxSoft.getPositionError(); Double kalSoftNdf = kalmanVtxSoft.getCustomParameters().get("ndf"); @@ -314,15 +354,6 @@ public void process(EventHeader event) { row.put("vtxZErr/D", vtxPosErr.z()); row.put("chi2/D", billoirVtx.getChi2()); row.put("mass/D", billoirVtx.getInvMass()); - row.put("kalVtxX/D", kalVtxPos.x()); - row.put("kalVtxY/D", kalVtxPos.y()); - row.put("kalVtxZ/D", kalVtxPos.z()); - row.put("kalVtxXErr/D", kalVtxPosErr.x()); - row.put("kalVtxYErr/D", kalVtxPosErr.y()); - row.put("kalVtxZErr/D", kalVtxPosErr.z()); - row.put("kalChi2/D", kalmanVtx.getChi2()); - row.put("kalNdf/I", kalNdf != null ? kalNdf : -9999.0); - row.put("kalMass/D", kalmanVtx.getInvMass()); row.put("kalSoftVtxX/D", kalSoftVtxPos.x()); row.put("kalSoftVtxY/D", kalSoftVtxPos.y()); row.put("kalSoftVtxZ/D", kalSoftVtxPos.z()); @@ -389,7 +420,7 @@ public void process(EventHeader event) { /** * Reads track {@code trackIndex}'s fitted momentum + diagonal error off {@code bv} (set - * unconditionally by {@code KalmanVertexFitterGainMatrix.fitVertex}'s dispatcher for every + * 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, @@ -416,7 +447,7 @@ private static void putTrackMomentum(Map row, BilliorVertex bv, * 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 KalmanVertexFitterGainMatrix#computeRawMomentum}), so it is directly + * ({@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}. */ @@ -431,7 +462,7 @@ private Hep3Vector[] getRawMomentumAndError(Track track) { } } TrackParams tp = new TrackParams(par[0], par[1], par[2], par[3], par[4], cov); - TrackMomentum tm = new KalmanVertexFitterGainMatrix(bField).computeRawMomentum(tp); + 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))); diff --git a/analysis/src/main/java/org/hps/analysis/tuple/TwoTrackVertexComparisonTupleDriver.java b/analysis/src/main/java/org/hps/analysis/tuple/TwoTrackVertexComparisonTupleDriver.java deleted file mode 100644 index 4f388f5bab..0000000000 --- a/analysis/src/main/java/org/hps/analysis/tuple/TwoTrackVertexComparisonTupleDriver.java +++ /dev/null @@ -1,239 +0,0 @@ -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 java.util.logging.Logger; - -import hep.physics.vec.Hep3Vector; - -import org.hps.recon.tracking.TrackStateUtils; -import org.hps.recon.vertexing.BilliorVertex; -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.TrackState; -import org.lcsim.geometry.Detector; -import org.lcsim.util.Driver; - -/** - * Writes a flat ASCII ntuple pairing, per unconstrained V0 (e-/e+) candidate, the - * existing Billoir vertex fit against the standalone unconstrained two-track Kalman - * vertex fit ({@code KalmanV0Vertexer}, wired through {@code HpsReconParticleDriver}), - * for offline comparison of the two algorithms. One row per V0 candidate. Header line is - * variable names joined by ":"; data rows are tab-separated, matching the convention - * used by {@link CascadeVertexTupleDriver}. - */ -public class TwoTrackVertexComparisonTupleDriver extends Driver { - - private static final Logger LOGGER = Logger.getLogger(TwoTrackVertexComparisonTupleDriver.class.getPackage().getName()); - - 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", - "kalVtxX/D", "kalVtxY/D", "kalVtxZ/D", - "kalVtxXErr/D", "kalVtxYErr/D", "kalVtxZErr/D", - "kalChi2/D", "kalNdf/I", "kalMass/D", - "eleTruthMatched/I", "posTruthMatched/I", "bothTruthMatchedToAp/I", - "apVtxXMC/D", "apVtxYMC/D", "apVtxZMC/D"); - - private String unconstrainedV0CandidatesColName = "UnconstrainedV0Candidates"; - private String kalmanV0CandidatesColName = "KalmanUnconstrainedV0Candidates"; - private String mcParticlesColName = null; - private String trackToMCParticleRelationsColName = null; - private String tupleFile = null; - private PrintWriter tupleWriter = null; - - public void setUnconstrainedV0CandidatesColName(String unconstrainedV0CandidatesColName) { - this.unconstrainedV0CandidatesColName = unconstrainedV0CandidatesColName; - } - - public void setKalmanV0CandidatesColName(String kalmanV0CandidatesColName) { - this.kalmanV0CandidatesColName = kalmanV0CandidatesColName; - } - - 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 two-track vertex comparison tuple file " + tupleFile, e); - } - tupleWriter.println(String.join(":", VARIABLES)); - } - - @Override - public void process(EventHeader event) { - if (tupleWriter == null || !event.hasCollection(ReconstructedParticle.class, unconstrainedV0CandidatesColName) - || !event.hasCollection(ReconstructedParticle.class, kalmanV0CandidatesColName)) { - return; - } - - Map trackToMC = 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()); - } - } - - MCParticle eleMC = null; - MCParticle posMC = null; - if (mcParticlesColName != null && event.hasCollection(MCParticle.class, mcParticlesColName)) { - for (MCParticle mcp : event.get(MCParticle.class, mcParticlesColName)) { - 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; - } - } - } - - // apMC.getOrigin()/getEndPoint() are unreliable for this generator sample: the - // .dat/LHE-to-stdhep converters apply the decay-length vertex shift to the A' record - // itself, and BaseMCParticle's endpoint field defaults to (0,0,0) unless set by an - // external writer. Use a daughter's own production point instead -- daughters are - // produced exactly at the parent's decay vertex (same convention as CascadeVertexTupleDriver). - Hep3Vector apVtxMC = eleMC != null ? eleMC.getOrigin() : (posMC != null ? posMC.getOrigin() : null); - - List billoirV0s = event.get(ReconstructedParticle.class, unconstrainedV0CandidatesColName); - List kalmanV0s = event.get(ReconstructedParticle.class, kalmanV0CandidatesColName); - if (billoirV0s.size() != kalmanV0s.size()) { - LOGGER.warning("Billoir/Kalman unconstrained V0 collection size mismatch (" - + billoirV0s.size() + " vs " + kalmanV0s.size() + "), skipping event " - + event.getEventNumber()); - return; - } - - for (int i = 0; i < billoirV0s.size(); i++) { - ReconstructedParticle billoirV0 = billoirV0s.get(i); - ReconstructedParticle kalmanV0 = kalmanV0s.get(i); - BilliorVertex billoirVtx = (BilliorVertex) billoirV0.getStartVertex(); - BilliorVertex kalmanVtx = (BilliorVertex) kalmanV0.getStartVertex(); - - List daughters = billoirV0.getParticles(); - ReconstructedParticle eleDaughter = daughters.get(0).getCharge() < 0 ? daughters.get(0) : daughters.get(1); - ReconstructedParticle posDaughter = daughters.get(0).getCharge() < 0 ? daughters.get(1) : daughters.get(0); - - if (event.getRunNumber() == 14596 && event.getEventNumber() == 734) { - System.out.println("CANDIDATE i=" + i + " billoirVtxZ=" + billoirVtx.getPosition().z() - + " billoirChi2=" + billoirVtx.getChi2() - + " kalmanVtxZ=" + kalmanVtx.getPosition().z() - + " kalmanChi2=" + kalmanVtx.getChi2()); - dumpTrack("ELECTRON", eleDaughter.getTracks().get(0)); - dumpTrack("POSITRON", posDaughter.getTracks().get(0)); - } - boolean eleMatched = eleMC != null && eleMC.equals(trackToMC.get(eleDaughter.getTracks().get(0))); - boolean posMatched = posMC != null && posMC.equals(trackToMC.get(posDaughter.getTracks().get(0))); - - Hep3Vector vtxPos = billoirVtx.getPosition(); - Hep3Vector vtxPosErr = billoirVtx.getPositionError(); - Hep3Vector kalVtxPos = kalmanVtx.getPosition(); - Hep3Vector kalVtxPosErr = kalmanVtx.getPositionError(); - Double kalNdf = kalmanVtx.getCustomParameters().get("ndf"); - - 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("kalVtxX/D", kalVtxPos.x()); - row.put("kalVtxY/D", kalVtxPos.y()); - row.put("kalVtxZ/D", kalVtxPos.z()); - row.put("kalVtxXErr/D", kalVtxPosErr.x()); - row.put("kalVtxYErr/D", kalVtxPosErr.y()); - row.put("kalVtxZErr/D", kalVtxPosErr.z()); - row.put("kalChi2/D", kalmanVtx.getChi2()); - row.put("kalNdf/I", kalNdf != null ? kalNdf : -9999.0); - row.put("kalMass/D", kalmanVtx.getInvMass()); - row.put("eleTruthMatched/I", eleMatched ? 1.0 : 0.0); - row.put("posTruthMatched/I", posMatched ? 1.0 : 0.0); - row.put("bothTruthMatchedToAp/I", eleMatched && posMatched ? 1.0 : 0.0); - 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); - - 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(); - } - } - - private static void dumpTrack(String label, Track track) { - TrackState ts = TrackStateUtils.getTrackStatesAtLocation(track, TrackState.AtPerigee).get(0); - double[] par = ts.getParameters(); - double[] cov = ts.getCovMatrix(); - System.out.println("=== " + label + " AtPerigee ==="); - System.out.println(" d0=" + par[0] + " phi0=" + par[1] + " omega=" + par[2] - + " z0=" + par[3] + " tanLambda=" + par[4] + " bLocal=" + ts.getBLocal()); - System.out.println(" covPacked=" + Arrays.toString(cov)); - System.out.println(" referencePoint=" + Arrays.toString(ts.getReferencePoint())); - - System.out.println(" track.getTrackStates().size()=" + track.getTrackStates().size()); - for (int k = 0; k < track.getTrackStates().size(); k++) { - TrackState tsk = track.getTrackStates().get(k); - System.out.println(" state[" + k + "] location=" + tsk.getLocation() - + " sameObjAsPerigeeLookup=" + (tsk == ts) - + " d0=" + tsk.getParameter(0) + " z0=" + tsk.getParameter(3)); - } - TrackState ts0 = track.getTrackStates().get(0); - System.out.println(" getTrackStates().get(0): d0=" + ts0.getParameter(0) - + " phi0=" + ts0.getParameter(1) + " omega=" + ts0.getParameter(2) - + " z0=" + ts0.getParameter(3) + " tanLambda=" + ts0.getParameter(4) - + " bLocal=" + ts0.getBLocal() + " location=" + ts0.getLocation()); - System.out.println(" ts0==tsPerigee: " + (ts0 == ts)); - } -} 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/analysis/src/test/java/org/hps/analysis/tuple/CascadeVertexTupleDriverTest.java b/analysis/src/test/java/org/hps/analysis/tuple/CascadeVertexTupleDriverTest.java deleted file mode 100644 index fae683f1c2..0000000000 --- a/analysis/src/test/java/org/hps/analysis/tuple/CascadeVertexTupleDriverTest.java +++ /dev/null @@ -1,102 +0,0 @@ -package org.hps.analysis.tuple; - -import java.io.BufferedReader; -import java.io.File; -import java.io.FileReader; - -import org.hps.conditions.database.DatabaseConditionsManager; -import org.hps.detector.svt.SvtDetectorSetup; -import org.hps.recon.particle.HpsReconParticleDriver; -import org.hps.util.test.TestUtil; -import org.lcsim.util.loop.LCSimLoop; - -import junit.framework.TestCase; - -/** - * End-to-end smoke test for the cascade-vertex reconstruction chain: runs a fresh - * {@code HpsReconParticleDriver} (so V0 candidates are rebuilt with the current - * full-covariance BilliorVertexer) followed by {@link CascadeVertexTupleDriver} over a - * real reconstructed 2016 physics-run sample, and checks the resulting flat ntuple. - */ -public class CascadeVertexTupleDriverTest extends TestCase { - - private static final String LCIO_INPUT_FILE_NAME = "hps_005772.0_recon_Rv4657-0-10000.slcio"; - private static final String TUPLE_OUTPUT_FILE_NAME = "target/test-output/cascade_vertex_tuple.txt"; - private static final String CASCADE_COL_NAME = "CascadeVertexCandidates"; - - public void testIt() throws Exception { - File lcioInputFile = TestUtil.downloadTestFile(LCIO_INPUT_FILE_NAME); - - LCSimLoop loop = new LCSimLoop(); - loop.setLCIORecordSource(lcioInputFile); - - final DatabaseConditionsManager manager = DatabaseConditionsManager.getInstance(); - manager.addConditionsListener(new SvtDetectorSetup()); - - loop.add(new org.lcsim.recon.tracking.digitization.sisim.config.ReadoutCleanupDriver()); - loop.add(new org.hps.recon.filtering.EventFlagFilter()); - - org.lcsim.recon.tracking.digitization.sisim.config.RawTrackerHitSensorSetup rthss - = new org.lcsim.recon.tracking.digitization.sisim.config.RawTrackerHitSensorSetup(); - rthss.setReadoutCollections(new String[]{"SVTRawTrackerHits"}); - loop.add(rthss); - - HpsReconParticleDriver reconParticleDriver = new HpsReconParticleDriver(); - reconParticleDriver.setEcalClusterCollectionName("EcalClustersCorr"); - reconParticleDriver.setTrackCollectionNames(new String[]{"GBLTracks"}); - reconParticleDriver.setUnconstrainedV0CandidatesColName("UnconstrainedV0CandidatesCascadeTest"); - reconParticleDriver.setFinalStateParticlesColName("FinalStateParticlesCascadeTest"); - reconParticleDriver.setCascadeVertexCandidatesColName(CASCADE_COL_NAME); - loop.add(reconParticleDriver); - - File tupleOutputFile = new File(TUPLE_OUTPUT_FILE_NAME); - tupleOutputFile.getParentFile().mkdirs(); - CascadeVertexTupleDriver tupleDriver = new CascadeVertexTupleDriver(); - tupleDriver.setCascadeVertexCandidatesColName(CASCADE_COL_NAME); - tupleDriver.setTupleFile(TUPLE_OUTPUT_FILE_NAME); - loop.add(tupleDriver); - - loop.loop(5000); - loop.dispose(); - - checkTupleOutput(tupleOutputFile); - } - - private void checkTupleOutput(File tupleOutputFile) throws Exception { - assertTrue("cascade vertex tuple file should exist", tupleOutputFile.exists()); - - BufferedReader reader = new BufferedReader(new FileReader(tupleOutputFile)); - String header = reader.readLine(); - assertNotNull("cascade vertex tuple file should have a header line", header); - String[] headerVars = header.split(":"); - - int nRows = 0; - String line; - while ((line = reader.readLine()) != null) { - if (line.trim().isEmpty()) { - continue; - } - nRows++; - String[] fields = line.trim().split("\\s+"); - assertEquals("row should have one value per header variable", headerVars.length, fields.length); - - double chi2 = -1, ndf = -1; - for (int i = 0; i < headerVars.length; i++) { - double value = Double.parseDouble(fields[i]); - assertTrue("field " + headerVars[i] + " should be finite, got " + value, - !Double.isNaN(value) && !Double.isInfinite(value)); - if (headerVars[i].equals("cascadeChi2/D")) { - chi2 = value; - } - if (headerVars[i].equals("cascadeNdf/I")) { - ndf = value; - } - } - assertTrue("cascadeChi2 should be non-negative, got " + chi2, chi2 >= 0); - assertEquals("cascadeNdf should be 1", 1.0, ndf); - } - reader.close(); - - assertTrue("at least one cascade vertex candidate should have been written", nRows > 0); - } -} diff --git a/recon/src/main/java/org/hps/recon/filtering/ThreeElectronTrackingFilter.java b/recon/src/main/java/org/hps/recon/filtering/ThreeElectronTrackingFilter.java index 28f1df57ab..0c36e0f04e 100644 --- a/recon/src/main/java/org/hps/recon/filtering/ThreeElectronTrackingFilter.java +++ b/recon/src/main/java/org/hps/recon/filtering/ThreeElectronTrackingFilter.java @@ -16,7 +16,7 @@ * 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/ThreeTrackVertexCandidates, + * 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. 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 f7eda42945..4755ab6231 100644 --- a/recon/src/main/java/org/hps/recon/particle/HpsReconParticleDriver.java +++ b/recon/src/main/java/org/hps/recon/particle/HpsReconParticleDriver.java @@ -15,7 +15,6 @@ import org.hps.recon.vertexing.BilliorTrack; import org.hps.recon.vertexing.BilliorVertex; import org.hps.recon.vertexing.BilliorVertexer; -import org.hps.recon.vertexing.KalmanV0Vertexer; import org.hps.record.StandardCuts; import org.hps.recon.tracking.TrackStateUtils; import org.lcsim.detector.tracker.silicon.HpsSiSensor; @@ -202,6 +201,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) { @@ -675,25 +679,6 @@ private void makeV0Candidates(ReconstructedParticle electron, ReconstructedParti if (eleIsTop != posIsTop) { unconstrainedV0Vertices.add(vtxFit); unconstrainedV0Candidates.add(candidate); - - // Standalone unconstrained two-track Kalman vertex fit, run in parallel with - // the Billoir fit above (for comparison) on the same track pair. Off unless - // kalmanUnconstrainedV0CandidatesColName is set. Kept in lockstep with - // unconstrainedV0Candidates (same cuts already applied above) so the two - // collections can be paired by index downstream. - if (kalmanUnconstrainedV0CandidatesColName != null) { - BilliorVertex kalmanVtxFit; - try { - kalmanVtxFit = new KalmanV0Vertexer(bLocalForTrack(electron.getTracks().get(0))) - .fitVertex(electron.getTracks().get(0), positron.getTracks().get(0)); - } catch (RuntimeException e) { - LOGGER.warning("KalmanV0Vertexer fit failed, using placeholder: " + e.getMessage()); - kalmanVtxFit = KalmanV0Vertexer.placeholderVertex(); - } - ReconstructedParticle kalmanCandidate = makeReconstructedParticle(electron, positron, kalmanVtxFit); - kalmanUnconstrainedV0Vertices.add(kalmanVtxFit); - kalmanUnconstrainedV0Candidates.add(kalmanCandidate); - } } else { unconstrainedVcVertices.add(vtxFit); unconstrainedVcCandidates.add(candidate); 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 e87cefaaed..b905b8b905 100644 --- a/recon/src/main/java/org/hps/recon/particle/ReconParticleDriver.java +++ b/recon/src/main/java/org/hps/recon/particle/ReconParticleDriver.java @@ -7,6 +7,7 @@ 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; @@ -16,7 +17,7 @@ import org.hps.conditions.beam.BeamEnergy.BeamEnergyCollection; import org.hps.recon.tracking.CoordinateTransformations; import org.hps.recon.vertexing.CascadeVertexer; -import org.hps.recon.vertexing.ThreeTrackVertexer; +import org.hps.recon.vertexing.NTrackVertexer; import org.hps.record.StandardCuts; import org.hps.recon.utils.TrackClusterMatcher; @@ -63,6 +64,7 @@ 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; @@ -138,22 +140,10 @@ public void setApplyClusterCorrections(boolean val) { * constraints. */ protected List targetConV0Candidates; - /** - * Stores reconstructed V0 candidate particles from the standalone unconstrained - * two-track Kalman vertex fit ({@link org.hps.recon.vertexing.KalmanV0Vertexer}), - * run in parallel with the ordinary Billoir unconstrained V0 fit for comparison. - * Off by default; only filled if {@link #kalmanUnconstrainedV0CandidatesColName} is set. - */ - protected List kalmanUnconstrainedV0Candidates; /** * Stores reconstructed V0 candidate vertices generated without constraints. */ protected List unconstrainedV0Vertices; - /** - * Stores reconstructed V0 candidate vertices from the standalone unconstrained - * two-track Kalman vertex fit. See {@link #kalmanUnconstrainedV0Candidates}. - */ - protected List kalmanUnconstrainedV0Vertices; /** * Stores reconstructed V0 candidate vertices generated with beam spot * constraints. @@ -165,15 +155,53 @@ public void setApplyClusterCorrections(boolean val) { */ protected List targetConV0Vertices; /** - * Stores reconstructed cascade (V0 + recoil electron production vertex) - * candidate particles. + * Stores reconstructed 3-track (V0 e-/e+ + recoil electron) simultaneous + * vertex candidate particles. */ protected List cascadeVertexCandidates; /** - * Stores reconstructed 3-track (V0 e-/e+ + recoil electron) simultaneous - * vertex candidate particles. + * 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 threeTrackVertexCandidates; + 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 /** @@ -217,17 +245,6 @@ public void setApplyClusterCorrections(boolean val) { * constraints. */ protected String unconstrainedV0VerticesColName = null; - /** - * LCIO collection name for V0 candidate particles from the standalone unconstrained - * two-track Kalman vertex fit. Defaults to null, i.e. this fit is off unless a - * collection name is explicitly set. See {@link #kalmanUnconstrainedV0Candidates}. - */ - protected String kalmanUnconstrainedV0CandidatesColName = null; - /** - * LCIO collection name for V0 candidate vertices from the standalone unconstrained - * two-track Kalman vertex fit. See {@link #kalmanUnconstrainedV0CandidatesColName}. - */ - protected String kalmanUnconstrainedV0VerticesColName = null; /** * LCIO collection name for V0 candidate vertices generated with beam spot * constraints. @@ -239,22 +256,76 @@ public void setApplyClusterCorrections(boolean val) { */ protected String targetConV0VerticesColName = null; /** - * LCIO collection name for cascade (V0 + recoil electron production vertex) - * candidate particles. Defaults to null, i.e. cascade vertexing is off unless + * 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 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. + * 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 threeTrackVertexCandidatesColName = null; + 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; /** * MC-truth collection names used only to gate the {@code BADFIT_DEBUG} diagnostic - * print in {@link ThreeTrackVertexer#fit} to genuinely truth-matched V0(e-/e+) + + * print in {@link CascadeVertexer#fit} to genuinely truth-matched V0(e-/e+) + * recoil-electron combinations, rather than every combinatorial pairing tried by - * {@link #findThreeTrackVertices}. Both default to null, i.e. no truth info is + * {@link #findCascadeVertices}. Both default to null, i.e. no truth info is * looked up and the debug print never fires, unless both are explicitly set (MC * steering files only). */ @@ -262,14 +333,19 @@ public void setApplyClusterCorrections(boolean val) { protected String trackToMCParticleRelationsColName = null; // Accumulated wall-clock time (ns) and count of calls spent in the cascade - // (helix + straight-line) vertex fit, reported in endOfData(). + // simultaneous vertex fit, reported in endOfData(). private long cascadeVertexFitTimeNs = 0L; private int cascadeVertexFitCount = 0; - // Accumulated wall-clock time (ns) and count of calls spent in the 3-track - // simultaneous vertex fit, reported in endOfData(). - private long threeTrackVertexFitTimeNs = 0L; - private int threeTrackVertexFitCount = 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 @@ -312,9 +388,9 @@ public void setBeamConV0CandidatesColName(String beamConV0CandidatesColName) { } /** - * Sets the name of the LCIO collection for cascade (V0 + recoil electron - * production vertex) candidate particles. Setting this enables cascade - * vertexing, which is off by default. + * 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. */ @@ -323,19 +399,122 @@ public void setCascadeVertexCandidatesColName(String cascadeVertexCandidatesColN } /** - * 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. + * 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 threeTrackVertexCandidatesColName - The LCIO collection name. + * @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 setThreeTrackVertexCandidatesColName(String threeTrackVertexCandidatesColName) { - this.threeTrackVertexCandidatesColName = threeTrackVertexCandidatesColName; + 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 MCParticle collection used only to gate the - * {@code BADFIT_DEBUG} print in {@link #findThreeTrackVertices} to truth-matched + * {@code BADFIT_DEBUG} print in {@link #findCascadeVertices} to truth-matched * candidates. Optional; MC steering files only. * * @param mcParticlesColName - The LCIO collection name. @@ -346,7 +525,7 @@ public void setMcParticlesColName(String mcParticlesColName) { /** * Sets the name of the LCIO Track-to-MCParticle LCRelation collection used only to - * gate the {@code BADFIT_DEBUG} print in {@link #findThreeTrackVertices} to + * gate the {@code BADFIT_DEBUG} print in {@link #findCascadeVertices} to * truth-matched candidates. Optional; MC steering files only. * * @param trackToMCParticleRelationsColName - The LCIO collection name. @@ -519,28 +698,6 @@ public void setUnconstrainedV0VerticesColName(String unconstrainedV0VerticesColN this.unconstrainedV0VerticesColName = unconstrainedV0VerticesColName; } - /** - * Sets the name of the LCIO collection for V0 candidate particles from the - * standalone unconstrained two-track Kalman vertex fit. Setting this enables that - * fit (run in parallel with, not instead of, the ordinary Billoir unconstrained V0 - * fit), which is off by default. - * - * @param kalmanUnconstrainedV0CandidatesColName - The LCIO collection name. - */ - public void setKalmanUnconstrainedV0CandidatesColName(String kalmanUnconstrainedV0CandidatesColName) { - this.kalmanUnconstrainedV0CandidatesColName = kalmanUnconstrainedV0CandidatesColName; - } - - /** - * Sets the name of the LCIO collection for V0 candidate vertices from the - * standalone unconstrained two-track Kalman vertex fit. - * - * @param kalmanUnconstrainedV0VerticesColName - The LCIO collection name. - */ - public void setKalmanUnconstrainedV0VerticesColName(String kalmanUnconstrainedV0VerticesColName) { - this.kalmanUnconstrainedV0VerticesColName = kalmanUnconstrainedV0VerticesColName; - } - /** * Set the names of the LCIO track collections used as input. * @@ -571,15 +728,27 @@ public void setDisablePID(boolean disablePID) { } /** - * When true, {@link #findThreeTrackVertices}'s {@link ThreeTrackVertexer} holds V2's + * 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 ThreeTrackVertexer#setFixV2BeamCoordinate}. Default false keeps the + * 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; } @@ -894,12 +1063,16 @@ protected void process(EventHeader event) { beamConV0Candidates = new ArrayList(); targetConV0Candidates = new ArrayList(); unconstrainedV0Vertices = new ArrayList(); - kalmanUnconstrainedV0Candidates = new ArrayList(); - kalmanUnconstrainedV0Vertices = new ArrayList(); beamConV0Vertices = new ArrayList(); targetConV0Vertices = new ArrayList(); cascadeVertexCandidates = new ArrayList(); - threeTrackVertexCandidates = 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. @@ -933,20 +1106,13 @@ else if (finalStateParticle.getCharge() < 0) { // VERBOSE :: Output the number of reconstructed particles. printDebug("Final State Particles :: " + goodFinalStateParticles.size()); - // Form cascade (V0 + recoil electron production 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"); - } // 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 (threeTrackVertexCandidatesColName != null) { - findThreeTrackVertices(event, unconstrainedV0Candidates, goodFinalStateParticles); - printDebug("[ReconParticleDriver] findThreeTrackVertices() finished"); + if (cascadeVertexCandidatesColName != null) { + findCascadeVertices(event, unconstrainedV0Candidates, goodFinalStateParticles); + printDebug("[ReconParticleDriver] findCascadeVertices() finished"); } // Add the final state ReconstructedParticles to the event event.put(finalStateParticlesColName, goodFinalStateParticles, ReconstructedParticle.class, 0); @@ -976,14 +1142,6 @@ else if (finalStateParticle.getCharge() < 0) { printDebug("Unconstrained V0 Vertices: " + unconstrainedV0Vertices.size()); event.put(unconstrainedV0VerticesColName, unconstrainedV0Vertices, Vertex.class, 0); } - if (kalmanUnconstrainedV0CandidatesColName != null) { - printDebug("Kalman Unconstrained V0 Candidates: " + kalmanUnconstrainedV0Candidates.size()); - event.put(kalmanUnconstrainedV0CandidatesColName, kalmanUnconstrainedV0Candidates, ReconstructedParticle.class, 0); - } - if (kalmanUnconstrainedV0VerticesColName != null) { - printDebug("Kalman Unconstrained V0 Vertices: " + kalmanUnconstrainedV0Vertices.size()); - event.put(kalmanUnconstrainedV0VerticesColName, kalmanUnconstrainedV0Vertices, Vertex.class, 0); - } if (beamConV0VerticesColName != null) { printDebug("Beam-Constrained V0 Vertices: " + beamConV0Vertices.size()); event.put(beamConV0VerticesColName, beamConV0Vertices, Vertex.class, 0); @@ -996,15 +1154,51 @@ else if (finalStateParticle.getCharge() < 0) { printDebug("Cascade Vertex Candidates: " + cascadeVertexCandidates.size()); event.put(cascadeVertexCandidatesColName, cascadeVertexCandidates, ReconstructedParticle.class, 0); } - if (threeTrackVertexCandidatesColName != null) { - printDebug("3-Track Vertex Candidates: " + threeTrackVertexCandidates.size()); - event.put(threeTrackVertexCandidatesColName, threeTrackVertexCandidates, 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}/{@code ThreeTrackVertexer} + * 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 @@ -1022,60 +1216,34 @@ protected double bLocalForTrack(Track track) { } /** - * Forms cascade (production) vertex candidates: for each unconstrained V0 candidate, + * 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 the production vertex where the V0's momentum line meets the recoil electron's - * helix, and add the result to {@link #cascadeVertexCandidates}. Mirrors the + * 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. - * - * @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); - CascadeVertexer cascadeVertexer = new CascadeVertexer(bLocalForTrack(v0EleDaughter.getTracks().get(0))); - for (ReconstructedParticle electron : finalStateElectrons) { - if (electron.getCharge() >= 0 || v0Daughters.contains(electron)) { - continue; - } - long fitStartTime = System.nanoTime(); - try { - ReconstructedParticle cascade = cascadeVertexer.fit(v0, electron); - if (cascade != null) { - cascadeVertexCandidates.add(cascade); - } - } catch (RuntimeException e) { - printDebug("[ReconParticleDriver] findCascadeVertices: skipping pair after RuntimeException: " + e.getMessage()); - continue; - } finally { - cascadeVertexFitTimeNs += System.nanoTime() - fitStartTime; - cascadeVertexFitCount++; - } - } - } - } - - /** - * Forms 3-track 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 #threeTrackVertexCandidates}. Mirrors - * {@link #findCascadeVertices} exactly, using {@link ThreeTrackVertexer} instead of - * {@link CascadeVertexer}. + * 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 event Current event, used only to look up MC truth (via * {@link #mcParticlesColName}/{@link #trackToMCParticleRelationsColName}) - * to gate {@link ThreeTrackVertexer}'s BADFIT_DEBUG print + * to gate {@link CascadeVertexer}'s BADFIT_DEBUG print * to truth-matched candidates; unused otherwise. * @param v0Candidates Unconstrained V0 candidates for this event. * @param finalStateElectrons Final-state electrons for this event. */ - protected void findThreeTrackVertices(EventHeader event, List v0Candidates, + protected void findCascadeVertices(EventHeader event, List v0Candidates, List finalStateElectrons) { Map trackToMC = new HashMap(); MCParticle eleMC = null; @@ -1127,8 +1295,12 @@ protected void findThreeTrackVertices(EventHeader event, List= 0 || v0Daughters.contains(electron)) { continue; @@ -1137,16 +1309,128 @@ protected void findThreeTrackVertices(EventHeader event, 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] findThreeTrackVertices: skipping pair after RuntimeException: " + e.getMessage()); + printDebug("[ReconParticleDriver] findCascadeVertices: skipping pair after RuntimeException: " + e.getMessage()); continue; } finally { - threeTrackVertexFitTimeNs += System.nanoTime() - fitStartTime; - threeTrackVertexFitCount++; + cascadeVertexFitTimeNs += System.nanoTime() - fitStartTime; + cascadeVertexFitCount++; } } } @@ -1193,17 +1477,9 @@ protected void endOfData() { if (cascadeVertexCandidatesColName != null && cascadeVertexFitCount > 0) { double totalTimeMs = cascadeVertexFitTimeNs / 1e6; double timePerFitMs = totalTimeMs / cascadeVertexFitCount; - System.out.format("ReconParticleDriver.endOfData: total cascade (helix+straight) vertex fit " + 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); - } - if (threeTrackVertexCandidatesColName != null && threeTrackVertexFitCount > 0) { - double totalTimeMs = threeTrackVertexFitTimeNs / 1e6; - double timePerFitMs = totalTimeMs / threeTrackVertexFitCount; - System.out.format("ReconParticleDriver.endOfData: total 3-track simultaneous vertex fit " - + "execution time=%12.4f ms for %d fits.\n", totalTimeMs, threeTrackVertexFitCount); - System.out.format(" 3-track vertex fit time per fit = %9.4f ms\n", + System.out.format(" cascade vertex fit time per fit = %9.4f ms\n", timePerFitMs); } } 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 c49f98b54a..eb7edd10ca 100644 --- a/recon/src/main/java/org/hps/recon/vertexing/BilliorVertex.java +++ b/recon/src/main/java/org/hps/recon/vertexing/BilliorVertex.java @@ -421,6 +421,9 @@ 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))); } diff --git a/recon/src/main/java/org/hps/recon/vertexing/CascadeVertexer.java b/recon/src/main/java/org/hps/recon/vertexing/CascadeVertexer.java index 3c1c668741..8ae382be2e 100644 --- a/recon/src/main/java/org/hps/recon/vertexing/CascadeVertexer.java +++ b/recon/src/main/java/org/hps/recon/vertexing/CascadeVertexer.java @@ -1,6 +1,8 @@ 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; @@ -8,6 +10,7 @@ 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; @@ -21,76 +24,384 @@ import org.lcsim.event.base.BaseReconstructedParticle; import org.hps.recon.tracking.TrackStateUtils; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.FitResult; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.LineParams; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.LinePlaneProjection; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TrackParams; +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 "cascade" (production) vertex where an already-vertexed neutral V0's momentum - * direction -- treated as a zero-curvature line, carrying its full 6x6 position-momentum - * joint covariance -- meets a third, curving recoil-electron track. Wraps - * {@link KalmanVertexFitterGainMatrix#fitCascadeVertex} and packages the result as a - * 2-daughter ReconstructedParticle (V0, recoil electron) with the production Vertex - * attached via setStartVertex(...), mirroring the conventions used by - * {@code HpsReconParticleDriver#makeReconstructedParticle}. + * 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 { +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; - private static final double ELECTRON_MASS = 0.000511; + public CascadeVertexer(double bField) { + super(bField); + } - private final double bField; + public void setFixV2BeamCoordinate(boolean fixV2BeamCoordinate) { + this.fixV2BeamCoordinate = fixV2BeamCoordinate; + } - public CascadeVertexer(double bField) { - this.bField = bField; + public void setUseBeamspotConstraintForV2(boolean useBeamspotConstraintForV2) { + this.useBeamspotConstraintForV2 = useBeamspotConstraintForV2; } /** - * Fit the production vertex where {@code v0Particle}'s momentum line meets - * {@code recoilElectron}'s helix. + * 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 {@code getStartVertex()} is a - * {@link BilliorVertex} (as produced by BilliorVertexer / - * HpsReconParticleDriver) + * @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, or null if the fit fails + * @param truthMatched whether this (v0Particle, recoilElectron) combination is the + * MC-truth-matched signal pairing; gates the BADFIT_DEBUG print + * below so it isn't flooded by wrong-pairing combinatorics that + * are expected to fit badly. Callers without truth info should + * pass {@code true} to preserve the original always-print behavior. + * @return the cascade ReconstructedParticle (V0 particle + recoil electron), or null + * if the fit fails */ - public ReconstructedParticle fit(ReconstructedParticle v0Particle, ReconstructedParticle recoilElectron) { - BilliorVertex v0Vertex = (BilliorVertex) v0Particle.getStartVertex(); - LineParams v0Line = KalmanVertexFitterGainMatrix.lineFromV0(v0Vertex); - - Track recoilTrack = recoilElectron.getTracks().get(0); - TrackState recoilTs = TrackStateUtils.getTrackStatesAtLocation(recoilTrack, TrackState.AtPerigee).get(0); - TrackParams recoilParams = trackParamsFromTrack(recoilTrack); + public ReconstructedParticle fit(ReconstructedParticle v0Particle, ReconstructedParticle recoilElectron, boolean truthMatched) { + return fit(v0Particle, recoilElectron, truthMatched, false, 0.0, 0.0, 0.0); + } - // recoilParams' raw d0/z0 (and hence the vertex unknown inside fitCascadeVertex) are - // relative to the recoil track's own reference point, whereas v0Line (built from an - // already-fit, absolute-frame BilliorVertex) is not -- shift v0Line into that same local - // frame before fitting, then shift the fitted vertex back to the absolute frame below. - double[] refPoint = recoilTs.getReferencePoint(); + /** + * Same as {@link #fit(ReconstructedParticle, ReconstructedParticle, boolean)}, 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 truthMatched, 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); - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(bField); - FitResult result = fitter.fitCascadeVertex(recoilParams, v0LineLocal); - if (result == null) { - return null; - } - result.vertex = result.vertex.add(MatrixUtils.createRealVector(refPoint)); - - // Diagnostic-only target-plane projections (not used by the fit 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 production vertex -- same convention as the analogous - // diagnostics added to ThreeTrackVertexer. v0Line is already absolute-frame; recoilParams - // is not, so its plane crossing must be evaluated at the plane's position in the recoil - // track's local frame, then shifted back to absolute. - LinePlaneProjection v0Proj = KalmanVertexFitterGainMatrix.propagateLineToPlane( + 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 recoilProjLocal = KalmanVertexFitterGainMatrix.propagateTrackToPlane( + 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); - return makeReconstructedParticle(v0Particle, recoilElectron, result, v0Line, v0Proj, recoilProj, v0Vertex); + 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); + if (truthMatched && finalResult.chi2 / finalResult.ndf > 1000) { + System.out.println("BADFIT_DEBUG bField=" + bField); + System.out.println("BADFIT_DEBUG v1Init=" + v1Init); + System.out.println("BADFIT_DEBUG v1Cov=" + v1Cov); + System.out.println("BADFIT_DEBUG v2Init=" + v2InitForFit); + System.out.println("BADFIT_DEBUG v2Cov=" + v2CovForFit); + System.out.println("BADFIT_DEBUG eleParams=" + dumpTrackParams(eleParams)); + System.out.println("BADFIT_DEBUG posParams=" + dumpTrackParams(posParams)); + System.out.println("BADFIT_DEBUG recoilParams=" + dumpTrackParams(recoilParams)); + System.out.println("BADFIT_DEBUG chi2=" + finalResult.chi2 + " ndf=" + finalResult.ndf); + System.out.println("BADFIT_DEBUG v1=" + finalResult.v1 + " v2=" + finalResult.v2); + } + + 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; } /** @@ -103,122 +414,194 @@ private static LineParams shiftLine(LineParams line, double[] refPoint, double s line.dx, line.dy, line.dz, line.cov); } - /** - * Extract perigee TrackParams (d0, phi0, omega, z0, tanLambda) from a Track's - * AtPerigee TrackState. Parameter order matches {@code TrackState.getParameters()} - * exactly (same convention as {@code BilliorTrack(Track)}), so no reordering is needed. - */ - private static TrackParams trackParamsFromTrack(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); + private static String dumpTrackParams(TrackParams tp) { + double[] a = tp.toArray(); + StringBuilder sb = new StringBuilder(); + sb.append("d0=").append(a[0]).append(" phi0=").append(a[1]).append(" omega=").append(a[2]) + .append(" z0=").append(a[3]).append(" tanLambda=").append(a[4]).append(" cov=["); for (int i = 0; i < 5; i++) { for (int j = 0; j < 5; j++) { - cov.setEntry(i, j, sm.e(i, j)); + sb.append(tp.cov.getEntry(i, j)); + if (j < 4) { + sb.append(","); + } + } + if (i < 4) { + sb.append(";"); } } - return new TrackParams(par[0], par[1], par[2], par[3], par[4], cov); + sb.append("]"); + return sb.toString(); } - private static ReconstructedParticle makeReconstructedParticle(ReconstructedParticle v0Particle, - ReconstructedParticle recoilElectron, FitResult result, LineParams v0Line, + 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 KalmanVertexFitterGainMatrix.fitVertex(...) and BilliorVertexer. - Hep3Vector vtxPos = new BasicHep3Vector( - result.vertex.getEntry(1), - result.vertex.getEntry(2), - result.vertex.getEntry(0)); - - double[] covPacked = new double[6]; - covPacked[0] = result.vertexCov.getEntry(1, 1); - covPacked[1] = result.vertexCov.getEntry(2, 1); - covPacked[2] = result.vertexCov.getEntry(2, 2); - covPacked[3] = result.vertexCov.getEntry(0, 1); - covPacked[4] = result.vertexCov.getEntry(0, 2); - covPacked[5] = result.vertexCov.getEntry(0, 0); - SymmetricMatrix covVtx = new SymmetricMatrix(3, covPacked, true); - - Hep3Vector vtxPosErr = new BasicHep3Vector( - FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(1, 1))), - FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(2, 2))), - FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(0, 0)))); - - // Recoil electron's momentum evaluated at the fitted production vertex. - RealVector pRecoilTrk = result.trackMomenta.get(0).p; - Hep3Vector pRecoil = new BasicHep3Vector( - pRecoilTrk.getEntry(1), pRecoilTrk.getEntry(2), pRecoilTrk.getEntry(0)); - - // The V0's momentum is constant along its (zero-curvature) line, so it is - // unchanged by the fit: it is exactly the direction vector carried by v0Line. - Hep3Vector pV0 = new BasicHep3Vector(v0Line.dy, v0Line.dz, v0Line.dx); - - Map pFitMap = new HashMap(); - pFitMap.put(0, pV0); - pFitMap.put(1, pRecoil); - - double v0Mass = v0Particle.getMass(); + // 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 eV0 = FastMath.sqrt(v0PMag * v0PMag + v0Mass * v0Mass); + 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; - BilliorVertex vtxFit = new BilliorVertex(vtxPos, covVtx, result.chi2, invMass, pFitMap, "CASCADE"); - vtxFit.setPositionError(vtxPosErr); - vtxFit.setProbability(result.ndf); - vtxFit.setParameter("ndf", (double) result.ndf); + 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). - vtxFit.setParameter("v0ProjX", v0Proj.position.getEntry(1)); - vtxFit.setParameter("v0ProjY", v0Proj.position.getEntry(2)); - vtxFit.setParameter("v0ProjXErr", FastMath.sqrt(FastMath.abs(v0Proj.cov.getEntry(1, 1)))); - vtxFit.setParameter("v0ProjYErr", FastMath.sqrt(FastMath.abs(v0Proj.cov.getEntry(2, 2)))); - vtxFit.setParameter("recoilProjX", recoilProj.position.getEntry(1)); - vtxFit.setParameter("recoilProjY", recoilProj.position.getEntry(2)); - vtxFit.setParameter("recoilProjXErr", FastMath.sqrt(FastMath.abs(recoilProj.cov.getEntry(1, 1)))); - vtxFit.setParameter("recoilProjYErr", FastMath.sqrt(FastMath.abs(recoilProj.cov.getEntry(2, 2)))); - - // The original two-body Billior V0 fit (position, chi2, mass). Here v0Particle is - // untouched by this fit, so this duplicates v0Particle.getStartVertex(); stored under - // the same parameter names as ThreeTrackVertexer's cascade (where it does NOT - // duplicate v0Particle's startVertex) so CascadeVertexTupleDriver can read both - // outputs uniformly. + // 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(); - vtxFit.setParameter("v0InputVtxX", v0InputPos.x()); - vtxFit.setParameter("v0InputVtxY", v0InputPos.y()); - vtxFit.setParameter("v0InputVtxZ", v0InputPos.z()); - vtxFit.setParameter("v0InputVtxXErr", v0InputPosErr.x()); - vtxFit.setParameter("v0InputVtxYErr", v0InputPosErr.y()); - vtxFit.setParameter("v0InputVtxZErr", v0InputPosErr.z()); - vtxFit.setParameter("v0InputChi2", v0InputVertex.getChi2()); - vtxFit.setParameter("v0InputMass", v0InputVertex.getInvMass()); - - ReconstructedParticle candidate = new BaseReconstructedParticle(); - ((BaseReconstructedParticle) candidate).setStartVertex(vtxFit); - candidate.addParticle(v0Particle); - candidate.addParticle(recoilElectron); - ((BaseReconstructedParticle) candidate).setType(recoilElectron.getType()); - ((BaseReconstructedParticle) candidate).setMass(invMass); - + 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) candidate).set4Vector(fourVector); + ((BaseReconstructedParticle) cascade).set4Vector(fourVector); + double particleCharge = v0Charge + recoilElectron.getCharge(); + ((BaseReconstructedParticle) cascade).setCharge(particleCharge); + v2Vtx.setAssociatedParticle(cascade); + ((BaseReconstructedParticle) cascade).setReferencePoint(v2Vtx.getPosition()); - double particleCharge = v0Particle.getCharge() + recoilElectron.getCharge(); - ((BaseReconstructedParticle) candidate).setCharge(particleCharge); + return cascade; + } - vtxFit.setAssociatedParticle(candidate); - ((BaseReconstructedParticle) candidate).setReferencePoint(vtxFit.getPosition()); + private static Hep3Vector toDetFrame(RealVector pTrk) { + return new BasicHep3Vector(pTrk.getEntry(1), pTrk.getEntry(2), pTrk.getEntry(0)); + } - return candidate; + 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/KalmanNTrackVertexer.java b/recon/src/main/java/org/hps/recon/vertexing/KalmanNTrackVertexer.java deleted file mode 100644 index 6dc1503df0..0000000000 --- a/recon/src/main/java/org/hps/recon/vertexing/KalmanNTrackVertexer.java +++ /dev/null @@ -1,251 +0,0 @@ -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.SymmetricMatrix; -import hep.physics.vec.BasicHep3Vector; -import hep.physics.vec.Hep3Vector; -import hep.physics.vec.VecOp; - -import org.lcsim.event.Track; -import org.lcsim.event.TrackState; - -import org.hps.recon.tracking.TrackStateUtils; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.FitResult; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TrackMomentum; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TrackParams; - -/** - * Fits a plain, unconstrained N-track common vertex (all input tracks constrained to the - * same (x,y,z) point, all assumed to be electron-mass) using the Kalman gain-matrix fitter - * ({@link KalmanVertexFitterGainMatrix}), as an alternative to {@link BilliorVertexer} for - * the same problem. This is a generalization of {@link KalmanV0Vertexer} (which is - * hardcoded to exactly 2 tracks) to arbitrary N -- e.g. for 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 KalmanNTrackVertexer { - - private static final double ELECTRON_MASS = 0.000511; - - private final double bField; - - public KalmanNTrackVertexer(double bField) { - this.bField = bField; - } - - /** - * Fit the common vertex of N tracks, with no vertex/momentum/mass constraint. All - * tracks are assumed to share the same reference point (true for AtPerigee states as - * produced by the tracking reconstruction). - * - * @param tracks the input tracks (any N >= 2) - * @return the fitted vertex, or a sentinel placeholder vertex if the fit fails - */ - public BilliorVertex fitVertex(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)); - } - - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(bField); - FitResult result = fitter.fit(trackParams); - if (result == null) { - return placeholderVertex(tracks.size()); - } - - return makeVertex(result, referencePoint); - } - - /** - * Fit the common vertex of N tracks with the total 3-momentum constrained to the beam - * value, using {@link KalmanVertexFitterGainMatrix#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 {@code makeVertex}-style 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 KalmanVertexFitterGainMatrix#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 KalmanVertexFitterGainMatrix#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)); - } - - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(bField); - fitter.setBeamEnergy(beamEnergy); - fitter.setBeamRotAngle(beamRotAngle); - fitter.setBeamMomentumTransverseNuclearRecoilSigma(transverseNuclearRecoilSigma); - fitter.setReferencePosition(referencePoint); - 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 KalmanVertexFitterGainMatrix#fitVertex(List, boolean, - * boolean, boolean)} directly (dispatches internally to {@code fitBillior1985}, which - * -- unlike the simpler {@code fit()} algorithm used by {@link #fitVertex(List)} -- - * already computes a real per-track momentum covariance). Kept as a separate method - * rather than changing {@link #fitVertex(List)} in place, so that method's existing - * (covariance-free) behavior remains available unchanged for any other caller. - * - * @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)); - } - - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(bField); - fitter.setReferencePosition(referencePoint); - BilliorVertex bv = fitter.fitVertex(trackParams, false, false, 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; - } - - /** - * Extract perigee TrackParams (d0, phi0, omega, z0, tanLambda) directly from an - * AtPerigee TrackState, with no reparametrization -- see {@link KalmanV0Vertexer}'s - * copy of this method for the full rationale (same convention here). - */ - private 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); - } - - /** - * Convert a Kalman {@code FitResult} for N tracks into a {@code BilliorVertex}, using - * the same tracking-frame -> detector-frame convention (det(x,y,z) = trk(y,z,x)) as - * {@link KalmanV0Vertexer#makeVertex}. All tracks are assumed electron-mass (valid for - * the trident use case: 2 e- + 1 e+). - * - * @param referencePoint the (shared) reference point of the input tracks, tracking - * frame; added back to {@code result.vertex} since the fitter's - * raw output is relative to that point, not the absolute frame - */ - private static BilliorVertex makeVertex(FitResult result, double[] referencePoint) { - Hep3Vector vtxPos = new BasicHep3Vector( - result.vertex.getEntry(1) + referencePoint[1], - result.vertex.getEntry(2) + referencePoint[2], - result.vertex.getEntry(0) + referencePoint[0]); - - double[] covPacked = new double[6]; - covPacked[0] = result.vertexCov.getEntry(1, 1); - covPacked[1] = result.vertexCov.getEntry(2, 1); - covPacked[2] = result.vertexCov.getEntry(2, 2); - covPacked[3] = result.vertexCov.getEntry(0, 1); - covPacked[4] = result.vertexCov.getEntry(0, 2); - covPacked[5] = result.vertexCov.getEntry(0, 0); - SymmetricMatrix covVtx = new SymmetricMatrix(3, covPacked, true); - - Hep3Vector vtxPosErr = new BasicHep3Vector( - FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(1, 1))), - FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(2, 2))), - FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(0, 0)))); - - Map pFitMap = new HashMap(); - double totalE = 0.0; - Hep3Vector totalP = new BasicHep3Vector(0, 0, 0); - for (int i = 0; i < result.trackMomenta.size(); i++) { - TrackMomentum tm = result.trackMomenta.get(i); - RealVector pTrk = tm.p; - Hep3Vector p = new BasicHep3Vector(pTrk.getEntry(1), pTrk.getEntry(2), pTrk.getEntry(0)); - pFitMap.put(i, p); - totalE += FastMath.sqrt(p.magnitudeSquared() + ELECTRON_MASS * ELECTRON_MASS); - totalP = VecOp.add(totalP, p); - } - double massSq = totalE * totalE - totalP.magnitudeSquared(); - double invMass = massSq > 0 ? FastMath.sqrt(massSq) : -99.0; - - BilliorVertex vtxFit = new BilliorVertex(vtxPos, covVtx, result.chi2, invMass, pFitMap, "KALMAN_NTRACK"); - vtxFit.setPositionError(vtxPosErr); - vtxFit.setProbability(result.ndf); - vtxFit.setParameter("ndf", (double) result.ndf); - return vtxFit; - } -} diff --git a/recon/src/main/java/org/hps/recon/vertexing/KalmanV0Vertexer.java b/recon/src/main/java/org/hps/recon/vertexing/KalmanV0Vertexer.java deleted file mode 100644 index 2846184731..0000000000 --- a/recon/src/main/java/org/hps/recon/vertexing/KalmanV0Vertexer.java +++ /dev/null @@ -1,155 +0,0 @@ -package org.hps.recon.vertexing; - -import java.util.Arrays; -import java.util.HashMap; -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.SymmetricMatrix; -import hep.physics.vec.BasicHep3Vector; -import hep.physics.vec.Hep3Vector; -import hep.physics.vec.VecOp; - -import org.lcsim.event.Track; -import org.lcsim.event.TrackState; - -import org.hps.recon.tracking.TrackStateUtils; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.FitResult; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TrackParams; - -/** - * Fits a plain, unconstrained two-track (e-/e+) V0 vertex using the Kalman gain-matrix - * fitter ({@link KalmanVertexFitterGainMatrix}), as an alternative to {@link - * BilliorVertexer} for the same problem. 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 the existing - * {@code HpsReconParticleDriver#makeReconstructedParticle} and tuple-reading code. - */ -public class KalmanV0Vertexer { - - private static final double ELECTRON_MASS = 0.000511; - - private final double bField; - - public KalmanV0Vertexer(double bField) { - this.bField = bField; - } - - /** - * Fit the vertex of an electron and a positron track, with no vertex/momentum/mass - * constraint. - * - * @param eleTrack the electron track - * @param posTrack the positron track - * @return the fitted vertex, or null if the fit fails - */ - public BilliorVertex fitVertex(Track eleTrack, Track posTrack) { - TrackState eleTs = TrackStateUtils.getTrackStatesAtLocation(eleTrack, TrackState.AtPerigee).get(0); - TrackParams eleParams = trackParamsFromTrack(eleTs); - TrackParams posParams = trackParamsFromTrack( - TrackStateUtils.getTrackStatesAtLocation(posTrack, TrackState.AtPerigee).get(0)); - - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(bField); - FitResult result = fitter.fit(Arrays.asList(eleParams, posParams)); - if (result == null) { - return placeholderVertex(); - } - - return makeVertex(result, eleTs.getReferencePoint()); - } - - /** - * 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 list - * index-for-index against another collection (e.g. the Billoir unconstrained V0 fit, - * for comparison) never see the two lists fall out of sync. - */ - public static BilliorVertex placeholderVertex() { - Map pFitMap = new HashMap(); - pFitMap.put(0, new BasicHep3Vector(0, 0, 0)); - pFitMap.put(1, new BasicHep3Vector(0, 0, 0)); - BilliorVertex vtxFit = new BilliorVertex(new BasicHep3Vector(0, 0, 0), - new SymmetricMatrix(3), -9999.0, -9999.0, pFitMap, "KALMAN_UNCONSTRAINED_FAILED"); - vtxFit.setPositionError(new BasicHep3Vector(0, 0, 0)); - vtxFit.setParameter("ndf", -9999.0); - return vtxFit; - } - - /** - * Extract perigee TrackParams (d0, phi0, omega, z0, tanLambda) directly from an - * AtPerigee TrackState, with no reparametrization. {@link - * KalmanVertexFitterGainMatrix}'s track-constraint math implicitly treats the vertex - * unknown as living in the same frame as the raw d0/phi0/omega/z0, so the fitted - * vertex it returns is relative to the track's reference point, not the absolute - * tracking/detector frame; {@link #makeVertex} adds that reference point back in - * afterward. Electron and positron tracks are assumed to share the same reference - * point (true for AtPerigee states as produced by the tracking reconstruction). - */ - private 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); - } - - /** - * Convert a Kalman {@code FitResult} for the (electron, positron) track pair into a - * {@code BilliorVertex}, using the same tracking-frame -> detector-frame convention - * (det(x,y,z) = trk(y,z,x)) as {@code CascadeVertexer#makeReconstructedParticle}. - * - * @param referencePoint the (shared) reference point of the input tracks, tracking - * frame; added back to {@code result.vertex} since the fitter's - * raw output is relative to that point, not the absolute frame - */ - private static BilliorVertex makeVertex(FitResult result, double[] referencePoint) { - Hep3Vector vtxPos = new BasicHep3Vector( - result.vertex.getEntry(1) + referencePoint[1], - result.vertex.getEntry(2) + referencePoint[2], - result.vertex.getEntry(0) + referencePoint[0]); - - double[] covPacked = new double[6]; - covPacked[0] = result.vertexCov.getEntry(1, 1); - covPacked[1] = result.vertexCov.getEntry(2, 1); - covPacked[2] = result.vertexCov.getEntry(2, 2); - covPacked[3] = result.vertexCov.getEntry(0, 1); - covPacked[4] = result.vertexCov.getEntry(0, 2); - covPacked[5] = result.vertexCov.getEntry(0, 0); - SymmetricMatrix covVtx = new SymmetricMatrix(3, covPacked, true); - - Hep3Vector vtxPosErr = new BasicHep3Vector( - FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(1, 1))), - FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(2, 2))), - FastMath.sqrt(FastMath.abs(result.vertexCov.getEntry(0, 0)))); - - RealVector pEleTrk = result.trackMomenta.get(0).p; - Hep3Vector pEle = new BasicHep3Vector(pEleTrk.getEntry(1), pEleTrk.getEntry(2), pEleTrk.getEntry(0)); - RealVector pPosTrk = result.trackMomenta.get(1).p; - Hep3Vector pPos = new BasicHep3Vector(pPosTrk.getEntry(1), pPosTrk.getEntry(2), pPosTrk.getEntry(0)); - - double eEle = FastMath.sqrt(pEle.magnitudeSquared() + ELECTRON_MASS * ELECTRON_MASS); - double ePos = FastMath.sqrt(pPos.magnitudeSquared() + ELECTRON_MASS * ELECTRON_MASS); - double totalE = eEle + ePos; - Hep3Vector totalP = VecOp.add(pEle, pPos); - double massSq = totalE * totalE - totalP.magnitudeSquared(); - double invMass = massSq > 0 ? FastMath.sqrt(massSq) : -99.0; - - Map pFitMap = new HashMap(); - pFitMap.put(0, pEle); - pFitMap.put(1, pPos); - - BilliorVertex vtxFit = new BilliorVertex(vtxPos, covVtx, result.chi2, invMass, pFitMap, "KALMAN_UNCONSTRAINED"); - vtxFit.setPositionError(vtxPosErr); - vtxFit.setProbability(result.ndf); - vtxFit.setParameter("ndf", (double) result.ndf); - return vtxFit; - } -} 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/ThreeTrackVertexer.java b/recon/src/main/java/org/hps/recon/vertexing/ThreeTrackVertexer.java deleted file mode 100644 index 7a7302f210..0000000000 --- a/recon/src/main/java/org/hps/recon/vertexing/ThreeTrackVertexer.java +++ /dev/null @@ -1,400 +0,0 @@ -package org.hps.recon.vertexing; - -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.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.KalmanVertexFitterGainMatrix.LineParams; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.LinePlaneProjection; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TrackParams; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.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 KalmanVertexFitterGainMatrix#fitCascadeVertexJoint} and - * packages the result as a 2-daughter cascade ReconstructedParticle (inner V0 particle at - * V1, recoil electron) nested exactly like {@link CascadeVertexer}'s output, so existing - * consumers of that output shape (e.g. {@code CascadeVertexTupleDriver}) work unchanged. - */ -public class ThreeTrackVertexer { - - private static final double ELECTRON_MASS = 0.000511; - - private final double bField; - - // 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; - - public ThreeTrackVertexer(double bField) { - this.bField = bField; - } - - public void setFixV2BeamCoordinate(boolean fixV2BeamCoordinate) { - this.fixV2BeamCoordinate = fixV2BeamCoordinate; - } - - /** - * 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 - * @param truthMatched whether this (v0Particle, recoilElectron) combination is the - * MC-truth-matched signal pairing; gates the BADFIT_DEBUG print - * below so it isn't flooded by wrong-pairing combinatorics that - * are expected to fit badly. Callers without truth info should - * pass {@code true} to preserve the original always-print behavior. - * @return the cascade ReconstructedParticle (V0 particle + recoil electron), or null - * if the fit fails - */ - public ReconstructedParticle fit(ReconstructedParticle v0Particle, ReconstructedParticle recoilElectron, boolean truthMatched) { - 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, same utility already - // used by CascadeVertexer). 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 = KalmanVertexFitterGainMatrix.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. - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(bField); - LinePlaneProjection v0Proj = KalmanVertexFitterGainMatrix.propagateLineToPlane( - v0Line, fitter.getBeamPosition()[0], 10.0); - LinePlaneProjection v2Proj = KalmanVertexFitterGainMatrix.propagateLineToPlane( - v0LineLocal, fitter.getBeamPosition()[0] - refPoint[0], 10.0); - - // 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 = KalmanVertexFitterGainMatrix.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 = KalmanVertexFitterGainMatrix.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, v2Proj.position, v2FixedX, v1Cov, v2Proj.cov) - : fitter.fitCascadeVertexJoint( - eleParams, posParams, recoilParams, v1Init, v2Proj.position, v1Cov, v2Proj.cov); - } 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, v2Proj.cov, 60, 1.0e-8) - : fitter.fitCascadeVertexJointForcedBranch( - eleParams, posParams, recoilParams, v1Init, theta, v2Forced, v1Cov, v2Proj.cov, 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; - } - RealVector refPointVec = MatrixUtils.createRealVector(refPoint); - result.v1 = result.v1.add(refPointVec); - result.v2 = result.v2.add(refPointVec); - if (truthMatched && result.chi2 / result.ndf > 1000) { - System.out.println("BADFIT_DEBUG bField=" + bField); - System.out.println("BADFIT_DEBUG v1Init=" + v1Init); - System.out.println("BADFIT_DEBUG v1Cov=" + v1Cov); - System.out.println("BADFIT_DEBUG v2Init=" + v2Proj.position); - System.out.println("BADFIT_DEBUG v2Cov=" + v2Proj.cov); - System.out.println("BADFIT_DEBUG eleParams=" + dumpTrackParams(eleParams)); - System.out.println("BADFIT_DEBUG posParams=" + dumpTrackParams(posParams)); - System.out.println("BADFIT_DEBUG recoilParams=" + dumpTrackParams(recoilParams)); - System.out.println("BADFIT_DEBUG chi2=" + result.chi2 + " ndf=" + result.ndf); - System.out.println("BADFIT_DEBUG v1=" + result.v1 + " v2=" + result.v2); - } - - return makeReconstructedParticle(result, eleDaughter, posDaughter, recoilElectron, v0Proj, recoilProj, v0Vertex); - } - - /** - * 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 String dumpTrackParams(TrackParams tp) { - double[] a = tp.toArray(); - StringBuilder sb = new StringBuilder(); - sb.append("d0=").append(a[0]).append(" phi0=").append(a[1]).append(" omega=").append(a[2]) - .append(" z0=").append(a[3]).append(" tanLambda=").append(a[4]).append(" cov=["); - for (int i = 0; i < 5; i++) { - for (int j = 0; j < 5; j++) { - sb.append(tp.cov.getEntry(i, j)); - if (j < 4) { - sb.append(","); - } - } - if (i < 4) { - sb.append(";"); - } - } - sb.append("]"); - return sb.toString(); - } - - /** - * Extract perigee TrackParams (d0, phi0, omega, z0, tanLambda) from a Track's - * AtPerigee TrackState. Duplicated from {@link CascadeVertexer}'s private helper of - * the same name rather than shared, matching that class's own convention. - */ - private static TrackParams trackParamsFromTrack(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)); - } - } - return new TrackParams(par[0], par[1], par[2], par[3], par[4], 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 KalmanVertexFitterGainMatrix.fitVertex(...) and CascadeVertexer. - 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); - - 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, identical nesting - // to CascadeVertexer's output. - 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); - - // 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/KalmanVertexFitterGainMatrix.java b/recon/src/main/java/org/hps/recon/vertexing/TrackConstraintVertexFitter.java similarity index 74% rename from recon/src/main/java/org/hps/recon/vertexing/KalmanVertexFitterGainMatrix.java rename to recon/src/main/java/org/hps/recon/vertexing/TrackConstraintVertexFitter.java index 72d9c377ee..1ac1f38e8b 100644 --- a/recon/src/main/java/org/hps/recon/vertexing/KalmanVertexFitterGainMatrix.java +++ b/recon/src/main/java/org/hps/recon/vertexing/TrackConstraintVertexFitter.java @@ -16,7 +16,7 @@ * Kalman Filter vertex fitter using Gain Matrix formalism * Follows the approach of Frühwirth and Billoir for vertex fitting */ -public class KalmanVertexFitterGainMatrix { +public class TrackConstraintVertexFitter { private double bField; private RealVector vertex; @@ -156,7 +156,7 @@ public FitResult(RealVector vertex, RealMatrix vertexCov, double chi2, /** * Constraint representation: c = constraint residual, H = constraint matrix, V = covariance */ - private static class Constraint { + static class Constraint { RealVector c; RealMatrix H; RealMatrix V; @@ -168,18 +168,18 @@ private static class Constraint { } } - public KalmanVertexFitterGainMatrix(double bField) { + public TrackConstraintVertexFitter(double bField) { this.bField = bField; } - public KalmanVertexFitterGainMatrix() { + public TrackConstraintVertexFitter() { this(2.0); } /** * Convert perigee to vertex parameters */ - private static class VertexParams { + static class VertexParams { double phiV, zV; VertexParams(double phiV, double zV) { this.phiV = phiV; @@ -187,7 +187,7 @@ private static class VertexParams { } } - private VertexParams perigeeToVertexParams(TrackParams track, double xV, double yV) { + VertexParams perigeeToVertexParams(TrackParams track, double xV, double yV) { double R = 1.0 / FastMath.abs(track.omega); double sign = FastMath.signum(track.omega); @@ -217,7 +217,7 @@ private VertexParams perigeeToVertexParams(TrackParams track, double xV, double * Compute track constraint using Gain Matrix formalism * The track provides a constraint on where the vertex should be */ - private Constraint computeTrackConstraint(TrackParams track, RealVector vertex) { + Constraint computeTrackConstraint(TrackParams track, RealVector vertex) { double xV = vertex.getEntry(0); double yV = vertex.getEntry(1); double zV = vertex.getEntry(2); @@ -309,10 +309,60 @@ private RealMatrix propagateTrackCovariance(TrackParams track, double xV, double 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 @@ -402,127 +452,6 @@ private Constraint computeLineConstraint(LineParams line, RealVector vertex) { return new Constraint(c, H, V); } - /** - * Compute vertex position constraint (beamspot) - */ - 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) { - // Calculate total momentum and total momentum covariance from tracks - 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 = computeMomentumAtVertex(track, vertex); - totalP = totalP.add(p); - - RealMatrix dpDv = computeMomentumVertexDerivatives(track, vertex); - dpDvertex = dpDvertex.add(dpDv); - - // Add track momentum covariance (propagated from track parameter errors) - RealMatrix pCov = computeMomentumCovariance(track, vertex); - totalPCov = totalPCov.add(pCov); - } - - RealVector c = totalP.subtract(momentumConstraint); - RealMatrix H = dpDvertex; - // Total covariance = beam momentum uncertainty + sum of track momentum uncertainties - RealMatrix V = momentumConstraintCov.add(totalPCov); - - return new Constraint(c, H, V); - } - - /** - * Compute mass constraint - * Invariant mass: M² = (ΣE)² - (Σp)² - * - * @param tracks List of track parameters - * @param vertex Vertex position - * @param massConstraint Constrained mass value (GeV/c²) - * @param massConstraintSigma Uncertainty on mass (GeV/c²) - * @return Constraint object - */ - private Constraint computeMassConstraint(List tracks, - RealVector vertex, - double massConstraint, - double massConstraintSigma) { - // Assume pion mass for all tracks (can be extended) - double mPi = 0.13957; // GeV/c² - - 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 = computeMomentumAtVertex(track, vertex); - double pMag = p.getNorm(); - - // Energy (assuming pion mass) - double E = FastMath.sqrt(pMag * pMag + mPi * mPi); - totalE += E; - totalP = totalP.add(p); - - // Derivatives of E w.r.t. vertex - // E = sqrt(p² + m²), so dE/dvertex = (p · dp/dvertex) / E - RealMatrix dpDv = 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); - } - - // Invariant mass - double totalPmag = totalP.getNorm(); - double M = FastMath.sqrt(totalE * totalE - totalPmag * totalPmag); - - // Constraint residual - RealVector c = MatrixUtils.createRealVector(new double[]{M - massConstraint}); - - // Derivatives of M w.r.t. vertex - // M² = E² - p², so 2M dM = 2E dE - 2p·dp - // dM/dvertex = (E * dE/dvertex - p · dp/dvertex) / M - 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); - - // Covariance (1x1 matrix) - RealMatrix V = MatrixUtils.createRealMatrix(1, 1); - V.setEntry(0, 0, massConstraintSigma * massConstraintSigma); - - return new Constraint(c, H, V); - } - /** * Compute momentum at vertex */ @@ -543,7 +472,7 @@ RealVector computeMomentumAtVertex(TrackParams track, RealVector vertex) { /** * Compute d(momentum)/d(vertex) */ - private RealMatrix computeMomentumVertexDerivatives(TrackParams track, RealVector vertex) { + RealMatrix computeMomentumVertexDerivatives(TrackParams track, RealVector vertex) { double xV = vertex.getEntry(0); double yV = vertex.getEntry(1); @@ -580,7 +509,7 @@ private RealMatrix computeMomentumVertexDerivatives(TrackParams track, RealVecto /** * Compute momentum covariance */ - private RealMatrix computeMomentumCovariance(TrackParams track, RealVector vertex) { + RealMatrix computeMomentumCovariance(TrackParams track, RealVector vertex) { RealMatrix Jp = computeMomentumTrackJacobian(track, vertex); return Jp.multiply(track.cov).multiply(Jp.transpose()); } @@ -606,7 +535,7 @@ public TrackMomentum computeRawMomentum(TrackParams track) { * to build the full state-vector Jacobian needed for a correctly-correlated TOTAL momentum * covariance (see the total-momentum computation in {@link #fitSoftConstrained}). */ - private RealMatrix computeMomentumTrackJacobian(TrackParams track, RealVector vertex) { + RealMatrix computeMomentumTrackJacobian(TrackParams track, RealVector vertex) { double xV = vertex.getEntry(0); double yV = vertex.getEntry(1); @@ -646,307 +575,36 @@ private RealMatrix computeMomentumTrackJacobian(TrackParams track, RealVector ve return Jp; } - - /** - * Fit vertex using Gain Matrix formalism - */ - 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); - - // Gain matrix: K = C * H^T * (H * C * H^T + V)^-1 - 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() - ); - - // Update: vertex = vertex - K * c - vertex = vertex.subtract(K.operate(constraint.c)); - - // Joseph-form covariance update: numerically stable under re-linearization - // across iterations, unlike the simple (I-KH)*C form which can - // systematically underestimate 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 = 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 = 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 - } - } - - // Print chi2 contributions - 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 = computeMomentumAtVertex(track, vertex); - RealMatrix pCov = computeMomentumCovariance(track, vertex); - trackMomenta.add(new TrackMomentum(p, pCov)); - } - - this.vertex = vertex; - this.vertexCov = C; - this.chi2 = chi2; - this.ndf = ndf; - this.trackMomenta = trackMomenta; - - return new FitResult(vertex, C, chi2, ndf, trackMomenta); - } /** - * Fit a "cascade" (production) vertex where a curving recoil track meets an - * already-fitted neutral V0's momentum direction, treated as a zero-curvature - * line. Same sequential Kalman gain-matrix update as {@link #fit}, specialized to - * exactly one track constraint and one line constraint (no vertex/momentum/mass - * constraints). NDF = (2 track-constraint dof + 2 line-constraint dof) - 3 (vertex - * position dof) = 1. + * 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. */ - public FitResult fitCascadeVertex(TrackParams recoilTrack, LineParams v0Line, - RealVector initialVertex, int maxIterations, double tolerance) { - RealVector vertex = (initialVertex != null) - ? initialVertex.copy() - : MatrixUtils.createRealVector(new double[]{v0Line.x0, v0Line.y0, v0Line.z0}); - - RealMatrix priorC = MatrixUtils.createRealIdentityMatrix(3).scalarMultiply(100.0); - RealMatrix I = MatrixUtils.createRealIdentityMatrix(3); - RealMatrix C = priorC; - - for (int iteration = 0; iteration < maxIterations; iteration++) { - RealVector vertexOld = vertex.copy(); - // Reset to the vague prior each iteration: the track and line constraints are - // the same two 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) -- exactly the - // ~20x variance underestimate (~4.5x pull-std inflation) seen before this fix. - C = priorC; - - Constraint trackConstraint = computeTrackConstraint(recoilTrack, vertex); - RealMatrix St = trackConstraint.H.multiply(C).multiply(trackConstraint.H.transpose()).add(trackConstraint.V); - RealMatrix Kt = C.multiply(trackConstraint.H.transpose()).multiply( - new LUDecomposition(St).getSolver().getInverse()); - vertex = vertex.subtract(Kt.operate(trackConstraint.c)); - // Joseph-form covariance update: numerically stable under re-linearization across - // iterations, unlike the simple (I-KH)*C form which can systematically underestimate C. - RealMatrix ImKHt = I.subtract(Kt.multiply(trackConstraint.H)); - C = ImKHt.multiply(C).multiply(ImKHt.transpose()) - .add(Kt.multiply(trackConstraint.V).multiply(Kt.transpose())); - - Constraint lineConstraint = computeLineConstraint(v0Line, vertex); - RealMatrix Sl = lineConstraint.H.multiply(C).multiply(lineConstraint.H.transpose()).add(lineConstraint.V); - RealMatrix Kl = C.multiply(lineConstraint.H.transpose()).multiply( - new LUDecomposition(Sl).getSolver().getInverse()); - vertex = vertex.subtract(Kl.operate(lineConstraint.c)); - RealMatrix ImKHl = I.subtract(Kl.multiply(lineConstraint.H)); - C = ImKHl.multiply(C).multiply(ImKHl.transpose()) - .add(Kl.multiply(lineConstraint.V).multiply(Kl.transpose())); - - if (vertex.subtract(vertexOld).getNorm() < tolerance) { - break; - } - } - - Constraint trackConstraint = computeTrackConstraint(recoilTrack, vertex); - RealMatrix VInvT = new LUDecomposition(trackConstraint.V).getSolver().getInverse(); - double chi2Track = trackConstraint.c.dotProduct(VInvT.operate(trackConstraint.c)); - - Constraint lineConstraint = computeLineConstraint(v0Line, vertex); - RealMatrix VInvL = new LUDecomposition(lineConstraint.V).getSolver().getInverse(); - double chi2Line = lineConstraint.c.dotProduct(VInvL.operate(lineConstraint.c)); - - double chi2 = chi2Track + chi2Line; - int ndf = 1; - - List trackMomenta = new ArrayList<>(); - RealVector p = computeMomentumAtVertex(recoilTrack, vertex); - RealMatrix pCov = computeMomentumCovariance(recoilTrack, vertex); - trackMomenta.add(new TrackMomentum(p, pCov)); + 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); - this.vertex = vertex; - this.vertexCov = C; - this.chi2 = chi2; - this.ndf = ndf; - this.trackMomenta = trackMomenta; + 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 new FitResult(vertex, C, chi2, ndf, trackMomenta); + return J.multiply(cFull).multiply(J.transpose()); } - - public FitResult fitCascadeVertex(TrackParams recoilTrack, LineParams v0Line) { - return fitCascadeVertex(recoilTrack, v0Line, null, 20, 1.0e-8); - } - + /** * 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 @@ -984,6 +642,18 @@ public TwoVertexFitResult(RealVector v1, RealMatrix v1Cov, RealVector v2, RealMa 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 @@ -1101,7 +771,7 @@ public TwoVertexFitResult fitCascadeVertexJoint(TrackParams eMinusIn, TrackParam * 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}/{@link CascadeVertexer}) -- there is no + * 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 @@ -1134,7 +804,7 @@ public TwoVertexFitResult fitCascadeVertexJoint(TrackParams eMinusIn, TrackParam 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 ThreeTrackVertexer); V1 -- + // 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); @@ -1182,10 +852,10 @@ public TwoVertexFitResult fitCascadeVertexJoint(TrackParams eMinusIn, TrackParam * {@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.ThreeTrackVertexer#fit}, which + * 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.ThreeTrackVertexer}. + * {@link org.hps.recon.vertexing.CascadeVertexer}. */ TwoVertexFitResult fitCascadeVertexJointForcedBranch(TrackParams eMinusIn, TrackParams ePlusIn, TrackParams recoilIn, RealVector v1Init, double thetaInit, RealVector v2InitVec, RealMatrix v1CovIn, RealMatrix v2CovIn, @@ -1197,6 +867,51 @@ TwoVertexFitResult fitCascadeVertexJointForcedBranch(TrackParams eMinusIn, Track 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) { @@ -1244,115 +959,716 @@ private TwoVertexFitResult fitCascadeVertexJointCore(TrackParams eMinus, TrackPa // 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(); + 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 v1 = y.getSubVector(TVJ_OFF_V1, 3); - double theta = y.getEntry(TVJ_OFF_THETA); - RealVector v2 = y.getSubVector(TVJ_OFF_V2, 3); + 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); - GeometricConstraint gc = computeGeometricConstraint(v1, theta, v2, eMinus, ePlus); + 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); - 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); + 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 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); + 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); + } - // 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); + finalH = H; + finalV = constraintCov; + finalHvec = h; - SequentialUpdate u = sequentialUpdate(y, C, cStack, HStack, VStack); + 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)); - // 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(); + double stepNorm = deltaX.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()); + deltaX = deltaX.mapMultiply(MAX_STEP_NORM / stepNorm); } + x = x.add(deltaX); - if (!Double.isFinite(y.getNorm())) { - y = yOld; + if (!Double.isFinite(x.getNorm())) { + x = xOld; break; } - if (y.subtract(yOld).getNorm() < tolerance) { + if (deltaX.getNorm() < tolerance && h.getNorm() < tolerance * 10) { break; } } catch (Exception e) { - if (DEBUG_JOINT_FIT) { - System.err.println("JFDEBUG it=" + iteration + " EXCEPTION " + e); - } - y = yOld; + x = xOld; 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); + 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()); - // 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); + 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 + } + } - 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); + 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); - int ndf = 2; + 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); - 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)); + 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); @@ -1424,7 +1740,7 @@ public TwoVertexFitResult fitCascadeVertexJointFixedV2X(TrackParams eMinusIn, Tr * 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 ThreeTrackVertexer#fit} to fit both {@link #transverseCircleRoots} candidates + * {@link CascadeVertexer#fit} to fit both {@link #transverseCircleRoots} candidates * and choose between them externally. Package-private, not part of the general-purpose * API. */ @@ -1968,7 +2284,17 @@ public FitResult fitSoftConstrained(List inputTracks, // Vertex part if (vertexConstraintCov != null) { - RealMatrix vertexCovInv = new LUDecomposition(vertexConstraintCov).getSolver().getInverse(); + // 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)); @@ -2226,7 +2552,19 @@ public FitResult fitSoftConstrained(List inputTracks, diagScale += HWInvHTforEps.getEntry(i, i); } diagScale = (nConstraints > 0) ? diagScale / nConstraints : 1.0; - double epsilon = (diagScale > 0) ? diagScale * 1e-6 : 1e-12; + // 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); @@ -2479,7 +2817,7 @@ public FitResult fitSoftConstrained(List inputTracks, * 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 KalmanNTrackVertexer#fitVertexBeamConstrained} with + * {@link NTrackVertexer#fitVertexBeamConstrained} with * {@code hardMomentumConstraint=false}) with a tuned * {@code setBeamMomentumTransverseNuclearRecoilSigma} instead. Kept for * reference/regression comparison, not recommended for new production use. @@ -2500,15 +2838,6 @@ public FitResult fitLagrangeMultiplier(List inputTracks, beamMomentum, null, maxIterations, tolerance); } - // Simplified methods - 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); - } - // Configurable fields for BilliorVertexer-style interface private double[] beamSize = {0.001, 0.150, 0.050}; private double[] beamPosition = {-1.1, 0, 0}; @@ -2537,6 +2866,45 @@ public FitResult fit(List tracks, int maxIterations, double toleran 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). @@ -2920,31 +3288,21 @@ public BilliorVertex fitVertex(List tracks, boolean beamspotConstra RealVector fourMomentumConstraintVec = null; RealMatrix fourMomentumConstraintCovMat = null; if (beamMomentumConstraint) { - // Beam momentum vector in tracking frame - // Detector frame: beam along HPS Z, rotated by rotAngle in HPS X-Z plane - // Tracking frame: X=HPS_Z, Y=HPS_X, Z=HPS_Y - 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 + // 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[]{pxBeam, pyBeam, pzBeam, eBeam}); + fourMomentumConstraintVec = MatrixUtils.createRealVector(new double[]{ + pBeamVec.getEntry(0), pBeamVec.getEntry(1), pBeamVec.getEntry(2), eBeam}); + RealMatrix pCovMat = 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; - fourMomentumConstraintCovMat = MatrixUtils.createRealMatrix(4, 4); - fourMomentumConstraintCovMat.setEntry(0, 0, sL2 * cosR * cosR + sT2 * sinR * sinR); - fourMomentumConstraintCovMat.setEntry(0, 1, (sT2 - sL2) * sinR * cosR); - fourMomentumConstraintCovMat.setEntry(1, 0, (sT2 - sL2) * sinR * cosR); - fourMomentumConstraintCovMat.setEntry(1, 1, sL2 * sinR * sinR + sT2 * cosR * cosR); - fourMomentumConstraintCovMat.setEntry(2, 2, sT2); double sigmaE = dpOverP * eBeam; + fourMomentumConstraintCovMat = MatrixUtils.createRealMatrix(4, 4); + fourMomentumConstraintCovMat.setSubMatrix(pCovMat.getData(), 0, 0); fourMomentumConstraintCovMat.setEntry(3, 3, sigmaE * sigmaE); } @@ -3119,7 +3477,7 @@ public BilliorVertex fitVertex(List tracks, boolean beamspotConstra } if (debugFlag) { - System.out.println("=== KalmanVertexFitterGainMatrix::fitVertex ==="); + System.out.println("=== TrackConstraintVertexFitter::fitVertex ==="); System.out.println(" B field: " + bField); System.out.println(" Beamspot constraint: " + beamspotConstraint); System.out.println(" Beam momentum constraint: " + beamMomentumConstraint); @@ -3168,7 +3526,7 @@ public BilliorVertex fitVertex(List tracks, boolean beamspotConstra 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 KalmanVertexFitterGainMatrix::fitVertex ==="); + System.out.println("=== End TrackConstraintVertexFitter::fitVertex ==="); } return bv; @@ -3181,375 +3539,4 @@ public BilliorVertex fitVertex(List tracks, boolean beamspotConstra public int getNdf() { return ndf; } public List getTrackMomenta() { return trackMomenta; } - /** - * Fit vertex using N-1 tracks and predict the momentum of the Nth track - * - * This method uses vertex and/or total momentum constraints to fit with a subset - * of tracks, then predicts the momentum of the excluded track. - * - * @param tracks All track parameters including the one to predict - * @param predictTrackIdx Index of track to predict (0-based) - * @param vertexConstraint Known vertex position - * @param vertexConstraintCov Vertex constraint covariance - * @param totalMomentumConstraint Known total momentum of ALL tracks - * @param totalMomentumConstraintCov Total momentum constraint covariance - * @param massConstraint Invariant mass constraint for ALL tracks - * @param massConstraintSigma Mass constraint uncertainty - * @param maxIterations Maximum iterations - * @param tolerance Convergence tolerance - * @return PredictedTrackResult containing vertex, predicted momentum, and all track momenta - */ - public PredictedTrackResult fitWithPredictedTrack( - List tracks, - int predictTrackIdx, - RealVector vertexConstraint, - RealMatrix vertexConstraintCov, - RealVector totalMomentumConstraint, - RealMatrix totalMomentumConstraintCov, - Double massConstraint, - Double massConstraintSigma, - int maxIterations, - double tolerance) { - - int nTracks = tracks.size(); - - if (predictTrackIdx < 0 || predictTrackIdx >= nTracks) { - throw new IllegalArgumentException( - "predictTrackIdx must be between 0 and " + (nTracks - 1)); - } - - // Fit all N tracks to get the best vertex - do NOT use momentum constraint here; - // we use momentum conservation to PREDICT the excluded track's momentum - FitResult fitResult = fit( - tracks, null, vertexConstraint, vertexConstraintCov, - null, null, // No momentum constraint for N-track fit - null, null, // No mass constraint either - maxIterations, tolerance - ); - - return fitWithPredictedTrack(fitResult, predictTrackIdx, - totalMomentumConstraint, totalMomentumConstraintCov, - massConstraint, massConstraintSigma); - } - - /** - * Predict the momentum of one track using momentum conservation, given a pre-computed - * vertex fit result. The vertex fit must already include all tracks. - * Use this overload to avoid recomputing the vertex fit for each predicted track. - * - * @param precomputedFit Result of a prior fit to all N tracks - * @param predictTrackIdx Index of the track whose momentum is to be predicted - * @param totalMomentumConstraint Known total momentum (beam momentum) - * @param totalMomentumConstraintCov Covariance on total momentum - * @param massConstraint Invariant mass constraint (null to skip) - * @param massConstraintSigma Mass constraint uncertainty (null to skip) - * @return PredictedTrackResult containing vertex, predicted momentum, and all track momenta - */ - public PredictedTrackResult fitWithPredictedTrack( - FitResult precomputedFit, - int predictTrackIdx, - RealVector totalMomentumConstraint, - RealMatrix totalMomentumConstraintCov, - Double massConstraint, - Double massConstraintSigma) { - - int nTracks = precomputedFit.trackMomenta.size(); - - if (predictTrackIdx < 0 || predictTrackIdx >= nTracks) { - throw new IllegalArgumentException( - "predictTrackIdx must be between 0 and " + (nTracks - 1)); - } - - FitResult fitResult = precomputedFit; - - // Predict momentum of excluded track - RealVector predictedP; - RealMatrix predictedPCov; - - if (totalMomentumConstraint != null) { - // Use momentum conservation: p_predicted = p_total - sum(p_fitted for other tracks) - RealVector fittedTotalP = MatrixUtils.createRealVector(new double[3]); - RealMatrix fittedPCov = MatrixUtils.createRealMatrix(3, 3); - for (int i = 0; i < nTracks; i++) { - if (i == predictTrackIdx) continue; - TrackMomentum mom = fitResult.trackMomenta.get(i); - fittedTotalP = fittedTotalP.add(mom.p); - fittedPCov = fittedPCov.add(mom.pCov); - if(debugFlag) - System.out.printf("DEBUG fitWithPredictedTrack: fitted track %d p (tracking) = [%.4f, %.4f, %.4f] |p|=%.4f%n", - i, mom.p.getEntry(0), mom.p.getEntry(1), mom.p.getEntry(2), mom.p.getNorm()); - } - predictedP = totalMomentumConstraint.subtract(fittedTotalP); - if(debugFlag){ - System.out.printf("DEBUG fitWithPredictedTrack: total fitted p (tracking) = [%.4f, %.4f, %.4f] |p|=%.4f%n", - fittedTotalP.getEntry(0), fittedTotalP.getEntry(1), fittedTotalP.getEntry(2), fittedTotalP.getNorm()); - System.out.printf("DEBUG fitWithPredictedTrack: momentum constraint (tracking) = [%.4f, %.4f, %.4f] |p|=%.4f%n", - totalMomentumConstraint.getEntry(0), totalMomentumConstraint.getEntry(1), totalMomentumConstraint.getEntry(2), - totalMomentumConstraint.getNorm()); - System.out.printf("DEBUG fitWithPredictedTrack: predicted p (tracking) = [%.4f, %.4f, %.4f] |p|=%.4f%n", - predictedP.getEntry(0), predictedP.getEntry(1), predictedP.getEntry(2), predictedP.getNorm()); - } - // Propagate uncertainty: Cov(p_pred) = Cov(p_total) + Cov(sum p_fitted for other tracks) - // Note: This assumes uncorrelated track momenta (ignores vertex correlations) - - if (totalMomentumConstraintCov != null) { - predictedPCov = totalMomentumConstraintCov.add(fittedPCov); - } else { - predictedPCov = fittedPCov; - } - - } else { - // Without momentum constraint, use the fitted momentum of the predicted track - predictedP = fitResult.trackMomenta.get(predictTrackIdx).p; - predictedPCov = fitResult.trackMomenta.get(predictTrackIdx).pCov; - } - - // Create momentum info for predicted track - TrackMomentum predictedMom = new TrackMomentum(predictedP, predictedPCov); - - // All track momenta in original order; replace predicted track entry with - // the conservation-predicted momentum (actual fitted momentum stored in actualP) - List allTrackMomenta = new ArrayList<>(fitResult.trackMomenta); - allTrackMomenta.set(predictTrackIdx, predictedMom); - - // Calculate total chi2: - // Start with chi2 from fitting all N tracks - double chi2Total = fitResult.chi2; - if(debugFlag) - System.out.printf("DEBUG fitWithPredictedTrack: N-track fit chi2=%.2f ndf=%d%n", fitResult.chi2, fitResult.ndf); - - // Add chi2 contribution from comparing predicted momentum to actual fitted track momentum - RealVector actualP = fitResult.trackMomenta.get(predictTrackIdx).p; - RealMatrix actualPCov = fitResult.trackMomenta.get(predictTrackIdx).pCov; - - if(debugFlag) - System.out.printf("DEBUG fitWithPredictedTrack: actual p (tracking) = [%.4f, %.4f, %.4f] |p|=%.4f%n", - actualP.getEntry(0), actualP.getEntry(1), actualP.getEntry(2), actualP.getNorm()); - - // Residual: predicted - actual - RealVector pResidual = predictedP.subtract(actualP); - if(debugFlag) - System.out.printf("DEBUG fitWithPredictedTrack: residual (pred-act) = [%.4f, %.4f, %.4f]%n", - pResidual.getEntry(0), pResidual.getEntry(1), pResidual.getEntry(2)); - - // Combined covariance for the comparison - RealMatrix combinedCov = predictedPCov.add(actualPCov); - - // Chi2 contribution from momentum comparison - double chi2Momentum = 0; - try { - RealMatrix combinedCovInv = new LUDecomposition(combinedCov).getSolver().getInverse(); - chi2Momentum = pResidual.dotProduct(combinedCovInv.operate(pResidual)); - chi2Total += chi2Momentum; - if(debugFlag) - System.out.printf("DEBUG fitWithPredictedTrack: chi2 from momentum comparison = %.2f%n", chi2Momentum); - } catch (SingularMatrixException e) { - // Combined covariance is singular; skip this chi2 contribution silently - } - if(debugFlag) - System.out.printf("DEBUG fitWithPredictedTrack: total chi2 = %.2f%n", chi2Total); - - // ndf: from N-track fit plus 3 for the 3-component momentum comparison - int ndfTotal = fitResult.ndf + 3; - - this.vertex = fitResult.vertex; - this.vertexCov = fitResult.vertexCov; - this.chi2 = chi2Total; - this.ndf = ndfTotal; - this.trackMomenta = allTrackMomenta; - - return new PredictedTrackResult( - fitResult.vertex, fitResult.vertexCov, - predictedP, predictedPCov, - actualP, actualPCov, - pResidual, - chi2Total, ndfTotal, allTrackMomenta - ); - } - - /** - * Convert a PredictedTrackResult to a BilliorVertex - * - * @param result The predicted track result - * @param predictedTrackIdx Index of the predicted track (0=ele, 1=pos, 2=rec) - * @param label Label for the vertex type - * @return BilliorVertex containing the fit results - */ - public BilliorVertex predictedResultToBilliorVertex(PredictedTrackResult result, int predictedTrackIdx, String label) { - // Tracking frame to detector frame: HPS X = TRACK Y, HPS Y = TRACK Z, HPS Z = TRACK X - double vtxX = result.vertex.getEntry(1); // tracking Y -> HPS X - double vtxY = result.vertex.getEntry(2); // tracking Z -> HPS Y - double vtxZ = result.vertex.getEntry(0); // tracking X -> HPS Z - hep.physics.vec.Hep3Vector vtxPos = new hep.physics.vec.BasicHep3Vector(vtxX, vtxY, vtxZ); - - // Convert covariance matrix (tracking -> detector frame) - double[] covPacked = new double[6]; - 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.allTrackMomenta.size(); i++) { - RealVector p = result.allTrackMomenta.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.setProbability(result.ndf); - bv.setParameter("ndf", (double) result.ndf); - bv.setPositionError(vtxPosErr); - - // Store predicted momentum (converted to detector frame) - // Tracking frame: (px_trk, py_trk, pz_trk) -> Detector frame: (py_trk, pz_trk, px_trk) - double predPx = result.predictedMomentum.getEntry(1); // trk Y -> det X - double predPy = result.predictedMomentum.getEntry(2); // trk Z -> det Y - double predPz = result.predictedMomentum.getEntry(0); // trk X -> det Z - double predPtot = Math.sqrt(predPx*predPx + predPy*predPy + predPz*predPz); - if(debugFlag){ - System.out.printf("DEBUG toBilliorVertex: predicted p (tracking) = [%.4f, %.4f, %.4f]%n", - result.predictedMomentum.getEntry(0), result.predictedMomentum.getEntry(1), result.predictedMomentum.getEntry(2)); - System.out.printf("DEBUG toBilliorVertex: predicted p (detector) = [%.4f, %.4f, %.4f] |p|=%.4f%n", - predPx, predPy, predPz, predPtot); - } - bv.setParameter("predictedPx", predPx); - bv.setParameter("predictedPy", predPy); - bv.setParameter("predictedPz", predPz); - - // Store actual momentum (converted to detector frame) - double actPx = result.actualMomentum.getEntry(1); - double actPy = result.actualMomentum.getEntry(2); - double actPz = result.actualMomentum.getEntry(0); - double actPtot = Math.sqrt(actPx*actPx + actPy*actPy + actPz*actPz); - if(debugFlag){ - System.out.printf("DEBUG toBilliorVertex: actual p (tracking) = [%.4f, %.4f, %.4f]%n", - result.actualMomentum.getEntry(0), result.actualMomentum.getEntry(1), result.actualMomentum.getEntry(2)); - System.out.printf("DEBUG toBilliorVertex: actual p (detector) = [%.4f, %.4f, %.4f] |p|=%.4f%n", - actPx, actPy, actPz, actPtot); - } - bv.setParameter("actualPx", actPx); - bv.setParameter("actualPy", actPy); - bv.setParameter("actualPz", actPz); - - // Store residuals (converted to detector frame) - double resPx = result.momentumResidual.getEntry(1); - double resPy = result.momentumResidual.getEntry(2); - double resPz = result.momentumResidual.getEntry(0); - bv.setParameter("residualPx", resPx); - bv.setParameter("residualPy", resPy); - bv.setParameter("residualPz", resPz); - - // Store predicted momentum covariance (converted to detector frame) - { - RealMatrix pCov = result.predictedMomentumCov; - int[] map = {1, 2, 0}; // detector index -> tracking index - bv.setParameter("predictedMomCovXX", pCov.getEntry(map[0], map[0])); - bv.setParameter("predictedMomCovXY", pCov.getEntry(map[0], map[1])); - bv.setParameter("predictedMomCovXZ", pCov.getEntry(map[0], map[2])); - bv.setParameter("predictedMomCovYY", pCov.getEntry(map[1], map[1])); - bv.setParameter("predictedMomCovYZ", pCov.getEntry(map[1], map[2])); - bv.setParameter("predictedMomCovZZ", pCov.getEntry(map[2], map[2])); - } - - // Store actual momentum covariance (converted to detector frame) - { - RealMatrix pCov = result.actualMomentumCov; - int[] map = {1, 2, 0}; // detector index -> tracking index - bv.setParameter("actualMomCovXX", pCov.getEntry(map[0], map[0])); - bv.setParameter("actualMomCovXY", pCov.getEntry(map[0], map[1])); - bv.setParameter("actualMomCovXZ", pCov.getEntry(map[0], map[2])); - bv.setParameter("actualMomCovYY", pCov.getEntry(map[1], map[1])); - bv.setParameter("actualMomCovYZ", pCov.getEntry(map[1], map[2])); - bv.setParameter("actualMomCovZZ", pCov.getEntry(map[2], map[2])); - } - - // Store which track was predicted - bv.setParameter("predictedTrackIdx", (double) predictedTrackIdx); - - // Store momentum covariances if requested - if (storeCovTrkMomList) { - java.util.List covTrkMomList = new java.util.ArrayList<>(); - for (int i = 0; i < result.allTrackMomenta.size(); i++) { - RealMatrix pCov = result.allTrackMomenta.get(i).pCov; - // Reorder tracking -> detector frame - 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); - } - - return bv; - } - - /** - * Result from fitting with predicted track - */ - public static class PredictedTrackResult { - public RealVector vertex; - public RealMatrix vertexCov; - public RealVector predictedMomentum; - public RealMatrix predictedMomentumCov; - public RealVector actualMomentum; // Measured momentum of excluded track - public RealMatrix actualMomentumCov; - public RealVector momentumResidual; // predicted - actual - public double chi2; - public int ndf; - public List allTrackMomenta; - - public PredictedTrackResult(RealVector vertex, RealMatrix vertexCov, - RealVector predictedMomentum, RealMatrix predictedMomentumCov, - RealVector actualMomentum, RealMatrix actualMomentumCov, - RealVector momentumResidual, - double chi2, int ndf, List allTrackMomenta) { - this.vertex = vertex; - this.vertexCov = vertexCov; - this.predictedMomentum = predictedMomentum; - this.predictedMomentumCov = predictedMomentumCov; - this.actualMomentum = actualMomentum; - this.actualMomentumCov = actualMomentumCov; - this.momentumResidual = momentumResidual; - this.chi2 = chi2; - this.ndf = ndf; - this.allTrackMomenta = allTrackMomenta; - } - } } 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/CascadeVertexerTest.java b/recon/src/test/java/org/hps/recon/vertexing/CascadeVertexerTest.java index 487730e0e2..2a60db489c 100644 --- a/recon/src/test/java/org/hps/recon/vertexing/CascadeVertexerTest.java +++ b/recon/src/test/java/org/hps/recon/vertexing/CascadeVertexerTest.java @@ -22,12 +22,12 @@ import org.lcsim.event.base.BaseTrackState; /** - * Exact-geometry sanity check for {@link CascadeVertexer}: build an already-fitted V0 - * BilliorVertex whose position exactly equals a known point (so its flight line trivially - * passes through that point regardless of direction) and a recoil-electron track whose helix - * is constructed to pass through the same point, with the recoil track's AtPerigee reference - * point set away from the origin (as in real reconstructed tracks) -- verifies the fitted - * production vertex correctly lands on that point in the absolute tracking/detector frame. + * 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 { @@ -35,41 +35,310 @@ public class CascadeVertexerTest extends TestCase { private static final double C = 2.99792458e-4; public void testFitExactGeometryNonOriginReferencePoint() { - // Target point, tracking frame (index0=detZ, index1=detX, index2=detY). - double xV = 5.0, yV = 0.3, zV = -0.2; double[] refPoint = {-1.1, 0.0, 0.0}; - Hep3Vector vtxPosDet = new BasicHep3Vector(yV, zV, xV); - Hep3Vector p1Det = new BasicHep3Vector(0.05, 0.02, 0.3); - Hep3Vector p2Det = new BasicHep3Vector(0.03, -0.01, 0.25); + // 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; - ReconstructedParticle v0Particle = makeV0Particle(vtxPosDet, p1Det, p2Det); + // 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}; - double pxRecoil = 0.15, pyRecoil = -0.05, pzRecoil = 0.8; - Track recoilTrack = makeTrackThroughPoint(xV, yV, zV, pxRecoil, pyRecoil, pzRecoil, -1, refPoint); + 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 candidate = vertexer.fit(v0Particle, recoilElectron); + ReconstructedParticle cascade = vertexer.fit(v0Particle, recoilElectron, true); + + assertNotNull("three-track fit should not be null", cascade); - assertNotNull("cascade fit should not be null", candidate); + BilliorVertex v2Vtx = (BilliorVertex) cascade.getStartVertex(); + Hep3Vector v2PosDet = v2Vtx.getPosition(); + ReconstructedParticle v0Out = cascade.getParticles().get(0); + BilliorVertex v1Vtx = (BilliorVertex) v0Out.getStartVertex(); + Hep3Vector v1PosDetFit = v1Vtx.getPosition(); - BilliorVertex vtx = (BilliorVertex) candidate.getStartVertex(); - Hep3Vector pos = vtx.getPosition(); - System.out.printf("Cascade 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()); + 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(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); + 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); } - private static ReconstructedParticle makeV0Particle(Hep3Vector vtxPosDet, Hep3Vector p1Det, Hep3Vector p2Det) { + /** + * 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, true); + 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, true); + 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, true); + 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, 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, true); + 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, 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, true); + 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); @@ -104,10 +373,20 @@ private static ReconstructedParticle makeV0Particle(Hep3Vector vtxPosDet, Hep3Ve 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; } @@ -121,7 +400,7 @@ private static ReconstructedParticle makeElectronParticle(Track track) { /** * 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 KalmanV0VertexerTest#makeTrackThroughPoint}. + * {@code NTrackVertexerTest#makeTrackThroughPoint}. */ private static Track makeTrackThroughPoint(double xV, double yV, double zV, double px, double py, double pz, int charge, double[] refPoint) { 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/KalmanV0VertexerTest.java b/recon/src/test/java/org/hps/recon/vertexing/KalmanV0VertexerTest.java deleted file mode 100644 index 1699ab9980..0000000000 --- a/recon/src/test/java/org/hps/recon/vertexing/KalmanV0VertexerTest.java +++ /dev/null @@ -1,166 +0,0 @@ -package org.hps.recon.vertexing; - -import hep.physics.matrix.SymmetricMatrix; -import hep.physics.vec.Hep3Vector; - -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 KalmanV0Vertexer}, mirroring - * {@code KalmanVertexFitterGainMatrixTest#testCascadeVertexExactGeometry}: 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 fitVertex} recovers that point - * with near-zero chi2. - */ -public class KalmanV0VertexerTest 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); - - KalmanV0Vertexer vertexer = new KalmanV0Vertexer(B_FIELD); - BilliorVertex vtx = vertexer.fitVertex(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); - - KalmanV0Vertexer vertexer = new KalmanV0Vertexer(B_FIELD); - BilliorVertex vtx = vertexer.fitVertex(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) - 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); - } - - public void testPlaceholderVertexOnFailedFit() { - BilliorVertex placeholder = KalmanV0Vertexer.placeholderVertex(); - 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 KalmanVertexFitterGainMatrixTest#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/KalmanVertexFitterGainMatrixTest.java b/recon/src/test/java/org/hps/recon/vertexing/KalmanVertexFitterGainMatrixTest.java deleted file mode 100644 index 52aaeca4d1..0000000000 --- a/recon/src/test/java/org/hps/recon/vertexing/KalmanVertexFitterGainMatrixTest.java +++ /dev/null @@ -1,1384 +0,0 @@ -package org.hps.recon.vertexing; - -import junit.framework.TestCase; - -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.KalmanVertexFitterGainMatrix; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TrackParams; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.FitResult; -import org.hps.recon.vertexing.KalmanVertexFitterGainMatrix.TwoVertexFitResult; - -import java.util.ArrayList; -import java.util.List; - -/** - * Test cases for KalmanVertexFitterGainMatrix, particularly the Lagrange multiplier - * constrained fitting for three-prong vertices with 4-momentum conservation. - */ -public class KalmanVertexFitterGainMatrixTest 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"); - - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(B_FIELD); - fitter.setDebug(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 - FitResult result = fitter.fit(tracks, 10, 1e-6); - - 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"); - - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(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 - - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(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 cascade (V0-line + recoil-track) vertex fit: - * construct a helix and a line that are both built to pass exactly through the same - * point, with small (non-singular) measurement covariances, and verify - * fitCascadeVertex recovers that point. - */ - public void testCascadeVertexExactGeometry() { - System.out.println("\n=== testCascadeVertexExactGeometry ===\n"); - - double xV = 0.3, yV = -0.2, zV = 5.0; - - RealMatrix trackCov = createTrackCovariance(1e-4, 1e-5, 1e-8, 1e-4, 1e-5); - TrackParams recoilTrack = createExactTrackThroughPoint( - xV, yV, zV, 0.15, -0.05, 1.0, -1, B_FIELD, trackCov); - - RealMatrix lineCov = MatrixUtils.createRealIdentityMatrix(6).scalarMultiply(1e-8); - KalmanVertexFitterGainMatrix.LineParams v0Line = - new KalmanVertexFitterGainMatrix.LineParams(xV, yV, zV, 0.10, 0.03, 2.5, lineCov); - - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(B_FIELD); - FitResult result = fitter.fitCascadeVertex(recoilTrack, v0Line); - - System.out.printf("Fitted vertex: [%.6f, %.6f, %.6f] (truth: [%.6f, %.6f, %.6f])%n", - result.vertex.getEntry(0), result.vertex.getEntry(1), result.vertex.getEntry(2), - xV, yV, zV); - System.out.printf("Chi2/NDF: %.6f / %d%n", result.chi2, result.ndf); - - assertEquals(xV, result.vertex.getEntry(0), 1e-4); - assertEquals(yV, result.vertex.getEntry(1), 1e-4); - assertEquals(zV, result.vertex.getEntry(2), 1e-4); - assertEquals(1, result.ndf); - assertTrue("chi2 should be small for an exactly-consistent geometry", result.chi2 < 1e-3); - } - - /** - * Toy-MC pull study for the cascade vertex fit: build a truth production vertex where a - * helix and a line cross, smear both inputs by their (correlated, full 6x6 for the line) - * covariances many times, refit, and check that the fitted-vertex pulls have mean ~0 / - * width ~1 and that chi2/ndf averages to ~1 (ndf=1), consistent with - * {@code testCascadeVertexExactGeometry}'s single noiseless case but now exercising the - * statistical behavior of the fit under realistic measurement uncertainty. - */ - public void testCascadeVertexSmearedPulls() { - System.out.println("\n=== testCascadeVertexSmearedPulls ===\n"); - - double xV = 0.5, yV = -0.3, zV = 8.0; - double px = 0.20, py = -0.06, pz = 1.2; // recoil track truth momentum - double dx = 0.35, dy = 0.12, dz = 3.0; // V0 line truth direction (= its momentum) - - // Line's own reference point need not be the intersection point -- any point on the - // line works. Offset it to exercise that generality. - double s = -2.0; - double refX = xV + s * dx, refY = yV + s * dy, refZ = zV + s * dz; - - 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 sigmaPos = 0.05; // mm, line reference-point uncertainty - double sigmaMom = 0.02; // GeV, line direction (momentum) uncertainty - double rho = 0.3; // correlation between position_i and momentum_i - RealMatrix lineCov = MatrixUtils.createRealMatrix(6, 6); - for (int i = 0; i < 3; i++) { - lineCov.setEntry(i, i, sigmaPos * sigmaPos); - lineCov.setEntry(3 + i, 3 + i, sigmaMom * sigmaMom); - double cross = rho * sigmaPos * sigmaMom; - lineCov.setEntry(i, 3 + i, cross); - lineCov.setEntry(3 + i, i, cross); - } - - java.util.Random rng = new java.util.Random(12345); - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(B_FIELD); - - int nToys = 3000; - double[] pullX = new double[nToys], pullY = new double[nToys], pullZ = new double[nToys]; - double[] chi2 = new double[nToys]; - int nFailed = 0; - - for (int toy = 0; toy < nToys; toy++) { - TrackParams truthTrack = createExactTrackThroughPoint( - xV, yV, zV, px, py, pz, -1, B_FIELD, trackCov); - double[] tp = truthTrack.toArray(); - double[] smearedTp = new double[5]; - double[] trackSigma = {d0Err, phi0Err, omegaErr, z0Err, tanLErr}; - for (int i = 0; i < 5; i++) { - smearedTp[i] = tp[i] + trackSigma[i] * rng.nextGaussian(); - } - TrackParams smearedTrack = new TrackParams( - smearedTp[0], smearedTp[1], smearedTp[2], smearedTp[3], smearedTp[4], trackCov); - - double[] dPos = new double[3], dMom = new double[3]; - for (int i = 0; i < 3; i++) { - double z1 = rng.nextGaussian(); - double z2 = rng.nextGaussian(); - dPos[i] = sigmaPos * z1; - dMom[i] = rho * sigmaMom * z1 + sigmaMom * FastMath.sqrt(1.0 - rho * rho) * z2; - } - KalmanVertexFitterGainMatrix.LineParams smearedLine = new KalmanVertexFitterGainMatrix.LineParams( - refX + dPos[0], refY + dPos[1], refZ + dPos[2], - dx + dMom[0], dy + dMom[1], dz + dMom[2], lineCov); - - FitResult result = fitter.fitCascadeVertex(smearedTrack, smearedLine); - if (result == null) { - nFailed++; - continue; - } - - pullX[toy] = (result.vertex.getEntry(0) - xV) / FastMath.sqrt(result.vertexCov.getEntry(0, 0)); - pullY[toy] = (result.vertex.getEntry(1) - yV) / FastMath.sqrt(result.vertexCov.getEntry(1, 1)); - pullZ[toy] = (result.vertex.getEntry(2) - zV) / FastMath.sqrt(result.vertexCov.getEntry(2, 2)); - chi2[toy] = result.chi2; - } - - assertEquals("no toy fits should fail", 0, nFailed); - - double meanChi2 = mean(chi2); - double[] meanX = {mean(pullX)}, meanY = {mean(pullY)}, meanZ = {mean(pullZ)}; - double stdX = std(pullX, meanX[0]), stdY = std(pullY, meanY[0]), stdZ = std(pullZ, meanZ[0]); - - System.out.printf("Pull X: mean=%.3f std=%.3f%n", meanX[0], stdX); - System.out.printf("Pull Y: mean=%.3f std=%.3f%n", meanY[0], stdY); - System.out.printf("Pull Z: mean=%.3f std=%.3f%n", meanZ[0], stdZ); - System.out.printf("Mean chi2 (ndf=1): %.3f%n", meanChi2); - - assertTrue("pull X mean should be near 0, got " + meanX[0], FastMath.abs(meanX[0]) < 0.15); - assertTrue("pull Y mean should be near 0, got " + meanY[0], FastMath.abs(meanY[0]) < 0.15); - assertTrue("pull Z mean should be near 0, got " + meanZ[0], FastMath.abs(meanZ[0]) < 0.15); - assertTrue("pull X std should be near 1, got " + stdX, stdX > 0.8 && stdX < 1.2); - assertTrue("pull Y std should be near 1, got " + stdY, stdY > 0.8 && stdY < 1.2); - assertTrue("pull Z std should be near 1, got " + stdZ, stdZ > 0.8 && stdZ < 1.2); - assertTrue("mean chi2 for ndf=1 should be near 1, got " + meanChi2, meanChi2 > 0.7 && meanChi2 < 1.4); - } - - /** - * 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 (ThreeTrackVertexer'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 ThreeTrackVertexer'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}); - - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(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 ThreeTrackVertexer's real usage). - RealVector v2Init = MatrixUtils.createRealVector(new double[]{0.0, 0.0, 0.0}); - - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(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); - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(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))); - } - - /** - * 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 = KalmanVertexFitterGainMatrix.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); - - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(B_FIELD); - KalmanVertexFitterGainMatrix.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 ThreeTrackVertexer}'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 ThreeTrackVertexer falls back to in practice. - RealVector v2Init = MatrixUtils.createRealVector(new double[]{0.0, 0.0, 0.0}); - - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(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 ThreeTrackVertexer}) 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 ThreeTrackVertexer}'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}); - - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(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.ThreeTrackVertexer} 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); - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(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))); - } - - 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); - } - - /** - * 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); - - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(bField); - KalmanVertexFitterGainMatrix.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 { - KalmanVertexFitterGainMatrix.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); - - KalmanVertexFitterGainMatrix fitter = new KalmanVertexFitterGainMatrix(bField); - KalmanVertexFitterGainMatrix.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 { - KalmanVertexFitterGainMatrix.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))); - } - } - - KalmanVertexFitterGainMatrix.LineParams line = - new KalmanVertexFitterGainMatrix.LineParams(x0, y0, z0, dx, dy, dz, cov6); - - double xPlane = -1.1; - double sigmaXFloor = 0.001; - KalmanVertexFitterGainMatrix.LinePlaneProjection proj = - KalmanVertexFitterGainMatrix.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; - KalmanVertexFitterGainMatrix.LinePlaneProjection proj = - KalmanVertexFitterGainMatrix.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); - KalmanVertexFitterGainMatrix.LinePlaneProjection pPlus = - KalmanVertexFitterGainMatrix.propagateTrackToPlane(tPlus, xPlane, sigmaXFloor); - KalmanVertexFitterGainMatrix.LinePlaneProjection pMinus = - KalmanVertexFitterGainMatrix.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/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/ThreeTrackVertexerTest.java b/recon/src/test/java/org/hps/recon/vertexing/ThreeTrackVertexerTest.java deleted file mode 100644 index 25721f39d6..0000000000 --- a/recon/src/test/java/org/hps/recon/vertexing/ThreeTrackVertexerTest.java +++ /dev/null @@ -1,204 +0,0 @@ -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 ThreeTrackVertexer}, mirroring {@link - * CascadeVertexerTest}: 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 ThreeTrackVertexerTest 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); - - ThreeTrackVertexer vertexer = new ThreeTrackVertexer(B_FIELD); - ReconstructedParticle cascade = vertexer.fit(v0Particle, recoilElectron, true); - - 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); - } - - 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 KalmanV0VertexerTest#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/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/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 From 56fbab6e65ec5e56baa6c724332f387a105ecc29 Mon Sep 17 00:00:00 2001 From: Matt Graham Date: Thu, 8 Oct 2026 11:57:30 -0700 Subject: [PATCH 4/4] Remove BADFIT_DEBUG print and dead truthMatched plumbing from CascadeVertexer/ReconParticleDriver --- .../recon/particle/ReconParticleDriver.java | 98 ++----------------- .../hps/recon/vertexing/CascadeVertexer.java | 45 +-------- .../recon/vertexing/CascadeVertexerTest.java | 16 +-- 3 files changed, 18 insertions(+), 141 deletions(-) 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 b905b8b905..50a41dd300 100644 --- a/recon/src/main/java/org/hps/recon/particle/ReconParticleDriver.java +++ b/recon/src/main/java/org/hps/recon/particle/ReconParticleDriver.java @@ -27,8 +27,6 @@ import org.lcsim.event.Cluster; 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.TrackState; @@ -321,16 +319,6 @@ public void setApplyClusterCorrections(boolean val) { * #ntrackVertexCandidatesColName} is also set. */ protected String ntrackVertexCandidatesBothConstrainedColName = null; - /** - * MC-truth collection names used only to gate the {@code BADFIT_DEBUG} diagnostic - * print in {@link CascadeVertexer#fit} to genuinely truth-matched V0(e-/e+) + - * recoil-electron combinations, rather than every combinatorial pairing tried by - * {@link #findCascadeVertices}. Both default to null, i.e. no truth info is - * looked up and the debug print never fires, unless both are explicitly set (MC - * steering files only). - */ - protected String mcParticlesColName = null; - protected String trackToMCParticleRelationsColName = null; // Accumulated wall-clock time (ns) and count of calls spent in the cascade // simultaneous vertex fit, reported in endOfData(). @@ -512,28 +500,6 @@ public void setBeamMomConstraintSigmaTNuclearRecoil(double beamMomConstraintSigm this.beamMomConstraintSigmaTNuclearRecoil = beamMomConstraintSigmaTNuclearRecoil; } - /** - * Sets the name of the LCIO MCParticle collection used only to gate the - * {@code BADFIT_DEBUG} print in {@link #findCascadeVertices} to truth-matched - * candidates. Optional; MC steering files only. - * - * @param mcParticlesColName - The LCIO collection name. - */ - public void setMcParticlesColName(String mcParticlesColName) { - this.mcParticlesColName = mcParticlesColName; - } - - /** - * Sets the name of the LCIO Track-to-MCParticle LCRelation collection used only to - * gate the {@code BADFIT_DEBUG} print in {@link #findCascadeVertices} to - * truth-matched candidates. Optional; MC steering files only. - * - * @param trackToMCParticleRelationsColName - The LCIO collection name. - */ - public void setTrackToMCParticleRelationsColName(String trackToMCParticleRelationsColName) { - this.trackToMCParticleRelationsColName = trackToMCParticleRelationsColName; - } - /** * Sets the name of the LCIO collection for beam spot constrained V0 * candidate vertices. @@ -1111,7 +1077,7 @@ else if (finalStateParticle.getCharge() < 0) { // already one of its daughters. Off by default; only runs if a collection // name has been set. if (cascadeVertexCandidatesColName != null) { - findCascadeVertices(event, unconstrainedV0Candidates, goodFinalStateParticles); + findCascadeVertices(unconstrainedV0Candidates, goodFinalStateParticles); printDebug("[ReconParticleDriver] findCascadeVertices() finished"); } // Add the final state ReconstructedParticles to the event @@ -1236,65 +1202,15 @@ protected double bLocalForTrack(Track track) { * than null on failure, so these two lists stay index-aligned with {@link * #cascadeVertexCandidates} automatically. * - * @param event Current event, used only to look up MC truth (via - * {@link #mcParticlesColName}/{@link #trackToMCParticleRelationsColName}) - * to gate {@link CascadeVertexer}'s BADFIT_DEBUG print - * to truth-matched candidates; unused otherwise. * @param v0Candidates Unconstrained V0 candidates for this event. * @param finalStateElectrons Final-state electrons for this event. */ - protected void findCascadeVertices(EventHeader event, List v0Candidates, + protected void findCascadeVertices(List v0Candidates, List finalStateElectrons) { - Map trackToMC = new HashMap(); - MCParticle eleMC = null; - MCParticle posMC = null; - MCParticle recoilMC = null; - if (mcParticlesColName != null && trackToMCParticleRelationsColName != null - && event.hasCollection(MCParticle.class, mcParticlesColName) - && event.hasCollection(LCRelation.class, trackToMCParticleRelationsColName)) { - for (LCRelation rel : event.get(LCRelation.class, trackToMCParticleRelationsColName)) { - trackToMC.put((Track) rel.getFrom(), (MCParticle) rel.getTo()); - } - MCParticle apMC = null; - 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 (see CascadeVertexTupleDriver): 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 -- it 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; - } - } - } - } - 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); - boolean v0Matched = eleMC != null && eleMC.equals(trackToMC.get(v0EleDaughter.getTracks().get(0))) - && posMC != null && posMC.equals(trackToMC.get(v0PosDaughter.getTracks().get(0))); CascadeVertexer cascadeVertexer = new CascadeVertexer(bLocalForTrack(v0EleDaughter.getTracks().get(0))); cascadeVertexer.setFixV2BeamCoordinate(fixV2BeamCoordinate); cascadeVertexer.setUseBeamspotConstraintForV2(useBeamspotConstraintForV2); @@ -1305,17 +1221,15 @@ protected void findCascadeVertices(EventHeader event, List= 0 || v0Daughters.contains(electron)) { continue; } - boolean truthMatched = v0Matched && recoilMC != null - && recoilMC.equals(trackToMC.get(electron.getTracks().get(0))); long fitStartTime = System.nanoTime(); try { - ReconstructedParticle cascadeVertex = cascadeVertexer.fit(v0, electron, truthMatched); + ReconstructedParticle cascadeVertex = cascadeVertexer.fit(v0, electron); if (cascadeVertex != null) { cascadeVertexCandidates.add(cascadeVertex); if (cascadeVertexCandidatesBeamConstrainedColName != null) { ReconstructedParticle cascadeVertexBC; try { - cascadeVertexBC = cascadeVertexer.fit(v0, electron, truthMatched, true, + cascadeVertexBC = cascadeVertexer.fit(v0, electron, true, beamMomConstraintEnergy, beamMomConstraintRotAngle, beamMomConstraintSigmaTNuclearRecoil); } catch (RuntimeException e) { @@ -1335,7 +1249,7 @@ protected void findCascadeVertices(EventHeader event, List v0Daughters = v0Particle.getParticles(); ReconstructedParticle eleDaughter = v0Daughters.get(0).getCharge() < 0 ? v0Daughters.get(0) : v0Daughters.get(1); @@ -332,18 +327,6 @@ public ReconstructedParticle fit(ReconstructedParticle v0Particle, Reconstructed RealVector refPointVec = MatrixUtils.createRealVector(refPoint); finalResult.v1 = finalResult.v1.add(refPointVec); finalResult.v2 = finalResult.v2.add(refPointVec); - if (truthMatched && finalResult.chi2 / finalResult.ndf > 1000) { - System.out.println("BADFIT_DEBUG bField=" + bField); - System.out.println("BADFIT_DEBUG v1Init=" + v1Init); - System.out.println("BADFIT_DEBUG v1Cov=" + v1Cov); - System.out.println("BADFIT_DEBUG v2Init=" + v2InitForFit); - System.out.println("BADFIT_DEBUG v2Cov=" + v2CovForFit); - System.out.println("BADFIT_DEBUG eleParams=" + dumpTrackParams(eleParams)); - System.out.println("BADFIT_DEBUG posParams=" + dumpTrackParams(posParams)); - System.out.println("BADFIT_DEBUG recoilParams=" + dumpTrackParams(recoilParams)); - System.out.println("BADFIT_DEBUG chi2=" + finalResult.chi2 + " ndf=" + finalResult.ndf); - System.out.println("BADFIT_DEBUG v1=" + finalResult.v1 + " v2=" + finalResult.v2); - } return makeReconstructedParticle(finalResult, eleDaughter, posDaughter, recoilElectron, v0Proj, recoilProj, v0Vertex); } @@ -414,26 +397,6 @@ private static LineParams shiftLine(LineParams line, double[] refPoint, double s line.dx, line.dy, line.dz, line.cov); } - private static String dumpTrackParams(TrackParams tp) { - double[] a = tp.toArray(); - StringBuilder sb = new StringBuilder(); - sb.append("d0=").append(a[0]).append(" phi0=").append(a[1]).append(" omega=").append(a[2]) - .append(" z0=").append(a[3]).append(" tanLambda=").append(a[4]).append(" cov=["); - for (int i = 0; i < 5; i++) { - for (int j = 0; j < 5; j++) { - sb.append(tp.cov.getEntry(i, j)); - if (j < 4) { - sb.append(","); - } - } - if (i < 4) { - sb.append(";"); - } - } - sb.append("]"); - return sb.toString(); - } - private static ReconstructedParticle makeReconstructedParticle(TwoVertexFitResult result, ReconstructedParticle eleDaughter, ReconstructedParticle posDaughter, ReconstructedParticle recoilElectron, LinePlaneProjection v0Proj, LinePlaneProjection recoilProj, BilliorVertex v0InputVertex) { diff --git a/recon/src/test/java/org/hps/recon/vertexing/CascadeVertexerTest.java b/recon/src/test/java/org/hps/recon/vertexing/CascadeVertexerTest.java index 2a60db489c..a3389198b3 100644 --- a/recon/src/test/java/org/hps/recon/vertexing/CascadeVertexerTest.java +++ b/recon/src/test/java/org/hps/recon/vertexing/CascadeVertexerTest.java @@ -61,7 +61,7 @@ public void testFitExactGeometryNonOriginReferencePoint() { ReconstructedParticle recoilElectron = makeElectronParticle(recoilTrack); CascadeVertexer vertexer = new CascadeVertexer(B_FIELD); - ReconstructedParticle cascade = vertexer.fit(v0Particle, recoilElectron, true); + ReconstructedParticle cascade = vertexer.fit(v0Particle, recoilElectron); assertNotNull("three-track fit should not be null", cascade); @@ -119,7 +119,7 @@ public void testFitWithBeamspotConstraintForV2PullsTowardBeamPosition() { ReconstructedParticle recoilElectron = makeElectronParticle(recoilTrack); CascadeVertexer defaultVertexer = new CascadeVertexer(B_FIELD); - ReconstructedParticle cascadeDefault = defaultVertexer.fit(v0Particle, recoilElectron, true); + ReconstructedParticle cascadeDefault = defaultVertexer.fit(v0Particle, recoilElectron); assertNotNull("default (unconstrained-V2) fit should not be null", cascadeDefault); Hep3Vector v2PosDetDefault = ((BilliorVertex) cascadeDefault.getStartVertex()).getPosition(); @@ -138,7 +138,7 @@ public void testFitWithBeamspotConstraintForV2PullsTowardBeamPosition() { CascadeVertexer beamspotVertexer = new CascadeVertexer(B_FIELD); beamspotVertexer.setUseBeamspotConstraintForV2(true); beamspotVertexer.setBeamspotConstraintForV2Params(beamPositionOverride, beamSizeOverride); - ReconstructedParticle cascadeBS = beamspotVertexer.fit(v0Particle, recoilElectron, true); + ReconstructedParticle cascadeBS = beamspotVertexer.fit(v0Particle, recoilElectron); assertNotNull("beamspot-constrained-V2 fit should not be null", cascadeBS); Hep3Vector v2PosDetBS = ((BilliorVertex) cascadeBS.getStartVertex()).getPosition(); @@ -209,7 +209,7 @@ public void testFitBothConstrainedPullsTowardBeamspotAndBeamMomentum() { ReconstructedParticle recoilElectron = makeElectronParticle(recoilTrack); CascadeVertexer defaultVertexer = new CascadeVertexer(B_FIELD); - ReconstructedParticle cascadeDefault = defaultVertexer.fit(v0Particle, recoilElectron, true); + 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(); @@ -235,7 +235,7 @@ public void testFitBothConstrainedPullsTowardBeamspotAndBeamMomentum() { bothVertexer.setUseBeamspotConstraintForV2(true); bothVertexer.setBeamspotConstraintForV2Params(beamPositionOverride, beamSizeOverride); ReconstructedParticle cascadeBoth = bothVertexer.fit( - v0Particle, recoilElectron, true, true, beamEnergy, beamRotAngle, 0.0); + 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(); @@ -270,7 +270,7 @@ public void testFitBothConstrainedPullsTowardBeamspotAndBeamMomentum() { CascadeVertexer beamspotOnlyVertexer = new CascadeVertexer(B_FIELD); beamspotOnlyVertexer.setUseBeamspotConstraintForV2(true); beamspotOnlyVertexer.setBeamspotConstraintForV2Params(beamPositionOverride, beamSizeOverride); - ReconstructedParticle cascadeBS = beamspotOnlyVertexer.fit(v0Particle, recoilElectron, true); + 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); @@ -279,7 +279,7 @@ public void testFitBothConstrainedPullsTowardBeamspotAndBeamMomentum() { CascadeVertexer beamMomOnlyVertexer = new CascadeVertexer(B_FIELD); ReconstructedParticle cascadeBM = beamMomOnlyVertexer.fit( - v0Particle, recoilElectron, true, true, beamEnergy, beamRotAngle, 0.0); + 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 " @@ -313,7 +313,7 @@ public void testPlaceholderCascade() { ReconstructedParticle recoilElectron = makeElectronParticle(recoilTrack); CascadeVertexer vertexer = new CascadeVertexer(B_FIELD); - ReconstructedParticle cascade = vertexer.fit(v0Particle, recoilElectron, true); + ReconstructedParticle cascade = vertexer.fit(v0Particle, recoilElectron); assertNotNull("cascade fit should not be null", cascade); ReconstructedParticle placeholder = CascadeVertexer.placeholderCascade(cascade);