From c8c7e331273ac918efd71de0d8db980708b893d0 Mon Sep 17 00:00:00 2001 From: featurescript <114271535+featurescript@users.noreply.github.com> Date: Sat, 31 Oct 2026 16:40:28 -0700 Subject: [PATCH 1/4] Add seeded vision faults for stress tests; fix three edge bugs they found VisionFaults configures injectable, seeded, deterministic camera faults: heavier pixel and angle noise, extra latency and jitter, dropped, repeated and out-of-order frames, missed detections, blackouts, an occluding patch, false detections and wrong IDs or kinds. Each fault draws from its own seeded stream, so turning one on does not change another's draws. VisionFaultInjector applies the content faults to sightings, tag sightings and rendered frames; FaultedFrameStream applies the timing faults to frames of any type. FakeLimelight3A takes them via Builder.faults(VisionFaults); with none set, every output is unchanged. Bugs the faults exposed, each with a regression test: - FakeLimelight3A: heavy tx/ty noise could report a tag far outside the image (tx of -188 degrees). A noisy tag whose centre leaves the image is now dropped (heavyNoiseNeverReportsATagOutsideTheImage). - SimCamera.groundPointInRobotFrame: on a lens whose distortion folds over, a pixel past the fold returned a mirrored floor point. It now returns null when the undistorted ray does not project back to the pixel (aPixelNoRayReachesHasNoGroundPoint). - SimFrameRenderer: corner pixels past such a fold were drawn along the mirrored ray. They are now black and unlabelled (cornersPastAFoldingLensesReachAreBlackAndUnlabelled). --- SimLoop/docs/packages/fakehardware.md | 8 +- SimLoop/docs/packages/vision.md | 48 +- .../simloop/fakehardware/FakeLimelight3A.java | 130 ++++- .../simloop/vision/CameraLens.java | 5 + .../simloop/vision/FaultedFrameStream.java | 278 ++++++++++ .../simloop/vision/SimCamera.java | 16 +- .../simloop/vision/SimFrameRenderer.java | 53 +- .../simloop/vision/VisionFaultInjector.java | 427 ++++++++++++++++ .../simloop/vision/VisionFaults.java | 478 ++++++++++++++++++ .../FakeLimelight3AFaultsTest.java | 216 ++++++++ .../vision/CameraLensDistortionTest.java | 23 + .../simloop/vision/SimFrameRendererTest.java | 27 + .../simloop/vision/VisionFaultsTest.java | 478 ++++++++++++++++++ 13 files changed, 2158 insertions(+), 29 deletions(-) create mode 100644 SimLoop/src/main/java/org/horizon36596/simloop/vision/FaultedFrameStream.java create mode 100644 SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaultInjector.java create mode 100644 SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaults.java create mode 100644 SimLoop/src/test/java/org/horizon36596/simloop/fakehardware/FakeLimelight3AFaultsTest.java create mode 100644 SimLoop/src/test/java/org/horizon36596/simloop/vision/VisionFaultsTest.java diff --git a/SimLoop/docs/packages/fakehardware.md b/SimLoop/docs/packages/fakehardware.md index bfabc0d..3fbf2f5 100644 --- a/SimLoop/docs/packages/fakehardware.md +++ b/SimLoop/docs/packages/fakehardware.md @@ -131,7 +131,13 @@ as usual and gets `botpose`, `getBotpose_MT2()` and fiducial results. Staleness time. Optional on the builder: seeded noise (and the matching `getStddevMt1/Mt2`), seeded dropouts, latency, a frame rate, lens distortion (`distortion(k1, k2, p1, p2, k3)`, which moves corner and centre pixels) and the robot's own body as boxes (`robotBody(...)`). The geometry is [`vision`](vision.md)'s -`SimTagCamera`, so tilted tags in the layout work too. +`SimTagCamera`, so tilted tags in the layout work too. Noise that would put a tag's centre outside the +image drops that tag from the frame instead of reporting an impossible `tx`. + +For stress tests, `faults(VisionFaults)` adds [`vision`](vision.md)'s fault injection: blackouts, missed, +misread and false tags, extra latency and jitter, and dropped or out-of-order frames. With faults on, +`botpose` is solved from the tags seen, so a misread tag moves it. The extra latency is not reported in +`cl` or `tl`. **Your test JVM needs the real `org.json`:** `testRuntimeOnly "org.json:json:20250517"`. Android's copy is a stub. diff --git a/SimLoop/docs/packages/vision.md b/SimLoop/docs/packages/vision.md index 29058d2..5f9f820 100644 --- a/SimLoop/docs/packages/vision.md +++ b/SimLoop/docs/packages/vision.md @@ -35,6 +35,9 @@ A simulated camera on the robot, at two levels: | `SimFrame` | One synthetic frame: RGB bytes, a ground-truth piece and tag label per pixel, and a PNG writer. | | `FrameStyle` | The flat colours and the one light frames are painted with. | | `RigidTransform3`, `RobotFramePoint` | A 3-D rotation plus translation, and a point on the floor in the robot frame. | +| `VisionFaults` | Faults to inject for a stress test: noise, latency, jitter, drops, repeats, reordering, occlusion, false detections, wrong ids. | +| `VisionFaultInjector` | Applies the content faults to one frame of sightings, tags or pixels. | +| `FaultedFrameStream` | Applies the timing faults to a stream of frames of any kind. | `FakeLimelight3A` in [`fakehardware`](fakehardware.md) is built on `SimTagCamera`, so a Limelight and a plain camera share one description. @@ -187,6 +190,44 @@ pixels. *A synthetic frame from `SimFrameRendererTest`: 320 x 240, two pieces, a wall box and a tag. Flat colours, one light, no shadows. Not a camera image.* +### Faults for stress tests + +To check what your vision code does when the camera misbehaves, describe the faults once and apply them +to each frame. Every fault is seeded, so a failing run replays exactly, and each fault draws from its own +stream, so turning one on does not change another's draws. Every rate is a test input you choose, not a +measured camera. + +```java +VisionFaults faults = VisionFaults.builder(36596) + .pixelNoise(8.0, 3.0) // extra centre and radius noise, px + .latency(0.120, 0.040) // 120 ms extra, up to 40 ms jitter either way + .dropFrames(0.10) + .repeatFrames(0.05) // same capture stamp delivered twice + .reorderFrames(0.05) // capture stamps go backwards + .missDetections(0.20) // partial occlusion + .blackout(0.01, 15) // full occlusion, 15 frames at a time + .falseDetections(0.10, "ball", 4.0, 20.0, 0xF0D020) + .wrongIds(0.05, "red", "blue") + .build(); +VisionFaultInjector injector = new VisionFaultInjector(faults); +FaultedFrameStream> stream = new FaultedFrameStream<>(faults); + +// each capture: +stream.offer(captureNanos, captureNanos + yourLatencyNanos, + injector.applyToSightings(camera.observe(pieces, x, y, heading), camera.getLens())); +// each loop: +for (FaultedFrameStream.Delivery> d : stream.pollDue(nowNanos)) { + yourPipeline.onFrame(d.getFrame(), d.getCaptureNanos()); +} +``` + +`applyToTags` and `applyToFrame` do the same for `TagSighting`s and synthetic frames. A false piece +sighting has piece id `VisionFaultInjector.FALSE_PIECE_ID` (-1, "no piece"); a misread or false tag names +the layout tag it claims to be while its transforms are what was actually seen, so a pose solved from it is +wrong the way it would be on a robot. No fault produces a non-finite number: a sighting noise pushes off +the image, or to a radius of zero or less, is dropped. `FakeLimelight3A.Builder.faults(...)` takes the same +description. + ## Units and frames - Robot pose, piece positions, tag positions, field boxes: field frame, FTC-Cartesian, centre origin, @@ -249,8 +290,11 @@ So: FIRST field CAD: its licence forbids redistribution. - **No noise in `SimCamera` or `SimTagCamera`.** `FakeLimelight3A` adds seeded noise and dropouts; `SimFrameRenderer` adds seeded pixel noise. A team's own sim glue adds detection noise on top of - `SimCamera` if it wants it. + `SimCamera` if it wants it, or uses `VisionFaultInjector`. - **No OpenCV.** SimLoop hands you bytes; your test turns them into a `Mat`. - **No frame timing.** `SimCamera` and `SimFrameRenderer` see the scene at the instant you call them. Frame rate, exposure latency and processing delay are your sim glue's job (`FakeLimelight3A` has its - own). + own). `FaultedFrameStream` adds faulty timing on top of whatever your glue does. +- **A folding lens images only inside its fold.** With distortion strong enough that + `getMaxValidRadius()` is finite, pixels past the fold have no ray: `groundPointInRobotFrame` returns + `null` for them and `SimFrameRenderer` draws them black and unlabelled. diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/fakehardware/FakeLimelight3A.java b/SimLoop/src/main/java/org/horizon36596/simloop/fakehardware/FakeLimelight3A.java index 169e87b..cd10807 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/fakehardware/FakeLimelight3A.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/fakehardware/FakeLimelight3A.java @@ -13,9 +13,12 @@ import org.horizon36596.simloop.vision.CameraLens; import org.horizon36596.simloop.vision.CameraOccluders; import org.horizon36596.simloop.vision.CameraMount; +import org.horizon36596.simloop.vision.FaultedFrameStream; import org.horizon36596.simloop.vision.RigidTransform3; import org.horizon36596.simloop.vision.SimTagCamera; import org.horizon36596.simloop.vision.TagSighting; +import org.horizon36596.simloop.vision.VisionFaultInjector; +import org.horizon36596.simloop.vision.VisionFaults; import org.json.JSONArray; import org.json.JSONException; import org.json.JSONObject; @@ -112,6 +115,22 @@ * visibility rule is missed in a frame with a fixed probability, drawn from its own {@link SeededRandom} in * tag id order, so a pose filter can be tested against frames that lose a tag now and then. * + *

Noise and the image edge. A tag whose noisy {@code tx}/{@code ty} put its centre outside the + * image is not reported in that frame, as a detector cannot report a target it does not see; the noise + * draws are the same either way, so this changes nothing for a tag well inside the image. + * + *

