diff --git a/SimLoop/README.md b/SimLoop/README.md index f99af64..3fea375 100644 --- a/SimLoop/README.md +++ b/SimLoop/README.md @@ -115,7 +115,7 @@ does **not** do, and a flat index written for AI agents. Start at `docs/index.md | `sim` | `FakeTimer` (the one clock), `ScenarioRunner` (the fixed-timestep tick loop plus the whole PsiKit lifecycle, and `runUntil` for a run of unknown length), and `RlogDecodedCompare` (field-by-field RLOG comparison for determinism tests). | | `sim/live` | Live drive: a recorded, replayable gamepad input track (`GamepadInputTrack`), a keyboard-to-gamepad keymap (`KeyboardGamepadSource`), and `RealTimePacer`. | | `field` | Field bookkeeping and game-piece simulation: the localizer-to-field frame transform, trigger and scoring volumes, obstacles, game-piece possession and rolling/flight physics, and a seeded random source. | -| `vision` | A simulated camera: lens and mount geometry, and which game pieces fall inside the image and where, as `CameraSighting`s. Geometry only; no image is rendered. | +| `vision` | A simulated camera: lens (optionally distorted) and mount geometry, which game pieces and AprilTags fall inside the image and where, with boxes, the robot body and other pieces in the way, and `SimFrameRenderer`'s synthetic RGB frames for running a pixel pipeline. Synthetic frames are flat colours, never for tuning thresholds. | | `viz` | `MechanismSketch` and its definitions: a robot's joints drawn as a PsiKit mechanism for AdvantageScope. `ArticulatedRobotModel`: a joint tree that turns joint values into the component poses a custom AdvantageScope robot asset draws. | | `loop` | Scoring runs from their logs and keeping the best: `RunResult` (a run read back from its RLOG), `Objective` and `Scorer` (measure), guardrails and `Gate` (accept or reject a candidate), and `LoopDriver` (last-best bookkeeping, `Milestone`s, the `StopReport`). It proposes nothing. | | `fit` | Fitting the sim to a real robot log: `LeastSquares` (Householder QR, written out), `FirstOrderFit` (the plant model `FakeMotor` steps, plus friction), `RlogTimeSeries` (timestamped columns of an RLOG) and `GapReport` (the markdown sim-vs-real table). A season supplies its own key names and sim parameters. | @@ -334,13 +334,15 @@ limelight.setRobotPose(fieldX, fieldY, fieldHeading); // or setRobotPoseFromLo is an empty, invalid result, as on the robot; `stop()` keeps the last result, which goes stale. - **Geometry** is `vision/SimTagCamera`: all four corners must be in the image and in front of the lens, the tag must face the camera (optionally within `maxIncidenceDegrees`), be within range, and not be - behind an `ObstacleBox` given to `occluders(...)`. MegaTag2 uses the yaw you sent. + behind an `ObstacleBox` given to `occluders(...)` or `robotBody(...)` (robot frame). Tags may be tilted. + MegaTag2 uses the yaw you sent. - **Optional:** seeded Gaussian noise on tx/ty and botpose (`noise(seed, ...)`, off by default, - byte-identical for a seed), capture and targeting latency, and a camera frame rate. + byte-identical for a seed) with `stdev_mt1`/`stdev_mt2` reporting its spread, seeded dropouts + (`dropout(seed, p)`), lens distortion on the pixel outputs (`distortion(k1, k2, p1, p2, k3)`), capture + and targeting latency, and a camera frame rate. - **Your test JVM needs the real `org.json`** (`testRuntimeOnly "org.json:json:20250517"`); Android's `android.jar` copy is a stub. `SimLoop/build.gradle` shows the line. -- **Not modelled:** non-AprilTag pipelines, lens distortion, blur and dropouts, tag-on-tag and robot-body - occlusion, tilted tags, stddevs, and the network-only calls (`uploadFieldmap`, snapshots, python, +- **Not modelled:** non-AprilTag pipelines, blur, tag-on-tag occlusion, and the network-only calls (`uploadFieldmap`, snapshots, python, calibration), which throw. Robot-space poses use `+y` left; this has not been checked on a real 3A. ## Running a team's OpMode unchanged (`opmode/`) diff --git a/SimLoop/docs/assets/sim-frame-example.png b/SimLoop/docs/assets/sim-frame-example.png new file mode 100644 index 0000000..534ae41 Binary files /dev/null and b/SimLoop/docs/assets/sim-frame-example.png differ diff --git a/SimLoop/docs/features.md b/SimLoop/docs/features.md index cc84c7b..d7f2aad 100644 --- a/SimLoop/docs/features.md +++ b/SimLoop/docs/features.md @@ -27,7 +27,8 @@ it does **not** do is on [What it does not do](limits.md). - Limelight 3A: AprilTag results from the tags the robot can see, parsed by the SDK. [`fakehardware`](packages/fakehardware.md#limelight-3a) - REV touch, 2m distance and Color Sensor V3 fakes, scripted or driven from the field. [`fakehardware`](packages/fakehardware.md#touch-distance-and-colour-sensors) - Sensors driven from the field: range ray, colour probe, contact switch on a robot mount. [`sensor`](packages/sensor.md) -- Simulated camera: which pieces and AprilTags are in the image, and where. No rendering. [`vision`](packages/vision.md) +- Simulated camera: which pieces and AprilTags are in the image, and where, through a distorted lens, with boxes, the robot body and other pieces in the way. [`vision`](packages/vision.md) +- Synthetic camera frames: the scene drawn as RGB pixels for a real `VisionProcessor`, with ground-truth labels and a PNG writer. Flat colours, not for tuning thresholds. [`vision`](packages/vision.md#synthetic-frames-for-a-pixel-pipeline) ## Physics and field diff --git a/SimLoop/docs/index.md b/SimLoop/docs/index.md index be1837a..9f02bbd 100644 --- a/SimLoop/docs/index.md +++ b/SimLoop/docs/index.md @@ -73,7 +73,7 @@ the log is what you assert on afterwards. | [`sim.live`](packages/live.md) | Driving from the keyboard, recording the session, and replaying it to the same log. | | [`field`](packages/field.md) | Game pieces with possession and physics, goals, field structures, and robot contact. | | [`sensor`](packages/sensor.md) | What a touch, distance or colour sensor on the robot sees on the field. | -| [`vision`](packages/vision.md) | Which game pieces and AprilTags a camera sees, and where. No image. | +| [`vision`](packages/vision.md) | Which game pieces and AprilTags a camera sees, and where, with distortion and occlusion. Synthetic frames for pixel pipelines, not photos. | | [`fit`](packages/fit.md) | Fitting the motor model to a real robot log, and a sim-against-robot gap report. | | [`loop`](packages/loop.md) | Scoring a run, and the automated edit-and-replay iteration built on top of it. | | [`viz`](packages/viz.md) | Mechanisms, robot parts and shot arcs drawn into the log for AdvantageScope. | diff --git a/SimLoop/docs/limits.md b/SimLoop/docs/limits.md index 367780c..10cd0dc 100644 --- a/SimLoop/docs/limits.md +++ b/SimLoop/docs/limits.md @@ -37,23 +37,29 @@ Game pieces do have physics: they roll, bounce, collide, fly and score - **`ObstacleBox` has no top**, so pieces cannot rest on one. Use a `KinematicBody`. - **The physics numbers are defaults** for a foam ball a few inches across. Measure yours. -## Vision is geometry only - -`vision` says which pieces and AprilTags a camera would see and where, and `FakeLimelight3A` turns that -into Limelight results. There is **no image**. So: - -- Colour thresholds, blob detection and lighting cannot be tested, and a synthetic scene must never be - used to tune them. -- No lens distortion, no motion blur, no dropouts. The robot's own body does not block the view. +## Vision: geometry, and drawings that are not photographs + +`vision` says which pieces and AprilTags a camera would see and where, through a pinhole or a distorted +lens, with boxes, the robot's own body and other pieces in the way. `FakeLimelight3A` turns that into +Limelight results. `SimFrameRenderer` can also draw the scene as RGB pixels so a real pixel pipeline runs. +Those **synthetic frames are flat colours under one light, not camera images**. So: + +- **Colour thresholds must never be tuned on synthetic frames.** A threshold that passes on one shows the + code path runs, nothing about real lighting. Tune on real photos from your camera. +- No shadows, highlights, textures, motion blur, rolling shutter, exposure or white balance. Synthetic tag + interiors do not decode as a real tag family. +- No roll. Only what you give as a box, a piece or a tag blocks the view; boxes are axis-aligned. +- No frame timing in `SimCamera` or the renderer: latency is your sim glue's job. - Limelight: AprilTag pipelines only. Its network-only calls throw. -- **No webcams.** A `WebcamName` lookup fails in `init()`. +- **No webcams.** A `WebcamName` lookup fails in `init()`. Your sim glue hands frames to your processor + itself. ## Sensors are simple - A distance sensor is a single ray, not a 25 degree cone, and sees every surface. - A colour sensor reads one set of counts per kind of piece, whatever the distance or angle. - No noise or drift on the IMU, Pinpoint, OTOS, distance or colour fakes. The Limelight has optional - seeded noise. + seeded noise and dropouts, and synthetic frames optional seeded pixel noise. - No legacy `BNO055IMU`, and no generic I2C devices. ## It ships no field diff --git a/SimLoop/docs/llms.md b/SimLoop/docs/llms.md index 8d7c92d..e49af36 100644 --- a/SimLoop/docs/llms.md +++ b/SimLoop/docs/llms.md @@ -10,9 +10,10 @@ the right page instead of crawling navigation. - **[Every feature](features.md)**: one line per feature, with a link to its page. - **[What it does not do](limits.md)**: the authoritative list of what is out of scope. If a task needs - something on that list, there is no API for it; say so rather than searching. A rendered camera image, + something on that list, there is no API for it; say so rather than searching. A photoreal camera image, intake rollers, a shipped field, a command scheduler and controller tuning are out of scope. Game-piece - physics, a geometry-only camera and fitting to a real log exist, each with stated limits. + physics, a camera with distortion and occlusion, synthetic frames (flat colours, never for tuning colour + thresholds) and fitting to a real log exist, each with stated limits. ## Pages @@ -34,7 +35,7 @@ the right page instead of crawling navigation. | [`sim.live`](packages/live.md) | Keyboard driving, recorded gamepad tracks, headless replay to the identical log, and how to use the unpublished keyboard-window module. | Driving the sim by hand, or replaying a session. | | [`field`](packages/field.md) | Game pieces with possession and physics (roll, bounce, fly, score), goals, obstacles, kinematic and hinged bodies, the robot as boxes, robot contact, and `FieldFrameTransform`. Intaking is a box test. | Modelling game pieces or the field, or crossing between the two frames. | | [`sensor`](packages/sensor.md) | `RangeFinder`, `ColorProbe`, `ContactSwitch`: what a sensor on a robot mount sees on the field, driving the sensor fakes. | Simulating touch, distance or colour sensors. | -| [`vision`](packages/vision.md) | `SimCamera`, `SimTagCamera`, lens and mount: which pieces and AprilTags a camera sees, and where. Geometry only. | Testing a vision pipeline's geometry. | +| [`vision`](packages/vision.md) | `SimCamera`, `SimTagCamera`, lens (with distortion), mount, `CameraOccluders`: which pieces and AprilTags a camera sees, and where. `SimFrameRenderer`: synthetic RGB frames for running a pixel pipeline, not for tuning it. | Testing a vision pipeline's geometry, or running a pixel pipeline end to end. | | [`fit`](packages/fit.md) | `FirstOrderFit`, `GapReport`: the plant fitted to a real robot log, with standard errors and warnings. | Comparing the sim with a real robot. | | [`loop`](packages/loop.md) | `RunResult`, `Objective`, `Guardrail`, `Scorer`, `Gate`, `LoopDriver`, `StopReport`. | Scoring a run or driving iteration. | | [`viz`](packages/viz.md) | Mechanism and turret sketches, `ArticulatedRobotModel`, `ShotArc` for AdvantageScope. Inches and radians in, metres in the log. | Drawing a mechanism, robot parts or a shot. | diff --git a/SimLoop/docs/packages/fakehardware.md b/SimLoop/docs/packages/fakehardware.md index 64cb1f2..a3b7752 100644 --- a/SimLoop/docs/packages/fakehardware.md +++ b/SimLoop/docs/packages/fakehardware.md @@ -128,8 +128,10 @@ limelight.setRobotPose(fieldX, fieldY, fieldHeading); // or setRobotPoseFromLo Robot code calls `start()`, `pipelineSwitch(i)`, `updateRobotOrientation(yawDeg)` and `getLatestResult()` as usual and gets `botpose`, `getBotpose_MT2()` and fiducial results. Staleness and timestamps run on sim -time. Seeded noise, latency and a frame rate are optional on the builder. The geometry is -[`vision`](vision.md)'s `SimTagCamera`. +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. **Your test JVM needs the real `org.json`:** `testRuntimeOnly "org.json:json:20250517"`. Android's copy is a stub. @@ -174,7 +176,7 @@ They are useful in a test and do not exist on real hardware, so never wire seaso constructor performs. Every read and write **throws**, on purpose: there is no peripheral behind the bus. The Pinpoint and OTOS fakes emulate their own register files instead. - **No sensor noise or drift** on the IMU, Pinpoint, OTOS, distance or colour fakes. The Limelight has - optional seeded noise. + optional seeded noise and dropouts. - **No webcams.** A `WebcamName` lookup fails in `init()` as on a robot without one. - **No Limelight pipelines but AprilTags.** Network-only calls (field map upload, snapshots, Python, calibration) throw. diff --git a/SimLoop/docs/packages/index.md b/SimLoop/docs/packages/index.md index 0fcc6b6..3238b57 100644 --- a/SimLoop/docs/packages/index.md +++ b/SimLoop/docs/packages/index.md @@ -16,7 +16,7 @@ what that package will not do. | [`loop`](loop.md) | once you score runs | Turning a log into a number, with guardrails on what may not get worse. | | [`field`](field.md) | if you model game pieces | Possession, game-piece physics, goals, field structures. | | [`sensor`](sensor.md) | if you use touch, distance or colour sensors | What a sensor on the robot sees or touches on the field. | -| [`vision`](vision.md) | if you use a camera | Which pieces and AprilTags a camera sees, and where. No image. | +| [`vision`](vision.md) | if you use a camera | Which pieces and AprilTags a camera sees, and where. Synthetic frames for pixel pipelines, not photos. | | [`fit`](fit.md) | once you have a real robot log | Fits the motor model to the log and reports the sim-against-robot gap. | | [`viz`](viz.md) | if you want to see it | Mechanisms, robot parts and shot arcs drawn into the log for AdvantageScope. | diff --git a/SimLoop/docs/packages/vision.md b/SimLoop/docs/packages/vision.md index 8ed1bdd..d2c3701 100644 --- a/SimLoop/docs/packages/vision.md +++ b/SimLoop/docs/packages/vision.md @@ -2,21 +2,32 @@ `org.horizon36596.simloop.vision` -A simulated camera on the robot. It works out which game pieces and AprilTags fall inside the image and -where, as numbers. **No image is rendered.** It is for testing the geometry half of a vision pipeline: -which pixel is which point on the floor, tracking, and pose from tags. +A simulated camera on the robot, at two levels: + +- **Geometry.** Which game pieces and AprilTags fall inside the image and where, as numbers. Through a + pinhole or a distorted lens, with field structures, the robot's own body and other pieces in the way. + For testing the geometry half of a vision pipeline: which pixel is which point on the floor, tracking, + and pose from tags. +- **Synthetic frames.** The same scene drawn as RGB pixels (`SimFrameRenderer`), so a real pixel pipeline + (an OpenCV `VisionProcessor`, a colour-blob detector, a contour splitter) can run unchanged in the sim. + **A synthetic frame is a drawing, not a photograph.** Use it to check code paths and geometry. Never + tune a colour threshold on it. ## What you use | Type | What it is | |---|---| -| `CameraLens` | The intrinsics: focal lengths, principal point and image size, in pixels. An OpenCV camera matrix. | +| `CameraLens` | The intrinsics: focal lengths, principal point, image size and, optionally, OpenCV's five distortion coefficients. An OpenCV camera matrix plus `distCoeffs`. | | `CameraMount` | The extrinsics: where the lens sits on the robot and which way it points. | +| `CameraOccluders` | Solid boxes the camera cannot see through: field boxes (field frame) and robot boxes (robot frame, they turn with the robot). | | `SimCamera` | Sees game pieces. Returns `CameraSighting`s. | -| `CameraSighting` | One piece in the image: centre pixel, apparent radius, range, kind, clipped or not. | -| `AprilTagLayout` | Where every tag on your field is. **You supply it.** | +| `CameraSighting` | One piece in the image: centre pixel, apparent radius, range, kind, clipped or not, and how much of it is visible. | +| `AprilTagLayout` | Where every tag on your field is, plumb or tilted. **You supply it.** | | `SimTagCamera` | Sees AprilTags. Returns `TagSighting`s. | | `TagSighting` | One tag: `tx`/`ty` in degrees, centre and corner pixels, range, area, camera-to-tag and robot-to-tag transforms. | +| `SimFrameRenderer` | Draws the scene as a `SimFrame`: floor tiles, boxes, pieces as shaded spheres, tags. | +| `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. | `FakeLimelight3A` in [`fakehardware`](fakehardware.md) is built on `SimTagCamera`, so a Limelight and a @@ -49,50 +60,149 @@ CameraSighting s = seen.get(0); RobotFramePoint p = camera.groundPointInRobotFrame(s.getCentreXPx(), s.getCentreYPx(), 1.4); // ball radius, in ``` -For tags, build a layout from numbers you are entitled to use, then a `SimTagCamera` with a range window, -a maximum viewing angle and any boxes that block the view: +### A distorted lens + +Give the lens your camera's calibration, in OpenCV's order (`k1, k2, p1, p2, k3`). Every camera in this +package then projects through it, and `groundPointInRobotFrame` undistorts first, as a calibrated +pipeline does: + +```java +CameraLens calibrated = new CameraLens(fx, fy, cx, cy, 640, 480, k1, k2, p1, p2, k3); // YOUR calibration +// or: CameraLens.fromHorizontalFov(70.0, 640, 480).withDistortion(k1, k2, p1, p2, k3) +``` + +Use it to test your undistortion: let the sim lens be distorted and check that your pipeline, given the +same coefficients, still puts balls where they are, and that a pipeline that ignores them does not. + +### Things in the way + +Pass `CameraOccluders` to the five-argument `observe`. Field boxes are your structures; robot boxes are +your chassis, bumper or a raised arm, in the robot frame (`X` forward, `Y` left, `Z` up, origin on the +floor under the robot centre): + +```java +CameraOccluders blockers = CameraOccluders.of( + world.getObstacles(), // field frame + Arrays.asList(new ObstacleBox("arm", 8, 9, -6, 6, 2, 12))); // robot frame: turns with the robot +List seen = camera.observe(tracker.getPieces(), x, y, heading, blockers); +``` + +Each piece gets 19 rays spread over its disc. `getVisibleFraction()` is the share not blocked by a box or +a nearer piece, and a piece below `getMinVisibleFraction()` (default 0.5, `withMinVisibleFraction(...)` +to change) is not reported. The four-argument `observe` keeps the old rule: nothing but other pieces' +centre pixels hides anything. + +### Tags + +Build a layout from numbers you are entitled to use, then a `SimTagCamera` with a range window, a maximum +viewing angle and any boxes that block the view: ```java AprilTagLayout layout = AprilTagLayout.builder() .addTag(1, 70.0, 30.0, 6.0, Math.PI, 4.0) // id, x, y, z (in), facing (rad), size (in): YOUR numbers + .addTiltedTag(2, 40.0, -20.0, 18.0, Math.PI, Math.toRadians(30.0), 4.0) // face tilted 30 deg up .build(); SimTagCamera tags = new SimTagCamera(lens, front, 0.0, Double.POSITIVE_INFINITY, 90.0, Collections.emptyList()); List visible = tags.observe(layout, robotX, robotY, robotHeading); +List notBehindTheArm = tags.observe(layout, robotX, robotY, robotHeading, blockers); +``` + +### Synthetic frames for a pixel pipeline + +```java +SimFrameRenderer renderer = SimFrameRenderer.builder(lens, front) + .style(FrameStyle.builder() + .pieceColour("red", 0xC02020) // made-up colours; see the warning below + .pieceColour("blue", 0x2040C0) + .build()) + .supersampling(2) // 4 rays a pixel: smooth edges + .sensorNoise(36596, 2.0) // seeded; PLACEHOLDER spread, in 8-bit counts + .build(); + +SimFrame frame = renderer.render(tracker.getPieces(), x, y, heading, blockers, layout); + +// Your real processor, unchanged. SimLoop has no OpenCV dependency; your test has it. +Mat rgb = new Mat(frame.getHeightPx(), frame.getWidthPx(), CvType.CV_8UC3); +rgb.put(0, 0, frame.getRgbBytes()); // getRgbaBytes() for CV_8UC4 +processor.init(frame.getWidthPx(), frame.getHeightPx(), null); // once +processor.processFrame(rgb, captureTimeNanos); ``` +`SimFrame.getPieceIdAt(x, y)`, `pixelCountOfPiece(id)` and `centroidOfPiece(id)` are ground truth for +checking what the detector found. `writePng(out)` saves a frame; the PNG carries a comment saying it is +synthetic. Rendering costs tens of milliseconds a frame at 640 x 480, so keep it to the tests that need +pixels. + +![A synthetic frame: two balls, the tile seams, a wall and a tag](../assets/sim-frame-example.png) + +*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.* + ## Units and frames -- Robot pose, piece positions, tag positions: field frame, FTC-Cartesian, centre origin, **inches**, - heading **radians** CCW from `+X`. -- `CameraMount`: **inches** in the robot frame (`+forward`, `+left`, `+up`, origin on the floor under the - robot centre). Yaw **radians** CCW from forward. Pitch **radians**, **positive tilts down**. - `CameraMount.front(...)` takes its pitch in **degrees**. +- Robot pose, piece positions, tag positions, field boxes: field frame, FTC-Cartesian, centre origin, + **inches**, heading **radians** CCW from `+X`. +- `CameraMount` and robot boxes: **inches** in the robot frame (`+forward`, `+left`, `+up`, origin on the + floor under the robot centre). Yaw **radians** CCW from forward. Pitch **radians**, **positive tilts + down**. `CameraMount.front(...)` takes its pitch in **degrees**. - Pixels: `(0, 0)` top-left, `+x` right, `+y` down (the OpenCV convention). - Optical frame: `+x` image right, `+y` image down, `+z` along the boresight. +- Distortion coefficients: dimensionless, OpenCV's model and order, acting on normalised image + coordinates `(X/Z, Y/Z)`. +- Colours: `0xRRGGBB`, 8 bits a channel. Sensor noise: standard deviation in 8-bit counts. - `SimCamera.fieldPose3d(...)`: **metres**, WPILib camera axes, so AdvantageScope's 3D Field can use it as a camera override and look through the camera. -- Tag size: **inches**, the edge of the square a detector reports corners for. `TagSighting` angles are **degrees** where the name says. +- Tag size: **inches**, the edge of the square a detector reports corners for. Tag tilt: **radians**, the + face normal's angle above horizontal. `TagSighting` angles are **degrees** where the name says. ## Visibility rules -A piece is sighted when it is on the field (not held), more than its own radius in front of the lens, and -its apparent disc overlaps the image. It is **clipped** when part of the disc is outside the image. A -piece whose centre pixel is covered by a nearer piece's disc is not reported. +A piece is sighted when it is on the field (not held), more than its own radius in front of the lens, +inside the lens's valid radius, and its apparent disc overlaps the image. It is **clipped** when part of +the disc is outside the image. Without occluders, a piece whose centre pixel is covered by a nearer +piece's disc is not reported. With occluders, the visible-fraction rule above decides, and a box that +holds the piece (a ball in a tray) or the lens (the camera's housing) does not hide it. A tag is seen only if, in this order: it is within range; it faces the camera within -`maxIncidenceDegrees`; all four corners are in front of the lens; all four corners are inside the image; -and no `ObstacleBox` you passed blocks the line from the lens to any corner. `visibility(...)` tells you -which rule failed. +`maxIncidenceDegrees` (a tilted tag's normal is tilted); all four corners are in front of the lens; all +four corners are inside the image, after distortion; and no box (constructor boxes, then field and robot +boxes passed to `observe`) blocks the line from the lens to any corner. `visibility(...)` tells you which +rule failed. Distortion moves the corner and centre pixels; `tx`, `ty` and the transforms stay the +undistorted geometry a calibrated detector recovers. + +## Synthetic frames are not real frames + +`SimFrameRenderer` draws flat colours under one light. It has no shadows, highlights, textures, motion +blur, rolling shutter, exposure time, white balance, lens blur, vignetting or compression. Its tag +interiors are a pattern made from the id, **not** a real tag family's code: a real AprilTag decoder will +not decode them, so test tag pose code with `SimTagCamera` or `FakeLimelight3A` instead. + +So: + +- **A colour threshold that passes on synthetic frames says nothing about real lighting.** If your + thresholds can be exercised on synthetic frames, that shows the code path runs. It does not show the + thresholds are right, and they must not be tuned there. Tune them on real photos from your camera. +- Pick piece colours that land inside your detector's bands so the code runs; do not move the bands to + fit the colours. +- What synthetic frames can show: the pipeline runs end to end, a blob comes back where the piece is, + edge-clipped and touching pieces are handled, occluded pieces are not reported, a distorted lens is + undistorted correctly, and the whole thing is deterministic. ## What this package will not do -- **It will not render an image.** And synthetic frames must never be used to tune colour thresholds. -- **No lens distortion.** An ideal pinhole. +- **No photoreal image.** See above. Synthetic frames must never be used to tune colour thresholds. - **No roll.** The image's horizontal stays level with the floor. -- **Most things do not block the view.** The robot's own body, field structures (unless given to - `SimTagCamera` as boxes), the far wall and other tags are transparent. -- **Tags hang vertical.** A tilted or flat tag is not modelled. -- **It ships no field.** Not this season's, not any season's. Do not build a layout from the FIRST field - CAD: its licence forbids redistribution. -- **No noise in `SimCamera` or `SimTagCamera`.** `FakeLimelight3A` can add seeded noise on top. +- **Only boxes, pieces and tags block the view.** Anything you do not give as a box (a curved structure, + another robot unless you give it a box, the far wall unless you give it a box) is transparent. Boxes + are axis-aligned, in the field frame or the robot frame. +- **Tag-on-tag occlusion is not modelled**, and a tag turned within its own plane is not modelled. +- **It ships no field.** Not this season's, not any season's. Do not build a layout or boxes from the + 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. +- **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). diff --git a/SimLoop/docs/tested.md b/SimLoop/docs/tested.md index e052d8d..226ba20 100644 --- a/SimLoop/docs/tested.md +++ b/SimLoop/docs/tested.md @@ -35,7 +35,13 @@ If you try it, you are testing it. Reports help most where this page says "not y - **Game-piece physics against a real field.** Bounces, rolling and contacts are plausible, not measured. - **The simulated camera and Limelight against real captures.** They are checked against the SDK's - parsing, not against what a real camera sees. + parsing and against their own geometry, not against what a real camera sees. +- **Synthetic frames against real frames.** `SimFrameRenderer` is checked against `SimCamera`'s geometry: + pieces land where `SimCamera` puts them, occluded ones are hidden, distortion bends the image the way + the lens model says, and the same seed gives the same pixels. Nobody has compared a synthetic frame with + a photo, and no colour threshold has been tuned on one. None should be. +- **Lens distortion against a real calibration.** The model is OpenCV's five-coefficient one, checked + against its own inverse, not against a calibrated camera. ## How to help 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 1bc079e..169e87b 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/fakehardware/FakeLimelight3A.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/fakehardware/FakeLimelight3A.java @@ -11,6 +11,7 @@ import org.horizon36596.simloop.field.SeededRandom; import org.horizon36596.simloop.vision.AprilTagLayout; import org.horizon36596.simloop.vision.CameraLens; +import org.horizon36596.simloop.vision.CameraOccluders; import org.horizon36596.simloop.vision.CameraMount; import org.horizon36596.simloop.vision.RigidTransform3; import org.horizon36596.simloop.vision.SimTagCamera; @@ -100,12 +101,27 @@ *

