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..9f2df5e 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,23 @@ * 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 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 * and poses stay the undistorted geometry a calibrated Limelight reports. {@link Builder#occluders} takes @@ -156,6 +176,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 +227,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 +304,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 +321,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 +363,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 +679,39 @@ 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]}); + } + boolean noiseDroppedEveryTag = !seen.isEmpty() && reported.isEmpty(); + seen = reported; JSONObject json = new JSONObject(); json.put("pID", pipelineIndex); json.put("pipelineType", PIPELINE_TYPE); @@ -664,10 +736,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 +751,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,12 +780,26 @@ 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(); 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)); @@ -731,11 +816,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[] { @@ -789,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) { @@ -890,6 +990,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 +1139,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..05e318c --- /dev/null +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/FaultedFrameStream.java @@ -0,0 +1,286 @@ +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 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, so at a reorder probability of 1.0 every frame waits on the next and nothing + is ever 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()) { + // A held-back frame stays held: the next frame that is not dropped releases it. + 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 p : late) { + 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..135c1fd --- /dev/null +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaultInjector.java @@ -0,0 +1,433 @@ +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 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; + 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.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); + 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(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; + } + 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(tagAngleNoiseRandom, sigma), + s.getTyDegrees() + gaussian(tagAngleNoiseRandom, 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(pixelNoiseRandom, 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..b980893 --- /dev/null +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/VisionFaults.java @@ -0,0 +1,482 @@ +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. 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. + * + *

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 next frame that is not dropped} */ + 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 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. 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 + */ + 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..e97a471 --- /dev/null +++ b/SimLoop/src/test/java/org/horizon36596/simloop/fakehardware/FakeLimelight3AFaultsTest.java @@ -0,0 +1,284 @@ +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; + +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 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. + 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); + } + + 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/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..8b945e4 --- /dev/null +++ b/SimLoop/src/test/java/org/horizon36596/simloop/vision/VisionFaultsTest.java @@ -0,0 +1,703 @@ +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.horizon36596.simloop.field.SeededRandom; +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); + } + + // 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()); + FaultedFrameStream dropAndJitter = new FaultedFrameStream( + VisionFaults.builder(3).dropFrames(0.3).latency(0.0, 0.004).build()); + 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) { + 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"); + } + + /** 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 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)); + 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"); + } +}