Faults (off unless {@link Builder#faults} is called): a {@link VisionFaults} for stress tests. + * Its content faults ({@link VisionFaultInjector}: blackouts, missed tags, extra tx/ty noise, wrong ids and + * false tags) act on the tags seen, after the dropouts above; a wrong or false tag moves both + * {@code botpose} and {@code botpose_orb}, because with faults on {@code botpose} is solved from the tags + * seen (each tag's robot-to-tag transform placed at the layout tag it claims to be, averaged) instead of + * read from the true pose. Its timing faults ({@link FaultedFrameStream}: extra latency and jitter, dropped + * and out-of-order frames) act on when a frame becomes the device's current result; the extra delay is + * not added to {@code cl} or {@code tl}, so it models delay the device does not report. The device + * serves the last frame delivered in a tick, so an out-of-order frame shows (its {@code ts} goes backwards) + * only on a tick with no newer frame, which needs a {@link Builder#frameRateHz} below the tick rate; a + * repeated frame is the frame already being served, so it changes nothing a poll can see. + * *

Optics and occlusion. {@link Builder#distortion} gives the lens OpenCV's distortion model: * corner pixels ({@code pts}) and the centre pixel ({@code txp}, {@code typ}) are distorted, and the angles * and poses stay the undistorted geometry a calibrated Limelight reports. {@link Builder#occluders} takes @@ -156,6 +175,9 @@ public class FakeLimelight3A extends Limelight3A implements FakeDevice { private final CameraOccluders robotBody; private final SeededRandom dropoutRandom; private final double dropoutProbability; + /** Null when no faults were asked for, so a fault-free fake runs exactly the code it always did. */ + private final VisionFaultInjector faultInjector; + private final FaultedFrameStream faultStream; private long simNanos; private boolean running; @@ -204,6 +226,8 @@ private FakeLimelight3A(Builder b) { this.robotBody = CameraOccluders.of(Collections.emptyList(), b.robotBody); this.dropoutRandom = b.dropoutProbability > 0.0 ? new SeededRandom(b.dropoutSeed) : null; this.dropoutProbability = b.dropoutProbability; + this.faultInjector = b.faults.isNone() ? null : new VisionFaultInjector(b.faults); + this.faultStream = b.faults.isNone() ? null : new FaultedFrameStream(b.faults); } /** 127.0.0.1, built without a DNS lookup: if an SDK call ever slipped past this fake, it hits nothing. */ @@ -279,7 +303,11 @@ public void update(double deltaTime) { simNanos += Math.round(deltaTime * 1e9); if (framePeriodNanos == 0L || simNanos >= nextCaptureNanos) { - inFlight.addLast(new Frame(simNanos + latencyNanos, buildFrameJson(simNanos))); + if (faultStream != null) { + faultStream.offer(simNanos, simNanos + latencyNanos, buildFrameJson(simNanos)); + } else { + inFlight.addLast(new Frame(simNanos + latencyNanos, buildFrameJson(simNanos))); + } if (framePeriodNanos > 0L) { // The schedule starts at the first capture; a tick longer than a frame skips frames. nextCaptureNanos = capturedOnce ? nextCaptureNanos + framePeriodNanos : simNanos + framePeriodNanos; @@ -292,6 +320,11 @@ public void update(double deltaTime) { while (!inFlight.isEmpty() && inFlight.peekFirst().availableAtNanos <= simNanos) { currentFrameJson = inFlight.pollFirst().json; } + if (faultStream != null) { + for (FaultedFrameStream.Delivery delivery : faultStream.pollDue(simNanos)) { + currentFrameJson = delivery.getFrame(); + } + } if (running && simNanos >= nextPollNanos) { latestResult = SimLLResult.parse(currentFrameJson, nowMs(), this); polledOnce = true; @@ -329,6 +362,11 @@ public AprilTagLayout getLayout() { return layout; } + /** {@return the frame-timing fault stream, for its counters; null when no faults were configured} */ + public FaultedFrameStream getFaultStream() { + return faultStream; + } + private long nowMs() { return simNanos / NANOS_PER_MS; } @@ -640,6 +678,38 @@ private String frameJson(long captureNanos) throws JSONException { } seen = kept; } + if (faultInjector != null) { + seen = faultInjector.applyToTags(seen, layout, camera, robotXInches, robotYInches, robotHeadingRad); + } + // Noise first, in tag order, two draws a tag as always; a tag the noise pushes out of the image is + // not reported, because a detector cannot report a target outside its image. + CameraLens lens = camera.getLens(); + List reported = new ArrayList(seen.size()); + List reportedAngles = new ArrayList(seen.size()); + for (TagSighting s : seen) { + double tx = s.getTxDegrees() + gaussian(txTyStdDevDegrees); + double ty = s.getTyDegrees() + gaussian(txTyStdDevDegrees); + double[] centrePx; + if (tx == s.getTxDegrees() && ty == s.getTyDegrees()) { + centrePx = lens.project(StrictMath.tan(StrictMath.toRadians(tx)), + -StrictMath.tan(StrictMath.toRadians(ty)), 1.0); + if (centrePx == null) { + centrePx = new double[] {s.getCentreXPx(), s.getCentreYPx()}; + } + } else { + centrePx = Math.abs(tx) < 90.0 && Math.abs(ty) < 90.0 + ? lens.project(StrictMath.tan(StrictMath.toRadians(tx)), + -StrictMath.tan(StrictMath.toRadians(ty)), 1.0) + : null; + if (centrePx == null || centrePx[0] < 0.0 || centrePx[0] > lens.getWidthPx() + || centrePx[1] < 0.0 || centrePx[1] > lens.getHeightPx()) { + continue; + } + } + reported.add(s); + reportedAngles.add(new double[] {tx, ty, centrePx[0], centrePx[1]}); + } + seen = reported; JSONObject json = new JSONObject(); json.put("pID", pipelineIndex); json.put("pipelineType", PIPELINE_TYPE); @@ -664,10 +734,14 @@ private String frameJson(long captureNanos) throws JSONException { double yawRad = StrictMath.toRadians(suppliedYawDegrees); double cosYaw = StrictMath.cos(yawRad); double sinYaw = StrictMath.sin(yawRad); - CameraLens lens = camera.getLens(); - for (TagSighting s : seen) { - double tx = s.getTxDegrees() + gaussian(txTyStdDevDegrees); - double ty = s.getTyDegrees() + gaussian(txTyStdDevDegrees); + double mt1X = 0.0; + double mt1Y = 0.0; + double mt1Cos = 0.0; + double mt1Sin = 0.0; + for (int i = 0; i < seen.size(); i++) { + TagSighting s = seen.get(i); + double tx = reportedAngles.get(i)[0]; + double ty = reportedAngles.get(i)[1]; JSONObject f = new JSONObject(); f.put("fID", s.getId()); f.put("fam", family); @@ -675,13 +749,8 @@ private String frameJson(long captureNanos) throws JSONException { f.put("ty", ty); f.put("tx_nocross", tx); f.put("ty_nocross", ty); - double[] centrePx = lens.project(StrictMath.tan(StrictMath.toRadians(tx)), - -StrictMath.tan(StrictMath.toRadians(ty)), 1.0); - if (centrePx == null) { - centrePx = new double[] {s.getCentreXPx(), s.getCentreYPx()}; - } - f.put("txp", centrePx[0]); - f.put("typ", centrePx[1]); + f.put("txp", reportedAngles.get(i)[2]); + f.put("typ", reportedAngles.get(i)[3]); f.put("ta", s.getAreaPercent()); JSONArray corners = new JSONArray(); for (double[] corner : s.getCornersPx()) { @@ -709,6 +778,15 @@ private String frameJson(long captureNanos) throws JSONException { mt2X += tag.getXInches() - (cosYaw * r.getXInches() - sinYaw * r.getYInches()); mt2Y += tag.getYInches() - (sinYaw * r.getXInches() + cosYaw * r.getYInches()); mt2Z += tag.getZInches() - r.getZInches(); + if (faultInjector != null) { + // MegaTag1 as a solver does it: the robot is wherever puts this tag, as seen, at the layout + // tag it claims to be. A correct tag gives the true pose; a wrong or false one does not. + RigidTransform3 solved = tag.getFieldToTag().compose(r.inverse()); + mt1X += solved.getXInches(); + mt1Y += solved.getYInches(); + mt1Cos += StrictMath.cos(solved.getYawRad()); + mt1Sin += StrictMath.sin(solved.getYawRad()); + } } json.put("Fiducial", fiducials); int count = seen.size(); @@ -731,11 +809,14 @@ private String frameJson(long captureNanos) throws JSONException { json.put("botpose_avgarea", sumArea / count); json.put("botpose_span", spanInches(seen) * METRES_PER_INCH); - double mt1X = robotXInches + gaussian(positionStdDevInches); - double mt1Y = robotYInches + gaussian(positionStdDevInches); - double mt1Yaw = wrapDegrees(StrictMath.toDegrees(robotHeadingRad) + gaussian(yawStdDevDegrees)); + double mt1BaseX = faultInjector != null ? mt1X / count : robotXInches; + double mt1BaseY = faultInjector != null ? mt1Y / count : robotYInches; + double mt1BaseYawRad = faultInjector != null ? StrictMath.atan2(mt1Sin, mt1Cos) : robotHeadingRad; + double botX = mt1BaseX + gaussian(positionStdDevInches); + double botY = mt1BaseY + gaussian(positionStdDevInches); + double mt1Yaw = wrapDegrees(StrictMath.toDegrees(mt1BaseYawRad) + gaussian(yawStdDevDegrees)); json.put("botpose", toJson(new double[] { - mt1X * METRES_PER_INCH, mt1Y * METRES_PER_INCH, 0.0, 0.0, 0.0, mt1Yaw})); + botX * METRES_PER_INCH, botY * METRES_PER_INCH, 0.0, 0.0, 0.0, mt1Yaw})); double orbX = mt2X / count + gaussian(positionStdDevInches); double orbY = mt2Y / count + gaussian(positionStdDevInches); json.put("botpose_orb", toJson(new double[] { @@ -890,6 +971,7 @@ public static final class Builder { private List robotBody = Collections.emptyList(); private int dropoutSeed; private double dropoutProbability; + private VisionFaults faults = VisionFaults.none(); private Builder(AprilTagLayout layout, CameraMount mount, double horizontalFovDegrees, double verticalFovDegrees) { @@ -1038,6 +1120,22 @@ public Builder dropout(int seed, double probability) { return this; } + /** + * Faults for stress-testing the robot's vision code: what the class comment's Faults paragraph + * lists. Default {@link VisionFaults#none()}. Every rate in it is a test input, not a Limelight + * specification. + * + * @param visionFaults the faults; {@link VisionFaults#none()} turns them off + * @return this builder + */ + public Builder faults(VisionFaults visionFaults) { + if (visionFaults == null) { + throw new IllegalArgumentException("faults must not be null; use VisionFaults.none()"); + } + this.faults = visionFaults; + return this; + } + /** * How often the camera captures, in sim time. Default 0, meaning once per {@code update} tick. * diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/CameraLens.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/CameraLens.java index 498166f..5e102e3 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/vision/CameraLens.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/CameraLens.java @@ -210,6 +210,11 @@ public double[] distortNormalized(double x, double y) { * pipeline does before casting a ray through a pixel. Newton's method on the distortion model, started * from the distorted point; deterministic. * + *

A lens whose distortion folds over ({@link #getMaxValidRadius()} finite) images only pixels inside + * the fold. For a pixel beyond it no direction exists, and the result is wherever the search stopped, + * which does not project back to the pixel. Check with {@link #project} when the pixel may lie there; + * {@link SimCamera#groundPointInRobotFrame} does. + * * @param xPx pixel x, from the left edge * @param yPx pixel y, from the top edge * @return {@code {X / Z, Y / Z}} of the ray through the pixel, optical frame diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/FaultedFrameStream.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/FaultedFrameStream.java new file mode 100644 index 0000000..088b9d9 --- /dev/null +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/FaultedFrameStream.java @@ -0,0 +1,278 @@ +package org.horizon36596.simloop.vision; + +import org.horizon36596.simloop.field.SeededRandom; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +/** + * The timing faults of a {@link VisionFaults}, for any kind of frame: extra latency and jitter, dropped + * frames, repeated frames (the same capture stamp delivered twice) and frames delivered out of capture order. + * The frame contents are whatever the caller hands in ({@code T}: a list of sightings, a {@link SimFrame}, + * a JSON string) and are never looked at; {@link VisionFaultInjector} changes contents. + * + *

How to drive it. On each capture, {@link #offer} the frame with its capture time and the time the + * caller's own model would deliver it (capture plus that model's latency; the capture time itself for none). + * On each loop, {@link #pollDue} takes every frame whose delivery time has come, in delivery order. All times + * are one clock, nanoseconds; sim time, never the wall clock. + * + *

The rules, per offered frame, each fault drawn from its own seeded stream: + *

    + *
  1. Drop: the frame is never delivered.
    2. + *
  2. Latency: delivery is the caller's time plus {@link VisionFaults#getExtraLatencySeconds()} plus + * a uniform shift of up to {@link VisionFaults#getLatencyJitterSeconds()} either way, never before the + * capture. A frame never overtakes the one offered before it because of jitter: it waits for it.
    3. + *
  3. Reorder: the frame is held back until the next frame offered has been delivered, and comes at + * the start of the next {@link #pollDue} call after that, so its capture stamp is older than the one + * delivered before it. Two held in a row come out newest first. A held frame with no next frame is + * never delivered.
    4. + *
  4. Repeat: after delivery, the same frame (same stamp, same contents object) is delivered once + * more, at the start of the next {@link #pollDue} call that is at least one nanosecond later.
    5. + *
+ * + *

A late frame (reordered or repeated) comes at the start of a poll, before the frames newly due in it. + * A consumer that keeps only the newest delivery of each poll sees it only on a poll with nothing newer. + * + *

Deterministic: the same faults and the same sequence of calls give the same deliveries. Not thread-safe. + * + * @param what a frame carries + */ +public final class FaultedFrameStream { + + /** + * One delivered frame. Immutable apart from whatever {@code T} is. + * + * @param what a frame carries + */ + public static final class Delivery { + private final long captureNanos; + private final long deliveredNanos; + private final T frame; + private final boolean repeat; + private final boolean outOfOrder; + + Delivery(long captureNanos, long deliveredNanos, T frame, boolean repeat, boolean outOfOrder) { + this.captureNanos = captureNanos; + this.deliveredNanos = deliveredNanos; + this.frame = frame; + this.repeat = repeat; + this.outOfOrder = outOfOrder; + } + + /** {@return the frame's capture time, nanoseconds, as offered: the stamp a consumer is handed} */ + public long getCaptureNanos() { + return captureNanos; + } + + /** {@return when the frame became due, nanoseconds; at or before the poll that returned it} */ + public long getDeliveredNanos() { + return deliveredNanos; + } + + /** {@return what the frame carries, the object offered} */ + public T getFrame() { + return frame; + } + + /** {@return true for the second delivery of a repeated frame} */ + public boolean isRepeat() { + return repeat; + } + + /** {@return true when a frame captured later was delivered before this one} */ + public boolean isOutOfOrder() { + return outOfOrder; + } + } + + private static final class Pending { + final long captureNanos; + final long dueNanos; + final T frame; + final boolean outOfOrder; + /** A held-back frame waiting for this one to be delivered; null for none. */ + Pending follower; + + Pending(long captureNanos, long dueNanos, T frame, boolean outOfOrder) { + this.captureNanos = captureNanos; + this.dueNanos = dueNanos; + this.frame = frame; + this.outOfOrder = outOfOrder; + } + } + + private final VisionFaults faults; + private final SeededRandom dropRandom; + private final SeededRandom jitterRandom; + private final SeededRandom reorderRandom; + private final SeededRandom repeatRandom; + private final List> queue = new ArrayList>(); + private final List> repeats = new ArrayList>(); + /** Held-back frames whose leader has been delivered: they come at the start of the next poll. */ + private final List> late = new ArrayList>(); + private Pending heldBack; + private long lastDueNanos = Long.MIN_VALUE; + private long lastOfferedCaptureNanos = Long.MIN_VALUE; + private long lastPollNanos = Long.MIN_VALUE; + private long offered; + private long dropped; + private long delivered; + private long repeated; + private long reordered; + + /** + * A stream for these faults, at the start of its random streams, with nothing in flight. + * + * @param faults the timing faults to apply; {@link VisionFaults#none()} delivers every frame at the + * caller's own time, in order, once + */ + public FaultedFrameStream(VisionFaults faults) { + if (faults == null) { + throw new IllegalArgumentException("faults must not be null; use VisionFaults.none()"); + } + this.faults = faults; + this.dropRandom = VisionFaultInjector.stream(faults.getSeed(), 11); + this.jitterRandom = VisionFaultInjector.stream(faults.getSeed(), 12); + this.reorderRandom = VisionFaultInjector.stream(faults.getSeed(), 13); + this.repeatRandom = VisionFaultInjector.stream(faults.getSeed(), 14); + } + + /** + * Hand in one captured frame. + * + * @param captureNanos the capture time, nanoseconds; not before the last frame offered + * @param readyNanos when the caller's own model would deliver it, nanoseconds; not before the capture + * @param frame what the frame carries; may be null if {@code T} allows it + * @throws IllegalArgumentException when the capture time goes backwards or the frame would be ready + * before it was captured + */ + public void offer(long captureNanos, long readyNanos, T frame) { + if (captureNanos < lastOfferedCaptureNanos) { + throw new IllegalArgumentException("frames are offered in capture order; " + captureNanos + + " ns follows " + lastOfferedCaptureNanos + " ns. Use reorderFrames to deliver out of order"); + } + if (readyNanos < captureNanos) { + throw new IllegalArgumentException("a frame cannot be ready (" + readyNanos + + " ns) before it is captured (" + captureNanos + " ns)"); + } + lastOfferedCaptureNanos = captureNanos; + offered++; + if (faults.getDropProbability() > 0.0 && dropRandom.nextFraction() < faults.getDropProbability()) { + dropped++; + return; + } + long due = readyNanos + Math.round(faults.getExtraLatencySeconds() * 1e9); + double jitter = faults.getLatencyJitterSeconds(); + if (jitter > 0.0) { + due += Math.round(jitterRandom.nextBetween(-jitter, jitter) * 1e9); + } + due = Math.max(due, captureNanos); + due = Math.max(due, lastDueNanos); // in order: wait for the frame before + lastDueNanos = due; + + boolean holdBack = faults.getReorderProbability() > 0.0 + && reorderRandom.nextFraction() < faults.getReorderProbability(); + Pending previouslyHeld = heldBack; + Pending current = new Pending(captureNanos, due, frame, holdBack); + if (previouslyHeld != null) { + current.follower = previouslyHeld; // after this frame, whenever this frame comes out + reordered++; + } + if (holdBack) { + heldBack = current; + } else { + heldBack = null; + queue.add(current); + } + } + + /** + * Every frame due at {@code nowNanos}: first any repeats owed from an earlier poll, then the frames whose + * delivery time has come, in delivery order. + * + * @param nowNanos the current time, nanoseconds, same clock as {@link #offer} + * @return the deliveries, possibly empty, unmodifiable + */ + public List> pollDue(long nowNanos) { + List> out = new ArrayList>(); + if (nowNanos > lastPollNanos) { + // Held-back frames first, then repeats. Delivering a held frame can release the one held behind + // it; that one waits for the next poll, so each comes on its own poll. + List> lateNow = new ArrayList>(late); + late.clear(); + for (Pending p : lateNow) { + deliver(p, false, out); + } + for (Pending p : repeats) { + out.add(new Delivery(p.captureNanos, p.dueNanos, p.frame, true, p.outOfOrder)); + delivered++; + repeated++; + } + repeats.clear(); + } + lastPollNanos = Math.max(lastPollNanos, nowNanos); + // The queue is sorted by due time by construction: jitter never lets a frame overtake the one before. + while (!queue.isEmpty() && queue.get(0).dueNanos <= nowNanos) { + deliver(queue.remove(0), true, out); + } + return Collections.unmodifiableList(out); + } + + private void deliver(Pending p, boolean mayRepeat, List> out) { + out.add(new Delivery(p.captureNanos, p.dueNanos, p.frame, false, p.outOfOrder)); + delivered++; + if (p.follower != null) { + late.add(p.follower); + p.follower = null; + } + if (mayRepeat && faults.getRepeatProbability() > 0.0 + && repeatRandom.nextFraction() < faults.getRepeatProbability()) { + repeats.add(p); + } + } + + /** {@return frames waiting for their delivery time, a held-back frame included} */ + public int inFlight() { + int held = 0; + for (Pending p : queue) { + for (Pending f = p.follower; f != null; f = f.follower) { + held++; + } + } + for (Pending f = heldBack; f != null; f = f.follower) { + held++; + } + return queue.size() + held + late.size() + repeats.size(); + } + + /** {@return frames offered so far} */ + public long getOffered() { + return offered; + } + + /** {@return frames dropped so far} */ + public long getDropped() { + return dropped; + } + + /** {@return deliveries so far, repeats included} */ + public long getDelivered() { + return delivered; + } + + /** {@return second deliveries of repeated frames so far} */ + public long getRepeated() { + return repeated; + } + + /** {@return frames queued behind a later frame so far} */ + public long getReordered() { + return reordered; + } + + /** {@return the faults this stream applies} */ + public VisionFaults getFaults() { + return faults; + } +} diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimCamera.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimCamera.java index 0e73562..7ce648c 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimCamera.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimCamera.java @@ -468,7 +468,9 @@ public List getLastSightings() { * @param pointHeightInches the height of the plane the ray is intersected with — the ball's centre * height, i.e. its radius, for a ball on the floor * @return the point in the robot frame, or {@code null} when the ray never reaches that plane (it - * points at or above the horizon relative to the plane) + * points at or above the horizon relative to the plane), or when no ray through the lens lands on + * that pixel at all: past a distorted lens's fold-over radius, where undistorting would otherwise + * return a direction that projects somewhere else (often mirrored through the centre) */ public RobotFramePoint groundPointInRobotFrame(double centreXPx, double centreYPx, double pointHeightInches) { requireFinite(centreXPx, "centreXPx"); @@ -478,6 +480,15 @@ public RobotFramePoint groundPointInRobotFrame(double centreXPx, double centreYP double[] normalized = lens.undistortPixel(centreXPx, centreYPx); double xOverZ = normalized[0]; double yOverZ = normalized[1]; + if (lens.hasDistortion()) { + // Undistortion is a search: a pixel the lens cannot image has no answer, and the search then stops + // on a direction that does not come back to the pixel. Check the round trip; a noisy detection + // off the image corner of a strongly distorted lens is exactly that case. + double[] back = lens.project(xOverZ, yOverZ, 1.0); + if (back == null || Math.hypot(back[0] - centreXPx, back[1] - centreYPx) > UNDISTORT_TOLERANCE_PX) { + return null; + } + } double[] rayEnd = mount.opticalToRobot(xOverZ, yOverZ, 1.0); double[] rayStart = mount.opticalToRobot(0.0, 0.0, 0.0); double dForward = rayEnd[0] - rayStart[0]; @@ -608,6 +619,9 @@ public CameraMount getMount() { return mount; } + /** How far an undistorted ray may project from its pixel and still count as the pixel's ray, pixels. */ + private static final double UNDISTORT_TOLERANCE_PX = 1e-6; + private static void requireFinite(double value, String name) { if (!Double.isFinite(value)) { throw new IllegalArgumentException(name + " must be finite, got " + value); diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimFrameRenderer.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimFrameRenderer.java index 14487e6..db45ae2 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimFrameRenderer.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimFrameRenderer.java @@ -116,14 +116,14 @@ private SimFrameRenderer(Builder b) { for (int x = 0; x < width; x++) { int p = y * width + x; int tile = (y / TILE) * tilesX + x / TILE; - double[] c = lens.undistortPixel(x + 0.5, y + 0.5); + double[] c = rayThrough(lens, x + 0.5, y + 0.5); centreRayX[p] = c[0]; centreRayY[p] = c[1]; grow(tile, c[0], c[1]); if (n > 1) { for (int j = 0; j < n; j++) { for (int i = 0; i < n; i++) { - double[] r = lens.undistortPixel(x + (i + 0.5) / n, y + (j + 0.5) / n); + double[] r = rayThrough(lens, x + (i + 0.5) / n, y + (j + 0.5) / n); int s = p * n * n + j * n + i; subRayX[s] = r[0]; subRayY[s] = r[1]; @@ -140,7 +140,27 @@ private SimFrameRenderer(Builder b) { axisZRobot = minus(mount.opticalToRobot(0.0, 0.0, 1.0), lensRobot); } + /** + * The ray a pixel looks along, or {@code {NaN, NaN}} when no ray through the lens lands on it: past a + * distorted lens's fold-over radius, where undistorting returns a direction that projects somewhere else + * (often mirrored through the centre). Such a pixel sees nothing and is drawn black, unlabelled, as the + * dark corner outside a real lens's image circle is. + */ + private static double[] rayThrough(CameraLens lens, double xPx, double yPx) { + double[] ray = lens.undistortPixel(xPx, yPx); + if (lens.hasDistortion()) { + double[] back = lens.project(ray[0], ray[1], 1.0); + if (back == null || Math.hypot(back[0] - xPx, back[1] - yPx) > 1e-6) { + return new double[] {Double.NaN, Double.NaN}; + } + } + return ray; + } + private void grow(int tile, double x, double y) { + if (Double.isNaN(x)) { + return; // a pixel no ray reaches widens no tile + } tileBounds[4 * tile] = Math.min(tileBounds[4 * tile], x); tileBounds[4 * tile + 1] = Math.max(tileBounds[4 * tile + 1], x); tileBounds[4 * tile + 2] = Math.min(tileBounds[4 * tile + 2], y); @@ -209,22 +229,37 @@ private void renderTileRow(Scene scene, int ty, int frameKey, byte[] rgb, int[] for (int y = ty * TILE; y < y1; y++) { for (int x = tx * TILE; x < x1; x++) { int p = y * width + x; - trace(scene, candidates, centreRayX[p], centreRayY[p], hit); - pieceIds[p] = hit.pieceId; - tagIds[p] = hit.tagId; + boolean imaged = !Double.isNaN(centreRayX[p]); + if (imaged) { + trace(scene, candidates, centreRayX[p], centreRayY[p], hit); + pieceIds[p] = hit.pieceId; + tagIds[p] = hit.tagId; + } else { + pieceIds[p] = -1; + tagIds[p] = -1; + } double r; double g; double bl; if (n == 1) { - shade(scene, hit, colour); - r = colour[0]; - g = colour[1]; - bl = colour[2]; + if (imaged) { + shade(scene, hit, colour); + r = colour[0]; + g = colour[1]; + bl = colour[2]; + } else { + r = 0.0; + g = 0.0; + bl = 0.0; + } } else { r = 0.0; g = 0.0; bl = 0.0; for (int s = p * n * n, end = s + n * n; s < end; s++) { + if (Double.isNaN(subRayX[s])) { + continue; // no ray reaches this sub-pixel: black + } trace(scene, candidates, subRayX[s], subRayY[s], hit); shade(scene, hit, colour); r += colour[0]; diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaultInjector.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaultInjector.java new file mode 100644 index 0000000..08ae8a7 --- /dev/null +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaultInjector.java @@ -0,0 +1,427 @@ +package org.horizon36596.simloop.vision; + +import org.horizon36596.simloop.field.ObstacleBox; +import org.horizon36596.simloop.field.SeededRandom; + +import java.util.ArrayList; +import java.util.Arrays; +import java.util.Collections; +import java.util.List; + +/** + * Applies the content faults of a {@link VisionFaults} to one frame at a time: noise beyond the default, + * missed sightings, blackouts, occluding patches, false detections and wrong ids. The timing faults + * (latency, jitter, drops, repeats, reordering) are {@link FaultedFrameStream}'s. + * + *

One call is one frame. Call exactly one {@code apply} method per captured frame, in capture order; + * a blackout counts frames by those calls. The output is a new list or frame; the input is never changed. + * + *

What a fault never produces. Every number in an output is finite. A piece sighting whose noisy + * radius is zero or less, or whose noisy disc no longer overlaps the image, and a tag whose noisy centre + * leaves the image, are dropped, because a detector cannot report them; a frame's pixels stay in 0..255. A test that wants a NaN in its pipeline must put it there itself. + * + *

Ground truth stays truthful. A false piece sighting carries {@link #FALSE_PIECE_ID}, the id + * {@link SimFrame#getPieceIdAt} uses for "no piece". A mislabelled piece keeps its true piece id; only its + * kind changes. A mislabelled or false tag sighting names the layout tag it claims to be, but its transforms + * are the geometry actually seen, so a pose solved from it is wrong exactly as it would be on a real robot. + * In a frame, pixels a patch or a blackout covers are relabelled "no piece" and "no tag", and a false blob + * is labelled "no piece". + * + *

Deterministic: the same {@link VisionFaults} and the same sequence of calls give the same outputs. + * Not thread-safe. + */ +public final class VisionFaultInjector { + + /** The piece id a false piece sighting carries: no piece, as in {@link SimFrame#getPieceIdAt}. */ + public static final int FALSE_PIECE_ID = -1; + + /** + * Range window a false tag is placed in, lens to tag centre, inches. SET BY HAND: 12 to 120 in, the span + * an FTC field gives a robot-mounted camera. + */ + private static final double FALSE_TAG_MIN_RANGE_IN = 12.0; + private static final double FALSE_TAG_MAX_RANGE_IN = 120.0; + + /** Placements tried for a false tag before giving up on it for this frame. */ + private static final int FALSE_TAG_ATTEMPTS = 16; + + /** The flat colour an occluding patch is painted, {@code 0xRRGGBB}: a dark grey, like a robot's side. */ + private static final int PATCH_RGB = 0x303030; + + private final VisionFaults faults; + private final SeededRandom noiseRandom; + private final SeededRandom missRandom; + private final SeededRandom blackoutRandom; + private final SeededRandom patchRandom; + private final SeededRandom falseRandom; + private final SeededRandom wrongIdRandom; + private int blackoutFramesLeft; + private long framesSeen; + private long blackedOutFrames; + + /** + * An injector for these faults, at the start of its random streams. + * + * @param faults what to inject; {@link VisionFaults#none()} passes every frame through unchanged + */ + public VisionFaultInjector(VisionFaults faults) { + if (faults == null) { + throw new IllegalArgumentException("faults must not be null; use VisionFaults.none()"); + } + this.faults = faults; + this.noiseRandom = stream(faults.getSeed(), 1); + this.missRandom = stream(faults.getSeed(), 2); + this.blackoutRandom = stream(faults.getSeed(), 3); + this.patchRandom = stream(faults.getSeed(), 4); + this.falseRandom = stream(faults.getSeed(), 5); + this.wrongIdRandom = stream(faults.getSeed(), 6); + } + + /** + * One independent generator per fault, so turning one fault on cannot move another's draws. The salt is + * scattered through a fixed odd multiplier so neighbouring seeds do not give neighbouring streams. + */ + static SeededRandom stream(int seed, int salt) { + return new SeededRandom(seed ^ (salt * 0x9E3779B9)); + } + + /** {@return the faults this injector applies} */ + public VisionFaults getFaults() { + return faults; + } + + /** {@return frames passed through any {@code apply} method so far} */ + public long getFramesSeen() { + return framesSeen; + } + + /** {@return frames so far that fell inside a blackout} */ + public long getBlackedOutFrames() { + return blackedOutFrames; + } + + // ------------------------------------------------------------------------------------- pieces + + /** + * One frame of piece sightings, faulted. In this order: a blackout empties the frame; each sighting may + * be missed; noise moves the centre and radius (a radius at or below zero, or a disc moved wholly off the + * image, drops the sighting); a sighting may be mislabelled; one false sighting may be added at the end. + * + * @param sightings what the camera saw this frame, e.g. {@link SimCamera#observe}'s result + * @param lens the camera's lens, for the image size + * @return the faulted sightings, unmodifiable; the input order, a false sighting last + */ + public List applyToSightings(List sightings, CameraLens lens) { + if (sightings == null || lens == null) { + throw new IllegalArgumentException("sightings and lens must not be null"); + } + if (startFrame()) { + return Collections.emptyList(); + } + List out = new ArrayList(sightings.size() + 1); + for (CameraSighting s : sightings) { + if (missed()) { + continue; + } + double x = s.getCentreXPx() + gaussian(noiseRandom, faults.getCentreNoiseStdDevPx()); + double y = s.getCentreYPx() + gaussian(noiseRandom, faults.getCentreNoiseStdDevPx()); + double r = s.getRadiusPx() + gaussian(noiseRandom, faults.getRadiusNoiseStdDevPx()); + if (!(r > 0.0) || !discOverlapsImage(x, y, r, lens)) { + continue; + } + String kind = s.getKind(); + String[] kinds = faults.getWrongKinds(); + if (faults.getWrongIdProbability() > 0.0 && kinds.length > 0 + && wrongIdRandom.nextFraction() < faults.getWrongIdProbability()) { + String other = pickOther(kinds, kind); + if (other != null) { + kind = other; + } + } + out.add(new CameraSighting(s.getPieceId(), kind, x, y, r, s.getRangeInches(), + clipped(x, y, r, lens), s.getVisibleFraction())); + } + if (faults.getFalseDetectionProbability() > 0.0 + && falseRandom.nextFraction() < faults.getFalseDetectionProbability()) { + double x = falseRandom.nextBetween(0.0, lens.getWidthPx()); + double y = falseRandom.nextBetween(0.0, lens.getHeightPx()); + double r = falseRandom.nextBetween(faults.getFalseDetectionMinRadiusPx(), + faults.getFalseDetectionMaxRadiusPx()); + // A range is part of a sighting; give the false one the range a 1 in radius piece of that apparent + // size would be at, so it is finite and plausible rather than a NaN that hides the real question. + double range = lens.getFocalXPx() * 1.0 / r; + out.add(new CameraSighting(FALSE_PIECE_ID, faults.getFalseDetectionKind(), x, y, r, range, + clipped(x, y, r, lens), 1.0)); + } + return Collections.unmodifiableList(out); + } + + // ---------------------------------------------------------------------------------------- tags + + /** + * One frame of tag sightings, faulted. In this order: a blackout empties the frame; each sighting may be + * missed; noise moves {@code tx}, {@code ty}, the centre and the corners together (a centre pushed out of + * the image drops the sighting); a sighting may report another layout tag's id; one false tag may be + * added: a random layout tag's id, seen at a random place in front of the camera. + * + * @param tags what the camera saw this frame, e.g. {@link SimTagCamera#observe}'s result + * @param layout the layout the ids come from + * @param camera the camera the tags were seen with, for its lens and mount + * @param robotXInches robot centre, field X, inches, where the frame was captured + * @param robotYInches robot centre, field Y, inches + * @param robotHeadingRad robot heading, radians CCW from field {@code +X} + * @return the faulted sightings, unmodifiable; ascending id order of the input, a false tag last + */ + public List applyToTags(List tags, AprilTagLayout layout, SimTagCamera camera, + double robotXInches, double robotYInches, double robotHeadingRad) { + if (tags == null || layout == null || camera == null) { + throw new IllegalArgumentException("tags, layout and camera must not be null"); + } + if (startFrame()) { + return Collections.emptyList(); + } + CameraLens lens = camera.getLens(); + List all = layout.getTags(); + List out = new ArrayList(tags.size() + 1); + for (TagSighting s : tags) { + if (missed()) { + continue; + } + TagSighting t = s; + double sigma = faults.getTagAngleNoiseStdDevDegrees(); + if (sigma > 0.0) { + t = withAngles(s, s.getTxDegrees() + gaussian(noiseRandom, sigma), + s.getTyDegrees() + gaussian(noiseRandom, sigma), lens); + if (t == null) { + continue; + } + } + if (faults.getWrongIdProbability() > 0.0 && all.size() > 1 + && wrongIdRandom.nextFraction() < faults.getWrongIdProbability()) { + AprilTagLayout.Tag claimed = pickOtherTag(all, t.getId()); + t = new TagSighting(claimed, t.getCameraToTag(), t.getRobotToTag(), t.getCentreXPx(), + t.getCentreYPx(), t.getCornersPx(), t.getTxDegrees(), t.getTyDegrees(), t.getAreaPercent()); + } + out.add(t); + } + if (!all.isEmpty() && faults.getFalseDetectionProbability() > 0.0 + && falseRandom.nextFraction() < faults.getFalseDetectionProbability()) { + TagSighting phantom = falseTag(all, camera, robotXInches, robotYInches, robotHeadingRad); + if (phantom != null) { + out.add(phantom); + } + } + return Collections.unmodifiableList(out); + } + + /** + * A tag sighting with new angles: the centre pixel is re-projected from them and the corners move with + * it. Null when the new centre is not in the image, or the angles point behind the lens. + */ + private static TagSighting withAngles(TagSighting s, double txDegrees, double tyDegrees, CameraLens lens) { + if (!(Math.abs(txDegrees) < 90.0) || !(Math.abs(tyDegrees) < 90.0)) { + return null; + } + double[] centre = lens.project(StrictMath.tan(StrictMath.toRadians(txDegrees)), + -StrictMath.tan(StrictMath.toRadians(tyDegrees)), 1.0); + if (centre == null || !insideImage(centre[0], centre[1], lens)) { + return null; + } + double dx = centre[0] - s.getCentreXPx(); + double dy = centre[1] - s.getCentreYPx(); + double[][] corners = s.getCornersPx(); + double[][] moved = new double[corners.length][]; + for (int i = 0; i < corners.length; i++) { + moved[i] = new double[] {corners[i][0] + dx, corners[i][1] + dy}; + } + return new TagSighting(s.getTag(), s.getCameraToTag(), s.getRobotToTag(), centre[0], centre[1], moved, + txDegrees, tyDegrees, s.getAreaPercent()); + } + + /** + * A layout tag's id seen somewhere it is not: a copy of a random tag placed at a random range and bearing + * in front of the camera, facing it, seen through the camera's own rules, then relabelled as the layout + * tag so that its transforms are the phantom's geometry. Null when no placement tried was visible. + */ + private TagSighting falseTag(List all, SimTagCamera camera, double robotXInches, + double robotYInches, double robotHeadingRad) { + AprilTagLayout.Tag claimed = all.get(Math.min(all.size() - 1, (int) (falseRandom.nextFraction() * all.size()))); + CameraLens lens = camera.getLens(); + RigidTransform3 fieldToCamera = RigidTransform3.planar(robotXInches, robotYInches, robotHeadingRad) + .compose(camera.getRobotToCamera()); + for (int attempt = 0; attempt < FALSE_TAG_ATTEMPTS; attempt++) { + double range = falseRandom.nextBetween(FALSE_TAG_MIN_RANGE_IN, FALSE_TAG_MAX_RANGE_IN); + double px = falseRandom.nextBetween(0.2, 0.8) * lens.getWidthPx(); + double py = falseRandom.nextBetween(0.2, 0.8) * lens.getHeightPx(); + double[] ray = lens.undistortPixel(px, py); + double norm = StrictMath.sqrt(ray[0] * ray[0] + ray[1] * ray[1] + 1.0); + double[] centre = fieldToCamera.apply(range * ray[0] / norm, range * ray[1] / norm, range / norm); + double[] lensField = fieldToCamera.apply(0.0, 0.0, 0.0); + double facing = StrictMath.atan2(lensField[1] - centre[1], lensField[0] - centre[0]); + AprilTagLayout phantomLayout = AprilTagLayout.builder() + .addTag(claimed.getId(), centre[0], centre[1], centre[2], facing, claimed.getSizeInches()) + .build(); + List seen = new SimTagCamera(lens, camera.getMount(), 0.0, Double.POSITIVE_INFINITY, + 90.0, Collections.emptyList()) + .observe(phantomLayout, robotXInches, robotYInches, robotHeadingRad); + if (!seen.isEmpty()) { + TagSighting p = seen.get(0); + return new TagSighting(claimed, p.getCameraToTag(), p.getRobotToTag(), p.getCentreXPx(), + p.getCentreYPx(), p.getCornersPx(), p.getTxDegrees(), p.getTyDegrees(), p.getAreaPercent()); + } + } + return null; + } + + // -------------------------------------------------------------------------------------- frames + + /** + * One synthetic frame, faulted. In this order: a blackout paints it black; one occluding patch may be + * painted; one false blob may be painted; extra noise goes on every channel. Pixels a blackout or a patch + * covers lose their piece and tag labels. + * + * @param frame the rendered frame + * @return a new frame with the same size and frame number; the input is unchanged + */ + public SimFrame applyToFrame(SimFrame frame) { + if (frame == null) { + throw new IllegalArgumentException("frame must not be null"); + } + int w = frame.getWidthPx(); + int h = frame.getHeightPx(); + byte[] rgb = frame.getRgbBytes(); + int[] pieces = new int[w * h]; + int[] tagIds = new int[w * h]; + for (int y = 0; y < h; y++) { + for (int x = 0; x < w; x++) { + pieces[y * w + x] = frame.getPieceIdAt(x, y); + tagIds[y * w + x] = frame.getTagIdAt(x, y); + } + } + if (startFrame()) { + Arrays.fill(rgb, (byte) 0); + Arrays.fill(pieces, -1); + Arrays.fill(tagIds, -1); + return new SimFrame(w, h, frame.getFrameNumber(), rgb, pieces, tagIds); + } + if (faults.getOcclusionPatchProbability() > 0.0 + && patchRandom.nextFraction() < faults.getOcclusionPatchProbability()) { + int pw = Math.max(1, (int) Math.round(faults.getOcclusionPatchFraction() * w)); + int ph = Math.max(1, (int) Math.round(faults.getOcclusionPatchFraction() * h)); + int x0 = (int) (patchRandom.nextFraction() * (w - pw + 1)); + int y0 = (int) (patchRandom.nextFraction() * (h - ph + 1)); + for (int y = y0; y < Math.min(h, y0 + ph); y++) { + for (int x = x0; x < Math.min(w, x0 + pw); x++) { + paint(rgb, y * w + x, PATCH_RGB); + pieces[y * w + x] = -1; + tagIds[y * w + x] = -1; + } + } + } + if (faults.getFalseDetectionProbability() > 0.0 + && falseRandom.nextFraction() < faults.getFalseDetectionProbability()) { + double cx = falseRandom.nextBetween(0.0, w); + double cy = falseRandom.nextBetween(0.0, h); + double r = falseRandom.nextBetween(faults.getFalseDetectionMinRadiusPx(), + faults.getFalseDetectionMaxRadiusPx()); + for (int y = Math.max(0, (int) (cy - r)); y <= Math.min(h - 1, (int) (cy + r)); y++) { + for (int x = Math.max(0, (int) (cx - r)); x <= Math.min(w - 1, (int) (cx + r)); x++) { + double dx = x + 0.5 - cx; + double dy = y + 0.5 - cy; + if (dx * dx + dy * dy <= r * r) { + paint(rgb, y * w + x, faults.getFalseDetectionColourRgb()); + pieces[y * w + x] = -1; + } + } + } + } + double sigma = faults.getPixelNoiseStdDevCounts(); + if (sigma > 0.0) { + for (int i = 0; i < rgb.length; i++) { + long v = Math.round((rgb[i] & 0xFF) + gaussian(noiseRandom, sigma)); + rgb[i] = (byte) Math.max(0L, Math.min(255L, v)); + } + } + return new SimFrame(w, h, frame.getFrameNumber(), rgb, pieces, tagIds); + } + + // ------------------------------------------------------------------------------------- helpers + + /** Counts the frame and reports whether it is blacked out, starting a blackout when one is drawn. */ + private boolean startFrame() { + framesSeen++; + if (blackoutFramesLeft == 0 && faults.getBlackoutProbability() > 0.0 + && blackoutRandom.nextFraction() < faults.getBlackoutProbability()) { + blackoutFramesLeft = faults.getBlackoutFrames(); + } + if (blackoutFramesLeft > 0) { + blackoutFramesLeft--; + blackedOutFrames++; + return true; + } + return false; + } + + private boolean missed() { + return faults.getMissProbability() > 0.0 && missRandom.nextFraction() < faults.getMissProbability(); + } + + private String pickOther(String[] kinds, String own) { + List others = new ArrayList(kinds.length); + for (String k : kinds) { + if (!k.equals(own)) { + others.add(k); + } + } + if (others.isEmpty()) { + return null; + } + return others.get(Math.min(others.size() - 1, (int) (wrongIdRandom.nextFraction() * others.size()))); + } + + private AprilTagLayout.Tag pickOtherTag(List all, int ownId) { + int pick = Math.min(all.size() - 2, (int) (wrongIdRandom.nextFraction() * (all.size() - 1))); + for (AprilTagLayout.Tag tag : all) { + if (tag.getId() == ownId) { + continue; + } + if (pick-- == 0) { + return tag; + } + } + throw new IllegalStateException("a layout of " + all.size() + " tags has another tag; this is a bug"); + } + + private static boolean insideImage(double x, double y, CameraLens lens) { + return x >= 0.0 && x <= lens.getWidthPx() && y >= 0.0 && y <= lens.getHeightPx(); + } + + /** The rule {@link SimCamera} sights a piece by: the image point nearest the disc's centre is in the disc. */ + private static boolean discOverlapsImage(double x, double y, double r, CameraLens lens) { + double nearestX = Math.max(0.0, Math.min(lens.getWidthPx(), x)); + double nearestY = Math.max(0.0, Math.min(lens.getHeightPx(), y)); + double offX = x - nearestX; + double offY = y - nearestY; + return offX * offX + offY * offY < r * r; + } + + private static boolean clipped(double x, double y, double r, CameraLens lens) { + return x - r < 0.0 || x + r > lens.getWidthPx() || y - r < 0.0 || y + r > lens.getHeightPx(); + } + + private static void paint(byte[] rgb, int pixel, int colour) { + rgb[3 * pixel] = (byte) (colour >>> 16); + rgb[3 * pixel + 1] = (byte) (colour >>> 8); + rgb[3 * pixel + 2] = (byte) colour; + } + + /** One zero-mean Gaussian draw, or exactly 0 with no draw when the spread is 0. Box-Muller, as elsewhere. */ + static double gaussian(SeededRandom random, double stdDev) { + if (stdDev == 0.0) { + return 0.0; + } + double u1 = 1.0 - random.nextFraction(); + double u2 = random.nextFraction(); + return stdDev * StrictMath.sqrt(-2.0 * StrictMath.log(u1)) * StrictMath.cos(2.0 * Math.PI * u2); + } +} diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaults.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaults.java new file mode 100644 index 0000000..a4995a5 --- /dev/null +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaults.java @@ -0,0 +1,478 @@ +package org.horizon36596.simloop.vision; + +/** + * Faults to inject into a simulated camera's output, for stress-testing the vision code that consumes it: + * what a pipeline does when the camera is noisier than usual, late, jittery, blocked, drops or repeats + * frames, delivers them out of order, or reports things that are not there. + * + *

This is a description only. Two classes act on it: + *

+ * {@code FakeLimelight3A.Builder#faults} uses both. + * + *

Determinism. Every fault draws from its own {@link org.horizon36596.simloop.field.SeededRandom}, + * seeded from {@link #getSeed()} and the fault's name, so the same seed and the same sequence of frames give + * the same faults, and turning one fault on or off does not move another fault's draws. + * + *

Every rate here is a test input, not a measurement. None of the defaults describe a real camera; + * the defaults are "off". Pick the levels your test needs and say so in the test. + * + *

Immutable. Build with {@link #builder(int)}; {@link #none()} turns everything off. + */ +public final class VisionFaults { + + private static final VisionFaults NONE = new Builder(0).build(); + + private final int seed; + private final double centreNoiseStdDevPx; + private final double radiusNoiseStdDevPx; + private final double tagAngleNoiseStdDevDegrees; + private final double pixelNoiseStdDevCounts; + private final double extraLatencySeconds; + private final double latencyJitterSeconds; + private final double dropProbability; + private final double repeatProbability; + private final double reorderProbability; + private final double missProbability; + private final double blackoutProbability; + private final int blackoutFrames; + private final double occlusionPatchProbability; + private final double occlusionPatchFraction; + private final double falseDetectionProbability; + private final String falseDetectionKind; + private final double falseDetectionMinRadiusPx; + private final double falseDetectionMaxRadiusPx; + private final int falseDetectionColourRgb; + private final double wrongIdProbability; + private final String[] wrongKinds; + + private VisionFaults(Builder b) { + this.seed = b.seed; + this.centreNoiseStdDevPx = b.centreNoiseStdDevPx; + this.radiusNoiseStdDevPx = b.radiusNoiseStdDevPx; + this.tagAngleNoiseStdDevDegrees = b.tagAngleNoiseStdDevDegrees; + this.pixelNoiseStdDevCounts = b.pixelNoiseStdDevCounts; + this.extraLatencySeconds = b.extraLatencySeconds; + this.latencyJitterSeconds = b.latencyJitterSeconds; + this.dropProbability = b.dropProbability; + this.repeatProbability = b.repeatProbability; + this.reorderProbability = b.reorderProbability; + this.missProbability = b.missProbability; + this.blackoutProbability = b.blackoutProbability; + this.blackoutFrames = b.blackoutFrames; + this.occlusionPatchProbability = b.occlusionPatchProbability; + this.occlusionPatchFraction = b.occlusionPatchFraction; + this.falseDetectionProbability = b.falseDetectionProbability; + this.falseDetectionKind = b.falseDetectionKind; + this.falseDetectionMinRadiusPx = b.falseDetectionMinRadiusPx; + this.falseDetectionMaxRadiusPx = b.falseDetectionMaxRadiusPx; + this.falseDetectionColourRgb = b.falseDetectionColourRgb; + this.wrongIdProbability = b.wrongIdProbability; + this.wrongKinds = b.wrongKinds.clone(); + } + + /** {@return no faults at all: an injector and a stream built from this pass frames through unchanged} */ + public static VisionFaults none() { + return NONE; + } + + /** + * Starts a description with every fault off. + * + * @param seed the seed every fault's random draws derive from; same seed, same faults + * @return the builder + */ + public static Builder builder(int seed) { + return new Builder(seed); + } + + /** {@return the seed every fault's random draws derive from} */ + public int getSeed() { + return seed; + } + + /** {@return extra Gaussian noise on each piece sighting's centre, per axis, standard deviation, pixels} */ + public double getCentreNoiseStdDevPx() { + return centreNoiseStdDevPx; + } + + /** {@return extra Gaussian noise on each piece sighting's apparent radius, standard deviation, pixels} */ + public double getRadiusNoiseStdDevPx() { + return radiusNoiseStdDevPx; + } + + /** {@return extra Gaussian noise on each tag sighting's tx and ty, standard deviation, degrees} */ + public double getTagAngleNoiseStdDevDegrees() { + return tagAngleNoiseStdDevDegrees; + } + + /** {@return extra Gaussian noise on every channel of every pixel of a frame, standard deviation, 8-bit counts} */ + public double getPixelNoiseStdDevCounts() { + return pixelNoiseStdDevCounts; + } + + /** {@return a fixed delay added to every frame's delivery, seconds} */ + public double getExtraLatencySeconds() { + return extraLatencySeconds; + } + + /** {@return the most a frame's delivery is moved earlier or later at random, uniform, seconds} */ + public double getLatencyJitterSeconds() { + return latencyJitterSeconds; + } + + /** {@return the chance a whole frame is never delivered} */ + public double getDropProbability() { + return dropProbability; + } + + /** {@return the chance a delivered frame is delivered a second time, with the same capture stamp} */ + public double getRepeatProbability() { + return repeatProbability; + } + + /** {@return the chance a frame is held back and delivered after the frame captured next} */ + public double getReorderProbability() { + return reorderProbability; + } + + /** {@return the chance each sighting in a frame is missed: partial occlusion} */ + public double getMissProbability() { + return missProbability; + } + + /** {@return the chance a frame starts a blackout: the camera sees nothing for {@link #getBlackoutFrames()}} */ + public double getBlackoutProbability() { + return blackoutProbability; + } + + /** {@return how many frames in a row a blackout lasts, the starting frame included} */ + public int getBlackoutFrames() { + return blackoutFrames; + } + + /** {@return the chance a frame's pixels get one occluding rectangle} */ + public double getOcclusionPatchProbability() { + return occlusionPatchProbability; + } + + /** {@return the occluding rectangle's width and height as a share of the image's, 0 to 1} */ + public double getOcclusionPatchFraction() { + return occlusionPatchFraction; + } + + /** {@return the chance a frame gains one false detection: a piece, a blob or a tag that is not there} */ + public double getFalseDetectionProbability() { + return falseDetectionProbability; + } + + /** {@return the kind label a false piece sighting carries} */ + public String getFalseDetectionKind() { + return falseDetectionKind; + } + + /** {@return the smallest apparent radius of a false piece sighting or blob, pixels} */ + public double getFalseDetectionMinRadiusPx() { + return falseDetectionMinRadiusPx; + } + + /** {@return the largest apparent radius of a false piece sighting or blob, pixels} */ + public double getFalseDetectionMaxRadiusPx() { + return falseDetectionMaxRadiusPx; + } + + /** {@return the colour a false blob is painted in a frame, {@code 0xRRGGBB}} */ + public int getFalseDetectionColourRgb() { + return falseDetectionColourRgb; + } + + /** + * {@return the chance each sighting is mislabelled: a tag reports another layout tag's id, a piece + * another of {@link #getWrongKinds()}} + */ + public double getWrongIdProbability() { + return wrongIdProbability; + } + + /** {@return the kind labels a mislabelled piece sighting can take; a copy} */ + public String[] getWrongKinds() { + return wrongKinds.clone(); + } + + /** {@return true when no fault is on} */ + public boolean isNone() { + return centreNoiseStdDevPx == 0.0 && radiusNoiseStdDevPx == 0.0 && tagAngleNoiseStdDevDegrees == 0.0 + && pixelNoiseStdDevCounts == 0.0 && extraLatencySeconds == 0.0 && latencyJitterSeconds == 0.0 + && dropProbability == 0.0 && repeatProbability == 0.0 && reorderProbability == 0.0 + && missProbability == 0.0 && blackoutProbability == 0.0 && occlusionPatchProbability == 0.0 + && falseDetectionProbability == 0.0 && wrongIdProbability == 0.0; + } + + @Override + public String toString() { + if (isNone()) { + return "VisionFaults(none)"; + } + StringBuilder s = new StringBuilder("VisionFaults(seed ").append(seed); + append(s, "centreNoisePx", centreNoiseStdDevPx); + append(s, "radiusNoisePx", radiusNoiseStdDevPx); + append(s, "tagAngleNoiseDeg", tagAngleNoiseStdDevDegrees); + append(s, "pixelNoiseCounts", pixelNoiseStdDevCounts); + append(s, "extraLatencyS", extraLatencySeconds); + append(s, "jitterS", latencyJitterSeconds); + append(s, "drop", dropProbability); + append(s, "repeat", repeatProbability); + append(s, "reorder", reorderProbability); + append(s, "miss", missProbability); + append(s, "blackout", blackoutProbability); + append(s, "patch", occlusionPatchProbability); + append(s, "falseDetection", falseDetectionProbability); + append(s, "wrongId", wrongIdProbability); + return s.append(')').toString(); + } + + private static void append(StringBuilder s, String name, double value) { + if (value != 0.0) { + s.append(", ").append(name).append(' ').append(value); + } + } + + /** Builds a {@link VisionFaults}. Every fault starts off; each setter turns one on. */ + public static final class Builder { + + private final int seed; + private double centreNoiseStdDevPx; + private double radiusNoiseStdDevPx; + private double tagAngleNoiseStdDevDegrees; + private double pixelNoiseStdDevCounts; + private double extraLatencySeconds; + private double latencyJitterSeconds; + private double dropProbability; + private double repeatProbability; + private double reorderProbability; + private double missProbability; + private double blackoutProbability; + private int blackoutFrames = 1; + private double occlusionPatchProbability; + private double occlusionPatchFraction; + private double falseDetectionProbability; + private String falseDetectionKind = "false"; + private double falseDetectionMinRadiusPx = 4.0; + private double falseDetectionMaxRadiusPx = 30.0; + private int falseDetectionColourRgb = 0xFFFFFF; + private double wrongIdProbability; + private String[] wrongKinds = new String[0]; + + private Builder(int seed) { + this.seed = seed; + } + + /** + * Extra Gaussian noise on each piece sighting's centre and apparent radius, on top of whatever noise + * the caller already adds. A radius pushed to zero or below is not a blob a detector can report, so + * that sighting is dropped instead. + * + * @param centreStdDevPx standard deviation per axis, pixels, finite and ≥ 0 + * @param radiusStdDevPx standard deviation, pixels, finite and ≥ 0 + * @return this builder + */ + public Builder pixelNoise(double centreStdDevPx, double radiusStdDevPx) { + this.centreNoiseStdDevPx = nonNegative(centreStdDevPx, "centre noise"); + this.radiusNoiseStdDevPx = nonNegative(radiusStdDevPx, "radius noise"); + return this; + } + + /** + * Extra Gaussian noise on each tag sighting's {@code tx} and {@code ty}. The centre pixel moves with + * them; a tag pushed out of the image is dropped, because a detector cannot report it there. + * + * @param stdDevDegrees standard deviation, degrees, finite and ≥ 0 + * @return this builder + */ + public Builder tagAngleNoise(double stdDevDegrees) { + this.tagAngleNoiseStdDevDegrees = nonNegative(stdDevDegrees, "tag angle noise"); + return this; + } + + /** + * Extra Gaussian noise on every channel of every pixel of a frame, clamped to 0..255. + * + * @param stdDevCounts standard deviation, 8-bit counts, finite and ≥ 0 + * @return this builder + */ + public Builder frameNoise(double stdDevCounts) { + this.pixelNoiseStdDevCounts = nonNegative(stdDevCounts, "frame noise"); + return this; + } + + /** + * Delay every frame's delivery by a fixed amount, and move each one by up to {@code jitterSeconds} + * earlier or later, uniform. Jitter never makes a frame arrive before it was captured, and on its own + * it never reorders frames: a pipeline delivers in order, so a frame that would overtake the one + * before it waits for it instead. Use {@link #reorderFrames} for out-of-order delivery. + * + * @param extraSeconds fixed extra delay, seconds, finite and ≥ 0 + * @param jitterSeconds largest random shift, seconds, finite and ≥ 0 + * @return this builder + */ + public Builder latency(double extraSeconds, double jitterSeconds) { + this.extraLatencySeconds = nonNegative(extraSeconds, "extra latency"); + this.latencyJitterSeconds = nonNegative(jitterSeconds, "latency jitter"); + return this; + } + + /** + * Lose whole frames. + * + * @param probability chance each frame is never delivered, in {@code [0, 1]} + * @return this builder + */ + public Builder dropFrames(double probability) { + this.dropProbability = probability(probability, "drop"); + return this; + } + + /** + * Deliver a frame twice: a stale frame handed over again with the same capture stamp and the same + * contents, on the next delivery after the first. + * + * @param probability chance each delivered frame is repeated once, in {@code [0, 1]} + * @return this builder + */ + public Builder repeatFrames(double probability) { + this.repeatProbability = probability(probability, "repeat"); + return this; + } + + /** + * Deliver a frame after the one captured next, so capture stamps go backwards. A frame held back + * when no later frame is ever offered is not delivered. + * + * @param probability chance each frame is held back, in {@code [0, 1]} + * @return this builder + */ + public Builder reorderFrames(double probability) { + this.reorderProbability = probability(probability, "reorder"); + return this; + } + + /** + * Partial occlusion of sightings: each piece or tag in a frame is missed independently. + * + * @param probability chance each sighting is missed, in {@code [0, 1]} + * @return this builder + */ + public Builder missDetections(double probability) { + this.missProbability = probability(probability, "miss"); + return this; + } + + /** + * Full occlusion: a frame may start a run of frames in which the camera sees nothing (a robot parked + * in front of it, a hand over the lens). Sightings are all removed; a frame's pixels are painted + * black. + * + * @param probability chance a frame not already blacked out starts a blackout, in {@code [0, 1]} + * @param frames how many frames the blackout lasts, the starting one included, ≥ 1 + * @return this builder + */ + public Builder blackout(double probability, int frames) { + if (frames < 1) { + throw new IllegalArgumentException("a blackout lasts at least one frame, got " + frames); + } + this.blackoutProbability = probability(probability, "blackout"); + this.blackoutFrames = frames; + return this; + } + + /** + * Partial occlusion of pixels: a frame may get one flat dark-grey rectangle at a random place. + * + * @param probability chance a frame gets a rectangle, in {@code [0, 1]} + * @param fraction the rectangle's width and height as a share of the image's, in {@code (0, 1]} + * @return this builder + */ + public Builder occlusionPatch(double probability, double fraction) { + if (!(fraction > 0.0 && fraction <= 1.0)) { + throw new IllegalArgumentException("patch fraction must be in (0, 1], got " + fraction); + } + this.occlusionPatchProbability = probability(probability, "occlusion patch"); + this.occlusionPatchFraction = fraction; + return this; + } + + /** + * False detections: a frame may gain one thing that is not there. For piece sightings, a sighting of + * kind {@code kind} with piece id {@link VisionFaultInjector#FALSE_PIECE_ID} at a random place in the + * image; for a frame's pixels, a disc of {@code colourRgb}; for tags, a sighting of a random layout + * tag at a random place in front of the camera, so a pose solved from it is wrong. + * + * @param probability chance a frame gains one, in {@code [0, 1]} + * @param kind the kind label a false piece sighting carries, not null + * @param minRadiusPx smallest apparent radius of a false piece or blob, pixels, > 0 + * @param maxRadiusPx largest, pixels, ≥ {@code minRadiusPx} + * @param colourRgb the colour of a false blob in a frame, {@code 0xRRGGBB} + * @return this builder + */ + public Builder falseDetections(double probability, String kind, double minRadiusPx, double maxRadiusPx, + int colourRgb) { + if (kind == null) { + throw new IllegalArgumentException("false detection kind must not be null"); + } + if (!(minRadiusPx > 0.0) || !(maxRadiusPx >= minRadiusPx) || Double.isInfinite(maxRadiusPx)) { + throw new IllegalArgumentException("false detection radii must satisfy 0 < min <= max < infinity," + + " got " + minRadiusPx + " and " + maxRadiusPx); + } + this.falseDetectionProbability = probability(probability, "false detection"); + this.falseDetectionKind = kind; + this.falseDetectionMinRadiusPx = minRadiusPx; + this.falseDetectionMaxRadiusPx = maxRadiusPx; + this.falseDetectionColourRgb = colourRgb & 0xFFFFFF; + return this; + } + + /** + * Mislabelled sightings: a tag reports the id of another tag in the layout (its geometry stays the + * tag actually seen, as a misdecode does); a piece reports one of {@code kinds} other than its own. + * + * @param probability chance each sighting is mislabelled, in {@code [0, 1]} + * @param kinds the kind labels a piece can be mislabelled as; empty leaves pieces alone + * @return this builder + */ + public Builder wrongIds(double probability, String... kinds) { + if (kinds == null) { + throw new IllegalArgumentException("kinds must not be null; pass none to leave pieces alone"); + } + for (String kind : kinds) { + if (kind == null) { + throw new IllegalArgumentException("a kind label must not be null"); + } + } + this.wrongIdProbability = probability(probability, "wrong id"); + this.wrongKinds = kinds.clone(); + return this; + } + + /** {@return the faults} */ + public VisionFaults build() { + return new VisionFaults(this); + } + + private static double nonNegative(double value, String what) { + if (!(value >= 0.0) || Double.isInfinite(value)) { + throw new IllegalArgumentException(what + " must be finite and >= 0, got " + value); + } + return value; + } + + private static double probability(double value, String what) { + if (!(value >= 0.0 && value <= 1.0)) { + throw new IllegalArgumentException(what + " probability must be in [0, 1], got " + value); + } + return value; + } + } +} diff --git a/SimLoop/src/test/java/org/horizon36596/simloop/fakehardware/FakeLimelight3AFaultsTest.java b/SimLoop/src/test/java/org/horizon36596/simloop/fakehardware/FakeLimelight3AFaultsTest.java new file mode 100644 index 0000000..f535d73 --- /dev/null +++ b/SimLoop/src/test/java/org/horizon36596/simloop/fakehardware/FakeLimelight3AFaultsTest.java @@ -0,0 +1,216 @@ +package org.horizon36596.simloop.fakehardware; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import com.qualcomm.hardware.limelightvision.LLResult; +import com.qualcomm.hardware.limelightvision.LLResultTypes; + +import java.util.ArrayList; +import java.util.List; + +import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; +import org.firstinspires.ftc.robotcore.external.navigation.Pose3D; +import org.horizon36596.simloop.vision.AprilTagLayout; +import org.horizon36596.simloop.vision.CameraMount; +import org.horizon36596.simloop.vision.VisionFaults; +import org.junit.jupiter.api.Test; + +/** + * INVARIANT-tier (domain R9) — the fake Limelight under stress: heavy noise and {@link VisionFaults}. + * + *

    + *
  1. Noise never reports a tag outside the image (regression: before the fix, 60 degrees of tx/ty noise + * reported tx = -188 degrees and a centre pixel of -11718).
    2. + *
  2. {@code faults(VisionFaults.none())} changes nothing.
    3. + *
  3. With faults on, botpose is solved from the tags seen: correct tags still give the true pose, wrong + * and false ones do not.
    4. + *
  4. Out-of-order delivery makes the capture stamp go backwards; extra latency makes it lag; a blackout + * empties results.
    5. + *
+ * + *

Same made-up rig as {@link FakeLimelight3ATest}: 54.5 x 42 degrees, lens 10 in up looking forward. + */ +class FakeLimelight3AFaultsTest { + + private static final double M = 0.0254; + private static final double TICK = 0.02; + private static final CameraMount LEVEL = new CameraMount(0.0, 0.0, 10.0, 0.0, 0.0); + private static final AprilTagLayout WALL = AprilTagLayout.builder() + .addTag(1, 72.0, -12.0, 10.0, Math.PI, 6.0) + .addTag(2, 72.0, 0.0, 10.0, Math.PI, 6.0) + .addTag(3, 72.0, 12.0, 10.0, Math.PI, 6.0) + .build(); + + private static double metres(Pose3D pose, char axis) { + double v = axis == 'x' ? pose.getPosition().toUnit(DistanceUnit.METER).x + : pose.getPosition().toUnit(DistanceUnit.METER).y; + return v; + } + + private static FakeLimelight3A.Builder builder() { + return FakeLimelight3A.builder(WALL, LEVEL, 54.5, 42.0); + } + + @Test + void heavyNoiseNeverReportsATagOutsideTheImage() { + FakeLimelight3A ll = builder().noise(3, 60.0, 0.0, 0.0).build(); + ll.start(); + int reported = 0; + for (int i = 0; i < 500; i++) { + ll.update(TICK); + for (LLResultTypes.FiducialResult f : ll.getLatestResult().getFiducialResults()) { + reported++; + assertTrue(Math.abs(f.getTargetXDegrees()) <= 54.5 / 2.0 + 1e-9, "tx " + f.getTargetXDegrees()); + assertTrue(Math.abs(f.getTargetYDegrees()) <= 42.0 / 2.0 + 1e-9, "ty " + f.getTargetYDegrees()); + assertTrue(f.getTargetXPixels() >= 0.0 && f.getTargetXPixels() <= 640.0, "txp " + f.getTargetXPixels()); + assertTrue(f.getTargetYPixels() >= 0.0 && f.getTargetYPixels() <= 480.0, "typ " + f.getTargetYPixels()); + } + } + assertTrue(reported > 0 && reported < 1500, "some tags stay in the image and some leave it: " + reported); + } + + @Test + void moderateNoiseIsUnchangedByTheImageEdgeRule() { + // 0.5 degree noise cannot push a tag 12 in off the axis at 72 in out of the image: nothing drops. + FakeLimelight3A ll = builder().noise(3, 0.5, 1.0, 1.0).build(); + ll.start(); + for (int i = 0; i < 200; i++) { + ll.update(TICK); + assertEquals(3, ll.getLatestResult().getFiducialResults().size()); + } + } + + private static String trace(FakeLimelight3A ll, int ticks) { + ll.start(); + StringBuilder s = new StringBuilder(); + for (int i = 0; i < ticks; i++) { + ll.setRobotPose(-10.0 + i * 0.2, 2.0, 0.05); + ll.update(TICK); + LLResult r = ll.getLatestResult(); + s.append(r.getTimestamp()).append(' ').append(metres(r.getBotpose(), 'x')).append(' ') + .append(metres(r.getBotpose_MT2(), 'y')).append(' '); + for (LLResultTypes.FiducialResult f : r.getFiducialResults()) { + s.append(f.getFiducialId()).append(':').append(f.getTargetXDegrees()).append(','); + } + s.append('\n'); + } + return s.toString(); + } + + @Test + void noFaultsChangesNothing() { + String plain = trace(builder().noise(4, 0.3, 1.0, 1.0).dropout(5, 0.2).latencyMs(20, 10).build(), 150); + String none = trace(builder().noise(4, 0.3, 1.0, 1.0).dropout(5, 0.2).latencyMs(20, 10) + .faults(VisionFaults.none()).build(), 150); + assertEquals(plain, none); + assertThrows(IllegalArgumentException.class, () -> builder().faults(null)); + } + + @Test + void withFaultsOnCorrectTagsStillSolveTheTruePose() { + FakeLimelight3A ll = builder().faults(VisionFaults.builder(1).latency(0.0, 0.0).dropFrames(0.0) + .missDetections(0.3).build()).build(); + ll.start(); + ll.setRobotPose(5.0, -3.0, 0.1); + for (int i = 0; i < 50; i++) { + ll.update(TICK); + LLResult r = ll.getLatestResult(); + if (r.getFiducialResults().isEmpty()) { + continue; + } + assertEquals(5.0 * M, metres(r.getBotpose(), 'x'), 1e-9); + assertEquals(-3.0 * M, metres(r.getBotpose(), 'y'), 1e-9); + } + } + + @Test + void wrongIdsMoveBothBotposes() { + FakeLimelight3A ll = builder().faults(VisionFaults.builder(1).wrongIds(1.0).build()).build(); + ll.start(); + ll.updateRobotOrientation(0.0); + ll.update(TICK); + LLResult r = ll.getLatestResult(); + List ids = new ArrayList(); + for (LLResultTypes.FiducialResult f : r.getFiducialResults()) { + ids.add(f.getFiducialId()); + } + assertEquals(3, ids.size()); + double mt1Error = Math.hypot(metres(r.getBotpose(), 'x'), metres(r.getBotpose(), 'y')) / M; + double mt2Error = Math.hypot(metres(r.getBotpose_MT2(), 'x'), metres(r.getBotpose_MT2(), 'y')) / M; + assertTrue(mt1Error > 1.0, "every tag misread: botpose must be off the true origin, was " + mt1Error + " in"); + assertTrue(mt2Error > 1.0, "botpose_orb too, was " + mt2Error + " in"); + } + + @Test + void falseTagsAddAFiducialAndPullBotpose() { + FakeLimelight3A ll = builder().faults(VisionFaults.builder(2) + .falseDetections(1.0, "tag", 1.0, 1.0, 0).build()).build(); + ll.start(); + int extra = 0; + double worst = 0.0; + for (int i = 0; i < 40; i++) { + ll.update(TICK); + LLResult r = ll.getLatestResult(); + if (r.getFiducialResults().size() == 4) { + extra++; + worst = Math.max(worst, Math.hypot(metres(r.getBotpose(), 'x'), metres(r.getBotpose(), 'y')) / M); + } + } + assertTrue(extra > 30, "frames with a false tag: " + extra); + assertTrue(worst > 1.0, "a false tag must pull botpose off the truth, worst " + worst + " in"); + } + + @Test + void outOfOrderFramesMakeTheCaptureStampGoBackwards() { + // Frames every other tick, so a tick with no newer frame shows the late one. + FakeLimelight3A ll = builder().frameRateHz(25.0) + .faults(VisionFaults.builder(3).reorderFrames(0.3).build()).build(); + ll.start(); + double last = Double.NEGATIVE_INFINITY; + int backwards = 0; + for (int i = 0; i < 300; i++) { + ll.update(TICK); + double ts = ll.getLatestResult().getTimestamp(); + if (ts < last) { + backwards++; + } + last = ts; + } + assertTrue(backwards > 15, "backward steps in ts: " + backwards); + assertTrue(backwards <= ll.getFaultStream().getReordered()); + } + + @Test + void extraLatencyMakesTheResultLagAndIsNotReportedAsTl() { + FakeLimelight3A ll = builder().latencyMs(10.0, 5.0) + .faults(VisionFaults.builder(3).latency(0.200, 0.0).build()).build(); + ll.start(); + for (int i = 0; i < 100; i++) { + ll.update(TICK); + } + LLResult r = ll.getLatestResult(); + double ageMs = ll.getSimTimeNanos() / 1e6 - r.getTimestamp(); + assertEquals(215.0, ageMs, 20.0 + 1e-9, "capture is 15 ms device latency plus 200 ms extra old"); + assertEquals(5.0, r.getTargetingLatency(), 0.0, "tl reports only the device's own latency"); + } + + @Test + void aBlackoutEmptiesResults() { + FakeLimelight3A ll = builder().faults(VisionFaults.builder(4).blackout(0.05, 10).build()).build(); + ll.start(); + int empty = 0; + for (int i = 0; i < 400; i++) { + ll.update(TICK); + LLResult r = ll.getLatestResult(); + if (!r.isValid()) { + empty++; + assertEquals(0, r.getFiducialResults().size()); + } + } + assertTrue(empty >= 10, "blacked-out results: " + empty); + assertNotEquals(400, empty); + } +} diff --git a/SimLoop/src/test/java/org/horizon36596/simloop/vision/CameraLensDistortionTest.java b/SimLoop/src/test/java/org/horizon36596/simloop/vision/CameraLensDistortionTest.java index debaade..f10f2a2 100644 --- a/SimLoop/src/test/java/org/horizon36596/simloop/vision/CameraLensDistortionTest.java +++ b/SimLoop/src/test/java/org/horizon36596/simloop/vision/CameraLensDistortionTest.java @@ -89,6 +89,29 @@ void barrelDistortionPullsTheCornersIn() { assertEquals(BARREL.getCentreYPx(), centre[1], 1e-12); } + /** + * Regression, found by noise injection: a noisy ball centre just off the corner of an image whose corners + * lie past a folding lens's reach used to come back from {@code groundPointInRobotFrame} as a confident + * floor point, mirrored through the image centre (pixel (-400, -300), up and left, gave a point to the + * RIGHT of the robot). No ray lands on such a pixel, so there is no floor point. + */ + @Test + void aPixelNoRayReachesHasNoGroundPoint() { + CameraLens folding = CameraLens.fromHorizontalFov(70.0, 640, 400).withDistortion(-0.45, 0.0, 0.0, 0.0, 0.0); + SimCamera camera = new SimCamera("fold", folding, CameraMount.front(6.0, 10.0, 22.0)); + for (double[] px : new double[][] {{-400.0, -300.0}, {-2000.0, 200.0}, {-50.0, 450.0}, {700.0, -40.0}}) { + double[] ray = folding.undistortPixel(px[0], px[1]); + double[] back = folding.project(ray[0], ray[1], 1.0); + assertTrue(back == null || Math.hypot(back[0] - px[0], back[1] - px[1]) > 1.0, + "the lens cannot image (" + px[0] + ", " + px[1] + ")"); + assertNull(camera.groundPointInRobotFrame(px[0], px[1], 1.4), + "no ray reaches (" + px[0] + ", " + px[1] + "), so no floor point"); + } + // The image centre and a pixel well inside still round-trip and still have a floor point. + assertNotNull(camera.groundPointInRobotFrame(320.0, 300.0, 1.4)); + assertNotNull(camera.groundPointInRobotFrame(200.0, 350.0, 1.4)); + } + @Test void pastTheFoldOverRadiusNothingIsImaged() { CameraLens lens = PINHOLE.withDistortion(-0.3, 0.0, 0.0, 0.0, 0.0); diff --git a/SimLoop/src/test/java/org/horizon36596/simloop/vision/SimFrameRendererTest.java b/SimLoop/src/test/java/org/horizon36596/simloop/vision/SimFrameRendererTest.java index d108e46..25a7947 100644 --- a/SimLoop/src/test/java/org/horizon36596/simloop/vision/SimFrameRendererTest.java +++ b/SimLoop/src/test/java/org/horizon36596/simloop/vision/SimFrameRendererTest.java @@ -109,6 +109,33 @@ void aDistortedLensDrawsThePieceWhereTheDistortedCameraSaysItIs() { "the distortion moved the piece by more than the tolerance, or this test proves nothing"); } + /** + * Regression: with a lens whose distortion folds over before the image corners, the corner pixels have no + * ray through the lens. They used to be traced along whatever direction the undistortion search stopped + * on, often mirrored through the centre, so a piece in the lower right could also be drawn in the upper + * left corner. They now see nothing: black, no label. The middle of the image is drawn as before. + */ + @Test + void cornersPastAFoldingLensesReachAreBlackAndUnlabelled() { + CameraLens folding = LENS.withDistortion(-0.45, 0.0, 0.0, 0.0, 0.0); + double[] cornerRay = folding.undistortPixel(0.5, 0.5); + double[] back = folding.project(cornerRay[0], cornerRay[1], 1.0); + assertTrue(back == null || Math.hypot(back[0] - 0.5, back[1] - 0.5) > 1.0, + "this lens must not reach the corner, or this test proves nothing"); + + GamePieceTracker tracker = pieces("red", 26.0, 0.0); + SimFrame frame = SimFrameRenderer.builder(folding, FRONT).style(STYLE).supersampling(2).build() + .render(tracker.getPieces(), 0.0, 0.0, 0.0, CameraOccluders.none(), AprilTagLayout.empty()); + assertEquals(0x000000, frame.getRgb(0, 0)); + assertEquals(-1, frame.getPieceIdAt(0, 0)); + assertEquals(-1, frame.getTagIdAt(0, 0)); + assertTrue(frame.getRgb(160, 120) != 0x000000, "the image centre is still drawn"); + int pieceId = tracker.getPieces().get(0).getId(); + assertTrue(frame.pixelCountOfPiece(pieceId) > 0, "the piece in the middle is still drawn"); + double[] centroid = frame.centroidOfPiece(pieceId); + assertTrue(Math.abs(centroid[0] - 160.0) < 20.0, "drawn once, near the middle: " + centroid[0]); + } + @Test void whatTheOccludedObserveHidesHasNoPixels() { GamePieceTracker tracker = pieces("red", 34.0, 0.0, "blue", 30.0, 14.0); diff --git a/SimLoop/src/test/java/org/horizon36596/simloop/vision/VisionFaultsTest.java b/SimLoop/src/test/java/org/horizon36596/simloop/vision/VisionFaultsTest.java new file mode 100644 index 0000000..5ff91a9 --- /dev/null +++ b/SimLoop/src/test/java/org/horizon36596/simloop/vision/VisionFaultsTest.java @@ -0,0 +1,478 @@ +package org.horizon36596.simloop.vision; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertNotEquals; +import static org.junit.jupiter.api.Assertions.assertSame; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +import org.horizon36596.simloop.field.FieldWorld; +import org.horizon36596.simloop.field.GamePiece; +import org.horizon36596.simloop.field.GamePieceTracker; +import org.horizon36596.simloop.field.ObstacleBox; +import org.junit.jupiter.api.Test; + +/** + * INVARIANT-tier (domain R9) — the vision fault injection: {@link VisionFaults}, {@link VisionFaultInjector} + * and {@link FaultedFrameStream}. + * + *

    + *
  1. No faults changes nothing.
    2. + *
  2. Every fault replays for the same seed, and turning one fault on does not move another's draws.
    3. + *
  3. Each fault does what it says, at about the asked-for rate.
    4. + *
  4. No fault produces a non-finite number, a non-positive radius or a sighting outside the image, at any + * level: a NaN reaching a consumer must be the consumer's own doing.
    5. + *
+ * + *

A made-up rig: 70 degrees, 640 x 400, 6 in forward, 10 in up, 22 degrees down; balls of 1.4 in radius. + */ +class VisionFaultsTest { + + private static final CameraLens LENS = CameraLens.fromHorizontalFov(70.0, 640, 400); + private static final CameraMount FRONT = CameraMount.front(6.0, 10.0, 22.0); + private static final long MS = 1_000_000L; + + /** Six balls in front of the robot, some near the image edges. */ + private static List sightings() { + GamePieceTracker tracker = new GamePieceTracker(8, FieldWorld.unbounded()); + tracker.updateRobotPose(0.0, 0.0, 0.0); + double[][] xy = {{24, 0}, {30, 8}, {30, -8}, {40, 14}, {20, -6}, {50, 0}}; + for (double[] p : xy) { + tracker.addLoosePiece(p[1] > 0 ? "red" : "blue", p[0], p[1], 1.4); + } + List seen = new SimCamera("t", LENS, FRONT).observe(tracker.getPieces(), 0.0, 0.0, 0.0); + assertEquals(6, seen.size(), "the rig must see every ball: " + seen); + return seen; + } + + private static final AprilTagLayout WALL = AprilTagLayout.builder() + .addTag(1, 72.0, -12.0, 10.0, Math.PI, 6.0) + .addTag(2, 72.0, 0.0, 10.0, Math.PI, 6.0) + .addTag(3, 72.0, 12.0, 10.0, Math.PI, 6.0) + .build(); + + private static SimTagCamera tagCamera() { + return new SimTagCamera(LENS, new CameraMount(0.0, 0.0, 10.0, 0.0, 0.0), 0.0, Double.POSITIVE_INFINITY, + 90.0, Collections.emptyList()); + } + + private static String describe(List list) { + StringBuilder s = new StringBuilder(); + for (CameraSighting c : list) { + s.append(c.getPieceId()).append(c.getKind()).append('@').append(c.getCentreXPx()).append(',') + .append(c.getCentreYPx()).append('r').append(c.getRadiusPx()).append(';'); + } + return s.toString(); + } + + // ------------------------------------------------------------------------------------- none + + @Test + void noFaultsPassesSightingsAndFramesThroughUnchanged() { + List in = sightings(); + VisionFaultInjector injector = new VisionFaultInjector(VisionFaults.none()); + assertEquals(describe(in), describe(injector.applyToSightings(in, LENS))); + assertTrue(VisionFaults.none().isNone()); + assertEquals("VisionFaults(none)", VisionFaults.none().toString()); + + FaultedFrameStream stream = new FaultedFrameStream(VisionFaults.none()); + List got = new ArrayList(); + for (int i = 0; i < 50; i++) { + stream.offer(i * 20 * MS, i * 20 * MS + 5 * MS, i); + for (FaultedFrameStream.Delivery d : stream.pollDue(i * 20 * MS + 5 * MS)) { + assertEquals(d.getFrame() * 20 * MS, d.getCaptureNanos()); + assertEquals(d.getFrame() * 20 * MS + 5 * MS, d.getDeliveredNanos()); + got.add(d.getFrame()); + } + } + for (int i = 0; i < 50; i++) { + assertEquals(Integer.valueOf(i), got.get(i)); + } + assertEquals(50, got.size()); + } + + // ---------------------------------------------------------------------------- determinism + + private static String sightingRun(VisionFaults faults, int frames) { + VisionFaultInjector injector = new VisionFaultInjector(faults); + List in = sightings(); + StringBuilder s = new StringBuilder(); + for (int i = 0; i < frames; i++) { + s.append(describe(injector.applyToSightings(in, LENS))).append('|'); + } + return s.toString(); + } + + @Test + void theSameSeedReplaysAndAnotherSeedDoesNot() { + VisionFaults a = VisionFaults.builder(7).pixelNoise(5.0, 2.0).missDetections(0.2).blackout(0.05, 3) + .falseDetections(0.3, "false", 3.0, 20.0, 0xFFFF00).wrongIds(0.2, "red", "blue").build(); + VisionFaults b = VisionFaults.builder(8).pixelNoise(5.0, 2.0).missDetections(0.2).blackout(0.05, 3) + .falseDetections(0.3, "false", 3.0, 20.0, 0xFFFF00).wrongIds(0.2, "red", "blue").build(); + assertEquals(sightingRun(a, 200), sightingRun(a, 200)); + assertNotEquals(sightingRun(a, 200), sightingRun(b, 200)); + } + + @Test + void turningOneFaultOnDoesNotMoveAnotherFaultsDraws() { + // The misses alone, and the misses with heavy noise on top: the same sightings must go missing. + VisionFaults missOnly = VisionFaults.builder(3).missDetections(0.3).build(); + VisionFaults missAndNoise = VisionFaults.builder(3).missDetections(0.3).pixelNoise(0.001, 0.0).build(); + VisionFaultInjector x = new VisionFaultInjector(missOnly); + VisionFaultInjector y = new VisionFaultInjector(missAndNoise); + List in = sightings(); + for (int i = 0; i < 200; i++) { + List idsX = ids(x.applyToSightings(in, LENS)); + List idsY = ids(y.applyToSightings(in, LENS)); + assertEquals(idsX, idsY, "frame " + i); + } + + // The drops alone, and the drops with jitter: the same frames must be dropped. + FaultedFrameStream dropOnly = new FaultedFrameStream( + VisionFaults.builder(3).dropFrames(0.3).build()); + FaultedFrameStream dropAndJitter = new FaultedFrameStream( + VisionFaults.builder(3).dropFrames(0.3).latency(0.0, 0.004).build()); + assertEquals(deliveredFrames(dropOnly, 300), deliveredFrames(dropAndJitter, 300)); + } + + private static List ids(List list) { + List out = new ArrayList(); + for (CameraSighting s : list) { + out.add(s.getPieceId()); + } + return out; + } + + /** Offers a frame every 20 ms for {@code frames} frames and returns what came out, in delivery order. */ + private static List deliveredFrames(FaultedFrameStream stream, int frames) { + List out = new ArrayList(); + for (int i = 0; i < frames + 50; i++) { + if (i < frames) { + stream.offer(i * 20 * MS, i * 20 * MS, i); + } + for (FaultedFrameStream.Delivery d : stream.pollDue(i * 20 * MS)) { + out.add(d.getFrame()); + } + } + return out; + } + + // ------------------------------------------------------------------------------- content faults + + @Test + void extremeNoiseNeverProducesANonFiniteNumberANonPositiveRadiusOrADiscOffTheImage() { + VisionFaultInjector injector = new VisionFaultInjector( + VisionFaults.builder(11).pixelNoise(400.0, 60.0).build()); + List in = sightings(); + int kept = 0; + for (int i = 0; i < 2000; i++) { + for (CameraSighting s : injector.applyToSightings(in, LENS)) { + kept++; + assertTrue(Double.isFinite(s.getCentreXPx()) && Double.isFinite(s.getCentreYPx()) + && Double.isFinite(s.getRadiusPx()) && Double.isFinite(s.getRangeInches()), s.toString()); + assertTrue(s.getRadiusPx() > 0.0, s.toString()); + double nearX = Math.max(0.0, Math.min(640.0, s.getCentreXPx())); + double nearY = Math.max(0.0, Math.min(400.0, s.getCentreYPx())); + assertTrue(Math.hypot(s.getCentreXPx() - nearX, s.getCentreYPx() - nearY) < s.getRadiusPx(), + "the disc overlaps the image: " + s); + } + } + assertTrue(kept > 0 && kept < 2000 * 6, "noise this heavy must drop some sightings and keep some: " + kept); + } + + @Test + void noiseHasAboutTheAskedForSpread() { + VisionFaultInjector injector = new VisionFaultInjector(VisionFaults.builder(5).pixelNoise(3.0, 0.0).build()); + CameraSighting centreBall = sightings().get(0); + double sum = 0.0; + double sumSq = 0.0; + int n = 4000; + for (int i = 0; i < n; i++) { + double dx = injector.applyToSightings(Collections.singletonList(centreBall), LENS).get(0).getCentreXPx() + - centreBall.getCentreXPx(); + sum += dx; + sumSq += dx * dx; + } + double mean = sum / n; + double sd = Math.sqrt(sumSq / n - mean * mean); + assertEquals(0.0, mean, 0.2); + assertEquals(3.0, sd, 0.15); + } + + @Test + void missesAndBlackoutsRemoveSightingsAtAboutTheRate() { + VisionFaultInjector miss = new VisionFaultInjector(VisionFaults.builder(2).missDetections(0.25).build()); + List in = sightings(); + int seen = 0; + for (int i = 0; i < 1000; i++) { + seen += miss.applyToSightings(in, LENS).size(); + } + assertEquals(0.25, 1.0 - seen / 6000.0, 0.03); + + VisionFaultInjector blackout = new VisionFaultInjector(VisionFaults.builder(2).blackout(0.05, 5).build()); + int run = 0; + List runs = new ArrayList(); + for (int i = 0; i < 2000; i++) { + if (blackout.applyToSightings(in, LENS).isEmpty()) { + run++; + } else if (run > 0) { + runs.add(run); + run = 0; + } + } + assertFalse(runs.isEmpty(), "a 5% blackout chance over 2000 frames must black some out"); + for (int r : runs) { + assertEquals(0, r % 5, "a blackout lasts whole multiples of 5 frames, back to back at most: " + runs); + } + assertEquals(blackout.getBlackedOutFrames(), sum(runs) + run); + assertEquals(2000, blackout.getFramesSeen()); + } + + private static int sum(List values) { + int total = 0; + for (int v : values) { + total += v; + } + return total; + } + + @Test + void falseDetectionsAreInTheImageFiniteAndCarryNoPieceId() { + VisionFaultInjector injector = new VisionFaultInjector( + VisionFaults.builder(4).falseDetections(0.5, "phantom", 3.0, 25.0, 0xFFFFFF).build()); + List in = sightings(); + int phantoms = 0; + for (int i = 0; i < 1000; i++) { + for (CameraSighting s : injector.applyToSightings(in, LENS)) { + if (s.getPieceId() == VisionFaultInjector.FALSE_PIECE_ID) { + phantoms++; + assertEquals("phantom", s.getKind()); + assertTrue(s.getRadiusPx() >= 3.0 && s.getRadiusPx() <= 25.0, s.toString()); + assertTrue(s.getCentreXPx() >= 0.0 && s.getCentreXPx() <= 640.0, s.toString()); + assertTrue(Double.isFinite(s.getRangeInches()) && s.getRangeInches() > 0.0, s.toString()); + } + } + } + assertEquals(500, phantoms, 50); + } + + @Test + void wrongIdsRelabelPiecesAsAnotherKindAndKeepThePieceId() { + VisionFaultInjector injector = new VisionFaultInjector( + VisionFaults.builder(9).wrongIds(1.0, "red", "blue").build()); + List in = sightings(); + List out = injector.applyToSightings(in, LENS); + assertEquals(in.size(), out.size()); + for (int i = 0; i < in.size(); i++) { + assertEquals(in.get(i).getPieceId(), out.get(i).getPieceId()); + assertNotEquals(in.get(i).getKind(), out.get(i).getKind()); + } + } + + @Test + void tagFaultsMislabelMissAndInventTagsWithWrongGeometry() { + SimTagCamera camera = tagCamera(); + List truth = camera.observe(WALL, 0.0, 0.0, 0.0); + assertEquals(3, truth.size()); + + VisionFaultInjector wrong = new VisionFaultInjector(VisionFaults.builder(1).wrongIds(1.0).build()); + List relabelled = wrong.applyToTags(truth, WALL, camera, 0.0, 0.0, 0.0); + for (int i = 0; i < truth.size(); i++) { + assertNotEquals(truth.get(i).getId(), relabelled.get(i).getId()); + assertSame(WALL.getTag(relabelled.get(i).getId()), relabelled.get(i).getTag()); + assertEquals(truth.get(i).getRobotToTag().getXInches(), relabelled.get(i).getRobotToTag().getXInches()); + } + + VisionFaultInjector phantom = new VisionFaultInjector( + VisionFaults.builder(1).falseDetections(1.0, "tag", 1.0, 1.0, 0).build()); + int found = 0; + for (int i = 0; i < 50; i++) { + List out = phantom.applyToTags(truth, WALL, camera, 0.0, 0.0, 0.0); + if (out.size() == 4) { + found++; + TagSighting p = out.get(3); + assertSame(WALL.getTag(p.getId()), p.getTag(), "a false tag claims a layout tag"); + // Where the robot would be if the tag really were the layout tag: not where it is. + RigidTransform3 solved = p.getTag().getFieldToTag().compose(p.getRobotToTag().inverse()); + assertTrue(Math.hypot(solved.getXInches(), solved.getYInches()) > 1.0, + "a false tag must not solve to the true pose: " + solved); + assertTrue(p.getCentreXPx() >= 0.0 && p.getCentreXPx() <= 640.0); + } + } + assertTrue(found > 40, "false tags placed: " + found); + + VisionFaultInjector noisy = new VisionFaultInjector(VisionFaults.builder(1).tagAngleNoise(80.0).build()); + for (int i = 0; i < 500; i++) { + for (TagSighting t : noisy.applyToTags(truth, WALL, camera, 0.0, 0.0, 0.0)) { + assertTrue(Math.abs(t.getTxDegrees()) <= 35.0 + 1e-9, "tx stays inside the 70 degree image: " + t); + assertTrue(t.getCentreXPx() >= 0.0 && t.getCentreXPx() <= 640.0, t.toString()); + assertTrue(t.getCentreYPx() >= 0.0 && t.getCentreYPx() <= 400.0, t.toString()); + } + } + } + + @Test + void frameFaultsPaintBlackoutsPatchesBlobsAndNoiseAndRelabelWhatTheyCover() { + GamePieceTracker tracker = new GamePieceTracker(4, FieldWorld.unbounded()); + tracker.updateRobotPose(0.0, 0.0, 0.0); + int id = tracker.addLoosePiece("red", 30.0, 0.0, 1.4); + List pieces = tracker.getPieces(); + SimFrameRenderer renderer = SimFrameRenderer.builder(CameraLens.fromHorizontalFov(70.0, 160, 100), FRONT) + .build(); + SimFrame frame = renderer.render(pieces, 0.0, 0.0, 0.0, CameraOccluders.none(), AprilTagLayout.empty()); + assertTrue(frame.pixelCountOfPiece(id) > 0); + + SimFrame black = new VisionFaultInjector(VisionFaults.builder(1).blackout(1.0, 1).build()).applyToFrame(frame); + for (byte b : black.getRgbBytes()) { + assertEquals(0, b); + } + assertEquals(0, black.pixelCountOfPiece(id)); + assertTrue(frame.pixelCountOfPiece(id) > 0, "the input frame is unchanged"); + + SimFrame patched = new VisionFaultInjector(VisionFaults.builder(1).occlusionPatch(1.0, 1.0).build()) + .applyToFrame(frame); + assertEquals(0, patched.pixelCountOfPiece(id), "a full-frame patch hides the piece"); + assertEquals(0x303030, patched.getRgb(80, 50)); + + SimFrame blob = new VisionFaultInjector( + VisionFaults.builder(1).falseDetections(1.0, "x", 10.0, 10.0, 0x00FF00).build()).applyToFrame(frame); + int green = 0; + for (int y = 0; y < 100; y++) { + for (int x = 0; x < 160; x++) { + if (blob.getRgb(x, y) == 0x00FF00) { + green++; + assertEquals(-1, blob.getPieceIdAt(x, y), "a false blob is no piece"); + } + } + } + assertTrue(green > 100, "a 10 px blob: " + green + " pixels"); + + SimFrame noisy = new VisionFaultInjector(VisionFaults.builder(1).frameNoise(500.0).build()) + .applyToFrame(frame); + assertNotEquals(java.util.Arrays.toString(frame.getRgbBytes()), java.util.Arrays.toString(noisy.getRgbBytes())); + assertEquals(frame.pixelCountOfPiece(id), noisy.pixelCountOfPiece(id), "noise does not move labels"); + } + + // -------------------------------------------------------------------------------- timing faults + + @Test + void latencyAndJitterDelayFramesWithoutReorderingThemOrDeliveringBeforeCapture() { + FaultedFrameStream stream = new FaultedFrameStream( + VisionFaults.builder(6).latency(0.100, 0.060).build()); + long lastCapture = Long.MIN_VALUE; + double sumDelay = 0.0; + int n = 0; + for (int i = 0; i < 2000; i++) { + long now = i * 20 * MS; + stream.offer(now, now, i); + for (FaultedFrameStream.Delivery d : stream.pollDue(now)) { + assertTrue(d.getCaptureNanos() > lastCapture, "in order"); + lastCapture = d.getCaptureNanos(); + assertTrue(d.getDeliveredNanos() >= d.getCaptureNanos()); + assertTrue(now >= d.getDeliveredNanos()); + sumDelay += (d.getDeliveredNanos() - d.getCaptureNanos()) / 1e9; + n++; + } + } + double meanDelay = sumDelay / n; + // 100 ms plus jitter that is symmetric before the in-order rule pushes some frames later. + assertTrue(meanDelay >= 0.095 && meanDelay <= 0.130, "mean delay " + meanDelay); + } + + @Test + void droppedFramesAreNeverDeliveredAtAboutTheRate() { + FaultedFrameStream stream = new FaultedFrameStream( + VisionFaults.builder(6).dropFrames(0.2).build()); + List out = deliveredFrames(stream, 2000); + assertEquals(0.2, 1.0 - out.size() / 2000.0, 0.03); + assertEquals(2000, stream.getOffered()); + assertEquals(2000 - out.size(), stream.getDropped()); + } + + @Test + void aRepeatedFrameComesAgainWithTheSameStampOnTheNextPoll() { + FaultedFrameStream stream = new FaultedFrameStream( + VisionFaults.builder(6).repeatFrames(0.3).build()); + List> all = new ArrayList>(); + for (int i = 0; i < 500; i++) { + stream.offer(i * 20 * MS, i * 20 * MS, i); + all.addAll(stream.pollDue(i * 20 * MS)); + } + int repeats = 0; + for (int i = 1; i < all.size(); i++) { + if (all.get(i).isRepeat()) { + repeats++; + assertEquals(all.get(i - 1).getCaptureNanos(), all.get(i).getCaptureNanos(), + "a repeat follows its original, same stamp"); + assertSame(all.get(i - 1).getFrame(), all.get(i).getFrame()); + } + } + assertEquals(stream.getRepeated(), repeats); + assertEquals(0.3, repeats / 500.0, 0.06); + } + + @Test + void aReorderedFrameArrivesAfterTheNextOneSoStampsGoBackwards() { + FaultedFrameStream stream = new FaultedFrameStream( + VisionFaults.builder(6).reorderFrames(0.2).build()); + List> all = new ArrayList>(); + for (int i = 0; i < 1050; i++) { + if (i < 1000) { + stream.offer(i * 20 * MS, i * 20 * MS, i); + } + all.addAll(stream.pollDue(i * 20 * MS)); + } + // A frame flagged out of order is older than one already delivered; every other frame is newer than + // everything before it. (Two held frames released on one poll can come out oldest first, so counting + // backward steps between neighbours would undercount.) + long newest = Long.MIN_VALUE; + int outOfOrder = 0; + List out = new ArrayList(); + for (FaultedFrameStream.Delivery d : all) { + out.add(d.getFrame()); + if (d.isOutOfOrder()) { + outOfOrder++; + assertTrue(d.getCaptureNanos() < newest, "frame " + d.getFrame() + " came after a newer one"); + } else { + assertTrue(d.getCaptureNanos() > newest, "frame " + d.getFrame() + " is in order"); + } + newest = Math.max(newest, d.getCaptureNanos()); + } + assertTrue(outOfOrder > 100, "out-of-order deliveries: " + outOfOrder); + // Every frame comes out once, except a run of held frames at the very end, each waiting on a next + // frame that is never offered: those are the last frames, and they are still in flight. + int neverDelivered = 1000 - out.size(); + List sorted = new ArrayList(out); + Collections.sort(sorted); + for (int i = 0; i < sorted.size(); i++) { + assertEquals(i, (int) sorted.get(i)); + } + assertTrue(neverDelivered <= 10, "frames never delivered: " + neverDelivered); + assertEquals(neverDelivered, stream.inFlight(), "the undelivered frames are still held"); + assertTrue(outOfOrder <= stream.getReordered() && outOfOrder >= stream.getReordered() - neverDelivered, + outOfOrder + " out of order, " + stream.getReordered() + " reordered, " + neverDelivered + " held"); + } + + @Test + void badArgumentsAreRefused() { + assertThrows(IllegalArgumentException.class, () -> VisionFaults.builder(1).dropFrames(1.5)); + assertThrows(IllegalArgumentException.class, () -> VisionFaults.builder(1).latency(-0.1, 0.0)); + assertThrows(IllegalArgumentException.class, () -> VisionFaults.builder(1).pixelNoise(Double.NaN, 0.0)); + assertThrows(IllegalArgumentException.class, () -> VisionFaults.builder(1).blackout(0.5, 0)); + assertThrows(IllegalArgumentException.class, () -> VisionFaults.builder(1).occlusionPatch(0.5, 0.0)); + assertThrows(IllegalArgumentException.class, + () -> VisionFaults.builder(1).falseDetections(0.5, "x", 5.0, 2.0, 0)); + assertThrows(IllegalArgumentException.class, () -> new VisionFaultInjector(null)); + FaultedFrameStream stream = new FaultedFrameStream(VisionFaults.none()); + stream.offer(10 * MS, 10 * MS, 1); + assertThrows(IllegalArgumentException.class, () -> stream.offer(5 * MS, 5 * MS, 2), + "frames are offered in capture order"); + assertThrows(IllegalArgumentException.class, () -> stream.offer(20 * MS, 19 * MS, 3), + "ready before captured"); + } +} From 2c6abde59ab1f5ca382212a4c987b115de3d1039 Mon Sep 17 00:00:00 2001 From: featurescript <114271535+featurescript@users.noreply.github.com> Date: Sun, 1 Nov 2026 15:54:05 -0800 Subject: [PATCH 2/4] Split the noise streams, count held-behind-held frames, fix the reorder javadoc --- .../simloop/vision/FaultedFrameStream.java | 15 ++- .../simloop/vision/VisionFaultInjector.java | 22 ++-- .../simloop/vision/VisionFaults.java | 6 +- .../simloop/vision/VisionFaultsTest.java | 121 ++++++++++++++++++ 4 files changed, 149 insertions(+), 15 deletions(-) diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/FaultedFrameStream.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/FaultedFrameStream.java index 088b9d9..abaaccd 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/vision/FaultedFrameStream.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/FaultedFrameStream.java @@ -23,10 +23,11 @@ *

  • Latency: delivery is the caller's time plus {@link VisionFaults#getExtraLatencySeconds()} plus * a uniform shift of up to {@link VisionFaults#getLatencyJitterSeconds()} either way, never before the * capture. A frame never overtakes the one offered before it because of jitter: it waits for it.
    • - *
  • Reorder: the frame is held back until the next frame offered has been delivered, and comes at - * the start of the next {@link #pollDue} call after that, so its capture stamp is older than the one - * delivered before it. Two held in a row come out newest first. A held frame with no next frame is - * never delivered.
    • + *
  • Reorder: the frame is held back until the next frame that is not dropped has been delivered, + * and comes at the start of the next {@link #pollDue} call after that, so its capture stamp is older + * than the one delivered before it. A dropped frame does not release it: it waits for the next + * delivered one. Two held in a row come out newest first. A held frame with no later delivered frame + * is never delivered.
    • *
  • Repeat: after delivery, the same frame (same stamp, same contents object) is delivered once * more, at the start of the next {@link #pollDue} call that is at least one nanosecond later.
    • * @@ -159,6 +160,7 @@ public void offer(long captureNanos, long readyNanos, T frame) { lastOfferedCaptureNanos = captureNanos; offered++; if (faults.getDropProbability() > 0.0 && dropRandom.nextFraction() < faults.getDropProbability()) { + // A held-back frame stays held: the next frame that is not dropped releases it. dropped++; return; } @@ -240,6 +242,11 @@ public int inFlight() { held++; } } + for (Pending p : late) { + for (Pending f = p.follower; f != null; f = f.follower) { + held++; + } + } for (Pending f = heldBack; f != null; f = f.follower) { held++; } diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaultInjector.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaultInjector.java index 08ae8a7..135c1fd 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaultInjector.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaultInjector.java @@ -49,7 +49,10 @@ public final class VisionFaultInjector { private static final int PATCH_RGB = 0x303030; private final VisionFaults faults; - private final SeededRandom noiseRandom; + private final SeededRandom centreNoiseRandom; + private final SeededRandom radiusNoiseRandom; + private final SeededRandom tagAngleNoiseRandom; + private final SeededRandom pixelNoiseRandom; private final SeededRandom missRandom; private final SeededRandom blackoutRandom; private final SeededRandom patchRandom; @@ -69,7 +72,10 @@ public VisionFaultInjector(VisionFaults faults) { throw new IllegalArgumentException("faults must not be null; use VisionFaults.none()"); } this.faults = faults; - this.noiseRandom = stream(faults.getSeed(), 1); + this.centreNoiseRandom = stream(faults.getSeed(), 1); + this.radiusNoiseRandom = stream(faults.getSeed(), 7); + this.tagAngleNoiseRandom = stream(faults.getSeed(), 8); + this.pixelNoiseRandom = stream(faults.getSeed(), 9); this.missRandom = stream(faults.getSeed(), 2); this.blackoutRandom = stream(faults.getSeed(), 3); this.patchRandom = stream(faults.getSeed(), 4); @@ -123,9 +129,9 @@ public List applyToSightings(List sightings, Cam if (missed()) { continue; } - double x = s.getCentreXPx() + gaussian(noiseRandom, faults.getCentreNoiseStdDevPx()); - double y = s.getCentreYPx() + gaussian(noiseRandom, faults.getCentreNoiseStdDevPx()); - double r = s.getRadiusPx() + gaussian(noiseRandom, faults.getRadiusNoiseStdDevPx()); + double x = s.getCentreXPx() + gaussian(centreNoiseRandom, faults.getCentreNoiseStdDevPx()); + double y = s.getCentreYPx() + gaussian(centreNoiseRandom, faults.getCentreNoiseStdDevPx()); + double r = s.getRadiusPx() + gaussian(radiusNoiseRandom, faults.getRadiusNoiseStdDevPx()); if (!(r > 0.0) || !discOverlapsImage(x, y, r, lens)) { continue; } @@ -190,8 +196,8 @@ public List applyToTags(List tags, AprilTagLayout layo TagSighting t = s; double sigma = faults.getTagAngleNoiseStdDevDegrees(); if (sigma > 0.0) { - t = withAngles(s, s.getTxDegrees() + gaussian(noiseRandom, sigma), - s.getTyDegrees() + gaussian(noiseRandom, sigma), lens); + t = withAngles(s, s.getTxDegrees() + gaussian(tagAngleNoiseRandom, sigma), + s.getTyDegrees() + gaussian(tagAngleNoiseRandom, sigma), lens); if (t == null) { continue; } @@ -338,7 +344,7 @@ public SimFrame applyToFrame(SimFrame frame) { double sigma = faults.getPixelNoiseStdDevCounts(); if (sigma > 0.0) { for (int i = 0; i < rgb.length; i++) { - long v = Math.round((rgb[i] & 0xFF) + gaussian(noiseRandom, sigma)); + long v = Math.round((rgb[i] & 0xFF) + gaussian(pixelNoiseRandom, sigma)); rgb[i] = (byte) Math.max(0L, Math.min(255L, v)); } } diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaults.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaults.java index a4995a5..17e0a0d 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaults.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaults.java @@ -134,7 +134,7 @@ public double getRepeatProbability() { return repeatProbability; } - /** {@return the chance a frame is held back and delivered after the frame captured next} */ + /** {@return the chance a frame is held back and delivered after the next frame that is not dropped} */ public double getReorderProbability() { return reorderProbability; } @@ -348,8 +348,8 @@ public Builder repeatFrames(double probability) { } /** - * Deliver a frame after the one captured next, so capture stamps go backwards. A frame held back - * when no later frame is ever offered is not delivered. + * Deliver a frame after the next frame that is not dropped, so capture stamps go backwards. A frame + * held back when no later frame is ever delivered is not delivered either. * * @param probability chance each frame is held back, in {@code [0, 1]} * @return this builder diff --git a/SimLoop/src/test/java/org/horizon36596/simloop/vision/VisionFaultsTest.java b/SimLoop/src/test/java/org/horizon36596/simloop/vision/VisionFaultsTest.java index 5ff91a9..81c2e80 100644 --- a/SimLoop/src/test/java/org/horizon36596/simloop/vision/VisionFaultsTest.java +++ b/SimLoop/src/test/java/org/horizon36596/simloop/vision/VisionFaultsTest.java @@ -15,6 +15,7 @@ import org.horizon36596.simloop.field.GamePiece; import org.horizon36596.simloop.field.GamePieceTracker; import org.horizon36596.simloop.field.ObstacleBox; +import org.horizon36596.simloop.field.SeededRandom; import org.junit.jupiter.api.Test; /** @@ -132,6 +133,39 @@ void turningOneFaultOnDoesNotMoveAnotherFaultsDraws() { assertEquals(idsX, idsY, "frame " + i); } + // Centre noise alone, and centre noise with radius noise: every centre must move the same way. + VisionFaultInjector centreOnly = new VisionFaultInjector(VisionFaults.builder(3).pixelNoise(4.0, 0.0).build()); + VisionFaultInjector centreAndRadius = new VisionFaultInjector( + VisionFaults.builder(3).pixelNoise(4.0, 0.001).build()); + for (int i = 0; i < 200; i++) { + List a = centreOnly.applyToSightings(in, LENS); + List b = centreAndRadius.applyToSightings(in, LENS); + assertEquals(a.size(), b.size(), "frame " + i); + for (int k = 0; k < a.size(); k++) { + assertEquals(a.get(k).getCentreXPx(), b.get(k).getCentreXPx(), "frame " + i + " sighting " + k); + assertEquals(a.get(k).getCentreYPx(), b.get(k).getCentreYPx(), "frame " + i + " sighting " + k); + } + } + + // Tag-angle noise alone, and with pixel noise on the rendered frames in between: the same tag angles. + SimTagCamera camera = tagCamera(); + List truth = camera.observe(WALL, 0.0, 0.0, 0.0); + SimFrame frame = smallFrame(); + VisionFaultInjector anglesOnly = new VisionFaultInjector(VisionFaults.builder(3).tagAngleNoise(2.0).build()); + VisionFaultInjector anglesAndPixels = new VisionFaultInjector( + VisionFaults.builder(3).tagAngleNoise(2.0).frameNoise(10.0).build()); + for (int i = 0; i < 50; i++) { + List a = anglesOnly.applyToTags(truth, WALL, camera, 0.0, 0.0, 0.0); + List b = anglesAndPixels.applyToTags(truth, WALL, camera, 0.0, 0.0, 0.0); + anglesOnly.applyToFrame(frame); + anglesAndPixels.applyToFrame(frame); + assertEquals(a.size(), b.size(), "frame " + i); + for (int k = 0; k < a.size(); k++) { + assertEquals(a.get(k).getTxDegrees(), b.get(k).getTxDegrees(), "frame " + i + " tag " + k); + assertEquals(a.get(k).getTyDegrees(), b.get(k).getTyDegrees(), "frame " + i + " tag " + k); + } + } + // The drops alone, and the drops with jitter: the same frames must be dropped. FaultedFrameStream dropOnly = new FaultedFrameStream( VisionFaults.builder(3).dropFrames(0.3).build()); @@ -140,6 +174,15 @@ void turningOneFaultOnDoesNotMoveAnotherFaultsDraws() { assertEquals(deliveredFrames(dropOnly, 300), deliveredFrames(dropAndJitter, 300)); } + /** A small rendered frame with one ball in it. */ + private static SimFrame smallFrame() { + GamePieceTracker tracker = new GamePieceTracker(4, FieldWorld.unbounded()); + tracker.updateRobotPose(0.0, 0.0, 0.0); + tracker.addLoosePiece("red", 30.0, 0.0, 1.4); + return SimFrameRenderer.builder(CameraLens.fromHorizontalFov(70.0, 40, 25), FRONT).build() + .render(tracker.getPieces(), 0.0, 0.0, 0.0, CameraOccluders.none(), AprilTagLayout.empty()); + } + private static List ids(List list) { List out = new ArrayList(); for (CameraSighting s : list) { @@ -458,6 +501,84 @@ void aReorderedFrameArrivesAfterTheNextOneSoStampsGoBackwards() { outOfOrder + " out of order, " + stream.getReordered() + " reordered, " + neverDelivered + " held"); } + /** True when the first draws of stream {@code salt} for {@code seed} fall below (true) or not (false) p. */ + private static boolean drawsAre(int seed, int salt, double p, boolean... below) { + SeededRandom r =VisionFaultInjector.stream(seed, salt); + for (boolean b : below) { + if ((r.nextFraction() < p) != b) { + return false; + } + } + return true; + } + + /** A seed whose first draws from one fault's stream fall below (true) or at or above (false) {@code p}. */ + private static int seedWhere(int salt, double p, boolean... below) { + for (int seed = 0; seed < 100_000; seed++) { + if (drawsAre(seed, salt, p, below)) { + return seed; + } + } + throw new AssertionError("no seed found"); + } + + @Test + void inFlightCountsAFrameHeldBehindAnotherHeldFrame() { + // Frames 0 and 1 held, frame 2 not (reorder draws on stream salt 13). Delivering 2 releases 1, and 0 + // still waits behind 1: two frames in flight, not one. + int seed = seedWhere(13, 0.5, true, true, false); + FaultedFrameStream stream = new FaultedFrameStream( + VisionFaults.builder(seed).reorderFrames(0.5).build()); + for (int i = 0; i < 3; i++) { + stream.offer(i * 20 * MS, i * 20 * MS, i); + } + assertEquals(1, stream.pollDue(40 * MS).size()); + assertEquals(2, stream.inFlight(), "frame 1 released, frame 0 still behind it"); + assertEquals(Integer.valueOf(1), stream.pollDue(60 * MS).get(0).getFrame()); + assertEquals(1, stream.inFlight()); + assertEquals(Integer.valueOf(0), stream.pollDue(80 * MS).get(0).getFrame()); + assertEquals(0, stream.inFlight()); + + // And at every step of a long run: in flight is offered, less dropped, less delivered. + FaultedFrameStream busy = new FaultedFrameStream( + VisionFaults.builder(9).reorderFrames(0.5).dropFrames(0.2).latency(0.0, 0.01).build()); + for (int i = 0; i < 600; i++) { + if (i < 500) { + busy.offer(i * 20 * MS, i * 20 * MS + 5 * MS, i); + } + busy.pollDue(i * 20 * MS); + assertEquals(busy.getOffered() - busy.getDropped() - busy.getDelivered(), busy.inFlight(), "step " + i); + } + } + + @Test + void aDroppedFrameDoesNotReleaseAHeldOneTheNextDeliveredFrameDoes() { + // Frame 0 kept and held, frame 1 dropped, frame 2 kept and not held. Drops draw on salt 11 for every + // frame; reorders on salt 13 only for the frames that are kept. + int seed = -1; + for (int s = 0; s < 100_000 && seed < 0; s++) { + if (drawsAre(s, 11, 0.5, false, true, false) && drawsAre(s, 13, 0.5, true, false)) { + seed = s; + } + } + assertTrue(seed >= 0, "no seed found"); + FaultedFrameStream stream = new FaultedFrameStream( + VisionFaults.builder(seed).dropFrames(0.5).reorderFrames(0.5).build()); + stream.offer(0, 0, 0); + assertTrue(stream.pollDue(0).isEmpty(), "frame 0 is held"); + stream.offer(20 * MS, 20 * MS, 1); + assertEquals(1, stream.getDropped()); + assertTrue(stream.pollDue(20 * MS).isEmpty(), "the dropped frame 1 does not release frame 0"); + stream.offer(40 * MS, 40 * MS, 2); + List> atTwo = stream.pollDue(40 * MS); + assertEquals(1, atTwo.size()); + assertEquals(Integer.valueOf(2), atTwo.get(0).getFrame()); + List> after = stream.pollDue(60 * MS); + assertEquals(1, after.size()); + assertEquals(Integer.valueOf(0), after.get(0).getFrame()); + assertTrue(after.get(0).isOutOfOrder()); + } + @Test void badArgumentsAreRefused() { assertThrows(IllegalArgumentException.class, () -> VisionFaults.builder(1).dropFrames(1.5)); From 4143c729fc0d458ee66ba5b1e94726645de68b79 Mon Sep 17 00:00:00 2001 From: featurescript <114271535+featurescript@users.noreply.github.com> Date: Sun, 1 Nov 2026 16:00:46 -0800 Subject: [PATCH 3/4] Pin the timing-fault books under all four faults, the 1.0 boundaries and empty Limelight results reorderFrames(1.0) never delivers a frame: documented in VisionFaults.Builder#reorderFrames and FaultedFrameStream, pinned by a test. --- .../simloop/vision/FaultedFrameStream.java | 3 +- .../simloop/vision/VisionFaults.java | 3 +- .../FakeLimelight3AFaultsTest.java | 30 +++++ .../simloop/vision/VisionFaultsTest.java | 104 ++++++++++++++++++ 4 files changed, 138 insertions(+), 2 deletions(-) diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/FaultedFrameStream.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/FaultedFrameStream.java index abaaccd..05e318c 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/vision/FaultedFrameStream.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/FaultedFrameStream.java @@ -27,7 +27,8 @@ * and comes at the start of the next {@link #pollDue} call after that, so its capture stamp is older * than the one delivered before it. A dropped frame does not release it: it waits for the next * delivered one. Two held in a row come out newest first. A held frame with no later delivered frame - * is never delivered. + * is never delivered, so at a reorder probability of 1.0 every frame waits on the next and nothing + is ever delivered. *
  • Repeat: after delivery, the same frame (same stamp, same contents object) is delivered once * more, at the start of the next {@link #pollDue} call that is at least one nanosecond later.
    • * diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaults.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaults.java index 17e0a0d..47d6f11 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaults.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaults.java @@ -349,7 +349,8 @@ public Builder repeatFrames(double probability) { /** * Deliver a frame after the next frame that is not dropped, so capture stamps go backwards. A frame - * held back when no later frame is ever delivered is not delivered either. + * held back when no later frame is ever delivered is not delivered either. At 1.0 every frame is held + * behind the next, so no frame is ever delivered: use it only to model a stream that never arrives. * * @param probability chance each frame is held back, in {@code [0, 1]} * @return this builder diff --git a/SimLoop/src/test/java/org/horizon36596/simloop/fakehardware/FakeLimelight3AFaultsTest.java b/SimLoop/src/test/java/org/horizon36596/simloop/fakehardware/FakeLimelight3AFaultsTest.java index f535d73..91bf3c3 100644 --- a/SimLoop/src/test/java/org/horizon36596/simloop/fakehardware/FakeLimelight3AFaultsTest.java +++ b/SimLoop/src/test/java/org/horizon36596/simloop/fakehardware/FakeLimelight3AFaultsTest.java @@ -1,6 +1,7 @@ package org.horizon36596.simloop.fakehardware; import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; import static org.junit.jupiter.api.Assertions.assertNotEquals; import static org.junit.jupiter.api.Assertions.assertThrows; import static org.junit.jupiter.api.Assertions.assertTrue; @@ -213,4 +214,33 @@ void aBlackoutEmptiesResults() { assertTrue(empty >= 10, "blacked-out results: " + empty); assertNotEquals(400, empty); } + + private static boolean finite(Pose3D pose) { + return Double.isFinite(pose.getPosition().x) && Double.isFinite(pose.getPosition().y) + && Double.isFinite(pose.getPosition().z) && Double.isFinite(pose.getOrientation().getRoll()) + && Double.isFinite(pose.getOrientation().getPitch()) && Double.isFinite(pose.getOrientation().getYaw()); + } + + @Test + void aResultWithNoTagsIsInvalidAndItsBotposesAreFiniteUnderEveryFaultAtOnce() { + FakeLimelight3A ll = builder().frameRateHz(25.0).faults(VisionFaults.builder(5).blackout(0.05, 10) + .reorderFrames(0.3).missDetections(0.9).build()).build(); + ll.start(); + int noTags = 0; + int withTags = 0; + for (int i = 0; i < 600; i++) { + ll.setRobotPose(-10.0 + i * 0.03, 2.0, 0.05); + ll.update(TICK); + LLResult r = ll.getLatestResult(); + assertTrue(finite(r.getBotpose()) && finite(r.getBotpose_MT2()), "tick " + i); + if (r.getFiducialResults().isEmpty()) { + noTags++; + assertFalse(r.isValid(), "tick " + i + ": a result with no tags is not valid"); + } else { + withTags++; + } + } + assertTrue(noTags > 100 && withTags > 20, noTags + " results with no tags, " + withTags + " with"); + assertTrue(ll.getFaultStream().getReordered() > 0); + } } diff --git a/SimLoop/src/test/java/org/horizon36596/simloop/vision/VisionFaultsTest.java b/SimLoop/src/test/java/org/horizon36596/simloop/vision/VisionFaultsTest.java index 81c2e80..8b945e4 100644 --- a/SimLoop/src/test/java/org/horizon36596/simloop/vision/VisionFaultsTest.java +++ b/SimLoop/src/test/java/org/horizon36596/simloop/vision/VisionFaultsTest.java @@ -579,6 +579,110 @@ void aDroppedFrameDoesNotReleaseAHeldOneTheNextDeliveredFrameDoes() { assertTrue(after.get(0).isOutOfOrder()); } + @Test + void allFourTimingFaultsTogetherLoseNoFrameAndDeliverNoneTwice() { + FaultedFrameStream stream = new FaultedFrameStream(VisionFaults.builder(21) + .latency(0.05, 0.03).dropFrames(0.2).repeatFrames(0.2).reorderFrames(0.2).build()); + boolean[] seen = new boolean[400]; + boolean[] repeatSeen = new boolean[400]; + List> previousPoll = Collections.emptyList(); + long nonRepeats = 0; + int repeats = 0; + for (int i = 0; i < 420; i++) { + long now = i * 20 * MS; + if (i < 400) { + stream.offer(now, now, i); + } + List> poll = stream.pollDue(now); + int newNonRepeats = 0; + for (FaultedFrameStream.Delivery d : poll) { + int f = d.getFrame(); + assertTrue(d.getDeliveredNanos() >= d.getCaptureNanos(), "frame " + f + " delivered before capture"); + assertTrue(d.getDeliveredNanos() <= now, "frame " + f + " delivered after the poll"); + if (d.isRepeat()) { + repeats++; + assertTrue(seen[f], "frame " + f + " repeated before it was delivered"); + assertFalse(repeatSeen[f], "frame " + f + " repeated twice"); + repeatSeen[f] = true; + boolean originalInPreviousPoll = false; + for (FaultedFrameStream.Delivery p : previousPoll) { + if (!p.isRepeat() && p.getFrame().equals(d.getFrame())) { + originalInPreviousPoll = true; + assertEquals(p.getCaptureNanos(), d.getCaptureNanos()); + } + } + assertTrue(originalInPreviousPoll, "the repeat of frame " + f + " follows the poll of its original"); + } else { + assertFalse(seen[f], "frame " + f + " delivered twice"); + seen[f] = true; + nonRepeats++; + newNonRepeats++; + } + } + // In flight holds every offered frame neither dropped nor delivered, plus the repeats this poll owes. + long owedRepeats = stream.inFlight() - (stream.getOffered() - stream.getDropped() - nonRepeats); + assertTrue(owedRepeats >= 0 && owedRepeats <= newNonRepeats, "step " + i + ": " + owedRepeats); + previousPoll = poll; + } + // Drained: nothing owed, so the books balance exactly. + assertEquals(stream.getOffered(), nonRepeats + stream.getDropped() + stream.inFlight()); + assertEquals(400, stream.getOffered()); + assertEquals(stream.getRepeated(), repeats); + assertEquals(stream.getDelivered(), nonRepeats + repeats); + assertTrue(stream.getDropped() > 40 && repeats > 20 && stream.getReordered() > 20, + stream.getDropped() + " dropped, " + repeats + " repeated, " + stream.getReordered() + " reordered"); + assertTrue(stream.inFlight() <= 10, "only a held tail stays in flight: " + stream.inFlight()); + } + + @Test + void dropFramesAtOneDropsEveryFrame() { + FaultedFrameStream stream = new FaultedFrameStream( + VisionFaults.builder(6).dropFrames(1.0).repeatFrames(1.0).reorderFrames(1.0).build()); + assertTrue(deliveredFrames(stream, 200).isEmpty()); + assertEquals(200, stream.getOffered()); + assertEquals(200, stream.getDropped()); + assertEquals(0, stream.getDelivered()); + assertEquals(0, stream.getReordered()); + assertEquals(0, stream.inFlight()); + } + + @Test + void repeatFramesAtOneDeliversEveryFrameExactlyTwiceBackToBack() { + FaultedFrameStream stream = new FaultedFrameStream( + VisionFaults.builder(6).repeatFrames(1.0).build()); + List> all = new ArrayList>(); + for (int i = 0; i < 201; i++) { + if (i < 200) { + stream.offer(i * 20 * MS, i * 20 * MS, i); + } + all.addAll(stream.pollDue(i * 20 * MS)); + } + assertEquals(400, all.size()); + for (int i = 0; i < 200; i++) { + assertEquals(Integer.valueOf(i), all.get(2 * i).getFrame()); + assertFalse(all.get(2 * i).isRepeat()); + assertEquals(Integer.valueOf(i), all.get(2 * i + 1).getFrame()); + assertTrue(all.get(2 * i + 1).isRepeat()); + } + assertEquals(400, stream.getDelivered()); + assertEquals(200, stream.getRepeated()); + assertEquals(0, stream.inFlight()); + } + + @Test + void reorderFramesAtOneNeverDeliversAnything() { + // Documented contract: every frame is held behind the next, and no frame is ever not held, so the chain + // only grows. All 200 frames stay in flight. + FaultedFrameStream stream = new FaultedFrameStream( + VisionFaults.builder(6).reorderFrames(1.0).build()); + assertTrue(deliveredFrames(stream, 200).isEmpty()); + assertTrue(stream.pollDue(10_000 * MS).isEmpty()); + assertEquals(200, stream.getOffered()); + assertEquals(0, stream.getDelivered()); + assertEquals(199, stream.getReordered()); + assertEquals(200, stream.inFlight()); + } + @Test void badArgumentsAreRefused() { assertThrows(IllegalArgumentException.class, () -> VisionFaults.builder(1).dropFrames(1.5)); From 59982b5053c427d0744408591ea9dfbd9f7e4892 Mon Sep 17 00:00:00 2001 From: Claude Date: Mon, 2 Nov 2026 01:55:55 +0000 Subject: [PATCH 4/4] Keep a frame's pose draws when the noise drops every tag With no faults set, a frame whose tags the noise pushed out of the image returned before its five pose draws, so every later frame's noise moved, tags well inside the image included. The frame now makes those draws and discards them. A test runs one seed through a narrow and a wide lens and checks the tag angles and MegaTag1 agree frame by frame; without the fix it fails at frame 9. Two doc sentences said more than the code does: turning on misses or drops changes which sightings get wrong ids and which frames are held back, and a repeated frame can hide an out-of-order one released on the same tick. --- .../simloop/fakehardware/FakeLimelight3A.java | 23 ++++++++++- .../simloop/vision/VisionFaults.java | 5 ++- .../FakeLimelight3AFaultsTest.java | 38 +++++++++++++++++++ 3 files changed, 63 insertions(+), 3 deletions(-) diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/fakehardware/FakeLimelight3A.java b/SimLoop/src/main/java/org/horizon36596/simloop/fakehardware/FakeLimelight3A.java index cd10807..9f2df5e 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/fakehardware/FakeLimelight3A.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/fakehardware/FakeLimelight3A.java @@ -128,8 +128,9 @@ * and out-of-order frames) act on when a frame becomes the device's current result; the extra delay is * not added to {@code cl} or {@code tl}, so it models delay the device does not report. The device * serves the last frame delivered in a tick, so an out-of-order frame shows (its {@code ts} goes backwards) - * only on a tick with no newer frame, which needs a {@link Builder#frameRateHz} below the tick rate; a - * repeated frame is the frame already being served, so it changes nothing a poll can see. + * only on a tick with no newer frame, which needs a {@link Builder#frameRateHz} below the tick rate. A + * repeated frame is delivered after any held frame released on the same tick, so it can hide that + * out-of-order frame; otherwise it is the frame already being served and changes nothing a poll can see. * *

    Optics and occlusion. {@link Builder#distortion} gives the lens OpenCV's distortion model: * corner pixels ({@code pts}) and the centre pixel ({@code txp}, {@code typ}) are distorted, and the angles @@ -709,6 +710,7 @@ private String frameJson(long captureNanos) throws JSONException { reported.add(s); reportedAngles.add(new double[] {tx, ty, centrePx[0], centrePx[1]}); } + boolean noiseDroppedEveryTag = !seen.isEmpty() && reported.isEmpty(); seen = reported; JSONObject json = new JSONObject(); json.put("pID", pipelineIndex); @@ -793,6 +795,11 @@ private String frameJson(long captureNanos) throws JSONException { json.put("v", count > 0 ? 1 : 0); json.put("botpose_tagcount", count); if (count == 0) { + if (noiseDroppedEveryTag) { + // Make the five pose draws a frame with a tag makes, and throw them away, so the noise of + // every later frame is the same as if the tags had been reported. + skipPoseDraws(); + } double[] zeros = new double[6]; json.put("botpose", toJson(zeros)); json.put("botpose_orb", toJson(zeros)); @@ -870,6 +877,18 @@ private static double wrapDegrees(double degrees) { return degrees - 360.0 * StrictMath.floor((degrees + 180.0) / 360.0); } + /** + * Makes the five draws a frame's pose estimates take (MegaTag1 x, y and yaw, MegaTag2 x and y), in that + * order, and discards them. + */ + private void skipPoseDraws() { + gaussian(positionStdDevInches); + gaussian(positionStdDevInches); + gaussian(yawStdDevDegrees); + gaussian(positionStdDevInches); + gaussian(positionStdDevInches); + } + /** One zero-mean Gaussian draw of this standard deviation, or exactly 0 with noise off (no draw). */ private double gaussian(double stdDev) { if (noiseRandom == null || stdDev == 0.0) { diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaults.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaults.java index 47d6f11..b980893 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaults.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaults.java @@ -15,7 +15,10 @@ * *

    Determinism. Every fault draws from its own {@link org.horizon36596.simloop.field.SeededRandom}, * seeded from {@link #getSeed()} and the fault's name, so the same seed and the same sequence of frames give - * the same faults, and turning one fault on or off does not move another fault's draws. + * the same faults. Turning one fault on or off leaves the others' draws alone, with one exception: a fault + * that draws only for what an earlier fault let through draws for less when that fault takes more away. A + * wrong id is drawn only for a sighting that was not missed, and a dropped frame draws no jitter, reorder or + * repeat, so turning on misses or drops changes which sightings get wrong ids and which frames are held back. * *

    Every rate here is a test input, not a measurement. None of the defaults describe a real camera; * the defaults are "off". Pick the levels your test needs and say so in the test. diff --git a/SimLoop/src/test/java/org/horizon36596/simloop/fakehardware/FakeLimelight3AFaultsTest.java b/SimLoop/src/test/java/org/horizon36596/simloop/fakehardware/FakeLimelight3AFaultsTest.java index 91bf3c3..e97a471 100644 --- a/SimLoop/src/test/java/org/horizon36596/simloop/fakehardware/FakeLimelight3AFaultsTest.java +++ b/SimLoop/src/test/java/org/horizon36596/simloop/fakehardware/FakeLimelight3AFaultsTest.java @@ -73,6 +73,44 @@ void heavyNoiseNeverReportsATagOutsideTheImage() { assertTrue(reported > 0 && reported < 1500, "some tags stay in the image and some leave it: " + reported); } + @Test + void aFrameWhoseTagsTheNoiseDropsMakesTheSameDrawsAsOneThatReportsThem() { + // The same seed through two lenses. The tags' true angles are the same in both, so the noise draws + // are too, but 60 degrees of noise often pushes all three tags out of the narrow image and seldom + // out of the wide one. A frame that drops every tag must still make its pose draws, or every later + // frame's noise moves: tag angles and MegaTag1 (from the true pose, with no faults) must match. + FakeLimelight3A narrow = builder().noise(3, 60.0, 1.0, 1.0).build(); + FakeLimelight3A wide = FakeLimelight3A.builder(WALL, LEVEL, 170.0, 170.0).noise(3, 60.0, 1.0, 1.0).build(); + narrow.start(); + wide.start(); + int framesNarrowDroppedEveryTag = 0; + int tagsCompared = 0; + for (int i = 0; i < 300; i++) { + narrow.update(TICK); + wide.update(TICK); + LLResult n = narrow.getLatestResult(); + LLResult w = wide.getLatestResult(); + if (n.getFiducialResults().isEmpty() && !w.getFiducialResults().isEmpty()) { + framesNarrowDroppedEveryTag++; + } + for (LLResultTypes.FiducialResult nf : n.getFiducialResults()) { + for (LLResultTypes.FiducialResult wf : w.getFiducialResults()) { + if (nf.getFiducialId() == wf.getFiducialId()) { + assertEquals(wf.getTargetXDegrees(), nf.getTargetXDegrees(), 0.0, "tx, frame " + i); + assertEquals(wf.getTargetYDegrees(), nf.getTargetYDegrees(), 0.0, "ty, frame " + i); + tagsCompared++; + } + } + } + if (!n.getFiducialResults().isEmpty() && !w.getFiducialResults().isEmpty()) { + assertEquals(metres(w.getBotpose(), 'x'), metres(n.getBotpose(), 'x'), 0.0, "MT1 x, frame " + i); + } + } + assertTrue(framesNarrowDroppedEveryTag > 10, "the narrow lens dropped every tag in some frames: " + + framesNarrowDroppedEveryTag); + assertTrue(tagsCompared > 30, "tags reported by both: " + tagsCompared); + } + @Test void moderateNoiseIsUnchangedByTheImageEdgeRule() { // 0.5 degree noise cannot push a tag 12 in off the axis at 72 in out of the image: nothing drops.