Noise (off unless {@link Builder#noise} is called): independent Gaussian draws, from a * {@link SeededRandom} so the same seed and the same sequence of calls give byte-identical results, added * to each tag's {@code tx} and {@code ty} (and the centre pixel with them), to {@code botpose}'s x, y and - * yaw, and to {@code botpose_orb}'s x and y. Target poses ({@code t6*}), corners and area stay exact. + * yaw, and to {@code botpose_orb}'s x and y. Target poses ({@code t6*}), corners and area stay exact. When + * noise is on, {@code stdev_mt1} and {@code stdev_mt2} ({@link LLResult#getStddevMt1()}, + * {@link LLResult#getStddevMt2()}) report the noise model's own standard deviations ({@code x, y} in + * metres, yaw in degrees, the rest 0), so a pose estimator that weights by them sees the spread it is + * actually given. A real device estimates them from its recent poses instead; this is the stated model's + * value, not that estimate. + * + *

Dropouts (off unless {@link Builder#dropout} is called): each tag that passes every + * 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. + * + *

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 + * field boxes and {@link Builder#robotBody} boxes fixed to the robot (its own chassis, an arm), and a tag + * hidden behind either is not seen. Tags tilted with {@link AprilTagLayout.Builder#addTiltedTag} are seen + * tilted. * *

Not modelled: any pipeline other than AprilTag (every pipeline index 0-9 behaves as an AprilTag - * pipeline), crosshair calibration, lens distortion, motion blur, detection dropouts, tag-on-tag and - * robot-body occlusion, tags that are not vertical, the {@code stddevs}, {@code botpose_wpiblue/wpired} - * and python outputs, and the device-side capture schedule's phase. Methods that would configure the + * pipeline), crosshair calibration, motion blur, tag-on-tag occlusion, a tag turned within its own plane, + * the {@code botpose_wpiblue/wpired} and python outputs, and the device-side capture schedule's phase. Methods that would configure the * device over the network ({@code uploadFieldmap}, {@code uploadPipeline}, {@code uploadPython}, * {@code updatePythonInputs}, snapshots and calibration) throw {@link UnsupportedOperationException}: the * fake has nothing behind them, and a silent {@code true} would let a robot believe a field map was @@ -137,6 +153,9 @@ public class FakeLimelight3A extends Limelight3A implements FakeDevice { private final double positionStdDevInches; private final double yawStdDevDegrees; private final double frameRateHz; + private final CameraOccluders robotBody; + private final SeededRandom dropoutRandom; + private final double dropoutProbability; private long simNanos; private boolean running; @@ -167,7 +186,8 @@ private FakeLimelight3A(Builder b) { super.stop(); double fx = (b.widthPx / 2.0) / StrictMath.tan(StrictMath.toRadians(b.horizontalFovDegrees) / 2.0); double fy = (b.heightPx / 2.0) / StrictMath.tan(StrictMath.toRadians(b.verticalFovDegrees) / 2.0); - CameraLens lens = new CameraLens(fx, fy, b.widthPx / 2.0, b.heightPx / 2.0, b.widthPx, b.heightPx); + CameraLens lens = new CameraLens(fx, fy, b.widthPx / 2.0, b.heightPx / 2.0, b.widthPx, b.heightPx, + b.k1, b.k2, b.p1, b.p2, b.k3); this.camera = new SimTagCamera(lens, b.mount, b.minRangeInches, b.maxRangeInches, b.maxIncidenceDegrees, b.occluders); this.layout = b.layout; @@ -181,6 +201,9 @@ private FakeLimelight3A(Builder b) { this.txTyStdDevDegrees = b.txTyStdDevDegrees; this.positionStdDevInches = b.positionStdDevInches; this.yawStdDevDegrees = b.yawStdDevDegrees; + this.robotBody = CameraOccluders.of(Collections.emptyList(), b.robotBody); + this.dropoutRandom = b.dropoutProbability > 0.0 ? new SeededRandom(b.dropoutSeed) : null; + this.dropoutProbability = b.dropoutProbability; } /** 127.0.0.1, built without a DNS lookup: if an SDK call ever slipped past this fake, it hits nothing. */ @@ -607,7 +630,16 @@ private static IllegalStateException jsonBug(JSONException e) { } private String frameJson(long captureNanos) throws JSONException { - List seen = camera.observe(layout, robotXInches, robotYInches, robotHeadingRad); + List seen = camera.observe(layout, robotXInches, robotYInches, robotHeadingRad, robotBody); + if (dropoutRandom != null) { + List kept = new ArrayList(seen.size()); + for (TagSighting s : seen) { + if (dropoutRandom.nextFraction() >= dropoutProbability) { + kept.add(s); + } + } + seen = kept; + } JSONObject json = new JSONObject(); json.put("pID", pipelineIndex); json.put("pipelineType", PIPELINE_TYPE); @@ -643,8 +675,13 @@ private String frameJson(long captureNanos) throws JSONException { f.put("ty", ty); f.put("tx_nocross", tx); f.put("ty_nocross", ty); - f.put("txp", lens.getCentreXPx() + lens.getFocalXPx() * StrictMath.tan(StrictMath.toRadians(tx))); - f.put("typ", lens.getCentreYPx() - lens.getFocalYPx() * StrictMath.tan(StrictMath.toRadians(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("ta", s.getAreaPercent()); JSONArray corners = new JSONArray(); for (double[] corner : s.getCornersPx()) { @@ -681,6 +718,8 @@ private String frameJson(long captureNanos) throws JSONException { double[] zeros = new double[6]; json.put("botpose", toJson(zeros)); json.put("botpose_orb", toJson(zeros)); + json.put("stdev_mt1", toJson(zeros)); + json.put("stdev_mt2", toJson(zeros)); return json.toString(); } json.put("tx", primaryTx); @@ -702,6 +741,12 @@ private String frameJson(long captureNanos) throws JSONException { json.put("botpose_orb", toJson(new double[] { orbX * METRES_PER_INCH, orbY * METRES_PER_INCH, mt2Z / count * METRES_PER_INCH, 0.0, 0.0, suppliedYawDegrees})); + double positionStdDevMetres = noiseRandom == null ? 0.0 : positionStdDevInches * METRES_PER_INCH; + double yawStdDev = noiseRandom == null ? 0.0 : yawStdDevDegrees; + json.put("stdev_mt1", toJson(new double[] {positionStdDevMetres, positionStdDevMetres, 0.0, 0.0, 0.0, + yawStdDev})); + json.put("stdev_mt2", toJson(new double[] {positionStdDevMetres, positionStdDevMetres, 0.0, 0.0, 0.0, + 0.0})); return json.toString(); } @@ -837,6 +882,14 @@ public static final class Builder { private double txTyStdDevDegrees; private double positionStdDevInches; private double yawStdDevDegrees; + private double k1; + private double k2; + private double p1; + private double p2; + private double k3; + private List robotBody = Collections.emptyList(); + private int dropoutSeed; + private double dropoutProbability; private Builder(AprilTagLayout layout, CameraMount mount, double horizontalFovDegrees, double verticalFovDegrees) { @@ -928,6 +981,63 @@ public Builder occluders(List boxes) { return this; } + /** + * Boxes fixed to the robot that block the line of sight to a tag: the robot's own chassis, an arm. + * Robot frame, as {@link CameraOccluders} defines it ({@code X} forward, {@code Y} left, {@code Z} up, + * inches). A box containing the lens is skipped. Default none. + * + * @param boxes robot-frame boxes; copied + * @return this builder + */ + public Builder robotBody(List boxes) { + if (boxes == null) { + throw new IllegalArgumentException("robot body list must not be null"); + } + this.robotBody = new ArrayList(boxes); + return this; + } + + /** + * Lens distortion, OpenCV's model and order (see {@link CameraLens}): pass the coefficients of a real + * calibration to put tag corners where that lens puts them. Default none. + * + * @param k1 first radial coefficient + * @param k2 second radial coefficient + * @param p1 first tangential coefficient + * @param p2 second tangential coefficient + * @param k3 third radial coefficient + * @return this builder + */ + public Builder distortion(double k1, double k2, double p1, double p2, double k3) { + if (!Double.isFinite(k1) || !Double.isFinite(k2) || !Double.isFinite(p1) || !Double.isFinite(p2) + || !Double.isFinite(k3)) { + throw new IllegalArgumentException("distortion coefficients must be finite"); + } + this.k1 = k1; + this.k2 = k2; + this.p1 = p1; + this.p2 = p2; + this.k3 = k3; + return this; + } + + /** + * Misses each visible tag in a frame with this probability, seeded. Off by default. The value is + * yours to choose (a PLACEHOLDER until measured on a real device), not a Limelight specification. + * + * @param seed the {@link SeededRandom} seed for the dropout draws; same seed, same misses + * @param probability chance a visible tag is missed in one frame, in {@code [0, 1)} + * @return this builder + */ + public Builder dropout(int seed, double probability) { + if (!(probability >= 0.0 && probability < 1.0)) { + throw new IllegalArgumentException("dropout probability must be in [0, 1), got " + probability); + } + this.dropoutSeed = seed; + this.dropoutProbability = probability; + 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/AprilTagLayout.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/AprilTagLayout.java index df4e652..7e7ac5a 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/vision/AprilTagLayout.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/AprilTagLayout.java @@ -22,8 +22,10 @@ * written in is the frame a fake Limelight's {@code botpose} comes back in, exactly as a real Limelight's * {@code botpose} is in the frame of the field map uploaded to it. * - *

Tags are vertical. Each tag hangs plumb, its face pointing horizontally in the direction - * {@code facingRad}. A tag tilted back or lying flat is not modelled. + *

Tags hang plumb unless you tilt them. {@link Builder#addTag} adds a tag whose face points + * horizontally in the direction {@code facingRad}. {@link Builder#addTiltedTag} also tilts the face up or + * down about the tag's own horizontal axis, for a tag on a sloped panel or lying flat. A tag turned within + * its own plane (its top edge not level) is not modelled. * *

Immutable. Build one with {@link #builder()}; {@link #empty()} is a field with no tags. */ @@ -85,27 +87,46 @@ public static final class Tag { private final double yInches; private final double zInches; private final double facingRad; + private final double tiltUpRad; private final double sizeInches; private final RigidTransform3 fieldToTag; + private final double[] faceNormal; private Tag(int id, double xInches, double yInches, double zInches, double facingRad, - double sizeInches) { + double tiltUpRad, double sizeInches) { this.id = id; this.xInches = xInches; this.yInches = yInches; this.zInches = zInches; this.facingRad = facingRad; + this.tiltUpRad = tiltUpRad; this.sizeInches = sizeInches; double cos = StrictMath.cos(facingRad); double sin = StrictMath.sin(facingRad); // Target space, as Limelight describes it: +X to the right and +Y down for someone looking at // the tag, +Z along the line of sight, into the tag. Right-handed. A viewer in front looks along // -normal, and their right is that direction turned 90 degrees clockwise seen from above. - this.fieldToTag = RigidTransform3.fromAxes( - new double[] {-sin, cos, 0.0}, - new double[] {0.0, 0.0, -1.0}, - new double[] {-cos, -sin, 0.0}, - xInches, yInches, zInches); + if (tiltUpRad == 0.0) { + // The plumb tag, written out so it is bit-identical to every earlier version. + this.fieldToTag = RigidTransform3.fromAxes( + new double[] {-sin, cos, 0.0}, + new double[] {0.0, 0.0, -1.0}, + new double[] {-cos, -sin, 0.0}, + xInches, yInches, zInches); + this.faceNormal = new double[] {cos, sin, 0.0}; + } else { + // Tilting turns the face normal up by tiltUpRad about the viewer's right, which stays level. + double cosTilt = StrictMath.cos(tiltUpRad); + double sinTilt = StrictMath.sin(tiltUpRad); + double[] right = {-sin, cos, 0.0}; + double[] into = {-cos * cosTilt, -sin * cosTilt, -sinTilt}; + double[] down = { + into[1] * right[2] - into[2] * right[1], + into[2] * right[0] - into[0] * right[2], + into[0] * right[1] - into[1] * right[0]}; + this.fieldToTag = RigidTransform3.fromAxes(right, down, into, xInches, yInches, zInches); + this.faceNormal = new double[] {-into[0], -into[1], -into[2]}; + } } /** {@return the tag id, as printed on the tag and reported by a detector} */ @@ -133,6 +154,19 @@ public double getFacingRad() { return facingRad; } + /** + * {@return how far the face normal points above horizontal, radians; 0 for a plumb tag, positive + * for a tag leaning back so its face looks up, {@code PI / 2} for a tag lying face up on the floor} + */ + public double getTiltUpRad() { + return tiltUpRad; + } + + /** {@return the unit vector the printed face points along, field frame, {@code {x, y, z}}, a fresh copy} */ + public double[] getFaceNormal() { + return faceNormal.clone(); + } + /** {@return the edge length of the tag square whose corners are reported, inches} */ public double getSizeInches() { return sizeInches; @@ -152,7 +186,9 @@ public RigidTransform3 getFieldToTag() { @Override public String toString() { return "Tag(" + id + " at (" + xInches + ", " + yInches + ", " + zInches + ") in, facing " - + StrictMath.toDegrees(facingRad) + " deg, " + sizeInches + " in)"; + + StrictMath.toDegrees(facingRad) + " deg" + + (tiltUpRad != 0.0 ? ", tilted up " + StrictMath.toDegrees(tiltUpRad) + " deg" : "") + + ", " + sizeInches + " in)"; } } @@ -179,6 +215,34 @@ private Builder() { */ public Builder addTag(int id, double xInches, double yInches, double zInches, double facingRad, double sizeInches) { + return addTiltedTag(id, xInches, yInches, zInches, facingRad, 0.0, sizeInches); + } + + /** + * Adds a tag whose face is tilted up or down from vertical: a tag on a sloped panel, or one lying + * flat. The viewer's right along the tag stays level; the face normal is {@code facingRad} in plan, + * raised by {@code tiltUpRad}. + * + * @param id the tag id; each id may appear once + * @param xInches tag centre, field X, inches + * @param yInches tag centre, field Y, inches + * @param zInches tag centre height above the floor, inches + * @param facingRad the direction the printed face points in plan, radians CCW from field {@code +X} + * @param tiltUpRad how far the face normal points above horizontal, radians, in + * {@code [-PI / 2, PI / 2]}: positive for a tag leaning back (face looking up), + * {@code PI / 2} for a tag lying face up on the floor, 0 for a plumb tag + * @param sizeInches the edge length of the square a detector reports corners for, inches (> 0) + * @return this builder + * @throws IllegalArgumentException on a repeated id, a non-finite number, a non-positive size or a + * tilt outside {@code [-PI / 2, PI / 2]} + */ + public Builder addTiltedTag(int id, double xInches, double yInches, double zInches, double facingRad, + double tiltUpRad, double sizeInches) { + requireFinite(tiltUpRad, "tiltUpRad"); + if (Math.abs(tiltUpRad) > Math.PI / 2.0) { + throw new IllegalArgumentException("tag " + id + " tilt must be within [-PI/2, PI/2] rad, got " + + tiltUpRad); + } requireFinite(xInches, "xInches"); requireFinite(yInches, "yInches"); requireFinite(zInches, "zInches"); @@ -190,7 +254,7 @@ public Builder addTag(int id, double xInches, double yInches, double zInches, do if (tags.containsKey(id)) { throw new IllegalArgumentException("tag id " + id + " is already in the layout"); } - tags.put(id, new Tag(id, xInches, yInches, zInches, facingRad, sizeInches)); + tags.put(id, new Tag(id, xInches, yInches, zInches, facingRad, tiltUpRad, sizeInches)); return this; } 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 439c736..498166f 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/vision/CameraLens.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/CameraLens.java @@ -1,16 +1,37 @@ package org.horizon36596.simloop.vision; /** - * A pinhole camera's intrinsics: focal lengths and principal point in pixels, and the image size. This is - * the same model, with the same quantities, as an OpenCV camera matrix ({@code fx, fy, cx, cy} plus - * {@code widthPx, heightPx}), so a lens described once can be handed to both the sim camera and a real - * OpenCV-based pipeline unchanged. + * A camera's intrinsics: focal lengths and principal point in pixels, the image size, and optionally a lens + * distortion model. This is the same model, with the same quantities, as an OpenCV camera matrix + * ({@code fx, fy, cx, cy} plus {@code widthPx, heightPx}) and OpenCV's five distortion coefficients in + * OpenCV's order ({@code k1, k2, p1, p2, k3}), so a lens described once, or copied out of a real camera's + * calibration, can be handed to both the sim camera and a real OpenCV-based pipeline unchanged. * - *

No distortion. A sim camera projects with the ideal pinhole model only. That is deliberate: a - * vision pipeline's geometry (where a pixel is on the floor) can be tested against an ideal lens, while - * its distortion handling and its colour thresholds need real frames from a real camera. Synthetic frames - * must never tune a colour range: a range fitted to a renderer's flat colours says nothing about real - * lighting. + *

The distortion model is the Brown-Conrady model OpenCV's {@code projectPoints} uses. A point + * on the image plane at normalised coordinates {@code (x, y) = (X / Z, Y / Z)} in the optical frame lands + * at + * 

+ *   r2 = x*x + y*y
+ *   radial = 1 + k1*r2 + k2*r2^2 + k3*r2^3
+ *   xd = x*radial + 2*p1*x*y + p2*(r2 + 2*x*x)
+ *   yd = y*radial + p1*(r2 + 2*y*y) + 2*p2*x*y
+ *   pixel = (cx + fx*xd, cy + fy*yd)
+ *
+ * With every coefficient zero (the default, and what the six-argument constructor gives) it is the ideal + * pinhole, computed with exactly the arithmetic SimLoop has always used, so a lens without distortion gives + * bit-identical results to earlier versions. + * + *

Where the model stops. A strong barrel distortion ({@code k1 < 0}) is not one-to-one far + * outside the image: past some radius the distorted radius turns round and shrinks again, so a point well + * outside the field of view would land back inside the picture. A real lens does not do that; the model + * simply stops describing it. This class finds that radius once, at construction + * ({@link #getMaxValidRadius()}), and {@link #project} reports a point beyond it as not imaged. Only the + * radial terms are used to find it; the tangential terms are small enough in any real calibration not to + * move it. + * + *

Synthetic images are never ground truth for a real lens. Distortion here makes the sim's + * geometry match a calibrated lens; it does not make a rendered frame look like that lens's pictures, and + * colour thresholds must never be tuned on rendered frames (see {@link SimFrameRenderer}). * *

Pixel frame: {@code (0, 0)} is the top-left corner, {@code +x} right, {@code +y} down, the OpenCV * convention. @@ -19,15 +40,28 @@ */ public final class CameraLens { + /** Newton steps {@link #undistortPixel} takes at most; it converges in a handful inside the image. */ + private static final int UNDISTORT_MAX_STEPS = 50; + + /** Normalised radius past which the fold-over search stops looking: about 84 degrees off the axis. */ + private static final double FOLD_SEARCH_LIMIT = 10.0; + private final double focalXPx; private final double focalYPx; private final double centreXPx; private final double centreYPx; private final int widthPx; private final int heightPx; + private final double k1; + private final double k2; + private final double p1; + private final double p2; + private final double k3; + private final boolean distorted; + private final double maxValidRadius; /** - * A pinhole lens with the given intrinsics. + * A pinhole lens with the given intrinsics and no distortion. * * @param focalXPx horizontal focal length, pixels * @param focalYPx vertical focal length, pixels (equal to {@code focalXPx} for square pixels) @@ -39,6 +73,29 @@ public final class CameraLens { */ public CameraLens(double focalXPx, double focalYPx, double centreXPx, double centreYPx, int widthPx, int heightPx) { + this(focalXPx, focalYPx, centreXPx, centreYPx, widthPx, heightPx, 0.0, 0.0, 0.0, 0.0, 0.0); + } + + /** + * A lens with the given intrinsics and Brown-Conrady distortion, coefficients in OpenCV's order: the + * numbers a camera calibration ({@code calibrateCamera}, or a {@code teamwebcamcalibrations.xml} entry) + * produces. + * + * @param focalXPx horizontal focal length, pixels + * @param focalYPx vertical focal length, pixels + * @param centreXPx principal point x, pixels from the left edge + * @param centreYPx principal point y, pixels from the top edge + * @param widthPx image width, pixels + * @param heightPx image height, pixels + * @param k1 first radial coefficient, dimensionless; negative is barrel distortion + * @param k2 second radial coefficient, dimensionless + * @param p1 first tangential coefficient, dimensionless + * @param p2 second tangential coefficient, dimensionless + * @param k3 third radial coefficient, dimensionless + * @throws IllegalArgumentException on a non-positive focal length or size, or a non-finite number + */ + public CameraLens(double focalXPx, double focalYPx, double centreXPx, double centreYPx, int widthPx, + int heightPx, double k1, double k2, double p1, double p2, double k3) { if (!Double.isFinite(focalXPx) || focalXPx <= 0.0 || !Double.isFinite(focalYPx) || focalYPx <= 0.0) { throw new IllegalArgumentException("focal lengths must be positive and finite"); } @@ -48,17 +105,29 @@ public CameraLens(double focalXPx, double focalYPx, double centreXPx, double cen if (widthPx <= 0 || heightPx <= 0) { throw new IllegalArgumentException("image size must be positive, got " + widthPx + "x" + heightPx); } + if (!Double.isFinite(k1) || !Double.isFinite(k2) || !Double.isFinite(p1) || !Double.isFinite(p2) + || !Double.isFinite(k3)) { + throw new IllegalArgumentException("distortion coefficients must be finite, got k1=" + k1 + " k2=" + + k2 + " p1=" + p1 + " p2=" + p2 + " k3=" + k3); + } this.focalXPx = focalXPx; this.focalYPx = focalYPx; this.centreXPx = centreXPx; this.centreYPx = centreYPx; this.widthPx = widthPx; this.heightPx = heightPx; + this.k1 = k1; + this.k2 = k2; + this.p1 = p1; + this.p2 = p2; + this.k3 = k3; + this.distorted = k1 != 0.0 || k2 != 0.0 || p1 != 0.0 || p2 != 0.0 || k3 != 0.0; + this.maxValidRadius = foldOverRadius(k1, k2, k3); } /** - * A square-pixel lens from its horizontal field of view, principal point at the image centre — the - * way a datasheet describes a webcam: {@code fx = fy = (width / 2) / tan(hfov / 2)}. + * A square-pixel lens from its horizontal field of view, principal point at the image centre, no + * distortion — the way a datasheet describes a webcam: {@code fx = fy = (width / 2) / tan(hfov / 2)}. * * @param horizontalFovDegrees the full horizontal field of view, degrees, in {@code (0, 180)} * @param widthPx image width, pixels @@ -75,6 +144,165 @@ public static CameraLens fromHorizontalFov(double horizontalFovDegrees, int widt return new CameraLens(focal, focal, widthPx / 2.0, heightPx / 2.0, widthPx, heightPx); } + /** + * This lens with a different distortion model: same focal lengths, principal point and image size. + * + * @param k1 first radial coefficient + * @param k2 second radial coefficient + * @param p1 first tangential coefficient + * @param p2 second tangential coefficient + * @param k3 third radial coefficient + * @return the new lens + */ + public CameraLens withDistortion(double k1, double k2, double p1, double p2, double k3) { + return new CameraLens(focalXPx, focalYPx, centreXPx, centreYPx, widthPx, heightPx, k1, k2, p1, p2, k3); + } + + /** + * Where a point in the optical frame lands in the image, distortion included. + * + * @param xOptical the point's optical {@code x} (image right), any length unit + * @param yOptical the point's optical {@code y} (image down), same unit + * @param zOptical the point's depth along the boresight, same unit + * @return {@code {x, y}} in pixels, or {@code null} when the point is not in front of the lens or lies + * beyond {@link #getMaxValidRadius()}, where the distortion model no longer describes a lens. + * The pixel may lie outside the image rectangle; checking that is the caller's job. + */ + public double[] project(double xOptical, double yOptical, double zOptical) { + if (!(zOptical > 0.0)) { + return null; + } + if (!distorted) { + // Exactly the arithmetic every earlier version used, so an undistorted lens is bit-identical. + return new double[] {centreXPx + focalXPx * xOptical / zOptical, + centreYPx + focalYPx * yOptical / zOptical}; + } + double x = xOptical / zOptical; + double y = yOptical / zOptical; + if (x * x + y * y > maxValidRadius * maxValidRadius) { + return null; + } + double[] d = distortNormalized(x, y); + return new double[] {centreXPx + focalXPx * d[0], centreYPx + focalYPx * d[1]}; + } + + /** + * The distortion model on its own: undistorted normalised image coordinates in, distorted ones out. + * + * @param x undistorted {@code X / Z} + * @param y undistorted {@code Y / Z} + * @return {@code {xd, yd}}, the distorted normalised coordinates + */ + public double[] distortNormalized(double x, double y) { + if (!distorted) { + return new double[] {x, y}; + } + double r2 = x * x + y * y; + double radial = 1.0 + r2 * (k1 + r2 * (k2 + r2 * k3)); + double xd = x * radial + 2.0 * p1 * x * y + p2 * (r2 + 2.0 * x * x); + double yd = y * radial + p1 * (r2 + 2.0 * y * y) + 2.0 * p2 * x * y; + return new double[] {xd, yd}; + } + + /** + * The inverse of {@link #project}'s image step: a pixel in, the undistorted normalised direction it + * looks along out. This is what OpenCV's {@code undistortPoints} computes, and what a ground-plane + * pipeline does before casting a ray through a pixel. Newton's method on the distortion model, started + * from the distorted point; deterministic. + * + * @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 + */ + public double[] undistortPixel(double xPx, double yPx) { + double xd = (xPx - centreXPx) / focalXPx; + double yd = (yPx - centreYPx) / focalYPx; + if (!distorted) { + return new double[] {xd, yd}; + } + double x = xd; + double y = yd; + for (int step = 0; step < UNDISTORT_MAX_STEPS; step++) { + double r2 = x * x + y * y; + double radial = 1.0 + r2 * (k1 + r2 * (k2 + r2 * k3)); + double fx = x * radial + 2.0 * p1 * x * y + p2 * (r2 + 2.0 * x * x) - xd; + double fy = y * radial + p1 * (r2 + 2.0 * y * y) + 2.0 * p2 * x * y - yd; + // d(radial)/d(r2), times 2 so that d(radial)/dx = slope * x. + double slope = 2.0 * (k1 + r2 * (2.0 * k2 + 3.0 * k3 * r2)); + double jxx = radial + x * x * slope + 2.0 * p1 * y + 6.0 * p2 * x; + double jxy = x * y * slope + 2.0 * p1 * x + 2.0 * p2 * y; + double jyy = radial + y * y * slope + 6.0 * p1 * y + 2.0 * p2 * x; + double det = jxx * jyy - jxy * jxy; + if (det == 0.0 || !Double.isFinite(det)) { + break; + } + double dx = (jyy * fx - jxy * fy) / det; + double dy = (jxx * fy - jxy * fx) / det; + x -= dx; + y -= dy; + if (Math.abs(dx) < 1e-15 && Math.abs(dy) < 1e-15) { + break; + } + } + return new double[] {x, y}; + } + + /** + * How much the lens magnifies a small patch at this undistorted normalised position, relative to the + * pinhole: the square root of the distortion map's Jacobian determinant. 1 everywhere for an + * undistorted lens; below 1 toward the corners under barrel distortion. {@link SimCamera} scales a + * piece's apparent radius by it. + * + * @param x undistorted {@code X / Z} + * @param y undistorted {@code Y / Z} + * @return the linear magnification, dimensionless + */ + public double localMagnification(double x, double y) { + if (!distorted) { + return 1.0; + } + double r2 = x * x + y * y; + double radial = 1.0 + r2 * (k1 + r2 * (k2 + r2 * k3)); + double slope = 2.0 * (k1 + r2 * (2.0 * k2 + 3.0 * k3 * r2)); + double jxx = radial + x * x * slope + 2.0 * p1 * y + 6.0 * p2 * x; + double jxy = x * y * slope + 2.0 * p1 * x + 2.0 * p2 * y; + double jyy = radial + y * y * slope + 6.0 * p1 * y + 2.0 * p2 * x; + return StrictMath.sqrt(Math.abs(jxx * jyy - jxy * jxy)); + } + + /** + * The normalised radius at which {@code r * radial(r)} stops growing, found by a fixed-step scan and a + * bisection; infinite when it keeps growing out to {@link #FOLD_SEARCH_LIMIT}. + */ + private static double foldOverRadius(double k1, double k2, double k3) { + if (k1 >= 0.0 && k2 >= 0.0 && k3 >= 0.0) { + return Double.POSITIVE_INFINITY; // every term grows: never folds + } + double step = 0.001; + for (double r = step; r <= FOLD_SEARCH_LIMIT; r += step) { + if (radialSlope(r, k1, k2, k3) <= 0.0) { + double low = r - step; + double high = r; + for (int i = 0; i < 60; i++) { + double mid = 0.5 * (low + high); + if (radialSlope(mid, k1, k2, k3) <= 0.0) { + high = mid; + } else { + low = mid; + } + } + return low; + } + } + return Double.POSITIVE_INFINITY; + } + + /** d/dr of {@code r * (1 + k1 r^2 + k2 r^4 + k3 r^6)}. */ + private static double radialSlope(double r, double k1, double k2, double k3) { + double r2 = r * r; + return 1.0 + 3.0 * k1 * r2 + 5.0 * k2 * r2 * r2 + 7.0 * k3 * r2 * r2 * r2; + } + /** {@return horizontal focal length, pixels} */ public double getFocalXPx() { return focalXPx; @@ -105,13 +333,59 @@ public int getHeightPx() { return heightPx; } - /** {@return the full horizontal field of view this lens gives, degrees} */ + /** {@return the first radial distortion coefficient, OpenCV's {@code k1}} */ + public double getK1() { + return k1; + } + + /** {@return the second radial distortion coefficient, OpenCV's {@code k2}} */ + public double getK2() { + return k2; + } + + /** {@return the first tangential distortion coefficient, OpenCV's {@code p1}} */ + public double getP1() { + return p1; + } + + /** {@return the second tangential distortion coefficient, OpenCV's {@code p2}} */ + public double getP2() { + return p2; + } + + /** {@return the third radial distortion coefficient, OpenCV's {@code k3}} */ + public double getK3() { + return k3; + } + + /** {@return the five coefficients in OpenCV's order, {@code {k1, k2, p1, p2, k3}}, as a fresh array} */ + public double[] getDistortionCoefficients() { + return new double[] {k1, k2, p1, p2, k3}; + } + + /** {@return true when any distortion coefficient is non-zero} */ + public boolean hasDistortion() { + return distorted; + } + + /** + * The normalised image radius ({@code sqrt((X/Z)^2 + (Y/Z)^2)}) beyond which the distortion model is + * not one-to-one, so {@link #project} reports nothing there. + * + * @return the radius, dimensionless; infinite for a lens that never folds over (including no distortion) + */ + public double getMaxValidRadius() { + return maxValidRadius; + } + + /** {@return the full horizontal field of view of the pinhole part of this lens, degrees} */ public double getHorizontalFovDegrees() { return StrictMath.toDegrees(2.0 * StrictMath.atan((widthPx / 2.0) / focalXPx)); } @Override public String toString() { - return "CameraLens(" + widthPx + "x" + heightPx + ", fx=" + focalXPx + ")"; + return "CameraLens(" + widthPx + "x" + heightPx + ", fx=" + focalXPx + + (distorted ? ", k1=" + k1 + " k2=" + k2 + " p1=" + p1 + " p2=" + p2 + " k3=" + k3 : "") + ")"; } } diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/CameraOccluders.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/CameraOccluders.java new file mode 100644 index 0000000..42d811e --- /dev/null +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/CameraOccluders.java @@ -0,0 +1,130 @@ +package org.horizon36596.simloop.vision; + +import org.horizon36596.simloop.field.ObstacleBox; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.List; + +/** + * The solid things a camera cannot see through, in two frames: boxes fixed to the field (structures, + * walls, goals) and boxes fixed to the robot (its own chassis, a raised arm, a bumper). Handed to + * {@link SimCamera}, {@link SimTagCamera} and {@link SimFrameRenderer} each frame, because both kinds can + * move: a field structure may be rebuilt every tick, and a robot's arm changes the robot's shape. + * + *

Frames. + *

    + *
  • Field boxes: {@link ObstacleBox} as everywhere else in SimLoop, field frame, inches, + * axis-aligned with the field.
    • + *
  • Robot boxes: also {@link ObstacleBox}, but read in the robot frame of + * {@link CameraMount}: {@code X} is forward, {@code Y} is left, {@code Z} is up off the floor, origin + * on the floor under the robot's centre of rotation. So {@code new ObstacleBox("chassis", -9, 9, -8, + * 8, 0, 4)} is an 18 x 16 in chassis 4 in tall, and it turns with the robot. Robot boxes are + * axis-aligned with the robot.
    • + *
+ * + *

A camera mounted inside one of the robot's own boxes would see nothing, so a box that contains the + * lens is skipped for that camera; give the camera's housing no box. + * + *

Immutable: both lists are copied. + */ +public final class CameraOccluders { + + private static final CameraOccluders NONE = new CameraOccluders(Collections.emptyList(), + Collections.emptyList()); + + private final List fieldBoxes; + private final List robotBoxes; + + private CameraOccluders(List fieldBoxes, List robotBoxes) { + this.fieldBoxes = Collections.unmodifiableList(new ArrayList(fieldBoxes)); + this.robotBoxes = Collections.unmodifiableList(new ArrayList(robotBoxes)); + } + + /** {@return nothing blocks the view} */ + public static CameraOccluders none() { + return NONE; + } + + /** + * Boxes in both frames. + * + * @param fieldBoxes boxes fixed to the field, field frame, inches; may be empty, not null + * @param robotBoxes boxes fixed to the robot, robot frame ({@code X} forward, {@code Y} left, {@code Z} + * up), inches; may be empty, not null + * @return the occluders + */ + public static CameraOccluders of(List fieldBoxes, List robotBoxes) { + if (fieldBoxes == null || robotBoxes == null) { + throw new IllegalArgumentException("occluder lists must not be null; pass an empty list"); + } + return new CameraOccluders(fieldBoxes, robotBoxes); + } + + /** + * Field boxes only. + * + * @param fieldBoxes boxes fixed to the field, field frame, inches + * @return the occluders + */ + public static CameraOccluders field(List fieldBoxes) { + return of(fieldBoxes, Collections.emptyList()); + } + + /** {@return the boxes fixed to the field, field frame, unmodifiable} */ + public List getFieldBoxes() { + return fieldBoxes; + } + + /** {@return the boxes fixed to the robot, robot frame, unmodifiable} */ + public List getRobotBoxes() { + return robotBoxes; + } + + /** {@return true when there are no boxes at all} */ + public boolean isEmpty() { + return fieldBoxes.isEmpty() && robotBoxes.isEmpty(); + } + + @Override + public String toString() { + return "CameraOccluders(" + fieldBoxes.size() + " field, " + robotBoxes.size() + " robot)"; + } + + // ------------------------------------------------------------------------------ shared ray geometry + + /** + * The ray parameter at which {@code origin + t * dir} first enters the box, or {@code +infinity} when it + * does not within {@code (minT, maxT)}. A ray starting inside the box enters at {@code minT}. + */ + static double enterParameter(double[] origin, double[] dir, ObstacleBox box, double minT, double maxT) { + double enter = minT; + double exit = maxT; + double[] min = {box.getMinXInches(), box.getMinYInches(), box.getMinZInches()}; + double[] max = {box.getMaxXInches(), box.getMaxYInches(), box.getMaxZInches()}; + for (int axis = 0; axis < 3; axis++) { + double d = dir[axis]; + if (Math.abs(d) < 1e-12) { + if (origin[axis] <= min[axis] || origin[axis] >= max[axis]) { + return Double.POSITIVE_INFINITY; + } + continue; + } + double t0 = (min[axis] - origin[axis]) / d; + double t1 = (max[axis] - origin[axis]) / d; + enter = Math.max(enter, Math.min(t0, t1)); + exit = Math.min(exit, Math.max(t0, t1)); + if (enter >= exit) { + return Double.POSITIVE_INFINITY; + } + } + return enter; + } + + /** Whether a point is inside the box or on its surface. */ + static boolean contains(ObstacleBox box, double x, double y, double z) { + return x >= box.getMinXInches() && x <= box.getMaxXInches() + && y >= box.getMinYInches() && y <= box.getMaxYInches() + && z >= box.getMinZInches() && z <= box.getMaxZInches(); + } +} diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/CameraSighting.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/CameraSighting.java index 3b91b7f..49f3106 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/vision/CameraSighting.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/CameraSighting.java @@ -21,9 +21,16 @@ public final class CameraSighting { private final double radiusPx; private final double rangeInches; private final boolean clipped; + private final double visibleFraction; CameraSighting(int pieceId, String kind, double centreXPx, double centreYPx, double radiusPx, double rangeInches, boolean clipped) { + this(pieceId, kind, centreXPx, centreYPx, radiusPx, rangeInches, clipped, 1.0); + } + + CameraSighting(int pieceId, String kind, double centreXPx, double centreYPx, double radiusPx, + double rangeInches, boolean clipped, double visibleFraction) { + this.visibleFraction = visibleFraction; this.pieceId = pieceId; this.kind = kind; this.centreXPx = centreXPx; @@ -79,9 +86,24 @@ public boolean isClipped() { return clipped; } + /** + * How much of the piece the camera can see past the occluders, 0 to 1: the share of sample rays + * spread over the piece's disc (as the camera sees it) that reach the piece without passing through a + * box or a nearer piece. Always 1 from {@link SimCamera#observe(java.util.List, double, double, double)}, + * which has no occluders; from the overload that takes {@link CameraOccluders} it is the fraction of + * {@link SimCamera#OCCLUSION_SAMPLES} rays. The image edge does not count here: that is + * {@link #isClipped()}. + * + * @return the visible fraction, 0 to 1 + */ + public double getVisibleFraction() { + return visibleFraction; + } + @Override public String toString() { return "CameraSighting(piece " + pieceId + " " + kind + " at (" + centreXPx + ", " + centreYPx - + ") r=" + radiusPx + "px range=" + rangeInches + "in" + (clipped ? " clipped" : "") + ")"; + + ") r=" + radiusPx + "px range=" + rangeInches + "in" + (clipped ? " clipped" : "") + + (visibleFraction < 1.0 ? " visible=" + visibleFraction : "") + ")"; } } diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/FrameStyle.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/FrameStyle.java new file mode 100644 index 0000000..adafa9f --- /dev/null +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/FrameStyle.java @@ -0,0 +1,359 @@ +package org.horizon36596.simloop.vision; + +import java.util.ArrayList; +import java.util.Collections; +import java.util.LinkedHashMap; +import java.util.List; +import java.util.Map; + +/** + * The flat colours and the one light a {@link SimFrameRenderer} paints a scene with. Every colour is a + * packed {@code 0xRRGGBB} integer, 8 bits per channel, sRGB as a webcam delivers it. + * + *

These colours are made up. The defaults are greys chosen to tell surfaces apart, and the + * colours you give pieces are whatever you choose. None of them is a measurement of a real material under + * real light. A colour threshold tuned on frames painted with them says nothing about a real camera, so + * never tune one on a rendered frame: tune on real photos, and use rendered frames only to check that the + * pipeline's code paths and geometry work (see {@link SimFrameRenderer}). + * + *

Shading is Lambert with one directional light plus ambient: a surface facing the light gets + * {@code ambient + diffuse} times its colour, one facing away gets {@code ambient} times it. No shadows, + * no specular highlights, no reflections. + * + *

Immutable. Build one with {@link #builder()}; {@link #defaults()} is the builder's starting point. + */ +public final class FrameStyle { + + private static final FrameStyle DEFAULTS = builder().build(); + + private final int floorRgb; + private final double tileSizeInches; + private final int seamRgb; + private final double seamWidthInches; + private final int backgroundRgb; + private final int boxRgb; + private final int robotRgb; + private final int pieceRgb; + private final int tagBlackRgb; + private final int tagWhiteRgb; + private final int tagBackRgb; + private final Map boxColoursByPrefix; + private final Map pieceColoursByKind; + private final double ambient; + private final double diffuse; + private final double[] lightDirection; + + private FrameStyle(Builder b) { + this.floorRgb = b.floorRgb; + this.tileSizeInches = b.tileSizeInches; + this.seamRgb = b.seamRgb; + this.seamWidthInches = b.seamWidthInches; + this.backgroundRgb = b.backgroundRgb; + this.boxRgb = b.boxRgb; + this.robotRgb = b.robotRgb; + this.pieceRgb = b.pieceRgb; + this.tagBlackRgb = b.tagBlackRgb; + this.tagWhiteRgb = b.tagWhiteRgb; + this.tagBackRgb = b.tagBackRgb; + this.boxColoursByPrefix = Collections.unmodifiableMap(new LinkedHashMap(b.boxColours)); + this.pieceColoursByKind = Collections.unmodifiableMap(new LinkedHashMap(b.pieceColours)); + this.ambient = b.ambient; + this.diffuse = b.diffuse; + double length = StrictMath.sqrt(b.lightX * b.lightX + b.lightY * b.lightY + b.lightZ * b.lightZ); + this.lightDirection = new double[] {b.lightX / length, b.lightY / length, b.lightZ / length}; + } + + /** {@return the default style: grey tiles, grey structures, a dark robot, light grey pieces} */ + public static FrameStyle defaults() { + return DEFAULTS; + } + + /** {@return a builder starting from the defaults} */ + public static Builder builder() { + return new Builder(); + } + + /** {@return the floor colour, {@code 0xRRGGBB}} */ + public int getFloorRgb() { + return floorRgb; + } + + /** {@return the tile edge, inches; 0 means a plain floor with no seams} */ + public double getTileSizeInches() { + return tileSizeInches; + } + + /** {@return the colour of the seams between tiles, {@code 0xRRGGBB}} */ + public int getSeamRgb() { + return seamRgb; + } + + /** {@return the seam width, inches} */ + public double getSeamWidthInches() { + return seamWidthInches; + } + + /** {@return the colour of a ray that hits nothing, {@code 0xRRGGBB}} */ + public int getBackgroundRgb() { + return backgroundRgb; + } + + /** {@return the colour of a field box no prefix matches, {@code 0xRRGGBB}} */ + public int getBoxRgb() { + return boxRgb; + } + + /** {@return the colour of a robot box no prefix matches, {@code 0xRRGGBB}} */ + public int getRobotRgb() { + return robotRgb; + } + + /** {@return the colour of a piece whose kind has no colour of its own, {@code 0xRRGGBB}} */ + public int getPieceRgb() { + return pieceRgb; + } + + /** {@return the dark cells of a tag, {@code 0xRRGGBB}} */ + public int getTagBlackRgb() { + return tagBlackRgb; + } + + /** {@return the light cells and border of a tag, {@code 0xRRGGBB}} */ + public int getTagWhiteRgb() { + return tagWhiteRgb; + } + + /** {@return the back of a tag, {@code 0xRRGGBB}} */ + public int getTagBackRgb() { + return tagBackRgb; + } + + /** {@return the ambient share of a surface's colour, 0 to 1} */ + public double getAmbient() { + return ambient; + } + + /** {@return the share a surface facing the light gets on top of ambient, 0 to 1} */ + public double getDiffuse() { + return diffuse; + } + + /** {@return the unit vector toward the light, field frame, {@code {x, y, z}}, a fresh copy} */ + public double[] getLightDirection() { + return lightDirection.clone(); + } + + /** + * The colour a box is painted: the first prefix (in the order they were added) its name starts with, + * else the field or robot default. + * + * @param boxName the {@code ObstacleBox}'s name + * @param robotBox true for a box fixed to the robot + * @return the colour, {@code 0xRRGGBB} + */ + public int colourOfBox(String boxName, boolean robotBox) { + for (Map.Entry entry : boxColoursByPrefix.entrySet()) { + if (boxName.startsWith(entry.getKey())) { + return entry.getValue(); + } + } + return robotBox ? robotRgb : boxRgb; + } + + /** + * The colour a piece is painted. + * + * @param kind the piece's kind label + * @return its colour, or {@link #getPieceRgb()} when the kind has none, {@code 0xRRGGBB} + */ + public int colourOfPiece(String kind) { + Integer rgb = pieceColoursByKind.get(kind); + return rgb != null ? rgb : pieceRgb; + } + + /** {@return the piece kinds that have their own colour, in the order added} */ + public List getColouredKinds() { + return Collections.unmodifiableList(new ArrayList(pieceColoursByKind.keySet())); + } + + /** Collects a {@link FrameStyle}. Every setting starts at the default stated on it. */ + public static final class Builder { + + private int floorRgb = 0x808080; + private double tileSizeInches = 24.0; + private int seamRgb = 0x5C5C5C; + private double seamWidthInches = 0.75; + private int backgroundRgb = 0x262626; + private int boxRgb = 0x4A4E58; + private int robotRgb = 0x2E2E2E; + private int pieceRgb = 0xC8C8C8; + private int tagBlackRgb = 0x141414; + private int tagWhiteRgb = 0xF0F0F0; + private int tagBackRgb = 0x6E6E6E; + private final Map boxColours = new LinkedHashMap(); + private final Map pieceColours = new LinkedHashMap(); + private double ambient = 0.45; + private double diffuse = 0.55; + private double lightX = 0.25; + private double lightY = 0.15; + private double lightZ = 1.0; + + private Builder() { + } + + /** + * The floor. Default mid grey {@code 0x808080}, 24 in tiles with 0.75 in seams of {@code 0x5C5C5C}. + * + * @param rgb floor colour, {@code 0xRRGGBB} + * @param tileSizeInches tile edge, inches; 0 for a plain floor + * @param seamRgb seam colour, {@code 0xRRGGBB} + * @param seamWidthInches seam width, inches, ≥ 0 and below the tile size + * @return this builder + */ + public Builder floor(int rgb, double tileSizeInches, int seamRgb, double seamWidthInches) { + if (!(tileSizeInches >= 0.0) || Double.isInfinite(tileSizeInches)) { + throw new IllegalArgumentException("tile size must be finite and >= 0, got " + tileSizeInches); + } + if (!(seamWidthInches >= 0.0) || (tileSizeInches > 0.0 && seamWidthInches >= tileSizeInches)) { + throw new IllegalArgumentException("seam width must be >= 0 and below the tile size, got " + + seamWidthInches); + } + this.floorRgb = rgb(rgb); + this.tileSizeInches = tileSizeInches; + this.seamRgb = rgb(seamRgb); + this.seamWidthInches = seamWidthInches; + return this; + } + + /** + * What a ray that hits nothing sees. Default {@code 0x262626}. + * + * @param rgb the colour, {@code 0xRRGGBB} + * @return this builder + */ + public Builder background(int rgb) { + this.backgroundRgb = rgb(rgb); + return this; + } + + /** + * Field boxes no prefix matches. Default {@code 0x4A4E58}. + * + * @param rgb the colour, {@code 0xRRGGBB} + * @return this builder + */ + public Builder boxes(int rgb) { + this.boxRgb = rgb(rgb); + return this; + } + + /** + * Robot boxes no prefix matches. Default {@code 0x2E2E2E}. + * + * @param rgb the colour, {@code 0xRRGGBB} + * @return this builder + */ + public Builder robot(int rgb) { + this.robotRgb = rgb(rgb); + return this; + } + + /** + * Paints every box, field or robot, whose name starts with {@code prefix}. Prefixes are tried in the + * order added; the first match wins. + * + * @param prefix the start of the box names, non-empty + * @param rgb the colour, {@code 0xRRGGBB} + * @return this builder + */ + public Builder boxColour(String prefix, int rgb) { + if (prefix == null || prefix.isEmpty()) { + throw new IllegalArgumentException("a box colour prefix must be non-empty"); + } + boxColours.put(prefix, rgb(rgb)); + return this; + } + + /** + * Pieces whose kind has no colour of its own. Default {@code 0xC8C8C8}. + * + * @param rgb the colour, {@code 0xRRGGBB} + * @return this builder + */ + public Builder pieces(int rgb) { + this.pieceRgb = rgb(rgb); + return this; + } + + /** + * Paints every piece of one kind. + * + * @param kind the {@code GamePiece} kind label + * @param rgb the colour, {@code 0xRRGGBB} + * @return this builder + */ + public Builder pieceColour(String kind, int rgb) { + if (kind == null) { + throw new IllegalArgumentException("piece kind must not be null"); + } + pieceColours.put(kind, rgb(rgb)); + return this; + } + + /** + * The tag colours. Defaults {@code 0x141414}, {@code 0xF0F0F0} and, for the back, {@code 0x6E6E6E}. + * + * @param blackRgb dark cells, {@code 0xRRGGBB} + * @param whiteRgb light cells and border, {@code 0xRRGGBB} + * @param backRgb the back of the tag, {@code 0xRRGGBB} + * @return this builder + */ + public Builder tags(int blackRgb, int whiteRgb, int backRgb) { + this.tagBlackRgb = rgb(blackRgb); + this.tagWhiteRgb = rgb(whiteRgb); + this.tagBackRgb = rgb(backRgb); + return this; + } + + /** + * The light. Default ambient 0.45, diffuse 0.55, light from above, a little toward {@code +X} and + * {@code +Y}: direction {@code (0.25, 0.15, 1)} before normalising. + * + * @param ambient share of colour every surface gets, 0 to 1 + * @param diffuse share a surface facing the light gets on top, 0 to 1 + * @param towardX direction toward the light, field X, any length + * @param towardY direction toward the light, field Y + * @param towardZ direction toward the light, up + * @return this builder + */ + public Builder light(double ambient, double diffuse, double towardX, double towardY, double towardZ) { + if (!(ambient >= 0.0 && ambient <= 1.0) || !(diffuse >= 0.0 && diffuse <= 1.0)) { + throw new IllegalArgumentException("ambient and diffuse must be in [0, 1], got " + ambient + + " and " + diffuse); + } + double length = StrictMath.sqrt(towardX * towardX + towardY * towardY + towardZ * towardZ); + if (!(length > 0.0) || Double.isInfinite(length)) { + throw new IllegalArgumentException("the light direction must be a finite, non-zero vector"); + } + this.ambient = ambient; + this.diffuse = diffuse; + this.lightX = towardX; + this.lightY = towardY; + this.lightZ = towardZ; + return this; + } + + /** {@return the style} */ + public FrameStyle build() { + return new FrameStyle(this); + } + + private static int rgb(int rgb) { + if ((rgb & 0xFF000000) != 0) { + throw new IllegalArgumentException("a colour is 0xRRGGBB, with nothing above bit 23; got 0x" + + Integer.toHexString(rgb)); + } + return rgb; + } + } +} 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 659a49a..0e73562 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimCamera.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimCamera.java @@ -1,6 +1,7 @@ package org.horizon36596.simloop.vision; import org.horizon36596.simloop.field.GamePiece; +import org.horizon36596.simloop.field.ObstacleBox; import org.psilynx.psikit.core.Logger; import org.psilynx.psikit.core.wpi.math.Pose3d; import org.psilynx.psikit.core.wpi.math.Rotation3d; @@ -14,30 +15,34 @@ /** * A camera on the robot that "sees" game pieces: given every piece's true position and the robot's pose, it * works out which pieces fall inside its image and where, as {@link CameraSighting}s. No image is rendered - * — the output is the geometry a renderer would draw and a detector would have to recover. + * here — the output is the geometry a renderer would draw and a detector would have to recover. + * {@link SimFrameRenderer} draws the same scene as synthetic pixels when a pixel pipeline needs them. * *

What this is for. A vision pipeline is two things: geometry (which pixel is which point on the - * floor, tracking, velocity) and appearance (colour thresholds, blob splitting). The sim can only ever - * test the first, and this class is the seam for that: a test hands the sightings to the pipeline's - * projection and tracking code and checks the answers against the pieces' true positions. Colour and - * thresholds need real frames from a real camera; a synthetic scene must never be used to tune them, - * because a colour range fitted to a renderer's flat colours says nothing about real lighting. + * floor, tracking, velocity) and appearance (colour thresholds, blob splitting). This class is the + * seam for the first: a test hands the sightings to the pipeline's projection and tracking code and checks + * the answers against the pieces' true positions. Colour and thresholds need real frames from a real + * camera; a synthetic scene (including {@link SimFrameRenderer}'s) must never be used to tune them, because + * a colour range fitted to a renderer's flat colours says nothing about real lighting. * *

Visibility rules, stated plainly. A piece is sighted when it is * {@linkplain GamePiece#isVisibleOnField() visible on the field} (not held, and not scored into no goal), * its centre is in front of the lens by more than its * own radius, and its apparent disc (a circle of {@link CameraSighting#getRadiusPx()} about the centre * pixel) overlaps the image rectangle. A sighting is clipped when part of that disc is outside - * the image. A piece whose centre pixel is covered by the disc of a nearer sighted piece is - * occluded and not reported at all — the one bit of "what is in front of what" the geometry can - * answer without rendering. Nothing else occludes: the robot's own body, the field structures and the far - * wall are all transparent to this camera, which is wrong for a real camera and documented here so a test - * does not depend on it. + * the image. What hides a piece depends on the overload. The four-argument + * {@link #observe(List, double, double, double)} hides only a piece whose centre pixel is covered by the + * disc of a nearer sighted piece; everything else is transparent. The overload taking + * {@link CameraOccluders} casts {@value #OCCLUSION_SAMPLES} rays over each piece's disc against the field + * boxes, the robot boxes (in the robot frame, turning with it) and every nearer piece, reports the share + * that gets through as {@link CameraSighting#getVisibleFraction()}, and drops a piece below + * {@link #getMinVisibleFraction()}. A box that holds the piece's centre or the lens does not hide it. + * Anything not given as a box (curved structures, the far wall unless it is a box) stays transparent. * *

A piece whose centre is no more than its own radius in front of the lens (behind the camera, or so * close that the ball would wrap around the lens) is dropped entirely: it is not sighted and it does not * occlude anything behind it either. A real camera would see such a piece as a blur filling the frame; the - * pinhole projection has no sensible answer for it, so this camera pretends it is not there. + * projection has no sensible answer for it, so this camera pretends it is not there. * *

Frames. Pieces and the robot pose are in the field frame (FTC-Cartesian, centre origin, inches, * heading CCW from +X, conventions §7). The mount is in the robot frame and the projection in the optical @@ -58,6 +63,37 @@ public final class SimCamera { private final CameraLens lens; private final CameraMount mount; + /** + * Rays the occluded {@link #observe(List, double, double, double, CameraOccluders)} casts per piece: the + * centre, six at 0.45 of the radius and twelve at 0.85, spread over the disc the camera sees. + */ + public static final int OCCLUSION_SAMPLES = 19; + + /** The visible fraction a piece needs, by default, to be reported when occluders are given: half. */ + public static final double DEFAULT_MIN_VISIBLE_FRACTION = 0.5; + + /** Sample offsets across the disc, in radii: {@code SAMPLE_A} along the horizontal axis, {@code B} across. */ + private static final double[] SAMPLE_A = new double[OCCLUSION_SAMPLES]; + private static final double[] SAMPLE_B = new double[OCCLUSION_SAMPLES]; + + static { + int k = 1; + for (int i = 0; i < 6; i++) { + double angle = 2.0 * Math.PI * i / 6.0; + SAMPLE_A[k] = 0.45 * StrictMath.cos(angle); + SAMPLE_B[k] = 0.45 * StrictMath.sin(angle); + k++; + } + for (int i = 0; i < 12; i++) { + double angle = 2.0 * Math.PI * (i + 0.5) / 12.0; + SAMPLE_A[k] = 0.85 * StrictMath.cos(angle); + SAMPLE_B[k] = 0.85 * StrictMath.sin(angle); + k++; + } + } + + private final double minVisibleFraction; + private List lastSightings = Collections.emptyList(); /** The camera's field pose at the last {@link #observe}; {@code null} until the first call. */ @@ -72,6 +108,15 @@ public final class SimCamera { * @param mount the extrinsics */ public SimCamera(String name, CameraLens lens, CameraMount mount) { + this(name, lens, mount, DEFAULT_MIN_VISIBLE_FRACTION); + } + + private SimCamera(String name, CameraLens lens, CameraMount mount, double minVisibleFraction) { + if (!(minVisibleFraction > 0.0 && minVisibleFraction <= 1.0)) { + throw new IllegalArgumentException("minimum visible fraction must be in (0, 1], got " + + minVisibleFraction); + } + this.minVisibleFraction = minVisibleFraction; if (name == null || !name.matches("[A-Za-z0-9_-]+")) { throw new IllegalArgumentException("a camera name becomes one segment of a log key, so it must be" + " non-empty and use only letters, digits, '_' and '-': \"" + name + "\""); @@ -96,6 +141,108 @@ public SimCamera(String name, CameraLens lens, CameraMount mount) { */ public List observe(List pieces, double robotXInches, double robotYInches, double robotHeadingRad) { + List candidates = candidates(pieces, robotXInches, robotYInches, robotHeadingRad); + + // Occlusion: drop a candidate whose centre pixel lies inside the disc of a nearer candidate. + List visible = new ArrayList<>(); + for (Candidate candidate : candidates) { + boolean hidden = false; + for (Candidate other : candidates) { + if (other == candidate || other.sighting.getRangeInches() >= candidate.sighting.getRangeInches()) { + continue; + } + double ddx = candidate.sighting.getCentreXPx() - other.sighting.getCentreXPx(); + double ddy = candidate.sighting.getCentreYPx() - other.sighting.getCentreYPx(); + if (ddx * ddx + ddy * ddy < other.sighting.getRadiusPx() * other.sighting.getRadiusPx()) { + hidden = true; + break; + } + } + if (!hidden) { + visible.add(candidate.sighting); + } + } + return keep(visible, robotXInches, robotYInches, robotHeadingRad); + } + + /** + * Work out what the camera sees this tick when solid things can block the view: the field's structures, + * the robot's own body, and the other pieces. Call once per tick instead of + * {@link #observe(List, double, double, double)}; the result is also kept for {@link #record}. + * + *

The occlusion rule. For each piece that passes the image test, {@link #OCCLUSION_SAMPLES} + * rays go from the lens to points spread over the piece's disc as the camera sees it (the centre, six + * points at 0.45 of the radius and twelve at 0.85). A ray is blocked when, before it reaches the piece's + * surface, it passes through a box in {@code occluders} or through another piece on the field. The + * piece's {@link CameraSighting#getVisibleFraction() visible fraction} is the share of rays not blocked, + * and the piece is reported only when that is at least {@link #getMinVisibleFraction()}. A box that + * contains the piece's centre (a piece resting inside a structure) does not hide it, and a box that + * contains the lens is skipped. The centre pixel and radius reported are the whole piece's, even when + * part of it is hidden; a real detector's blob centre would move toward the visible part. + * + * @param pieces every piece, from {@code GamePieceTracker.getPieces()} + * @param robotXInches robot centre, field frame, inches + * @param robotYInches robot centre, field frame, inches + * @param robotHeadingRad robot heading, radians CCW from field +X + * @param occluders field and robot boxes that block the view; {@link CameraOccluders#none()} leaves + * only piece-on-piece occlusion + * @return the sightings, sorted by piece id, unmodifiable + */ + public List observe(List pieces, double robotXInches, double robotYInches, + double robotHeadingRad, CameraOccluders occluders) { + if (occluders == null) { + throw new IllegalArgumentException("occluders must not be null; use CameraOccluders.none()"); + } + List candidates = candidates(pieces, robotXInches, robotYInches, robotHeadingRad); + double cos = StrictMath.cos(robotHeadingRad); + double sin = StrictMath.sin(robotHeadingRad); + double[] lensRobot = mount.opticalToRobot(0.0, 0.0, 0.0); + double[] lensField = robotToField(lensRobot, robotXInches, robotYInches, cos, sin); + + // Every piece that can block a ray: on the field and in front of the lens by more than its radius + // (a piece closer than that is dropped entirely, as observe() without occluders drops it). + List blockers = new ArrayList<>(); + for (GamePiece piece : pieces) { + if (!piece.isVisibleOnField()) { + continue; + } + double[] robot = fieldToRobot(piece, robotXInches, robotYInches, cos, sin); + if (mount.robotToOptical(robot[0], robot[1], robot[2])[2] > piece.getRadiusInches()) { + blockers.add(piece); + } + } + List robotBoxes = withoutBoxesContaining(occluders.getRobotBoxes(), lensRobot); + List fieldBoxes = withoutBoxesContaining(occluders.getFieldBoxes(), lensField); + + List visible = new ArrayList<>(); + for (Candidate candidate : candidates) { + double fraction = visibleFraction(candidate.piece, candidate.robot, lensRobot, lensField, cos, sin, + robotBoxes, fieldBoxes, blockers); + if (fraction > 0.0 && fraction >= minVisibleFraction) { + CameraSighting s = candidate.sighting; + visible.add(new CameraSighting(s.getPieceId(), s.getKind(), s.getCentreXPx(), s.getCentreYPx(), + s.getRadiusPx(), s.getRangeInches(), s.isClipped(), fraction)); + } + } + return keep(visible, robotXInches, robotYInches, robotHeadingRad); + } + + /** One piece that passed the image test, with its centre in the robot frame. */ + private static final class Candidate { + final GamePiece piece; + final double[] robot; + final CameraSighting sighting; + + Candidate(GamePiece piece, double[] robot, CameraSighting sighting) { + this.piece = piece; + this.robot = robot; + this.sighting = sighting; + } + } + + /** Every piece in front of the lens whose disc overlaps the image, in the order given, unoccluded. */ + private List candidates(List pieces, double robotXInches, double robotYInches, + double robotHeadingRad) { if (pieces == null) { throw new IllegalArgumentException("pieces must not be null"); } @@ -106,28 +253,31 @@ public List observe(List pieces, double robotXInches, double cos = StrictMath.cos(robotHeadingRad); double sin = StrictMath.sin(robotHeadingRad); - List candidates = new ArrayList<>(); + List candidates = new ArrayList<>(); for (GamePiece piece : pieces) { if (!piece.isVisibleOnField()) { continue; } // Field -> robot frame: translate to the robot, rotate by -heading. Height is already robot up. - double dx = piece.getFieldXInches() - robotXInches; - double dy = piece.getFieldYInches() - robotYInches; - double forward = dx * cos + dy * sin; - double left = -dx * sin + dy * cos; - double[] optical = mount.robotToOptical(forward, left, piece.getFieldZInches()); + double[] robot = fieldToRobot(piece, robotXInches, robotYInches, cos, sin); + double[] optical = mount.robotToOptical(robot[0], robot[1], robot[2]); double depth = optical[2]; if (depth <= piece.getRadiusInches()) { continue; // behind, or wrapped around, the lens } - double centreX = lens.getCentreXPx() + lens.getFocalXPx() * optical[0] / depth; - double centreY = lens.getCentreYPx() + lens.getFocalYPx() * optical[1] / depth; + double[] pixel = lens.project(optical[0], optical[1], depth); + if (pixel == null) { + continue; // past where the lens's distortion model holds + } + double centreX = pixel[0]; + double centreY = pixel[1]; double range = StrictMath.sqrt(optical[0] * optical[0] + optical[1] * optical[1] + depth * depth); // Apparent radius as focal length times real radius over distance to the lens -- an // approximation, documented on CameraSighting.getRadiusPx(), kept because it is what a simple - // synthetic renderer draws. - double radiusPx = lens.getFocalXPx() * piece.getRadiusInches() / range; + // synthetic renderer draws -- scaled by the lens's local magnification, exactly 1 without + // distortion. + double radiusPx = lens.getFocalXPx() * piece.getRadiusInches() / range + * lens.localMagnification(optical[0] / depth, optical[1] / depth); // Disc-versus-rectangle overlap: the nearest image point to the disc's centre must lie inside // the disc. (Testing the disc's bounding square instead would wrongly count a disc sitting just @@ -142,30 +292,15 @@ public List observe(List pieces, double robotXInches, } boolean clipped = centreX - radiusPx < 0.0 || centreX + radiusPx > lens.getWidthPx() || centreY - radiusPx < 0.0 || centreY + radiusPx > lens.getHeightPx(); - candidates.add(new CameraSighting(piece.getId(), piece.getKind(), centreX, centreY, radiusPx, - range, clipped)); - } - - // Occlusion: drop a candidate whose centre pixel lies inside the disc of a nearer candidate. - List visible = new ArrayList<>(); - for (CameraSighting candidate : candidates) { - boolean hidden = false; - for (CameraSighting other : candidates) { - if (other == candidate || other.getRangeInches() >= candidate.getRangeInches()) { - continue; - } - double ddx = candidate.getCentreXPx() - other.getCentreXPx(); - double ddy = candidate.getCentreYPx() - other.getCentreYPx(); - if (ddx * ddx + ddy * ddy < other.getRadiusPx() * other.getRadiusPx()) { - hidden = true; - break; - } - } - if (!hidden) { - visible.add(candidate); - } + candidates.add(new Candidate(piece, robot, new CameraSighting(piece.getId(), piece.getKind(), + centreX, centreY, radiusPx, range, clipped))); } + return candidates; + } + /** Sorts by piece id, keeps the list and the camera pose for {@link #record}, and returns the list. */ + private List keep(List visible, double robotXInches, double robotYInches, + double robotHeadingRad) { // Piece id order, as documented. The list a tracker hands out happens to be in id order already, but // a caller may pass a filtered or reordered list, and the output order must not depend on that. Collections.sort(visible, new Comparator() { @@ -179,6 +314,143 @@ public int compare(CameraSighting a, CameraSighting b) { return lastSightings; } + /** A piece's centre in the robot frame: translate to the robot, rotate by -heading. */ + private static double[] fieldToRobot(GamePiece piece, double robotX, double robotY, double cos, double sin) { + double dx = piece.getFieldXInches() - robotX; + double dy = piece.getFieldYInches() - robotY; + return new double[] {dx * cos + dy * sin, -dx * sin + dy * cos, piece.getFieldZInches()}; + } + + private static double[] robotToField(double[] robot, double robotX, double robotY, double cos, double sin) { + return new double[] {robotX + robot[0] * cos - robot[1] * sin, robotY + robot[0] * sin + robot[1] * cos, + robot[2]}; + } + + /** The share of the {@link #OCCLUSION_SAMPLES} rays from the lens to the piece's disc that are not blocked. */ + private static double visibleFraction(GamePiece piece, double[] centre, double[] lensRobot, + double[] lensField, double cos, double sin, List robotBoxes, + List fieldBoxes, List blockers) { + double radius = piece.getRadiusInches(); + double vx = centre[0] - lensRobot[0]; + double vy = centre[1] - lensRobot[1]; + double vz = centre[2] - lensRobot[2]; + double length = StrictMath.sqrt(vx * vx + vy * vy + vz * vz); + // Two unit vectors across the line of sight: u1 horizontal, u2 = (line of sight) x u1. + double u1x = vy; + double u1y = -vx; + double u1Length = StrictMath.sqrt(u1x * u1x + u1y * u1y); + if (u1Length < 1e-9) { + u1x = 1.0; + u1y = 0.0; + u1Length = 1.0; + } + u1x /= u1Length; + u1y /= u1Length; + double wx = vx / length; + double wy = vy / length; + double wz = vz / length; + double u2x = -wz * u1y; + double u2y = wz * u1x; + double u2z = wx * u1y - wy * u1x; + + double[] centreField = {piece.getFieldXInches(), piece.getFieldYInches(), piece.getFieldZInches()}; + List robotCandidates = withoutBoxesContaining(robotBoxes, centre); + List fieldCandidates = withoutBoxesContaining(fieldBoxes, centreField); + + int clear = 0; + for (int k = 0; k < OCCLUSION_SAMPLES; k++) { + double a = SAMPLE_A[k] * radius; + double b = SAMPLE_B[k] * radius; + double[] dirRobot = {vx + a * u1x + b * u2x, vy + a * u1y + b * u2y, vz + b * u2z}; + double tSurface = sphereEntry(lensRobot, dirRobot, centre, radius); + if (!(tSurface > 0.0)) { + continue; // cannot happen for a point inside the disc + } + if (blockedByBoxes(lensRobot, dirRobot, tSurface, robotCandidates)) { + continue; + } + double[] dirField = {dirRobot[0] * cos - dirRobot[1] * sin, dirRobot[0] * sin + dirRobot[1] * cos, + dirRobot[2]}; + if (blockedByBoxes(lensField, dirField, tSurface, fieldCandidates)) { + continue; + } + boolean hitPiece = false; + for (GamePiece other : blockers) { + if (other.getId() == piece.getId()) { + continue; + } + double[] otherCentre = {other.getFieldXInches(), other.getFieldYInches(), other.getFieldZInches()}; + double t = sphereEntry(lensField, dirField, otherCentre, other.getRadiusInches()); + if (t > 0.0 && t < tSurface) { + hitPiece = true; + break; + } + } + if (!hitPiece) { + clear++; + } + } + return clear / (double) OCCLUSION_SAMPLES; + } + + private static List withoutBoxesContaining(List boxes, double[] point) { + List kept = new ArrayList<>(boxes.size()); + for (ObstacleBox box : boxes) { + if (!CameraOccluders.contains(box, point[0], point[1], point[2])) { + kept.add(box); + } + } + return kept; + } + + private static boolean blockedByBoxes(double[] origin, double[] dir, double tLimit, List boxes) { + for (ObstacleBox box : boxes) { + if (CameraOccluders.enterParameter(origin, dir, box, 0.0, tLimit) < tLimit) { + return true; + } + } + return false; + } + + /** + * The smallest positive {@code t} at which {@code origin + t * dir} meets the sphere's surface from + * outside, or {@code NaN} when the ray misses it, starts inside it, or the sphere is behind the origin. + */ + static double sphereEntry(double[] origin, double[] dir, double[] centre, double radius) { + double ox = origin[0] - centre[0]; + double oy = origin[1] - centre[1]; + double oz = origin[2] - centre[2]; + double a = dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]; + double b = 2.0 * (ox * dir[0] + oy * dir[1] + oz * dir[2]); + double c = ox * ox + oy * oy + oz * oz - radius * radius; + if (c <= 0.0) { + return Double.NaN; + } + double disc = b * b - 4.0 * a * c; + if (disc < 0.0) { + return Double.NaN; + } + double t0 = (-b - StrictMath.sqrt(disc)) / (2.0 * a); + return t0 > 0.0 ? t0 : Double.NaN; + } + + /** {@return the visible fraction a piece needs to be reported by the occluded {@code observe}, 0 to 1} */ + public double getMinVisibleFraction() { + return minVisibleFraction; + } + + /** + * This camera with a different minimum visible fraction for the occluded + * {@link #observe(List, double, double, double, CameraOccluders)}: a detector that still finds a ball + * when a quarter of it shows wants 0.25. The default is {@link #DEFAULT_MIN_VISIBLE_FRACTION}. + * + * @param fraction the minimum share of sample rays that must reach the piece, in {@code (0, 1]} + * @return a new camera with the same name, lens and mount + */ + public SimCamera withMinVisibleFraction(double fraction) { + return new SimCamera(name, lens, mount, fraction); + } + /** {@return what {@link #observe} last returned; empty before the first call} */ public List getLastSightings() { return lastSightings; @@ -188,7 +460,8 @@ public List getLastSightings() { * The point on a horizontal plane that a pixel looks at — the inverse of {@link #observe}'s projection * for a point at a known height, in the robot frame. This is the calculation a ground-plane vision * pipeline does to turn a detected ball centre into a floor position, so a test can round-trip a - * sighting through it and check the result against the piece's true position. + * sighting through it and check the result against the piece's true position. A distorted lens is + * undistorted first ({@link CameraLens#undistortPixel}), as a calibrated pipeline does. * * @param centreXPx the pixel's x, from the left edge * @param centreYPx the pixel's y, from the top edge @@ -202,8 +475,9 @@ public RobotFramePoint groundPointInRobotFrame(double centreXPx, double centreYP requireFinite(centreYPx, "centreYPx"); requireFinite(pointHeightInches, "pointHeightInches"); // A ray through the pixel in the optical frame, at unit depth. - double xOverZ = (centreXPx - lens.getCentreXPx()) / lens.getFocalXPx(); - double yOverZ = (centreYPx - lens.getCentreYPx()) / lens.getFocalYPx(); + double[] normalized = lens.undistortPixel(centreXPx, centreYPx); + double xOverZ = normalized[0]; + double yOverZ = normalized[1]; double[] rayEnd = mount.opticalToRobot(xOverZ, yOverZ, 1.0); double[] rayStart = mount.opticalToRobot(0.0, 0.0, 0.0); double dForward = rayEnd[0] - rayStart[0]; @@ -248,7 +522,8 @@ public Pose3d fieldPose3d(double robotXInches, double robotYInches, double robot *

  • {@code sightingCount} — int
    • *
  • {@code sightings/pieceIds} — int[]; {@code sightings/kinds} — String[]; * {@code sightings/centreXPx}, {@code centreYPx}, {@code radiusPx}, {@code rangeInches} — - * double[]; {@code sightings/clipped} — boolean[]; all index-aligned
    • + * double[]; {@code sightings/clipped} — boolean[]; {@code sightings/visibleFraction} — double[] + * (1 without occluders); all index-aligned *
  • {@code seenPositions3d} — Translation3d[] of the sighted pieces' true positions, metres, for * a Vision Target object on the 3D Field, so a viewer sees which balls the camera saw
    • * @@ -276,6 +551,7 @@ public void record(String keyPrefix, List pieces) { double[] radius = new double[count]; double[] range = new double[count]; boolean[] clipped = new boolean[count]; + double[] visibleFraction = new double[count]; Translation3d[] seen = new Translation3d[count]; for (int i = 0; i < count; i++) { CameraSighting sighting = lastSightings.get(i); @@ -286,6 +562,7 @@ public void record(String keyPrefix, List pieces) { radius[i] = sighting.getRadiusPx(); range[i] = sighting.getRangeInches(); clipped[i] = sighting.isClipped(); + visibleFraction[i] = sighting.getVisibleFraction(); GamePiece piece = pieceWithId(pieces, sighting.getPieceId()); seen[i] = new Translation3d(piece.getFieldXInches() * METRES_PER_INCH, piece.getFieldYInches() * METRES_PER_INCH, piece.getFieldZInches() * METRES_PER_INCH); @@ -299,6 +576,7 @@ public void record(String keyPrefix, List pieces) { Logger.recordOutput(prefix + "/sightings/radiusPx", radius); Logger.recordOutput(prefix + "/sightings/rangeInches", range); Logger.recordOutput(prefix + "/sightings/clipped", clipped); + Logger.recordOutput(prefix + "/sightings/visibleFraction", visibleFraction); Logger.recordOutput(prefix + "/seenPositions3d", seen); } diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimFrame.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimFrame.java new file mode 100644 index 0000000..69a5134 --- /dev/null +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimFrame.java @@ -0,0 +1,258 @@ +package org.horizon36596.simloop.vision; + +import java.io.ByteArrayOutputStream; +import java.io.IOException; +import java.io.OutputStream; +import java.nio.charset.Charset; +import java.util.zip.CRC32; +import java.util.zip.Deflater; + +/** + * One synthetic camera frame from a {@link SimFrameRenderer}: 8-bit RGB pixels, plus a label per pixel + * saying which piece or tag it shows. It is a drawing, not a photograph: flat colours, one light, no + * shadows, no blur, no compression. Use it to drive a real vision pipeline's code paths and check its + * geometry, never to tune a colour threshold. The PNG it writes says so in a text chunk. + * + *

    Pixel layout. Row-major from the top-left, {@code +x} right, {@code +y} down. {@link #getRgbBytes()} + * is interleaved {@code R, G, B}, three bytes a pixel, which is exactly the buffer an OpenCV + * {@code CV_8UC3} {@code Mat} of {@code height} rows and {@code width} columns holds: + * 

    + *   Mat rgb = new Mat(frame.getHeightPx(), frame.getWidthPx(), CvType.CV_8UC3);
+ *   rgb.put(0, 0, frame.getRgbBytes());
+ *   processor.processFrame(rgb, captureTimeNanos);      // your real VisionProcessor
+ *
    + * {@link #getRgbaBytes()} gives the four-channel form, alpha 255, for a pipeline that expects + * {@code CV_8UC4}. SimLoop itself never touches OpenCV. + * + *

    Immutable: every accessor that returns an array returns a copy. + */ +public final class SimFrame { + + /** The text the PNG's {@code tEXt} chunk carries, so a saved frame cannot pass for a camera image. */ + public static final String PNG_COMMENT = "SimLoop synthetic frame, rendered from sim geometry. Not a camera" + + " image: do not tune colour thresholds on it."; + + private static final byte[] PNG_SIGNATURE = {(byte) 0x89, 'P', 'N', 'G', '\r', '\n', 0x1A, '\n'}; + private static final Charset LATIN_1 = Charset.forName("ISO-8859-1"); + + private final int widthPx; + private final int heightPx; + private final long frameNumber; + private final byte[] rgb; + private final int[] pieceIds; + private final int[] tagIds; + + SimFrame(int widthPx, int heightPx, long frameNumber, byte[] rgb, int[] pieceIds, int[] tagIds) { + this.widthPx = widthPx; + this.heightPx = heightPx; + this.frameNumber = frameNumber; + this.rgb = rgb; + this.pieceIds = pieceIds; + this.tagIds = tagIds; + } + + /** {@return the image width, pixels} */ + public int getWidthPx() { + return widthPx; + } + + /** {@return the image height, pixels} */ + public int getHeightPx() { + return heightPx; + } + + /** {@return the renderer's count of frames before this one, from 0: the sensor-noise stream it used} */ + public long getFrameNumber() { + return frameNumber; + } + + /** {@return the pixels as interleaved R, G, B bytes, row-major from the top-left; a fresh copy} */ + public byte[] getRgbBytes() { + return rgb.clone(); + } + + /** {@return the pixels as interleaved R, G, B, A bytes with A = 255, row-major; a fresh copy} */ + public byte[] getRgbaBytes() { + byte[] rgba = new byte[widthPx * heightPx * 4]; + for (int i = 0, j = 0; i < rgb.length; i += 3, j += 4) { + rgba[j] = rgb[i]; + rgba[j + 1] = rgb[i + 1]; + rgba[j + 2] = rgb[i + 2]; + rgba[j + 3] = (byte) 0xFF; + } + return rgba; + } + + /** + * One pixel's colour. + * + * @param x column, 0 to width - 1 + * @param y row, 0 to height - 1 + * @return {@code 0xRRGGBB} + */ + public int getRgb(int x, int y) { + int i = index(x, y) * 3; + return ((rgb[i] & 0xFF) << 16) | ((rgb[i + 1] & 0xFF) << 8) | (rgb[i + 2] & 0xFF); + } + + /** + * Which piece a pixel shows: ground truth, from the ray through the pixel's centre. + * + * @param x column + * @param y row + * @return the piece id, or -1 when the pixel shows no piece + */ + public int getPieceIdAt(int x, int y) { + return pieceIds[index(x, y)]; + } + + /** + * Which tag a pixel shows, white border included: ground truth, from the ray through the pixel's centre. + * + * @param x column + * @param y row + * @return the tag id, or -1 when the pixel shows no tag + */ + public int getTagIdAt(int x, int y) { + return tagIds[index(x, y)]; + } + + /** + * How many pixels show a piece. 0 for a piece the camera cannot see, whether it is out of frame or + * hidden behind something. + * + * @param pieceId the piece's id + * @return the pixel count + */ + public int pixelCountOfPiece(int pieceId) { + int count = 0; + for (int id : pieceIds) { + if (id == pieceId) { + count++; + } + } + return count; + } + + /** + * The mean position of the pixels that show a piece: where a perfect blob detector would put its + * centre, which moves toward the visible part when the piece is partly hidden or cut off by the edge. + * + * @param pieceId the piece's id + * @return {@code {x, y}} in pixels, measured to pixel centres ({@code x + 0.5}), or {@code null} when no + * pixel shows the piece + */ + public double[] centroidOfPiece(int pieceId) { + double sumX = 0.0; + double sumY = 0.0; + int count = 0; + for (int y = 0; y < heightPx; y++) { + for (int x = 0; x < widthPx; x++) { + if (pieceIds[y * widthPx + x] == pieceId) { + sumX += x + 0.5; + sumY += y + 0.5; + count++; + } + } + } + return count == 0 ? null : new double[] {sumX / count, sumY / count}; + } + + /** + * The frame as a PNG: 8-bit RGB, no interlace, with a {@code tEXt} {@code Comment} chunk holding + * {@link #PNG_COMMENT}. Written with {@code java.util.zip} only, so it works wherever SimLoop runs. + * + * @param out where to write; not closed + * @throws IOException when {@code out} throws + */ + public void writePng(OutputStream out) throws IOException { + out.write(PNG_SIGNATURE); + ByteArrayOutputStream header = new ByteArrayOutputStream(); + writeInt(header, widthPx); + writeInt(header, heightPx); + header.write(8); // bit depth + header.write(2); // colour type: truecolour RGB + header.write(0); // deflate + header.write(0); // adaptive filtering, filter type per row + header.write(0); // no interlace + writeChunk(out, "IHDR", header.toByteArray()); + + ByteArrayOutputStream text = new ByteArrayOutputStream(); + text.write("Comment".getBytes(LATIN_1)); + text.write(0); + text.write(PNG_COMMENT.getBytes(LATIN_1)); + writeChunk(out, "tEXt", text.toByteArray()); + + int stride = widthPx * 3; + byte[] raw = new byte[(stride + 1) * heightPx]; + for (int y = 0; y < heightPx; y++) { + raw[y * (stride + 1)] = 0; // filter: none + System.arraycopy(rgb, y * stride, raw, y * (stride + 1) + 1, stride); + } + Deflater deflater = new Deflater(Deflater.DEFAULT_COMPRESSION); + try { + deflater.setInput(raw); + deflater.finish(); + ByteArrayOutputStream compressed = new ByteArrayOutputStream(); + byte[] buffer = new byte[64 * 1024]; + while (!deflater.finished()) { + int n = deflater.deflate(buffer); + compressed.write(buffer, 0, n); + } + writeChunk(out, "IDAT", compressed.toByteArray()); + } finally { + deflater.end(); + } + writeChunk(out, "IEND", new byte[0]); + } + + /** + * {@link #writePng} into a byte array. + * + * @return the PNG file's bytes + */ + public byte[] toPng() { + ByteArrayOutputStream out = new ByteArrayOutputStream(); + try { + writePng(out); + } catch (IOException impossible) { + throw new IllegalStateException("a ByteArrayOutputStream does not throw", impossible); + } + return out.toByteArray(); + } + + private static void writeChunk(OutputStream out, String type, byte[] data) throws IOException { + byte[] typeBytes = type.getBytes(LATIN_1); + ByteArrayOutputStream length = new ByteArrayOutputStream(4); + writeInt(length, data.length); + out.write(length.toByteArray()); + out.write(typeBytes); + out.write(data); + CRC32 crc = new CRC32(); + crc.update(typeBytes); + crc.update(data); + ByteArrayOutputStream crcBytes = new ByteArrayOutputStream(4); + writeInt(crcBytes, (int) crc.getValue()); + out.write(crcBytes.toByteArray()); + } + + private static void writeInt(ByteArrayOutputStream out, int value) { + out.write((value >>> 24) & 0xFF); + out.write((value >>> 16) & 0xFF); + out.write((value >>> 8) & 0xFF); + out.write(value & 0xFF); + } + + private int index(int x, int y) { + if (x < 0 || x >= widthPx || y < 0 || y >= heightPx) { + throw new IndexOutOfBoundsException("pixel (" + x + ", " + y + ") is outside the " + widthPx + "x" + + heightPx + " frame"); + } + return y * widthPx + x; + } + + @Override + public String toString() { + return "SimFrame(" + widthPx + "x" + heightPx + ", frame " + frameNumber + ", synthetic)"; + } +} diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimFrameRenderer.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimFrameRenderer.java new file mode 100644 index 0000000..14487e6 --- /dev/null +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimFrameRenderer.java @@ -0,0 +1,900 @@ +package org.horizon36596.simloop.vision; + +import org.horizon36596.simloop.field.GamePiece; +import org.horizon36596.simloop.field.ObstacleBox; + +import java.util.ArrayList; +import java.util.List; +import java.util.stream.IntStream; + +/** + * Draws what a {@link CameraLens} on a {@link CameraMount} would see, as a {@link SimFrame} of RGB pixels, + * so a team's real pixel pipeline (an OpenCV {@code VisionProcessor}, a colour-blob detector, a contour + * splitter) can run unchanged on frames from the sim. It is a ray caster: one ray per pixel (or a grid of + * {@code n x n} per pixel when supersampling), through the same lens model, distortion included, that + * {@link SimCamera} and {@link SimTagCamera} use, so a piece is drawn exactly where those cameras say it is. + * + *

    What it draws. The floor, as tiles with seams; field boxes and robot boxes from + * {@link CameraOccluders}, flat-coloured; game pieces as spheres in their kind's colour; AprilTags from an + * {@link AprilTagLayout} as black-and-white squares with a white border, lying in their own plane (tilted + * tags included). Colours come from a {@link FrameStyle}; shading is Lambert with one light and ambient. + * Optional extras: supersampling for smooth edges, a gain, and seeded Gaussian sensor noise. + * + *

    What it does not draw, stated so nothing depends on it: shadows, specular highlights, + * reflections, textures, motion blur, rolling shutter, exposure time, white balance, lens blur, vignetting, + * compression artefacts, anything not given as a box, piece or tag. Tag interiors are a deterministic + * pattern made from the id, not a real tag family's code, so a real AprilTag decoder will not decode + * them; use {@link SimTagCamera} or {@code FakeLimelight3A} to test tag pose code. + * + *

    Synthetic frames are never real frames. Use them to check that a pipeline's code paths run, + * that its geometry puts a blob back where the piece is, that edge-clipped and touching pieces are handled, + * and that occluded pieces are not reported. Never tune a colour threshold, an exposure or a minimum blob + * area on them: a threshold fitted to these flat colours says nothing about real lighting. {@link SimFrame} + * carries a per-pixel ground-truth label for checking, and its PNG says it is synthetic. + * + *

    Frames. Pieces, field boxes and tags in the field frame; robot boxes in the robot frame; the + * robot pose as {@link SimCamera#observe(List, double, double, double)} takes it. All inches and radians. + * + *

    Determinism. No clock and no shared random state: the sensor noise of frame {@code k} is a pure + * function of the seed, {@code k} and the pixel, so the same inputs in the same order give byte-identical + * frames. StrictMath for all trigonometry. Rows of tiles are drawn in parallel on the common fork-join + * pool, but every pixel is a pure function of the scene and writes only its own slots, so the bytes do not + * depend on how many threads ran or in what order. + * + *

    Cost. Pixels are grouped in 16 x 16 tiles, and each tile only tests the objects whose image + * bounds overlap it, so the cost is roughly one floor test per ray plus the objects actually near that + * ray. A 640 x 480 frame of a few dozen objects takes tens of milliseconds of CPU on a laptop at + * {@code supersampling(1)}, spread over its cores; keep rendered frames to the tests that need pixels. + */ +public final class SimFrameRenderer { + + private static final int TILE = 16; + + /** + * Where shape bounds are cut, inches in front of the lens. Anything closer than this cannot draw; a box + * holding the lens is left out anyway. + */ + private static final double NEAR_PLANE_INCHES = 1e-3; + /** A tag is pulled this far toward the camera so one mounted on a box face is drawn, not the face. */ + private static final double TAG_DEPTH_BIAS_INCHES = 0.02; + + private final CameraLens lens; + private final CameraMount mount; + private final FrameStyle style; + private final int supersampling; + private final int noiseSeed; + private final double noiseStdDevCounts; + private final double gain; + + private final int width; + private final int height; + /** Undistorted normalised ray through each pixel's centre. */ + private final double[] centreRayX; + private final double[] centreRayY; + /** Undistorted normalised rays for the supersampling grid, {@code n * n} per pixel; null when n = 1. */ + private final double[] subRayX; + private final double[] subRayY; + private final int tilesX; + private final int tilesY; + /** Per tile: min x, max x, min y, max y of its rays, normalised. */ + private final double[] tileBounds; + + private final double[] lensRobot; + private final double[] axisXRobot; + private final double[] axisYRobot; + private final double[] axisZRobot; + + private long framesRendered; + + private SimFrameRenderer(Builder b) { + this.lens = b.lens; + this.mount = b.mount; + this.style = b.style; + this.supersampling = b.supersampling; + this.noiseSeed = b.noiseSeed; + this.noiseStdDevCounts = b.noiseStdDevCounts; + this.gain = b.gain; + this.width = lens.getWidthPx(); + this.height = lens.getHeightPx(); + + int pixels = width * height; + centreRayX = new double[pixels]; + centreRayY = new double[pixels]; + int n = supersampling; + subRayX = n > 1 ? new double[pixels * n * n] : null; + subRayY = n > 1 ? new double[pixels * n * n] : null; + tilesX = (width + TILE - 1) / TILE; + tilesY = (height + TILE - 1) / TILE; + tileBounds = new double[tilesX * tilesY * 4]; + for (int t = 0; t < tilesX * tilesY; t++) { + tileBounds[4 * t] = Double.POSITIVE_INFINITY; + tileBounds[4 * t + 1] = Double.NEGATIVE_INFINITY; + tileBounds[4 * t + 2] = Double.POSITIVE_INFINITY; + tileBounds[4 * t + 3] = Double.NEGATIVE_INFINITY; + } + for (int y = 0; y < height; y++) { + 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); + 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); + int s = p * n * n + j * n + i; + subRayX[s] = r[0]; + subRayY[s] = r[1]; + grow(tile, r[0], r[1]); + } + } + } + } + } + + lensRobot = mount.opticalToRobot(0.0, 0.0, 0.0); + axisXRobot = minus(mount.opticalToRobot(1.0, 0.0, 0.0), lensRobot); + axisYRobot = minus(mount.opticalToRobot(0.0, 1.0, 0.0), lensRobot); + axisZRobot = minus(mount.opticalToRobot(0.0, 0.0, 1.0), lensRobot); + } + + private void grow(int tile, double x, double y) { + 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); + tileBounds[4 * tile + 3] = Math.max(tileBounds[4 * tile + 3], y); + } + + /** + * A builder for a renderer with this lens and mount, the default {@link FrameStyle}, no supersampling, + * gain 1 and no noise. + * + * @param lens the intrinsics, distortion included; its size is the frame size + * @param mount the extrinsics + * @return the builder + */ + public static Builder builder(CameraLens lens, CameraMount mount) { + return new Builder(lens, mount); + } + + /** + * Draws one frame. Advances the frame counter the sensor noise is keyed on. + * + * @param pieces the game pieces; only those {@linkplain GamePiece#isVisibleOnField() on the + * field} are drawn + * @param robotXInches robot centre, field X, inches + * @param robotYInches robot centre, field Y, inches + * @param robotHeadingRad robot heading, radians CCW from field {@code +X} + * @param boxes field and robot boxes to draw; a box containing the lens is left out + * @param tags the tags to draw; {@link AprilTagLayout#empty()} for none + * @return the frame + */ + public SimFrame render(List pieces, double robotXInches, double robotYInches, + double robotHeadingRad, CameraOccluders boxes, AprilTagLayout tags) { + if (pieces == null || boxes == null || tags == null) { + throw new IllegalArgumentException("pieces, boxes and tags must not be null; pass empty ones"); + } + requireFinite(robotXInches, "robotXInches"); + requireFinite(robotYInches, "robotYInches"); + requireFinite(robotHeadingRad, "robotHeadingRad"); + double cos = StrictMath.cos(robotHeadingRad); + double sin = StrictMath.sin(robotHeadingRad); + Scene scene = new Scene(robotXInches, robotYInches, cos, sin); + buildShapes(scene, pieces, boxes, tags); + + int pixels = width * height; + byte[] rgb = new byte[pixels * 3]; + int[] pieceIds = new int[pixels]; + int[] tagIds = new int[pixels]; + long frame = framesRendered++; + int frameKey = noiseSeed ^ mix((int) frame ^ mix((int) (frame >>> 32))); + // One row of tiles per task. Each task traces with its own copy of the scene's per-ray state. + IntStream.range(0, tilesY).parallel().forEach(ty -> renderTileRow(new Scene(scene), ty, frameKey, + rgb, pieceIds, tagIds)); + return new SimFrame(width, height, frame, rgb, pieceIds, tagIds); + } + + /** Draws one row of tiles into the frame's arrays; touches only that row's pixels. */ + private void renderTileRow(Scene scene, int ty, int frameKey, byte[] rgb, int[] pieceIds, int[] tagIds) { + int n = supersampling; + Hit hit = new Hit(); + double[] colour = new double[3]; + for (int tx = 0; tx < tilesX; tx++) { + int tile = ty * tilesX + tx; + Shape[] candidates = scene.candidatesFor(tileBounds, tile); + int x1 = Math.min(width, (tx + 1) * TILE); + int y1 = Math.min(height, (ty + 1) * TILE); + 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; + double r; + double g; + double bl; + if (n == 1) { + shade(scene, hit, colour); + r = colour[0]; + g = colour[1]; + bl = colour[2]; + } else { + r = 0.0; + g = 0.0; + bl = 0.0; + for (int s = p * n * n, end = s + n * n; s < end; s++) { + trace(scene, candidates, subRayX[s], subRayY[s], hit); + shade(scene, hit, colour); + r += colour[0]; + g += colour[1]; + bl += colour[2]; + } + r /= n * n; + g /= n * n; + bl /= n * n; + } + rgb[3 * p] = toByte(r * gain + noise(frameKey, p, 0)); + rgb[3 * p + 1] = toByte(g * gain + noise(frameKey, p, 1)); + rgb[3 * p + 2] = toByte(bl * gain + noise(frameKey, p, 2)); + } + } + } + } + + /** {@return how many frames this renderer has drawn} */ + public long getFramesRendered() { + return framesRendered; + } + + /** {@return the lens frames are drawn through} */ + public CameraLens getLens() { + return lens; + } + + /** {@return the mount frames are drawn from} */ + public CameraMount getMount() { + return mount; + } + + /** {@return the colours and light} */ + public FrameStyle getStyle() { + return style; + } + + // --------------------------------------------------------------------------------------------- scene + + /** The camera's pose for this frame and every shape to test, in both frames a shape may live in. */ + private final class Scene { + final double robotX; + final double robotY; + final double cos; + final double sin; + final double[] lensField; + final double[] axisXField; + final double[] axisYField; + final double[] axisZField; + final List shapes = new ArrayList(); + /** The ray for the current sample, field frame and robot frame. */ + double fdx; + double fdy; + double fdz; + double rdx; + double rdy; + double rdz; + + /** A copy for another thread: the same pose and shapes, its own per-ray state. */ + Scene(Scene shared) { + this.robotX = shared.robotX; + this.robotY = shared.robotY; + this.cos = shared.cos; + this.sin = shared.sin; + this.lensField = shared.lensField; + this.axisXField = shared.axisXField; + this.axisYField = shared.axisYField; + this.axisZField = shared.axisZField; + this.shapes.addAll(shared.shapes); + } + + Scene(double robotX, double robotY, double cos, double sin) { + this.robotX = robotX; + this.robotY = robotY; + this.cos = cos; + this.sin = sin; + this.lensField = pointToField(lensRobot); + this.axisXField = vectorToField(axisXRobot); + this.axisYField = vectorToField(axisYRobot); + this.axisZField = vectorToField(axisZRobot); + } + + double[] pointToField(double[] robot) { + return new double[] {robotX + robot[0] * cos - robot[1] * sin, robotY + robot[0] * sin + robot[1] * cos, + robot[2]}; + } + + double[] vectorToField(double[] robot) { + return new double[] {robot[0] * cos - robot[1] * sin, robot[0] * sin + robot[1] * cos, robot[2]}; + } + + /** + * Sets a shape's image bounds from field points whose convex hull holds it. The hull is cut at + * {@link #NEAR_PLANE_INCHES} in front of the lens first: the points in front, plus where each segment + * between a point in front and one behind crosses that plane (the segments between every pair + * include the hull's edges, so the bound stays conservative). A shape wholly behind the plane gets + * empty bounds and is never tested; a ray cannot hit it. + */ + void bound(Shape shape, double[][] fieldPoints) { + int n = fieldPoints.length; + double[][] optical = new double[n][]; + for (int i = 0; i < n; i++) { + optical[i] = toOptical(fieldPoints[i]); + } + double minX = Double.POSITIVE_INFINITY; + double maxX = Double.NEGATIVE_INFINITY; + double minY = Double.POSITIVE_INFINITY; + double maxY = Double.NEGATIVE_INFINITY; + for (int i = 0; i < n; i++) { + double[] a = optical[i]; + if (a[2] < NEAR_PLANE_INCHES) { + continue; + } + minX = Math.min(minX, a[0] / a[2]); + maxX = Math.max(maxX, a[0] / a[2]); + minY = Math.min(minY, a[1] / a[2]); + maxY = Math.max(maxY, a[1] / a[2]); + for (int j = 0; j < n; j++) { + double[] b = optical[j]; + if (b[2] >= NEAR_PLANE_INCHES) { + continue; + } + double s = (a[2] - NEAR_PLANE_INCHES) / (a[2] - b[2]); + double x = (a[0] + s * (b[0] - a[0])) / NEAR_PLANE_INCHES; + double y = (a[1] + s * (b[1] - a[1])) / NEAR_PLANE_INCHES; + minX = Math.min(minX, x); + maxX = Math.max(maxX, x); + minY = Math.min(minY, y); + maxY = Math.max(maxY, y); + } + } + // Wholly behind the plane: the infinities left here overlap no tile. + shape.minX = minX; + shape.maxX = maxX; + shape.minY = minY; + shape.maxY = maxY; + } + + /** A field point in the optical frame, inches: {@code +x} image right, {@code +y} down, {@code +z} ahead. */ + double[] toOptical(double[] p) { + double dx = p[0] - lensField[0]; + double dy = p[1] - lensField[1]; + double dz = p[2] - lensField[2]; + return new double[] { + dx * axisXField[0] + dy * axisXField[1] + dz * axisXField[2], + dx * axisYField[0] + dy * axisYField[1] + dz * axisYField[2], + dx * axisZField[0] + dy * axisZField[1] + dz * axisZField[2]}; + } + + Shape[] candidatesFor(double[] bounds, int tile) { + double minX = bounds[4 * tile]; + double maxX = bounds[4 * tile + 1]; + double minY = bounds[4 * tile + 2]; + double maxY = bounds[4 * tile + 3]; + List list = new ArrayList(); + for (Shape shape : shapes) { + if (shape.maxX >= minX && shape.minX <= maxX && shape.maxY >= minY && shape.minY <= maxY) { + list.add(shape); + } + } + return list.toArray(new Shape[0]); + } + } + + private void buildShapes(Scene scene, List pieces, CameraOccluders boxes, AprilTagLayout tags) { + for (ObstacleBox box : boxes.getFieldBoxes()) { + if (CameraOccluders.contains(box, scene.lensField[0], scene.lensField[1], scene.lensField[2])) { + continue; + } + Shape shape = new BoxShape(box, false, style.colourOfBox(box.getName(), false)); + scene.bound(shape, boxCorners(box, scene, false)); + scene.shapes.add(shape); + } + for (ObstacleBox box : boxes.getRobotBoxes()) { + if (CameraOccluders.contains(box, lensRobot[0], lensRobot[1], lensRobot[2])) { + continue; + } + Shape shape = new BoxShape(box, true, style.colourOfBox(box.getName(), true)); + scene.bound(shape, boxCorners(box, scene, true)); + scene.shapes.add(shape); + } + for (GamePiece piece : pieces) { + if (!piece.isVisibleOnField()) { + continue; + } + double r = piece.getRadiusInches(); + SphereShape shape = new SphereShape(piece.getId(), piece.getFieldXInches(), piece.getFieldYInches(), + piece.getFieldZInches(), r, style.colourOfPiece(piece.getKind())); + double[][] corners = new double[8][]; + for (int i = 0; i < 8; i++) { + corners[i] = new double[] {shape.cx + ((i & 1) == 0 ? -r : r), shape.cy + ((i & 2) == 0 ? -r : r), + shape.cz + ((i & 4) == 0 ? -r : r)}; + } + scene.bound(shape, corners); + scene.shapes.add(shape); + } + for (AprilTagLayout.Tag tag : tags.getTags()) { + TagShape shape = new TagShape(tag); + double h = shape.halfOuter; + double[][] corners = new double[4][]; + int k = 0; + for (double u : new double[] {-h, h}) { + for (double v : new double[] {-h, h}) { + corners[k++] = tag.getFieldToTag().apply(u, v, 0.0); + } + } + scene.bound(shape, corners); + scene.shapes.add(shape); + } + } + + private static double[][] boxCorners(ObstacleBox box, Scene scene, boolean robotFrame) { + double[][] corners = new double[8][]; + for (int i = 0; i < 8; i++) { + double[] c = { + (i & 1) == 0 ? box.getMinXInches() : box.getMaxXInches(), + (i & 2) == 0 ? box.getMinYInches() : box.getMaxYInches(), + (i & 4) == 0 ? box.getMinZInches() : box.getMaxZInches()}; + corners[i] = robotFrame ? scene.pointToField(c) : c; + } + return corners; + } + + // -------------------------------------------------------------------------------------- ray casting + + /** What the current ray hit: the shape (null for floor or background), where, and its labels. */ + private static final class Hit { + Shape shape; + boolean floor; + double t; + int pieceId; + int tagId; + } + + private void trace(Scene scene, Shape[] candidates, double xn, double yn, Hit hit) { + double[] ax = scene.axisXField; + double[] ay = scene.axisYField; + double[] az = scene.axisZField; + scene.fdx = xn * ax[0] + yn * ay[0] + az[0]; + scene.fdy = xn * ax[1] + yn * ay[1] + az[1]; + scene.fdz = xn * ax[2] + yn * ay[2] + az[2]; + scene.rdx = xn * axisXRobot[0] + yn * axisYRobot[0] + axisZRobot[0]; + scene.rdy = xn * axisXRobot[1] + yn * axisYRobot[1] + axisZRobot[1]; + scene.rdz = xn * axisXRobot[2] + yn * axisYRobot[2] + axisZRobot[2]; + + double best = Double.POSITIVE_INFINITY; + hit.shape = null; + hit.floor = false; + if (scene.fdz < -1e-12) { + best = -scene.lensField[2] / scene.fdz; + hit.floor = true; + } + for (Shape shape : candidates) { + double t = shape.intersect(scene, best); + if (t < best) { + best = t; + hit.shape = shape; + hit.floor = false; + } + } + hit.t = best; + hit.pieceId = hit.shape instanceof SphereShape ? ((SphereShape) hit.shape).pieceId : -1; + hit.tagId = hit.shape instanceof TagShape ? ((TagShape) hit.shape).tag.getId() : -1; + } + + /** Colour, 0 to 255 per channel before gain, of what the current ray hit. */ + private void shade(Scene scene, Hit hit, double[] out) { + int rgb; + double[] normal; + if (hit.shape != null) { + double[] p = {scene.lensField[0] + hit.t * scene.fdx, scene.lensField[1] + hit.t * scene.fdy, + scene.lensField[2] + hit.t * scene.fdz}; + rgb = hit.shape.colourAt(scene, p, hit.t); + normal = hit.shape.normalAt(scene, p, hit.t); + } else if (hit.floor) { + double x = scene.lensField[0] + hit.t * scene.fdx; + double y = scene.lensField[1] + hit.t * scene.fdy; + rgb = floorColour(x, y); + normal = new double[] {0.0, 0.0, 1.0}; + } else { + int bg = style.getBackgroundRgb(); + out[0] = (bg >> 16) & 0xFF; + out[1] = (bg >> 8) & 0xFF; + out[2] = bg & 0xFF; + return; + } + double[] light = style.getLightDirection(); + double lambert = Math.max(0.0, normal[0] * light[0] + normal[1] * light[1] + normal[2] * light[2]); + double k = style.getAmbient() + style.getDiffuse() * lambert; + out[0] = ((rgb >> 16) & 0xFF) * k; + out[1] = ((rgb >> 8) & 0xFF) * k; + out[2] = (rgb & 0xFF) * k; + } + + private int floorColour(double x, double y) { + double tile = style.getTileSizeInches(); + if (tile > 0.0 && style.getSeamWidthInches() > 0.0) { + double half = style.getSeamWidthInches() / 2.0; + double fx = x - tile * StrictMath.floor(x / tile); + double fy = y - tile * StrictMath.floor(y / tile); + if (fx < half || fx > tile - half || fy < half || fy > tile - half) { + return style.getSeamRgb(); + } + } + return style.getFloorRgb(); + } + + /** One thing a ray can hit, with its image bounds (normalised coordinates) for tile culling. */ + private abstract static class Shape { + double minX = Double.NEGATIVE_INFINITY; + double maxX = Double.POSITIVE_INFINITY; + double minY = Double.NEGATIVE_INFINITY; + double maxY = Double.POSITIVE_INFINITY; + + /** The ray parameter of the first hit below {@code limit}, or {@code +infinity}. */ + abstract double intersect(Scene scene, double limit); + + abstract int colourAt(Scene scene, double[] fieldPoint, double t); + + abstract double[] normalAt(Scene scene, double[] fieldPoint, double t); + } + + private static final class SphereShape extends Shape { + final int pieceId; + final double cx; + final double cy; + final double cz; + final double r; + final int rgb; + + SphereShape(int pieceId, double cx, double cy, double cz, double r, int rgb) { + this.pieceId = pieceId; + this.cx = cx; + this.cy = cy; + this.cz = cz; + this.r = r; + this.rgb = rgb; + } + + @Override + double intersect(Scene scene, double limit) { + double ox = scene.lensField[0] - cx; + double oy = scene.lensField[1] - cy; + double oz = scene.lensField[2] - cz; + double a = scene.fdx * scene.fdx + scene.fdy * scene.fdy + scene.fdz * scene.fdz; + double b = 2.0 * (ox * scene.fdx + oy * scene.fdy + oz * scene.fdz); + double c = ox * ox + oy * oy + oz * oz - r * r; + if (c <= 0.0) { + return Double.POSITIVE_INFINITY; // the lens is inside the piece: draw nothing + } + double disc = b * b - 4.0 * a * c; + if (disc < 0.0) { + return Double.POSITIVE_INFINITY; + } + double t = (-b - StrictMath.sqrt(disc)) / (2.0 * a); + return t > 0.0 && t < limit ? t : Double.POSITIVE_INFINITY; + } + + @Override + int colourAt(Scene scene, double[] p, double t) { + return rgb; + } + + @Override + double[] normalAt(Scene scene, double[] p, double t) { + return new double[] {(p[0] - cx) / r, (p[1] - cy) / r, (p[2] - cz) / r}; + } + } + + private final class BoxShape extends Shape { + final ObstacleBox box; + final boolean robotFrame; + final int rgb; + + BoxShape(ObstacleBox box, boolean robotFrame, int rgb) { + this.box = box; + this.robotFrame = robotFrame; + this.rgb = rgb; + } + + @Override + double intersect(Scene scene, double limit) { + // The slab test, written out on primitives: this is the renderer's innermost loop. + double[] o = robotFrame ? lensRobot : scene.lensField; + double enter = 1e-9; + double exit = limit; + double d = robotFrame ? scene.rdx : scene.fdx; + if (Math.abs(d) < 1e-12) { + if (o[0] <= box.getMinXInches() || o[0] >= box.getMaxXInches()) { + return Double.POSITIVE_INFINITY; + } + } else { + double t0 = (box.getMinXInches() - o[0]) / d; + double t1 = (box.getMaxXInches() - o[0]) / d; + enter = Math.max(enter, Math.min(t0, t1)); + exit = Math.min(exit, Math.max(t0, t1)); + } + d = robotFrame ? scene.rdy : scene.fdy; + if (Math.abs(d) < 1e-12) { + if (o[1] <= box.getMinYInches() || o[1] >= box.getMaxYInches()) { + return Double.POSITIVE_INFINITY; + } + } else { + double t0 = (box.getMinYInches() - o[1]) / d; + double t1 = (box.getMaxYInches() - o[1]) / d; + enter = Math.max(enter, Math.min(t0, t1)); + exit = Math.min(exit, Math.max(t0, t1)); + } + d = robotFrame ? scene.rdz : scene.fdz; + if (Math.abs(d) < 1e-12) { + if (o[2] <= box.getMinZInches() || o[2] >= box.getMaxZInches()) { + return Double.POSITIVE_INFINITY; + } + } else { + double t0 = (box.getMinZInches() - o[2]) / d; + double t1 = (box.getMaxZInches() - o[2]) / d; + enter = Math.max(enter, Math.min(t0, t1)); + exit = Math.min(exit, Math.max(t0, t1)); + } + return enter < exit && enter < limit ? enter : Double.POSITIVE_INFINITY; + } + + @Override + int colourAt(Scene scene, double[] p, double t) { + return rgb; + } + + @Override + double[] normalAt(Scene scene, double[] p, double t) { + double[] q = robotFrame ? new double[] {lensRobot[0] + t * scene.rdx, lensRobot[1] + t * scene.rdy, + lensRobot[2] + t * scene.rdz} : p; + double[] faces = { + Math.abs(q[0] - box.getMinXInches()), Math.abs(q[0] - box.getMaxXInches()), + Math.abs(q[1] - box.getMinYInches()), Math.abs(q[1] - box.getMaxYInches()), + Math.abs(q[2] - box.getMinZInches()), Math.abs(q[2] - box.getMaxZInches())}; + int nearest = 0; + for (int i = 1; i < 6; i++) { + if (faces[i] < faces[nearest]) { + nearest = i; + } + } + double[] normal = new double[3]; + normal[nearest / 2] = nearest % 2 == 0 ? -1.0 : 1.0; + return robotFrame ? scene.vectorToField(normal) : normal; + } + } + + private final class TagShape extends Shape { + final AprilTagLayout.Tag tag; + final double[] centre; + final double[] right; + final double[] down; + final double[] normal; + final double cell; + final double halfOuter; + + TagShape(AprilTagLayout.Tag tag) { + this.tag = tag; + RigidTransform3 t = tag.getFieldToTag(); + this.centre = new double[] {tag.getXInches(), tag.getYInches(), tag.getZInches()}; + this.right = new double[] {t.rotation(0, 0), t.rotation(1, 0), t.rotation(2, 0)}; + this.down = new double[] {t.rotation(0, 1), t.rotation(1, 1), t.rotation(2, 1)}; + this.normal = tag.getFaceNormal(); + // The reported size is the black square: 8 of the 10 cells across, the outer ring being white. + this.cell = tag.getSizeInches() / 8.0; + this.halfOuter = 5.0 * cell; + } + + @Override + double intersect(Scene scene, double limit) { + double denom = scene.fdx * normal[0] + scene.fdy * normal[1] + scene.fdz * normal[2]; + if (Math.abs(denom) < 1e-12) { + return Double.POSITIVE_INFINITY; + } + double t = ((centre[0] - scene.lensField[0]) * normal[0] + (centre[1] - scene.lensField[1]) * normal[1] + + (centre[2] - scene.lensField[2]) * normal[2]) / denom; + if (!(t > 0.0)) { + return Double.POSITIVE_INFINITY; + } + double px = scene.lensField[0] + t * scene.fdx - centre[0]; + double py = scene.lensField[1] + t * scene.fdy - centre[1]; + double pz = scene.lensField[2] + t * scene.fdz - centre[2]; + double u = px * right[0] + py * right[1] + pz * right[2]; + double v = px * down[0] + py * down[1] + pz * down[2]; + if (Math.abs(u) > halfOuter || Math.abs(v) > halfOuter) { + return Double.POSITIVE_INFINITY; + } + double length = StrictMath.sqrt(scene.fdx * scene.fdx + scene.fdy * scene.fdy + scene.fdz * scene.fdz); + double biased = Math.max(1e-9, t - TAG_DEPTH_BIAS_INCHES / length); + return biased < limit ? biased : Double.POSITIVE_INFINITY; + } + + @Override + int colourAt(Scene scene, double[] p, double t) { + double towardLens = scene.fdx * normal[0] + scene.fdy * normal[1] + scene.fdz * normal[2]; + if (towardLens > 0.0) { + return style.getTagBackRgb(); + } + // Recompute on the tag's plane: the depth bias moved p a hair off it. + double length = StrictMath.sqrt(scene.fdx * scene.fdx + scene.fdy * scene.fdy + scene.fdz * scene.fdz); + double tPlane = t + TAG_DEPTH_BIAS_INCHES / length; + double px = scene.lensField[0] + tPlane * scene.fdx - centre[0]; + double py = scene.lensField[1] + tPlane * scene.fdy - centre[1]; + double pz = scene.lensField[2] + tPlane * scene.fdz - centre[2]; + double u = px * right[0] + py * right[1] + pz * right[2]; + double v = px * down[0] + py * down[1] + pz * down[2]; + int i = Math.max(0, Math.min(9, (int) StrictMath.floor((u + halfOuter) / cell))); + int j = Math.max(0, Math.min(9, (int) StrictMath.floor((v + halfOuter) / cell))); + if (i == 0 || i == 9 || j == 0 || j == 9) { + return style.getTagWhiteRgb(); + } + if (i == 1 || i == 8 || j == 1 || j == 8) { + return style.getTagBlackRgb(); + } + int bit = mix(tag.getId() * 36 + (j - 2) * 6 + (i - 2)) & 1; + return bit == 1 ? style.getTagBlackRgb() : style.getTagWhiteRgb(); + } + + @Override + double[] normalAt(Scene scene, double[] p, double t) { + double towardLens = scene.fdx * normal[0] + scene.fdy * normal[1] + scene.fdz * normal[2]; + return towardLens > 0.0 ? new double[] {-normal[0], -normal[1], -normal[2]} : normal; + } + } + + // ------------------------------------------------------------------------------------------- sensor + + /** Entries in {@link #STANDARD_NORMAL}: a power of two, so a hash's top bits index it. */ + private static final int NORMAL_TABLE_BITS = 16; + + /** + * Standard-normal samples (mean 0, standard deviation 1), fixed at class load by Box-Muller over a hashed + * counter. Looking one up per channel costs a hash and an array read; computing Box-Muller per channel with + * {@link StrictMath} cost about 0.25 microseconds, which at 640 x 400 x 3 channels was most of a frame. + */ + private static final float[] STANDARD_NORMAL = new float[1 << NORMAL_TABLE_BITS]; + + static { + for (int i = 0; i < STANDARD_NORMAL.length; i++) { + double u1 = ((mix(i * 2 + 1) >>> 8) + 1) / 16777217.0; // (0, 1]: log is finite + double u2 = (mix(i * 2 + 2) >>> 8) / 16777216.0; + STANDARD_NORMAL[i] = (float) (StrictMath.sqrt(-2.0 * StrictMath.log(u1)) + * StrictMath.cos(2.0 * Math.PI * u2)); + } + } + + /** + * Zero-mean Gaussian sensor noise for one channel of one pixel of one frame, counts; 0 with noise off. + * + * @param frameKey the seed and frame number hashed together, once per frame + */ + private double noise(int frameKey, int pixel, int channel) { + if (noiseStdDevCounts == 0.0) { + return 0.0; + } + int key = mix(frameKey ^ mix(pixel * 3 + channel)); + return noiseStdDevCounts * STANDARD_NORMAL[key >>> (32 - NORMAL_TABLE_BITS)]; + } + + /** + * The 32-bit finaliser of MurmurHash3 (public domain): a fixed permutation that scatters every input + * bit across the output, used as a stateless random source. + */ + static int mix(int h) { + h ^= h >>> 16; + h *= 0x85EBCA6B; + h ^= h >>> 13; + h *= 0xC2B2AE35; + h ^= h >>> 16; + return h; + } + + private static byte toByte(double value) { + long rounded = Math.round(value); + return (byte) Math.max(0L, Math.min(255L, rounded)); + } + + private static double[] minus(double[] a, double[] b) { + return new double[] {a[0] - b[0], a[1] - b[1], a[2] - b[2]}; + } + + private static void requireFinite(double value, String name) { + if (!Double.isFinite(value)) { + throw new IllegalArgumentException(name + " must be finite, got " + value); + } + } + + /** Configures a {@link SimFrameRenderer}. */ + public static final class Builder { + + private final CameraLens lens; + private final CameraMount mount; + private FrameStyle style = FrameStyle.defaults(); + private int supersampling = 1; + private int noiseSeed; + private double noiseStdDevCounts; + private double gain = 1.0; + + private Builder(CameraLens lens, CameraMount mount) { + if (lens == null || mount == null) { + throw new IllegalArgumentException("a renderer needs a lens and a mount"); + } + this.lens = lens; + this.mount = mount; + } + + /** + * The colours and light. Default {@link FrameStyle#defaults()}. + * + * @param frameStyle the style + * @return this builder + */ + public Builder style(FrameStyle frameStyle) { + if (frameStyle == null) { + throw new IllegalArgumentException("style must not be null"); + } + this.style = frameStyle; + return this; + } + + /** + * Rays per pixel along each axis, averaged: 2 casts 4 rays a pixel and smooths edges. Default 1. + * The per-pixel labels always come from the ray through the pixel's centre. + * + * @param raysPerAxis 1 to 4 + * @return this builder + */ + public Builder supersampling(int raysPerAxis) { + if (raysPerAxis < 1 || raysPerAxis > 4) { + throw new IllegalArgumentException("supersampling must be 1 to 4, got " + raysPerAxis); + } + this.supersampling = raysPerAxis; + return this; + } + + /** + * Multiplies every pixel before noise and clamping: above 1 is a brighter exposure. Default 1. + * + * @param factor the gain, > 0 + * @return this builder + */ + public Builder gain(double factor) { + if (!(factor > 0.0) || Double.isInfinite(factor)) { + throw new IllegalArgumentException("gain must be positive and finite, got " + factor); + } + this.gain = factor; + return this; + } + + /** + * Independent Gaussian noise on every channel of every pixel, seeded. Off by default. The value is a + * PLACEHOLDER of your choosing, not a measured sensor's noise. + * + * @param seed the seed; same seed, same frames + * @param stdDevCounts standard deviation, in 8-bit counts, ≥ 0 + * @return this builder + */ + public Builder sensorNoise(int seed, double stdDevCounts) { + if (!(stdDevCounts >= 0.0) || Double.isInfinite(stdDevCounts)) { + throw new IllegalArgumentException("noise must be finite and >= 0, got " + stdDevCounts); + } + this.noiseSeed = seed; + this.noiseStdDevCounts = stdDevCounts; + return this; + } + + /** {@return the renderer; precomputes one ray per pixel (and per supersample), so build it once} */ + public SimFrameRenderer build() { + return new SimFrameRenderer(this); + } + } +} diff --git a/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimTagCamera.java b/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimTagCamera.java index ad1ec02..cdb44cf 100644 --- a/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimTagCamera.java +++ b/SimLoop/src/main/java/org/horizon36596/simloop/vision/SimTagCamera.java @@ -9,7 +9,7 @@ /** * A camera on the robot that "sees" AprilTags: given a team-supplied {@link AprilTagLayout} and the robot's * pose, it works out which tags a detector would find and where, as {@link TagSighting}s. No image is - * rendered. It is the tag counterpart of {@link SimCamera}, and uses the same {@link CameraLens} and + * rendered here ({@link SimFrameRenderer} draws tags as synthetic pixels, not decodable). It is the tag counterpart of {@link SimCamera}, and uses the same {@link CameraLens} and * {@link CameraMount}, so one camera description serves both. * *

    Visibility rules, in the order they are checked (the first one that fails is what @@ -19,19 +19,24 @@ * {@code [minRangeInches, maxRangeInches]}. *

  • {@link Visibility#FACING_AWAY}: the angle between the tag's face normal and the line from the tag * to the lens is {@code maxIncidenceDegrees} or more. With the default of 90 this rejects exactly - * the tags seen from behind or edge-on.
    • + * the tags seen from behind or edge-on. A tilted tag's normal is tilted with it. *
  • {@link Visibility#BEHIND_CAMERA}: any corner of the tag is not in front of the lens.
    • - *
  • {@link Visibility#OUTSIDE_IMAGE}: any corner projects outside the image rectangle. A detector + *
  • {@link Visibility#OUTSIDE_IMAGE}: any corner projects outside the image rectangle, through the + * lens's distortion when it has one, or lies where the distortion model no longer holds. A detector * needs the whole tag border, so a tag that is only partly in frame is not seen at all.
    • *
  • {@link Visibility#OCCLUDED}: the straight line from the lens to any corner passes through the - * inside of one of the {@link ObstacleBox}es given at construction. A tag mounted on a box's face is - * not hidden by that box. Nothing else occludes: other tags, the robot's own body and anything not - * given as a box are transparent.
    • + * inside of one of the {@link ObstacleBox}es given at construction, or of a box in the + * {@link CameraOccluders} given to {@link #observe(AprilTagLayout, double, double, double, + * CameraOccluders)}: field boxes, and robot boxes such as the robot's own chassis. A tag mounted on a + * box's face is not hidden by that box. Nothing else occludes: other tags and anything not given as a + * box are transparent. * * *

    Frames. The layout and the robot pose are in the field frame (FTC-Cartesian, centre origin, * inches, heading CCW from {@code +X}). The mount is in the robot frame, the projection in the optical - * frame, both as {@link CameraMount} defines them. The lens is an ideal pinhole: no distortion. + * frame, both as {@link CameraMount} defines them. Pixels (the centre and the corners) go through the + * lens's distortion model when it has one ({@link CameraLens#project}); the angles {@code tx} and + * {@code ty} and the transforms are the undistorted geometry a calibrated detector recovers. * *

    Deterministic and clock-free (domain R5): the same layout and pose give the same sightings, in * ascending tag id order, with all trigonometry in {@link StrictMath}. @@ -132,13 +137,33 @@ private static double[] minus(double[] a, double[] b) { */ public List observe(AprilTagLayout layout, double robotXInches, double robotYInches, double robotHeadingRad) { + return observe(layout, robotXInches, robotYInches, robotHeadingRad, CameraOccluders.none()); + } + + /** + * Every tag in the layout the camera sees from this robot pose, with more boxes in the way than the + * ones given at construction: boxes that move from tick to tick, and the robot's own body. + * + * @param layout the tags on the field + * @param robotXInches robot centre, field X, inches + * @param robotYInches robot centre, field Y, inches + * @param robotHeadingRad robot heading, radians CCW from field {@code +X} + * @param extra more occluders, checked after the constructor's boxes; field boxes in the field + * frame, robot boxes in the robot frame ({@link CameraOccluders}) + * @return the sightings, in ascending tag id order, unmodifiable; empty when nothing is seen + */ + public List observe(AprilTagLayout layout, double robotXInches, double robotYInches, + double robotHeadingRad, CameraOccluders extra) { if (layout == null) { throw new IllegalArgumentException("layout must not be null; use AprilTagLayout.empty()"); } + if (extra == null) { + throw new IllegalArgumentException("extra occluders must not be null; use CameraOccluders.none()"); + } RigidTransform3 fieldToRobot = fieldToRobot(robotXInches, robotYInches, robotHeadingRad); List seen = new ArrayList(); for (AprilTagLayout.Tag tag : layout.getTags()) { - TagSighting sighting = sight(tag, fieldToRobot, null); + TagSighting sighting = sight(tag, fieldToRobot, extra, null); if (sighting != null) { seen.add(sighting); } @@ -157,11 +182,31 @@ public List observe(AprilTagLayout layout, double robotXInches, dou */ public Visibility visibility(AprilTagLayout.Tag tag, double robotXInches, double robotYInches, double robotHeadingRad) { + return visibility(tag, robotXInches, robotYInches, robotHeadingRad, CameraOccluders.none()); + } + + /** + * Whether one tag is seen from this robot pose with more occluders in the way, and if not, the first + * rule it fails. + * + * @param tag the tag + * @param robotXInches robot centre, field X, inches + * @param robotYInches robot centre, field Y, inches + * @param robotHeadingRad robot heading, radians CCW from field {@code +X} + * @param extra more occluders, as for {@link #observe(AprilTagLayout, double, double, double, + * CameraOccluders)} + * @return {@link Visibility#VISIBLE}, or the reason it is not + */ + public Visibility visibility(AprilTagLayout.Tag tag, double robotXInches, double robotYInches, + double robotHeadingRad, CameraOccluders extra) { if (tag == null) { throw new IllegalArgumentException("tag must not be null"); } + if (extra == null) { + throw new IllegalArgumentException("extra occluders must not be null; use CameraOccluders.none()"); + } Visibility[] reason = new Visibility[1]; - sight(tag, fieldToRobot(robotXInches, robotYInches, robotHeadingRad), reason); + sight(tag, fieldToRobot(robotXInches, robotYInches, robotHeadingRad), extra, reason); return reason[0]; } @@ -173,7 +218,8 @@ private static RigidTransform3 fieldToRobot(double x, double y, double heading) } /** One tag through every rule; null when unseen, with the reason written to {@code reason} if given. */ - private TagSighting sight(AprilTagLayout.Tag tag, RigidTransform3 fieldToRobot, Visibility[] reason) { + private TagSighting sight(AprilTagLayout.Tag tag, RigidTransform3 fieldToRobot, CameraOccluders extra, + Visibility[] reason) { RigidTransform3 fieldToCamera = fieldToRobot.compose(robotToCamera); RigidTransform3 fieldToTag = tag.getFieldToTag(); RigidTransform3 cameraToTag = fieldToCamera.inverse().compose(fieldToTag); @@ -184,13 +230,17 @@ private TagSighting sight(AprilTagLayout.Tag tag, RigidTransform3 fieldToRobot, return reject(reason, Visibility.OUT_OF_RANGE); } - // The face normal is horizontal, the tag frame's -Z (target space +Z points into the tag), so only - // the horizontal part of the tag-to-lens line contributes to the dot product. + // The face normal is the tag frame's -Z (target space +Z points into the tag). For a plumb tag it is + // horizontal and its z component is exactly 0. double toLensX = lensInField[0] - tag.getXInches(); double toLensY = lensInField[1] - tag.getYInches(); - double normalX = StrictMath.cos(tag.getFacingRad()); - double normalY = StrictMath.sin(tag.getFacingRad()); - double cosIncidence = range == 0.0 ? 0.0 : (toLensX * normalX + toLensY * normalY) / range; + double toLensZ = lensInField[2] - tag.getZInches(); + double[] normal = tag.getFaceNormal(); + double dot = toLensX * normal[0] + toLensY * normal[1]; + if (normal[2] != 0.0) { + dot += toLensZ * normal[2]; + } + double cosIncidence = range == 0.0 ? 0.0 : dot / range; if (cosIncidence <= cosMaxIncidence) { return reject(reason, Visibility.FACING_AWAY); } @@ -204,11 +254,11 @@ private TagSighting sight(AprilTagLayout.Tag tag, RigidTransform3 fieldToRobot, if (optical[2] <= 1e-9) { return reject(reason, Visibility.BEHIND_CAMERA); } - cornersPx[i] = project(optical); + cornersPx[i] = lens.project(optical[0], optical[1], optical[2]); cornersField[i] = fieldToTag.apply(cornersInTag[i][0], cornersInTag[i][1], 0.0); } for (double[] px : cornersPx) { - if (px[0] < 0.0 || px[0] > lens.getWidthPx() || px[1] < 0.0 || px[1] > lens.getHeightPx()) { + if (px == null || px[0] < 0.0 || px[0] > lens.getWidthPx() || px[1] < 0.0 || px[1] > lens.getHeightPx()) { return reject(reason, Visibility.OUTSIDE_IMAGE); } } @@ -218,12 +268,35 @@ private TagSighting sight(AprilTagLayout.Tag tag, RigidTransform3 fieldToRobot, return reject(reason, Visibility.OCCLUDED); } } + for (ObstacleBox box : extra.getFieldBoxes()) { + if (segmentEntersBox(lensInField, corner, box)) { + return reject(reason, Visibility.OCCLUDED); + } + } + } + if (!extra.getRobotBoxes().isEmpty()) { + RigidTransform3 robotFromField = fieldToRobot.inverse(); + double[] lensInRobot = robotToCamera.apply(0.0, 0.0, 0.0); + for (double[] corner : cornersField) { + double[] cornerInRobot = robotFromField.apply(corner[0], corner[1], corner[2]); + for (ObstacleBox box : extra.getRobotBoxes()) { + if (CameraOccluders.contains(box, lensInRobot[0], lensInRobot[1], lensInRobot[2])) { + continue; // the camera's own housing + } + if (segmentEntersBox(lensInRobot, cornerInRobot, box)) { + return reject(reason, Visibility.OCCLUDED); + } + } + } } double x = cameraToTag.getXInches(); double y = cameraToTag.getYInches(); double z = cameraToTag.getZInches(); - double[] centrePx = project(new double[] {x, y, z}); + double[] centrePx = lens.project(x, y, z); + if (centrePx == null) { + return reject(reason, Visibility.OUTSIDE_IMAGE); + } double tx = StrictMath.toDegrees(StrictMath.atan2(x, z)); double ty = StrictMath.toDegrees(StrictMath.atan2(-y, z)); double area = 100.0 * polygonArea(cornersPx) / ((double) lens.getWidthPx() * lens.getHeightPx()); @@ -241,13 +314,6 @@ private static TagSighting reject(Visibility[] reason, Visibility why) { return null; } - private double[] project(double[] optical) { - return new double[] { - lens.getCentreXPx() + lens.getFocalXPx() * optical[0] / optical[2], - lens.getCentreYPx() + lens.getFocalYPx() * optical[1] / optical[2], - }; - } - /** Shoelace area of a simple polygon, square pixels. */ private static double polygonArea(double[][] points) { double twice = 0.0; diff --git a/SimLoop/src/test/java/org/horizon36596/simloop/fakehardware/FakeLimelight3AFidelityTest.java b/SimLoop/src/test/java/org/horizon36596/simloop/fakehardware/FakeLimelight3AFidelityTest.java new file mode 100644 index 0000000..2a3d7b9 --- /dev/null +++ b/SimLoop/src/test/java/org/horizon36596/simloop/fakehardware/FakeLimelight3AFidelityTest.java @@ -0,0 +1,151 @@ +package org.horizon36596.simloop.fakehardware; + +import static org.junit.jupiter.api.Assertions.assertArrayEquals; +import static org.junit.jupiter.api.Assertions.assertEquals; +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.Arrays; +import java.util.List; + +import org.horizon36596.simloop.field.ObstacleBox; +import org.horizon36596.simloop.vision.AprilTagLayout; +import org.horizon36596.simloop.vision.CameraMount; +import org.junit.jupiter.api.Test; + +/** + * INVARIANT-tier (domain R9) — the fake Limelight's fidelity options, read through the SDK's + * {@link LLResult}: + * + *

      + *
    1. dropouts miss tags at about the asked-for rate, the same tags for the same seed, and none when + * off;
      2. + *
    2. {@code getStddevMt1/Mt2} report the noise model's spread, and zeros with noise off;
      3. + *
    3. the robot's own body hides a tag;
      4. + *
    4. distortion moves the corner and centre pixels and leaves {@code tx}, {@code ty} and the poses + * alone;
      5. + *
    5. a tilted tag is seen from where a plumb one is not.
      6. + *
    + * + *

    Same made-up rig as {@link FakeLimelight3ATest}: 54.5 x 42 degrees, lens 10 in up looking forward. + */ +class FakeLimelight3AFidelityTest { + + 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 FakeLimelight3A.Builder builder(AprilTagLayout layout) { + return FakeLimelight3A.builder(layout, LEVEL, 54.5, 42.0); + } + + private static List tagCounts(FakeLimelight3A ll, int frames) { + ll.start(); + List counts = new ArrayList(); + for (int i = 0; i < frames; i++) { + ll.update(TICK); + counts.add(ll.getLatestResult().getFiducialResults().size()); + } + return counts; + } + + @Test + void dropoutsMissTagsAtAboutTheRateAndReplayForTheSameSeed() { + List a = tagCounts(builder(WALL).dropout(11, 0.3).build(), 400); + List b = tagCounts(builder(WALL).dropout(11, 0.3).build(), 400); + List c = tagCounts(builder(WALL).dropout(12, 0.3).build(), 400); + List off = tagCounts(builder(WALL).build(), 400); + assertEquals(a, b); + assertTrue(!a.equals(c), "another seed misses other tags"); + int seen = 0; + for (int n : a) { + seen += n; + } + double missed = 1.0 - seen / (3.0 * 400); + assertEquals(0.3, missed, 0.05, "missed share " + missed); + for (int n : off) { + assertEquals(3, n); + } + assertThrows(IllegalArgumentException.class, () -> builder(WALL).dropout(1, 1.0)); + assertThrows(IllegalArgumentException.class, () -> builder(WALL).dropout(1, -0.1)); + } + + @Test + void stddevsReportTheNoiseModelAndZerosWithNoiseOff() { + FakeLimelight3A noisy = builder(WALL).noise(5, 0.2, 1.5, 2.0).build(); + noisy.start(); + noisy.update(TICK); + LLResult r = noisy.getLatestResult(); + assertArrayEquals(new double[] {1.5 * M, 1.5 * M, 0.0, 0.0, 0.0, 2.0}, r.getStddevMt1(), 1e-12); + assertArrayEquals(new double[] {1.5 * M, 1.5 * M, 0.0, 0.0, 0.0, 0.0}, r.getStddevMt2(), 1e-12); + + FakeLimelight3A clean = builder(WALL).build(); + clean.start(); + clean.update(TICK); + assertArrayEquals(new double[6], clean.getLatestResult().getStddevMt1(), 0.0); + assertArrayEquals(new double[6], clean.getLatestResult().getStddevMt2(), 0.0); + } + + @Test + void theRobotsOwnBodyHidesATag() { + // A post 4-5 in ahead of the robot centre, 2 in wide, on the centreline: hides the middle tag only. + ObstacleBox post = new ObstacleBox("post", 4.0, 5.0, -0.5, 0.5, 0.0, 20.0); + FakeLimelight3A ll = builder(WALL).robotBody(Arrays.asList(post)).build(); + ll.start(); + ll.update(TICK); + List ids = new ArrayList(); + for (LLResultTypes.FiducialResult f : ll.getLatestResult().getFiducialResults()) { + ids.add(f.getFiducialId()); + } + assertEquals(Arrays.asList(1, 3), ids); + } + + @Test + void distortionMovesPixelsAndLeavesAnglesAndPosesAlone() { + // Well off the axis, where barrel distortion moves a pixel by several pixels. + AprilTagLayout offAxis = AprilTagLayout.builder().addTag(1, 40.0, -16.0, 10.0, Math.PI, 6.0).build(); + FakeLimelight3A pinhole = builder(offAxis).build(); + FakeLimelight3A barrel = builder(offAxis).distortion(-0.3, 0.08, 0.0, 0.0, 0.0).build(); + pinhole.start(); + barrel.start(); + pinhole.update(TICK); + barrel.update(TICK); + LLResultTypes.FiducialResult a = pinhole.getLatestResult().getFiducialResults().get(0); + LLResultTypes.FiducialResult b = barrel.getLatestResult().getFiducialResults().get(0); + assertEquals(a.getTargetXDegrees(), b.getTargetXDegrees(), 0.0); + assertEquals(a.getTargetYDegrees(), b.getTargetYDegrees(), 0.0); + assertEquals(a.getTargetPoseCameraSpace().getPosition().x, b.getTargetPoseCameraSpace().getPosition().x, 0.0); + assertTrue(Math.abs(a.getTargetXPixels() - b.getTargetXPixels()) > 1.0, "the centre pixel moves"); + double cx = 320.0; + double pinholeCorner = Math.abs(a.getTargetCorners().get(0).get(0) - cx); + double barrelCorner = Math.abs(b.getTargetCorners().get(0).get(0) - cx); + assertTrue(barrelCorner < pinholeCorner - 1.0, "barrel pulls the corner in: " + barrelCorner + " vs " + + pinholeCorner); + } + + @Test + void aTiltedTagIsSeenFromWhereAPlumbOneIsNot() { + // A tag on the floor ahead, face up; the level camera on a tall mast looking down sees it. + CameraMount mast = new CameraMount(0.0, 0.0, 30.0, 0.0, Math.toRadians(45.0)); + AprilTagLayout flat = AprilTagLayout.builder() + .addTiltedTag(8, 30.0, 0.0, 0.0, Math.PI, Math.PI / 2.0, 6.0).build(); + FakeLimelight3A ll = FakeLimelight3A.builder(flat, mast, 54.5, 42.0).build(); + ll.start(); + ll.update(TICK); + assertEquals(1, ll.getLatestResult().getFiducialResults().size()); + AprilTagLayout plumbFacingAway = AprilTagLayout.builder().addTag(8, 30.0, 0.0, 0.0, 0.0, 6.0).build(); + FakeLimelight3A away = FakeLimelight3A.builder(plumbFacingAway, mast, 54.5, 42.0).build(); + away.start(); + away.update(TICK); + assertEquals(0, away.getLatestResult().getFiducialResults().size()); + } +} diff --git a/SimLoop/src/test/java/org/horizon36596/simloop/vision/CameraLensDistortionTest.java b/SimLoop/src/test/java/org/horizon36596/simloop/vision/CameraLensDistortionTest.java new file mode 100644 index 0000000..debaade --- /dev/null +++ b/SimLoop/src/test/java/org/horizon36596/simloop/vision/CameraLensDistortionTest.java @@ -0,0 +1,127 @@ +package org.horizon36596.simloop.vision; + +import static org.junit.jupiter.api.Assertions.assertArrayEquals; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertNotNull; +import static org.junit.jupiter.api.Assertions.assertNull; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import org.junit.jupiter.api.Test; + +/** + * INVARIANT-tier (domain R9) — the lens model's distortion, which every camera in this package projects + * through: + * + *

      + *
    1. with no coefficients it is the old pinhole, bit for bit;
      2. + *
    2. the forward model is OpenCV's Brown-Conrady formula, checked against values worked by hand;
      3. + *
    3. {@code undistortPixel} inverts {@code project} everywhere in the image;
      4. + *
    4. past the fold-over radius a point is not imaged, so a far-off point cannot land inside the + * picture;
      5. + *
    5. the local magnification is 1 for a pinhole and shrinks toward the corners under barrel + * distortion.
      6. + *
    + * + *

    The coefficients are made up, in the range a cheap wide webcam's calibration gives. + */ +class CameraLensDistortionTest { + + private static final CameraLens PINHOLE = CameraLens.fromHorizontalFov(70.0, 640, 480); + private static final CameraLens BARREL = PINHOLE.withDistortion(-0.30, 0.08, 0.001, -0.0008, -0.01); + + @Test + void noCoefficientsIsThePinholeBitForBit() { + double[][] points = {{1.0, 2.0, 30.0}, {-7.5, 3.25, 11.0}, {0.0, 0.0, 5.0}, {40.0, -30.0, 12.0}}; + for (double[] p : points) { + double[] px = PINHOLE.project(p[0], p[1], p[2]); + assertEquals(PINHOLE.getCentreXPx() + PINHOLE.getFocalXPx() * p[0] / p[2], px[0], 0.0); + assertEquals(PINHOLE.getCentreYPx() + PINHOLE.getFocalYPx() * p[1] / p[2], px[1], 0.0); + } + assertFalse(PINHOLE.hasDistortion()); + assertEquals(Double.POSITIVE_INFINITY, PINHOLE.getMaxValidRadius()); + assertArrayEquals(new double[] {0.1, -0.2}, PINHOLE.undistortPixel( + PINHOLE.getCentreXPx() + 0.1 * PINHOLE.getFocalXPx(), + PINHOLE.getCentreYPx() - 0.2 * PINHOLE.getFocalYPx()), 1e-15); + } + + @Test + void radialTermsMatchTheFormulaWorkedByHand() { + CameraLens lens = PINHOLE.withDistortion(-0.2, 0.0, 0.0, 0.0, 0.0); + // r2 = 0.09 + 0.01 = 0.1; radial = 1 - 0.02 = 0.98. + assertArrayEquals(new double[] {0.294, 0.098}, lens.distortNormalized(0.3, 0.1), 1e-15); + double[] px = lens.project(0.3, 0.1, 1.0); + assertEquals(lens.getCentreXPx() + lens.getFocalXPx() * 0.294, px[0], 1e-9); + assertEquals(lens.getCentreYPx() + lens.getFocalYPx() * 0.098, px[1], 1e-9); + } + + @Test + void tangentialTermsMatchTheFormulaWorkedByHand() { + CameraLens lens = PINHOLE.withDistortion(0.0, 0.0, 0.01, -0.005, 0.0); + // x = 0.2, y = -0.1, r2 = 0.05. + // xd = 0.2 + 2(0.01)(0.2)(-0.1) + (-0.005)(0.05 + 0.08) = 0.2 - 0.0004 - 0.00065 = 0.19895 + // yd = -0.1 + 0.01(0.05 + 0.02) + 2(-0.005)(0.2)(-0.1) = -0.1 + 0.0007 + 0.0002 = -0.0991 + assertArrayEquals(new double[] {0.19895, -0.0991}, lens.distortNormalized(0.2, -0.1), 1e-15); + } + + @Test + void undistortInvertsProjectEverywhereInTheImage() { + double worst = 0.0; + for (int y = 0; y <= 480; y += 16) { + for (int x = 0; x <= 640; x += 16) { + double[] ray = BARREL.undistortPixel(x, y); + double[] back = BARREL.project(ray[0], ray[1], 1.0); + assertNotNull(back, "pixel (" + x + ", " + y + ") came back outside the valid radius"); + worst = Math.max(worst, Math.hypot(back[0] - x, back[1] - y)); + } + } + assertTrue(worst < 1e-9, "worst round trip " + worst + " px"); + } + + @Test + void barrelDistortionPullsTheCornersIn() { + double[] corner = PINHOLE.undistortPixel(0.0, 0.0); + double[] distorted = BARREL.project(corner[0], corner[1], 1.0); + assertTrue(distorted[0] > 0.0 && distorted[1] > 0.0, "barrel pulls a corner point toward the centre"); + double[] centre = BARREL.project(0.0, 0.0, 1.0); + assertEquals(BARREL.getCentreXPx(), centre[0], 1e-12); + assertEquals(BARREL.getCentreYPx(), centre[1], 1e-12); + } + + @Test + void pastTheFoldOverRadiusNothingIsImaged() { + CameraLens lens = PINHOLE.withDistortion(-0.3, 0.0, 0.0, 0.0, 0.0); + // d/dr of r(1 - 0.3 r^2) is 1 - 0.9 r^2: zero at r = 1 / sqrt(0.9). + assertEquals(1.0 / Math.sqrt(0.9), lens.getMaxValidRadius(), 1e-9); + assertNotNull(lens.project(1.0, 0.0, 1.0)); + assertNull(lens.project(1.5, 0.0, 1.0), "r = 1.5 would fold back to 1.5(1 - 0.675) = 0.49: inside the image"); + assertNull(lens.project(0.0, 0.0, -1.0), "behind the lens"); + assertNull(lens.project(0.0, 0.0, 0.0), "on the lens plane"); + } + + @Test + void localMagnificationIsOneForAPinholeAndShrinksTowardTheCorners() { + assertEquals(1.0, PINHOLE.localMagnification(0.4, 0.3), 0.0); + assertEquals(1.0, BARREL.localMagnification(0.0, 0.0), 1e-12); + assertTrue(BARREL.localMagnification(0.6, 0.4) < BARREL.localMagnification(0.2, 0.1)); + assertTrue(BARREL.localMagnification(0.6, 0.4) < 1.0); + } + + @Test + void withDistortionKeepsTheIntrinsics() { + assertEquals(PINHOLE.getFocalXPx(), BARREL.getFocalXPx(), 0.0); + assertEquals(PINHOLE.getCentreYPx(), BARREL.getCentreYPx(), 0.0); + assertEquals(PINHOLE.getWidthPx(), BARREL.getWidthPx()); + assertArrayEquals(new double[] {-0.30, 0.08, 0.001, -0.0008, -0.01}, BARREL.getDistortionCoefficients(), 0.0); + assertTrue(BARREL.hasDistortion()); + assertTrue(BARREL.toString().contains("k1=-0.3")); + } + + @Test + void nonFiniteCoefficientsAreRejected() { + assertThrows(IllegalArgumentException.class, () -> PINHOLE.withDistortion(Double.NaN, 0, 0, 0, 0)); + assertThrows(IllegalArgumentException.class, + () -> PINHOLE.withDistortion(0, 0, 0, 0, Double.POSITIVE_INFINITY)); + } +} diff --git a/SimLoop/src/test/java/org/horizon36596/simloop/vision/SimCameraOcclusionTest.java b/SimLoop/src/test/java/org/horizon36596/simloop/vision/SimCameraOcclusionTest.java new file mode 100644 index 0000000..85b6ff9 --- /dev/null +++ b/SimLoop/src/test/java/org/horizon36596/simloop/vision/SimCameraOcclusionTest.java @@ -0,0 +1,181 @@ +package org.horizon36596.simloop.vision; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.Arrays; +import java.util.Collections; +import java.util.List; + +import org.horizon36596.simloop.field.FieldWorld; +import org.horizon36596.simloop.field.GamePieceTracker; +import org.horizon36596.simloop.field.ObstacleBox; +import org.junit.jupiter.api.Test; + +/** + * INVARIANT-tier (domain R9) — what {@link SimCamera} hides when solid things are in the way, and how + * distortion moves its sightings: + * + *

      + *
    1. with no occluders the occluded {@code observe} reports what the plain one does, fully visible;
      2. + *
    2. a field box across the line of sight hides a piece, half a box half-hides it, and the + * minimum visible fraction decides whether a half-hidden piece is reported;
      3. + *
    3. a robot box turns with the robot and hides what is behind it;
      4. + *
    4. a box holding the piece, or the lens, does not hide it;
      5. + *
    5. a nearer piece hides a farther one by how much of it it covers;
      6. + *
    6. with a distorted lens, a sighting's pixel goes back to the floor point it came from.
      7. + *
    + * + *

    Rig as in {@link SimCameraTest}: 70 degrees at 640 x 400, 6 in forward, 10 in up, 22 degrees down. The + * boxes are invented for the test. + */ +class SimCameraOcclusionTest { + + 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 double RADIUS = 1.4; + private static final List NO_BOXES = Collections.emptyList(); + + private static SimCamera camera() { + return new SimCamera("front", LENS, FRONT); + } + + private static GamePieceTracker piecesAt(double[]... fieldXY) { + GamePieceTracker tracker = new GamePieceTracker(4, FieldWorld.unbounded()); + tracker.updateRobotPose(0.0, 0.0, 0.0); + for (double[] xy : fieldXY) { + tracker.addLoosePiece("piece", xy[0], xy[1], RADIUS); + } + return tracker; + } + + @Test + void noOccludersMatchesThePlainObserveAndIsFullyVisible() { + GamePieceTracker tracker = piecesAt(new double[] {30.0, 0.0}, new double[] {26.0, 8.0}); + List plain = camera().observe(tracker.getPieces(), 0.0, 0.0, 0.0); + List occluded = camera().observe(tracker.getPieces(), 0.0, 0.0, 0.0, CameraOccluders.none()); + assertEquals(plain.size(), occluded.size()); + for (int i = 0; i < plain.size(); i++) { + assertEquals(plain.get(i).getPieceId(), occluded.get(i).getPieceId()); + assertEquals(plain.get(i).getCentreXPx(), occluded.get(i).getCentreXPx(), 0.0); + assertEquals(plain.get(i).getCentreYPx(), occluded.get(i).getCentreYPx(), 0.0); + assertEquals(plain.get(i).getRadiusPx(), occluded.get(i).getRadiusPx(), 0.0); + assertEquals(1.0, occluded.get(i).getVisibleFraction(), 0.0); + assertEquals(1.0, plain.get(i).getVisibleFraction(), 0.0); + } + } + + @Test + void aFieldBoxAcrossTheLineOfSightHidesThePiece() { + GamePieceTracker tracker = piecesAt(new double[] {30.0, 0.0}); + ObstacleBox wall = new ObstacleBox("wall", 20.0, 21.0, -6.0, 6.0, 0.0, 12.0); + assertEquals(1, camera().observe(tracker.getPieces(), 0.0, 0.0, 0.0).size(), + "the plain observe does not know about boxes"); + assertTrue(camera().observe(tracker.getPieces(), 0.0, 0.0, 0.0, + CameraOccluders.field(Arrays.asList(wall))).isEmpty()); + } + + @Test + void halfABoxHalfHidesThePieceAndTheThresholdDecides() { + GamePieceTracker tracker = piecesAt(new double[] {30.0, 0.0}); + // Covers field +Y (the robot's left, the image's left) from the piece's centre line outward. + ObstacleBox half = new ObstacleBox("half", 20.0, 21.0, 0.0, 6.0, 0.0, 12.0); + CameraOccluders boxes = CameraOccluders.field(Arrays.asList(half)); + List lenient = camera().withMinVisibleFraction(0.25) + .observe(tracker.getPieces(), 0.0, 0.0, 0.0, boxes); + assertEquals(1, lenient.size()); + double fraction = lenient.get(0).getVisibleFraction(); + assertTrue(fraction > 0.3 && fraction < 0.7, "about half visible, got " + fraction); + assertTrue(camera().withMinVisibleFraction(0.9).observe(tracker.getPieces(), 0.0, 0.0, 0.0, boxes).isEmpty()); + } + + @Test + void aRobotBoxTurnsWithTheRobot() { + // A bumper 9-10 in ahead of the robot centre, 12 in tall: in front of the 6 in-forward lens. + ObstacleBox bumper = new ObstacleBox("bumper", 9.0, 10.0, -8.0, 8.0, 0.0, 12.0); + CameraOccluders body = CameraOccluders.of(NO_BOXES, Arrays.asList(bumper)); + // Robot facing +Y: its forward is field +Y, so the piece 30 in "ahead" is at field (0, 30). + GamePieceTracker tracker = piecesAt(new double[] {0.0, 30.0}); + double heading = Math.PI / 2.0; + assertEquals(1, camera().observe(tracker.getPieces(), 0.0, 0.0, heading, CameraOccluders.none()).size()); + assertTrue(camera().observe(tracker.getPieces(), 0.0, 0.0, heading, body).isEmpty()); + // The same box read in the FIELD frame sits at field x 9-10, nowhere near the line of sight. + assertEquals(1, camera().observe(tracker.getPieces(), 0.0, 0.0, heading, + CameraOccluders.field(Arrays.asList(bumper))).size()); + } + + @Test + void aBoxHoldingThePieceOrTheLensDoesNotHideIt() { + GamePieceTracker tracker = piecesAt(new double[] {30.0, 0.0}); + ObstacleBox tray = new ObstacleBox("tray", 27.0, 33.0, -3.0, 3.0, 0.0, 4.0); + ObstacleBox housing = new ObstacleBox("housing", 5.0, 7.0, -1.0, 1.0, 9.0, 11.0); + assertEquals(1, camera().observe(tracker.getPieces(), 0.0, 0.0, 0.0, + CameraOccluders.of(Arrays.asList(tray), Arrays.asList(housing))).size()); + } + + @Test + void aNearerPieceHidesAFartherOneByHowMuchItCovers() { + // A low, level camera (2 in up), so a nearer floor piece can stand in front of a farther one. + SimCamera low = new SimCamera("low", LENS, CameraMount.front(6.0, 2.0, 0.0)); + // Straight behind: the near piece covers the far one's whole disc as the camera sees it. + GamePieceTracker behind = piecesAt(new double[] {24.0, 0.0}, new double[] {36.0, 0.0}); + List seen = low.observe(behind.getPieces(), 0.0, 0.0, 0.0, CameraOccluders.none()); + assertEquals(1, seen.size()); + assertEquals(24.0, nearestPieceX(behind, seen.get(0).getPieceId()), 1e-9); + // Offset sideways by a radius and a bit: partly covered, reported with less than full visibility. + GamePieceTracker offset = piecesAt(new double[] {24.0, 0.0}, new double[] {36.0, 2.2}); + List both = low.withMinVisibleFraction(0.05) + .observe(offset.getPieces(), 0.0, 0.0, 0.0, CameraOccluders.none()); + assertEquals(2, both.size()); + double far = Math.min(both.get(0).getVisibleFraction(), both.get(1).getVisibleFraction()); + assertTrue(far > 0.0 && far < 1.0, "the far piece is partly hidden, got " + far); + } + + private static double nearestPieceX(GamePieceTracker tracker, int id) { + for (org.horizon36596.simloop.field.GamePiece piece : tracker.getPieces()) { + if (piece.getId() == id) { + return piece.getFieldXInches(); + } + } + throw new AssertionError("no piece " + id); + } + + @Test + void withADistortedLensASightingGoesBackToItsFloorPoint() { + CameraLens barrel = LENS.withDistortion(-0.28, 0.07, 0.0005, -0.0004, -0.008); + SimCamera distorted = new SimCamera("front", barrel, FRONT); + GamePieceTracker tracker = piecesAt(new double[] {28.0, 14.0}, new double[] {40.0, -16.0}, + new double[] {22.0, 0.0}); + List pinholeSeen = camera().observe(tracker.getPieces(), 0.0, 0.0, 0.0); + List seen = distorted.observe(tracker.getPieces(), 0.0, 0.0, 0.0); + assertEquals(3, seen.size()); + for (CameraSighting s : seen) { + RobotFramePoint p = distorted.groundPointInRobotFrame(s.getCentreXPx(), s.getCentreYPx(), RADIUS); + boolean matched = false; + for (org.horizon36596.simloop.field.GamePiece piece : tracker.getPieces()) { + if (piece.getId() == s.getPieceId()) { + assertEquals(piece.getFieldXInches(), p.forwardInches, 1e-6); + assertEquals(piece.getFieldYInches(), p.leftInches, 1e-6); + matched = true; + } + } + assertTrue(matched); + } + // The off-axis pieces move toward the image centre under barrel distortion. + for (int i = 0; i < 2; i++) { + double pinholeOff = Math.abs(pinholeSeen.get(i).getCentreXPx() - LENS.getCentreXPx()); + double barrelOff = Math.abs(seen.get(i).getCentreXPx() - LENS.getCentreXPx()); + assertTrue(barrelOff < pinholeOff, "piece " + i + ": " + barrelOff + " vs " + pinholeOff); + } + } + + @Test + void nullOccludersAndABadThresholdAreRejected() { + GamePieceTracker tracker = piecesAt(new double[] {30.0, 0.0}); + assertThrows(IllegalArgumentException.class, + () -> camera().observe(tracker.getPieces(), 0.0, 0.0, 0.0, null)); + assertThrows(IllegalArgumentException.class, () -> camera().withMinVisibleFraction(0.0)); + assertThrows(IllegalArgumentException.class, () -> camera().withMinVisibleFraction(1.5)); + } +} diff --git a/SimLoop/src/test/java/org/horizon36596/simloop/vision/SimFrameRendererTest.java b/SimLoop/src/test/java/org/horizon36596/simloop/vision/SimFrameRendererTest.java new file mode 100644 index 0000000..d108e46 --- /dev/null +++ b/SimLoop/src/test/java/org/horizon36596/simloop/vision/SimFrameRendererTest.java @@ -0,0 +1,378 @@ +package org.horizon36596.simloop.vision; + +import static org.junit.jupiter.api.Assertions.assertArrayEquals; +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertFalse; +import static org.junit.jupiter.api.Assertions.assertNotNull; +import static org.junit.jupiter.api.Assertions.assertNull; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.io.ByteArrayOutputStream; +import java.io.IOException; +import java.io.OutputStream; +import java.nio.charset.StandardCharsets; +import java.nio.file.Files; +import java.nio.file.Path; +import java.nio.file.Paths; +import java.util.Arrays; +import java.util.Collections; +import java.util.List; +import java.util.concurrent.ExecutionException; +import java.util.concurrent.ForkJoinPool; +import java.util.zip.CRC32; +import java.util.zip.DataFormatException; +import java.util.zip.Inflater; + +import org.horizon36596.simloop.field.FieldWorld; +import org.horizon36596.simloop.field.GamePiece; +import org.horizon36596.simloop.field.GamePieceTracker; +import org.horizon36596.simloop.field.ObstacleBox; +import org.junit.jupiter.api.Test; + +/** + * INVARIANT-tier (domain R9) — the synthetic frame renderer agrees with the geometric cameras, so a pixel + * pipeline run on its frames is being tested against the same scene the rest of the sim describes: + * + *

      + *
    1. a piece is drawn where {@link SimCamera} says it is, about the size it says, through a pinhole or a + * distorted lens;
      2. + *
    2. what {@link SimCamera}'s occluded {@code observe} hides (behind a field box, behind the robot's own + * body, even a box reaching behind the lens) has no pixels in the frame;
      3. + *
    3. a tag is drawn inside the corners {@link SimTagCamera} reports, white border outside them;
      4. + *
    4. frames are deterministic: same inputs, same bytes, on one thread or many; sensor noise is seeded;
      5. + *
    5. the PNG is a valid PNG with the pixels in it, and says it is synthetic.
      6. + *
    + * + *

    The scene, colours and boxes are all invented for the test. Nothing here measures a real camera. + */ +class SimFrameRendererTest { + + private static final CameraLens LENS = CameraLens.fromHorizontalFov(70.0, 320, 240); + private static final CameraMount FRONT = CameraMount.front(6.0, 10.0, 22.0); + private static final double RADIUS = 2.5; + private static final List NONE = Collections.emptyList(); + private static final FrameStyle STYLE = FrameStyle.builder() + .pieceColour("red", 0xC02020) + .pieceColour("blue", 0x2040C0) + .build(); + + private static GamePieceTracker pieces(Object... kindXY) { + GamePieceTracker tracker = new GamePieceTracker(4, FieldWorld.unbounded()); + tracker.updateRobotPose(0.0, 0.0, 0.0); + for (int i = 0; i < kindXY.length; i += 3) { + tracker.addLoosePiece((String) kindXY[i], (Double) kindXY[i + 1], (Double) kindXY[i + 2], RADIUS); + } + return tracker; + } + + private static SimFrameRenderer renderer(CameraLens lens) { + return SimFrameRenderer.builder(lens, FRONT).style(STYLE).build(); + } + + @Test + void aPieceIsDrawnWhereSimCameraSaysItIs() { + GamePieceTracker tracker = pieces("red", 30.0, 4.0, "blue", 40.0, -12.0); + SimFrame frame = renderer(LENS).render(tracker.getPieces(), 0.0, 0.0, 0.0, CameraOccluders.none(), + AprilTagLayout.empty()); + List seen = new SimCamera("front", LENS, FRONT).observe(tracker.getPieces(), 0.0, 0.0, 0.0); + assertEquals(2, seen.size()); + for (CameraSighting s : seen) { + double[] centroid = frame.centroidOfPiece(s.getPieceId()); + assertNotNull(centroid); + // A sphere's image is an ellipse a little larger than the projected centre's disc and stretched + // away from the image centre, so allow a pixel; the sighting is the projected centre. + assertEquals(s.getCentreXPx(), centroid[0], 1.0, "piece " + s.getPieceId() + " x"); + assertEquals(s.getCentreYPx(), centroid[1], 1.0, "piece " + s.getPieceId() + " y"); + double area = frame.pixelCountOfPiece(s.getPieceId()); + double disc = Math.PI * s.getRadiusPx() * s.getRadiusPx(); + assertTrue(area > 0.85 * disc && area < 1.35 * disc, "area " + area + " vs disc " + disc); + } + // The kind's colour, shaded: red pixels are red. + double[] c = frame.centroidOfPiece(seen.get(0).getPieceId()); + int rgb = frame.getRgb((int) c[0], (int) c[1]); + assertTrue(((rgb >> 16) & 0xFF) > 2 * ((rgb >> 8) & 0xFF), "red piece pixel 0x" + Integer.toHexString(rgb)); + } + + @Test + void aDistortedLensDrawsThePieceWhereTheDistortedCameraSaysItIs() { + CameraLens barrel = LENS.withDistortion(-0.30, 0.08, 0.0, 0.0, 0.0); + GamePieceTracker tracker = pieces("red", 26.0, 13.0); + SimFrame frame = renderer(barrel).render(tracker.getPieces(), 0.0, 0.0, 0.0, CameraOccluders.none(), + AprilTagLayout.empty()); + CameraSighting distorted = new SimCamera("front", barrel, FRONT).observe(tracker.getPieces(), 0.0, 0.0, 0.0).get(0); + CameraSighting pinhole = new SimCamera("front", LENS, FRONT).observe(tracker.getPieces(), 0.0, 0.0, 0.0).get(0); + double[] centroid = frame.centroidOfPiece(distorted.getPieceId()); + assertEquals(distorted.getCentreXPx(), centroid[0], 1.0); + assertEquals(distorted.getCentreYPx(), centroid[1], 1.0); + assertTrue(Math.abs(pinhole.getCentreXPx() - centroid[0]) > 3.0, + "the distortion moved the piece by more than the tolerance, or this test proves nothing"); + } + + @Test + void whatTheOccludedObserveHidesHasNoPixels() { + GamePieceTracker tracker = pieces("red", 34.0, 0.0, "blue", 30.0, 14.0); + ObstacleBox wall = new ObstacleBox("wall", 22.0, 23.0, -5.0, 5.0, 0.0, 14.0); + CameraOccluders boxes = CameraOccluders.field(Arrays.asList(wall)); + SimFrame frame = renderer(LENS).render(tracker.getPieces(), 0.0, 0.0, 0.0, boxes, AprilTagLayout.empty()); + List seen = new SimCamera("front", LENS, FRONT).observe(tracker.getPieces(), 0.0, 0.0, 0.0, + boxes); + assertEquals(1, seen.size(), "only the blue piece, beside the wall, is seen"); + assertEquals(0, frame.pixelCountOfPiece(0), "the red piece behind the wall"); + assertTrue(frame.pixelCountOfPiece(seen.get(0).getPieceId()) > 0); + // The wall itself is drawn, in the box colour, in front of where the red piece would be. + CameraSighting hidden = new SimCamera("front", LENS, FRONT).observe(tracker.getPieces(), 0.0, 0.0, 0.0).get(0); + int rgb = frame.getRgb((int) hidden.getCentreXPx(), (int) hidden.getCentreYPx()); + assertEquals(-1, frame.getPieceIdAt((int) hidden.getCentreXPx(), (int) hidden.getCentreYPx())); + assertTrue(Math.abs(((rgb >> 16) & 0xFF) - (rgb & 0xFF)) < 40, "box grey, got 0x" + Integer.toHexString(rgb)); + } + + @Test + void theRobotsOwnBodyHidesAPieceAndTurnsWithTheRobot() { + // A raised arm 8-9 in ahead of the robot centre, across the camera's view. + ObstacleBox arm = new ObstacleBox("arm", 8.0, 9.0, -6.0, 6.0, 2.0, 12.0); + CameraOccluders body = CameraOccluders.of(NONE, Arrays.asList(arm)); + GamePieceTracker tracker = new GamePieceTracker(4, FieldWorld.unbounded()); + tracker.addLoosePiece("red", -30.0, 0.0, RADIUS); // ahead of a robot facing -X + double heading = Math.PI; + SimFrame open = renderer(LENS).render(tracker.getPieces(), 0.0, 0.0, heading, CameraOccluders.none(), + AprilTagLayout.empty()); + SimFrame blocked = renderer(LENS).render(tracker.getPieces(), 0.0, 0.0, heading, body, AprilTagLayout.empty()); + assertTrue(open.pixelCountOfPiece(0) > 0); + assertEquals(0, blocked.pixelCountOfPiece(0)); + assertTrue(new SimCamera("front", LENS, FRONT).observe(tracker.getPieces(), 0.0, 0.0, heading, body).isEmpty()); + } + + @Test + void aBoxReachingBehindTheLensIsStillDrawnAndStillHides() { + // A side plate on the robot from 8 in behind the centre to 20 in ahead, left of the lens: part of it + // is behind the lens plane, so its image bounds have to be cut there rather than given up on. + ObstacleBox plate = new ObstacleBox("plate", -8.0, 20.0, 3.0, 4.0, 0.0, 20.0); + CameraOccluders body = CameraOccluders.of(NONE, Arrays.asList(plate)); + GamePieceTracker tracker = pieces("red", 24.0, 9.0, "blue", 24.0, -9.0); + // A green robot, so the plate's pixels cannot be mistaken for floor or background grey. + FrameStyle green = FrameStyle.builder().robot(0x00C000).build(); + SimFrame frame = SimFrameRenderer.builder(LENS, FRONT).style(green).build() + .render(tracker.getPieces(), 0.0, 0.0, 0.0, body, AprilTagLayout.empty()); + List seen = new SimCamera("front", LENS, FRONT).observe(tracker.getPieces(), 0.0, 0.0, 0.0, + body); + assertEquals(1, seen.size(), "only the blue piece, right of the plate, is seen"); + assertEquals(1, seen.get(0).getPieceId()); + assertEquals(0, frame.pixelCountOfPiece(0), "the red piece is behind the plate"); + assertTrue(frame.pixelCountOfPiece(1) > 0); + // The plate covers the image's left edge at mid height. + int rgb = frame.getRgb(2, LENS.getHeightPx() / 2); + assertTrue(((rgb >> 8) & 0xFF) > 40 && ((rgb >> 16) & 0xFF) < 10 && (rgb & 0xFF) < 10, + "plate pixel 0x" + Integer.toHexString(rgb)); + assertEquals(-1, frame.getPieceIdAt(2, LENS.getHeightPx() / 2)); + } + + @Test + void aTagIsDrawnInsideTheCornersSimTagCameraReports() { + AprilTagLayout layout = AprilTagLayout.builder() + .addTag(5, 40.0, 0.0, 6.0, Math.PI, 6.0) + .addTiltedTag(9, 30.0, 8.0, 0.05, Math.PI, Math.PI / 2.0, 6.0) // lying face up on the floor + .build(); + SimFrame frame = renderer(LENS).render(Collections.emptyList(), 0.0, 0.0, 0.0, + CameraOccluders.none(), layout); + SimTagCamera tags = new SimTagCamera(LENS, FRONT, 0.0, Double.POSITIVE_INFINITY, 90.0, NONE); + List seen = tags.observe(layout, 0.0, 0.0, 0.0); + assertEquals(2, seen.size()); + for (TagSighting s : seen) { + double[] c = {s.getCentreXPx(), s.getCentreYPx()}; + assertEquals(s.getId(), frame.getTagIdAt((int) c[0], (int) c[1]), "tag " + s.getId() + " centre"); + // Every pixel inside the black square's corners belongs to the tag. + double[][] corners = s.getCornersPx(); + double minX = Double.MAX_VALUE; + double maxX = -Double.MAX_VALUE; + double minY = Double.MAX_VALUE; + double maxY = -Double.MAX_VALUE; + for (double[] corner : corners) { + minX = Math.min(minX, corner[0]); + maxX = Math.max(maxX, corner[0]); + minY = Math.min(minY, corner[1]); + maxY = Math.max(maxY, corner[1]); + } + int tagPixels = 0; + int outside = 0; + for (int y = 0; y < frame.getHeightPx(); y++) { + for (int x = 0; x < frame.getWidthPx(); x++) { + if (frame.getTagIdAt(x, y) == s.getId()) { + tagPixels++; + // The white border is a cell wide; nothing of the tag lies beyond 1.5 cells out. + double margin = 0.25 * Math.max(maxX - minX, maxY - minY) + 2.0; + if (x < minX - margin || x > maxX + margin || y < minY - margin || y > maxY + margin) { + outside++; + } + } + } + } + assertTrue(tagPixels > 20, "tag " + s.getId() + " has " + tagPixels + " pixels"); + assertEquals(0, outside, "tag " + s.getId() + " pixels far outside its corners"); + } + } + + @Test + void framesAreDeterministicAndNoiseIsSeeded() { + GamePieceTracker tracker = pieces("red", 30.0, 4.0, "blue", 40.0, -12.0); + ObstacleBox wall = new ObstacleBox("wall", 60.0, 62.0, -40.0, 40.0, 0.0, 12.0); + CameraOccluders boxes = CameraOccluders.field(Arrays.asList(wall)); + byte[] a = renderer(LENS).render(tracker.getPieces(), 1.0, 2.0, 0.1, boxes, AprilTagLayout.empty()).getRgbBytes(); + byte[] b = renderer(LENS).render(tracker.getPieces(), 1.0, 2.0, 0.1, boxes, AprilTagLayout.empty()).getRgbBytes(); + assertArrayEquals(a, b); + // Rows are drawn in parallel; one thread must give the same bytes. + ForkJoinPool single = new ForkJoinPool(1); + try { + byte[] oneThread = single.submit(() -> renderer(LENS).render(tracker.getPieces(), 1.0, 2.0, 0.1, boxes, + AprilTagLayout.empty()).getRgbBytes()).get(); + assertArrayEquals(a, oneThread); + } catch (InterruptedException | ExecutionException e) { + throw new AssertionError(e); + } finally { + single.shutdown(); + } + + SimFrameRenderer noisyA = SimFrameRenderer.builder(LENS, FRONT).style(STYLE).sensorNoise(7, 3.0).build(); + SimFrameRenderer noisyB = SimFrameRenderer.builder(LENS, FRONT).style(STYLE).sensorNoise(7, 3.0).build(); + SimFrameRenderer otherSeed = SimFrameRenderer.builder(LENS, FRONT).style(STYLE).sensorNoise(8, 3.0).build(); + SimFrame first = noisyA.render(tracker.getPieces(), 1.0, 2.0, 0.1, boxes, AprilTagLayout.empty()); + SimFrame second = noisyA.render(tracker.getPieces(), 1.0, 2.0, 0.1, boxes, AprilTagLayout.empty()); + assertArrayEquals(first.getRgbBytes(), + noisyB.render(tracker.getPieces(), 1.0, 2.0, 0.1, boxes, AprilTagLayout.empty()).getRgbBytes()); + assertFalse(Arrays.equals(first.getRgbBytes(), second.getRgbBytes()), "frame 1's noise is not frame 0's"); + assertFalse(Arrays.equals(first.getRgbBytes(), + otherSeed.render(tracker.getPieces(), 1.0, 2.0, 0.1, boxes, AprilTagLayout.empty()).getRgbBytes())); + assertFalse(Arrays.equals(a, first.getRgbBytes())); + assertEquals(0L, first.getFrameNumber()); + assertEquals(1L, second.getFrameNumber()); + + // The noise has about the asked-for spread: compare against the noise-free frame. + double sum = 0.0; + double sumSq = 0.0; + byte[] clean = a; + byte[] noisy = first.getRgbBytes(); + int n = 0; + for (int i = 0; i < clean.length; i++) { + int c = clean[i] & 0xFF; + if (c < 20 || c > 235) { + continue; // clamped values would bias the spread + } + double d = (noisy[i] & 0xFF) - c; + sum += d; + sumSq += d * d; + n++; + } + double mean = sum / n; + double sd = Math.sqrt(sumSq / n - mean * mean); + // Flat surfaces have one colour each, so rounding both frames to whole counts can shift the mean + // by up to half a count; the noise itself is zero-mean. + assertEquals(0.0, mean, 0.5); + assertEquals(3.0, sd, 0.3, "rounding to whole counts adds about 0.3 in quadrature"); + } + + @Test + void supersamplingKeepsLabelsOnPixelCentresAndSmoothsEdges() { + GamePieceTracker tracker = pieces("red", 30.0, 4.0); + SimFrame one = renderer(LENS).render(tracker.getPieces(), 0.0, 0.0, 0.0, CameraOccluders.none(), + AprilTagLayout.empty()); + SimFrame four = SimFrameRenderer.builder(LENS, FRONT).style(STYLE).supersampling(2).build() + .render(tracker.getPieces(), 0.0, 0.0, 0.0, CameraOccluders.none(), AprilTagLayout.empty()); + assertEquals(one.pixelCountOfPiece(0), four.pixelCountOfPiece(0)); + // Some edge pixel is a blend: neither the floor's colour nor the piece's. + int blends = 0; + for (int y = 0; y < four.getHeightPx(); y++) { + for (int x = 0; x < four.getWidthPx(); x++) { + if (four.getRgb(x, y) != one.getRgb(x, y)) { + blends++; + } + } + } + assertTrue(blends > 0); + } + + @Test + void thePngIsValidHoldsThePixelsAndSaysItIsSynthetic() throws IOException { + GamePieceTracker tracker = pieces("red", 30.0, 4.0, "blue", 40.0, -12.0); + AprilTagLayout layout = AprilTagLayout.builder().addTag(3, 70.0, 10.0, 8.0, Math.PI, 6.0).build(); + ObstacleBox wall = new ObstacleBox("wall", 72.0, 74.0, -72.0, 72.0, 0.0, 12.0); + SimFrame frame = SimFrameRenderer.builder(LENS, FRONT).style(STYLE).supersampling(2).build() + .render(tracker.getPieces(), 0.0, 0.0, 0.0, CameraOccluders.field(Arrays.asList(wall)), layout); + byte[] png = frame.toPng(); + byte[] pixels = decodePng(png, frame.getWidthPx(), frame.getHeightPx()); + assertArrayEquals(frame.getRgbBytes(), pixels); + assertTrue(new String(png, StandardCharsets.ISO_8859_1).contains(SimFrame.PNG_COMMENT)); + + // Kept for the docs: build/sim/vision/sim-frame-example.png. + Path dir = Paths.get("build", "sim", "vision"); + Files.createDirectories(dir); + try (OutputStream out = Files.newOutputStream(dir.resolve("sim-frame-example.png"))) { + frame.writePng(out); + } + } + + /** + * The test's own reader for the one PNG layout {@link SimFrame} writes (8-bit RGB, filter 0 rows, one + * IDAT), checking the signature, the header and every chunk's CRC: the unit-test classpath has no + * {@code javax.imageio}. + */ + private static byte[] decodePng(byte[] png, int width, int height) throws IOException { + byte[] signature = {(byte) 0x89, 0x50, 0x4E, 0x47, 0x0D, 0x0A, 0x1A, 0x0A}; + assertArrayEquals(signature, Arrays.copyOf(png, 8)); + ByteArrayOutputStream idat = new ByteArrayOutputStream(); + int at = 8; + boolean sawEnd = false; + while (at < png.length) { + int length = readInt(png, at); + String type = new String(png, at + 4, 4, StandardCharsets.ISO_8859_1); + CRC32 crc = new CRC32(); + crc.update(png, at + 4, 4 + length); + assertEquals((int) crc.getValue(), readInt(png, at + 8 + length), type + " CRC"); + if (type.equals("IHDR")) { + assertEquals(width, readInt(png, at + 8)); + assertEquals(height, readInt(png, at + 12)); + assertEquals(8, png[at + 16]); + assertEquals(2, png[at + 17]); + } else if (type.equals("IDAT")) { + idat.write(png, at + 8, length); + } else if (type.equals("IEND")) { + sawEnd = true; + } + at += 12 + length; + } + assertTrue(sawEnd); + Inflater inflater = new Inflater(); + inflater.setInput(idat.toByteArray()); + byte[] raw = new byte[(width * 3 + 1) * height]; + try { + assertEquals(raw.length, inflater.inflate(raw)); + } catch (DataFormatException e) { + throw new AssertionError("IDAT does not inflate", e); + } finally { + inflater.end(); + } + byte[] pixels = new byte[width * height * 3]; + for (int y = 0; y < height; y++) { + assertEquals(0, raw[y * (width * 3 + 1)], "row " + y + " filter"); + System.arraycopy(raw, y * (width * 3 + 1) + 1, pixels, y * width * 3, width * 3); + } + return pixels; + } + + private static int readInt(byte[] b, int at) { + return ((b[at] & 0xFF) << 24) | ((b[at + 1] & 0xFF) << 16) | ((b[at + 2] & 0xFF) << 8) | (b[at + 3] & 0xFF); + } + + @Test + void rgbaAndAccessorsAgree() { + SimFrame frame = renderer(LENS).render(pieces("red", 30.0, 4.0).getPieces(), 0.0, 0.0, 0.0, + CameraOccluders.none(), AprilTagLayout.empty()); + byte[] rgb = frame.getRgbBytes(); + byte[] rgba = frame.getRgbaBytes(); + assertEquals(rgb.length / 3 * 4, rgba.length); + assertEquals(rgb[3 * 1000 + 1], rgba[4 * 1000 + 1]); + assertEquals((byte) 0xFF, rgba[4 * 1000 + 3]); + assertThrows(IndexOutOfBoundsException.class, () -> frame.getRgb(320, 0)); + assertNull(frame.centroidOfPiece(99)); + assertTrue(frame.toString().contains("synthetic")); + } +} diff --git a/SimLoop/src/test/java/org/horizon36596/simloop/vision/SimTagCameraFidelityTest.java b/SimLoop/src/test/java/org/horizon36596/simloop/vision/SimTagCameraFidelityTest.java new file mode 100644 index 0000000..36fa8c9 --- /dev/null +++ b/SimLoop/src/test/java/org/horizon36596/simloop/vision/SimTagCameraFidelityTest.java @@ -0,0 +1,124 @@ +package org.horizon36596.simloop.vision; + +import static org.junit.jupiter.api.Assertions.assertEquals; +import static org.junit.jupiter.api.Assertions.assertNotNull; +import static org.junit.jupiter.api.Assertions.assertThrows; +import static org.junit.jupiter.api.Assertions.assertTrue; + +import java.util.Arrays; +import java.util.Collections; +import java.util.List; + +import org.horizon36596.simloop.field.ObstacleBox; +import org.junit.jupiter.api.Test; + +/** + * INVARIANT-tier (domain R9) — the tag camera beyond plumb tags and a pinhole: + * + *

      + *
    1. a tilted tag's frame is a rotation, its normal tilts with it, and tilt 0 is the plumb tag exactly;
      2. + *
    2. a tag lying face up is seen from above and not from below;
      3. + *
    3. the robot's own body hides a tag, and turns with the robot;
      4. + *
    4. a distorted lens moves the corner pixels and leaves the angles and transforms alone.
      5. + *
    + * + *

    Tags, boxes and coefficients are invented for the test. + */ +class SimTagCameraFidelityTest { + + private static final CameraLens LENS = CameraLens.fromHorizontalFov(60.0, 640, 480); + private static final CameraMount LEVEL = new CameraMount(0.0, 0.0, 10.0, 0.0, 0.0); + private static final CameraMount DOWN = new CameraMount(0.0, 0.0, 20.0, 0.0, Math.toRadians(40.0)); + private static final List NONE = Collections.emptyList(); + + private static SimTagCamera camera(CameraLens lens, CameraMount mount) { + return new SimTagCamera(lens, mount, 0.0, Double.POSITIVE_INFINITY, 90.0, NONE); + } + + @Test + void tiltZeroIsThePlumbTagExactly() { + AprilTagLayout.Tag plumb = AprilTagLayout.builder().addTag(1, 50.0, 5.0, 8.0, 2.5, 6.0).build().getTag(1); + AprilTagLayout.Tag zero = AprilTagLayout.builder() + .addTiltedTag(1, 50.0, 5.0, 8.0, 2.5, 0.0, 6.0).build().getTag(1); + for (int r = 0; r < 3; r++) { + for (int c = 0; c < 3; c++) { + assertEquals(plumb.getFieldToTag().rotation(r, c), zero.getFieldToTag().rotation(r, c), 0.0); + } + } + assertEquals(0.0, plumb.getFaceNormal()[2], 0.0); + } + + @Test + void aTiltedTagsFrameIsARotationAndItsNormalTiltsWithIt() { + double tilt = Math.toRadians(30.0); + AprilTagLayout.Tag tag = AprilTagLayout.builder() + .addTiltedTag(4, 60.0, 0.0, 20.0, Math.PI, tilt, 6.0).build().getTag(4); + double[] n = tag.getFaceNormal(); + assertEquals(-Math.cos(tilt), n[0], 1e-12); + assertEquals(0.0, n[1], 1e-12); + assertEquals(Math.sin(tilt), n[2], 1e-12, "tilted up: the face looks up"); + // Viewer's right stays level. + assertEquals(0.0, tag.getFieldToTag().rotation(2, 0), 1e-12); + // Target +Z points into the tag: against the normal. + assertEquals(-n[0], tag.getFieldToTag().rotation(0, 2), 1e-12); + assertEquals(-n[2], tag.getFieldToTag().rotation(2, 2), 1e-12); + assertThrows(IllegalArgumentException.class, + () -> AprilTagLayout.builder().addTiltedTag(1, 0, 0, 0, 0, 2.0, 6.0)); + } + + @Test + void aTagLyingFaceUpIsSeenFromAboveAndNotFromBelow() { + AprilTagLayout layout = AprilTagLayout.builder() + .addTiltedTag(2, 22.0, 0.0, 0.0, 0.0, Math.PI / 2.0, 6.0) + .addTiltedTag(3, 22.0, 0.0, 0.0, 0.0, -Math.PI / 2.0, 6.0) + .build(); + SimTagCamera down = camera(LENS, DOWN); + assertEquals(SimTagCamera.Visibility.VISIBLE, down.visibility(layout.getTag(2), 0.0, 0.0, 0.0)); + assertEquals(SimTagCamera.Visibility.FACING_AWAY, down.visibility(layout.getTag(3), 0.0, 0.0, 0.0)); + // A plumb tag at the same place facing away from the camera's side would be seen edge-on or not at + // all; the flat one is seen with its corners spread in both image axes. + TagSighting seen = down.observe(layout, 0.0, 0.0, 0.0).get(0); + double[][] corners = seen.getCornersPx(); + assertTrue(Math.abs(corners[0][1] - corners[3][1]) > 10.0, "a flat tag has height in the image"); + } + + @Test + void theRobotsOwnBodyHidesATagAndTurnsWithTheRobot() { + AprilTagLayout layout = AprilTagLayout.builder().addTag(7, 0.0, 60.0, 10.0, -Math.PI / 2.0, 6.0).build(); + ObstacleBox mast = new ObstacleBox("mast", 4.0, 5.0, -3.0, 3.0, 0.0, 30.0); + CameraOccluders body = CameraOccluders.of(NONE, Arrays.asList(mast)); + SimTagCamera camera = camera(LENS, LEVEL); + double facingTag = Math.PI / 2.0; // robot forward = field +Y, toward the tag + assertEquals(SimTagCamera.Visibility.VISIBLE, camera.visibility(layout.getTag(7), 0.0, 0.0, facingTag)); + assertEquals(SimTagCamera.Visibility.OCCLUDED, + camera.visibility(layout.getTag(7), 0.0, 0.0, facingTag, body)); + assertTrue(camera.observe(layout, 0.0, 0.0, facingTag, body).isEmpty()); + // The same box as a FIELD box stands at field x 4-5, off the line of sight. + assertEquals(SimTagCamera.Visibility.VISIBLE, camera.visibility(layout.getTag(7), 0.0, 0.0, facingTag, + CameraOccluders.field(Arrays.asList(mast)))); + } + + @Test + void distortionMovesCornerPixelsAndLeavesAnglesAlone() { + AprilTagLayout layout = AprilTagLayout.builder().addTag(1, 40.0, 14.0, 16.0, Math.PI, 6.0).build(); + CameraLens barrel = LENS.withDistortion(-0.25, 0.05, 0.0, 0.0, 0.0); + TagSighting pinhole = camera(LENS, LEVEL).observe(layout, 0.0, 0.0, 0.0).get(0); + TagSighting distorted = camera(barrel, LEVEL).observe(layout, 0.0, 0.0, 0.0).get(0); + assertEquals(pinhole.getTxDegrees(), distorted.getTxDegrees(), 0.0); + assertEquals(pinhole.getTyDegrees(), distorted.getTyDegrees(), 0.0); + assertEquals(pinhole.getRangeInches(), distorted.getRangeInches(), 0.0); + double[][] a = pinhole.getCornersPx(); + double[][] b = distorted.getCornersPx(); + for (int i = 0; i < 4; i++) { + double pinholeOff = Math.hypot(a[i][0] - LENS.getCentreXPx(), a[i][1] - LENS.getCentreYPx()); + double barrelOff = Math.hypot(b[i][0] - LENS.getCentreXPx(), b[i][1] - LENS.getCentreYPx()); + assertTrue(barrelOff < pinholeOff - 1.0, "corner " + i + " pulled toward the centre"); + } + // And each distorted corner is exactly the lens's projection of the corner's optical position. + double[] optical = distorted.getCameraToTag().apply(-3.0, -3.0, 0.0); + double[] expected = barrel.project(optical[0], optical[1], optical[2]); + assertNotNull(expected); + assertEquals(expected[0], b[0][0], 1e-9); + assertEquals(expected[1], b[0][1], 1e-9); + } +} diff --git a/simloop-agents/AGENTS.md b/simloop-agents/AGENTS.md index cd99e72..6499efb 100644 --- a/simloop-agents/AGENTS.md +++ b/simloop-agents/AGENTS.md @@ -53,9 +53,10 @@ before writing a test, not after one fails. The public surface is completely listed below. If the thing you want is not in it, it does not exist, and the right move is to say so. **Before claiming SimLoop can do something, check -[what it does not do](https://libraries.horizon36596.org/simloop/limits/).** A rendered camera image, +[what it does not do](https://libraries.horizon36596.org/simloop/limits/).** A photoreal camera image, intake rollers that really grip, a shipped field, a command scheduler and controller tuning are all -deliberately out of scope. Game-piece physics, a geometry-only camera and fitting the plant to a real log +deliberately out of scope. Game-piece physics, a camera with distortion and occlusion, synthetic frames (never for tuning colour +thresholds) and fitting the plant to a real log do exist, each with stated limits. A plausible method name half-remembered from another library is the single most common way this goes wrong. @@ -149,7 +150,7 @@ Thirteen packages. Complete as of `v0.2.0-alpha.1`. Every feature, one line each | `simclock` | `SimClockLoader`, `SimClockElapsedTime`, `SimClockHooks`, `SimClockScheduler`, `JvmInstructions` | Used by `OpModeRunner.forTeamClass(...)` to move this project's own timer calls onto sim time. Not called directly. | | `field` | `GamePiece`, `GamePieceTracker`, `PossessionState`, `TriggerVolume`, `FieldFrameTransform`, `FieldWorld`, `GamePiecePhysicsConfig`, `ObstacleBox`, `ScoringVolume`, `KinematicBody`, `HingeDynamics`, `BodyCell`, `ConvexShape`, `RobotShape`, `OrientedBox`, `StaticRobot`, `FootprintContact`, `RobotContact`, `SeededRandom` | Game pieces with possession and physics, goals, field structures, robot contact. Intaking is still a box test. | | `sensor` | `SensorMount`, `RangeFinder`, `RangeHit`, `ColorProbe`, `ContactSwitch` | What a touch, distance or colour sensor on the robot sees or touches on the field. | -| `vision` | `CameraLens`, `CameraMount`, `SimCamera`, `CameraSighting`, `AprilTagLayout`, `SimTagCamera`, `TagSighting`, `RigidTransform3`, `RobotFramePoint` | Which pieces and AprilTags a camera sees, and where. Geometry only; no image. | +| `vision` | `CameraLens`, `CameraMount`, `CameraOccluders`, `SimCamera`, `CameraSighting`, `AprilTagLayout`, `SimTagCamera`, `TagSighting`, `SimFrameRenderer`, `SimFrame`, `FrameStyle`, `RigidTransform3`, `RobotFramePoint` | Which pieces and AprilTags a camera sees, and where, through a distorted lens with occlusion. Synthetic RGB frames for running a pixel pipeline; flat colours, never for tuning thresholds. | | `fit` | `FirstOrderFit`, `GapReport`, `LeastSquares`, `RlogTimeSeries` | Fitting the first-order plant to a real robot log, and a sim-against-robot gap report. Changes no constant. | | `loop` | `RunResult`, `Objective`, `Guardrail`, `EnvelopeGuardrails`, `Scorer`, `Gate`, `Milestone`, `LoopDriver`, `StopReport` | Reading a log back and scoring it. | | `viz` | `MechanismSketch`, `MechanismSketchDefinition`, `JointDefinition`, `RevoluteJointDefinition`, `LinearJointDefinition`, `TurretSketch`, `TurretSketchDefinition`, `ArticulatedRobotModel`, `ShotArc`, `ShotArcRecorder`, `RobotVector3` | Drawing mechanisms, robot parts and shots for AdvantageScope. Inches and radians in, metres in the log. |