diff --git a/common-tools/clas-tracking/pom.xml b/common-tools/clas-tracking/pom.xml index 6de3ed7f02..ab952d8186 100644 --- a/common-tools/clas-tracking/pom.xml +++ b/common-tools/clas-tracking/pom.xml @@ -14,6 +14,10 @@ + + junit + junit + org.apache.commons commons-math3 diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AKFitter.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AKFitter.java index a940a6f9fe..a9c4a9aac1 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AKFitter.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/AKFitter.java @@ -23,6 +23,11 @@ public abstract class AKFitter { private double resiCut = 50;//residual cut for the measurements // return variables + /** Temporary A/B switch for the former behaviour, where a null intermediate + * smoothed state made the whole chi2 NaN and failed the track. */ + protected static final boolean LEGACY_NULL_SMOOTH_FAIL = + Boolean.getBoolean("kf.legacyNullSmoothFail"); + public boolean setFitFailed = false; public double chi2; public int NDF; @@ -180,12 +185,16 @@ public void setTrajectory(AStateVecs sv, AMeasVecs mv) { int index = mv.measurements.get(k).layer; int layer = mv.measurements.get(k).surface.getLayer(); int sector = mv.measurements.get(k).surface.getSector(); + // A null smoothed state means the swim from the smoothed helix could not reach + // this surface, so there is no smoothed residual to report. Use -999, the same + // convention as a passive surface below, rather than the NaN dh() returns. + boolean nullSmooth = sv.smoothed().get(k)==null && !LEGACY_NULL_SMOOTH_FAIL; double tRes = mv.dh(k, sv.transported().get(k)); double fRes = mv.dh(k, sv.filtered(false).get(k)); - double sRes = mv.dh(k, sv.smoothed().get(k)); + double sRes = nullSmooth ? -999 : mv.dh(k, sv.smoothed().get(k)); if(!mv.measurements.get(k).surface.passive) { trajPoints.put(index, new HitOnTrack(layer, sector, sv.transported().get(k), tRes, fRes, sRes)); - if(mv.measurements.get(k).skip) + if(mv.measurements.get(k).skip || nullSmooth) trajPoints.get(index).isUsed = false; } else { trajPoints.put(index, new HitOnTrack(layer, sector, sv.transported().get(k), -999, -999, -999)); @@ -211,7 +220,14 @@ public double calc_chi2(AStateVecs sv, AMeasVecs mv) { sv.transported().put(0, sv.smoothed().get(0).clone()); for(int k = 0; k< mv.measurements.size(); k++) { - if(!mv.measurements.get(k).skip) { + if(!mv.measurements.get(k).skip && sv.smoothed().get(k)==null + && !LEGACY_NULL_SMOOTH_FAIL) { + // smooth() returns null when the swim from the smoothed helix cannot reach + // this surface. dh() returns NaN by contract for a null state, which would + // make the whole chi2 NaN and fail a track whose other measurements are + // fine. Treat it like a skipped measurement: excluded from chi2 and NDF. + } + else if(!mv.measurements.get(k).skip) { double dh = mv.dh(k, sv.smoothed().get(k)); double error = mv.measurements.get(k).error; chisq += dh*dh / error/error; diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/KFitter.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/KFitter.java index f50f453521..695df478fa 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/KFitter.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/KFitter.java @@ -96,11 +96,13 @@ public double getChi2(int mode) { if(mode==1) { double chi2 = 0; for(int k = 1; k< mv.measurements.size(); k++) { - if(!mv.measurements.get(k).skip) { - double dh = mv.dh(k, sv.smoothed().get(k)); - double error = mv.measurements.get(k).error; - chi2 += dh*dh / error/error; - } + if(mv.measurements.get(k).skip) continue; + // same rule as AKFitter.calc_chi2: a null smoothed state contributes nothing, + // rather than the NaN dh() returns for it, which would make this chi2 NaN + if(sv.smoothed().get(k)==null && !LEGACY_NULL_SMOOTH_FAIL) continue; + double dh = mv.dh(k, sv.smoothed().get(k)); + double error = mv.measurements.get(k).error; + chi2 += dh*dh / error/error; } return chi2; } @@ -223,6 +225,14 @@ public StateVec filter(int k, StateVec vec, AMeasVecs mv) { // System.out.println(dh); //get the projector Matrix double[] H = mv.H(fVec, sv, mv.measurements.get(k), this.getSwimmer()); + if (H == null) { + // A perturbed state could not be placed on the measurement surface. A + // nominal-surface substitute would make the derivative geometrically + // inconsistent, so exclude this measurement and retain the incoming state. + this.NDF--; + mv.measurements.get(k).skip = true; + return fVec; + } // System.out.println(k + " " + mv.measurements.get(k).layer + " " + H[0] + " " + H[1] + " " + H[2] + " " + H[3] + " " + H[4] + " " +dh ); double[][] CaInv = this.getMatrixOps().filterCovMat(H, fVec.covMat, V); diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/MeasVecs.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/MeasVecs.java index fc15c2c77e..b7786b9614 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/MeasVecs.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/MeasVecs.java @@ -74,8 +74,9 @@ public double[] H(AStateVecs.StateVec stateVec, AStateVecs sv, MeasVec mv, Swim //SVplus.updateFromHelix(); //SVminus.updateFromHelix(); - sv.setStateVecPosAtMeasSite(SVplus, mv, null); - sv.setStateVecPosAtMeasSite(SVminus, mv, null); + boolean plusOK = sv.setStateVecPosAtMeasSite(SVplus, mv, null); + boolean minusOK = sv.setStateVecPosAtMeasSite(SVminus, mv, null); + if (!plusOK || !minusOK) return null; // sv.printlnStateVec(SVplus); // sv.printlnStateVec(SVminus); Hval[i] = (this.h(stateVec.k, SVplus) - this.h(stateVec.k, SVminus)) / getDelta_d_a()[i] ; diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/StateVecs.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/StateVecs.java index 067bcf5c5a..fec3d1486b 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/StateVecs.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/kalmanfilter/helical/StateVecs.java @@ -18,7 +18,6 @@ */ public class StateVecs extends AStateVecs { - @Override public boolean getStateVecPosAtMeasSite(StateVec sv, AMeasVecs.MeasVec mv, Swim swim) { double[] swimPars = new double[7]; @@ -73,30 +72,48 @@ else if(mv.surface.cylinder!=null) { if(swim==null) { // applicable only to planes parallel to the z -axis Helix helix = sv.getHelix(xref, yref); if(mv.surface.plane!=null) { - Point3D pos = helix.getHelixPointAtPlane(mv.surface.finitePlaneCorner1.x(), mv.surface.finitePlaneCorner1.y(), - mv.surface.finitePlaneCorner2.x(), mv.surface.finitePlaneCorner2.y(), 10); - Vector3D mom = helix.getMomentumAtPlane(mv.surface.finitePlaneCorner1.x(), mv.surface.finitePlaneCorner1.y(), - mv.surface.finitePlaneCorner2.x(), mv.surface.finitePlaneCorner2.y(), 10); - sv.path = helix.getLAtPlane(mv.surface.finitePlaneCorner1.x(), mv.surface.finitePlaneCorner1.y(), - mv.surface.finitePlaneCorner2.x(), mv.surface.finitePlaneCorner2.y(), 10); - sv.x = pos.x(); - sv.y = pos.y(); - sv.z = pos.z(); - sv.px = mom.x(); - sv.py = mom.y(); - sv.pz = mom.z(); + Vector3D normal = mv.surface.plane.normal(); + Point3D point = mv.surface.plane.point(); + double l = helix.getLAtPlane3D(point.x(), point.y(), point.z(), + normal.x(), normal.y(), normal.z(), + point.x(), point.y()); + if(!Double.isFinite(l)) return false; + sv.path = l; + sv.x = helix.getX(l); + sv.y = helix.getY(l); + sv.z = helix.getZ(l); + sv.px = helix.getPx(helix.getB(), l); + sv.py = helix.getPy(helix.getB(), l); + sv.pz = helix.getPz(helix.getB()); } else { double r = mv.surface.cylinder.baseArc().radius(); - Point3D pos = helix.getHelixPointAtR(r); - Vector3D mom = helix.getMomentumAtR(r); - sv.path = helix.getLAtR(r); - sv.x = pos.x(); - sv.y = pos.y(); - sv.z = pos.z(); - sv.px = mom.x(); - sv.py = mom.y(); - sv.pz = mom.z(); + double l = helix.getLAtR(r); + Point3D axisStart = mv.surface.cylinder.getAxis().origin(); + Point3D axisEnd = mv.surface.cylinder.getAxis().end(); + + // Preserve the exact closed-form result for an ideal beam-axis cylinder. + // Alignment may displace or tilt the BMT axis; in that case intersect the + // same cylinder used by the measurement surface. + boolean beamAxis = Math.abs(axisStart.x()) < 1.0e-9 + && Math.abs(axisStart.y()) < 1.0e-9 + && Math.abs(axisEnd.x()) < 1.0e-9 + && Math.abs(axisEnd.y()) < 1.0e-9; + if (!beamAxis) { + l = helix.getLAtCylinder3D( + axisStart.x(), axisStart.y(), axisStart.z(), + axisEnd.x() - axisStart.x(), + axisEnd.y() - axisStart.y(), + axisEnd.z() - axisStart.z(), r, l); + if (!Double.isFinite(l)) return false; + } + sv.path = l; + sv.x = helix.getX(l); + sv.y = helix.getY(l); + sv.z = helix.getZ(l); + sv.px = helix.getPx(helix.getB(), l); + sv.py = helix.getPy(helix.getB(), l); + sv.pz = helix.getPz(helix.getB()); } } else { diff --git a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/trackrep/Helix.java b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/trackrep/Helix.java index 39582caf7b..e9000263db 100644 --- a/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/trackrep/Helix.java +++ b/common-tools/clas-tracking/src/main/java/org/jlab/clas/tracking/trackrep/Helix.java @@ -241,6 +241,161 @@ public double getPz() { return this.getPz(this.getB()); } + /** Intersect the helix with the complete plane n.(r-p)=0, including nz. */ + public double getLAtPlane3D(double px, double py, double pz, + double nx, double ny, double nz, + double xNear, double yNear) { + // Start at the point on the helix circle nearest the module. The projected trace of a + // nearly tangential plane can miss the circle even when the full 3-D plane intersects it. + double phi0 = Math.atan2(_yd - getYc(), _xd - getXc()); + double phiNear = Math.atan2(yNear - getYc(), xNear - getXc()); + double dphi = phiNear - phi0; + if (dphi > Math.PI) dphi -= 2.0 * Math.PI; + if (dphi < -Math.PI) dphi += 2.0 * Math.PI; + double l = dphi / getOmega(); + if (!Double.isFinite(l)) return Double.NaN; + + final double dl = 1.0e-3; + for (int i = 0; i < 12; i++) { + double f = nx * (getX(l) - px) + ny * (getY(l) - py) + + nz * (getZ(l) - pz); + if (Math.abs(f) < 1.0e-7) return l; + double derivative = nx * (getX(l + dl) - getX(l - dl)) / (2 * dl) + + ny * (getY(l + dl) - getY(l - dl)) / (2 * dl) + + nz * (getZ(l + dl) - getZ(l - dl)) / (2 * dl); + if (!Double.isFinite(derivative) || Math.abs(derivative) < 1.0e-10) + return Double.NaN; + double step = f / derivative; + if (Math.abs(step) > 100) step = Math.copySign(100, step); + l -= step; + if (!Double.isFinite(l)) return Double.NaN; + } + double residual = nx * (getX(l) - px) + ny * (getY(l) - py) + + nz * (getZ(l) - pz); + return Math.abs(residual) < 1.0e-5 ? l : Double.NaN; + } + + /** + * Intersect this helix with a cylinder of radius {@code radius} about an arbitrary axis. + * The supplied starting value normally comes from the closed-form beam-axis solution. + * + * @return the local root nearest {@code lStart}, or NaN when no root can be established + */ + public double getLAtCylinder3D(double ax, double ay, double az, + double ux, double uy, double uz, + double radius, double lStart) { + double un = Math.sqrt(ux * ux + uy * uy + uz * uz); + if (!(un > 0) || !Double.isFinite(lStart)) return Double.NaN; + ux /= un; + uy /= un; + uz /= un; + + double l = lStart; + for (int i = 0; i < 12; i++) { + double f = radialMiss(l, ax, ay, az, ux, uy, uz, radius); + if (!Double.isFinite(f)) break; + if (Math.abs(f) < 1.0e-7) return l; + + double derivative = radialMissDerivative(l, ax, ay, az, ux, uy, uz); + if (!Double.isFinite(derivative) || Math.abs(derivative) < 1.0e-10) break; + double step = f / derivative; + if (Math.abs(step) > 100.0) step = Math.copySign(100.0, step); + l -= step; + if (!Double.isFinite(l)) break; + } + double residual = radialMiss(l, ax, ay, az, ux, uy, uz, radius); + if (Double.isFinite(residual) && Math.abs(residual) < 1.0e-5) return l; + + // Safeguard Newton near tangencies by bracketing the first local root on each side. + // The quarter-turn limit prevents a failure from selecting the opposite crossing. + double maxSpan = Math.min(200.0, Math.PI / (2.0 * Math.abs(getOmega()))); + double left = lStart; + double right = lStart; + double fLeft = radialMiss(left, ax, ay, az, ux, uy, uz, radius); + double fRight = fLeft; + double span = 0.5; + while (Double.isFinite(fLeft) && Double.isFinite(fRight) + && Math.abs(right - lStart) < maxSpan) { + double nextSpan = Math.min(span, maxSpan); + double nextLeft = lStart - nextSpan; + double nextRight = lStart + nextSpan; + double nextFLeft = radialMiss(nextLeft, ax, ay, az, ux, uy, uz, radius); + double nextFRight = radialMiss(nextRight, ax, ay, az, ux, uy, uz, radius); + + double rootLeft = bracketedCylinderRoot(nextLeft, left, nextFLeft, fLeft, + ax, ay, az, ux, uy, uz, radius); + double rootRight = bracketedCylinderRoot(right, nextRight, fRight, nextFRight, + ax, ay, az, ux, uy, uz, radius); + if (Double.isFinite(rootLeft) || Double.isFinite(rootRight)) { + if (!Double.isFinite(rootLeft)) return rootRight; + if (!Double.isFinite(rootRight)) return rootLeft; + return Math.abs(rootLeft - lStart) <= Math.abs(rootRight - lStart) + ? rootLeft : rootRight; + } + left = nextLeft; + right = nextRight; + fLeft = nextFLeft; + fRight = nextFRight; + span *= 2.0; + } + return Double.NaN; + } + + private double radialMissDerivative(double l, double ax, double ay, double az, + double ux, double uy, double uz) { + double wx = getX(l) - ax; + double wy = getY(l) - ay; + double wz = getZ(l) - az; + double along = wx * ux + wy * uy + wz * uz; + double qx = wx - along * ux; + double qy = wy - along * uy; + double qz = wz - along * uz; + double distance = Math.sqrt(qx * qx + qy * qy + qz * qz); + if (!(distance > 0)) return Double.NaN; + + double s = -KFitter.polarity; + double phi = getPhi(l); + double vx = s * getTurningSign() * getR() * getOmega() * Math.cos(phi); + double vy = s * getTurningSign() * getR() * getOmega() * Math.sin(phi); + double vz = -getTanL(); + return (qx * vx + qy * vy + qz * vz) / distance; + } + + private double bracketedCylinderRoot(double lo, double hi, double flo, double fhi, + double ax, double ay, double az, + double ux, double uy, double uz, double radius) { + if (!Double.isFinite(flo) || !Double.isFinite(fhi) || flo * fhi > 0) { + return Double.NaN; + } + if (Math.abs(flo) < 1.0e-7) return lo; + if (Math.abs(fhi) < 1.0e-7) return hi; + for (int i = 0; i < 80; i++) { + double mid = 0.5 * (lo + hi); + double fm = radialMiss(mid, ax, ay, az, ux, uy, uz, radius); + if (!Double.isFinite(fm)) return Double.NaN; + if (Math.abs(fm) < 1.0e-7 || Math.abs(hi - lo) < 1.0e-9) return mid; + if (flo * fm <= 0) { + hi = mid; + } else { + lo = mid; + flo = fm; + } + } + return Double.NaN; + } + + private double radialMiss(double l, double ax, double ay, double az, + double ux, double uy, double uz, double radius) { + double wx = getX(l) - ax; + double wy = getY(l) - ay; + double wz = getZ(l) - az; + double along = wx * ux + wy * uy + wz * uz; + double dx = wx - along * ux; + double dy = wy - along * uy; + double dz = wz - along * uz; + return Math.sqrt(dx * dx + dy * dy + dz * dz) - radius; + } + public double getLAtPlane(double X1, double Y1, double X2, double Y2, double tolerance) { // Find the intersection of the helix circle with the module plane projection in XY which is a line diff --git a/common-tools/clas-tracking/src/test/java/org/jlab/clas/tracking/trackrep/HelixTest.java b/common-tools/clas-tracking/src/test/java/org/jlab/clas/tracking/trackrep/HelixTest.java new file mode 100644 index 0000000000..25e6173354 --- /dev/null +++ b/common-tools/clas-tracking/src/test/java/org/jlab/clas/tracking/trackrep/HelixTest.java @@ -0,0 +1,202 @@ +package org.jlab.clas.tracking.trackrep; + +import org.jlab.clas.tracking.kalmanfilter.Units; +import org.junit.Test; + +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertTrue; + +public class HelixTest { + + private static Helix helix() { + return new Helix(0.2, 0.4, 0.004, 20.0, 0.6, + 1, 5.0, 0.0, 0.0, Units.MM); + } + + @Test + public void full3DPlaneIntersectionIncludesLongitudinalTilt() { + Helix helix = helix(); + double expectedL = 80.0; + double x = helix.getX(expectedL); + double y = helix.getY(expectedL); + double z = helix.getZ(expectedL); + double nx = 0.8; + double ny = 0.6; + double nz = 0.02; + + double offset = 50.0; + double px = x + nz * offset; + double py = y; + double pz = z - nx * offset; + + double actualL = helix.getLAtPlane3D(px, py, pz, nx, ny, nz, px, py); + double projectedL = helix.getLAtPlane3D(px, py, pz, nx, ny, 0.0, px, py); + double residual = nx * (helix.getX(actualL) - px) + + ny * (helix.getY(actualL) - py) + + nz * (helix.getZ(actualL) - pz); + + assertTrue(Double.isFinite(actualL)); + assertEquals(expectedL, actualL, 1.0e-5); + assertEquals(0.0, residual, 1.0e-7); + assertTrue("2-D approximation must differ for a tilted plane", + Math.abs(projectedL - actualL) > 0.1); + } + + @Test + public void full3DPlaneIntersectionHandlesBothSignsAndRotations() { + for (int turn : new int[] {-1, 1}) { + Helix helix = new Helix(0.2, 0.4, turn * 0.004, 20.0, 0.6, + turn, 5.0, 0.0, 0.0, Units.MM); + for (double nz : new double[] {-0.02, 0.02}) { + for (double angle : new double[] {0.3, 1.7}) { + double expectedL = 70.0; + double x = helix.getX(expectedL); + double y = helix.getY(expectedL); + double z = helix.getZ(expectedL); + double nx = Math.cos(angle); + double ny = Math.sin(angle); + + double offset = 5.0; + double px = x + nz * offset; + double py = y; + double pz = z - nx * offset; + double actualL = helix.getLAtPlane3D( + px, py, pz, nx, ny, nz, px, py); + double residual = nx * (helix.getX(actualL) - px) + + ny * (helix.getY(actualL) - py) + + nz * (helix.getZ(actualL) - pz); + + assertTrue("turn=" + turn + " nz=" + nz + " angle=" + angle, + Double.isFinite(actualL)); + assertEquals(expectedL, actualL, 1.0e-4); + assertEquals(0.0, residual, 1.0e-7); + } + } + } + } + + @Test + public void full3DMatchesLegacyForIdealBarrelPlane() { + Helix helix = helix(); + double expectedL = 60.0; + double px = helix.getX(expectedL); + double py = helix.getY(expectedL); + double pz = helix.getZ(expectedL); + double nx = 0.7; + double ny = Math.sqrt(1.0 - nx * nx); + + double full = helix.getLAtPlane3D(px, py, pz, nx, ny, 0.0, px, py); + double projected = helix.getLAtPlane3D( + px, py, pz, nx, ny, 0.0, px, py); + + assertTrue(Double.isFinite(full)); + assertEquals(expectedL, full, 1.0e-7); + assertEquals(projected, full, 0.0); + } + + private static double distanceToAxis(Helix helix, double l, + double ax, double ay, double az, + double ux, double uy, double uz) { + double un = Math.sqrt(ux * ux + uy * uy + uz * uz); + ux /= un; + uy /= un; + uz /= un; + double wx = helix.getX(l) - ax; + double wy = helix.getY(l) - ay; + double wz = helix.getZ(l) - az; + double along = wx * ux + wy * uy + wz * uz; + double dx = wx - along * ux; + double dy = wy - along * uy; + double dz = wz - along * uz; + return Math.sqrt(dx * dx + dy * dy + dz * dz); + } + + @Test + public void full3DCylinderIntersectionUsesDisplacedTiltedAxis() { + double ax = 2.0; + double ay = -1.5; + double az = -300.0; + double ux = 0.003; + double uy = -0.004; + double uz = 1.0; + + for (int turn : new int[] {-1, 1}) { + Helix helix = new Helix(0.2, 0.4, turn * 0.004, 20.0, 0.6, + turn, 5.0, 0.0, 0.0, Units.MM); + double expectedL = 70.0; + double radius = distanceToAxis(helix, expectedL, ax, ay, az, ux, uy, uz); + double legacyL = helix.getLAtR(radius); + double actualL = helix.getLAtCylinder3D( + ax, ay, az, ux, uy, uz, radius, legacyL); + + assertTrue("turn=" + turn, Double.isFinite(actualL)); + assertEquals(expectedL, actualL, 1.0e-5); + assertEquals(radius, + distanceToAxis(helix, actualL, ax, ay, az, ux, uy, uz), 1.0e-7); + assertTrue("the beam-axis crossing must miss the aligned cylinder", + Math.abs(distanceToAxis( + helix, legacyL, ax, ay, az, ux, uy, uz) - radius) > 0.1); + } + } + + @Test + public void full3DCylinderIntersectionIsIndependentOfAxisScaleAndOrigin() { + Helix helix = helix(); + double expectedL = 80.0; + double ax = -1.2; + double ay = 2.3; + double az = -250.0; + double ux = -0.002; + double uy = 0.005; + double uz = 1.0; + double radius = distanceToAxis(helix, expectedL, ax, ay, az, ux, uy, uz); + double seed = helix.getLAtR(radius); + + double nominal = helix.getLAtCylinder3D( + ax, ay, az, ux, uy, uz, radius, seed); + double scaledDirection = helix.getLAtCylinder3D( + ax, ay, az, 100.0 * ux, 100.0 * uy, 100.0 * uz, radius, seed); + double shiftedOrigin = helix.getLAtCylinder3D( + ax + 37.0 * ux, ay + 37.0 * uy, az + 37.0 * uz, + ux, uy, uz, radius, seed); + + assertEquals(expectedL, nominal, 1.0e-5); + assertEquals(nominal, scaledDirection, 1.0e-10); + assertEquals(nominal, shiftedOrigin, 1.0e-10); + } + + @Test + public void full3DCylinderMatchesClosedFormForBeamAxis() { + Helix helix = helix(); + double radius = Math.hypot(helix.getX(60.0), helix.getY(60.0)); + double legacy = helix.getLAtR(radius); + double full = helix.getLAtCylinder3D( + 0.0, 0.0, -500.0, 0.0, 0.0, 1000.0, radius, legacy); + + assertTrue(Double.isFinite(full)); + assertEquals(legacy, full, 0.0); + } + + @Test + public void full3DCylinderFallsBackToBracketAtStationarySeed() { + Helix helix = helix(); + double expectedAbsL = 60.0; + double radius = Math.hypot(helix.getX(expectedAbsL), helix.getY(expectedAbsL)); + + double actualL = helix.getLAtCylinder3D( + 0.0, 0.0, -500.0, 0.0, 0.0, 1000.0, radius, 0.0); + + assertTrue(Double.isFinite(actualL)); + assertEquals(expectedAbsL, Math.abs(actualL), 1.0e-5); + assertEquals(radius, Math.hypot( + helix.getX(actualL), helix.getY(actualL)), 1.0e-7); + } + + @Test + public void full3DCylinderRejectsDegenerateAxis() { + Helix helix = helix(); + double result = helix.getLAtCylinder3D( + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 150.0, 10.0); + assertTrue(Double.isNaN(result)); + } +} diff --git a/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/banks/HitReader.java b/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/banks/HitReader.java index 18ceecc6f0..01fb8a792e 100644 --- a/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/banks/HitReader.java +++ b/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/banks/HitReader.java @@ -146,13 +146,15 @@ public void fetch_BMTHits(DataEvent event, Swim swim, IndexedTable status, double hv1 = bmtStripVoltageThresh.getDoubleValue("HV1", sector,layer,0); double hv2 = bmtStripVoltageThresh.getDoubleValue("HV2", sector,layer,0); double hv3 = bmtStripVoltageThresh.getDoubleValue("HV3", sector,layer,0); - - if(hv=hv1 && hv=hv2 && hv=hv1 && hv=hv2 && hv0 && currentStatus<5) + return currentStatus; + if(hvStatus>0 && hvStatus<5) + return hvStatus; + if(currentStatus!=0) + return currentStatus; + return hvStatus; + } + /** * Gets the SVT hits from the BMT dgtz bank * diff --git a/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/banks/RecoBankWriter.java b/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/banks/RecoBankWriter.java index 64388e3aac..40ca839128 100644 --- a/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/banks/RecoBankWriter.java +++ b/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/banks/RecoBankWriter.java @@ -543,6 +543,9 @@ public static DataBank fillTrackBank(DataEvent event, List trkcands, Stri // | 0 0 0 d_Z0*d_Z0 | // | 0 0 0 0 d_tandip*d_tandip |X double[][] covmatrix = helix.getCovMatrix(); + if (!trkcands.get(i).isCovFromFilteredFit()) { + covmatrix = null; + } if (covmatrix != null) { bank.setFloat(CVTRec_Tracks.cov_d02, i, (float) covmatrix[0][0]/10/10 ); bank.setFloat(CVTRec_Tracks.cov_d0phi0, i, (float) covmatrix[0][1]/10 ); @@ -616,6 +619,9 @@ public static DataBank fillUTrackBank(DataEvent event, List trkcands, Str // | 0 0 0 d_Z0*d_Z0 | // | 0 0 0 0 d_tandip*d_tandip |X double[][] covmatrix = helix.getCovMatrix(); + if (!trkcands.get(i).isCovFromFilteredFit()) { + covmatrix = null; + } if (covmatrix != null) { bank.setFloat(CVTRec_UTracks.cov_d02, i, (float) covmatrix[0][0]/10/10 ); bank.setFloat(CVTRec_UTracks.cov_d0phi0, i, (float) covmatrix[0][1]/10 ); diff --git a/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/cluster/ClusterFinder.java b/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/cluster/ClusterFinder.java index 7213366c6a..02ed50e9ed 100644 --- a/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/cluster/ClusterFinder.java +++ b/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/cluster/ClusterFinder.java @@ -26,6 +26,9 @@ public HitMapPool() { @Override public Hit[][][] poll() { Hit[][][] x = super.poll(); + if (x == null) { + return new Hit[NSTRIP][NLAYR][NSEC]; + } for (int i=0; i findClusters(List hits2) // the number of int l = hit.getLayer(); int s = hit.getSector(); - if (w > 0 && w < NSTRIP) { - HitArray[w - 1][l - 1][s - 1] = hit; + if (w < 1 || w > NSTRIP || l < 1 || l > NLAYR || s < 1 || s > NSEC) { + throw new IllegalArgumentException(String.format( + "Hit outside ClusterFinder bounds: strip=%d layer=%d sector=%d", + w, l, s)); } + HitArray[w - 1][l - 1][s - 1] = hit; } int cid = 1; // cluster id, will increment with each new good cluster @@ -84,7 +90,8 @@ public static ArrayList findClusters(List hits2) // the number of // adding all hits in this and all the subsequent // strip until there's a strip with no hit - while ((si < NSTRIP - 1 && HitArray[si + 1][l][s] != null) || (HitArray[si][l][s] != null && si < NSTRIP)) { + while ((si < NSTRIP - 1 && HitArray[si + 1][l][s] != null) + || (si < NSTRIP && HitArray[si][l][s] != null)) { if (HitArray[si][l][s] != null) { // continue clustering skipping over bad hit if (this_cluster == null) { Hit seed = HitArray[si][l][s]; diff --git a/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/track/Track.java b/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/track/Track.java index d6d47f2280..6515675d9f 100644 --- a/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/track/Track.java +++ b/reconstruction/cvt/src/main/java/org/jlab/rec/cvt/track/Track.java @@ -60,6 +60,9 @@ public class Track extends Trajectory implements Comparable { private double secondaryChi2; // for track with no beamSpot information private int secondaryNDF; // for track with no beamSpot information private int _status = 0; + // Only a filtered Kalman fit has a posterior covariance. The no-filter fail-safe retains + // the seed prior and must not publish it as a fitted uncertainty. + private boolean covFromFilteredFit = false; public Track(Helix helix) { @@ -90,7 +93,9 @@ public Track(Seed seed, KFitter kf) { this.setSeed(seed); this.addAll(seed.getCrosses()); this.setKFTrajectories(kf.trajPoints); - if (kf.filterOn && kf.setFitFailed == false && kf.NDF>0 && kf.getHelix()!=null) { + this.covFromFilteredFit = kf.filterOn && kf.setFitFailed == false + && kf.NDF > 0 && kf.getHelix() != null; + if (this.covFromFilteredFit) { this.setStatus(1); } else { this.setStatus(-1); @@ -444,6 +449,15 @@ public final void setKFTrajectories(Map trajectory) { public int getStatus() { return _status; } + + /** + * Returns true only when this track covariance is the posterior of a successful filtered + * Kalman fit. A false value includes seed tracks and the unfiltered fail-safe path. + */ + public boolean isCovFromFilteredFit() { + return covFromFilteredFit; + } + public void setStatus(int passKFFlag) { //for status word: int nSVT = 0; diff --git a/reconstruction/cvt/src/test/java/org/jlab/rec/cvt/banks/HitReaderTest.java b/reconstruction/cvt/src/test/java/org/jlab/rec/cvt/banks/HitReaderTest.java new file mode 100644 index 0000000000..7736ee7a41 --- /dev/null +++ b/reconstruction/cvt/src/test/java/org/jlab/rec/cvt/banks/HitReaderTest.java @@ -0,0 +1,23 @@ +package org.jlab.rec.cvt.banks; + +import org.junit.Test; +import static org.junit.Assert.assertEquals; + +public class HitReaderTest { + + @Test + public void rejectingStatusTakesPrecedenceOverHvAnnotation() { + assertEquals(2, HitReader.mergeBmtStatus(2, 6)); + assertEquals(3, HitReader.mergeBmtStatus(3, 5)); + assertEquals(4, HitReader.mergeBmtStatus(0, 4)); + assertEquals(4, HitReader.mergeBmtStatus(6, 4)); + } + + @Test + public void nonRejectingHvStatusIsKeptForOtherwiseGoodHit() { + assertEquals(0, HitReader.mergeBmtStatus(0, 0)); + assertEquals(5, HitReader.mergeBmtStatus(0, 5)); + assertEquals(6, HitReader.mergeBmtStatus(0, 6)); + assertEquals(5, HitReader.mergeBmtStatus(5, 6)); + } +} diff --git a/reconstruction/cvt/src/test/java/org/jlab/rec/cvt/banks/RecoBankWriterTest.java b/reconstruction/cvt/src/test/java/org/jlab/rec/cvt/banks/RecoBankWriterTest.java new file mode 100644 index 0000000000..b8d9b11ba3 --- /dev/null +++ b/reconstruction/cvt/src/test/java/org/jlab/rec/cvt/banks/RecoBankWriterTest.java @@ -0,0 +1,68 @@ +package org.jlab.rec.cvt.banks; + +import java.nio.file.Files; +import java.nio.file.Path; +import java.nio.file.Paths; +import java.util.ArrayList; +import java.util.Collections; +import org.jlab.analysis.physics.TestEvent; +import org.jlab.detector.base.DetectorType; +import org.jlab.io.base.DataBank; +import org.jlab.io.base.DataEvent; +import org.jlab.rec.cvt.track.Seed; +import org.jlab.rec.cvt.track.Track; +import org.jlab.rec.cvt.trajectory.Helix; +import org.junit.Test; +import static org.junit.Assert.assertEquals; +import static org.junit.Assert.assertFalse; + +public class RecoBankWriterTest { + + private static Path repositoryRoot() { + Path root = Paths.get(System.getProperty("user.dir")).toAbsolutePath(); + while (root != null && !Files.isDirectory(root.resolve("etc/bankdefs/hipo4"))) { + root = root.getParent(); + } + if (root == null) { + throw new IllegalStateException("Cannot locate etc/bankdefs/hipo4 from the test directory"); + } + return root; + } + + @Test + public void seedCovarianceIsNotReportedAsFilteredFitCovariance() { + double[][] covariance = new double[5][5]; + for (int i = 0; i < covariance.length; i++) { + covariance[i][i] = i + 1; + } + Helix helix = new Helix(0.1, 0.2, 0.01, 0.3, 0.4, 0, 0, covariance); + Seed seed = new Seed(new ArrayList<>(), helix); + seed.setId(7); + seed.setStatus(1); + Track track = new Track(seed); + track.setId(1); + + assertFalse(track.isCovFromFilteredFit()); + + String originalClas12Dir = System.getProperty("CLAS12DIR"); + DataEvent event; + try { + System.setProperty("CLAS12DIR", repositoryRoot().toString()); + event = TestEvent.get(DetectorType.CVT); + } finally { + if (originalClas12Dir == null) { + System.clearProperty("CLAS12DIR"); + } else { + System.setProperty("CLAS12DIR", originalClas12Dir); + } + } + DataBank bank = RecoBankWriter.fillTrackBank( + event, Collections.singletonList(track), "CVTRec::Tracks"); + + assertEquals(-999.0f, bank.getFloat("cov_d02", 0), 0.0f); + assertEquals(-999.0f, bank.getFloat("cov_phi02", 0), 0.0f); + assertEquals(-999.0f, bank.getFloat("cov_rho2", 0), 0.0f); + assertEquals(-999.0f, bank.getFloat("cov_z02", 0), 0.0f); + assertEquals(-999.0f, bank.getFloat("cov_tandip2", 0), 0.0f); + } +} diff --git a/reconstruction/cvt/src/test/java/org/jlab/rec/cvt/cluster/ClusterFinderTest.java b/reconstruction/cvt/src/test/java/org/jlab/rec/cvt/cluster/ClusterFinderTest.java new file mode 100644 index 0000000000..6a41d88ff5 --- /dev/null +++ b/reconstruction/cvt/src/test/java/org/jlab/rec/cvt/cluster/ClusterFinderTest.java @@ -0,0 +1,42 @@ +package org.jlab.rec.cvt.cluster; + +import java.util.Arrays; +import java.util.List; +import org.jlab.detector.base.DetectorType; +import org.jlab.geom.prim.Line3D; +import org.jlab.geom.prim.Vector3D; +import org.jlab.rec.cvt.bmt.BMTType; +import org.jlab.rec.cvt.hit.Hit; +import org.jlab.rec.cvt.hit.Strip; +import static org.junit.Assert.assertEquals; +import org.junit.Test; + +public class ClusterFinderTest { + + private static Hit hit(int strip, double energy) { + Strip sensorStrip = new Strip(strip, energy, 0); + sensorStrip.setLine(new Line3D(0, strip, 0, 1, strip, 0)); + sensorStrip.setNormal(new Vector3D(0, 0, 1)); + return new Hit(DetectorType.BST, BMTType.UNDEFINED, 1, 1, sensorStrip); + } + + @Test + public void includesHighestRepresentableStrip() { + Hit edge = hit(ClusterFinder.NSTRIP, 10); + List clusters = ClusterFinder.findClusters(Arrays.asList(edge)); + + assertEquals(1, clusters.size()); + assertEquals(1, clusters.get(0).size()); + assertEquals(ClusterFinder.NSTRIP, clusters.get(0).get(0).getStrip().getStrip()); + assertEquals(1, edge.getAssociatedClusterID()); + } + + @Test + public void poolExhaustionFallsBackToAnAllocatedMap() { + ClusterFinder.HitMapPool localPool = new ClusterFinder.HitMapPool(); + for (int i = 0; i < 65; i++) { + Hit[][][] map = localPool.poll(); + assertEquals(ClusterFinder.NSTRIP, map.length); + } + } +}