diff --git a/docs/adr/ADR-043-stereo-camera-source.md b/docs/adr/ADR-043-stereo-camera-source.md index 9495750153..56b1823eee 100644 --- a/docs/adr/ADR-043-stereo-camera-source.md +++ b/docs/adr/ADR-043-stereo-camera-source.md @@ -2,7 +2,7 @@ **Status:** Proposed (2026-09-25) · **R1 implemented** 2026-09-26 (runtime, hardware-free; maintainer defaults recorded in the roadmap §G: stereo-only, iface slots, service clients only, no raw frames -to pages) · introduces +to pages) · **R2 implemented** 2026-09-26 (the service-side rectifier, decision 2 below) · introduces [`XR_DXR_stereo_camera`](../specs/extensions/XR_DXR_stereo_camera.md) · appends optional camera slots to `xrt_plugin_iface` under the [ADR-020](ADR-020-plugin-abi-compatibility-policy.md) append-at-end rule · related: [ADR-019](ADR-019-vendor-plugin-aux-boundary.md), diff --git a/docs/reference/xrt_plugin_iface.md b/docs/reference/xrt_plugin_iface.md index 40f5e18bb7..d6f00c39e5 100644 --- a/docs/reference/xrt_plugin_iface.md +++ b/docs/reference/xrt_plugin_iface.md @@ -631,7 +631,7 @@ void (*stereo_camera_close)(cam); | `wait_frame` is called from **one runtime-owned thread per open camera**, may block up to `timeout_ns`, and returns an `enum xrt_plugin_stereo_camera_wait` (`OK` / `TIMEOUT` / `SUSPENDED` / `ERROR`). Planes stay valid until `release_frame`. | The service maps `TIMEOUT` → `WAITING` (then `SUSPENDED` after 1 s without frames), `SUSPENDED` → `SUSPENDED`, `ERROR` → close + retry in 1 s | | **Never wait on a wake object shared with other readers** of the vendor channel — poll it. | An auto-reset event wakes exactly one waiter: two readers steal each other's frames (ADR-043) | | Decode your transport (JPEG, YUY2, …) to `GRAY8`, `NV12` or `BGRA8` yourself; always deliver **one side-by-side image**, `2·eye_width × eye_height`, left eye left, unmirrored, even extents. | The runtime owns format conversion, decimation, rectification (R2) and transport; one SBS layout for every vendor | -| Calibration is the **ACTIVE** device's — the unit you weave for — keyed by its own identity (serial). RAW, OpenCV convention: `k[eye] = fx fy cx cy`, `distortion_model` + coefficients, and `R`, `T` with **`x_R = R·x_L + T`** (T in mm; a right camera at +x of the left has `T = (−B, 0, 0)`). The runtime converts it to the client's pose-of-the-right-camera form. Return `XRT_ERROR_FEATURE_NOT_SUPPORTED` and do **not** set `CALIBRATED` if you cannot resolve it. | One test box carried eleven calibration folders; "the first one" is the wrong camera | +| Calibration is the **ACTIVE** device's — the unit you weave for — keyed by its own identity (serial). RAW, OpenCV convention: `k[eye] = fx fy cx cy`, `distortion_model` + coefficients, and `R`, `T` with **`x_R = R·x_L + T`** (T in mm; a right camera at +x of the left has `T = (−B, 0, 0)`). The runtime converts it to the client's pose-of-the-right-camera form. Return `XRT_ERROR_FEATURE_NOT_SUPPORTED` and do **not** set `CALIBRATED` if you cannot resolve it. `image_width/height` = the size the intrinsics were calibrated at; the service rescales them to the frame's eye size when the two differ. Unless you set `NATIVELY_RECTIFIED`, the service rectifies from exactly these numbers (R2, `u_stereo_rectify`), so a wrong calibration shows up as misaligned rows in `displayxr-cli camera probe --rectified`. | One test box carried eleven calibration folders; "the first one" is the wrong camera | | `device_identity` is private: the service hashes it per consumer (`persistentId`) and never exports it. | Fingerprinting (spec §7.5) | | Nothing throws across the boundary. | C ABI | diff --git a/docs/roadmap/stereo-camera-source.md b/docs/roadmap/stereo-camera-source.md index 575344dfc6..859e3fbe4d 100644 --- a/docs/roadmap/stereo-camera-source.md +++ b/docs/roadmap/stereo-camera-source.md @@ -1,7 +1,8 @@ # Stereo camera source — browser integration, vendor plug-in notes, phased plan -**Status:** design (2026-09-25); **R1 implemented** on `feat/stereo-camera-r1` (2026-09-26) — -see §E; maintainer decisions for R1 in §G. Decision: [ADR-043](../adr/ADR-043-stereo-camera-source.md). +**Status:** design (2026-09-25); **R1 implemented** on `feat/stereo-camera-r1` (2026-09-26), +**R2 implemented** on `feat/stereo-camera-r2` (2026-09-26, stacked) — see §E; maintainer decisions +for R1 in §G. **L1** (Leia provider) is on `displayxr-leia-plugin` `feat/stereo-camera-l1`. Decision: [ADR-043](../adr/ADR-043-stereo-camera-source.md). Extension: [`XR_DXR_stereo_camera`](../specs/extensions/XR_DXR_stereo_camera.md). Consumer driving it: the web SDK's 3D call module (`@displayxr/inline3d/call`, RFC 0002 on `displayxr-web` branch `feat/call-p1`, §4 *Capture*). @@ -194,7 +195,7 @@ The implementation belongs in `displayxr-leia-plugin` (`src/drv_leia/`), per ADR |---|---|---|---| | **R0** | runtime | This design: ADR-043, spec, this page | Maintainer sign-off on the open questions below | | **R1** ✅ | runtime | **Implemented 2026-09-26 (see "R1 as built" below).** Header `XR_DXR_stereo_camera.h` + `index.json` note (together — the catalog lint requires both); `xrt_plugin_iface` slots + `XRT_PLUGIN_IFACE_HAS_STEREO_CAMERA` (appended after `create_dp_d3d11_lift`, so **lands after ADR-042's PR**); platform-neutral camera manager (thread, refcount + linger, 3-slot pinned ring, per-stream wake handles, decimation, NV12/BGRA conversion); IPC + OpenXR entry points; sim_display fake; `displayxr-cli camera list/calib/probe`; selftest check | `camera probe` on the sim fake writes frames on Windows + Linux CI; ring/pinning unit tests (mirror `tests_lift_mailbox`) | -| **R2** | runtime | `u_stereo_rectify` (Bouguet, valid-region crop, LUT remap) with golden tests against OpenCV-generated fixtures; RECTIFIED output | sim fake: row error < 0.5 px, disparity = f·B/Z within 0.5 px | +| **R2** ✅ | runtime | **Implemented 2026-09-26 (see "R2 as built" below).** `u_stereo_rectify` (Bouguet, valid-region crop, LUT remap) with golden tests against OpenCV-generated fixtures; RECTIFIED output | sim fake: row error < 0.5 px, disparity = f·B/Z within 0.5 px — **measured 0.061 px worst block, 0.063 px worst disparity error** | | **R3** | runtime | Privacy: OS consent check, consent store + tray prompt, delegating-client registration, visibility/lock suspension, indicator, kill switches | Manual matrix: allow/deny/revoke, OS switch off, lock screen, background app — each blocks frames | | **L1** | leia-plugin (Windows) | Slots over the SR raw-camera channel per §D; calibration by active serial; keep-alive; repin runtime (feature-macro repin, `downstream-pins.json` `features` track) | On a panel box, incl. the multi-folder box: `camera probe --rectified` rows aligned; the vendor call app running at the same time still gets every frame; tracking/weave unaffected (frame-time + tracking-state logs) | | **B1** | browser (Windows) | §B device factory + duplicate hiding + hint + delegating registration in the installer; web SDK: prefer the hint in `camera:'auto'`, fill `hello` from it | RFC 0002 P1 call between two panel laptops sends rectified SBS from the tracker camera **while both are tracking** | @@ -230,6 +231,64 @@ against the sim fake). sync_file as the #1699 weave path), a read-only POSIX section, and a distorted/misaligned fake (all with R2/R3/A1). +### R2 as built + +- **`auxiliary/util/u_stereo_rectify.{h,c}`**, no OpenCV. Three layers so a GPU path replaces only + the last: geometry (`u_stereo_rectify_compute`: OpenCV's `stereoRectify(CALIB_ZERO_DISPARITY, + alpha = 0)` in C), float maps (`u_stereo_rectify_build_map`, what a GPU backend uploads as an + RG32F texture), and the CPU backend (a fixed-point bilinear LUT over the whole SBS image; NV12 = + full-res Y LUT + half-res UV LUT). RADTAN5 / RADTAN8 / KB4. Calibration at a different size than + the frames is rescaled. The alpha = 0 crop is verified on every border pixel of the real maps. +- **Service:** a per-camera `struct scam_rectifier` built at manager create from the plug-in's + RAW calibration (WARN line with f, principal point, baseline, zoom, build time). It runs once + per source frame on the camera thread with the lock released, only while a started stream wants + RECTIFIED; RAW streams read the original. `get_calibration(RECTIFIED)` returns the same + geometry (one pinhole for both eyes, pure +x baseline); `horizontalFovDeg` is the rectified one. + The browser is refused (`INPUT_UNSUPPORTED`) a camera the rectifier rejected rather than being + given the RAW-flagged fallback. INFO stats add `rectify ms/frame`. +- **Fake:** `SIM_DISPLAY_FAKE_STEREO_CAMERA_DISTORT=1` — the raw distorted, vertically misaligned, + rolled pair with exact ground truth (spec §10). +- **Tests** (`tests_stereo_rectify`, fixtures in `tests/fixtures/` from + `gen_stereo_rectify_fixtures.py`, OpenCV 4.10, offline): R1/R2 to 1e-12, principal point to the + converged reference within 1e-4 px (and OpenCV's own P1 within 0.75 px — OpenCV stops + `undistortPoints` at 5 iterations), maps to 3e-5 px, no black pixel, and end to end on the fake: + raw |Δy| median 2.9 px → rectified worst block 0.061 px; disparity 11.33 / 37.77 px ground truth + vs 11.31 / 37.79 measured (worst block 0.063 px off). +- **CLI:** `camera calib --rectified` prints P1/P2 and `Z = f·B/d`; every probe reports row + alignment and the dominant disparities as depths; `probe --rectified` exits 5 unless rows align. + Against the fake: |Δy| median 0.016 px, depths read back 2.004 m and 0.599 m. +- **Perf:** 0.94 ms GRAY8 / 1.36 ms NV12 / 2.25 ms BGRA8 per 1280×480 frame, M1 Pro, -O2, one + thread (3 ms GRAY8 in a Debug service). At 30 Hz that is ~3 % of one core for the Leia GRAY8 + source; **budget: ≤ 2 ms per frame on the camera thread**, above which the rows split across + threads (`u_stereo_rectify_lut_apply_rows`) or the GPU backend takes over. Setup (geometry + both + LUTs) 9–18 ms, once per calibration; LUT memory 6 bytes per output sample (3.7 MB + 0.9 MB). +- **Online vertical-alignment refinement** (`auxiliary/util/u_stereo_vrefine.{h,c}` + the service + worker). Why: on a Leia SR laptop the rectified rows were still +1.71 px apart, and OpenCV's own + `stereoRectify` on the same frame and calibration gave the same +1.71 px — the stored calibration + is slightly off for the frames, not the rectifier. Model `dy = a + b·(y − cy)` (right − left, + output px); the x (roll) term measured 0.04 px / 100 px on that data and is left out. A worker + measures rectified frames the camera thread hands it (Shi-Tomasi patches on a 2× downscale, NCC + over d × dy, full-res NCC refinement ±2 px with parabolic sub-pixel; gated on NCC ≥ 0.9, d > 0, + unclamped peaks, real vertical curvature), fits robustly (median start, Tukey IRLS; the slope + only when rows spread ≥ 40 px), and when the correction moves by > 0.2 px rebuilds the maps with + it folded into the geometry: `v_offset = a/f`, `v_slope = b`, split symmetrically as a per-eye + vertical affine in normalized rectified coordinates **before** the principal point and the + alpha = 0 crop are computed, so P1/P2 describe the corrected frames (still one pinhole, pure +x + baseline) and `calibrationGeneration` bumps on the swap. Duty cycle: every 250 ms for 5 s after + an open or an update, then one ≥ 3-frame window every 30 s; 2.7 ms per measurement at -O2 on the + worker (< 0.4 ms per frame amortised at 30 Hz), 8 ms per rebuild. Safety: |a| ≤ 6 px, + |b| ≤ 2 px / 100 px, ≥ 50 matches over ≥ 3 frames, re-verified on every re-open, WARN on first + application and INFO after, `DXR_STEREO_CAMERA_REFINE=0` kill switch. Fake: + `SIM_DISPLAY_FAKE_STEREO_CAMERA_DY` / `_DY_SLOPE` (px, px per 100 px) inject an uncalibrated + misalignment. `probe` prints the state, the applied a/b and dy before → after. + Tests (`tests_stereo_vrefine`): the fake with the SR model (+2.6 px, −0.47 px / 100 px) converges + to +0.000 px (probe matcher, 338 blocks) in one rebuild; flat / uncorrelated / vertical-stripe + frames never apply; clamps hold. **Real SR frame** (`tests/fixtures/stereo_vrefine_sr_raw.jpg`, + 62 KB, + the device calibration + 63 OpenCV SIFT matches as an independent reference): SIFT + signed dy median +1.715 px → **+0.075 px**. +- **Not in R2:** the GPU backend (seam in place), and a rectifier for a camera whose calibration + changes at runtime (the refinement's reset hook is there for it). + ## F. Risks - **Undocumented vendor channel.** The raw-camera shared memory is a reader-defined contract with diff --git a/docs/specs/extensions/XR_DXR_stereo_camera.md b/docs/specs/extensions/XR_DXR_stereo_camera.md index 8ee1034f5a..7773f530d4 100644 --- a/docs/specs/extensions/XR_DXR_stereo_camera.md +++ b/docs/specs/extensions/XR_DXR_stereo_camera.md @@ -6,7 +6,7 @@ | **Spec Version** | 1 | | **Extension Type** | Instance extension, service path only (an in-process instance enumerates zero cameras) | | **Header** | [`src/external/openxr_includes/openxr/XR_DXR_stereo_camera.h`](../../../src/external/openxr_includes/openxr/XR_DXR_stereo_camera.h) (+ its `index.json` catalog note) | -| **Status** | **R1 implemented** (runtime, hardware-free): header, plug-in slots, service camera manager, IPC, OpenXR entry points, sim_display fake, `displayxr-cli camera`, selftest check. **Not yet:** rectification (R2), consent / indicator / foreground rule (R3 — deny-by-default hooks in place), GPU transports, state-change events, the Leia provider (L1), the browser (B1). Provisional type values `1004999300–310` (relocated from `290–300` when `XR_DXR_weave` v14 took `290`; `XR_DXR_lift` holds `270–289`), pending Khronos registry | +| **Status** | **R1 implemented** (runtime, hardware-free): header, plug-in slots, service camera manager, IPC, OpenXR entry points, sim_display fake, `displayxr-cli camera`, selftest check. **R2 implemented**: the service-side rectifier (`u_stereo_rectify`, CPU) — RECTIFIED frames + rectified calibration from any CALIBRATED source, golden-tested against OpenCV, and a distorted sim fake with ground truth. **Not yet:** consent / indicator / foreground rule (R3 — deny-by-default hooks in place), GPU transports (the rectifier has the seam), state-change events, the Leia provider (L1), the browser (B1). Provisional type values `1004999300–310` (relocated from `290–300` when `XR_DXR_weave` v14 took `290`; `XR_DXR_lift` holds `270–289`), pending Khronos registry | | **R1 decisions** | stereo-only (names/structs kept open: `viewCount`, next chains) · camera provider = `xrt_plugin_iface` slots · service clients only (in-process enumerates zero) · raw frames never reach web pages (`RAW` refused to `PRESENT_OWNER` clients) — see [roadmap §G](../../roadmap/stereo-camera-source.md#g-open-questions--maintainer-decisions) | | **Decision record** | [ADR-043](../../adr/ADR-043-stereo-camera-source.md) | | **Plug-in contract** | appended `xrt_plugin_iface` camera slots, `XRT_PLUGIN_IFACE_HAS_STEREO_CAMERA` (§9) | @@ -155,13 +155,32 @@ one field box carried eleven. A plug-in that cannot resolve the active device's reports the camera without `CALIBRATED`, and the service offers RAW only. **Rectification** (when not `NATIVELY_RECTIFIED`) is the service's, vendor-neutral -(`u_stereo_rectify`, planned in `auxiliary/util`): Bouguet rectification to a **parallel** pair +(`auxiliary/util/u_stereo_rectify.{h,c}`, R2): Bouguet rectification to a **parallel** pair with zero disparity at infinity, cropped to the valid region (no black corners), per-eye size unchanged; applied as a per-pixel remap LUT built once per (calibration, size). **No convergence shear is baked in.** Placing the subject at the display plane is the receiver's decision (a per-eye crop offset of `f_px · baseline / (2 · subjectZ)`), so the source stays a physically honest parallel pair. +> **R2 as built.** The geometry is OpenCV's `stereoRectify(CALIB_ZERO_DISPARITY, alpha = 0)` +> re-implemented in C (no OpenCV dependency), golden-tested against OpenCV 4.10 fixtures +> (`tests/fixtures/`, generated offline): R1/R2 agree to 1e-12, the principal point matches the +> converged reference to 1e-4 px (OpenCV itself stops `undistortPoints` after 5 iterations and +> lands a fraction of a pixel off), the maps agree with `initUndistortRectifyMap` to 3e-5 px. Two +> deliberate differences: the alpha = 0 zoom is **verified on every border pixel** of the real +> maps and nudged up until no output pixel samples outside its raw image (OpenCV's 9-point +> inner rectangle leaves a few black pixels on strong lenses) — at most +0.13 % focal on the +> fixtures; and calibration given at a different size than the frames (`image_width/height` ≠ the +> eye size) is rescaled pixel-centre-aware. Lens models: RADTAN5, RADTAN8 (rational), KB4 +> (fisheye; same Bouguet geometry). The service rectifies **once per source frame** on the camera +> thread (outside its lock) and only while some started stream wants RECTIFIED; RAW streams read +> the original. `xrGetStereoCameraCalibrationDXR(RECTIFIED)` returns that same geometry: both +> eyes `fx = fy = f` with one principal point, model NONE, and `rightFromLeft` = identity +> rotation + `(+baseline, 0, 0)` m — so frames and numbers cannot disagree. The browser +> (`PRESENT_OWNER`) is refused a stream on a calibrated camera the rectifier rejected, instead of +> getting the RAW-flagged fallback native clients get. CPU cost per 1280×480 frame (M1 Pro, -O2): +> **0.94 ms GRAY8, 1.36 ms NV12, 2.25 ms BGRA8**; setup (geometry + both LUTs) 9–18 ms, once. + **Eye order and mirroring.** Left half = the lens on the camera's own left (as seen from behind the camera, looking at the scene) = the remote viewer's left eye. Frames are **never mirrored**; a self-view mirror is the consumer's job (mirror each half AND swap halves). @@ -452,7 +471,17 @@ owns threads, fan-out, rectification, format conversion, transport, consent and > `SIM_DISPLAY_FAKE_STEREO_CAMERA=1`, `_SIZE=WxH` (per eye), `_FPS=N`, `_FORMAT=gray8|nv12|bgra`, > `_SUSPEND_PERIOD_MS=N`, `_ADVERTISED_FPS=N` (what enumerate claims, default = `_FPS`; `0` = unknown — > exercises the service's measured-rate path), `_BASELINE_MM=N` (default 50; `120` reproduces the Leia SR -> tracking camera: the bar at 0.6 m is ~95 px). The distorted + misaligned variant below is R2's, with the rectifier. +> tracking camera: the bar at 0.6 m is ~95 px). +> +> **R2 as built:** `SIM_DISPLAY_FAKE_STEREO_CAMERA_DISTORT=1` renders the same scene through two +> RAW cameras with known ground truth (`sim_stereo_camera_truth_init`): different per-eye +> intrinsics and principal points, RADTAN5 barrel lenses (k1 −0.21 / −0.17), and each camera +> turned by half of (pitch 0.35°, yaw 0.25°, roll 0.45°) in opposite senses — 0.7° of relative +> pitch, 0.9° of relative roll. The raw pair measures |Δy| median 2.6 px, p90 5.3 px. The +> rotation split is symmetric (left `Sᵀ`, right `S`), which makes Bouguet recover **exactly** the +> virtual parallel pair the scene is defined in, so the ground truth after rectification is +> exact: rows aligned and disparity `f_rect · B / Z`. The camera then drops `NATIVELY_RECTIFIED` +> and its calibration slot returns the truth (RADTAN5, `R = S²`, `T = −S·(B, 0, 0)`, B = `_BASELINE_MM`). `SIM_DISPLAY_FAKE_STEREO_CAMERA=1` makes sim_display advertise one camera (`SHARED_WITH_EYE_TRACKING | USER_FACING | CALIBRATED | MONOCHROME`, 640×480 per eye, 30 Hz, @@ -492,6 +521,58 @@ block disparity (left x - right x): 12 px (38/54 blocks), 40 px (16/54 blocks), service stats: source 30.49 Hz, delivered 30.49 Hz, published 155, skipped 4, acquired 150, mean latency 12.417 ms ``` +R2 adds row alignment to every probe (2-D block matching of the last frame, textured 32×32 +blocks: SIGNED Δy median, |Δy| median / p90 / count above 1 px, and the dominant disparities +converted to depth with the RECTIFIED calibration), `camera calib --rectified` prints P1/P2 +and `Z = f·B/d`, and `probe --rectified` insists on RECTIFIED output and exits 5 unless the median +|Δy| ≤ 0.5 px **and** the signed median is within ±0.5 px (a constant vertical offset fails even +when the scatter is small). + +**Matching (after the first SR-hardware run).** Zero-mean NCC with parabolic sub-pixel refinement +(coarse pass on every second pixel, full-resolution refinement), blind to the two sensors' +gain/offset differences. The disparity window is **baseline-aware**: `f·B / 0.4 m` from the +camera's own calibration (quarter eye width uncalibrated, clamped to [64, 0.6 · eye]); the +vertical window is ±24 px. Blocks with peak NCC < 0.90, or whose peak sits **on** either +window's bound, are counted and excluded — a bound hit is a clamp, not a measurement. (The first +SR run read "d = 64, |dy| = 10.000" because a face at 0.6 m on the 120 mm tracker is ~95 px and +both fixed windows clamped.) Knobs: `--max-disparity N`, `--max-dy N`, `--min-ncc C`. +Against the DISTORTED fake (macOS, Debug service), default 50 mm and the SR-like 120 mm: + +``` +$ displayxr-cli camera calib 1 --rectified + left eye: 640x480 fx 453.181 fy 453.181 cx 320.426 cy 239.364 model 0 + P2 = [453.181 0 320.426 -22659.030; 0 453.181 239.364 0; 0 0 1 0] (P2[0][3] = f * Tx, Tx = -50.000 mm) +$ displayxr-cli camera probe --rectified --seconds 2 +row alignment (RECTIFIED): NCC 32x32 blocks, search d -8..64 px, dy +-24 px, accept NCC >= 0.90 + 266 textured block(s): 255 accepted, 11 below NCC 0.90, 0 AT the disparity bound, 0 AT the dy bound + dy (right - left) median +0.001 px SIGNED; |dy| median 0.016 px, p90 0.038 px, 1 block(s) > 1 px + disparity mode 1: 11.30 px (213 blocks) -> Z = f*B/d = 2.005 m + disparity mode 2: 37.81 px (40 blocks) -> Z = f*B/d = 0.599 m +--rectified: PASS — rows aligned +$ displayxr-cli camera probe --raw --seconds 2 + dy (right - left) median +0.019 px SIGNED; |dy| median 2.015 px, p90 4.026 px, 100 block(s) > 1 px +# service with SIM_DISPLAY_FAKE_STEREO_CAMERA_BASELINE_MM=120: +$ displayxr-cli camera probe --rectified --seconds 2 +row alignment (RECTIFIED): NCC 32x32 blocks, search d -8..136 px, dy +-24 px, accept NCC >= 0.90 + 266 textured block(s): 233 accepted, 33 below NCC 0.90, 0 AT the disparity bound, 0 AT the dy bound + dy (right - left) median +0.000 px SIGNED; |dy| median 0.011 px, p90 0.031 px, 1 block(s) > 1 px + disparity mode 1: 27.17 px (191 blocks) -> Z = f*B/d = 2.002 m + disparity mode 2: 90.68 px (40 blocks) -> Z = f*B/d = 0.600 m +--rectified: PASS — rows aligned +``` + +(The blocks above 1 px on the rectified pair are mismatches at the bar's occlusion edges and on +the frame counter, which is burned in raw space.) + +**Open: a +1.7 px signed Δy on real SR data after rectification.** The `[vshift]` tests in +`tests_stereo_rectify` pin what each candidate cause looks like on the 120 mm distorted fake, so +the raw frames + calibration dumps can be matched to one: a per-eye principal-point error Δcy is +a *constant* Δy ≈ −(f_rect/fy)·Δcy with small spread (flipped row convention, a crop taken for a +scale, a one-eye ±0.5 px centre convention); a transposed R is a large, roll-shaped Δy (spread +> 1 px), so a clean constant offset is *not* that; the calibration-size rescale and the plug-in's +half swap (`R' = Rᵀ`, `T' = −RᵀT`) are exact; an identity calibration maps as a pure scale about +(cx, cy) — no half-pixel offset. + Runs as a DIAG IPC client, non-elevated. `probe` goes through the full consent path (so its first run on a box raises the prompt — which is how a developer checks the prompt). `selftest` gains `stereo_camera_caps`: zero cameras passes; malformed properties (zero extent, CALIBRATED @@ -510,4 +591,5 @@ camera state change and per stream start/stop with the peer executable; INFO sta | Version | Change | |---|---| +| 1 (R2) | No API change. RECTIFIED output and rectified calibration now come from the service's rectifier for any CALIBRATED, non-natively-rectified source; the browser is refused a camera it cannot rectify; sim_display `_DISTORT=1` fake; `camera probe` row alignment + `--rectified` | | 1 | R1 implements: enumerate, calibration (RAW; RECTIFIED for natively rectified sources), streams with start/stop, latest-wins acquire over a pinned 3-slot shared-memory ring with per-stream wake handles, stream stats, plug-in slots, sim_display fake, CLI, selftest. Deferred: state-change events (structs defined, not delivered), GPU transports, rectifier (R2), consent/indicator (R3). Design scope: Enumerate + state events, calibration (raw / rectified), streams with start/stop, latest-wins acquire over a pinned 3-slot shared-memory ring with per-stream wake handles, optional GPU transports, runtime-enforced consent / foreground / indicator, plug-in iface slots, sim_display fake, CLI. | diff --git a/src/xrt/auxiliary/util/CMakeLists.txt b/src/xrt/auxiliary/util/CMakeLists.txt index 4c6484c67d..12e3914326 100644 --- a/src/xrt/auxiliary/util/CMakeLists.txt +++ b/src/xrt/auxiliary/util/CMakeLists.txt @@ -35,6 +35,10 @@ add_library( u_capture_dims.h u_stereo_camera.c u_stereo_camera.h + u_stereo_rectify.c + u_stereo_rectify.h + u_stereo_vrefine.c + u_stereo_vrefine.h u_color_encoding.h u_debug.c u_debug.h diff --git a/src/xrt/auxiliary/util/u_stereo_camera.c b/src/xrt/auxiliary/util/u_stereo_camera.c index d11af289f3..3da7c9ee92 100644 --- a/src/xrt/auxiliary/util/u_stereo_camera.c +++ b/src/xrt/auxiliary/util/u_stereo_camera.c @@ -8,7 +8,9 @@ #include "util/u_stereo_camera.h" +#include #include +#include #include @@ -427,3 +429,248 @@ u_stereo_camera_estimate_disparity(const uint8_t *gray, *out_disparity = (float)best_d + sub; return true; } + +bool +u_stereo_camera_estimate_offset(const uint8_t *gray, + uint32_t pitch, + uint32_t eye_width, + uint32_t height, + uint32_t x0, + uint32_t y0, + uint32_t w, + uint32_t h, + uint32_t max_disparity, + uint32_t max_dy, + float *out_dx, + float *out_dy) +{ + if (gray == NULL || out_dx == NULL || out_dy == NULL || w == 0 || h == 0 || x0 + w > eye_width || + y0 + h > height || y0 < max_dy || y0 + h + max_dy > height || max_dy > 32) { + return false; + } + uint32_t dmax = max_disparity < x0 ? max_disparity : x0; + if (dmax > 255) { + dmax = 255; + } + const uint32_t ny = 2 * max_dy + 1; + // SSD surface over (d, dy); a quadratic cost makes the parabola fit unbiased. + static const uint64_t k_inf = UINT64_MAX; + uint64_t *ssd = (uint64_t *)malloc(sizeof(uint64_t) * (dmax + 1) * ny); + if (ssd == NULL) { + return false; + } + uint64_t best = k_inf; + uint32_t bd = 0, by = 0; + for (uint32_t j = 0; j < ny; j++) { + int dy = (int)j - (int)max_dy; + for (uint32_t d = 0; d <= dmax; d++) { + uint64_t s = 0; + for (uint32_t y = y0; y < y0 + h; y++) { + const uint8_t *l = gray + (size_t)y * pitch + x0; + const uint8_t *r = gray + (size_t)((int)y + dy) * pitch + eye_width + x0 - d; + for (uint32_t x = 0; x < w; x++) { + int diff = (int)l[x] - (int)r[x]; + s += (uint64_t)(diff * diff); + } + } + ssd[j * (dmax + 1) + d] = s; + if (s < best) { + best = s; + bd = d; + by = j; + } + } + } + float sx = 0.0f, sy = 0.0f; + if (bd > 0 && bd < dmax) { + double a = (double)ssd[by * (dmax + 1) + bd - 1], b = (double)ssd[by * (dmax + 1) + bd], + c = (double)ssd[by * (dmax + 1) + bd + 1]; + double den = a - 2.0 * b + c; + if (den > 0.0) { + sx = (float)(0.5 * (a - c) / den); + } + } + if (by > 0 && by + 1 < ny) { + double a = (double)ssd[(by - 1) * (dmax + 1) + bd], b = (double)ssd[by * (dmax + 1) + bd], + c = (double)ssd[(by + 1) * (dmax + 1) + bd]; + double den = a - 2.0 * b + c; + if (den > 0.0) { + sy = (float)(0.5 * (a - c) / den); + } + } + free(ssd); + *out_dx = (float)bd + sx; + // A left feature at row y is found at row y + dy in the right eye. + *out_dy = (float)((int)by - (int)max_dy) + sy; + return true; +} + + +/* + * + * NCC block matcher (R2 follow-up: baseline-aware probe). + * + */ + +struct ncc_left +{ + double sum, norm; //!< sum and sqrt(n * var) of the left block's samples + uint32_t n; +}; + +static bool +ncc_left_init(const uint8_t *gray, + uint32_t pitch, + uint32_t x0, + uint32_t y0, + uint32_t w, + uint32_t h, + uint32_t step, + struct ncc_left *l) +{ + double s = 0.0, ss = 0.0; + uint32_t n = 0; + for (uint32_t j = 0; j < h; j += step) { + const uint8_t *row = gray + (size_t)(y0 + j) * pitch + x0; + for (uint32_t i = 0; i < w; i += step) { + s += row[i]; + ss += (double)row[i] * row[i]; + n++; + } + } + double var_n = ss - s * s / n; // n * variance + l->sum = s; + l->n = n; + l->norm = var_n > 0.0 ? sqrt(var_n) : 0.0; + // Flat: < 1 grey level of standard deviation matches anywhere. + return var_n >= (double)n; +} + +static double +ncc_at(const uint8_t *gray, + uint32_t pitch, + uint32_t eye_width, + uint32_t x0, + uint32_t y0, + uint32_t w, + uint32_t h, + uint32_t step, + const struct ncc_left *l, + int32_t d, + int32_t dy) +{ + uint64_t cross = 0, sr = 0, srr = 0; + for (uint32_t j = 0; j < h; j += step) { + const uint8_t *lr = gray + (size_t)(y0 + j) * pitch + x0; + const uint8_t *rr = gray + (size_t)((int64_t)y0 + j + dy) * pitch + eye_width + (int64_t)x0 - d; + for (uint32_t i = 0; i < w; i += step) { + uint32_t r = rr[i]; + cross += (uint64_t)lr[i] * r; + sr += r; + srr += (uint64_t)r * r; + } + } + double var_r = (double)srr - (double)sr * (double)sr / l->n; + if (var_r <= 0.0 || l->norm <= 0.0) { + return -1.0; + } + double num = (double)cross - l->sum * (double)sr / l->n; + return num / (l->norm * sqrt(var_r)); +} + +//! Parabola vertex offset of three samples around a MAXIMUM; 0 if not one. +static float +peak_offset(double a, double b, double c) +{ + double den = a - 2.0 * b + c; + if (den >= 0.0) { + return 0.0f; + } + double o = 0.5 * (a - c) / den; + return (float)(o < -0.5 ? -0.5 : (o > 0.5 ? 0.5 : o)); +} + +bool +u_stereo_camera_match_block(const uint8_t *gray, + uint32_t pitch, + uint32_t eye_width, + uint32_t height, + uint32_t x0, + uint32_t y0, + uint32_t w, + uint32_t h, + uint32_t max_disparity, + uint32_t max_dy, + struct u_stereo_camera_block_match *out) +{ + if (gray == NULL || out == NULL || w < 4 || h < 4 || x0 + w > eye_width || y0 + h > height) { + return false; + } + memset(out, 0, sizeof(*out)); + // Right block x = x0 - d must stay inside the right eye. + const int32_t d_hi = (int32_t)(max_disparity < x0 ? max_disparity : x0); + const uint32_t room_r = eye_width - (x0 + w); + const int32_t d_lo = -(int32_t)(room_r < 8 ? room_r : 8); + const int32_t dy_lo = -(int32_t)(max_dy < y0 ? max_dy : y0); + const uint32_t room_b = height - (y0 + h); + const int32_t dy_hi = (int32_t)(max_dy < room_b ? max_dy : room_b); + out->d_lo = d_lo; + out->d_hi = d_hi; + out->dy_lo = dy_lo; + out->dy_hi = dy_hi; + + struct ncc_left lc, lf; + if (!ncc_left_init(gray, pitch, x0, y0, w, h, 2, &lc) || !ncc_left_init(gray, pitch, x0, y0, w, h, 1, &lf)) { + return false; + } + + // Coarse: every integer (d, dy), every second pixel of the block. + double best = -2.0; + int32_t bd = 0, by = 0; + for (int32_t dy = dy_lo; dy <= dy_hi; dy++) { + for (int32_t d = d_lo; d <= d_hi; d++) { + double c = ncc_at(gray, pitch, eye_width, x0, y0, w, h, 2, &lc, d, dy); + if (c > best) { + best = c; + bd = d; + by = dy; + } + } + } + + // Fine: full resolution, hill-climb on the 3x3 neighbourhood (a few steps at + // most — the coarse peak is within a pixel or two of the true one). + double f[3][3]; + for (int iter = 0;; iter++) { + int32_t md = 0, my = 0; + double fb = -2.0; + for (int32_t j = -1; j <= 1; j++) { + for (int32_t i = -1; i <= 1; i++) { + int32_t d = bd + i, dy = by + j; + double c = (d < d_lo || d > d_hi || dy < dy_lo || dy > dy_hi) + ? -2.0 + : ncc_at(gray, pitch, eye_width, x0, y0, w, h, 1, &lf, d, dy); + f[j + 1][i + 1] = c; + if (c > fb) { + fb = c; + md = i; + my = j; + } + } + } + if ((md == 0 && my == 0) || iter >= 8) { + break; // f[][] is centred on (bd, by) + } + bd += md; + by += my; + } + + out->ncc = (float)f[1][1]; + out->dx_at_edge = bd == d_hi || bd == d_lo; + out->dy_at_edge = by == dy_hi || by == dy_lo; + float sx = (bd > d_lo && bd < d_hi) ? peak_offset(f[1][0], f[1][1], f[1][2]) : 0.0f; + float sy = (by > dy_lo && by < dy_hi) ? peak_offset(f[0][1], f[1][1], f[2][1]) : 0.0f; + out->dx = (float)bd + sx; + out->dy = (float)by + sy; + return true; +} diff --git a/src/xrt/auxiliary/util/u_stereo_camera.h b/src/xrt/auxiliary/util/u_stereo_camera.h index ab17c84ad2..f2d41db1e4 100644 --- a/src/xrt/auxiliary/util/u_stereo_camera.h +++ b/src/xrt/auxiliary/util/u_stereo_camera.h @@ -220,6 +220,73 @@ u_stereo_camera_estimate_disparity(const uint8_t *gray, uint32_t max_disparity, float *out_disparity); +/*! + * Two-dimensional version of @ref u_stereo_camera_estimate_disparity (R2): + * SSD block matching over horizontal disparity 0..@p max_disparity AND a + * vertical offset -@p max_dy..@p max_dy, sub-pixel on both axes. @p out_dy is + * the row offset of the match in the RIGHT eye (right y - left y): the + * vertical disparity a rectified pair must bring to ~0. Used by the + * rectifier's tests and `displayxr-cli camera probe --rectified`. + * @return false if the region (plus the dy search band) does not fit. + */ +bool +u_stereo_camera_estimate_offset(const uint8_t *gray, + uint32_t pitch, + uint32_t eye_width, + uint32_t height, + uint32_t x0, + uint32_t y0, + uint32_t w, + uint32_t h, + uint32_t max_disparity, + uint32_t max_dy, + float *out_dx, + float *out_dy); + +/*! + * One block's 2-D match, @ref u_stereo_camera_match_block. + * + * The edge flags are the point: a peak ON the search boundary is a clamp, not a + * measurement (the R2 probe reported "d = 64, |dy| = 10.000" on the Leia SR + * camera because both windows were too small). Callers must exclude and count + * flagged blocks, never average them in. + */ +struct u_stereo_camera_block_match +{ + float dx; //!< left x - right x, px, sub-pixel + float dy; //!< right y - left y, px, sub-pixel, SIGNED + float ncc; //!< peak zero-mean normalised cross-correlation, -1..1 + bool dx_at_edge; //!< integer peak on the disparity window's bound + bool dy_at_edge; //!< integer peak on the vertical window's bound + int32_t d_lo, d_hi; //!< the disparity window actually searched (clipped by the image) + int32_t dy_lo, dy_hi; //!< the vertical window actually searched (clipped by the image) +}; + +/*! + * Zero-mean NCC block matching of the LEFT-eye block [x0, x0+w) x [y0, y0+h) + * against the right eye over disparity -8..@p max_disparity and vertical offset + * -@p max_dy..@p max_dy (both clipped to the image), with parabolic sub-pixel + * refinement on each axis. Coarse pass on every second pixel of the block, + * then a full-resolution pass around the coarse peak. NCC, unlike SSD, is + * blind to the gain/offset differences of two real sensors, and its peak value + * is a usable confidence: callers threshold it (the field reference used + * 0.93). + * + * @return false if the block does not fit or is flat (no texture to match). + */ +bool +u_stereo_camera_match_block(const uint8_t *gray, + uint32_t pitch, + uint32_t eye_width, + uint32_t height, + uint32_t x0, + uint32_t y0, + uint32_t w, + uint32_t h, + uint32_t max_disparity, + uint32_t max_dy, + struct u_stereo_camera_block_match *out); + #ifdef __cplusplus } #endif diff --git a/src/xrt/auxiliary/util/u_stereo_rectify.c b/src/xrt/auxiliary/util/u_stereo_rectify.c new file mode 100644 index 0000000000..25fcdd62b0 --- /dev/null +++ b/src/xrt/auxiliary/util/u_stereo_rectify.c @@ -0,0 +1,721 @@ +// Copyright 2026, The DisplayXR Project +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief XR_DXR_stereo_camera (ADR-043) R2: vendor-neutral stereo + * rectification. See u_stereo_rectify.h. + * + * The geometry is Bouguet's algorithm exactly as OpenCV's cvStereoRectify + * implements it (horizontal pair, CALIB_ZERO_DISPARITY, alpha = 0), so the + * numbers can be golden-tested against OpenCV without depending on it: + * + * 1. split the relative rotation in half (each camera turns by -om/2 / +om/2 + * so both look the same way): r_r = rodrigues(-rodrigues_inv(R) / 2); + * 2. rotate both about the axis t x e_x so the baseline becomes the x axis: + * R1 = wR * r_r^T, R2 = wR * r_r; + * 3. common focal = the smaller fy (reduced for barrel lenses, as OpenCV); + * common principal point = the mean of both eyes' rectified corner + * centroids (one cy for row alignment, one cx for zero disparity at + * infinity); + * 4. alpha = 0: zoom about the principal point until the inner rectangle of + * every eye's rectified raw-image border covers the whole output — then + * verified on every border pixel of the real maps and nudged up until no + * output pixel samples outside its raw image (no black corners). + * + * @ingroup aux_util + */ + +#include "util/u_stereo_rectify.h" + +#include +#include +#include +#include + + +/* + * + * Small 3x3 helpers. + * + */ + +/* + * Flat row-major 3x3 (m[i * 3 + j]): a `double (*)[3]` -> `const double (*)[3]` + * argument is a -Wpedantic warning before C23 (GCC/MinGW), a flat pointer is not. + */ +#define M3(m) (&(m)[0][0]) + +static void +mat3_mul(const double *a, const double *b, double *out) +{ + double r[9]; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + r[i * 3 + j] = + a[i * 3 + 0] * b[0 * 3 + j] + a[i * 3 + 1] * b[1 * 3 + j] + a[i * 3 + 2] * b[2 * 3 + j]; + } + } + memcpy(out, r, sizeof(r)); +} + +static void +mat3_transpose(const double *a, double *out) +{ + double r[9]; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + r[i * 3 + j] = a[j * 3 + i]; + } + } + memcpy(out, r, sizeof(r)); +} + +static void +mat3_vec(const double *a, const double v[3], double out[3]) +{ + double r[3]; + for (int i = 0; i < 3; i++) { + r[i] = a[i * 3 + 0] * v[0] + a[i * 3 + 1] * v[1] + a[i * 3 + 2] * v[2]; + } + memcpy(out, r, sizeof(r)); +} + +//! a^T * v +static void +mat3t_vec(const double *a, const double v[3], double out[3]) +{ + double r[3]; + for (int i = 0; i < 3; i++) { + r[i] = a[0 * 3 + i] * v[0] + a[1 * 3 + i] * v[1] + a[2 * 3 + i] * v[2]; + } + memcpy(out, r, sizeof(r)); +} + +void +u_stereo_rectify_rodrigues(const double rvec[3], double out[3][3]) +{ + double th = sqrt(rvec[0] * rvec[0] + rvec[1] * rvec[1] + rvec[2] * rvec[2]); + if (th < 1e-15) { + memset(out, 0, sizeof(double) * 9); + out[0][0] = out[1][1] = out[2][2] = 1.0; + return; + } + double x = rvec[0] / th, y = rvec[1] / th, z = rvec[2] / th; + double c = cos(th), s = sin(th), C = 1.0 - c; + out[0][0] = c + x * x * C; + out[0][1] = x * y * C - z * s; + out[0][2] = x * z * C + y * s; + out[1][0] = y * x * C + z * s; + out[1][1] = c + y * y * C; + out[1][2] = y * z * C - x * s; + out[2][0] = z * x * C - y * s; + out[2][1] = z * y * C + x * s; + out[2][2] = c + z * z * C; +} + +void +u_stereo_rectify_rodrigues_inv(const double m[3][3], double out_rvec[3]) +{ + double rx = m[2][1] - m[1][2]; + double ry = m[0][2] - m[2][0]; + double rz = m[1][0] - m[0][1]; + double s = sqrt(rx * rx + ry * ry + rz * rz) * 0.5; + double c = (m[0][0] + m[1][1] + m[2][2] - 1.0) * 0.5; + c = c > 1.0 ? 1.0 : c < -1.0 ? -1.0 : c; + double th = atan2(s, c); + if (s < 1e-5) { + if (c > 0.0) { + // Near identity: theta ~ s, vec = r / 2 (first order, exact to O(th^3)). + out_rvec[0] = rx * 0.5; + out_rvec[1] = ry * 0.5; + out_rvec[2] = rz * 0.5; + return; + } + // Near pi: the axis from the diagonal. + double t[3] = {(m[0][0] + 1.0) * 0.5, (m[1][1] + 1.0) * 0.5, (m[2][2] + 1.0) * 0.5}; + double ax = sqrt(t[0] > 0 ? t[0] : 0), ay = sqrt(t[1] > 0 ? t[1] : 0), az = sqrt(t[2] > 0 ? t[2] : 0); + if (m[0][1] < 0) { + ay = -ay; + } + if (m[0][2] < 0) { + az = -az; + } + if (fabs(ax) < fabs(ay) && fabs(ax) < fabs(az) && (m[1][2] > 0) != (ay * az > 0)) { + az = -az; + } + double n = sqrt(ax * ax + ay * ay + az * az); + n = n > 0 ? th / n : 0; + out_rvec[0] = ax * n; + out_rvec[1] = ay * n; + out_rvec[2] = az * n; + return; + } + double k = th / (2.0 * s); + out_rvec[0] = rx * k; + out_rvec[1] = ry * k; + out_rvec[2] = rz * k; +} + + +/* + * + * Lens models. + * + */ + +void +u_stereo_rectify_distort(const struct u_stereo_rectify_lens *l, double x, double y, double *out_x, double *out_y) +{ + switch (l->model) { + case U_STEREO_RECTIFY_MODEL_RADTAN5: + case U_STEREO_RECTIFY_MODEL_RADTAN8: { + const double *d = l->d; + double k4 = 0, k5 = 0, k6 = 0; + if (l->model == U_STEREO_RECTIFY_MODEL_RADTAN8) { + k4 = d[5]; + k5 = d[6]; + k6 = d[7]; + } + double r2 = x * x + y * y, r4 = r2 * r2, r6 = r4 * r2; + double cdist = 1.0 + d[0] * r2 + d[1] * r4 + d[4] * r6; + double icdist = 1.0 / (1.0 + k4 * r2 + k5 * r4 + k6 * r6); + double a1 = 2.0 * x * y, a2 = r2 + 2.0 * x * x, a3 = r2 + 2.0 * y * y; + *out_x = x * cdist * icdist + d[2] * a1 + d[3] * a2; + *out_y = y * cdist * icdist + d[2] * a3 + d[3] * a1; + return; + } + case U_STEREO_RECTIFY_MODEL_KB4: { + double r = sqrt(x * x + y * y); + if (r < 1e-12) { + *out_x = x; + *out_y = y; + return; + } + double th = atan(r), t2 = th * th, t4 = t2 * t2, t6 = t4 * t2, t8 = t4 * t4; + double thd = th * (1.0 + l->d[0] * t2 + l->d[1] * t4 + l->d[2] * t6 + l->d[3] * t8); + double s = thd / r; + *out_x = x * s; + *out_y = y * s; + return; + } + default: + *out_x = x; + *out_y = y; + return; + } +} + +void +u_stereo_rectify_undistort_pixel( + const struct u_stereo_rectify_lens *l, double u, double v, double *out_x, double *out_y) +{ + double x0 = (u - l->cx) / l->fx; + double y0 = (v - l->cy) / l->fy; + switch (l->model) { + case U_STEREO_RECTIFY_MODEL_RADTAN5: + case U_STEREO_RECTIFY_MODEL_RADTAN8: { + const double *d = l->d; + double k4 = 0, k5 = 0, k6 = 0; + if (l->model == U_STEREO_RECTIFY_MODEL_RADTAN8) { + k4 = d[5]; + k5 = d[6]; + k6 = d[7]; + } + // OpenCV's fixed-point iteration, run to convergence. + double x = x0, y = y0; + for (int it = 0; it < 100; it++) { + double r2 = x * x + y * y; + double icdist = + (1.0 + ((k6 * r2 + k5) * r2 + k4) * r2) / (1.0 + ((d[4] * r2 + d[1]) * r2 + d[0]) * r2); + if (!(icdist > 0.0)) { + break; // outside the model's invertible range: keep the last estimate + } + double dx = 2.0 * d[2] * x * y + d[3] * (r2 + 2.0 * x * x); + double dy = d[2] * (r2 + 2.0 * y * y) + 2.0 * d[3] * x * y; + double nx = (x0 - dx) * icdist, ny = (y0 - dy) * icdist; + double delta = fabs(nx - x) + fabs(ny - y); + x = nx; + y = ny; + if (delta < 1e-14) { + break; + } + } + *out_x = x; + *out_y = y; + return; + } + case U_STEREO_RECTIFY_MODEL_KB4: { + double thd = sqrt(x0 * x0 + y0 * y0); + if (thd < 1e-12) { + *out_x = x0; + *out_y = y0; + return; + } + // Newton on theta(1 + k1 th^2 + ...) = thd. + double th = thd; + for (int it = 0; it < 50; it++) { + double t2 = th * th, t4 = t2 * t2, t6 = t4 * t2, t8 = t4 * t4; + double f = th * (1.0 + l->d[0] * t2 + l->d[1] * t4 + l->d[2] * t6 + l->d[3] * t8) - thd; + double df = + 1.0 + 3.0 * l->d[0] * t2 + 5.0 * l->d[1] * t4 + 7.0 * l->d[2] * t6 + 9.0 * l->d[3] * t8; + double step = f / df; + th -= step; + if (fabs(step) < 1e-15) { + break; + } + } + double s = tan(th) / thd; + *out_x = x0 * s; + *out_y = y0 * s; + return; + } + default: + *out_x = x0; + *out_y = y0; + return; + } +} + + +/* + * + * Geometry. + * + */ + +/* + * The vertical correction (u_stereo_rectify_input::v_offset / v_slope): output + * normalized row y -> uncorrected rectified row, and back. s = -1 left, +1 right. + */ +static inline double +vcorr_src(const struct u_stereo_rectify_result *r, uint32_t e, double y) +{ + const double s = e == 0 ? -0.5 : 0.5; + return y * (1.0 + s * r->v_slope) + s * r->v_offset; +} + +static inline double +vcorr_out(const struct u_stereo_rectify_result *r, uint32_t e, double y_src) +{ + const double s = e == 0 ? -0.5 : 0.5; + return (y_src - s * r->v_offset) / (1.0 + s * r->v_slope); +} + +//! RAW pixel of eye @p e -> normalized OUTPUT coordinates (correction applied). +static bool +raw_to_rect_norm(const struct u_stereo_rectify_result *r, uint32_t e, double u, double v, double *ox, double *oy) +{ + double x, y; + u_stereo_rectify_undistort_pixel(&r->raw[e], u, v, &x, &y); + double p[3] = {x, y, 1.0}, q[3]; + mat3_vec(M3(r->rot[e]), p, q); + if (q[2] <= 1e-12) { + return false; + } + *ox = q[0] / q[2]; + *oy = vcorr_out(r, e, q[1] / q[2]); + return true; +} + +//! RAW pixel of eye @p e -> rectified pixel under (f, cx, cy). false = behind. +static bool +raw_to_rect(const struct u_stereo_rectify_result *r, uint32_t e, double u, double v, double *ou, double *ov) +{ + double x, y; + if (!raw_to_rect_norm(r, e, u, v, &x, &y)) { + return false; + } + *ou = r->f * x + r->cx; + *ov = r->f * y + r->cy; + return true; +} + +bool +u_stereo_rectify_rect_from_raw( + const struct u_stereo_rectify_result *r, uint32_t eye, double u, double v, double *out_u, double *out_v) +{ + return eye < 2 && raw_to_rect(r, eye, u, v, out_u, out_v); +} + +bool +u_stereo_rectify_map_point( + const struct u_stereo_rectify_result *r, uint32_t eye, double u, double v, double *out_x, double *out_y) +{ + double p[3] = {(u - r->cx) / r->f, vcorr_src(r, eye, (v - r->cy) / r->f), 1.0}, q[3]; + mat3t_vec(M3(r->rot[eye]), p, q); // raw ray = R_e^T * rectified ray + if (q[2] <= 1e-12) { + return false; + } + double xd, yd; + u_stereo_rectify_distort(&r->raw[eye], q[0] / q[2], q[1] / q[2], &xd, &yd); + *out_x = r->raw[eye].fx * xd + r->raw[eye].cx; + *out_y = r->raw[eye].fy * yd + r->raw[eye].cy; + return true; +} + +//! Does every border pixel of both eyes' output sample inside its raw image? +static bool +border_valid(const struct u_stereo_rectify_result *r) +{ + const double wmax = (double)r->width - 1.0, hmax = (double)r->height - 1.0; + for (uint32_t e = 0; e < 2; e++) { + for (uint32_t i = 0; i < 2 * (r->width + r->height); i++) { + double u, v; + if (i < r->width) { + u = i, v = 0; + } else if (i < 2 * r->width) { + u = i - r->width, v = hmax; + } else if (i < 2 * r->width + r->height) { + u = 0, v = i - 2 * r->width; + } else { + u = wmax, v = i - 2 * r->width - r->height; + } + double x, y; + if (!u_stereo_rectify_map_point(r, e, u, v, &x, &y) || x < 0.0 || y < 0.0 || x > wmax || + y > hmax) { + return false; + } + } + } + return true; +} + +bool +u_stereo_rectify_compute(const struct u_stereo_rectify_input *in, struct u_stereo_rectify_result *out) +{ + memset(out, 0, sizeof(*out)); + if (in->width < 8 || in->height < 8) { + return false; + } + const double nx = in->width, ny = in->height; + out->width = in->width; + out->height = in->height; + out->v_offset = in->v_offset; + out->v_slope = in->v_slope; + if (!(fabs(in->v_offset) < 0.5) || !(fabs(in->v_slope) < 0.5)) { + return false; // not a small correction (or NaN) + } + + // 0. Intrinsics at the frame size (pixel centres: u' = (u + 0.5) * s - 0.5). + double sx = in->calib_width ? nx / (double)in->calib_width : 1.0; + double sy = in->calib_height ? ny / (double)in->calib_height : 1.0; + for (int e = 0; e < 2; e++) { + struct u_stereo_rectify_lens *l = &out->raw[e]; + *l = in->eye[e]; + l->fx = in->eye[e].fx * sx; + l->fy = in->eye[e].fy * sy; + l->cx = (in->eye[e].cx + 0.5) * sx - 0.5; + l->cy = (in->eye[e].cy + 0.5) * sy - 0.5; + if (!(l->fx > 0.0) || !(l->fy > 0.0) || l->model > U_STEREO_RECTIFY_MODEL_KB4) { + return false; + } + } + double nt = sqrt(in->T[0] * in->T[0] + in->T[1] * in->T[1] + in->T[2] * in->T[2]); + if (!(nt > 0.0)) { + return false; + } + out->baseline = nt; + + // 1. Half rotation each. + double om[3], r_r[3][3]; + u_stereo_rectify_rodrigues_inv(in->R, om); + for (int i = 0; i < 3; i++) { + om[i] *= -0.5; + } + u_stereo_rectify_rodrigues(om, r_r); + double t[3]; + mat3_vec(M3(r_r), in->T, t); + if (fabs(t[0]) < fabs(t[1])) { + return false; // a vertical pair: not a side-by-side camera + } + + // 2. Baseline onto the x axis. + double c = t[0]; + double uu[3] = {c > 0.0 ? 1.0 : -1.0, 0.0, 0.0}; + double ww[3] = {t[1] * uu[2] - t[2] * uu[1], t[2] * uu[0] - t[0] * uu[2], t[0] * uu[1] - t[1] * uu[0]}; + double nw = sqrt(ww[0] * ww[0] + ww[1] * ww[1] + ww[2] * ww[2]); + if (nw > 0.0) { + double k = acos(fabs(c) / nt) / nw; + for (int i = 0; i < 3; i++) { + ww[i] *= k; + } + } + double wR[3][3], r_rt[3][3]; + u_stereo_rectify_rodrigues(ww, wR); + mat3_transpose(M3(r_r), M3(r_rt)); + mat3_mul(M3(wR), M3(r_rt), M3(out->rot[0])); + mat3_mul(M3(wR), M3(r_r), M3(out->rot[1])); + mat3_vec(M3(out->rot[1]), in->T, out->t_rect); + + // 3. Common focal: the smaller fy, shrunk for barrel lenses (OpenCV). + double fc = DBL_MAX; + for (int e = 0; e < 2; e++) { + double f = out->raw[e].fy; + double dk1 = (out->raw[e].model == U_STEREO_RECTIFY_MODEL_RADTAN5 || + out->raw[e].model == U_STEREO_RECTIFY_MODEL_RADTAN8) + ? out->raw[e].d[0] + : 0.0; + if (dk1 < 0.0) { + f *= 1.0 + dk1 * (nx * nx + ny * ny) / (4.0 * f * f); + } + fc = f < fc ? f : fc; + } + if (!(fc > 0.0)) { + return false; + } + + // Common principal point: each eye's rectified corner centroid, averaged. + double ccx[2], ccy[2]; + for (uint32_t e = 0; e < 2; e++) { + const double corners[4][2] = {{0, 0}, {nx - 1, 0}, {0, ny - 1}, {nx - 1, ny - 1}}; + double ax = 0, ay = 0; + for (int k = 0; k < 4; k++) { + double x, y; + if (!raw_to_rect_norm(out, e, corners[k][0], corners[k][1], &x, &y)) { + return false; + } + ax += x; + ay += y; + } + ccx[e] = (nx - 1) * 0.5 - fc * ax * 0.25; + ccy[e] = (ny - 1) * 0.5 - fc * ay * 0.25; + } + out->cx = (ccx[0] + ccx[1]) * 0.5; + out->cy = (ccy[0] + ccy[1]) * 0.5; + out->f = fc; + + // 4. alpha = 0: the inner rectangle of each eye's rectified border (the 9x9 + // grid OpenCV's icvGetRectangles uses) must cover the whole output. + enum + { + N = 9 + }; + double s0 = 0.0; + for (uint32_t e = 0; e < 2; e++) { + double ix0 = -DBL_MAX, ix1 = DBL_MAX, iy0 = -DBL_MAX, iy1 = DBL_MAX; + for (int i = 0; i < N; i++) { + for (int j = 0; j < N; j++) { + double u, v; + if (!raw_to_rect(out, e, j * (nx - 1) / (N - 1), i * (ny - 1) / (N - 1), &u, &v)) { + return false; + } + if (j == 0 && u > ix0) { + ix0 = u; + } + if (j == N - 1 && u < ix1) { + ix1 = u; + } + if (i == 0 && v > iy0) { + iy0 = v; + } + if (i == N - 1 && v < iy1) { + iy1 = v; + } + } + } + if (!(ix0 < out->cx && out->cx < ix1 && iy0 < out->cy && out->cy < iy1)) { + return false; // the principal point is outside the valid region + } + double s = out->cx / (out->cx - ix0); + double s2 = out->cy / (out->cy - iy0); + double s3 = (nx - 1 - out->cx) / (ix1 - out->cx); + double s4 = (ny - 1 - out->cy) / (iy1 - out->cy); + s = s2 > s ? s2 : s; + s = s3 > s ? s3 : s; + s = s4 > s ? s4 : s; + s0 = s > s0 ? s : s0; + } + out->crop_scale = s0; + out->f = fc * s0; + + // The 9-point inner rectangle is an approximation of the valid region's + // inscribed rectangle: verify on every border pixel of the real maps + // and zoom a little further until no output pixel samples outside. + for (int it = 0; it < 200 && !border_valid(out); it++) { + out->crop_scale *= 1.001; + out->f = fc * out->crop_scale; + } + if (!border_valid(out)) { + return false; + } + + memset(out->P, 0, sizeof(out->P)); + for (int e = 0; e < 2; e++) { + out->P[e][0][0] = out->f; + out->P[e][0][2] = out->cx; + out->P[e][1][1] = out->f; + out->P[e][1][2] = out->cy; + out->P[e][2][2] = 1.0; + } + out->P[1][0][3] = out->f * out->t_rect[0]; + return true; +} + +uint32_t +u_stereo_rectify_build_map(const struct u_stereo_rectify_result *r, uint32_t eye, uint32_t sub, float *map_xy) +{ + if (sub == 0) { + sub = 1; + } + const uint32_t w = r->width / sub, h = r->height / sub; + const double half = (sub - 1) * 0.5; // sample centre offset in full-res pixels + uint32_t valid = 0; + for (uint32_t y = 0; y < h; y++) { + for (uint32_t x = 0; x < w; x++) { + double sxf, syf; + float *o = map_xy + 2 * ((size_t)y * w + x); + if (u_stereo_rectify_map_point(r, eye, x * (double)sub + half, y * (double)sub + half, &sxf, + &syf)) { + sxf = (sxf - half) / sub; + syf = (syf - half) / sub; + if (sxf >= 0.0 && syf >= 0.0 && sxf <= w - 1.0 && syf <= h - 1.0) { + o[0] = (float)sxf; + o[1] = (float)syf; + valid++; + continue; + } + } + o[0] = -1.0f; + o[1] = -1.0f; + } + } + return valid; +} + + +/* + * + * CPU LUT. + * + */ + +bool +u_stereo_rectify_lut_init(struct u_stereo_rectify_lut *lut, const struct u_stereo_rectify_result *r, uint32_t sub) +{ + memset(lut, 0, sizeof(*lut)); + if (sub == 0) { + sub = 1; + } + const uint32_t ew = r->width / sub, eh = r->height / sub; + if (ew < 2 || eh < 2 || 2 * ew > 32767 || eh > 32767) { + return false; + } + float *map = malloc(sizeof(float) * 2 * (size_t)ew * eh); + lut->taps = malloc(sizeof(*lut->taps) * 2 * (size_t)ew * eh); + if (map == NULL || lut->taps == NULL) { + free(map); + free(lut->taps); + lut->taps = NULL; + return false; + } + lut->width = 2 * ew; + lut->height = eh; + lut->sub = sub; + for (uint32_t e = 0; e < 2; e++) { + lut->valid += u_stereo_rectify_build_map(r, e, sub, map); + for (uint32_t y = 0; y < eh; y++) { + for (uint32_t x = 0; x < ew; x++) { + const float *m = map + 2 * ((size_t)y * ew + x); + struct u_stereo_rectify_tap *t = &lut->taps[(size_t)y * lut->width + e * ew + x]; + if (m[0] < 0.0f) { + t->sx = -1; + t->sy = -1; + t->wx = t->wy = 0; + continue; + } + int ix = (int)floorf(m[0]), iy = (int)floorf(m[1]); + int wx = (int)lrintf((m[0] - (float)ix) * 256.0f); + int wy = (int)lrintf((m[1] - (float)iy) * 256.0f); + if (wx >= 256) { + ix++, wx = 0; + } + if (wy >= 256) { + iy++, wy = 0; + } + // Keep both bilinear taps inside this eye's half. + if (ix >= (int)ew - 1) { + ix = (int)ew - 2, wx = 255; + } + if (iy >= (int)eh - 1) { + iy = (int)eh - 2, wy = 255; + } + t->sx = (int16_t)(ix + (int)(e * ew)); + t->sy = (int16_t)iy; + t->wx = (uint8_t)wx; + t->wy = (uint8_t)wy; + } + } + } + free(map); + return true; +} + +void +u_stereo_rectify_lut_fini(struct u_stereo_rectify_lut *lut) +{ + free(lut->taps); + memset(lut, 0, sizeof(*lut)); +} + +void +u_stereo_rectify_lut_apply_rows(const struct u_stereo_rectify_lut *lut, + uint32_t channels, + const uint8_t *src, + uint32_t src_pitch, + uint8_t *dst, + uint32_t dst_pitch, + uint32_t y0, + uint32_t y1) +{ + if (y1 > lut->height) { + y1 = lut->height; + } + const uint32_t W = lut->width; + for (uint32_t y = y0; y < y1; y++) { + const struct u_stereo_rectify_tap *t = lut->taps + (size_t)y * W; + uint8_t *d = dst + (size_t)y * dst_pitch; + if (channels == 1) { + for (uint32_t x = 0; x < W; x++, t++) { + if (t->sx < 0) { + d[x] = 0; + continue; + } + const uint8_t *p = src + (size_t)t->sy * src_pitch + t->sx; + uint32_t wx = t->wx, wy = t->wy; + uint32_t top = p[0] * (256 - wx) + p[1] * wx; + uint32_t bot = p[src_pitch] * (256 - wx) + p[src_pitch + 1] * wx; + d[x] = (uint8_t)((top * (256 - wy) + bot * wy + 32768u) >> 16); + } + continue; + } + for (uint32_t x = 0; x < W; x++, t++) { + uint8_t *o = d + (size_t)x * channels; + if (t->sx < 0) { + for (uint32_t ch = 0; ch < channels; ch++) { + // black: neutral chroma for NV12 UV, opaque for BGRA + o[ch] = channels == 2 ? 128 : (channels == 4 && ch == 3) ? 255 : 0; + } + continue; + } + const uint8_t *p = src + (size_t)t->sy * src_pitch + (size_t)t->sx * channels; + const uint8_t *q = p + src_pitch; + uint32_t wx = t->wx, wy = t->wy; + for (uint32_t ch = 0; ch < channels; ch++) { + uint32_t top = p[ch] * (256 - wx) + p[ch + channels] * wx; + uint32_t bot = q[ch] * (256 - wx) + q[ch + channels] * wx; + o[ch] = (uint8_t)((top * (256 - wy) + bot * wy + 32768u) >> 16); + } + } + } +} + +void +u_stereo_rectify_lut_apply(const struct u_stereo_rectify_lut *lut, + uint32_t channels, + const uint8_t *src, + uint32_t src_pitch, + uint8_t *dst, + uint32_t dst_pitch) +{ + u_stereo_rectify_lut_apply_rows(lut, channels, src, src_pitch, dst, dst_pitch, 0, lut->height); +} diff --git a/src/xrt/auxiliary/util/u_stereo_rectify.h b/src/xrt/auxiliary/util/u_stereo_rectify.h new file mode 100644 index 0000000000..dbcf09f10a --- /dev/null +++ b/src/xrt/auxiliary/util/u_stereo_rectify.h @@ -0,0 +1,242 @@ +// Copyright 2026, The DisplayXR Project +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief XR_DXR_stereo_camera (ADR-043) R2: vendor-neutral stereo + * rectification — Bouguet rectification to a parallel pair with zero + * disparity at infinity, cropped to the valid region, applied as a + * per-pixel remap LUT. No OpenCV dependency; the numbers follow + * OpenCV's `stereoRectify(..., CALIB_ZERO_DISPARITY, alpha = 0)` + + * `initUndistortRectifyMap` (golden-tested against OpenCV-generated + * fixtures in tests_stereo_rectify). + * + * Three layers, so a GPU path can replace only the last one: + * + * 1. **Geometry** — @ref u_stereo_rectify_compute: from the RAW calibration + * (per-eye K + distortion, OpenCV extrinsics `x_R = R x_L + T`) to the + * rectified rotations R1/R2, the common focal and principal point, the + * projection matrices P1/P2 and the baseline. Pure math, no allocation. + * 2. **Maps** — @ref u_stereo_rectify_map_point / @ref + * u_stereo_rectify_build_map: for each OUTPUT pixel, the RAW source + * position (float). A float map is what a GPU backend uploads (RG32F + * texture sampled bilinearly in a compute/fragment pass). + * 3. **CPU backend** — @ref u_stereo_rectify_lut: the maps packed as + * fixed-point bilinear taps over the whole side-by-side image, applied per + * plane (GRAY8 = 1 channel, NV12 = a full-res Y LUT + a half-res 2-channel + * UV LUT, BGRA8 = 4 channels). + * + * Conventions: pixel centres at integer coordinates (OpenCV), intrinsics fx + * fy cx cy with zero skew, distortion coefficients in OpenCV order + * (k1 k2 p1 p2 k3 k4 k5 k6 for RADTAN5/8; k1..k4 for KB4 fisheye). + * + * @ingroup aux_util + */ + +#pragma once + +#include "xrt/xrt_compiler.h" + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//! Lens models (values equal xrt_plugin_stereo_camera_distortion). +enum u_stereo_rectify_model +{ + U_STEREO_RECTIFY_MODEL_NONE = 0, + U_STEREO_RECTIFY_MODEL_RADTAN5 = 1, + U_STEREO_RECTIFY_MODEL_RADTAN8 = 2, + U_STEREO_RECTIFY_MODEL_KB4 = 3, +}; + +//! One eye's pinhole + lens model. +struct u_stereo_rectify_lens +{ + double fx, fy, cx, cy; + uint32_t model; //!< enum u_stereo_rectify_model + double d[8]; //!< OpenCV order +}; + +//! The RAW calibration of a pair. +struct u_stereo_rectify_input +{ + //! Per-eye size of the frames to rectify. + uint32_t width, height; + //! Per-eye size the intrinsics describe; 0 = same as the frames. When it + //! differs, K is rescaled (pixel-centre aware); distortion is unitless. + uint32_t calib_width, calib_height; + struct u_stereo_rectify_lens eye[2]; + //! OpenCV extrinsics: x_right = R * x_left + T (T in any unit, e.g. mm). + double R[3][3]; + double T[3]; + /*! + * Online vertical-alignment correction (the service's refinement, see + * u_stereo_vrefine.h); both 0 = none, the plain Bouguet result. The + * calibration leaves the RIGHT eye's rows displaced by + * D(y) = v_offset + v_slope * y (normalized rectified units, + * y = (v - cy) / f) + * relative to the left eye's. The correction is split symmetrically: the + * output row of eye e samples the uncorrected rectified row + * y_src = y * (1 + s_e * v_slope / 2) + s_e * v_offset / 2, + * s_left = -1, s_right = +1 — a per-eye vertical affine in normalized + * rectified coordinates (equivalently a per-eye rectification homography + * diag(1, 1 + s_e B/2, 1) + a row shift, folded into the maps). Rows then + * agree to second order (B^2, A*B ~ 1e-5). Because it is folded in BEFORE + * the principal point and the alpha = 0 crop are computed, the result's f / + * cx / cy / P1 / P2 describe the corrected frames: one pinhole for both + * eyes and a pure +x baseline, exactly the contract of a rectified pair. + */ + double v_offset, v_slope; +}; + +//! The rectified geometry. +struct u_stereo_rectify_result +{ + uint32_t width, height; //!< per eye, unchanged by rectification + //! RAW intrinsics rescaled to the frame size (what the maps sample). + struct u_stereo_rectify_lens raw[2]; + //! Rectified-from-raw rotation per eye (OpenCV R1, R2). + double rot[2][3][3]; + //! Rectified intrinsics shared by both eyes: fx = fy = f, one principal + //! point (CALIB_ZERO_DISPARITY: a point at infinity has zero disparity). + double f, cx, cy; + //! OpenCV P1, P2; P[1][0][3] = f * Tx (units of T; Tx < 0 when the right + //! camera is at +x). + double P[2][3][4]; + //! |T|, units of T. + double baseline; + //! Rectified translation R2 * T (≈ (-baseline, 0, 0)), units of T. + double t_rect[3]; + //! The valid-region zoom applied to f (alpha = 0): >= 1 when the lens and + //! rotations would otherwise leave black borders. + double crop_scale; + //! The vertical correction folded in (copied from the input). + double v_offset, v_slope; +}; + +/*! + * Bouguet rectification of @p in, horizontal pair, zero disparity at + * infinity, cropped so every output pixel of both eyes samples inside its raw + * image (no black corners), per-eye size unchanged, no convergence shear. + * @return false on a degenerate calibration (zero baseline, non-positive + * focal, a vertical pair, or a rotation that does not converge). + */ +bool +u_stereo_rectify_compute(const struct u_stereo_rectify_input *in, struct u_stereo_rectify_result *out); + +/*! + * Map one rectified pixel (@p u, @p v) of eye @p eye to its RAW source pixel. + * @return false when the ray points behind the raw camera. + */ +bool +u_stereo_rectify_map_point( + const struct u_stereo_rectify_result *r, uint32_t eye, double u, double v, double *out_x, double *out_y); + +/*! + * The inverse of @ref u_stereo_rectify_map_point: the rectified (output) pixel + * of eye @p eye where the RAW pixel (@p u, @p v) lands, correction included. + * @return false when the ray points behind the rectified camera. + */ +bool +u_stereo_rectify_rect_from_raw( + const struct u_stereo_rectify_result *r, uint32_t eye, double u, double v, double *out_u, double *out_v); + +/*! + * Float map of eye @p eye on a grid subsampled by @p sub (1 = full + * resolution; 2 = NV12 chroma: output sample (x, y) sits at full-res + * (2x + 0.5, 2y + 0.5) and the result is in half-res source coordinates). + * @p map_xy gets 2 floats per output sample, row-major, + * (width/sub) x (height/sub) samples. Samples whose source falls outside the + * raw image are written as (-1, -1). + * @return the number of valid samples. + */ +uint32_t +u_stereo_rectify_build_map(const struct u_stereo_rectify_result *r, uint32_t eye, uint32_t sub, float *map_xy); + +/*! + * Forward lens model: undistorted normalized (x, y) -> distorted normalized. + */ +void +u_stereo_rectify_distort(const struct u_stereo_rectify_lens *lens, double x, double y, double *out_x, double *out_y); + +/*! + * Inverse lens model: RAW pixel -> undistorted normalized coordinates + * (iterative, converged to 1e-12). + */ +void +u_stereo_rectify_undistort_pixel( + const struct u_stereo_rectify_lens *lens, double u, double v, double *out_x, double *out_y); + +//! Rotation vector (axis * angle, radians) -> matrix. +void +u_stereo_rectify_rodrigues(const double rvec[3], double out[3][3]); + +//! Rotation matrix -> rotation vector (axis * angle, radians). +void +u_stereo_rectify_rodrigues_inv(const double m[3][3], double out_rvec[3]); + + +/* + * + * CPU backend: fixed-point bilinear LUT over the whole SBS image. + * + */ + +//! One output sample: top-left source tap + 8-bit fractions. sx < 0 = black. +struct u_stereo_rectify_tap +{ + int16_t sx, sy; + uint8_t wx, wy; //!< fraction * 256, 0..255 (the 256 case folds into the next tap) +}; + +struct u_stereo_rectify_lut +{ + uint32_t width; //!< SBS width of this plane (samples) + uint32_t height; //!< rows of this plane + uint32_t sub; //!< 1 = full-res, 2 = NV12 chroma + uint32_t valid; //!< taps that sample inside the raw image + struct u_stereo_rectify_tap *taps; +}; + +/*! + * Build the SBS LUT for one plane: the left eye fills x < width/2, the right + * eye x >= width/2 (sampling its own half of the source). + * @return false on allocation failure or an extent the tap format cannot hold. + */ +bool +u_stereo_rectify_lut_init(struct u_stereo_rectify_lut *lut, const struct u_stereo_rectify_result *r, uint32_t sub); + +void +u_stereo_rectify_lut_fini(struct u_stereo_rectify_lut *lut); + +/*! + * Remap one plane of @p channels interleaved 8-bit channels (1, 2 or 4). + * @p src / @p dst are SBS planes of lut->width x lut->height samples. + */ +void +u_stereo_rectify_lut_apply(const struct u_stereo_rectify_lut *lut, + uint32_t channels, + const uint8_t *src, + uint32_t src_pitch, + uint8_t *dst, + uint32_t dst_pitch); + +/*! + * Remap rows [@p y0, @p y1) only — the unit a threaded CPU path splits on. + */ +void +u_stereo_rectify_lut_apply_rows(const struct u_stereo_rectify_lut *lut, + uint32_t channels, + const uint8_t *src, + uint32_t src_pitch, + uint8_t *dst, + uint32_t dst_pitch, + uint32_t y0, + uint32_t y1); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/auxiliary/util/u_stereo_vrefine.c b/src/xrt/auxiliary/util/u_stereo_vrefine.c new file mode 100644 index 0000000000..6240c7ca8c --- /dev/null +++ b/src/xrt/auxiliary/util/u_stereo_vrefine.c @@ -0,0 +1,592 @@ +// Copyright 2026, The DisplayXR Project +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief XR_DXR_stereo_camera (ADR-043) R2: online vertical-alignment + * refinement. See u_stereo_vrefine.h. + * @ingroup aux_util + */ + +#include "util/u_stereo_vrefine.h" + +#include +#include +#include + + +/* + * + * Measurement. + * + */ + +#define PR_HALF 5 //!< half-res patch radius (11x11) +#define PR_FULL 10 //!< full-res patch radius (21x21) +#define FINE_R 2 //!< full-res search radius around the doubled coarse peak +#define MIN_EIG 12.0 +#define MIN_CURV 0.004 + +void +u_stereo_vrefine_measure_defaults(struct u_stereo_vrefine_measure_params *p, uint32_t max_disparity) +{ + p->max_disparity = max_disparity; + p->max_dy = 10; + p->min_ncc = 0.90f; + p->max_samples = 160; +} + +//! A left patch, copied contiguous, with its sums (integer NCC). +struct lpatch +{ + uint8_t px[(2 * PR_FULL + 1) * (2 * PR_FULL + 1)]; + int32_t side; + double n, sum, var_n; //!< var_n = n * sum(l^2) - sum(l)^2 +}; + +static bool +lpatch_init(struct lpatch *lp, const uint8_t *img, uint32_t pitch, int32_t x, int32_t y, int32_t rad) +{ + const int32_t side = 2 * rad + 1; + uint32_t s = 0, s2 = 0; + for (int32_t j = 0; j < side; j++) { + const uint8_t *row = img + (size_t)(y - rad + j) * pitch + (x - rad); + uint8_t *o = lp->px + j * side; + for (int32_t i = 0; i < side; i++) { + o[i] = row[i]; + s += row[i]; + s2 += (uint32_t)row[i] * row[i]; + } + } + lp->side = side; + lp->n = (double)(side * side); + lp->sum = s; + lp->var_n = lp->n * (double)s2 - (double)s * (double)s; + return lp->var_n > 1e-6; +} + +//! sum(l * r) over the right patch centred at (x, y). Integer, vectorizable. +static uint32_t +dot_at(const struct lpatch *lp, const uint8_t *img, uint32_t pitch, int32_t x, int32_t y) +{ + const int32_t side = lp->side, rad = side / 2; + uint32_t acc = 0; + for (int32_t j = 0; j < side; j++) { + const uint8_t *row = img + (size_t)(y - rad + j) * pitch + (x - rad); + const uint8_t *l = lp->px + j * side; + uint32_t a = 0; + for (int32_t i = 0; i < side; i++) { + a += (uint32_t)l[i] * row[i]; + } + acc += a; + } + return acc; +} + +static double +ncc_from(const struct lpatch *lp, double dot, double sr, double sr2) +{ + double var_r = lp->n * sr2 - sr * sr; + if (!(var_r > 1e-6)) { + return -1.0; + } + return (lp->n * dot - lp->sum * sr) / sqrt(lp->var_n * var_r); +} + +//! NCC against the right patch at (x, y), right sums computed directly. +static double +ncc_direct(const struct lpatch *lp, const uint8_t *img, uint32_t pitch, int32_t x, int32_t y) +{ + const int32_t side = lp->side, rad = side / 2; + uint32_t s = 0, s2 = 0; + for (int32_t j = 0; j < side; j++) { + const uint8_t *row = img + (size_t)(y - rad + j) * pitch + (x - rad); + for (int32_t i = 0; i < side; i++) { + s += row[i]; + s2 += (uint32_t)row[i] * row[i]; + } + } + return ncc_from(lp, (double)dot_at(lp, img, pitch, x, y), (double)s, (double)s2); +} + +static float +parabola(double m, double c, double p) +{ + double den = m - 2.0 * c + p; + if (!(fabs(den) > 1e-12)) { + return 0.0f; + } + double o = 0.5 * (m - p) / den; + return (float)(o > 0.5 ? 0.5 : o < -0.5 ? -0.5 : o); +} + +uint32_t +u_stereo_vrefine_measure(const uint8_t *gray, + uint32_t pitch, + uint32_t eye_width, + uint32_t height, + const struct u_stereo_vrefine_measure_params *p, + struct u_stereo_vrefine_sample *out, + uint32_t cap) +{ + if (gray == NULL || p == NULL || out == NULL || cap == 0 || eye_width < 64 || height < 64) { + return 0; + } + const int32_t hw = (int32_t)(eye_width / 2), hh = (int32_t)(height / 2); + const int32_t max_d = (int32_t)p->max_disparity, max_dy = (int32_t)p->max_dy; + const int32_t max_d_h = (max_d + 1) / 2, max_dy_h = (max_dy + 1) / 2; + + // 2x-downscaled eyes (box filter) + the structure-tensor integral images. + uint8_t *half = malloc((size_t)2 * hw * hh); + double *integ = malloc(sizeof(double) * 5 * (size_t)(hw + 1) * (hh + 1)); + if (half == NULL || integ == NULL) { + free(half); + free(integ); + return 0; + } + for (int32_t e = 0; e < 2; e++) { + uint8_t *dst = half + (size_t)e * hw * hh; + const uint8_t *src = gray + (size_t)e * eye_width; + for (int32_t y = 0; y < hh; y++) { + const uint8_t *r0 = src + (size_t)(2 * y) * pitch, *r1 = r0 + pitch; + for (int32_t x = 0; x < hw; x++) { + dst[(size_t)y * hw + x] = + (uint8_t)((r0[2 * x] + r0[2 * x + 1] + r1[2 * x] + r1[2 * x + 1] + 2) >> 2); + } + } + } + const uint8_t *L = half, *R = half + (size_t)hw * hh; + const size_t iw = (size_t)hw + 1; + double *Ixx = integ, *Iyy = integ + iw * (hh + 1), *Ixy = integ + 2 * iw * (hh + 1); + double *Rs = integ + 3 * iw * (hh + 1), *Rs2 = integ + 4 * iw * (hh + 1); + memset(integ, 0, sizeof(double) * 5 * iw * (hh + 1)); + for (int32_t y = 0; y < hh; y++) { + double rxx = 0, ryy = 0, rxy = 0; + for (int32_t x = 0; x < hw; x++) { + double gx = 0, gy = 0; + if (x > 0 && x < hw - 1 && y > 0 && y < hh - 1) { + gx = 0.5 * ((double)L[(size_t)y * hw + x + 1] - L[(size_t)y * hw + x - 1]); + gy = 0.5 * ((double)L[(size_t)(y + 1) * hw + x] - L[(size_t)(y - 1) * hw + x]); + } + rxx += gx * gx; + ryy += gy * gy; + rxy += gx * gy; + size_t o = (size_t)(y + 1) * iw + (x + 1); + Ixx[o] = Ixx[o - iw] + rxx; + Iyy[o] = Iyy[o - iw] + ryy; + Ixy[o] = Ixy[o - iw] + rxy; + } + } + for (int32_t y = 0; y < hh; y++) { + double rs = 0, rs2 = 0; + for (int32_t x = 0; x < hw; x++) { + double v = R[(size_t)y * hw + x]; + rs += v; + rs2 += v * v; + size_t o = (size_t)(y + 1) * iw + (x + 1); + Rs[o] = Rs[o - iw] + rs; + Rs2[o] = Rs2[o - iw] + rs2; + } + } +#define BOX(I, x0, y0, x1, y1) \ + ((I)[(size_t)(y1)*iw + (x1)] - (I)[(size_t)(y0)*iw + (x1)] - (I)[(size_t)(y1)*iw + (x0)] + \ + (I)[(size_t)(y0)*iw + (x0)]) + + // Grid of cells, one best Shi-Tomasi patch per cell. The patch must leave + // room for the full-resolution refinement too. + const int32_t margin = PR_HALF + 2; + const int32_t ux0 = margin, ux1 = hw - margin, uy0 = margin, uy1 = hh - margin; + uint32_t want = p->max_samples > 0 ? p->max_samples : 160; + double area = (double)(ux1 - ux0) * (double)(uy1 - uy0); + int32_t cs = (int32_t)sqrt(area / (double)want); + if (cs < 2 * PR_HALF + 1) { + cs = 2 * PR_HALF + 1; + } + const int32_t side_h = 2 * PR_HALF + 1; + struct lpatch lp_h, lp_f; + double fine[2 * FINE_R + 3][2 * FINE_R + 3]; + uint32_t n_out = 0; + + for (int32_t cy0 = uy0; cy0 + cs <= uy1 && n_out < cap; cy0 += cs) { + for (int32_t cx0 = ux0; cx0 + cs <= ux1 && n_out < cap; cx0 += cs) { + // Best corner in the cell (stride 2). + double best = MIN_EIG; + int32_t bx = -1, by = -1; + for (int32_t y = cy0; y < cy0 + cs; y += 2) { + for (int32_t x = cx0; x < cx0 + cs; x += 2) { + int32_t x0 = x - PR_HALF, y0 = y - PR_HALF, x1 = x + PR_HALF + 1, + y1 = y + PR_HALF + 1; + double a = BOX(Ixx, x0, y0, x1, y1) / (side_h * side_h); + double c = BOX(Iyy, x0, y0, x1, y1) / (side_h * side_h); + double b = BOX(Ixy, x0, y0, x1, y1) / (side_h * side_h); + double mn = 0.5 * (a + c) - sqrt(0.25 * (a - c) * (a - c) + b * b); + if (mn > best) { + best = mn; + bx = x; + by = y; + } + } + } + if (bx < 0) { + continue; + } + + // Coarse: half resolution, every integer (d, dy) in the window. + if (!lpatch_init(&lp_h, L, (uint32_t)hw, bx, by, PR_HALF)) { + continue; + } + int32_t d_lo = -1, d_hi = max_d_h; + if (d_hi > bx - PR_HALF) { + d_hi = bx - PR_HALF; // right patch x = bx - d must stay inside + } + if (d_lo < bx + PR_HALF - (hw - 1)) { + d_lo = bx + PR_HALF - (hw - 1); + } + int32_t y_lo = -max_dy_h, y_hi = max_dy_h; + if (by + y_lo - PR_HALF < 0) { + y_lo = PR_HALF - by; + } + if (by + y_hi + PR_HALF > hh - 1) { + y_hi = hh - 1 - PR_HALF - by; + } + if (d_hi - d_lo < 2 || y_hi - y_lo < 2) { + continue; + } + double cbest = -2.0; + int32_t cd = 0, cdy = 0; + for (int32_t dy = y_lo; dy <= y_hi; dy++) { + for (int32_t d = d_lo; d <= d_hi; d++) { + const int32_t rx = bx - d, ry = by + dy; + const int32_t x0 = rx - PR_HALF, y0 = ry - PR_HALF, x1 = rx + PR_HALF + 1, + y1 = ry + PR_HALF + 1; + double c = ncc_from(&lp_h, (double)dot_at(&lp_h, R, (uint32_t)hw, rx, ry), + BOX(Rs, x0, y0, x1, y1), BOX(Rs2, x0, y0, x1, y1)); + if (c > cbest) { + cbest = c; + cd = d; + cdy = dy; + } + } + } + if (cbest < p->min_ncc - 0.15 || cd == d_lo || cd == d_hi || cdy == y_lo || cdy == y_hi) { + continue; // weak, or a clamp on the window: not a measurement + } + + // Fine: full resolution around the doubled peak. + const int32_t fx = 2 * bx, fy = 2 * by; // full-res left patch centre + if (fx - PR_FULL < 0 || fx + PR_FULL >= (int32_t)eye_width || fy - PR_FULL < 0 || + fy + PR_FULL >= (int32_t)height) { + continue; + } + const uint8_t *Rf = gray + eye_width; + if (!lpatch_init(&lp_f, gray, pitch, fx, fy, PR_FULL)) { + continue; + } + int32_t pd = 2 * cd, pdy = 2 * cdy; + for (int iter = 0; iter < 4; iter++) { + double fb = -2.0; + int32_t md = 0, mdy = 0; + for (int32_t j = -FINE_R - 1; j <= FINE_R + 1; j++) { + for (int32_t i = -FINE_R - 1; i <= FINE_R + 1; i++) { + int32_t d = pd + i, dy = pdy + j; + int32_t rx = fx - d, ry = fy + dy; + double c = -2.0; + if (d >= -2 && d <= max_d && dy >= -max_dy && dy <= max_dy && + rx - PR_FULL >= 0 && rx + PR_FULL < (int32_t)eye_width && + ry - PR_FULL >= 0 && ry + PR_FULL < (int32_t)height) { + c = ncc_direct(&lp_f, Rf, pitch, rx, ry); + } + fine[j + FINE_R + 1][i + FINE_R + 1] = c; + if (abs(i) <= FINE_R && abs(j) <= FINE_R && c > fb) { + fb = c; + md = i; + mdy = j; + } + } + } + if (abs(md) < FINE_R && abs(mdy) < FINE_R) { + pd += md; + pdy += mdy; + // Re-centre the 3x3 on the peak (still inside the table). + const int32_t cj = mdy + FINE_R + 1, ci = md + FINE_R + 1; + double c = fine[cj][ci], up = fine[cj - 1][ci], dn = fine[cj + 1][ci]; + double lf = fine[cj][ci - 1], rt = fine[cj][ci + 1]; + bool nbr_ok = up > -1.5 && dn > -1.5 && lf > -1.5 && rt > -1.5; + if (!nbr_ok || c < p->min_ncc || (2.0 * c - up - dn) < MIN_CURV || + (2.0 * c - lf - rt) < MIN_CURV) { + break; + } + float sx = parabola(lf, c, rt), sy = parabola(up, c, dn); + // d = left x - right x: the right patch at x - (pd + i), + // a higher score at i = +1 means a larger d. + float d = (float)pd + sx; + float dyv = (float)pdy + sy; + if (d > 0.5f) { + struct u_stereo_vrefine_sample *o = &out[n_out++]; + o->x = (float)fx; + o->y = (float)fy; + o->d = d; + o->dy = dyv; + o->ncc = (float)c; + } + break; + } + pd += md; + pdy += mdy; // peak on the fine window's edge: move and retry + } + } + } +#undef BOX + free(half); + free(integ); + return n_out; +} + + +/* + * + * Robust fit. + * + */ + +static int +cmp_double(const void *a, const void *b) +{ + double x = *(const double *)a, y = *(const double *)b; + return x < y ? -1 : x > y ? 1 : 0; +} + +static double +median_of(double *v, uint32_t n) +{ + qsort(v, n, sizeof(double), cmp_double); + return n % 2 ? v[n / 2] : 0.5 * (v[n / 2 - 1] + v[n / 2]); +} + +bool +u_stereo_vrefine_fit_samples(const struct u_stereo_vrefine_sample *s, + uint32_t n, + double cy, + struct u_stereo_vrefine_fit *out) +{ + memset(out, 0, sizeof(*out)); + if (s == NULL || n < 8) { + return false; + } + double *tmp = malloc(sizeof(double) * n); + double *w = malloc(sizeof(double) * n); + if (tmp == NULL || w == NULL) { + free(tmp); + free(w); + return false; + } + for (uint32_t i = 0; i < n; i++) { + tmp[i] = s[i].dy; + } + out->median_dy = median_of(tmp, n); + + double ym = 0.0, yv = 0.0; + for (uint32_t i = 0; i < n; i++) { + ym += s[i].y; + } + ym /= n; + for (uint32_t i = 0; i < n; i++) { + yv += (s[i].y - ym) * (s[i].y - ym); + } + out->slope_determined = sqrt(yv / n) >= 40.0; + + double a = out->median_dy, b = 0.0, sigma = 0.1; + for (int it = 0; it < 10; it++) { + for (uint32_t i = 0; i < n; i++) { + tmp[i] = fabs(s[i].dy - (a + b * (s[i].y - cy))); + } + sigma = 1.4826 * median_of(tmp, n); + if (sigma < 0.1) { + sigma = 0.1; + } + const double c = 4.685 * sigma; + double sw = 0, swy = 0, swyy = 0, swd = 0, swyd = 0; + for (uint32_t i = 0; i < n; i++) { + double y = s[i].y - cy; + double r = s[i].dy - (a + b * y); + double u = r / c; + w[i] = fabs(u) < 1.0 ? (1.0 - u * u) * (1.0 - u * u) : 0.0; + sw += w[i]; + swy += w[i] * y; + swyy += w[i] * y * y; + swd += w[i] * s[i].dy; + swyd += w[i] * y * s[i].dy; + } + if (!(sw > 0.0)) { + break; + } + double na, nb; + double det = sw * swyy - swy * swy; + if (out->slope_determined && det > 1e-9 * sw * sw) { + nb = (sw * swyd - swy * swd) / det; + na = (swd - nb * swy) / sw; + } else { + nb = 0.0; + na = swd / sw; + } + bool conv = fabs(na - a) < 1e-4 && fabs(nb - b) < 1e-7; + a = na; + b = nb; + if (conv) { + break; + } + } + uint32_t inl = 0; + const double c = 4.685 * sigma; + for (uint32_t i = 0; i < n; i++) { + if (fabs(s[i].dy - (a + b * (s[i].y - cy))) < c) { + inl++; + } + } + out->a = a; + out->b = b; + out->sigma = sigma; + out->inliers = inl; + free(tmp); + free(w); + return true; +} + + +/* + * + * Controller. + * + */ + +void +u_stereo_vrefine_config_defaults(struct u_stereo_vrefine_config *c, uint32_t height, double cy) +{ + memset(c, 0, sizeof(*c)); + c->height = height; + c->cy = cy; + c->min_matches = 50; + c->min_frames = 3; + c->max_frames = 12; + c->deadband_px = 0.2; + c->max_offset_px = 6.0; + c->max_slope = 0.02; + c->fast_period_ns = 250ll * 1000 * 1000; + c->fast_phase_ns = 5000ll * 1000 * 1000; + c->slow_period_ns = 30000ll * 1000 * 1000; +} + +void +u_stereo_vrefine_init(struct u_stereo_vrefine *r, const struct u_stereo_vrefine_config *cfg) +{ + memset(r, 0, sizeof(*r)); + r->cfg = *cfg; +} + +void +u_stereo_vrefine_reset(struct u_stereo_vrefine *r) +{ + struct u_stereo_vrefine_config cfg = r->cfg; + u_stereo_vrefine_init(r, &cfg); +} + +void +u_stereo_vrefine_restart(struct u_stereo_vrefine *r, int64_t now_ns) +{ + r->win_n = 0; + r->win_frames = 0; + r->started = true; + r->phase_start_ns = now_ns; + r->next_due_ns = now_ns; +} + +bool +u_stereo_vrefine_due(struct u_stereo_vrefine *r, int64_t now_ns) +{ + if (!r->started) { + u_stereo_vrefine_restart(r, now_ns); + } + return now_ns >= r->next_due_ns; +} + +static double +clampd(double v, double lim) +{ + return v > lim ? lim : v < -lim ? -lim : v; +} + +static void +schedule_after_window(struct u_stereo_vrefine *r, int64_t now_ns) +{ + bool fast = now_ns - r->phase_start_ns < r->cfg.fast_phase_ns; + r->next_due_ns = now_ns + (fast ? r->cfg.fast_period_ns : r->cfg.slow_period_ns); + r->win_n = 0; + r->win_frames = 0; +} + +enum u_stereo_vrefine_result +u_stereo_vrefine_push(struct u_stereo_vrefine *r, int64_t now_ns, const struct u_stereo_vrefine_sample *s, uint32_t n) +{ + if (!r->started) { + u_stereo_vrefine_restart(r, now_ns); + } + for (uint32_t i = 0; i < n && r->win_n < U_STEREO_VREFINE_WINDOW_CAP; i++) { + r->win[r->win_n++] = s[i]; + } + r->win_frames++; + + if (r->win_n < r->cfg.min_matches || r->win_frames < r->cfg.min_frames) { + if (r->win_frames >= r->cfg.max_frames) { + schedule_after_window(r, now_ns); + return U_STEREO_VREFINE_DROPPED; + } + r->next_due_ns = now_ns + r->cfg.fast_period_ns; + return U_STEREO_VREFINE_ACCUMULATING; + } + + struct u_stereo_vrefine_fit fit; + if (!u_stereo_vrefine_fit_samples(r->win, r->win_n, r->cfg.cy, &fit) || fit.inliers < r->cfg.min_matches || + !isfinite(fit.a) || !isfinite(fit.b)) { + schedule_after_window(r, now_ns); + return U_STEREO_VREFINE_DROPPED; + } + r->windows++; + r->last_residual_dy = fit.median_dy; + r->last_matches = fit.inliers; + if (!r->have_initial) { + r->have_initial = true; + // The first window after a reset is measured without any correction. + r->initial_dy = fit.median_dy; + r->initial_a = fit.a; + r->initial_b = fit.b; + } + + double na = clampd(r->a + fit.a, r->cfg.max_offset_px); + double nb = clampd(r->b + fit.b, r->cfg.max_slope); + double extent = + r->cfg.cy > (double)r->cfg.height - 1.0 - r->cfg.cy ? r->cfg.cy : (double)r->cfg.height - 1.0 - r->cfg.cy; + double effect = fabs(na - r->a) + fabs(nb - r->b) * extent; + enum u_stereo_vrefine_result res = U_STEREO_VREFINE_STABLE; + if (effect > r->cfg.deadband_px) { + r->prev_a = r->a; + r->prev_b = r->b; + r->a = na; + r->b = nb; + r->applied = true; + r->updates++; + r->phase_start_ns = now_ns; // unstable: back to the settling cadence + res = U_STEREO_VREFINE_UPDATED; + } + schedule_after_window(r, now_ns); + return res; +} + +void +u_stereo_vrefine_revert(struct u_stereo_vrefine *r) +{ + r->a = r->prev_a; + r->b = r->prev_b; + if (r->updates > 0) { + r->updates--; + } + r->applied = r->updates > 0 || r->a != 0.0 || r->b != 0.0; +} diff --git a/src/xrt/auxiliary/util/u_stereo_vrefine.h b/src/xrt/auxiliary/util/u_stereo_vrefine.h new file mode 100644 index 0000000000..38fd8494cb --- /dev/null +++ b/src/xrt/auxiliary/util/u_stereo_vrefine.h @@ -0,0 +1,203 @@ +// Copyright 2026, The DisplayXR Project +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief XR_DXR_stereo_camera (ADR-043) R2: online VERTICAL-ALIGNMENT + * refinement of a service-rectified pair. + * + * Why: a device's stored calibration can be slightly off for the frames it + * actually delivers. On a Leia SR laptop the rectifier (and OpenCV's own + * stereoRectify, on the same frame and the same calibration) left a signed + * vertical residual of +1.71 px, fitted as dy = +2.6 px at the centre with a + * -0.47 px / 100 px slope in y (a small pitch / vertical-scale mismatch). The + * x term was +0.04 px / 100 px (<= 0.15 px at the image edge, under the match + * noise), so the model is + * + * dy(y) = a + b * (y - cy) (right row - left row, output pixels) + * + * with no roll term. It is measured on the RECTIFIED frames the service + * delivers, so every measurement is a RESIDUAL of the correction already + * applied, and the controller integrates it (a closed loop: new total = + * applied + residual). The correction is folded into the rectification maps + * as u_stereo_rectify_input::v_offset = a / f, v_slope = b (see there for the + * math); frames and the rectified calibration therefore stay consistent. + * + * Two layers, both pure (no threads, no OS) so they are unit-tested: + * + * 1. **Measurement** — @ref u_stereo_vrefine_measure: Shi-Tomasi-selected + * patches of the left eye (one per grid cell, on a 2x-downscaled copy), + * zero-mean NCC search over disparity x vertical offset at half + * resolution, then a full-resolution NCC refinement +-2 px around it with + * parabolic sub-pixel peaks. A match is kept only with NCC >= min_ncc, a + * positive disparity, a peak strictly inside both windows, and a real + * vertical curvature (no aperture-problem edges). + * 2. **Controller** — @ref u_stereo_vrefine: duty cycle, accumulation over + * frames, robust fit (median start, Tukey IRLS), clamps, the >= N matches + * over >= F frames gate and the deadband that decides when maps rebuild. + * + * @ingroup aux_util + */ + +#pragma once + +#include "xrt/xrt_compiler.h" + +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif + +//! One matched feature (full-resolution output pixels). +struct u_stereo_vrefine_sample +{ + float x, y; //!< left-eye position (patch centre) + float d; //!< horizontal disparity, left x - right x + float dy; //!< right y - left y, SIGNED + float ncc; +}; + +struct u_stereo_vrefine_measure_params +{ + uint32_t max_disparity; //!< full-res px; the search is -2..max + uint32_t max_dy; //!< full-res px; the search is +-max_dy + float min_ncc; //!< 0.90 is a good default + uint32_t max_samples; //!< patches tried per frame (grid cells) +}; + +//! Defaults for a pair of @p eye_width px per eye whose nearest subject gives +//! at most @p max_disparity px. +void +u_stereo_vrefine_measure_defaults(struct u_stereo_vrefine_measure_params *p, uint32_t max_disparity); + +/*! + * Match features between the halves of a rectified GRAY8 side-by-side image + * (left eye x < eye_width). Allocates its own scratch (called off the frame + * path). @return the number of samples written (<= @p cap). + */ +uint32_t +u_stereo_vrefine_measure(const uint8_t *gray, + uint32_t pitch, + uint32_t eye_width, + uint32_t height, + const struct u_stereo_vrefine_measure_params *p, + struct u_stereo_vrefine_sample *out, + uint32_t cap); + +//! A robust fit of dy = a + b * (y - cy). +struct u_stereo_vrefine_fit +{ + double a, b; //!< px, px per px + double median_dy; //!< signed median of the samples + double sigma; //!< robust residual scale, px + uint32_t inliers; //!< samples with a non-zero final weight + bool slope_determined; //!< false = rows too clustered, b fixed at 0 +}; + +/*! + * Median start, then Tukey-biweight IRLS. @p cy is the pivot row. The slope is + * only fitted when the samples span enough rows (std >= 40 px); else b = 0. + * @return false with fewer than 8 samples. + */ +bool +u_stereo_vrefine_fit_samples(const struct u_stereo_vrefine_sample *s, + uint32_t n, + double cy, + struct u_stereo_vrefine_fit *out); + + +/* + * + * Controller. + * + */ + +#define U_STEREO_VREFINE_WINDOW_CAP 1024 //!< samples one window holds + +struct u_stereo_vrefine_config +{ + uint32_t height; //!< per-eye rows (the deadband's extent) + double cy; //!< the rectified principal point row + uint32_t min_matches; //!< per window, default 50 + uint32_t min_frames; //!< per window, default 3 + uint32_t max_frames; //!< a window that has not filled by then is dropped, default 12 + double deadband_px; //!< rebuild when the correction moves by more, default 0.2 + double max_offset_px; //!< |a| clamp, default 6 + double max_slope; //!< |b| clamp, px per px, default 0.02 (2 px / 100 px) + int64_t fast_period_ns; //!< sampling period while settling, default 250 ms + int64_t fast_phase_ns; //!< settling phase after a (re)start or an update, default 5 s + int64_t slow_period_ns; //!< then one window every, default 30 s +}; + +void +u_stereo_vrefine_config_defaults(struct u_stereo_vrefine_config *c, uint32_t height, double cy); + +enum u_stereo_vrefine_result +{ + U_STEREO_VREFINE_ACCUMULATING = 0, //!< window not full yet + U_STEREO_VREFINE_STABLE = 1, //!< window fitted, residual within the deadband + U_STEREO_VREFINE_UPDATED = 2, //!< window fitted, new correction: rebuild the maps + U_STEREO_VREFINE_DROPPED = 3, //!< window expired without enough matches +}; + +struct u_stereo_vrefine +{ + struct u_stereo_vrefine_config cfg; + + //! The correction (total, output px) the maps currently carry. + double a, b; + bool applied; //!< a correction was ever applied since the last reset + uint32_t updates; //!< UPDATED results since the last reset + //! The previous correction, for u_stereo_vrefine_revert(). + double prev_a, prev_b; + + // The window. + struct u_stereo_vrefine_sample win[U_STEREO_VREFINE_WINDOW_CAP]; + uint32_t win_n; + uint32_t win_frames; + + // Schedule. + int64_t phase_start_ns; + int64_t next_due_ns; + bool started; + + // Stats for logs / the probe. + bool have_initial; + double initial_dy; //!< signed median dy of the first fitted window (pre-correction) + double initial_a, initial_b; + double last_residual_dy; //!< signed median dy of the latest fitted window + uint32_t last_matches; //!< inliers of the latest fit + uint32_t windows; //!< fitted windows since the last reset +}; + +//! Zero correction, settling phase from the first due() call. +void +u_stereo_vrefine_init(struct u_stereo_vrefine *r, const struct u_stereo_vrefine_config *cfg); + +//! Drop the correction and the window (calibration change): back to zero. +void +u_stereo_vrefine_reset(struct u_stereo_vrefine *r); + +//! Keep the correction, drop the window, re-enter the settling phase (camera re-open). +void +u_stereo_vrefine_restart(struct u_stereo_vrefine *r, int64_t now_ns); + +//! Should the frame at @p now_ns be measured? +bool +u_stereo_vrefine_due(struct u_stereo_vrefine *r, int64_t now_ns); + +/*! + * Feed one measured frame. On UPDATED, @ref a / @ref b hold the new total + * (clamped) correction for the maps. + */ +enum u_stereo_vrefine_result +u_stereo_vrefine_push(struct u_stereo_vrefine *r, int64_t now_ns, const struct u_stereo_vrefine_sample *s, uint32_t n); + +//! The maps could not be rebuilt with the last UPDATED correction: undo it. +void +u_stereo_vrefine_revert(struct u_stereo_vrefine *r); + +#ifdef __cplusplus +} +#endif diff --git a/src/xrt/drivers/sim_display/sim_display_stereo_camera.c b/src/xrt/drivers/sim_display/sim_display_stereo_camera.c index 9908a35015..caf56718aa 100644 --- a/src/xrt/drivers/sim_display/sim_display_stereo_camera.c +++ b/src/xrt/drivers/sim_display/sim_display_stereo_camera.c @@ -39,6 +39,12 @@ struct fake_config double baseline_mm; //!< 50 mm default; 120 mm reproduces the Leia SR tracking camera's disparities uint32_t format; int64_t suspend_period_ns; + //! R2: raw, distorted, misaligned pair with known ground truth (not + //! NATIVELY_RECTIFIED; the service's rectifier does the work). + bool distort; + //! R2 refinement: an UNCALIBRATED extra vertical offset (px) + slope (px + //! per 100 px) of the right eye on the distorted pair. + double extra_dy_px, extra_dy_slope_100; }; //! Completes the plug-in-owned opaque type. @@ -46,6 +52,7 @@ struct xrt_plugin_stereo_camera { struct fake_config cfg; struct sim_stereo_camera_scene scene; + struct sim_stereo_camera_distorted distorted; //!< cfg.distort only uint8_t *gray; uint32_t gray_pitch; uint8_t *converted; @@ -103,10 +110,27 @@ fake_config(void) if (sp != NULL && atol(sp) > 0) { cfg.suspend_period_ns = (int64_t)atol(sp) * 1000000; } + const char *dist = getenv("SIM_DISPLAY_FAKE_STEREO_CAMERA_DISTORT"); + cfg.distort = dist != NULL && dist[0] != '\0' && dist[0] != '0'; + const char *edy = getenv("SIM_DISPLAY_FAKE_STEREO_CAMERA_DY"); + if (edy != NULL && fabs(atof(edy)) <= 20.0) { + cfg.extra_dy_px = atof(edy); + } + const char *eds = getenv("SIM_DISPLAY_FAKE_STEREO_CAMERA_DY_SLOPE"); + if (eds != NULL && fabs(atof(eds)) <= 5.0) { + cfg.extra_dy_slope_100 = atof(eds); + } if (cfg.enabled) { - U_LOG_W("sim_display: FAKE stereo camera ON — %ux%u per eye @ %.1f Hz, baseline %.1f mm, format %u%s", + U_LOG_W("sim_display: FAKE stereo camera ON — %ux%u per eye @ %.1f Hz, baseline %.1f mm, format %u%s%s", cfg.eye_w, cfg.eye_h, cfg.fps, cfg.baseline_mm, cfg.format, - cfg.suspend_period_ns > 0 ? ", suspend square-wave" : ""); + cfg.suspend_period_ns > 0 ? ", suspend square-wave" : "", + cfg.distort ? ", DISTORTED raw pair (rectified by the service)" : ""); + if (cfg.distort && (cfg.extra_dy_px != 0.0 || cfg.extra_dy_slope_100 != 0.0)) { + U_LOG_W( + "sim_display: FAKE stereo camera — UNCALIBRATED vertical misalignment of the right eye: " + "%+.2f px %+.3f px/100 px (the service's refinement should remove it)", + cfg.extra_dy_px, cfg.extra_dy_slope_100); + } } return &cfg; } @@ -135,7 +159,10 @@ sim_display_stereo_camera_enumerate(struct xrt_plugin_instance *inst, snprintf(info.display_name, sizeof(info.display_name), "Sim 3D camera (fake)"); snprintf(info.device_identity, sizeof(info.device_identity), "sim-display-fake-stereo-camera-0001"); info.flags = XRT_PLUGIN_STEREO_CAMERA_SHARED_WITH_EYE_TRACKING | XRT_PLUGIN_STEREO_CAMERA_USER_FACING | - XRT_PLUGIN_STEREO_CAMERA_CALIBRATED | XRT_PLUGIN_STEREO_CAMERA_NATIVELY_RECTIFIED; + XRT_PLUGIN_STEREO_CAMERA_CALIBRATED; + if (!cfg->distort) { + info.flags |= XRT_PLUGIN_STEREO_CAMERA_NATIVELY_RECTIFIED; + } if (cfg->format == XRT_PLUGIN_STEREO_CAMERA_FORMAT_GRAY8) { info.flags |= XRT_PLUGIN_STEREO_CAMERA_MONOCHROME; } @@ -166,6 +193,27 @@ sim_display_stereo_camera_get_calibration(struct xrt_plugin_instance *inst, c.struct_size = (uint32_t)sizeof(c); c.image_width = cfg->eye_w; c.image_height = cfg->eye_h; + if (cfg->distort) { + // The raw pair's ground truth (sim_display_stereo_camera_pattern.h). + struct sim_stereo_camera_truth t; + sim_stereo_camera_truth_init(&t, cfg->eye_w, cfg->eye_h, fake_fx(cfg->eye_w), cfg->baseline_mm, + FAKE_BG_DEPTH_M, FAKE_BAR_DEPTH_M); + for (int e = 0; e < 2; e++) { + c.k[e][0] = t.fx[e]; + c.k[e][1] = t.fy[e]; + c.k[e][2] = t.cx[e]; + c.k[e][3] = t.cy[e]; + for (int k = 0; k < 5; k++) { + c.distortion[e][k] = t.dist[e][k]; + } + } + c.distortion_model = XRT_PLUGIN_STEREO_CAMERA_DISTORTION_RADTAN5; + memcpy(c.rotation_right_from_left, t.R, sizeof(t.R)); + memcpy(c.translation_right_from_left_mm, t.T_mm, sizeof(t.T_mm)); + memcpy(out, &c, sz < sizeof(c) ? sz : sizeof(c)); + out->struct_size = sz; + return XRT_SUCCESS; + } double fx = fake_fx(cfg->eye_w); for (int e = 0; e < 2; e++) { c.k[e][0] = fx; @@ -202,6 +250,16 @@ sim_display_stereo_camera_open(struct xrt_plugin_instance *inst, cam->cfg = *cfg; sim_stereo_camera_scene_init(&cam->scene, cfg->eye_w, cfg->eye_h, fake_fx(cfg->eye_w), cfg->baseline_mm, FAKE_BG_DEPTH_M, FAKE_BAR_DEPTH_M); + if (cfg->distort && + !sim_stereo_camera_distorted_init(&cam->distorted, cfg->eye_w, cfg->eye_h, fake_fx(cfg->eye_w), + cfg->baseline_mm, FAKE_BG_DEPTH_M, FAKE_BAR_DEPTH_M)) { + free(cam); + return XRT_ERROR_ALLOCATION; + } + if (cfg->distort) { + sim_stereo_camera_distorted_set_vmisalign(&cam->distorted, cfg->extra_dy_px, + cfg->extra_dy_slope_100 / 100.0); + } cam->gray_pitch = 2 * cfg->eye_w; cam->gray = calloc((size_t)cam->gray_pitch * cfg->eye_h, 1); if (cfg->format != XRT_PLUGIN_STEREO_CAMERA_FORMAT_GRAY8) { @@ -211,14 +269,16 @@ sim_display_stereo_camera_open(struct xrt_plugin_instance *inst, if (cam->gray == NULL || (cfg->format != XRT_PLUGIN_STEREO_CAMERA_FORMAT_GRAY8 && cam->converted == NULL)) { free(cam->gray); free(cam->converted); + sim_stereo_camera_distorted_fini(&cam->distorted); free(cam); return XRT_ERROR_ALLOCATION; } cam->period_ns = (int64_t)(1e9 / cfg->fps); cam->t0_ns = os_monotonic_get_ns(); cam->open_ns = cam->t0_ns; - U_LOG_W("sim_display: FAKE stereo camera opened (bg disparity %u px, bar disparity %u px)", - cam->scene.bg_disparity, cam->scene.bar_disparity); + U_LOG_W("sim_display: FAKE stereo camera opened (%s; bg disparity %.2f px, bar disparity %.2f px in the %s)", + cfg->distort ? "DISTORTED raw pair" : "parallel pair", cam->scene.bg_disparity_f, + cam->scene.bar_disparity_f, cfg->distort ? "virtual parallel frame" : "frame"); *out_cam = cam; return XRT_SUCCESS; } @@ -260,7 +320,11 @@ sim_display_stereo_camera_wait_frame(struct xrt_plugin_stereo_camera *cam, sleep_ns(due - now); cam->seq++; - sim_stereo_camera_render_gray(&cam->scene, cam->seq, cam->gray, cam->gray_pitch); + if (cam->cfg.distort) { + sim_stereo_camera_render_gray_distorted(&cam->distorted, cam->seq, cam->gray, cam->gray_pitch); + } else { + sim_stereo_camera_render_gray(&cam->scene, cam->seq, cam->gray, cam->gray_pitch); + } memset(out, 0, sizeof(*out)); out->sequence = cam->seq; @@ -300,5 +364,6 @@ sim_display_stereo_camera_close(struct xrt_plugin_stereo_camera *cam) U_LOG_W("sim_display: FAKE stereo camera closed after %llu frames", (unsigned long long)cam->seq); free(cam->gray); free(cam->converted); + sim_stereo_camera_distorted_fini(&cam->distorted); free(cam); } diff --git a/src/xrt/drivers/sim_display/sim_display_stereo_camera.h b/src/xrt/drivers/sim_display/sim_display_stereo_camera.h index 94f1f8809e..b90b316bb4 100644 --- a/src/xrt/drivers/sim_display/sim_display_stereo_camera.h +++ b/src/xrt/drivers/sim_display/sim_display_stereo_camera.h @@ -11,12 +11,22 @@ * SIM_DISPLAY_FAKE_STEREO_CAMERA_FPS=N source rate (default 30) * SIM_DISPLAY_FAKE_STEREO_CAMERA_FORMAT=gray8|nv12|bgra native format (default gray8) * SIM_DISPLAY_FAKE_STEREO_CAMERA_SUSPEND_PERIOD_MS=N square-wave SUSPENDED/AVAILABLE + * SIM_DISPLAY_FAKE_STEREO_CAMERA_DISTORT=1 R2: a RAW distorted pair (see below) * * The camera: SHARED_WITH_EYE_TRACKING | USER_FACING | CALIBRATED | * NATIVELY_RECTIFIED (+ MONOCHROME for gray8), 68 deg horizontal FOV per eye, * 50 mm baseline, background at 2.0 m and a bar at 0.6 m — at 640 px per eye * that is fx = 474.4 px and disparities of 12 px and 40 px. * + * With _DISTORT=1 the same scene is seen through two raw cameras with known + * ground truth (per-eye intrinsics, RADTAN5 barrel lenses, 0.7 deg relative + * pitch = ~6 px of vertical misalignment, 0.9 deg relative roll, 0.5 deg yaw; + * sim_stereo_camera_truth_init), the calibration slot returns that truth, and + * NATIVELY_RECTIFIED is dropped — so RECTIFIED output exercises the service's + * rectifier (u_stereo_rectify). After rectification rows align and the + * disparities are f_rect * B / Z (f_rect: `displayxr-cli camera calib + * --rectified`). + * * @ingroup drv_sim_display */ diff --git a/src/xrt/drivers/sim_display/sim_display_stereo_camera_pattern.c b/src/xrt/drivers/sim_display/sim_display_stereo_camera_pattern.c index b0d52b7102..b50ebcf930 100644 --- a/src/xrt/drivers/sim_display/sim_display_stereo_camera_pattern.c +++ b/src/xrt/drivers/sim_display/sim_display_stereo_camera_pattern.c @@ -8,8 +8,11 @@ #include "sim_display_stereo_camera_pattern.h" +#include "util/u_stereo_rectify.h" + #include #include +#include #include //! 5x7 digits, one byte per row, bit 4 = leftmost column. @@ -63,6 +66,8 @@ sim_stereo_camera_scene_init(struct sim_stereo_camera_scene *s, s->eye_height = eye_height; s->bg_disparity = (uint32_t)lround(fx_px * (baseline_mm / 1000.0) / bg_depth_m); s->bar_disparity = (uint32_t)lround(fx_px * (baseline_mm / 1000.0) / bar_depth_m); + s->bg_disparity_f = fx_px * (baseline_mm / 1000.0) / bg_depth_m; + s->bar_disparity_f = fx_px * (baseline_mm / 1000.0) / bar_depth_m; s->bar_x0 = eye_width * 3 / 8; s->bar_x1 = eye_width * 5 / 8; s->bar_y0 = eye_height / 4; @@ -135,3 +140,193 @@ sim_stereo_camera_render_gray(const struct sim_stereo_camera_scene *s, render_counter(dst, pitch, 0, s->eye_width, s->eye_height, frame_number); render_counter(dst, pitch, s->eye_width, s->eye_width, s->eye_height, frame_number); } + + +/* + * + * DISTORTED variant (R2): the same scene through two raw cameras with known + * ground truth. + * + */ + +#define DEG(x) ((x)*3.14159265358979323846 / 180.0) + +//! Bilinear value noise on 2-pixel cells: continuous, so a resampled image is +//! an honest sample of the scene (no pixel-locked texture). +static float +vnoise(double u, double v, uint32_t seed, float lo, float span) +{ + double gu = u * 0.5, gv = v * 0.5; + double fu = floor(gu), fv = floor(gv); + int32_t iu = (int32_t)fu, iv = (int32_t)fv; + float a = (float)(gu - fu), b = (float)(gv - fv); + uint32_t sp = (uint32_t)span; + float h00 = (float)(hash3((uint32_t)iu, (uint32_t)iv, seed) % sp); + float h10 = (float)(hash3((uint32_t)(iu + 1), (uint32_t)iv, seed) % sp); + float h01 = (float)(hash3((uint32_t)iu, (uint32_t)(iv + 1), seed) % sp); + float h11 = (float)(hash3((uint32_t)(iu + 1), (uint32_t)(iv + 1), seed) % sp); + float top = h00 + (h10 - h00) * a, bot = h01 + (h11 - h01) * a; + return lo + top + (bot - top) * b; +} + +float +sim_stereo_camera_scene_sample(const struct sim_stereo_camera_scene *s, int right, double u, double v) +{ + double ub = right ? u + s->bg_disparity_f : u; + double ubar = right ? u + s->bar_disparity_f : u; + if (v >= s->bar_y0 && v < s->bar_y1 && ubar >= s->bar_x0 && ubar < s->bar_x1) { + return vnoise(ubar, v, 0xBA5u, 128.0f, 127.0f); + } + return vnoise(ub, v, 0x0B6u, 0.0f, 111.0f); +} + +void +sim_stereo_camera_truth_init(struct sim_stereo_camera_truth *t, + uint32_t eye_width, + uint32_t eye_height, + double ideal_fx, + double baseline_mm, + double bg_depth_m, + double bar_depth_m) +{ + memset(t, 0, sizeof(*t)); + const double w = eye_width, h = eye_height; + t->eye_width = eye_width; + t->eye_height = eye_height; + t->ideal_fx = ideal_fx; + t->ideal_cx = (w - 1.0) * 0.5; + t->ideal_cy = (h - 1.0) * 0.5; + t->baseline_mm = baseline_mm; + t->bg_depth_m = bg_depth_m; + t->bar_depth_m = bar_depth_m; + + // Two different, off-centre, barrel lenses (scale with the eye size). + t->fx[0] = ideal_fx * 1.012; + t->fy[0] = ideal_fx * 1.008; + t->cx[0] = t->ideal_cx + w * 0.009; + t->cy[0] = t->ideal_cy - h * 0.008; + const double d0[5] = {-0.21, 0.06, 0.0008, -0.0006, -0.005}; + t->fx[1] = ideal_fx * 0.991; + t->fy[1] = ideal_fx * 0.994; + t->cx[1] = t->ideal_cx - w * 0.007; + t->cy[1] = t->ideal_cy + h * 0.007; + const double d1[5] = {-0.17, 0.04, -0.0005, 0.0007, 0.0}; + memcpy(t->dist[0], d0, sizeof(d0)); + memcpy(t->dist[1], d1, sizeof(d1)); + + // Each camera turned by half of (pitch 0.35 deg, yaw 0.25 deg, roll 0.45 + // deg), in opposite senses: 0.7 deg of relative pitch is the vertical + // misalignment, 0.9 deg of relative roll the tilt. + const double rv[3] = {DEG(0.35), DEG(0.25), DEG(0.45)}; + u_stereo_rectify_rodrigues(rv, t->S); + // R = S * S (right-from-left rotation); T = -S * C_right, C_right = (B, 0, 0). + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + t->R[i][j] = t->S[i][0] * t->S[0][j] + t->S[i][1] * t->S[1][j] + t->S[i][2] * t->S[2][j]; + } + t->T_mm[i] = -t->S[i][0] * baseline_mm; + } +} + +bool +sim_stereo_camera_distorted_init(struct sim_stereo_camera_distorted *d, + uint32_t eye_width, + uint32_t eye_height, + double ideal_fx, + double baseline_mm, + double bg_depth_m, + double bar_depth_m) +{ + memset(d, 0, sizeof(*d)); + sim_stereo_camera_scene_init(&d->scene, eye_width, eye_height, ideal_fx, baseline_mm, bg_depth_m, bar_depth_m); + sim_stereo_camera_truth_init(&d->truth, eye_width, eye_height, ideal_fx, baseline_mm, bg_depth_m, bar_depth_m); + d->ideal_uv = malloc(sizeof(float) * 4 * (size_t)eye_width * eye_height); + if (d->ideal_uv == NULL) { + return false; + } + const struct sim_stereo_camera_truth *t = &d->truth; + for (uint32_t e = 0; e < 2; e++) { + struct u_stereo_rectify_lens lens; + memset(&lens, 0, sizeof(lens)); + lens.fx = t->fx[e]; + lens.fy = t->fy[e]; + lens.cx = t->cx[e]; + lens.cy = t->cy[e]; + lens.model = U_STEREO_RECTIFY_MODEL_RADTAN5; + memcpy(lens.d, t->dist[e], sizeof(t->dist[e])); + float *uv = d->ideal_uv + (size_t)e * 2 * eye_width * eye_height; + for (uint32_t y = 0; y < eye_height; y++) { + for (uint32_t x = 0; x < eye_width; x++) { + double xn, yn; + u_stereo_rectify_undistort_pixel(&lens, x, y, &xn, &yn); + // Raw ray -> virtual parallel frame: left x_rect = S x_raw + // (x_raw = S^T x_rect), right x_rect = S^T x_raw. + double r[3] = {xn, yn, 1.0}, q[3]; + for (int i = 0; i < 3; i++) { + q[i] = e == 0 ? t->S[i][0] * r[0] + t->S[i][1] * r[1] + t->S[i][2] * r[2] + : t->S[0][i] * r[0] + t->S[1][i] * r[1] + t->S[2][i] * r[2]; + } + float *o = uv + 2 * ((size_t)y * eye_width + x); + if (q[2] <= 1e-9) { + o[0] = o[1] = -1e9f; + continue; + } + o[0] = (float)(t->ideal_fx * q[0] / q[2] + t->ideal_cx); + o[1] = (float)(t->ideal_fx * q[1] / q[2] + t->ideal_cy); + } + } + } + return true; +} + +void +sim_stereo_camera_distorted_set_vmisalign(struct sim_stereo_camera_distorted *d, double dy_px, double slope) +{ + if (d->ideal_uv == NULL || (dy_px == 0.0 && slope == 0.0) || !(slope > -0.5 && slope < 0.5)) { + return; + } + // The right raw pixel that looks at virtual row v sees the scene row whose + // image the misalignment moved there: v = vs + dy + slope (vs - cy). + const uint32_t w = d->scene.eye_width, h = d->scene.eye_height; + const double cy = d->truth.ideal_cy; + float *uv = d->ideal_uv + (size_t)2 * w * h; + for (size_t i = 0; i < (size_t)w * h; i++) { + if (uv[2 * i] < -1e8f) { + continue; + } + uv[2 * i + 1] = (float)(cy + ((double)uv[2 * i + 1] - cy - dy_px) / (1.0 + slope)); + } +} + +void +sim_stereo_camera_distorted_fini(struct sim_stereo_camera_distorted *d) +{ + free(d->ideal_uv); + d->ideal_uv = NULL; +} + +void +sim_stereo_camera_render_gray_distorted(const struct sim_stereo_camera_distorted *d, + uint64_t frame_number, + uint8_t *dst, + uint32_t pitch) +{ + const uint32_t w = d->scene.eye_width, h = d->scene.eye_height; + for (uint32_t e = 0; e < 2; e++) { + const float *uv = d->ideal_uv + (size_t)e * 2 * w * h; + for (uint32_t y = 0; y < h; y++) { + uint8_t *row = dst + (size_t)y * pitch + e * w; + const float *p = uv + 2 * (size_t)y * w; + for (uint32_t x = 0; x < w; x++, p += 2) { + if (p[0] < -1e8f) { + row[x] = 0; + continue; + } + float val = sim_stereo_camera_scene_sample(&d->scene, (int)e, p[0], p[1]); + row[x] = (uint8_t)(val < 0.0f ? 0 : val > 255.0f ? 255 : (int)(val + 0.5f)); + } + } + } + render_counter(dst, pitch, 0, w, h, frame_number); + render_counter(dst, pitch, w, w, h, frame_number); +} diff --git a/src/xrt/drivers/sim_display/sim_display_stereo_camera_pattern.h b/src/xrt/drivers/sim_display/sim_display_stereo_camera_pattern.h index 81b4953522..247843a0d4 100644 --- a/src/xrt/drivers/sim_display/sim_display_stereo_camera_pattern.h +++ b/src/xrt/drivers/sim_display/sim_display_stereo_camera_pattern.h @@ -10,15 +10,24 @@ * The scene is a random-dot stereogram with two planes at KNOWN integer * disparities (left x - right x, pixels): a textured background and a nearer * textured bar in the middle of the frame. A frame counter is burned into the - * top-left of BOTH halves at zero disparity. The pair is parallel and - * undistorted (the R1 fake reports NATIVELY_RECTIFIED); a distorted, - * misaligned variant for the rectifier's tests is R2's. + * top-left of BOTH halves at zero disparity. The default pair is parallel and + * undistorted (the fake then reports NATIVELY_RECTIFIED). + * + * The DISTORTED variant (R2, SIM_DISPLAY_FAKE_STEREO_CAMERA_DISTORT=1) renders + * the same scene through two raw cameras with known ground truth: per-eye + * intrinsics that differ, RADTAN5 barrel distortion, and a small rotation of + * each camera (pitch -> vertical misalignment, roll, yaw) — the kind of pair a + * real tracker camera delivers. The rotations are split symmetrically + * (left = S^T, right = S), which makes Bouguet rectification recover EXACTLY + * the virtual parallel pair the scene is defined in, so after rectification + * rows must align and the disparity must be f_rect * B / Z. * * @ingroup drv_sim_display */ #pragma once +#include #include #ifdef __cplusplus @@ -33,8 +42,93 @@ struct sim_stereo_camera_scene uint32_t bar_disparity; //!< near bar, pixels //! The bar's rectangle in LEFT-eye coordinates. uint32_t bar_x0, bar_y0, bar_x1, bar_y1; + //! Unrounded disparities (fx * B / Z), used by the distorted variant. + double bg_disparity_f; + double bar_disparity_f; +}; + +/*! + * Ground truth of the DISTORTED fake: the virtual parallel camera the scene is + * defined in, and the two raw cameras that look at it. + */ +struct sim_stereo_camera_truth +{ + uint32_t eye_width, eye_height; + //! The virtual rectified camera (both eyes): the scene's pixel frame. + double ideal_fx, ideal_cx, ideal_cy; + double baseline_mm; + double bg_depth_m, bar_depth_m; + //! Raw per-eye intrinsics (pixels) + RADTAN5 (k1 k2 p1 p2 k3). + double fx[2], fy[2], cx[2], cy[2]; + double dist[2][5]; + //! Half rotation: x_raw_left = S^T x_rect, x_raw_right = S (x_rect - C_right). + double S[3][3]; + //! OpenCV extrinsics of the raw pair: x_R = R x_L + T, T in mm. + double R[3][3]; + double T_mm[3]; +}; + +//! Fill the truth for a pair of @p eye_width x @p eye_height whose virtual +//! parallel camera has focal @p ideal_fx and baseline @p baseline_mm. +void +sim_stereo_camera_truth_init(struct sim_stereo_camera_truth *t, + uint32_t eye_width, + uint32_t eye_height, + double ideal_fx, + double baseline_mm, + double bg_depth_m, + double bar_depth_m); + +/*! + * The distorted renderer: the truth + a per-raw-pixel lookup of where each + * raw pixel looks in the virtual parallel frame (built once). + */ +struct sim_stereo_camera_distorted +{ + struct sim_stereo_camera_scene scene; + struct sim_stereo_camera_truth truth; + //! 2 floats per raw pixel, eye-major: virtual-frame (u, v); u < -1e8 = none. + float *ideal_uv; }; +bool +sim_stereo_camera_distorted_init(struct sim_stereo_camera_distorted *d, + uint32_t eye_width, + uint32_t eye_height, + double ideal_fx, + double baseline_mm, + double bg_depth_m, + double bar_depth_m); + +void +sim_stereo_camera_distorted_fini(struct sim_stereo_camera_distorted *d); + +/*! + * An UNCALIBRATED vertical misalignment on top of the distorted pair (R2 + * refinement tests): the right eye shows the scene @p dy_px lower, plus + * @p slope px per px of distance from the centre row, in the virtual parallel + * frame — dy(v) = dy_px + slope * (v - ideal_cy). The calibration the fake + * reports does NOT know about it, like a real device whose stored calibration + * is slightly off (the Leia SR laptop: +2.6 px, -0.0047 px/px). Call once + * after init. + */ +void +sim_stereo_camera_distorted_set_vmisalign(struct sim_stereo_camera_distorted *d, double dy_px, double slope); + +//! Render frame @p frame_number of the distorted pair (GRAY8 SBS). +void +sim_stereo_camera_render_gray_distorted(const struct sim_stereo_camera_distorted *d, + uint64_t frame_number, + uint8_t *dst, + uint32_t pitch); + +/*! + * The continuous scene as the VIRTUAL parallel camera sees it: eye @p right at + * virtual pixel (@p u, @p v), bilinear value noise (so resampling is honest). + */ +float +sim_stereo_camera_scene_sample(const struct sim_stereo_camera_scene *s, int right, double u, double v); + /*! * Scene for a pinhole pair of focal @p fx_px and baseline @p baseline_mm, with * the background at @p bg_depth_m and the bar at @p bar_depth_m: disparities diff --git a/src/xrt/include/xrt/xrt_stereo_camera.h b/src/xrt/include/xrt/xrt_stereo_camera.h index 73fdcd7b57..6fa17a3ea0 100644 --- a/src/xrt/include/xrt/xrt_stereo_camera.h +++ b/src/xrt/include/xrt/xrt_stereo_camera.h @@ -157,6 +157,16 @@ struct xrt_stereo_camera_frame_info uint32_t reserved; }; +//! State of the service's online vertical-alignment refinement (R2). +enum xrt_stereo_camera_refine_state +{ + XRT_STEREO_CAMERA_REFINE_NONE = 0, //!< not a service-rectified RECTIFIED stream + XRT_STEREO_CAMERA_REFINE_OFF = 1, //!< DXR_STEREO_CAMERA_REFINE=0 + XRT_STEREO_CAMERA_REFINE_MEASURING = 2, //!< no window fitted yet + XRT_STEREO_CAMERA_REFINE_ALIGNED = 3, //!< fitted, the calibration was already within the deadband + XRT_STEREO_CAMERA_REFINE_APPLIED = 4, //!< a correction is folded into the maps +}; + struct xrt_stereo_camera_stream_stats { float source_frame_rate; @@ -165,6 +175,18 @@ struct xrt_stereo_camera_stream_stats uint64_t frames_skipped; uint64_t frames_acquired; uint64_t mean_latency_ns; + + // R2 refinement (diagnostics; the service's view of the rows). + uint32_t refine_state; //!< enum xrt_stereo_camera_refine_state + uint32_t refine_updates; + uint32_t refine_windows; + uint32_t refine_matches; //!< inliers of the latest fitted window + float refine_offset_px; //!< applied a + float refine_slope_per_100px; //!< applied b * 100 + float refine_initial_dy_px; //!< signed median dy of the first window (uncorrected) + float refine_residual_dy_px; //!< signed median dy of the latest window + float refine_measure_ms; //!< mean cost of one measurement (worker thread) + uint32_t reserved; }; /*! diff --git a/src/xrt/ipc/server/ipc_server_handler.c b/src/xrt/ipc/server/ipc_server_handler.c index 75466650f4..c526875a66 100644 --- a/src/xrt/ipc/server/ipc_server_handler.c +++ b/src/xrt/ipc/server/ipc_server_handler.c @@ -232,6 +232,48 @@ verify_client_class(volatile struct ipc_client_state *ics, uint32_t declared, co return XRT_CLIENT_CLASS_APP; } +// PRESENT_OWNER quota counts OWNERS, not connections: one browser is several +// processes of one executable (its GPU process weaves, the browser process binds +// the window, and its video-capture utility reads the stereo camera through +// XR_DXR_stereo_camera), and all of them are the one owner competing for the +// panel. A connection from the same executable as an already-admitted +// PRESENT_OWNER is a SIBLING and takes no new slot; otherwise the quota is +// compared against the number of DISTINCT owner executables. A peer whose image +// path the OS will not reveal ("") is never a sibling and counts as its own +// owner (fails closed). Caller holds global_state.lock; the exe lookups run at +// admission only. +static uint32_t +count_present_owners_locked(struct ipc_server *s, volatile struct ipc_client_state *ics, const char *ics_exe) +{ + static char seen[IPC_MAX_CLIENTS][512]; // under global_state.lock + uint32_t n = 0; + for (uint32_t i = 0; i < IPC_MAX_CLIENTS; i++) { + volatile struct ipc_client_state *o = &s->threads[i].ics; + if (o == ics || o->server_thread_index < 0 || !o->class_verified || + o->client_state.client_class != XRT_CLIENT_CLASS_PRESENT_OWNER) { + continue; + } + char exe[512]; + exe[0] = '\0'; + ipc_server_peer_exe_path(o->peer_pid, exe, sizeof(exe)); + if (exe[0] != '\0' && ics_exe[0] != '\0' && strcmp(exe, ics_exe) == 0) { + return 0; // a sibling of an admitted owner: no new slot + } + bool dup = false; + for (uint32_t k = 0; k < n && exe[0] != '\0'; k++) { + if (strcmp(seen[k], exe) == 0) { + dup = true; + break; + } + } + if (!dup) { + snprintf(seen[n], sizeof(seen[n]), "%s", exe); + n++; + } + } + return n; +} + // Count connected clients of a class, EXCLUDING ics. Caller holds global_state.lock. static uint32_t count_class_locked(struct ipc_server *s, volatile struct ipc_client_state *ics, uint32_t client_class) @@ -2132,7 +2174,8 @@ ipc_handle_instance_describe_client(volatile struct ipc_client_state *ics, struct ipc_server *s = ics->server; os_mutex_lock(&s->global_state.lock); - uint32_t in_use = count_class_locked(s, ics, verified); + uint32_t in_use = verified == XRT_CLIENT_CLASS_PRESENT_OWNER ? count_present_owners_locked(s, ics, exe) + : count_class_locked(s, ics, verified); uint32_t quota = ipc_server_client_class_quota(s, verified); bool refused = quota != 0 && in_use >= quota; if (!refused) { diff --git a/src/xrt/ipc/server/ipc_server_stereo_camera.c b/src/xrt/ipc/server/ipc_server_stereo_camera.c index 7c343449c1..d3ca57ff22 100644 --- a/src/xrt/ipc/server/ipc_server_stereo_camera.c +++ b/src/xrt/ipc/server/ipc_server_stereo_camera.c @@ -25,6 +25,37 @@ * - the in-use indicator (tray badge naming consumers) is R3; R1 logs one * WARN per stream start/stop naming the peer executable. * + * Rectification (R2) — when the plug-in reports CALIBRATED but not + * NATIVELY_RECTIFIED, the manager builds a per-camera rectifier at create time + * from the plug-in's RAW calibration (u_stereo_rectify: Bouguet, zero + * disparity at infinity, alpha = 0 crop, no convergence shear). It runs ONCE + * per source frame, on the camera thread, with the manager lock RELEASED, and + * only while at least one started stream wants RECTIFIED; every RECTIFIED + * stream then converts from the rectified image, RAW streams from the + * original. The seam is `struct scam_rectifier`: the geometry and the float + * maps are backend-neutral; `apply` is the backend. CPU today (fixed-point + * bilinear LUT: ~0.9 ms GRAY8 / ~1.4 ms NV12 per 1280x480 frame on an M1 Pro, + * -O2). A GPU backend uploads u_stereo_rectify_build_map() as an RG32F texture + * and remaps straight into the GPU transports' slots below. Rectified + * calibration (common f / principal point, rightFromLeft = pure +x baseline) + * comes from the same geometry, so frames and numbers cannot disagree. + * + * Online vertical-alignment refinement (R2) — a device's stored calibration + * can be slightly off for its frames (a Leia SR laptop: +1.7 px of row + * residual that OpenCV's own stereoRectify reproduces on the same frame). A + * per-camera WORKER thread measures the residual dy(y) = a + b (y - cy) on + * rectified frames the camera thread hands it (a copy of the luma, only when + * u_stereo_vrefine_due(): every 250 ms for 5 s after an open or an update, + * then one 3-frame window every 30 s; ~2.7 ms per measurement at -O2, i.e. + * < 0.4 ms per frame amortised at 30 Hz). When the fitted correction moves by + * more than 0.2 px it rebuilds the geometry + LUTs with the correction folded + * in (u_stereo_rectify_input::v_offset / v_slope; ~8 ms, on the worker) and + * parks them; the camera thread swaps them in between two frames under the + * lock and bumps calibration_generation, so the rectified calibration a + * consumer re-reads describes exactly the frames it gets. Clamped (|a| <= 6 + * px, |b| <= 2 px / 100 px), gated (>= 50 matches over >= 3 frames), + * re-verified on every re-open, and DXR_STEREO_CAMERA_REFINE=0 turns it off. + * * Transports — R1 implements SHARED_MEMORY only. The GPU transports are * designed to reuse this ring's state machine unchanged, only the slot storage * differs: @@ -61,9 +92,12 @@ #include "util/u_logging.h" #include "util/u_misc.h" #include "util/u_stereo_camera.h" +#include "util/u_stereo_rectify.h" +#include "util/u_stereo_vrefine.h" #include #include +#include #include #ifdef XRT_OS_WINDOWS @@ -76,6 +110,7 @@ DEBUG_GET_ONCE_BOOL_OPTION(stereo_camera_enabled, "DXR_STEREO_CAMERA", true) DEBUG_GET_ONCE_BOOL_OPTION(stereo_camera_dev_allow, "DXR_STEREO_CAMERA_DEV_ALLOW", false) +DEBUG_GET_ONCE_BOOL_OPTION(stereo_camera_refine, "DXR_STEREO_CAMERA_REFINE", true) #define MAX_STREAMS (XRT_STEREO_CAMERA_MAX_CAMERAS * XRT_STEREO_CAMERA_MAX_STREAMS_PER_CAMERA) #define LINGER_NS (2000ll * 1000 * 1000) @@ -83,6 +118,7 @@ DEBUG_GET_ONCE_BOOL_OPTION(stereo_camera_dev_allow, "DXR_STEREO_CAMERA_DEV_ALLOW #define SUSPEND_AFTER_NS (1000ll * 1000 * 1000) #define REOPEN_BACKOFF_NS (1000ll * 1000 * 1000) #define STATS_LOG_NS (5000ll * 1000 * 1000) +#define REFINE_SAMPLES 512 //!< matches one measurement may return /* @@ -201,6 +237,57 @@ struct scam_stream uint64_t latency_count; }; +/*! + * The rectifier seam (R2). Geometry + maps are backend-neutral; @ref apply is + * the backend. Owned by one camera, used only on its camera thread. + */ +struct scam_rectifier +{ + struct u_stereo_rectify_result geo; + //! [0] full resolution (GRAY8, NV12 Y, BGRA8), [1] NV12 UV (half-res, 2 ch). + struct u_stereo_rectify_lut lut[2]; + //! Rectified output in the SOURCE frame's format (tight layout). + uint8_t *buf; + uint64_t buf_size; + uint32_t buf_format; + struct u_stereo_camera_planes buf_layout; + //! Backend: rectify @p in into @p out (planes point into buf). CPU LUT today. + bool (*apply)(struct scam_rectifier *r, + const struct xrt_plugin_stereo_camera_frame *in, + struct xrt_plugin_stereo_camera_frame *out); + // stats + uint64_t ns_sum; + uint64_t count; + + /* + * Online vertical-alignment refinement. Everything below is guarded by the + * manager lock except where noted; the worker never touches geo / lut. + */ + bool refine; //!< enabled (DXR_STEREO_CAMERA_REFINE) + struct u_stereo_rectify_input in; //!< the RAW calibration, rebuilt with a correction + struct u_stereo_vrefine ref; //!< the controller + struct u_stereo_vrefine_measure_params mp; + struct ipc_server_stereo_camera *mgr; + uint64_t camera_id; + //! Luma of one rectified SBS frame. Written by the camera thread while + //! !job_pending, read by the worker while job_pending. + uint8_t *job_gray; + bool job_pending; + int64_t job_ns; + struct u_stereo_vrefine_sample *samples; //!< worker-only + //! A rebuilt geometry + LUTs the camera thread swaps in before its next frame. + bool pending_ready; + struct u_stereo_rectify_result pending_geo; + struct u_stereo_rectify_lut pending_lut[2]; + bool quit; + bool worker_started; + struct os_thread worker; + struct os_cond worker_cond; + uint64_t measure_ns_sum; + uint32_t measure_count; + bool stable_logged; +}; + struct scam_camera { uint32_t index; //!< plug-in index @@ -210,6 +297,9 @@ struct scam_camera struct xrt_plugin_stereo_camera_calibration calib; uint32_t state; uint32_t calibration_generation; + //! R2: set when CALIBRATED and not NATIVELY_RECTIFIED and the geometry is + //! sane; NULL = no service-side rectification (RAW-flagged fallback). + struct scam_rectifier *rect; struct xrt_plugin_stereo_camera *src; //!< owned by the camera thread uint32_t started_count; @@ -363,9 +453,9 @@ output_allowed(volatile struct ipc_client_state *ics, uint32_t output) static bool camera_can_rectify(const struct scam_camera *cam) { - // R1 has no rectifier (u_stereo_rectify is R2): only a natively rectified - // source yields RECTIFIED frames; a CALIBRATED one is delivered RAW-flagged. - return (cam->info.flags & XRT_PLUGIN_STEREO_CAMERA_NATIVELY_RECTIFIED) != 0; + // A natively rectified source, or a calibrated one the service rectifies + // (R2). A calibrated camera whose geometry was rejected stays RAW-only. + return (cam->info.flags & XRT_PLUGIN_STEREO_CAMERA_NATIVELY_RECTIFIED) != 0 || cam->rect != NULL; } static void @@ -435,6 +525,384 @@ baseline_mm(const struct xrt_plugin_stereo_camera_calibration *c) } +/* + * + * Rectifier (R2). + * + */ + +//! CPU backend: LUT remap per plane into the rectifier's buffer. +static bool +rectify_apply_cpu(struct scam_rectifier *r, + const struct xrt_plugin_stereo_camera_frame *in, + struct xrt_plugin_stereo_camera_frame *out) +{ + if (in->width != 2 * r->geo.width || in->height != r->geo.height || in->planes[0] == NULL) { + return false; + } + if (r->buf == NULL || r->buf_format != in->format) { + struct u_stereo_camera_planes lay; + if (!u_stereo_camera_layout(in->format, in->width, in->height, &lay)) { + return false; + } + if (lay.size > r->buf_size) { + uint8_t *nb = realloc(r->buf, (size_t)lay.size); + if (nb == NULL) { + return false; + } + r->buf = nb; + r->buf_size = lay.size; + } + r->buf_layout = lay; + r->buf_format = in->format; + } + int64_t t0 = os_monotonic_get_ns(); + const struct u_stereo_camera_planes *lay = &r->buf_layout; + switch (in->format) { + case XRT_PLUGIN_STEREO_CAMERA_FORMAT_GRAY8: + u_stereo_rectify_lut_apply(&r->lut[0], 1, in->planes[0], in->pitches[0], r->buf + lay->offset[0], + lay->pitch[0]); + break; + case XRT_PLUGIN_STEREO_CAMERA_FORMAT_NV12: + if (in->planes[1] == NULL) { + return false; + } + u_stereo_rectify_lut_apply(&r->lut[0], 1, in->planes[0], in->pitches[0], r->buf + lay->offset[0], + lay->pitch[0]); + u_stereo_rectify_lut_apply(&r->lut[1], 2, in->planes[1], in->pitches[1], r->buf + lay->offset[1], + lay->pitch[1]); + break; + case XRT_PLUGIN_STEREO_CAMERA_FORMAT_BGRA8: + u_stereo_rectify_lut_apply(&r->lut[0], 4, in->planes[0], in->pitches[0], r->buf + lay->offset[0], + lay->pitch[0]); + break; + default: return false; + } + r->ns_sum += (uint64_t)(os_monotonic_get_ns() - t0); + r->count++; + *out = *in; + out->planes[0] = r->buf + lay->offset[0]; + out->pitches[0] = lay->pitch[0]; + out->planes[1] = lay->plane_count > 1 ? r->buf + lay->offset[1] : NULL; + out->pitches[1] = lay->plane_count > 1 ? lay->pitch[1] : 0; + return true; +} + +static void +rectifier_destroy(struct scam_rectifier **rp) +{ + struct scam_rectifier *r = *rp; + if (r == NULL) { + return; + } + u_stereo_rectify_lut_fini(&r->lut[0]); + u_stereo_rectify_lut_fini(&r->lut[1]); + if (r->pending_ready) { + u_stereo_rectify_lut_fini(&r->pending_lut[0]); + u_stereo_rectify_lut_fini(&r->pending_lut[1]); + } + if (r->refine) { + os_cond_destroy(&r->worker_cond); + } + free(r->job_gray); + free(r->samples); + free(r->buf); + free(r); + *rp = NULL; +} + +//! Stop and join the refinement worker (manager lock NOT held). +static void +rectifier_stop_worker(struct ipc_server_stereo_camera *m, struct scam_rectifier *r) +{ + if (r == NULL || !r->worker_started) { + return; + } + os_mutex_lock(&m->lock); + r->quit = true; + os_cond_signal(&r->worker_cond); + os_mutex_unlock(&m->lock); + os_thread_join(&r->worker); + os_thread_destroy(&r->worker); + r->worker_started = false; +} + +/*! + * Build the rectifier from the plug-in's RAW calibration: per-eye K + + * distortion at image_width x image_height (rescaled to the frame's eye size + * when they differ), OpenCV extrinsics x_R = R x_L + T. + */ +static struct scam_rectifier * +rectifier_create(const struct scam_camera *cam) +{ + const struct xrt_plugin_stereo_camera_calibration *c = &cam->calib; + struct u_stereo_rectify_input in; + memset(&in, 0, sizeof(in)); + in.width = cam->info.eye_width; + in.height = cam->info.eye_height; + in.calib_width = c->image_width; + in.calib_height = c->image_height; + for (int e = 0; e < 2; e++) { + in.eye[e].fx = c->k[e][0]; + in.eye[e].fy = c->k[e][1]; + in.eye[e].cx = c->k[e][2]; + in.eye[e].cy = c->k[e][3]; + in.eye[e].model = c->distortion_model; + for (int k = 0; k < 8; k++) { + in.eye[e].d[k] = c->distortion[e][k]; + } + } + memcpy(in.R, c->rotation_right_from_left, sizeof(in.R)); + memcpy(in.T, c->translation_right_from_left_mm, sizeof(in.T)); + + int64_t t0 = os_monotonic_get_ns(); + struct scam_rectifier *r = U_TYPED_CALLOC(struct scam_rectifier); + if (!u_stereo_rectify_compute(&in, &r->geo)) { + U_LOG_W( + "stereo camera %llu: calibration rejected by the rectifier (degenerate or vertical pair) — " + "RAW only", + (unsigned long long)cam->camera_id); + free(r); + return NULL; + } + if (!u_stereo_rectify_lut_init(&r->lut[0], &r->geo, 1) || !u_stereo_rectify_lut_init(&r->lut[1], &r->geo, 2)) { + U_LOG_W("stereo camera %llu: rectifier LUT allocation failed — RAW only", + (unsigned long long)cam->camera_id); + rectifier_destroy(&r); + return NULL; + } + r->apply = rectify_apply_cpu; + r->in = in; + r->mgr = cam->mgr; + r->camera_id = cam->camera_id; + r->refine = debug_get_bool_option_stereo_camera_refine(); + if (r->refine) { + r->job_gray = malloc((size_t)2 * in.width * in.height); + r->samples = U_TYPED_ARRAY_CALLOC(struct u_stereo_vrefine_sample, REFINE_SAMPLES); + if (r->job_gray == NULL || r->samples == NULL) { + free(r->job_gray); + free(r->samples); + r->job_gray = NULL; + r->samples = NULL; + r->refine = false; + } + } + if (r->refine) { + os_cond_init(&r->worker_cond); + struct u_stereo_vrefine_config cfg; + u_stereo_vrefine_config_defaults(&cfg, in.height, r->geo.cy); + u_stereo_vrefine_init(&r->ref, &cfg); + // The disparity window: a subject at 0.3 m (the probe uses 0.4 m). + double dmax = r->geo.f * r->geo.baseline / 300.0; + dmax = dmax < 24.0 ? 24.0 : dmax > in.width / 2.0 ? in.width / 2.0 : dmax; + u_stereo_vrefine_measure_defaults(&r->mp, (uint32_t)dmax); + } else { + U_LOG_W("stereo camera %llu: vertical-alignment refinement OFF (DXR_STEREO_CAMERA_REFINE=0)", + (unsigned long long)cam->camera_id); + } + if (r->geo.t_rect[0] > 0.0) { + // Contract: SBS left half = the camera's LEFT lens, T.x < 0 (xrt_plugin.h). + U_LOG_W( + "stereo camera %llu: calibration puts the right camera at -x (T.x > 0) — the plug-in's SBS halves " + "look swapped; rectified disparities will be NEGATIVE", + (unsigned long long)cam->camera_id); + } + U_LOG_W( + "stereo camera %llu: rectifier ready (CPU) — f %.2f px, principal point (%.2f, %.2f), baseline %.2f mm, " + "valid-region zoom x%.4f, %u/%u valid taps, built in %.1f ms", + (unsigned long long)cam->camera_id, r->geo.f, r->geo.cx, r->geo.cy, r->geo.baseline, r->geo.crop_scale, + r->lut[0].valid, r->lut[0].width * r->lut[0].height, (double)(os_monotonic_get_ns() - t0) * 1e-6); + return r; +} + +/*! + * The refinement worker: measures the frame the camera thread parked, feeds + * the controller and, when the correction moved, rebuilds geometry + LUTs off + * the frame path and parks them for the camera thread to swap in. + */ +static void * +refine_worker(void *ptr) +{ + struct scam_rectifier *r = (struct scam_rectifier *)ptr; + struct ipc_server_stereo_camera *m = r->mgr; + const uint32_t w = r->in.width, h = r->in.height; + + os_mutex_lock(&m->lock); + while (!r->quit && !m->shutting_down) { + if (!r->job_pending) { + os_cond_wait(&r->worker_cond, &m->lock); + continue; + } + const int64_t t_job = r->job_ns; + const struct u_stereo_vrefine_measure_params mp = r->mp; + os_mutex_unlock(&m->lock); + + int64_t t0 = os_monotonic_get_ns(); + uint32_t n = u_stereo_vrefine_measure(r->job_gray, 2 * w, w, h, &mp, r->samples, REFINE_SAMPLES); + int64_t t1 = os_monotonic_get_ns(); + + os_mutex_lock(&m->lock); + r->measure_ns_sum += (uint64_t)(t1 - t0); + r->measure_count++; + enum u_stereo_vrefine_result res = u_stereo_vrefine_push(&r->ref, t_job, r->samples, n); + if (res != U_STEREO_VREFINE_UPDATED) { + // One INFO when a correction is confirmed, then one per slow-cadence + // window (every 30 s) — never one per settling window. + bool settling = t_job - r->ref.phase_start_ns < r->ref.cfg.fast_phase_ns; + if (res == U_STEREO_VREFINE_STABLE && (!r->stable_logged || !settling)) { + r->stable_logged = true; + U_LOG_I( + "stereo camera %llu: row alignment check — residual %+.2f px over %u matches, " + "correction a %+.2f px b %+.3f px/100 px unchanged", + (unsigned long long)r->camera_id, r->ref.last_residual_dy, r->ref.last_matches, + r->ref.a, r->ref.b * 100.0); + } + r->job_pending = false; + continue; + } + r->stable_logged = false; + const double a = r->ref.a, b = r->ref.b, residual = r->ref.last_residual_dy; + const uint32_t matches = r->ref.last_matches, updates = r->ref.updates; + struct u_stereo_rectify_input in = r->in; + in.v_offset = a / r->geo.f; // a is in pixels of the frames currently delivered + in.v_slope = b; + os_mutex_unlock(&m->lock); + + int64_t b0 = os_monotonic_get_ns(); + struct u_stereo_rectify_result g; + struct u_stereo_rectify_lut lut[2]; + memset(lut, 0, sizeof(lut)); + bool ok = u_stereo_rectify_compute(&in, &g) && u_stereo_rectify_lut_init(&lut[0], &g, 1) && + u_stereo_rectify_lut_init(&lut[1], &g, 2); + double build_ms = (double)(os_monotonic_get_ns() - b0) * 1e-6; + + os_mutex_lock(&m->lock); + if (!ok) { + u_stereo_rectify_lut_fini(&lut[0]); + u_stereo_rectify_lut_fini(&lut[1]); + u_stereo_vrefine_revert(&r->ref); + U_LOG_W( + "stereo camera %llu: vertical correction a %+.2f px b %+.3f px/100 px rejected by the " + "rectifier — kept the previous maps", + (unsigned long long)r->camera_id, a, b * 100.0); + } else { + if (r->pending_ready) { + u_stereo_rectify_lut_fini(&r->pending_lut[0]); + u_stereo_rectify_lut_fini(&r->pending_lut[1]); + } + r->pending_geo = g; + r->pending_lut[0] = lut[0]; + r->pending_lut[1] = lut[1]; + r->pending_ready = true; + if (updates == 1) { + U_LOG_W( + "stereo camera %llu: vertical-alignment refinement APPLIED — the calibration left " + "the rows %+.2f px off (fit %+.2f px %+.3f px/100 px about the centre row, %u " + "matches); correction a %+.2f px, b %+.3f px/100 px, maps rebuilt in %.1f ms", + (unsigned long long)r->camera_id, r->ref.initial_dy, r->ref.initial_a, + r->ref.initial_b * 100.0, matches, a, b * 100.0, build_ms); + } else { + U_LOG_I( + "stereo camera %llu: vertical correction updated — residual %+.2f px over %u " + "matches; now a %+.2f px, b %+.3f px/100 px (update %u, %.1f ms)", + (unsigned long long)r->camera_id, residual, matches, a, b * 100.0, updates, + build_ms); + } + } + r->job_pending = false; + } + os_mutex_unlock(&m->lock); + return NULL; +} + +/*! + * Camera thread, lock held, before rectifying a frame: swap in maps the worker + * rebuilt. The old LUTs are only ever read by this thread, so they can go now. + */ +static void +rectifier_swap_pending_locked(struct scam_camera *cam) +{ + struct scam_rectifier *r = cam->rect; + if (r == NULL || !r->pending_ready) { + return; + } + u_stereo_rectify_lut_fini(&r->lut[0]); + u_stereo_rectify_lut_fini(&r->lut[1]); + r->geo = r->pending_geo; + r->lut[0] = r->pending_lut[0]; + r->lut[1] = r->pending_lut[1]; + r->pending_ready = false; + // Consumers re-read the rectified calibration (f / principal point move + // by the alpha = 0 re-crop) on the next frame's generation change. + cam->calibration_generation++; +} + +/*! + * Camera thread, lock held: should this rectified frame be measured? Starts + * the worker on first use. + */ +static bool +rectifier_want_sample_locked(struct scam_rectifier *r, int64_t now) +{ + if (r == NULL || !r->refine || r->job_pending || r->pending_ready || r->quit) { + return false; + } + if (!u_stereo_vrefine_due(&r->ref, now)) { + return false; + } + if (!r->worker_started) { + if (os_thread_init(&r->worker) != 0 || os_thread_start(&r->worker, refine_worker, r) != 0) { + U_LOG_W("stereo camera %llu: refinement worker failed to start — refinement OFF", + (unsigned long long)r->camera_id); + r->refine = false; + return false; + } + r->worker_started = true; + } + return true; +} + +//! Camera thread, lock NOT held: copy the luma of rectified frame @p rf. +static bool +rectifier_copy_luma(struct scam_rectifier *r, const struct xrt_plugin_stereo_camera_frame *rf) +{ + const uint32_t W = 2 * r->in.width, H = r->in.height; + if (rf->width != W || rf->height != H || rf->planes[0] == NULL) { + return false; + } + for (uint32_t y = 0; y < H; y++) { + const uint8_t *src = rf->planes[0] + (size_t)y * rf->pitches[0]; + uint8_t *dst = r->job_gray + (size_t)y * W; + if (rf->format == XRT_PLUGIN_STEREO_CAMERA_FORMAT_BGRA8) { + for (uint32_t x = 0; x < W; x++) { + const uint8_t *p = src + 4 * (size_t)x; + dst[x] = (uint8_t)((p[0] + 2 * p[1] + p[2] + 2) >> 2); // B + 2G + R: luma enough + } + } else { + memcpy(dst, src, W); // GRAY8, or NV12's Y plane + } + } + return true; +} + +//! Does any started stream on @p cam want the service's rectified image? +static bool +camera_wants_rectified_locked(struct ipc_server_stereo_camera *m, const struct scam_camera *cam) +{ + if (cam->rect == NULL) { + return false; + } + for (uint32_t i = 0; i < MAX_STREAMS; i++) { + const struct scam_stream *s = &m->streams[i]; + if (s->used && s->started && s->camera == cam->index && + s->output == XRT_STEREO_CAMERA_OUTPUT_RECTIFIED) { + return true; + } + } + return false; +} + + /* * * Stream storage. @@ -511,10 +979,15 @@ source_rate_hz(const struct scam_camera *cam) * */ +/*! + * Fan one source frame out. @p rf is the service-rectified image of @p f (NULL + * when the camera has no rectifier or nobody wanted it this frame). + */ static void publish_locked(struct ipc_server_stereo_camera *m, struct scam_camera *cam, const struct xrt_plugin_stereo_camera_frame *f, + const struct xrt_plugin_stereo_camera_frame *rf, int64_t now) { if (cam->last_frame_ns > 0) { @@ -560,6 +1033,17 @@ publish_locked(struct ipc_server_stereo_camera *m, u_stereo_camera_ring_clear(&s->ring); continue; } + // A RECTIFIED stream on a service-rectified camera takes the rectified + // image; if it was not produced this frame (the stream started while the + // camera thread was rectifying without it), skip — never RAW pixels + // labelled RECTIFIED. + const struct xrt_plugin_stereo_camera_frame *src = f; + if (s->output == XRT_STEREO_CAMERA_OUTPUT_RECTIFIED && cam->rect != NULL) { + if (rf == NULL) { + continue; + } + src = rf; + } if (!u_stereo_camera_decimator_accept(&s->dec, f->time_ns)) { continue; } @@ -568,8 +1052,8 @@ publish_locked(struct ipc_server_stereo_camera *m, continue; } uint8_t *dst = s->map + (uint64_t)slot * s->slot_stride; - if (!u_stereo_camera_convert(f->format, f->planes, f->pitches, s->req.format, dst, &s->layout, f->width, - f->height)) { + if (!u_stereo_camera_convert(src->format, src->planes, src->pitches, s->req.format, dst, &s->layout, + src->width, src->height)) { u_stereo_camera_ring_abort_write(&s->ring); continue; } @@ -605,8 +1089,12 @@ publish_locked(struct ipc_server_stereo_camera *m, if (now - cam->last_stats_log_ns >= STATS_LOG_NS) { cam->last_stats_log_ns = now; - U_LOG_I("stereo camera %llu: source %.1f Hz, %u started stream(s)", (unsigned long long)cam->camera_id, - cam->source_rate, cam->started_count); + double rect_ms = (cam->rect != NULL && cam->rect->count > 0) + ? (double)cam->rect->ns_sum / (double)cam->rect->count * 1e-6 + : 0.0; + U_LOG_I("stereo camera %llu: source %.1f Hz, %u started stream(s), rectify %.3f ms/frame (%llu frames)", + (unsigned long long)cam->camera_id, cam->source_rate, cam->started_count, rect_ms, + (unsigned long long)(cam->rect != NULL ? cam->rect->count : 0)); } } @@ -668,6 +1156,10 @@ camera_thread(void *ptr) cam->src = src; cam->last_frame_ns = 0; u_stereo_camera_rate_meter_restart(&cam->rate_meter); + if (cam->rect != NULL && cam->rect->refine) { + // Keep the correction, re-verify it at the settling cadence. + u_stereo_vrefine_restart(&cam->rect->ref, os_monotonic_get_ns()); + } U_LOG_W("stereo camera %llu: source opened", (unsigned long long)cam->camera_id); set_state_locked(cam, XRT_STEREO_CAMERA_STATE_WAITING); } @@ -681,10 +1173,31 @@ camera_thread(void *ptr) now = os_monotonic_get_ns(); switch (w) { - case XRT_PLUGIN_STEREO_CAMERA_WAIT_OK: - publish_locked(m, cam, &f, now); + case XRT_PLUGIN_STEREO_CAMERA_WAIT_OK: { + // R2: rectify once per source frame, outside the lock (only this + // thread touches cam->rect after create), only if someone wants it. + struct xrt_plugin_stereo_camera_frame rf; + const struct xrt_plugin_stereo_camera_frame *rfp = NULL; + if (camera_wants_rectified_locked(m, cam)) { + struct scam_rectifier *rect = cam->rect; + rectifier_swap_pending_locked(cam); + bool sample = rectifier_want_sample_locked(rect, now); + os_mutex_unlock(&m->lock); + bool ok = rect->apply(rect, &f, &rf); + bool copied = ok && sample && rectifier_copy_luma(rect, &rf); + os_mutex_lock(&m->lock); + if (copied) { + rect->job_ns = f.time_ns > 0 ? f.time_ns : now; + rect->job_pending = true; + os_cond_signal(&rect->worker_cond); + } + now = os_monotonic_get_ns(); + rfp = ok ? &rf : NULL; + } + publish_locked(m, cam, &f, rfp, now); iface->stereo_camera_release_frame(src); break; + } case XRT_PLUGIN_STEREO_CAMERA_WAIT_TIMEOUT: if (cam->last_frame_ns == 0) { set_state_locked(cam, XRT_STEREO_CAMERA_STATE_WAITING); @@ -790,6 +1303,9 @@ ipc_server_stereo_camera_create(struct xrt_instance *xinst) cam->info.flags &= ~XRT_PLUGIN_STEREO_CAMERA_CALIBRATED; } } + if (cam->have_calib && (cam->info.flags & XRT_PLUGIN_STEREO_CAMERA_NATIVELY_RECTIFIED) == 0) { + cam->rect = rectifier_create(cam); + } if (!(info->max_frame_rate >= 0.0f && info->max_frame_rate <= 1000.0f)) { cam->info.max_frame_rate = 0.0f; // NaN / negative / absurd = unknown } @@ -828,6 +1344,8 @@ ipc_server_stereo_camera_destroy(struct ipc_server_stereo_camera **mgr_ptr) os_thread_destroy(&m->cams[i].thread); } os_cond_destroy(&m->cams[i].cond); + rectifier_stop_worker(m, m->cams[i].rect); + rectifier_destroy(&m->cams[i].rect); } for (uint32_t i = 0; i < MAX_STREAMS; i++) { if (m->streams[i].used) { @@ -944,7 +1462,9 @@ ipc_handle_stereo_camera_get_properties(volatile struct ipc_client_state *ics, out_props->max_frame_rate = source_rate_hz(cam); if (cam->have_calib) { out_props->baseline_mm = (float)baseline_mm(&cam->calib); - double fx = cam->calib.k[0][0]; + // Spec §3: per eye, RECTIFIED (the service's rectified focal when it + // rectifies; the plug-in's own for a natively rectified source). + double fx = cam->rect != NULL ? cam->rect->geo.f : cam->calib.k[0][0]; if (fx > 0.0) { out_props->horizontal_fov_deg = (float)(2.0 * atan(cam->info.eye_width / (2.0 * fx)) * 180.0 / 3.14159265358979323846); @@ -988,12 +1508,38 @@ ipc_handle_stereo_camera_get_calibration(volatile struct ipc_client_state *ics, xret = XRT_ERROR_FEATURE_NOT_SUPPORTED; } if (xret == XRT_SUCCESS && output == XRT_STEREO_CAMERA_OUTPUT_RECTIFIED && !camera_can_rectify(cam)) { - xret = XRT_ERROR_FEATURE_NOT_SUPPORTED; // R2 computes post-rectification numbers + xret = XRT_ERROR_FEATURE_NOT_SUPPORTED; } if (xret != XRT_SUCCESS) { os_mutex_unlock(&m->lock); return xret; } + if (output == XRT_STEREO_CAMERA_OUTPUT_RECTIFIED && cam->rect != NULL) { + // R2: the numbers of the frames the service rectifies — one pinhole for + // both eyes (fx = fy = f, one principal point: zero disparity at + // infinity), no distortion, and the right camera a pure +x translation + // of the baseline (OpenCV P1/P2 with P2[0][3] = f * Tx, Tx = -baseline). + const struct u_stereo_rectify_result *g = &cam->rect->geo; + out_calib->output = output; + for (int e = 0; e < 2; e++) { + struct xrt_stereo_camera_intrinsics *in = &out_calib->eye[e]; + in->width = g->width; + in->height = g->height; + in->fx = (float)g->f; + in->fy = (float)g->f; + in->cx = (float)g->cx; + in->cy = (float)g->cy; + in->model = XRT_PLUGIN_STEREO_CAMERA_DISTORTION_NONE; + } + out_calib->orientation[3] = 1.0f; // identity + // Right camera centre in the rectified left frame = -t_rect (mm -> m). + for (int i = 0; i < 3; i++) { + out_calib->position[i] = (float)(-g->t_rect[i] / 1000.0); + } + out_calib->baseline_mm = (float)g->baseline; + os_mutex_unlock(&m->lock); + return XRT_SUCCESS; + } const struct xrt_plugin_stereo_camera_calibration *c = &cam->calib; out_calib->output = output; for (int e = 0; e < 2; e++) { @@ -1074,6 +1620,17 @@ ipc_handle_stereo_camera_stream_create(volatile struct ipc_client_state *ics, os_mutex_unlock(&m->lock); return XRT_ERROR_INPUT_UNSUPPORTED; } + // Raw frames never reach web pages: the browser gets truly rectified + // frames or nothing, never the RAW-flagged fallback. + if (req->output == XRT_STEREO_CAMERA_OUTPUT_RECTIFIED && !camera_can_rectify(cam) && + !output_allowed(ics, XRT_STEREO_CAMERA_OUTPUT_RAW)) { + os_mutex_unlock(&m->lock); + U_LOG_W( + "stereo_camera_stream_create: camera %llu cannot be rectified; refused for pid %ld " + "(PRESENT_OWNER: rectified only)", + (unsigned long long)cam->camera_id, ics->peer_pid); + return XRT_ERROR_INPUT_UNSUPPORTED; + } uint32_t per_camera = 0; struct scam_stream *slot = NULL; for (uint32_t i = 0; i < MAX_STREAMS; i++) { @@ -1099,8 +1656,9 @@ ipc_handle_stereo_camera_stream_create(volatile struct ipc_client_state *ics, slot->ics = ics; slot->camera = cam->index; slot->req = *req; - // R1: RECTIFIED only from a natively rectified source; a calibrated one is - // delivered RAW and FLAGGED (frame.output) until u_stereo_rectify lands (R2). + // RECTIFIED from a natively rectified source or the service's rectifier + // (R2); a calibrated camera whose geometry the rectifier rejected is + // delivered RAW and FLAGGED (frame.output) — to native clients only. slot->output = (req->output == XRT_STEREO_CAMERA_OUTPUT_RECTIFIED && camera_can_rectify(cam)) ? XRT_STEREO_CAMERA_OUTPUT_RECTIFIED : XRT_STEREO_CAMERA_OUTPUT_RAW; @@ -1337,6 +1895,23 @@ ipc_handle_stereo_camera_stream_stats(volatile struct ipc_client_state *ics, out_stats->frames_skipped = s->ring.skipped; out_stats->frames_acquired = s->ring.acquired; out_stats->mean_latency_ns = s->latency_count > 0 ? s->latency_sum_ns / s->latency_count : 0; + const struct scam_rectifier *r = m->cams[s->camera].rect; + if (r != NULL && s->output == XRT_STEREO_CAMERA_OUTPUT_RECTIFIED) { + const struct u_stereo_vrefine *ref = &r->ref; + out_stats->refine_state = !r->refine ? XRT_STEREO_CAMERA_REFINE_OFF + : ref->applied ? XRT_STEREO_CAMERA_REFINE_APPLIED + : ref->windows > 0 ? XRT_STEREO_CAMERA_REFINE_ALIGNED + : XRT_STEREO_CAMERA_REFINE_MEASURING; + out_stats->refine_updates = ref->updates; + out_stats->refine_windows = ref->windows; + out_stats->refine_matches = ref->last_matches; + out_stats->refine_offset_px = (float)ref->a; + out_stats->refine_slope_per_100px = (float)(ref->b * 100.0); + out_stats->refine_initial_dy_px = (float)ref->initial_dy; + out_stats->refine_residual_dy_px = (float)ref->last_residual_dy; + out_stats->refine_measure_ms = + r->measure_count > 0 ? (float)((double)r->measure_ns_sum / r->measure_count * 1e-6) : 0.0f; + } os_mutex_unlock(&m->lock); return XRT_SUCCESS; } diff --git a/src/xrt/targets/cli/cli_cmd_camera.c b/src/xrt/targets/cli/cli_cmd_camera.c index 1b2c20a492..cf5b71986c 100644 --- a/src/xrt/targets/cli/cli_cmd_camera.c +++ b/src/xrt/targets/cli/cli_cmd_camera.c @@ -6,8 +6,9 @@ * * camera list [--json] properties + state of every camera * camera calib [--raw|--rectified] [--json] - * camera probe [] [--raw] [--format gray8|nv12|bgra8] [--fps F] + * camera probe [] [--raw|--rectified] [--format gray8|nv12|bgra8] [--fps F] * [--frames N] [--seconds S] [--out DIR] + * [--max-disparity N] [--max-dy N] [--min-ncc C] * * `probe` is a real consumer: it creates and starts a stream (so it goes * through the service's authorisation — in R1, DXR_STEREO_CAMERA_DEV_ALLOW=1 @@ -17,6 +18,31 @@ * scene is 12 px background / 40 px bar), and the service's stream stats. * With --out it writes the last frame as cam_.png. * + * R2: every probe also reports ROW ALIGNMENT — 2-D block matching (dx AND dy) + * of the last frame on textured 32x32 blocks: the vertical disparity a + * rectified pair must bring to ~0 (median / p90 / max |dy|) and the dominant + * horizontal disparities, each converted to a depth Z = f * B / d with the + * RECTIFIED calibration. `--rectified` insists on RECTIFIED output and fails + * (exit 5) if the frames came back RAW-flagged or the rows do not align: + * |SIGNED median dy| > 0.5 px (a constant vertical offset), a robust spread + * (MAD about that median) > 1.5 px, or fewer than 20 accepted blocks. The + * unsigned |dy| statistics are printed but not gated (block-match noise). Matching is zero-mean NCC with sub-pixel refinement; the + * disparity window comes from the camera's own f * B (a subject at 0.4 m), + * the vertical one is +-24 px, blocks under NCC 0.90 or peaking ON either + * bound are counted and excluded (a bound hit is a clamp, not a measurement — + * the first Leia SR run read "d = 64, |dy| = 10.000" that way). On the + * distorted sim fake + * (SIM_DISPLAY_FAKE_STEREO_CAMERA_DISTORT=1) the depths read back 2.00 m + * (background) and 0.60 m (bar); `--raw` shows the misalignment it fixes. + * + * The service stats line is followed by the ONLINE VERTICAL REFINEMENT state + * (R2): applied correction a / b, match count, and the signed dy of its first + * (uncorrected) window next to its latest one and this probe's own last-frame + * measurement. It settles in ~1 s after a stream starts, so give the probe a + * few seconds (the default 90 frames is ~3 s at 30 Hz). On the fake, inject a + * calibration error with SIM_DISPLAY_FAKE_STEREO_CAMERA_DY=2.6 and + * _DY_SLOPE=-0.47 (px per 100 px) next to _DISTORT=1. + * * Connects as a DIAG client, like `clients`: on Windows run it NON-elevated. */ @@ -229,6 +255,16 @@ cmd_calib(struct ipc_connection *ipc_c, uint64_t id, uint32_t output, bool json) } printf(" right camera in left frame: q (%.5f %.5f %.5f %.5f) p (%.5f %.5f %.5f) m\n", c.orientation[0], c.orientation[1], c.orientation[2], c.orientation[3], c.position[0], c.position[1], c.position[2]); + if (output == XRT_STEREO_CAMERA_OUTPUT_RECTIFIED) { + // OpenCV-style projection matrices of the rectified pair (Tx in mm). + double tx = -(double)c.position[0] * 1000.0; + printf(" P1 = [%.3f 0 %.3f 0; 0 %.3f %.3f 0; 0 0 1 0]\n", c.eye[0].fx, c.eye[0].cx, c.eye[0].fy, + c.eye[0].cy); + printf(" P2 = [%.3f 0 %.3f %.3f; 0 %.3f %.3f 0; 0 0 1 0] (P2[0][3] = f * Tx, Tx = %.3f mm)\n", + c.eye[1].fx, c.eye[1].cx, c.eye[1].fx * tx, c.eye[1].fy, c.eye[1].cy, tx); + printf(" depth from disparity: Z = f * B / d = %.1f / d(px) m\n", + c.eye[0].fx * c.baseline_mm / 1000.0); + } return 0; } @@ -402,6 +438,142 @@ report_disparity(const uint8_t *gray, uint32_t pitch, uint32_t eye_w, uint32_t h } } +static int +cmp_float(const void *a, const void *b) +{ + float x = *(const float *)a, y = *(const float *)b; + return (x > y) - (x < y); +} + +struct row_alignment +{ + int textured; //!< blocks that passed the texture gate and were matched + int blocks; //!< ...of which accepted (NCC >= min, peak inside both windows) + int low_corr; //!< rejected: peak NCC below the threshold + int dx_edge; //!< rejected: peak ON the disparity bound (a clamp, not a measurement) + int dy_edge; //!< rejected: peak ON the vertical bound + float dy_signed_median; //!< right y - left y, px (a constant vertical offset shows here) + float dy_median, dy_p90; //!< |dy|, px (informational: block-match noise on a face scene inflates these) + float dy_mad; //!< median |dy - signed median|, px: the robust spread the gate uses + int outliers; //!< accepted blocks with |dy| > 1 px + int modes; + float mode_dx[2]; + int mode_count[2]; +}; + +struct row_params +{ + uint32_t max_d; //!< disparity window (probe_max_disparity) + uint32_t max_dy; //!< vertical window, +-px + float min_ncc; //!< acceptance threshold on the peak correlation +}; + +/*! + * 2-D NCC block matching (u_stereo_camera_match_block) of a GRAY8 SBS frame on + * a 32 px grid, textured blocks only (a flat wall matches anywhere): signed and + * |dy| statistics and the two dominant horizontal disparities (1 px clusters). + * Blocks below the correlation threshold or whose peak sits on either window's + * bound are counted and excluded — never clamped into the statistics. + */ +static void +measure_rows(const uint8_t *gray, + uint32_t pitch, + uint32_t eye_w, + uint32_t h, + const struct row_params *rp, + struct row_alignment *out) +{ + memset(out, 0, sizeof(*out)); + enum + { + B = 32, + MAXB = 1024 + }; + static float dxs[MAXB], ady[MAXB], sdy[MAXB]; + int n = 0; + for (uint32_t y = 16; y + B + 16 <= h && out->textured < MAXB; y += B) { + for (uint32_t x = 16; x + B + 8 <= eye_w && out->textured < MAXB; x += B) { + // Texture gate: standard deviation of the left block >= 6 levels. + double sum = 0, sq = 0; + for (uint32_t j = 0; j < B; j++) { + const uint8_t *r = gray + (size_t)(y + j) * pitch + x; + for (uint32_t i = 0; i < B; i++) { + sum += r[i]; + sq += (double)r[i] * r[i]; + } + } + double mean = sum / (B * B); + if (sq / (B * B) - mean * mean < 36.0) { + continue; + } + struct u_stereo_camera_block_match m; + if (!u_stereo_camera_match_block(gray, pitch, eye_w, h, x, y, B, B, rp->max_d, rp->max_dy, + &m)) { + continue; + } + out->textured++; + if (m.ncc < rp->min_ncc) { + out->low_corr++; + continue; + } + if (m.dx_at_edge) { + out->dx_edge++; + continue; + } + if (m.dy_at_edge) { + out->dy_edge++; + continue; + } + dxs[n] = m.dx; + sdy[n] = m.dy; + ady[n] = m.dy < 0 ? -m.dy : m.dy; + n++; + } + } + out->blocks = n; + if (n == 0) { + return; + } + qsort(sdy, (size_t)n, sizeof(float), cmp_float); + out->dy_signed_median = sdy[n / 2]; + qsort(ady, (size_t)n, sizeof(float), cmp_float); + out->dy_median = ady[n / 2]; + out->dy_p90 = ady[(n * 9) / 10]; + // Robust spread about the signed median (MAD). ady[] is free again once its stats are read. + for (int i = 0; i < n; i++) { + float r = sdy[i] - out->dy_signed_median; + ady[i] = r < 0 ? -r : r; + } + qsort(ady, (size_t)n, sizeof(float), cmp_float); + out->dy_mad = ady[n / 2]; + for (int i = 0; i < n; i++) { + out->outliers += ady[i] > 1.0f; + } + qsort(dxs, (size_t)n, sizeof(float), cmp_float); + // Greedy 1 px clusters; keep the two most populated. + for (int i = 0; i < n;) { + int j = i; + double s = 0; + while (j < n && dxs[j] - dxs[i] <= 1.0f) { + s += dxs[j]; + j++; + } + int c = j - i; + float mean = (float)(s / c); + if (c > out->mode_count[0]) { + out->mode_dx[1] = out->mode_dx[0]; + out->mode_count[1] = out->mode_count[0]; + out->mode_dx[0] = mean; + out->mode_count[0] = c; + } else if (c > out->mode_count[1]) { + out->mode_dx[1] = mean; + out->mode_count[1] = c; + } + i = j; + } + out->modes = out->mode_count[1] > 0 ? 2 : 1; +} + static int cmd_probe(struct ipc_connection *ipc_c, int argc, const char **argv) { @@ -412,9 +584,17 @@ cmd_probe(struct ipc_connection *ipc_c, int argc, const char **argv) int frames = 90; float seconds = 0.0f; const char *out_dir = NULL; + bool require_rectified = false; + uint32_t max_d_arg = 0; // 0 = from the camera's geometry + struct row_params rp = {.max_d = 0, .max_dy = 24, .min_ncc = 0.90f}; + char rows_txt[64] = ""; + bool have_rows = false; for (int i = 3; i < argc; i++) { if (strcmp(argv[i], "--raw") == 0) { output = XRT_STEREO_CAMERA_OUTPUT_RAW; + } else if (strcmp(argv[i], "--rectified") == 0) { + output = XRT_STEREO_CAMERA_OUTPUT_RECTIFIED; + require_rectified = true; } else if (strcmp(argv[i], "--format") == 0 && i + 1 < argc) { const char *f = argv[++i]; format = strcmp(f, "gray8") == 0 ? XRT_STEREO_CAMERA_FORMAT_GRAY8 @@ -426,6 +606,12 @@ cmd_probe(struct ipc_connection *ipc_c, int argc, const char **argv) frames = atoi(argv[++i]); } else if (strcmp(argv[i], "--seconds") == 0 && i + 1 < argc) { seconds = (float)atof(argv[++i]); + } else if (strcmp(argv[i], "--max-disparity") == 0 && i + 1 < argc) { + max_d_arg = (uint32_t)strtoul(argv[++i], NULL, 10); + } else if (strcmp(argv[i], "--max-dy") == 0 && i + 1 < argc) { + rp.max_dy = (uint32_t)strtoul(argv[++i], NULL, 10); + } else if (strcmp(argv[i], "--min-ncc") == 0 && i + 1 < argc) { + rp.min_ncc = (float)atof(argv[++i]); } else if (strcmp(argv[i], "--out") == 0 && i + 1 < argc) { out_dir = argv[++i]; } else if (argv[i][0] != '-') { @@ -505,6 +691,7 @@ cmd_probe(struct ipc_connection *ipc_c, int argc, const char **argv) int64_t t_start = os_monotonic_get_ns(); int64_t deadline = seconds > 0.0f ? t_start + (int64_t)(seconds * 1e9) : t_start + 60ll * 1000000000; int got = 0, wakes = 0, not_ready = 0; + bool rows_ok = true; uint64_t first_index = 0, last_index = 0, gaps = 0; int64_t t_first = 0, t_last = 0; struct xrt_stereo_camera_frame_info last; @@ -566,9 +753,66 @@ cmd_probe(struct ipc_connection *ipc_c, int argc, const char **argv) u_stereo_camera_convert(last.format, src, last.row_pitch, XRT_STEREO_CAMERA_FORMAT_GRAY8, gray, &gl, last.width, last.height)) { char disp[256]; - report_disparity(gray, gl.pitch[0], last.width / 2, last.height, - probe_max_disparity(&props, last.width / 2), disp, sizeof(disp)); + rp.max_d = max_d_arg > 0 ? max_d_arg : probe_max_disparity(&props, last.width / 2); + report_disparity(gray, gl.pitch[0], last.width / 2, last.height, rp.max_d, disp, sizeof(disp)); printf("block disparity (left x - right x): %s\n", disp); + + // R2: row alignment + depth of the dominant disparities. + struct row_alignment ra; + measure_rows(gray, gl.pitch[0], last.width / 2, last.height, &rp, &ra); + struct xrt_stereo_camera_calibration rc; + double fb = 0.0; // f * B, px * m + if (last.output == XRT_STEREO_CAMERA_OUTPUT_RECTIFIED && + ipc_client_stereo_camera_get_calibration(ipc_c, id, XRT_STEREO_CAMERA_OUTPUT_RECTIFIED, + &rc) == XRT_SUCCESS) { + fb = rc.eye[0].fx * rc.baseline_mm / 1000.0; + } + const char *out_name = last.output == XRT_STEREO_CAMERA_OUTPUT_RECTIFIED ? "RECTIFIED" : "RAW"; + printf( + "row alignment (%s): NCC 32x32 blocks, search d -8..%u px, dy +-%u px, accept NCC >= " + "%.2f\n", + out_name, rp.max_d, rp.max_dy, rp.min_ncc); + printf( + " %d textured block(s): %d accepted, %d below NCC %.2f, %d AT the disparity bound, %d AT " + "the dy bound\n", + ra.textured, ra.blocks, ra.low_corr, rp.min_ncc, ra.dx_edge, ra.dy_edge); + if (ra.dx_edge * 5 > ra.textured || ra.dy_edge * 5 > ra.textured) { + printf( + " WARNING: over 20%% of blocks peak ON a search bound — that is a clamp, not a " + "measurement; widen it (--max-disparity N / --max-dy N)\n"); + } + if (ra.blocks < 5) { + printf(" row alignment: n/a (fewer than 5 accepted blocks)\n"); + rows_ok = false; + } else { + printf( + " dy (right - left) median %+.3f px SIGNED; |dy| median %.3f px, p90 %.3f px, %d " + "block(s) > 1 px\n", + ra.dy_signed_median, ra.dy_median, ra.dy_p90, ra.outliers); + for (int k = 0; k < ra.modes; k++) { + if (fb > 0.0 && ra.mode_dx[k] > 0.25f) { + printf( + " disparity mode %d: %.2f px (%d blocks) -> Z = f*B/d = %.3f m\n", + k + 1, ra.mode_dx[k], ra.mode_count[k], fb / ra.mode_dx[k]); + } else { + printf(" disparity mode %d: %.2f px (%d blocks)\n", k + 1, + ra.mode_dx[k], ra.mode_count[k]); + } + } + snprintf(rows_txt, sizeof(rows_txt), "%+.2f px (%d blocks)", ra.dy_signed_median, + ra.blocks); + have_rows = true; + // A constant vertical offset fails even when |dy| scatter is small. + // The gate is ROBUST: |signed median| <= 0.5 px (the rows' actual offset) with a + // bounded MAD spread and enough blocks. The unsigned |dy| median is NOT gated: on a + // face scene block-match noise alone put it at ~1.0 px while the signed median was + // -0.1 px (SR laptop, 2026-09-27). + float sm = ra.dy_signed_median < 0 ? -ra.dy_signed_median : ra.dy_signed_median; + rows_ok = sm <= 0.5f && ra.dy_mad <= 1.5f && ra.blocks >= 20; + printf(" row gate: |signed median| %.3f px (<= 0.5), MAD %.3f px (<= 1.5), %d blocks " + "(>= 20)\n", + sm, ra.dy_mad, ra.blocks); + } } if (out_dir != NULL) { char path[1024]; @@ -609,13 +853,46 @@ cmd_probe(struct ipc_connection *ipc_c, int argc, const char **argv) st.source_frame_rate, st.delivered_frame_rate, (unsigned long long)st.frames_published, (unsigned long long)st.frames_skipped, (unsigned long long)st.frames_acquired, (double)st.mean_latency_ns * 1e-6); + // R2: the service's online vertical-alignment refinement. + static const char *const rs[] = {"n/a", "OFF (DXR_STEREO_CAMERA_REFINE=0)", "MEASURING", + "ALIGNED (calibration within 0.2 px)", "APPLIED"}; + if (st.refine_state != XRT_STEREO_CAMERA_REFINE_NONE && + st.refine_state <= XRT_STEREO_CAMERA_REFINE_APPLIED) { + printf( + "vertical refinement: %s — correction a %+.2f px, b %+.3f px/100 px; %u update(s), " + "%u window(s), %u matches in the latest, %.2f ms per measurement\n", + rs[st.refine_state], st.refine_offset_px, st.refine_slope_per_100px, st.refine_updates, + st.refine_windows, st.refine_matches, st.refine_measure_ms); + if (st.refine_windows > 0) { + printf( + "vertical refinement: signed dy BEFORE (first window, uncorrected) %+.2f px -> " + "latest window %+.2f px; this probe's last frame %s\n", + st.refine_initial_dy_px, st.refine_residual_dy_px, have_rows ? rows_txt : "n/a"); + } + } } unmap_readonly(&map); close_wake(wake); ipc_client_stereo_camera_stream_stop(ipc_c, sid); ipc_client_stereo_camera_stream_destroy(ipc_c, sid); - return got > 0 ? 0 : 4; + if (got == 0) { + return 4; + } + if (require_rectified) { + if (last.output != XRT_STEREO_CAMERA_OUTPUT_RECTIFIED) { + printf("--rectified: FAIL — frames are RAW (the camera is not calibrated / not rectifiable)\n"); + return 5; + } + if (!rows_ok) { + printf( + "--rectified: FAIL — rows do not align (|signed median dy| > 0.5 px, MAD > 1.5 px, or fewer " + "than 20 matched blocks)\n"); + return 5; + } + printf("--rectified: PASS — rows aligned\n"); + } + return 0; } int @@ -639,8 +916,10 @@ cli_cmd_camera(int argc, const char **argv) } else { printf( "usage: displayxr-cli camera list [--json] | calib [--raw|--rectified] [--json] |\n" - " probe [] [--raw] [--format gray8|nv12|bgra8] [--fps F] [--frames N] [--seconds S] " - "[--out DIR]\n"); + " probe [] [--raw|--rectified] [--format gray8|nv12|bgra8] [--fps F] [--frames N] " + "[--seconds S] " + "[--out DIR]\n" + " [--max-disparity N] [--max-dy N (24)] [--min-ncc C (0.90)]\n"); ret = 1; } ipc_client_connection_fini(&ipc_c); diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index f3c18c6555..3e508da75c 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -67,6 +67,8 @@ set(tests tests_comp_color_policy tests_m_matrix_projection tests_stereo_camera + tests_stereo_rectify + tests_stereo_vrefine ) if(XRT_BUILD_DRIVER_QWERTY) list(APPEND tests tests_qwerty_camera_profile tests_qwerty_velocity) @@ -708,3 +710,16 @@ target_include_directories(tests_lift_mailbox PRIVATE ${PROJECT_SOURCE_DIR}/src/ # the ring / conversion / disparity-probe tests (it compiles into the plug-in). target_sources(tests_stereo_camera PRIVATE ${PROJECT_SOURCE_DIR}/src/xrt/drivers/sim_display/sim_display_stereo_camera_pattern.c) target_include_directories(tests_stereo_camera PRIVATE ${PROJECT_SOURCE_DIR}/src/xrt/drivers/sim_display) + +# ADR-043 R2: the rectifier (u_stereo_rectify) against OpenCV-generated golden +# fixtures, and end to end on the sim_display DISTORTED fake (its scene + +# synthetic lens live in the pattern file). +target_sources(tests_stereo_rectify PRIVATE ${PROJECT_SOURCE_DIR}/src/xrt/drivers/sim_display/sim_display_stereo_camera_pattern.c) +target_include_directories(tests_stereo_rectify PRIVATE ${PROJECT_SOURCE_DIR}/src/xrt/drivers/sim_display ${CMAKE_CURRENT_SOURCE_DIR}/fixtures) + +# ADR-043 R2: the online vertical-alignment refinement — on the distorted fake +# with an injected, uncalibrated offset + slope, and on one REAL raw frame from +# a Leia SR laptop (fixtures/stereo_vrefine_sr_raw.jpg, decoded with stb_image). +target_sources(tests_stereo_vrefine PRIVATE ${PROJECT_SOURCE_DIR}/src/xrt/drivers/sim_display/sim_display_stereo_camera_pattern.c) +target_include_directories(tests_stereo_vrefine PRIVATE ${PROJECT_SOURCE_DIR}/src/xrt/drivers/sim_display ${CMAKE_CURRENT_SOURCE_DIR}/fixtures ${PROJECT_SOURCE_DIR}/src/external/stb) +target_compile_definitions(tests_stereo_vrefine PRIVATE STEREO_FIXTURE_DIR="${CMAKE_CURRENT_SOURCE_DIR}/fixtures") diff --git a/tests/fixtures/gen_stereo_rectify_fixtures.py b/tests/fixtures/gen_stereo_rectify_fixtures.py new file mode 100644 index 0000000000..26eb1535fc --- /dev/null +++ b/tests/fixtures/gen_stereo_rectify_fixtures.py @@ -0,0 +1,145 @@ +#!/usr/bin/env python3 +# Copyright 2026, The DisplayXR Project +# SPDX-License-Identifier: BSL-1.0 +"""Generate stereo_rectify_fixtures.h: OpenCV's answers for u_stereo_rectify. + +ADR-043 R2. The runtime's rectifier (src/xrt/auxiliary/util/u_stereo_rectify.c) +has NO OpenCV dependency; these fixtures pin it to OpenCV's +stereoRectify(flags=CALIB_ZERO_DISPARITY, alpha=0) + initUndistortRectifyMap. +Run offline (needs opencv-python + numpy) and commit the header: + + python3 tests/fixtures/gen_stereo_rectify_fixtures.py > tests/fixtures/stereo_rectify_fixtures.h + +Case "sim_distorted" mirrors sim_stereo_camera_truth_init() in +src/xrt/drivers/sim_display/sim_display_stereo_camera_pattern.c — keep the two +in sync (tests_stereo_rectify also checks the C truth against these inputs). +""" +import math + +import cv2 +import numpy as np + + +def sim_truth(w, h): + ideal_fx = (w / 2.0) / math.tan(math.radians(34.0)) + icx, icy = (w - 1) * 0.5, (h - 1) * 0.5 + K1 = np.array([[ideal_fx * 1.012, 0, icx + w * 0.009], [0, ideal_fx * 1.008, icy - h * 0.008], [0, 0, 1]]) + K2 = np.array([[ideal_fx * 0.991, 0, icx - w * 0.007], [0, ideal_fx * 0.994, icy + h * 0.007], [0, 0, 1]]) + D1 = np.array([-0.21, 0.06, 0.0008, -0.0006, -0.005, 0, 0, 0]) + D2 = np.array([-0.17, 0.04, -0.0005, 0.0007, 0.0, 0, 0, 0]) + S, _ = cv2.Rodrigues(np.radians(np.array([0.35, 0.25, 0.45]))) + R = S @ S + T = -S @ np.array([50.0, 0, 0]) + return dict(name="sim_distorted", w=w, h=h, cw=0, ch=0, model=1, K=[K1, K2], D=[D1, D2], R=R, T=T) + + +def sr_like(): + K1 = np.array([[421.7, 0, 318.2], [0, 422.9, 243.6], [0, 0, 1]]) + K2 = np.array([[424.1, 0, 322.9], [0, 425.0, 238.1], [0, 0, 1]]) + D1 = np.array([-0.082, 0.121, 0.0011, -0.0004, -0.061, 0, 0, 0]) + D2 = np.array([-0.075, 0.098, -0.0007, 0.0009, -0.044, 0, 0, 0]) + R, _ = cv2.Rodrigues(np.array([0.0021, -0.0043, 0.0032])) + T = np.array([-63.47, 0.41, -0.83]) + return dict(name="sr_like", w=640, h=480, cw=0, ch=0, model=1, K=[K1, K2], D=[D1, D2], R=R, T=T) + + +def radtan8_scaled(): + # Calibrated at 1280x960, frames at 640x480: the rectifier rescales K. + K1c = np.array([[905.0, 0, 641.5], [0, 903.0, 477.0], [0, 0, 1]]) + K2c = np.array([[899.0, 0, 636.0], [0, 900.5, 483.5], [0, 0, 1]]) + D1 = np.array([0.21, -0.05, 0.0006, 0.0003, 0.01, 0.43, 0.02, 0.004]) + D2 = np.array([0.19, -0.04, -0.0004, 0.0005, 0.008, 0.40, 0.015, 0.003]) + R, _ = cv2.Rodrigues(np.array([-0.0035, 0.0061, -0.0018])) + T = np.array([-40.2, -0.35, 0.52]) + + def scale(K, s): + K = K.copy() + K[0, 0] *= s + K[1, 1] *= s + K[0, 2] = (K[0, 2] + 0.5) * s - 0.5 + K[1, 2] = (K[1, 2] + 0.5) * s - 0.5 + return K + + return dict(name="radtan8_scaled", w=640, h=480, cw=1280, ch=960, model=2, K=[K1c, K2c], D=[D1, D2], R=R, + T=T, Kframe=[scale(K1c, 0.5), scale(K2c, 0.5)]) + + +def converged_cc(Kf, D, R1, R2, w, h): + """OpenCV's principal-point rule with undistortPoints run to convergence. + + cvStereoRectify undistorts the four corners with only 5 fixed-point + iterations, which leaves a strong lens tenths of a pixel (or more) short; + the runtime iterates to convergence. This is the converged reference. + """ + fc = 1e300 + for K, d in zip(Kf, D): + f = K[1, 1] + if d[0] < 0: + f *= 1 + d[0] * (w * w + h * h) / (4 * f * f) + fc = min(fc, f) + corners = np.array([[[0, 0]], [[w - 1, 0]], [[0, h - 1]], [[w - 1, h - 1]]], np.float64) + cc = [] + for K, d, Rr in zip(Kf, D, (R1, R2)): + u = cv2.undistortPointsIter(corners, K, d, None, None, + (cv2.TERM_CRITERIA_COUNT | cv2.TERM_CRITERIA_EPS, 500, 1e-15)).reshape(-1, 2) + q = (Rr @ np.hstack([u, np.ones((4, 1))]).T).T + avg = (q[:, :2] / q[:, 2:3]).mean(axis=0) + cc.append(((w - 1) / 2 - fc * avg[0], (h - 1) / 2 - fc * avg[1])) + return fc, (cc[0][0] + cc[1][0]) / 2, (cc[0][1] + cc[1][1]) / 2 + + +def arr(a): + return "{" + ", ".join("%.17g" % float(x) for x in np.asarray(a).ravel()) + "}" + + +GRID_X, GRID_Y = 7, 5 + + +def emit(c): + w, h = c["w"], c["h"] + Kf = c.get("Kframe", c["K"]) + D = [d if c["model"] == 2 else d[:5] for d in c["D"]] + R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(Kf[0], D[0], Kf[1], D[1], (w, h), c["R"], c["T"], + flags=cv2.CALIB_ZERO_DISPARITY, alpha=0) + fc, ccx, ccy = converged_cc(Kf, D, R1, R2, w, h) + samples = [] + for e, (Rr, P) in enumerate(((R1, P1), (R2, P2))): + mx, my = cv2.initUndistortRectifyMap(Kf[e], D[e], Rr, P, (w, h), cv2.CV_32FC1) + for j in range(GRID_Y): + for i in range(GRID_X): + u = round(i * (w - 1) / (GRID_X - 1)) + v = round(j * (h - 1) / (GRID_Y - 1)) + samples += [e, u, v, float(mx[v, u]), float(my[v, u])] + n = c["name"] + print("static const struct stereo_rectify_fixture k_fixture_%s = {" % n) + print('\t"%s", %d, %d, %d, %d, %d,' % (n, w, h, c["cw"], c["ch"], c["model"])) + print("\t{%s, %s}," % (arr(c["K"][0]), arr(c["K"][1]))) + print("\t{%s, %s}," % (arr(c["D"][0]), arr(c["D"][1]))) + print("\t%s, %s," % (arr(c["R"]), arr(c["T"]))) + print("\t%s, %s, %s, %s," % (arr(R1), arr(R2), arr(P1), arr(P2))) + print("\t%s," % arr([fc, ccx, ccy])) + print("\t%s," % arr(samples)) + print("};\n") + + +def main(): + print("// Copyright 2026, The DisplayXR Project") + print("// SPDX-License-Identifier: BSL-1.0") + print("// GENERATED by gen_stereo_rectify_fixtures.py with OpenCV %s — do not edit." % cv2.__version__) + print("// OpenCV stereoRectify(CALIB_ZERO_DISPARITY, alpha=0) + initUndistortRectifyMap samples.") + print("#pragma once\n") + print("#define STEREO_RECTIFY_FIXTURE_SAMPLES (2 * %d * %d)\n" % (GRID_X, GRID_Y)) + print("struct stereo_rectify_fixture\n{") + print("\tconst char *name;\n\tint width, height, calib_width, calib_height, model;") + print("\tdouble K[2][9]; //!< as calibrated (calib size when set)\n\tdouble D[2][8];") + print("\tdouble R[9], T[3];") + print("\tdouble R1[9], R2[9], P1[12], P2[12]; //!< OpenCV's answer") + print("\tdouble converged[3]; //!< pre-crop focal, cx, cy with converged undistortPoints") + print("\tdouble samples[STEREO_RECTIFY_FIXTURE_SAMPLES * 5]; //!< eye, u, v, map_x, map_y (OpenCV R/P)") + print("};\n") + for c in (sim_truth(640, 480), sr_like(), radtan8_scaled()): + emit(c) + + +if __name__ == "__main__": + main() diff --git a/tests/fixtures/gen_stereo_vrefine_sr_fixture.py b/tests/fixtures/gen_stereo_vrefine_sr_fixture.py new file mode 100644 index 0000000000..68176eb425 --- /dev/null +++ b/tests/fixtures/gen_stereo_vrefine_sr_fixture.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python3 +# Copyright 2026, The DisplayXR Project +# SPDX-License-Identifier: BSL-1.0 +"""Generate the REAL-hardware fixture for the vertical-alignment refinement. + +ADR-043 R2. One raw side-by-side GRAY8 frame (1280x480) from a Leia SR laptop's +tracking camera (a ceiling scene, camera 150), recompressed as a JPEG (the PNG +is 297 KB of sensor noise; JPEG q90 keeps the statistics: the signed residual +reads the same +1.7 px), plus the device's calibration as the plug-in hands it +to the runtime (left eye = camera 2 = M2/D2, right = camera 1 = M1/D1, +R' = R^T, T' = -R^T T) and an INDEPENDENT reference: SIFT matches found by +OpenCV on OpenCV's own rectification of the same frame, mapped back to RAW +pixels through OpenCV's maps. tests_stereo_rectify maps them forward through +the runtime's (corrected) rectification and asserts the row residual. + +The runtime never depends on OpenCV; run this offline and commit both outputs: + + python3 tests/fixtures/gen_stereo_vrefine_sr_fixture.py tests/fixtures +""" +import sys + +import cv2 +import numpy as np + +M1 = np.array([[4.7623373282393203e+02, 0, 3.1361746988405270e+02], [0, 4.7573318336977724e+02, 2.4590743710177546e+02], [0, 0, 1]]) +D1 = np.array([1.2332390163153338e-01, -1.8065702171496908e-01, -1.7852161465149287e-03, 7.0567885578275071e-04, 8.7485984937824945e-02]) +M2 = np.array([[4.7753198103487864e+02, 0, 3.2061581703236254e+02], [0, 4.7719412297375959e+02, 2.2625301107241927e+02], [0, 0, 1]]) +D2 = np.array([1.0906040224963771e-01, -1.2479660062568124e-01, -1.1093907046283672e-03, 1.0008674090348705e-03, 2.5815446523384768e-02]) +R = np.array([9.9998661420584478e-01, -2.5373925908838452e-03, 4.5092181108025075e-03, 2.5193814304075575e-03, 9.9998884428889723e-01, + 3.9954993384893533e-03, -4.5193059576863282e-03, -3.9840854151836471e-03, 9.9998185130384509e-01]).reshape(3, 3) +T = np.array([1.1996226177065833e+02, -3.9419132551517166e-01, -8.7010595776014110e-01]).reshape(3, 1) + + +def bilinear(m, x, y): + x0, y0 = int(np.floor(x)), int(np.floor(y)) + fx, fy = x - x0, y - y0 + return (m[y0, x0] * (1 - fx) * (1 - fy) + m[y0, x0 + 1] * fx * (1 - fy) + m[y0 + 1, x0] * (1 - fx) * fy + + m[y0 + 1, x0 + 1] * fx * fy) + + +def main(): + src, outdir = sys.argv[1], sys.argv[2] + raw = cv2.imread(src, 0) + cv2.imwrite(outdir + "/stereo_vrefine_sr_raw.jpg", raw, [cv2.IMWRITE_JPEG_QUALITY, 90]) + raw = cv2.imread(outdir + "/stereo_vrefine_sr_raw.jpg", 0) # what the test sees + L, Rr = raw[:, :640], raw[:, 640:] + KL, DL, KR, DR = M2, D2, M1, D1 + Rl2r, Tl2r = R.T, -R.T @ T + R1, R2, P1, P2, _, _, _ = cv2.stereoRectify(KL, DL, KR, DR, (640, 480), Rl2r, Tl2r, alpha=0, + flags=cv2.CALIB_ZERO_DISPARITY) + mL = cv2.initUndistortRectifyMap(KL, DL, R1, P1, (640, 480), cv2.CV_32FC1) + mR = cv2.initUndistortRectifyMap(KR, DR, R2, P2, (640, 480), cv2.CV_32FC1) + a, b = cv2.remap(L, *mL, cv2.INTER_LINEAR), cv2.remap(Rr, *mR, cv2.INTER_LINEAR) + sift = cv2.SIFT_create(8000) + ka, da = sift.detectAndCompute(a, None) + kb, db = sift.detectAndCompute(b, None) + good = [x for x, y in cv2.BFMatcher().knnMatch(da, db, k=2) if x.distance < 0.7 * y.distance] + pa = np.float32([ka[g.queryIdx].pt for g in good]) + pb = np.float32([kb[g.trainIdx].pt for g in good]) + d, dy = pa[:, 0] - pb[:, 0], pb[:, 1] - pa[:, 1] + ok = (d > 0) & (d < 200) & (np.abs(dy - np.median(dy)) < 5) + pa, pb, dy = pa[ok], pb[ok], dy[ok] + rows = [] + for (xa, ya), (xb, yb) in zip(pa, pb): + if not (1 <= xa < 638 and 1 <= ya < 478 and 1 <= xb < 638 and 1 <= yb < 478): + continue + rows.append((bilinear(mL[0], xa, ya), bilinear(mL[1], xa, ya), bilinear(mR[0], xb, yb), bilinear(mR[1], xb, yb))) + fit = np.linalg.lstsq(np.c_[np.ones(len(dy)), pa[:, 1] - P1[1, 2]], dy, rcond=None)[0] + with open(outdir + "/stereo_vrefine_sr_fixture.h", "w") as f: + def p(*x): + print(*x, file=f) + p("// Copyright 2026, The DisplayXR Project") + p("// SPDX-License-Identifier: BSL-1.0") + p("// GENERATED by gen_stereo_vrefine_sr_fixture.py (OpenCV %s) — do not edit." % cv2.__version__) + p("// A Leia SR laptop tracking camera: calibration as the plug-in hands it over, and") + p("// SIFT matches (OpenCV, on OpenCV's rectification) mapped back to RAW pixels.") + p("#pragma once") + p("// clang-format off") + p("") + p("struct stereo_vrefine_sr_match { double lx, ly, rx, ry; }; // RAW pixels, left / right eye") + p("") + p("static const double k_sr_K[2][4] = { // fx fy cx cy; [0] = left = camera 2") + p(" {%.17g, %.17g, %.17g, %.17g}," % (KL[0, 0], KL[1, 1], KL[0, 2], KL[1, 2])) + p(" {%.17g, %.17g, %.17g, %.17g}," % (KR[0, 0], KR[1, 1], KR[0, 2], KR[1, 2])) + p("};") + p("static const double k_sr_D[2][5] = {") + p(" {%s}," % ", ".join("%.17g" % v for v in DL)) + p(" {%s}," % ", ".join("%.17g" % v for v in DR)) + p("};") + p("static const double k_sr_R[3][3] = {%s};" % ", ".join("{%s}" % ", ".join("%.17g" % v for v in r) for r in Rl2r)) + p("static const double k_sr_T[3] = {%s};" % ", ".join("%.17g" % v for v in Tl2r.ravel())) + p("// OpenCV's rectification of the same frame: signed dy median %+.3f px over %d matches," % (np.median(dy), len(dy))) + p("// fit dy = %+.3f %+.4f * (y - cy) px." % (fit[0], fit[1])) + p("static const double k_sr_ocv_median_dy = %.6f;" % np.median(dy)) + p("static const struct stereo_vrefine_sr_match k_sr_matches[] = {") + for r in rows: + p(" {%.4f, %.4f, %.4f, %.4f}," % r) + p("};") + p("// clang-format on") + print("matches", len(rows), "median dy", np.median(dy), "fit", fit) + + +if __name__ == "__main__": + main() diff --git a/tests/fixtures/stereo_rectify_fixtures.h b/tests/fixtures/stereo_rectify_fixtures.h new file mode 100644 index 0000000000..58a1296487 --- /dev/null +++ b/tests/fixtures/stereo_rectify_fixtures.h @@ -0,0 +1,50 @@ +// Copyright 2026, The DisplayXR Project +// SPDX-License-Identifier: BSL-1.0 +// GENERATED by gen_stereo_rectify_fixtures.py with OpenCV 4.10.0 — do not edit. +// OpenCV stereoRectify(CALIB_ZERO_DISPARITY, alpha=0) + initUndistortRectifyMap samples. +#pragma once + +#define STEREO_RECTIFY_FIXTURE_SAMPLES (2 * 7 * 5) + +struct stereo_rectify_fixture +{ + const char *name; + int width, height, calib_width, calib_height, model; + double K[2][9]; //!< as calibrated (calib size when set) + double D[2][8]; + double R[9], T[3]; + double R1[9], R2[9], P1[12], P2[12]; //!< OpenCV's answer + double converged[3]; //!< pre-crop focal, cx, cy with converged undistortPoints + double samples[STEREO_RECTIFY_FIXTURE_SAMPLES * 5]; //!< eye, u, v, map_x, map_y (OpenCV R/P) +}; + +static const struct stereo_rectify_fixture k_fixture_sim_distorted = { + "sim_distorted", 640, 480, 0, 0, 1, + {{480.11254404316571, 0, 325.25999999999999, 0, 478.21486600346941, 235.66, 0, 0, 1}, {470.14973433476013, 0, 315.01999999999998, 0, 471.57299286453235, 242.86000000000001, 0, 0, 1}}, + {{-0.20999999999999999, 0.059999999999999998, 0.00080000000000000004, -0.00059999999999999995, -0.0050000000000000001, 0, 0, 0}, {-0.17000000000000001, 0.040000000000000001, -0.00050000000000000001, 0.00069999999999999999, 0, 0, 0, 0}}, + {0.99983855911978303, -0.015653421241978178, 0.0088219102634899179, 0.015760033144008215, 0.9998020064676586, -0.012147807149594446, -0.0086300088398358569, 0.012284879595061634, 0.99988729598928239}, {-49.99798192944499, -0.39335770014464566, 0.21696244520423971}, + {0.9999596385888998, -0.0078405002408463032, 0.0043872256757703267, 0.0078671540028936084, 0.99995050015619791, -0.0060913976445827285, -0.0043392489040851804, 0.0061256667672149765, 0.99997182316583566}, {0.9999596385888998, 0.0078671540028936084, -0.0043392489040851804, -0.0078405002408463032, 0.99995050015619791, 0.0061256667672149765, 0.0043872256757703267, -0.0060913976445827285, 0.99997182316583566}, {452.68462339907495, 0, 320.57709884643555, 0, 0, 452.68462339907495, 239.3410758972168, 0, 0, 0, 1, 0}, {452.68462339907495, 0, 320.57709884643555, -22634.231169953749, 0, 452.68462339907495, 239.3410758972168, 0, 0, 0, 1, 0}, + {407.95356216571662, 320.42562266203367, 239.36410779466294}, + {0, 0, 0, 25.628753662109375, 20.80931282043457, 0, 106, 0, 114.78317260742188, 10.564011573791504, 0, 213, 0, 214.73896789550781, 3.1000354290008545, 0, 320, 0, 320.75457763671875, -0.008007747121155262, 0, 426, 0, 425.70660400390625, 1.7409622669219971, 0, 532, 0, 524.57373046875, 7.8479142189025879, 0, 639, 0, 614.207275390625, 17.065437316894531, 0, 0, 120, 16.38664436340332, 127.46588134765625, 0, 106, 120, 107.5118408203125, 121.24662017822266, 0, 213, 120, 211.09086608886719, 116.63072967529297, 0, 320, 120, 321.60733032226562, 114.47244262695312, 0, 426, 120, 431.00091552734375, 115.06279754638672, 0, 532, 120, 533.433349609375, 118.15943145751953, 0, 639, 120, 625.0633544921875, 123.12503051757812, 0, 0, 240, 13.342729568481445, 241.57992553710938, 0, 106, 240, 105.31760406494141, 240.92079162597656, 0, 213, 240, 210.32466125488281, 240.15034484863281, 0, 320, 240, 322.56997680664062, 239.2904052734375, 0, 426, 240, 433.653076171875, 238.39909362792969, 0, 532, 240, 537.4432373046875, 237.53233337402344, 0, 639, 240, 629.86065673828125, 236.74293518066406, 0, 0, 359, 17.423341751098633, 354.94357299804688, 0, 106, 359, 108.85330963134766, 359.81146240234375, 0, 213, 359, 212.74403381347656, 362.88214111328125, 0, 320, 359, 323.56402587890625, 363.33782958984375, 0, 426, 359, 433.21401977539062, 360.99703979492188, 0, 532, 359, 535.8306884765625, 356.21051025390625, 0, 639, 359, 627.58306884765625, 349.72073364257812, 0, 0, 479, 27.618436813354492, 462.25592041015625, 0, 106, 479, 117.3876953125, 471.15267944335938, 0, 213, 479, 217.93798828125, 477.12750244140625, 0, 320, 479, 324.51718139648438, 478.60397338867188, 0, 426, 479, 429.94891357421875, 475.16604614257812, 0, 532, 479, 529.18408203125, 467.41690063476562, 0, 639, 479, 619.11138916015625, 456.72235107421875, 1, 0, 0, 22.116100311279297, 15.563237190246582, 1, 106, 0, 112.76789093017578, 7.8436222076416016, 1, 213, 0, 212.92782592773438, 2.6479806900024414, 1, 320, 0, 318.38385009765625, 1.1816521883010864, 1, 426, 0, 423.07159423828125, 3.7653830051422119, 1, 532, 0, 522.976806640625, 9.9769821166992188, 1, 639, 0, 615.55633544921875, 18.768739700317383, 1, 0, 120, 13.51005744934082, 124.00925445556641, 1, 106, 120, 105.67232513427734, 119.92747497558594, 1, 213, 120, 208.59645080566406, 117.38086700439453, 1, 320, 120, 317.45404052734375, 116.96125793457031, 1, 426, 120, 425.53848266601562, 118.80167388916016, 1, 532, 120, 528.25469970703125, 122.67184448242188, 1, 639, 120, 622.42694091796875, 128.04299926757812, 1, 0, 240, 10.047764778137207, 238.30494689941406, 1, 106, 240, 102.69903564453125, 239.01350402832031, 1, 213, 240, 206.51715087890625, 239.81143188476562, 1, 320, 240, 316.47799682617188, 240.6690673828125, 1, 426, 240, 425.67752075195312, 241.53524780273438, 1, 532, 240, 529.34136962890625, 242.36903381347656, 1, 639, 240, 624.0872802734375, 243.13426208496094, 1, 0, 359, 12.477910041809082, 351.41976928710938, 1, 106, 359, 104.35537719726562, 356.85409545898438, 1, 213, 359, 206.97434997558594, 360.9371337890625, 1, 320, 359, 315.52447509765625, 363.03515625, 1, 426, 359, 423.33761596679688, 362.92056274414062, 1, 532, 359, 525.84564208984375, 360.74029541015625, 1, 639, 359, 619.86956787109375, 356.94107055664062, 1, 0, 479, 20.100147247314453, 459.20794677734375, 1, 106, 479, 110.1802978515625, 468.25103759765625, 1, 213, 479, 209.76011657714844, 474.91726684570312, 1, 320, 479, 314.64419555664062, 477.99530029296875, 1, 426, 479, 418.82501220703125, 477.0888671875, 1, 532, 479, 518.32244873046875, 472.53683471679688, 1, 639, 479, 610.56573486328125, 465.28729248046875}, +}; + +static const struct stereo_rectify_fixture k_fixture_sr_like = { + "sr_like", 640, 480, 0, 0, 1, + {{421.69999999999999, 0, 318.19999999999999, 0, 422.89999999999998, 243.59999999999999, 0, 0, 1}, {424.10000000000002, 0, 322.89999999999998, 0, 425, 238.09999999999999, 0, 0, 1}}, + {{-0.082000000000000003, 0.121, 0.0011000000000000001, -0.00040000000000000002, -0.060999999999999999, 0, 0, 0}, {-0.074999999999999997, 0.098000000000000004, -0.00069999999999999999, 0.00089999999999999998, -0.043999999999999997, 0, 0, 0}}, + {0.99998563503967131, -0.0032044973128937176, -0.0042966162589852044, 0.0031954673378315228, 0.99999267502022915, -0.0021068683820189863, 0.0043033362404268378, 0.0020931084200194522, 0.99998855003162102}, {-63.469999999999999, 0.40999999999999998, -0.82999999999999996}, + {0.99991491406671129, -0.0096357981924167398, 0.0087929528689994867, 0.0096451782567072188, 0.99995295886120927, -0.001024988345914208, -0.0087826626576323896, 0.0011097107316485149, 0.99996081592167119}, {0.99989364827379268, -0.006459057756292289, 0.01307565350664453, 0.0064450971160348661, 0.99997861459363835, 0.0011095397931037939, -0.013082540460087409, -0.001025147934925552, 0.99991389439632339}, {429.86495602842393, 0, 313.38402938842773, 0, 0, 429.86495602842393, 241.02614402770996, 0, 0, 0, 1, 0}, {429.86495602842393, 0, 313.38402938842773, -27286.430718132982, 0, 429.86495602842393, 241.02614402770996, 0, 0, 0, 1, 0}, + {391.87611728541026, 313.84673321042442, 240.91068135796641}, + {0, 0, 0, 9.2827739715576172, 13.777859687805176, 0, 106, 0, 112.01531982421875, 12.907480239868164, 0, 213, 0, 215.40269470214844, 11.963018417358398, 0, 320, 0, 318.64385986328125, 11.201150894165039, 0, 426, 0, 420.40866088867188, 11.097481727600098, 0, 532, 0, 521.31646728515625, 11.158949851989746, 0, 639, 0, 622.492431640625, 11.143622398376465, 0, 0, 120, 9.8415422439575195, 129.75579833984375, 0, 106, 120, 112.40514373779297, 128.54570007324219, 0, 213, 120, 215.66424560546875, 126.94230651855469, 0, 320, 120, 319.816650390625, 125.78262329101562, 0, 426, 120, 422.45095825195312, 125.53066253662109, 0, 532, 120, 523.2119140625, 125.69633483886719, 0, 639, 120, 624.20782470703125, 125.48140716552734, 0, 0, 240, 10.620198249816895, 246.25874328613281, 0, 106, 240, 112.88777160644531, 245.1260986328125, 0, 213, 240, 216.22691345214844, 244.03797912597656, 0, 320, 240, 320.97607421875, 242.9971923828125, 0, 426, 240, 424.17300415039062, 242.03355407714844, 0, 532, 240, 524.97601318359375, 241.15110778808594, 0, 639, 240, 625.6756591796875, 240.32371520996094, 0, 0, 359, 11.514342308044434, 362.07369995117188, 0, 106, 359, 114.25772857666016, 361.017822265625, 0, 213, 359, 217.71333312988281, 360.45199584960938, 0, 320, 359, 322.0709228515625, 359.54281616210938, 0, 426, 359, 424.89013671875, 357.87509155273438, 0, 532, 359, 525.81005859375, 355.93862915039062, 0, 639, 359, 626.96600341796875, 354.48715209960938, 0, 0, 479, 12.622738838195801, 478.9141845703125, 0, 106, 479, 115.73625183105469, 477.50299072265625, 0, 213, 479, 219.4949951171875, 476.28683471679688, 0, 320, 479, 323.11593627929688, 475.00119018554688, 0, 426, 479, 425.24160766601562, 473.18679809570312, 0, 532, 479, 526.49114990234375, 471.33016967773438, 0, 639, 479, 628.023193359375, 469.67559814453125, 1, 0, 0, 10.344363212585449, 3.6066198348999023, 1, 106, 0, 114.04583740234375, 3.4317293167114258, 1, 213, 0, 218.2841796875, 2.9229519367218018, 1, 320, 0, 322.4739990234375, 2.5816488265991211, 1, 426, 0, 425.1719970703125, 2.8780093193054199, 1, 532, 0, 526.87469482421875, 3.3557379245758057, 1, 639, 0, 628.93365478515625, 3.4970929622650146, 1, 0, 120, 11.003759384155273, 121.06476593017578, 1, 106, 120, 114.13439178466797, 120.36711120605469, 1, 213, 120, 218.17823791503906, 119.25482177734375, 1, 320, 120, 323.13818359375, 118.52081298828125, 1, 426, 120, 426.58740234375, 118.56651306152344, 1, 532, 120, 528.11944580078125, 118.98307800292969, 1, 639, 120, 629.578857421875, 119.02649688720703, 1, 0, 240, 11.82154369354248, 238.47807312011719, 1, 106, 240, 114.50177001953125, 237.90115356445312, 1, 213, 240, 218.50215148925781, 237.27389526367188, 1, 320, 240, 323.87161254882812, 236.60771179199219, 1, 426, 240, 427.711669921875, 235.92776489257812, 1, 532, 240, 529.1932373046875, 235.23455810546875, 1, 639, 240, 630.21673583984375, 234.50439453125, 1, 0, 359, 12.93834400177002, 354.7034912109375, 1, 106, 359, 115.89998626708984, 354.262451171875, 1, 213, 359, 219.80398559570312, 354.13876342773438, 1, 320, 359, 324.64675903320312, 353.55496215820312, 1, 426, 359, 427.9708251953125, 352.15399169921875, 1, 532, 359, 529.348876953125, 350.34158325195312, 1, 639, 359, 630.63555908203125, 348.82196044921875, 1, 0, 479, 14.214091300964355, 471.5322265625, 1, 106, 479, 117.6163330078125, 470.56671142578125, 1, 213, 479, 221.54965209960938, 469.88070678710938, 1, 320, 479, 325.46670532226562, 468.93719482421875, 1, 426, 479, 427.88900756835938, 467.2821044921875, 1, 532, 479, 529.28631591796875, 465.37380981445312, 1, 639, 479, 631.0482177734375, 463.74661254882812}, +}; + +static const struct stereo_rectify_fixture k_fixture_radtan8_scaled = { + "radtan8_scaled", 640, 480, 1280, 960, 2, + {{905, 0, 641.5, 0, 903, 477, 0, 0, 1}, {899, 0, 636, 0, 900.5, 483.5, 0, 0, 1}}, + {{0.20999999999999999, -0.050000000000000003, 0.00059999999999999995, 0.00029999999999999997, 0.01, 0.42999999999999999, 0.02, 0.0040000000000000001}, {0.19, -0.040000000000000001, -0.00040000000000000002, 0.00050000000000000001, 0.0080000000000000002, 0.40000000000000002, 0.014999999999999999, 0.0030000000000000001}}, + {0.9999797750888213, 0.0017893092369226367, 0.0061030964079741151, -0.0018106591431606817, 0.99999225503401346, 0.0034944792825245545, -0.0060967964356415764, -0.0035054592343041206, 0.99997527010860576}, {-40.200000000000003, -0.34999999999999998, 0.52000000000000002}, + {0.99992132904265318, 0.010539772551984196, -0.0068006558600212045, -0.010527985703966673, 0.99994301857135048, 0.0017666712860391006, 0.0068188886624635211, -0.0016949350926458114, 0.99997531467153766}, {0.99987845957661869, 0.0087054094739190593, -0.012933751218397103, -0.0087277833316827796, 0.99996051023646515, -0.0016744449060687062, 0.012918663739071023, 0.0017871243716259515, 0.99991495354038851}, {425.59643371580466, 0, 328.47037124633789, 0, 0, 425.59643371580466, 240.27606582641602, 0, 0, 0, 1, 0}, {425.59643371580466, 0, 328.47037124633789, -17111.056320405034, 0, 425.59643371580466, 240.27606582641602, 0, 0, 0, 1, 0}, + {450.25, 328.81033655905583, 240.34823533756631}, + {0, 0, 0, 39.806583404541016, 26.450933456420898, 0, 106, 0, 118.28645324707031, 12.094629287719727, 0, 213, 0, 213.35528564453125, 2.9100487232208252, 0, 320, 0, 317.53338623046875, -0.0073644723743200302, 0, 426, 0, 421.48922729492188, 3.6148638725280762, 0, 532, 0, 517.80120849609375, 13.458554267883301, 0, 639, 0, 599.892333984375, 28.825506210327148, 0, 0, 120, 23.845294952392578, 124.79419708251953, 0, 106, 120, 106.19843292236328, 117.49972534179688, 0, 213, 120, 206.2528076171875, 113.09403228759766, 0, 320, 120, 316.01846313476562, 112.08939361572266, 0, 426, 120, 425.62353515625, 114.57035064697266, 0, 532, 120, 527.20294189453125, 120.33719635009766, 0, 639, 120, 613.689697265625, 129.01432800292969, 0, 0, 240, 17.447366714477539, 234.26361083984375, 0, 106, 240, 101.13021850585938, 234.98895263671875, 0, 213, 240, 202.89291381835938, 235.96000671386719, 0, 320, 240, 314.58450317382812, 237.097412109375, 0, 426, 240, 426.1614990234375, 238.29718017578125, 0, 532, 240, 529.61895751953125, 239.47416687011719, 0, 639, 240, 617.7322998046875, 240.55113220214844, 0, 0, 359, 21.263168334960938, 343.102783203125, 0, 106, 359, 103.56227874755859, 351.74288940429688, 0, 213, 359, 203.59284973144531, 358.00244140625, 0, 320, 359, 313.385498046875, 361.22146606445312, 0, 426, 359, 423.08840942382812, 361.11367797851562, 0, 532, 359, 524.851806640625, 357.70999145507812, 0, 639, 359, 611.6148681640625, 351.23043823242188, 0, 0, 479, 34.816947937011719, 442.3594970703125, 0, 106, 479, 113.21434783935547, 457.9637451171875, 0, 213, 479, 208.26432800292969, 468.88372802734375, 0, 320, 479, 312.52322387695312, 473.8944091796875, 0, 426, 479, 416.69427490234375, 472.53567504882812, 0, 532, 479, 513.38165283203125, 464.96435546875, 0, 639, 479, 596.0145263671875, 451.72686767578125, 1, 0, 0, 37.449436187744141, 29.096076965332031, 1, 106, 0, 117.55707550048828, 15.905220031738281, 1, 213, 0, 212.98945617675781, 7.2534098625183105, 1, 320, 0, 316.95709228515625, 4.2553362846374512, 1, 426, 0, 420.79556274414062, 7.2715229988098145, 1, 532, 0, 517.6470947265625, 16.048103332519531, 1, 639, 0, 601.45654296875, 29.928749084472656, 1, 0, 120, 23.134662628173828, 129.13873291015625, 1, 106, 120, 106.65638732910156, 122.45317840576172, 1, 213, 120, 206.61662292480469, 118.25478363037109, 1, 320, 120, 315.77243041992188, 117.09933471679688, 1, 426, 120, 424.88827514648438, 119.14150238037109, 1, 532, 120, 526.606689453125, 124.21871948242188, 1, 639, 120, 614.3831787109375, 131.96627807617188, 1, 0, 240, 17.487483978271484, 239.18548583984375, 1, 106, 240, 102.14824676513672, 240.00784301757812, 1, 213, 240, 203.65617370605469, 240.94680786132812, 1, 320, 240, 314.61508178710938, 241.93450927734375, 1, 426, 240, 425.58609008789062, 242.88748168945312, 1, 532, 240, 529.03094482421875, 243.74014282226562, 1, 639, 240, 618.23284912109375, 244.43392944335938, 1, 0, 359, 21.163570404052734, 348.37802124023438, 1, 106, 359, 104.54096984863281, 356.64190673828125, 1, 213, 359, 204.43681335449219, 362.66021728515625, 1, 320, 359, 313.60433959960938, 365.74667358398438, 1, 426, 359, 422.789306640625, 365.58413696289062, 1, 532, 359, 524.60986328125, 362.20648193359375, 1, 639, 359, 612.49774169921875, 355.86325073242188, 1, 0, 479, 33.737724304199219, 448.59530639648438, 1, 106, 479, 113.56036376953125, 463.35128784179688, 1, 213, 479, 208.85313415527344, 473.77713012695312, 1, 320, 479, 312.8216552734375, 478.62850952148438, 1, 426, 479, 416.76837158203125, 477.38937377929688, 1, 532, 479, 513.79119873046875, 470.19485473632812, 1, 639, 479, 597.79248046875, 457.59927368164062}, +}; + diff --git a/tests/fixtures/stereo_vrefine_sr_fixture.h b/tests/fixtures/stereo_vrefine_sr_fixture.h new file mode 100644 index 0000000000..f34b96935f --- /dev/null +++ b/tests/fixtures/stereo_vrefine_sr_fixture.h @@ -0,0 +1,89 @@ +// Copyright 2026, The DisplayXR Project +// SPDX-License-Identifier: BSL-1.0 +// GENERATED by gen_stereo_vrefine_sr_fixture.py (OpenCV 4.10.0) — do not edit. +// A Leia SR laptop tracking camera: calibration as the plug-in hands it over, and +// SIFT matches (OpenCV, on OpenCV's rectification) mapped back to RAW pixels. +#pragma once +// clang-format off + +struct stereo_vrefine_sr_match { double lx, ly, rx, ry; }; // RAW pixels, left / right eye + +static const double k_sr_K[2][4] = { // fx fy cx cy; [0] = left = camera 2 + {477.53198103487864, 477.19412297375959, 320.61581703236254, 226.25301107241927}, + {476.23373282393203, 475.73318336977724, 313.6174698840527, 245.90743710177546}, +}; +static const double k_sr_D[2][5] = { + {0.10906040224963771, -0.12479660062568124, -0.0011093907046283672, 0.0010008674090348705, 0.025815446523384768}, + {0.12332390163153338, -0.18065702171496908, -0.0017852161465149287, 0.00070567885578275071, 0.087485984937824945}, +}; +static const double k_sr_R[3][3] = {{0.99998661420584478, 0.0025193814304075575, -0.0045193059576863282}, {-0.0025373925908838452, 0.99998884428889723, -0.0039840854151836471}, {0.0045092181108025075, 0.0039954993384893533, 0.99998185130384509}}; +static const double k_sr_T[3] = {-119.96359513724906, 0.69511170577718517, 0.33072915426274135}; +// OpenCV's rectification of the same frame: signed dy median +1.713 px over 63 matches, +// fit dy = +1.196 +0.0029 * (y - cy) px. +static const double k_sr_ocv_median_dy = 1.713043; +static const struct stereo_vrefine_sr_match k_sr_matches[] = { + {42.6564, 426.7630, 27.4999, 444.9824}, + {42.6564, 426.7630, 27.4999, 444.9824}, + {45.4189, 430.8741, 28.6479, 450.2414}, + {46.1918, 398.6357, 29.8170, 418.2359}, + {49.3301, 403.5512, 32.8850, 423.4252}, + {52.4935, 393.9206, 36.4898, 413.5976}, + {72.5452, 440.0057, 60.4815, 459.6938}, + {75.2405, 393.6760, 58.5862, 413.1019}, + {75.5995, 402.3505, 59.3827, 421.6918}, + {76.2475, 440.4224, 60.4815, 459.6938}, + {79.4465, 399.2680, 63.2164, 418.6798}, + {82.6062, 402.6128, 66.1633, 421.5816}, + {84.8178, 441.8156, 69.7202, 460.9757}, + {86.8865, 393.6164, 69.8893, 412.8817}, + {91.7771, 407.4662, 75.3370, 426.7444}, + {91.9771, 431.8771, 75.2770, 451.4111}, + {94.5774, 436.6982, 80.3050, 455.8034}, + {108.1426, 393.5991, 92.4104, 412.8610}, + {116.8991, 303.6753, 94.2693, 323.5830}, + {123.2176, 358.5301, 106.7471, 378.3642}, + {123.5129, 359.1039, 106.7471, 378.3642}, + {125.8485, 371.8545, 109.2153, 391.4623}, + {128.5398, 367.5976, 111.9416, 387.3667}, + {128.5398, 367.5976, 111.9416, 387.3667}, + {129.3160, 353.3312, 112.7393, 372.7762}, + {136.2990, 357.6942, 119.9553, 377.4528}, + {138.7735, 348.3766, 122.0013, 367.9796}, + {138.7846, 358.1171, 121.9061, 378.6802}, + {139.6527, 342.8509, 123.1754, 362.7422}, + {143.6656, 371.9719, 126.6894, 391.4378}, + {148.4452, 364.5681, 131.8349, 384.1829}, + {163.9910, 357.7573, 146.8525, 377.0127}, + {417.9524, 378.5617, 399.9272, 396.7706}, + {447.3165, 371.4063, 429.8571, 389.2461}, + {457.6725, 365.3000, 436.5449, 383.7210}, + {463.4951, 378.0696, 445.4244, 395.7342}, + {482.1422, 364.9758, 463.9877, 383.3140}, + {482.7975, 392.2909, 464.6279, 409.6623}, + {490.8043, 387.2012, 471.6787, 405.1959}, + {507.0309, 398.0456, 493.1158, 415.8319}, + {512.5277, 397.9995, 493.1158, 415.8319}, + {524.7873, 406.4124, 505.5220, 424.1204}, + {532.0674, 446.3396, 512.3051, 463.5432}, + {532.5904, 432.4044, 512.7141, 449.9389}, + {533.2227, 439.8077, 513.0821, 456.9149}, + {551.1292, 409.8321, 531.1552, 427.1079}, + {551.1292, 409.8321, 531.1552, 427.1079}, + {586.0905, 437.3112, 566.1793, 455.1066}, + {587.0881, 446.6747, 566.9643, 463.7476}, + {588.3311, 442.3581, 568.1820, 460.1863}, + {589.0005, 449.9309, 568.6943, 466.9713}, + {590.7910, 426.7519, 570.5395, 443.9697}, + {590.7910, 426.7519, 570.5395, 443.9697}, + {598.5165, 441.1796, 578.2290, 458.3316}, + {599.2280, 435.7159, 578.2069, 452.9794}, + {602.1029, 440.5636, 581.8928, 457.8357}, + {603.0875, 449.2186, 582.8894, 466.3867}, + {605.0008, 446.1069, 585.2899, 464.0975}, + {606.7680, 452.2210, 586.3251, 468.9636}, + {607.5725, 442.2518, 587.0052, 460.8015}, + {611.3437, 453.8625, 590.4322, 472.3528}, + {613.6957, 446.9045, 592.9471, 463.1025}, + {618.1396, 410.9941, 597.7014, 427.6205}, +}; +// clang-format on diff --git a/tests/fixtures/stereo_vrefine_sr_raw.jpg b/tests/fixtures/stereo_vrefine_sr_raw.jpg new file mode 100644 index 0000000000..d1ed289244 Binary files /dev/null and b/tests/fixtures/stereo_vrefine_sr_raw.jpg differ diff --git a/tests/tests_stereo_rectify.cpp b/tests/tests_stereo_rectify.cpp new file mode 100644 index 0000000000..8637968f7c --- /dev/null +++ b/tests/tests_stereo_rectify.cpp @@ -0,0 +1,894 @@ +// Copyright 2026, The DisplayXR Project +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief XR_DXR_stereo_camera (ADR-043) R2: the vendor-neutral rectifier. + * + * - GOLDEN: u_stereo_rectify_compute against OpenCV 4.10's + * stereoRectify(CALIB_ZERO_DISPARITY, alpha = 0) on three calibrations + * (the sim_display distorted fake, an SR-like tracker pair with a + * non-axial baseline, a RADTAN8 pair calibrated at 2x the frame size), and + * the per-pixel maps against initUndistortRectifyMap samples + * (fixtures/stereo_rectify_fixtures.h, generated offline); + * - the lens models invert (RADTAN5/8, KB4) and Rodrigues round-trips; + * - alpha = 0 really holds: every output pixel of both eyes samples inside its + * raw image (no black corners); + * - END TO END on the sim_display DISTORTED fake: the raw pair is visibly + * misaligned; after the CPU LUT the vertical disparity on matched blocks is + * <= 0.5 px and the horizontal disparity equals f_rect * B / Z within + * 0.5 px, for the background and the bar; + * - the NV12 / BGRA8 plane paths agree with GRAY8; + * - [.perf] (hidden): CPU cost per 1280x480 frame. + */ + +#include "catch_amalgamated.hpp" + +#include "util/u_stereo_camera.h" +#include "util/u_stereo_rectify.h" + +#include "sim_display_stereo_camera_pattern.h" +#include "stereo_rectify_fixtures.h" + +#include +#include +#include +#include +#include +#include + +namespace { + +const stereo_rectify_fixture *const k_fixtures[] = { + &k_fixture_sim_distorted, + &k_fixture_sr_like, + &k_fixture_radtan8_scaled, +}; + +u_stereo_rectify_input +input_from(const stereo_rectify_fixture &f) +{ + u_stereo_rectify_input in; + std::memset(&in, 0, sizeof(in)); + in.width = (uint32_t)f.width; + in.height = (uint32_t)f.height; + in.calib_width = (uint32_t)f.calib_width; + in.calib_height = (uint32_t)f.calib_height; + for (int e = 0; e < 2; e++) { + in.eye[e].fx = f.K[e][0]; + in.eye[e].fy = f.K[e][4]; + in.eye[e].cx = f.K[e][2]; + in.eye[e].cy = f.K[e][5]; + in.eye[e].model = (uint32_t)f.model; + for (int k = 0; k < 8; k++) { + in.eye[e].d[k] = f.D[e][k]; + } + } + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + in.R[i][j] = f.R[i * 3 + j]; + } + in.T[i] = f.T[i]; + } + return in; +} + +// Not M_PI: MSVC only defines it with _USE_MATH_DEFINES before the first , +// which Catch may already have included. +constexpr double k_pi = 3.14159265358979323846; +const double k_ideal_fx_640 = 320.0 / std::tan(34.0 * k_pi / 180.0); + +} // namespace + + +TEST_CASE("stereo rectify: Rodrigues and lens models invert", "[stereo_rectify]") +{ + const double rv[3] = {0.012, -0.031, 0.007}; + double m[3][3], back[3]; + u_stereo_rectify_rodrigues(rv, m); + u_stereo_rectify_rodrigues_inv(m, back); + for (int i = 0; i < 3; i++) { + CHECK(back[i] == Catch::Approx(rv[i]).margin(1e-14)); + } + const double big[3] = {1.1, -0.4, 2.2}; + u_stereo_rectify_rodrigues(big, m); + u_stereo_rectify_rodrigues_inv(m, back); + for (int i = 0; i < 3; i++) { + CHECK(back[i] == Catch::Approx(big[i]).margin(1e-12)); + } + + u_stereo_rectify_lens lenses[3]; + std::memset(lenses, 0, sizeof(lenses)); + const double r5[8] = {-0.21, 0.06, 0.0008, -0.0006, -0.005, 0, 0, 0}; + const double r8[8] = {0.31, -0.12, 0.0006, 0.0003, 0.02, 0.61, -0.05, 0.03}; + const double kb[8] = {0.021, -0.013, 0.004, -0.001, 0, 0, 0, 0}; + const uint32_t models[3] = {U_STEREO_RECTIFY_MODEL_RADTAN5, U_STEREO_RECTIFY_MODEL_RADTAN8, + U_STEREO_RECTIFY_MODEL_KB4}; + const double *coeffs[3] = {r5, r8, kb}; + for (int l = 0; l < 3; l++) { + lenses[l].fx = 480.0; + lenses[l].fy = 478.0; + lenses[l].cx = 325.0; + lenses[l].cy = 236.0; + lenses[l].model = models[l]; + std::memcpy(lenses[l].d, coeffs[l], sizeof(r5)); + for (double y = -0.45; y <= 0.45; y += 0.15) { + for (double x = -0.6; x <= 0.6; x += 0.15) { + double xd, yd, xu, yu; + u_stereo_rectify_distort(&lenses[l], x, y, &xd, &yd); + u_stereo_rectify_undistort_pixel(&lenses[l], lenses[l].fx * xd + lenses[l].cx, + lenses[l].fy * yd + lenses[l].cy, &xu, &yu); + CHECK(xu == Catch::Approx(x).margin(1e-9)); + CHECK(yu == Catch::Approx(y).margin(1e-9)); + } + } + } +} + +TEST_CASE("stereo rectify: GOLDEN geometry vs OpenCV stereoRectify", "[stereo_rectify]") +{ + for (const stereo_rectify_fixture *fx : k_fixtures) { + const stereo_rectify_fixture &f = *fx; + INFO("fixture " << f.name); + u_stereo_rectify_input in = input_from(f); + u_stereo_rectify_result r; + REQUIRE(u_stereo_rectify_compute(&in, &r)); + + // R1 / R2: identical algorithm, so identical to double precision. + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + CHECK(r.rot[0][i][j] == Catch::Approx(f.R1[i * 3 + j]).margin(1e-12)); + CHECK(r.rot[1][i][j] == Catch::Approx(f.R2[i * 3 + j]).margin(1e-12)); + } + } + // Principal point: OpenCV's corner-centroid rule, one cx for both eyes + // (zero disparity at infinity). cvStereoRectify runs undistortPoints for + // only 5 iterations; we converge, so we match the CONVERGED reference + // exactly and OpenCV's own P1 to a fraction of a pixel. + CHECK(r.cx == Catch::Approx(f.converged[1]).margin(1e-4)); + CHECK(r.cy == Catch::Approx(f.converged[2]).margin(1e-4)); + CHECK(r.cx == Catch::Approx(f.P1[2]).margin(0.75)); + CHECK(r.cy == Catch::Approx(f.P1[6]).margin(0.75)); + CHECK(f.P2[2] == Catch::Approx(f.P1[2]).margin(1e-9)); + // Focal before the crop zoom: OpenCV's min-fy rule, exactly. + CHECK(r.f / r.crop_scale == Catch::Approx(f.converged[0]).epsilon(1e-9)); + // After the alpha = 0 zoom: OpenCV's 9x9 inner rectangle, plus our + // border verification, which may only zoom further (OpenCV itself leaves + // a few black pixels on strong lenses). Within 1 % of OpenCV. + double f_cv = f.P1[0]; + CHECK(r.f == Catch::Approx(f_cv).epsilon(0.01)); + std::printf( + "[golden] %-15s f %.4f (OpenCV %.4f, %+.3f%%) cx %.4f (OpenCV %.4f) cy %.4f (OpenCV %.4f) crop " + "x%.4f baseline %.3f\n", + f.name, r.f, f_cv, 100.0 * (r.f / f_cv - 1.0), r.cx, f.P1[2], r.cy, f.P1[6], r.crop_scale, + r.baseline); + // Rectified translation: OpenCV P2[0][3] = f * Tx. + CHECK(r.P[1][0][3] / r.f == Catch::Approx(f.P2[3] / f.P2[0]).margin(1e-9)); + CHECK(std::fabs(r.t_rect[1]) < 1e-7 * r.baseline); // baseline on the x axis + CHECK(std::fabs(r.t_rect[2]) < 1e-7 * r.baseline); + CHECK(std::fabs(r.t_rect[0]) == Catch::Approx(r.baseline).epsilon(1e-12)); + } +} + +TEST_CASE("stereo rectify: GOLDEN maps vs OpenCV initUndistortRectifyMap", "[stereo_rectify]") +{ + for (const stereo_rectify_fixture *fx : k_fixtures) { + const stereo_rectify_fixture &f = *fx; + INFO("fixture " << f.name); + u_stereo_rectify_input in = input_from(f); + u_stereo_rectify_result r; + REQUIRE(u_stereo_rectify_compute(&in, &r)); + // Feed OpenCV's own R/P so the map math is compared in isolation. + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + r.rot[0][i][j] = f.R1[i * 3 + j]; + r.rot[1][i][j] = f.R2[i * 3 + j]; + } + } + r.f = f.P1[0]; + r.cx = f.P1[2]; + r.cy = f.P1[6]; + double worst = 0.0; + for (int s = 0; s < STEREO_RECTIFY_FIXTURE_SAMPLES; s++) { + const double *smp = &f.samples[s * 5]; + double x, y; + REQUIRE(u_stereo_rectify_map_point(&r, (uint32_t)smp[0], smp[1], smp[2], &x, &y)); + worst = std::max(worst, std::max(std::fabs(x - smp[3]), std::fabs(y - smp[4]))); + } + // OpenCV stores float32 maps: agreement to its precision. + CHECK(worst < 2e-3); + std::printf("[golden] %-15s map max |ours - OpenCV| = %.2e px over %d samples\n", f.name, worst, + STEREO_RECTIFY_FIXTURE_SAMPLES); + } +} + +TEST_CASE("stereo rectify: alpha = 0 leaves no black pixel", "[stereo_rectify]") +{ + for (const stereo_rectify_fixture *fx : k_fixtures) { + INFO("fixture " << fx->name); + u_stereo_rectify_input in = input_from(*fx); + u_stereo_rectify_result r; + REQUIRE(u_stereo_rectify_compute(&in, &r)); + for (uint32_t sub = 1; sub <= 2; sub++) { + u_stereo_rectify_lut lut; + REQUIRE(u_stereo_rectify_lut_init(&lut, &r, sub)); + CHECK(lut.width == 2 * r.width / sub); + CHECK(lut.valid == lut.width * lut.height); + u_stereo_rectify_lut_fini(&lut); + } + } +} + +TEST_CASE("stereo rectify: the C fake's truth equals the fixture's inputs", "[stereo_rectify]") +{ + sim_stereo_camera_truth t; + sim_stereo_camera_truth_init(&t, 640, 480, k_ideal_fx_640, 50.0, 2.0, 0.6); + const stereo_rectify_fixture &f = k_fixture_sim_distorted; + for (int e = 0; e < 2; e++) { + CHECK(t.fx[e] == Catch::Approx(f.K[e][0]).margin(1e-9)); + CHECK(t.fy[e] == Catch::Approx(f.K[e][4]).margin(1e-9)); + CHECK(t.cx[e] == Catch::Approx(f.K[e][2]).margin(1e-9)); + CHECK(t.cy[e] == Catch::Approx(f.K[e][5]).margin(1e-9)); + for (int k = 0; k < 5; k++) { + CHECK(t.dist[e][k] == Catch::Approx(f.D[e][k]).margin(1e-15)); + } + } + for (int i = 0; i < 9; i++) { + CHECK(t.R[i / 3][i % 3] == Catch::Approx(f.R[i]).margin(1e-12)); + } + for (int i = 0; i < 3; i++) { + CHECK(t.T_mm[i] == Catch::Approx(f.T[i]).margin(1e-9)); + } +} + + +/* + * + * End to end on the distorted fake. + * + */ + +namespace { + +struct block_stats +{ + std::vector dy_bg, dy_bar, dx_bg, dx_bar; +}; + +float +median(std::vector v) +{ + if (v.empty()) { + return NAN; + } + std::sort(v.begin(), v.end()); + return v[v.size() / 2]; +} + +float +max_abs_dev(const std::vector &v, float ref) +{ + float m = 0.0f; + for (float x : v) { + m = std::max(m, std::fabs(x - ref)); + } + return m; +} + +/*! + * Block-match a GRAY8 SBS pair on a grid. @p to_ideal maps a LEFT-eye pixel of + * @p img to the virtual parallel frame the scene is defined in, so each block + * is classified as background or bar (and skipped when it straddles an edge, + * the occlusion band next to the bar, or the frame counter). + */ +template +block_stats +match_blocks(const uint8_t *img, + uint32_t pitch, + uint32_t eye_w, + uint32_t h, + const sim_stereo_camera_scene &sc, + ToIdeal to_ideal, + uint32_t max_d = 64, + bool ncc = false) +{ + block_stats st; + const uint32_t B = 32, max_dy = 10; + for (uint32_t y = 48; y + B + max_dy <= h; y += B / 2) { + for (uint32_t x = 72; x + B <= eye_w; x += B / 2) { + double u0, v0, u1, v1; + to_ideal(x, y, &u0, &v0); + to_ideal(x + B, y + B, &u1, &v1); + const double m = 4.0; + bool rows_in = v0 >= sc.bar_y0 + m && v1 <= sc.bar_y1 - m; + bool rows_out = v1 < sc.bar_y0 - m || v0 > sc.bar_y1 + m; + bool cols_in = u0 >= sc.bar_x0 + m && u1 <= sc.bar_x1 - m; + // Background blocks must also keep clear of where the bar occludes the + // right eye's view of them (left x in [x0 - d_bar, x1)). + bool cols_out = u1 < sc.bar_x0 - sc.bar_disparity_f - m || u0 > sc.bar_x1 + m; + bool is_bar = rows_in && cols_in; + bool is_bg = rows_out || cols_out; + if (!is_bar && !is_bg) { + continue; + } + float dx, dy; + if (ncc) { + // The probe's matcher: flagged / weak blocks are excluded, never clamped. + u_stereo_camera_block_match bm; + if (!u_stereo_camera_match_block(img, pitch, eye_w, h, x, y, B, B, max_d, max_dy, + &bm) || + bm.dx_at_edge || bm.dy_at_edge || bm.ncc < 0.9f) { + continue; + } + dx = bm.dx; + dy = bm.dy; + } else if (!u_stereo_camera_estimate_offset(img, pitch, eye_w, h, x, y, B, B, max_d, max_dy, + &dx, &dy)) { + continue; + } + (is_bar ? st.dx_bar : st.dx_bg).push_back(dx); + (is_bar ? st.dy_bar : st.dy_bg).push_back(dy); + } + } + return st; +} + +} // namespace + +TEST_CASE("stereo rectify: END TO END on the distorted sim fake", "[stereo_rectify]") +{ + const uint32_t W = 640, H = 480; + sim_stereo_camera_distorted d; + REQUIRE(sim_stereo_camera_distorted_init(&d, W, H, k_ideal_fx_640, 50.0, 2.0, 0.6)); + std::vector raw(2 * W * H), rect(2 * W * H); + sim_stereo_camera_render_gray_distorted(&d, 1234, raw.data(), 2 * W); + + // The RAW pair really is misaligned (the thing the rectifier must fix): + // measured in raw pixels, the rows of the two eyes differ by several px. + { + block_stats st = + match_blocks(raw.data(), 2 * W, W, H, d.scene, [](uint32_t x, uint32_t y, double *u, double *v) { + *u = x; // roughly: classification only needs to be conservative + *v = y; + }); + std::vector all = st.dy_bg; + all.insert(all.end(), st.dy_bar.begin(), st.dy_bar.end()); + REQUIRE(all.size() > 20); + std::vector absdy; + for (float v : all) { + absdy.push_back(std::fabs(v)); + } + float med = median(absdy); + float mx = *std::max_element(absdy.begin(), absdy.end()); + std::printf("[e2e] RAW pair: |dy| median %.2f px, max %.2f px over %zu blocks\n", med, mx, all.size()); + CHECK(med > 2.0f); + } + + // Rectify with the calibration the fake's plug-in slot reports. + u_stereo_rectify_input in; + std::memset(&in, 0, sizeof(in)); + in.width = W; + in.height = H; + for (int e = 0; e < 2; e++) { + in.eye[e].fx = d.truth.fx[e]; + in.eye[e].fy = d.truth.fy[e]; + in.eye[e].cx = d.truth.cx[e]; + in.eye[e].cy = d.truth.cy[e]; + in.eye[e].model = U_STEREO_RECTIFY_MODEL_RADTAN5; + std::memcpy(in.eye[e].d, d.truth.dist[e], sizeof(d.truth.dist[e])); + } + std::memcpy(in.R, d.truth.R, sizeof(in.R)); + std::memcpy(in.T, d.truth.T_mm, sizeof(in.T)); + u_stereo_rectify_result r; + REQUIRE(u_stereo_rectify_compute(&in, &r)); + u_stereo_rectify_lut lut; + REQUIRE(u_stereo_rectify_lut_init(&lut, &r, 1)); + u_stereo_rectify_lut_apply(&lut, 1, raw.data(), 2 * W, rect.data(), 2 * W); + + // The symmetric construction makes the rectified frame the virtual one: + // rectified (u, v) -> virtual pixel by the focal / principal point only. + const double ifx = d.truth.ideal_fx, icx = d.truth.ideal_cx, icy = d.truth.ideal_cy; + auto to_ideal = [&](uint32_t x, uint32_t y, double *u, double *v) { + *u = ifx * (x - r.cx) / r.f + icx; + *v = ifx * (y - r.cy) / r.f + icy; + }; + block_stats st = match_blocks(rect.data(), 2 * W, W, H, d.scene, to_ideal); + REQUIRE(st.dx_bg.size() > 20); + REQUIRE(st.dx_bar.size() > 5); + + const double baseline_m = 0.05; + const float gt_bg = (float)(r.f * baseline_m / 2.0); + const float gt_bar = (float)(r.f * baseline_m / 0.6); + std::vector dy_all = st.dy_bg; + dy_all.insert(dy_all.end(), st.dy_bar.begin(), st.dy_bar.end()); + float dy_max = max_abs_dev(dy_all, 0.0f); + float bg_med = median(st.dx_bg), bar_med = median(st.dx_bar); + float bg_dev = max_abs_dev(st.dx_bg, gt_bg), bar_dev = max_abs_dev(st.dx_bar, gt_bar); + std::printf( + "[e2e] RECTIFIED (f %.3f px, crop x%.4f): |dy| max %.3f px over %zu blocks (median %.3f)\n" + "[e2e] background: GT f*B/Z = %.3f px, measured median %.3f, max |err| %.3f px (%zu blocks)\n" + "[e2e] bar: GT f*B/Z = %.3f px, measured median %.3f, max |err| %.3f px (%zu blocks)\n", + r.f, r.crop_scale, dy_max, dy_all.size(), median(dy_all), gt_bg, bg_med, bg_dev, st.dx_bg.size(), gt_bar, + bar_med, bar_dev, st.dx_bar.size()); + CHECK(dy_max <= 0.5f); + CHECK(bg_dev <= 0.5f); + CHECK(bar_dev <= 0.5f); + + u_stereo_rectify_lut_fini(&lut); + sim_stereo_camera_distorted_fini(&d); +} + +namespace { + +//! A parallel pair of the scene, the right eye shifted DOWN by @p dy_px rows +//! (a feature at left row v is at right row v + dy) and given a gain/offset. +std::vector +render_parallel(const sim_stereo_camera_scene &sc, double dy_px, double gain = 1.0, double offset = 0.0) +{ + const uint32_t W = sc.eye_width, H = sc.eye_height; + std::vector img(2 * W * H); + for (uint32_t y = 0; y < H; y++) { + for (uint32_t x = 0; x < W; x++) { + img[y * 2 * W + x] = (uint8_t)std::lround(sim_stereo_camera_scene_sample(&sc, 0, x, y)); + double v = gain * sim_stereo_camera_scene_sample(&sc, 1, x, y - dy_px) + offset; + img[y * 2 * W + W + x] = (uint8_t)std::lround(std::min(255.0, std::max(0.0, v))); + } + } + return img; +} + +} // namespace + +TEST_CASE("probe matcher: Leia-SR-sized disparities, SIGNED dy, bound hits flagged", "[stereo_rectify][probe]") +{ + // 120 mm at f 474 px: the bar at 0.6 m is ~95 px (the field case: a face at + // 0.6 m on the SR tracking camera, which the old fixed 0..64 search clamped). + const uint32_t W = 640, H = 480; + sim_stereo_camera_scene sc; + sim_stereo_camera_scene_init(&sc, W, H, k_ideal_fx_640, 120.0, 2.0, 0.6); + REQUIRE(sc.bar_disparity_f > 90.0); + const double dy0 = 1.68; // the field's signed residual + std::vector img = render_parallel(sc, dy0); + const uint32_t max_d = (uint32_t)std::ceil(k_ideal_fx_640 * 0.120 / 0.4); // what the probe computes + const uint32_t bx = 280, by = 200; // inside the bar + + u_stereo_camera_block_match m; + REQUIRE(u_stereo_camera_match_block(img.data(), 2 * W, W, H, bx, by, 32, 32, max_d, 24, &m)); + std::printf("[probe] bar block: dx %.3f (GT %.3f), dy %+.3f (GT %+.3f), ncc %.4f, window d %d..%d dy %d..%d\n", + m.dx, sc.bar_disparity_f, m.dy, dy0, m.ncc, m.d_lo, m.d_hi, m.dy_lo, m.dy_hi); + CHECK_FALSE(m.dx_at_edge); + CHECK_FALSE(m.dy_at_edge); + CHECK(m.ncc > 0.93f); + CHECK(m.dx == Catch::Approx(sc.bar_disparity_f).margin(0.25)); + CHECK(m.dy == Catch::Approx(dy0).margin(0.15)); // SIGNED, sub-pixel + + // Background block (28.5 px), same signed dy. + REQUIRE(u_stereo_camera_match_block(img.data(), 2 * W, W, H, 520, 40, 32, 32, max_d, 24, &m)); + CHECK(m.dx == Catch::Approx(sc.bg_disparity_f).margin(0.25)); + CHECK(m.dy == Catch::Approx(dy0).margin(0.15)); + + // The field failure, reproduced: with the old 64 px window the true match is + // out of reach. The block must then be REJECTED — a peak on the bound, or a + // correlation under the probe's 0.90 — never reported as a disparity. + REQUIRE(u_stereo_camera_match_block(img.data(), 2 * W, W, H, bx, by, 32, 32, 64, 24, &m)); + std::printf("[probe] same block, 0..64 window: dx %.2f, ncc %.3f, at_edge %d\n", m.dx, m.ncc, + (int)m.dx_at_edge); + CHECK((m.dx_at_edge || m.ncc < 0.90f)); + // Likewise a dy window narrower than the true offset. + std::vector img3 = render_parallel(sc, 3.0); + REQUIRE(u_stereo_camera_match_block(img3.data(), 2 * W, W, H, bx, by, 32, 32, max_d, 1, &m)); + CHECK(m.dy_at_edge); + REQUIRE(u_stereo_camera_match_block(img3.data(), 2 * W, W, H, bx, by, 32, 32, max_d, 24, &m)); + CHECK_FALSE(m.dy_at_edge); + CHECK(m.dy == Catch::Approx(3.0).margin(0.15)); + + // NCC is blind to a gain/offset mismatch between the two sensors. + std::vector img_g = render_parallel(sc, dy0, 0.75, 30.0); + REQUIRE(u_stereo_camera_match_block(img_g.data(), 2 * W, W, H, bx, by, 32, 32, max_d, 24, &m)); + CHECK(m.ncc > 0.93f); + CHECK(m.dx == Catch::Approx(sc.bar_disparity_f).margin(0.25)); + CHECK(m.dy == Catch::Approx(dy0).margin(0.15)); + + // A flat block is refused, not matched anywhere. + std::vector flat(2 * W * H, 128); + CHECK_FALSE(u_stereo_camera_match_block(flat.data(), 2 * W, W, H, bx, by, 32, 32, max_d, 24, &m)); +} + +TEST_CASE("stereo rectify: END TO END at the Leia SR baseline (120 mm)", "[stereo_rectify][probe]") +{ + const uint32_t W = 640, H = 480; + const double B_mm = 120.0; + sim_stereo_camera_distorted d; + REQUIRE(sim_stereo_camera_distorted_init(&d, W, H, k_ideal_fx_640, B_mm, 2.0, 0.6)); + std::vector raw(2 * W * H), rect(2 * W * H); + sim_stereo_camera_render_gray_distorted(&d, 77, raw.data(), 2 * W); + + u_stereo_rectify_input in; + std::memset(&in, 0, sizeof(in)); + in.width = W; + in.height = H; + for (int e = 0; e < 2; e++) { + in.eye[e].fx = d.truth.fx[e]; + in.eye[e].fy = d.truth.fy[e]; + in.eye[e].cx = d.truth.cx[e]; + in.eye[e].cy = d.truth.cy[e]; + in.eye[e].model = U_STEREO_RECTIFY_MODEL_RADTAN5; + std::memcpy(in.eye[e].d, d.truth.dist[e], sizeof(d.truth.dist[e])); + } + std::memcpy(in.R, d.truth.R, sizeof(in.R)); + std::memcpy(in.T, d.truth.T_mm, sizeof(in.T)); + u_stereo_rectify_result r; + REQUIRE(u_stereo_rectify_compute(&in, &r)); + u_stereo_rectify_lut lut; + REQUIRE(u_stereo_rectify_lut_init(&lut, &r, 1)); + u_stereo_rectify_lut_apply(&lut, 1, raw.data(), 2 * W, rect.data(), 2 * W); + + const double ifx = d.truth.ideal_fx, icx = d.truth.ideal_cx, icy = d.truth.ideal_cy; + auto to_ideal = [&](uint32_t x, uint32_t y, double *u, double *v) { + *u = ifx * (x - r.cx) / r.f + icx; + *v = ifx * (y - r.cy) / r.f + icy; + }; + const uint32_t max_d = (uint32_t)std::ceil(r.f * (B_mm / 1000.0) / 0.4); + block_stats st = match_blocks(rect.data(), 2 * W, W, H, d.scene, to_ideal, max_d, true); + REQUIRE(st.dx_bg.size() > 20); + REQUIRE(st.dx_bar.size() > 5); + const float gt_bg = (float)(r.f * (B_mm / 1000.0) / 2.0); + const float gt_bar = (float)(r.f * (B_mm / 1000.0) / 0.6); + std::vector dy_all = st.dy_bg; + dy_all.insert(dy_all.end(), st.dy_bar.begin(), st.dy_bar.end()); + std::printf( + "[e2e 120mm] f %.3f: bar GT %.3f px measured median %.3f (%zu blocks); bg GT %.3f median %.3f; " + "signed dy median %+.4f px, |dy| max %.3f\n", + r.f, gt_bar, median(st.dx_bar), st.dx_bar.size(), gt_bg, median(st.dx_bg), median(dy_all), + max_abs_dev(dy_all, 0.0f)); + CHECK(gt_bar > 64.0f); // the case the old fixed window could not measure + CHECK(max_abs_dev(st.dx_bar, gt_bar) <= 0.5f); + CHECK(max_abs_dev(st.dx_bg, gt_bg) <= 0.5f); + CHECK(max_abs_dev(dy_all, 0.0f) <= 0.5f); + CHECK(std::fabs(median(dy_all)) <= 0.1f); // no constant vertical offset + u_stereo_rectify_lut_fini(&lut); + sim_stereo_camera_distorted_fini(&d); +} + +/* + * CONSTANT VERTICAL OFFSET hypotheses (field: +1.7 px signed dy after + * rectification on the Leia SR laptop). Each case feeds the rectifier a + * calibration that differs from the truth in ONE specific way and measures the + * signed dy it leaves, so the field residual can be matched to a cause once the + * raw frames + calibration dumps arrive. The matched pair is always the + * distorted fake at the SR-like 120 mm baseline. + */ +namespace { + +struct vshift_rig +{ + sim_stereo_camera_distorted d; + std::vector raw; + u_stereo_rectify_input truth_in; + static constexpr uint32_t W = 640, H = 480; + + vshift_rig() + { + REQUIRE(sim_stereo_camera_distorted_init(&d, W, H, k_ideal_fx_640, 120.0, 2.0, 0.6)); + raw.resize(2 * W * H); + sim_stereo_camera_render_gray_distorted(&d, 5, raw.data(), 2 * W); + std::memset(&truth_in, 0, sizeof(truth_in)); + truth_in.width = W; + truth_in.height = H; + for (int e = 0; e < 2; e++) { + truth_in.eye[e].fx = d.truth.fx[e]; + truth_in.eye[e].fy = d.truth.fy[e]; + truth_in.eye[e].cx = d.truth.cx[e]; + truth_in.eye[e].cy = d.truth.cy[e]; + truth_in.eye[e].model = U_STEREO_RECTIFY_MODEL_RADTAN5; + std::memcpy(truth_in.eye[e].d, d.truth.dist[e], sizeof(d.truth.dist[e])); + } + std::memcpy(truth_in.R, d.truth.R, sizeof(truth_in.R)); + std::memcpy(truth_in.T, d.truth.T_mm, sizeof(truth_in.T)); + } + ~vshift_rig() + { + sim_stereo_camera_distorted_fini(&d); + } + + //! Rectify @p img with @p in; signed dy median + spread over NCC-accepted blocks. + void + measure(const u_stereo_rectify_input &in, + const std::vector &img, + float *dy_median, + float *dy_spread, + float *dx_median, + double *f_rect = nullptr) + { + u_stereo_rectify_result r; + REQUIRE(u_stereo_rectify_compute(&in, &r)); + u_stereo_rectify_lut lut; + REQUIRE(u_stereo_rectify_lut_init(&lut, &r, 1)); + std::vector rect(2 * W * H); + u_stereo_rectify_lut_apply(&lut, 1, img.data(), 2 * W, rect.data(), 2 * W); + u_stereo_rectify_lut_fini(&lut); + std::vector dys, dxs; + const uint32_t max_d = (uint32_t)std::ceil(r.f * 0.120 / 0.4); + for (uint32_t y = 40; y + 72 <= H; y += 32) { + for (uint32_t x = 40; x + 40 <= W; x += 32) { + u_stereo_camera_block_match m; + if (u_stereo_camera_match_block(rect.data(), 2 * W, W, H, x, y, 32, 32, max_d, 24, + &m) && + !m.dx_at_edge && !m.dy_at_edge && m.ncc >= 0.9f) { + dys.push_back(m.dy); + dxs.push_back(m.dx); + } + } + } + REQUIRE(dys.size() > 30); + *dy_median = median(dys); + std::vector dev; + for (float v : dys) { + dev.push_back(std::fabs(v - *dy_median)); + } + std::sort(dev.begin(), dev.end()); + *dy_spread = dev[(dev.size() * 9) / 10]; // p90 |dy - median| + *dx_median = median(dxs); + if (f_rect != nullptr) { + *f_rect = r.f; + } + } +}; + +} // namespace + +TEST_CASE("vertical offset H0: the true calibration leaves no constant dy", "[stereo_rectify][vshift]") +{ + vshift_rig rig; + float med, spread, dx; + rig.measure(rig.truth_in, rig.raw, &med, &spread, &dx); + std::printf("[vshift] H0 truth: signed dy median %+.3f px, p90 spread %.3f\n", med, spread); + CHECK(std::fabs(med) <= 0.1f); +} + +TEST_CASE("vertical offset H1: a per-eye cy error is a CONSTANT dy of (f_rect/fy) * dcy", "[stereo_rectify][vshift]") +{ + // Candidate field causes that all reduce to "one eye's cy is off": a flipped + // (bottom-up) row convention (dcy = H-1-2cy per eye), a crop instead of a + // scale between calibration and frame size, a +-0.5 px centre convention + // applied to one eye only, an integer-rounded cy on one eye. + vshift_rig rig; + for (double dcy : {0.5, 1.0, -1.5}) { + u_stereo_rectify_input in = rig.truth_in; + in.eye[1].cy += dcy; // the calibration says the RIGHT principal point is dcy lower + float med, spread, dx; + double f; + rig.measure(in, rig.raw, &med, &spread, &dx, &f); + double pred = -f / rig.d.truth.fy[1] * dcy; // first order; barrel magnification adds a few % + std::printf( + "[vshift] H1 right cy %+.1f px: signed dy median %+.3f px (predicted %+.3f), p90 spread %.3f\n", + dcy, med, pred, spread); + CHECK(med == Catch::Approx(pred).epsilon(0.1).margin(0.05)); + CHECK(spread <= 0.35f); // constant across the frame: an offset, not a rotation + } +} + +TEST_CASE("vertical offset H2: R given in the wrong direction (R^T) is a large, roll-shaped dy", + "[stereo_rectify][vshift]") +{ + // If the vendor's R were x_left = R x_right (camera-to-camera the other way) + // the rectifier would double the relative rotation instead of cancelling it. + // Signature: a dy offset ~ f * 2 * relative pitch PLUS a roll gradient (large + // spread) — so a pure constant +1.7 px does NOT look like this. + vshift_rig rig; + u_stereo_rectify_input in = rig.truth_in; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + in.R[i][j] = rig.truth_in.R[j][i]; + } + } + float med, spread, dx; + rig.measure(in, rig.raw, &med, &spread, &dx); + std::printf("[vshift] H2 R^T: signed dy median %+.3f px, p90 spread %.3f\n", med, spread); + CHECK((std::fabs(med) > 2.0f || spread > 1.0f)); +} + +TEST_CASE("vertical offset H3: calibration size vs frame size (pixel-centre rescale)", "[stereo_rectify][vshift]") +{ + // Intrinsics expressed at 2x the frame (u2 = 2u + 0.5) must rectify + // identically; the WRONG convention (u2 = 2u) shifts both eyes' cy by 0.25 px + // at frame scale — equal in both eyes, so it must NOT create relative dy. + vshift_rig rig; + u_stereo_rectify_input in = rig.truth_in; + in.calib_width = 2 * rig.W; + in.calib_height = 2 * rig.H; + for (int e = 0; e < 2; e++) { + in.eye[e].fx *= 2.0; + in.eye[e].fy *= 2.0; + in.eye[e].cx = (in.eye[e].cx + 0.5) * 2.0 - 0.5; + in.eye[e].cy = (in.eye[e].cy + 0.5) * 2.0 - 0.5; + } + float med, spread, dx, med0, spread0, dx0; + rig.measure(rig.truth_in, rig.raw, &med0, &spread0, &dx0); + rig.measure(in, rig.raw, &med, &spread, &dx); + CHECK(med == Catch::Approx(med0).margin(0.02)); + CHECK(dx == Catch::Approx(dx0).margin(0.02)); + for (int e = 0; e < 2; e++) { + in.eye[e].cx = rig.truth_in.eye[e].cx * 2.0; // wrong (corner) convention + in.eye[e].cy = rig.truth_in.eye[e].cy * 2.0; + } + rig.measure(in, rig.raw, &med, &spread, &dx); + std::printf("[vshift] H3 corner-convention rescale: signed dy median %+.3f px (truth %+.3f)\n", med, med0); + CHECK(std::fabs(med - med0) <= 0.1f); // common-mode: no relative dy +} + +TEST_CASE("vertical offset H4: the plug-in's half swap + (R^T, -R^T T) is exact", "[stereo_rectify][vshift]") +{ + // L1 (Leia) swaps the SBS halves and re-expresses the calibration from camera + // 2's side: K/D swapped, R' = R^T, T' = -R^T T. Model exactly that here. + vshift_rig rig; + const uint32_t W = rig.W, H = rig.H; + std::vector swapped(2 * W * H); + for (uint32_t y = 0; y < H; y++) { + std::memcpy(&swapped[y * 2 * W], &rig.raw[y * 2 * W + W], W); + std::memcpy(&swapped[y * 2 * W + W], &rig.raw[y * 2 * W], W); + } + // The vendor's own calibration of the swapped pair (camera 1 = SBS RIGHT): + // x_2 = R_v x_1 + T_v with camera 1 the truth's RIGHT eye. + u_stereo_rectify_input v = rig.truth_in; + v.eye[0] = rig.truth_in.eye[1]; + v.eye[1] = rig.truth_in.eye[0]; + double Rv[3][3], Tv[3]; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + Rv[i][j] = rig.truth_in.R[j][i]; + } + } + for (int i = 0; i < 3; i++) { + Tv[i] = -(Rv[i][0] * rig.truth_in.T[0] + Rv[i][1] * rig.truth_in.T[1] + Rv[i][2] * rig.truth_in.T[2]); + } + REQUIRE(Tv[0] > 0.0); // what makes L1 decide to swap + // L1's leia_scam_calibration_for_runtime(swap = true): + u_stereo_rectify_input in = v; + in.eye[0] = v.eye[1]; + in.eye[1] = v.eye[0]; + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + in.R[i][j] = Rv[j][i]; + } + } + for (int i = 0; i < 3; i++) { + in.T[i] = -(in.R[i][0] * Tv[0] + in.R[i][1] * Tv[1] + in.R[i][2] * Tv[2]); + } + // ...applied to the halves swapped BACK, which is what L1 delivers. + std::vector back(2 * W * H); + for (uint32_t y = 0; y < H; y++) { + std::memcpy(&back[y * 2 * W], &swapped[y * 2 * W + W], W); + std::memcpy(&back[y * 2 * W + W], &swapped[y * 2 * W], W); + } + float med, spread, dx; + rig.measure(in, back, &med, &spread, &dx); + std::printf("[vshift] H4 swap round trip: signed dy median %+.3f px, dx median %.2f\n", med, dx); + CHECK(std::fabs(med) <= 0.1f); + CHECK(dx > 20.0f); // positive disparities + // Swapping the intrinsics but NOT the distortion (a plausible slip) is visible: + u_stereo_rectify_input bad = in; + std::memcpy(bad.eye[0].d, in.eye[1].d, sizeof(bad.eye[0].d)); + std::memcpy(bad.eye[1].d, in.eye[0].d, sizeof(bad.eye[1].d)); + rig.measure(bad, back, &med, &spread, &dx); + std::printf("[vshift] H4 distortion NOT swapped: signed dy median %+.3f px, p90 spread %.3f\n", med, spread); +} + +TEST_CASE("vertical offset H5: identity calibration is an identity LUT (no half-pixel shift)", + "[stereo_rectify][vshift]") +{ + u_stereo_rectify_input in; + std::memset(&in, 0, sizeof(in)); + in.width = 640; + in.height = 480; + for (int e = 0; e < 2; e++) { + in.eye[e].fx = in.eye[e].fy = 500.0; + in.eye[e].cx = 319.5; + in.eye[e].cy = 239.5; + in.eye[e].model = U_STEREO_RECTIFY_MODEL_RADTAN5; + } + in.R[0][0] = in.R[1][1] = in.R[2][2] = 1.0; + in.T[0] = -120.0; + u_stereo_rectify_result r; + REQUIRE(u_stereo_rectify_compute(&in, &r)); + CHECK(r.cy == Catch::Approx(239.5).margin(1e-6)); + // The border check may zoom a hair (x1.001 per step) even here, but the map + // must then be a PURE scale about (cx, cy) — identical in both eyes, with the + // principal row fixed: no half-pixel or rounding offset anywhere. + std::printf("[vshift] H5 identity: f %.4f (raw 500), crop x%.5f, cy %.4f\n", r.f, r.crop_scale, r.cy); + CHECK(r.crop_scale < 1.0025); + double worst = 0.0; + for (uint32_t e = 0; e < 2; e++) { + for (double v : {0.0, 100.0, 239.5, 479.0}) { + for (double u : {0.0, 320.0, 639.0}) { + double x, y; + REQUIRE(u_stereo_rectify_map_point(&r, e, u, v, &x, &y)); + worst = std::max(worst, std::fabs((y - 239.5) - (v - 239.5) * 500.0 / r.f)); + } + } + } + CHECK(worst < 1e-6); +} + +TEST_CASE("stereo rectify: NV12 and BGRA8 planes agree with GRAY8", "[stereo_rectify]") +{ + const uint32_t W = 640, H = 480, SW = 2 * W; + sim_stereo_camera_distorted d; + REQUIRE(sim_stereo_camera_distorted_init(&d, W, H, k_ideal_fx_640, 50.0, 2.0, 0.6)); + std::vector gray(SW * H), gray_r(SW * H); + sim_stereo_camera_render_gray_distorted(&d, 7, gray.data(), SW); + + u_stereo_rectify_input in = input_from(k_fixture_sim_distorted); + u_stereo_rectify_result r; + REQUIRE(u_stereo_rectify_compute(&in, &r)); + u_stereo_rectify_lut full, half; + REQUIRE(u_stereo_rectify_lut_init(&full, &r, 1)); + REQUIRE(u_stereo_rectify_lut_init(&half, &r, 2)); + u_stereo_rectify_lut_apply(&full, 1, gray.data(), SW, gray_r.data(), SW); + + // BGRA with B = G = R = gray: every colour channel equals the GRAY8 result. + std::vector bgra(SW * H * 4), bgra_r(SW * H * 4); + for (size_t i = 0; i < gray.size(); i++) { + bgra[4 * i] = bgra[4 * i + 1] = bgra[4 * i + 2] = gray[i]; + bgra[4 * i + 3] = 255; + } + u_stereo_rectify_lut_apply(&full, 4, bgra.data(), SW * 4, bgra_r.data(), SW * 4); + size_t mism = 0; + for (size_t i = 0; i < gray.size(); i++) { + mism += bgra_r[4 * i] != gray_r[i] || bgra_r[4 * i + 2] != gray_r[i] || bgra_r[4 * i + 3] != 255; + } + CHECK(mism == 0); + + // NV12 chroma: a constant UV plane stays constant (no black pull-in). + std::vector uv(SW * H / 2, 0), uv_r(SW * H / 2, 0); + for (size_t i = 0; i < uv.size(); i += 2) { + uv[i] = 90; + uv[i + 1] = 170; + } + REQUIRE(half.width == W); + u_stereo_rectify_lut_apply(&half, 2, uv.data(), SW, uv_r.data(), SW); + size_t off = 0; + for (size_t i = 0; i < uv_r.size(); i += 2) { + off += uv_r[i] != 90 || uv_r[i + 1] != 170; + } + CHECK(off == 0); + + u_stereo_rectify_lut_fini(&full); + u_stereo_rectify_lut_fini(&half); + sim_stereo_camera_distorted_fini(&d); +} + +TEST_CASE("stereo rectify: CPU cost per 1280x480 frame", "[.perf][stereo_rectify]") +{ + const uint32_t W = 640, H = 480, SW = 2 * W; + u_stereo_rectify_input in = input_from(k_fixture_sim_distorted); + u_stereo_rectify_result r; + auto t0 = std::chrono::steady_clock::now(); + REQUIRE(u_stereo_rectify_compute(&in, &r)); + u_stereo_rectify_lut full, half; + REQUIRE(u_stereo_rectify_lut_init(&full, &r, 1)); + REQUIRE(u_stereo_rectify_lut_init(&half, &r, 2)); + double setup_ms = std::chrono::duration(std::chrono::steady_clock::now() - t0).count(); + + std::vector src(SW * H * 4, 77), dst(SW * H * 4); + for (size_t i = 0; i < src.size(); i++) { + src[i] = (uint8_t)(i * 2654435761u >> 24); + } + const int N = 200; + auto run = [&](const u_stereo_rectify_lut &lut, uint32_t ch) { + auto a = std::chrono::steady_clock::now(); + for (int i = 0; i < N; i++) { + u_stereo_rectify_lut_apply(&lut, ch, src.data(), lut.width * ch, dst.data(), lut.width * ch); + } + return std::chrono::duration(std::chrono::steady_clock::now() - a).count() / N; + }; + double g = run(full, 1); + double y = g; + double uvp = run(half, 2); + double b = run(full, 4); + std::printf( + "[perf] setup (geometry + both LUTs) %.1f ms | per 1280x480 frame: GRAY8 %.3f ms, NV12 %.3f ms " + "(Y %.3f + UV %.3f), BGRA8 %.3f ms\n", + setup_ms, g, y + uvp, y, uvp, b); + u_stereo_rectify_lut_fini(&full); + u_stereo_rectify_lut_fini(&half); +} diff --git a/tests/tests_stereo_vrefine.cpp b/tests/tests_stereo_vrefine.cpp new file mode 100644 index 0000000000..5c96322688 --- /dev/null +++ b/tests/tests_stereo_vrefine.cpp @@ -0,0 +1,547 @@ +// Copyright 2026, The DisplayXR Project +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief XR_DXR_stereo_camera (ADR-043) R2: the online vertical-alignment + * refinement (u_stereo_vrefine + the correction folded into + * u_stereo_rectify). + * + * - the correction math: map <-> inverse round trip, and a pair displaced by + * a known (A, B) lands on the same row once the correction is folded in; + * - the measurement recovers a known UNCALIBRATED offset + slope injected into + * the sim_display distorted fake; + * - CLOSED LOOP on the fake (render -> rectify -> measure -> fit -> rebuild): + * converges to <= 0.2 px residual, judged by the probe's own NCC matcher, + * not by the refiner's; + * - degenerate input (flat, uncorrelated noise, vertical stripes) never + * applies anything, and the clamps hold for any input; + * - REAL SR hardware: one raw frame from a Leia SR laptop + its calibration + * (fixtures/stereo_vrefine_sr_*): the refinement, run on the runtime's own + * rectification of that frame, brings the signed dy median of OpenCV SIFT + * matches (an independent reference) from ~+1.7 px to <= 0.5 px; + * - [.perf] (hidden): cost of one measurement. + */ + +#include "catch_amalgamated.hpp" + +#include "util/u_stereo_camera.h" +#include "util/u_stereo_rectify.h" +#include "util/u_stereo_vrefine.h" + +#include "sim_display_stereo_camera_pattern.h" +#include "stereo_vrefine_sr_fixture.h" + +#if defined(__GNUC__) || defined(__clang__) +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-function" +#endif +#define STB_IMAGE_STATIC +#define STB_IMAGE_IMPLEMENTATION +#define STBI_ONLY_JPEG +#include "stb_image.h" +#if defined(__GNUC__) || defined(__clang__) +#pragma GCC diagnostic pop +#endif + +#include +#include +#include +#include +#include +#include + +namespace { + +constexpr double k_pi = 3.14159265358979323846; +const double k_ideal_fx_640 = 320.0 / std::tan(34.0 * k_pi / 180.0); +const uint32_t W = 640, H = 480; + +double +median(std::vector v) +{ + if (v.empty()) { + return 0.0; + } + std::sort(v.begin(), v.end()); + size_t n = v.size(); + return n % 2 ? v[n / 2] : 0.5 * (v[n / 2 - 1] + v[n / 2]); +} + +u_stereo_rectify_input +input_from_truth(const sim_stereo_camera_truth &t) +{ + u_stereo_rectify_input in; + std::memset(&in, 0, sizeof(in)); + in.width = t.eye_width; + in.height = t.eye_height; + for (int e = 0; e < 2; e++) { + in.eye[e].fx = t.fx[e]; + in.eye[e].fy = t.fy[e]; + in.eye[e].cx = t.cx[e]; + in.eye[e].cy = t.cy[e]; + in.eye[e].model = U_STEREO_RECTIFY_MODEL_RADTAN5; + std::memcpy(in.eye[e].d, t.dist[e], sizeof(t.dist[e])); + } + std::memcpy(in.R, t.R, sizeof(in.R)); + std::memcpy(in.T, t.T_mm, sizeof(in.T)); + return in; +} + +//! Rectify @p raw with the correction (a px, b) — the way the service rebuilds. +struct rectifier +{ + u_stereo_rectify_input in; + u_stereo_rectify_result geo; + u_stereo_rectify_lut lut; + bool have_lut = false; + + bool + build(double a_px, double b) + { + if (have_lut) { + u_stereo_rectify_lut_fini(&lut); + have_lut = false; + } + // a is in pixels of the current output; the normalized offset uses its f. + double f = geo.f > 0.0 ? geo.f : 1.0; + in.v_offset = geo.f > 0.0 ? a_px / f : 0.0; + in.v_slope = b; + if (!u_stereo_rectify_compute(&in, &geo)) { + return false; + } + have_lut = u_stereo_rectify_lut_init(&lut, &geo, 1); + return have_lut; + } + void + apply(const uint8_t *raw, uint8_t *out) const + { + u_stereo_rectify_lut_apply(&lut, 1, raw, 2 * geo.width, out, 2 * geo.width); + } + ~rectifier() + { + if (have_lut) { + u_stereo_rectify_lut_fini(&lut); + } + } +}; + +//! The probe's matcher (u_stereo_camera_match_block) on a grid: signed dy of +//! every confident, unclamped block — an independent judge of the rows. +std::vector +probe_dy(const uint8_t *img, uint32_t eye_w, uint32_t h, uint32_t max_d) +{ + std::vector out; + const uint32_t B = 32; + for (uint32_t y = 48; y + B + 16 <= h; y += 24) { + for (uint32_t x = 96; x + B <= eye_w; x += 24) { + u_stereo_camera_block_match m; + if (!u_stereo_camera_match_block(img, 2 * eye_w, eye_w, h, x, y, B, B, max_d, 12, &m) || + m.dx_at_edge || m.dy_at_edge || m.ncc < 0.93f || m.dx < 0.5f) { + continue; + } + out.push_back(m.dy); + } + } + return out; +} + +struct fake +{ + sim_stereo_camera_distorted d; + fake(double dy_px, double slope) + { + REQUIRE(sim_stereo_camera_distorted_init(&d, W, H, k_ideal_fx_640, 50.0, 2.0, 0.6)); + sim_stereo_camera_distorted_set_vmisalign(&d, dy_px, slope); + } + ~fake() + { + sim_stereo_camera_distorted_fini(&d); + } +}; + +} // namespace + + +TEST_CASE("stereo vrefine: the correction folded into the rectification", "[stereo_vrefine]") +{ + fake fk(0.0, 0.0); + u_stereo_rectify_input in = input_from_truth(fk.d.truth); + u_stereo_rectify_result r0, r1; + REQUIRE(u_stereo_rectify_compute(&in, &r0)); + const double A = 3.1 / r0.f, B = -0.006; + in.v_offset = A; + in.v_slope = B; + REQUIRE(u_stereo_rectify_compute(&in, &r1)); + + // Still the rectified-pair contract: one pinhole, a pure +x baseline. + CHECK(r1.P[0][1][1] == r1.f); + CHECK(r1.P[1][1][2] == r1.P[0][1][2]); + CHECK(r1.P[1][0][3] == Catch::Approx(r1.f * r1.t_rect[0])); + CHECK(r1.v_offset == A); + CHECK(r1.v_slope == B); + + // map_point and rect_from_raw are inverses, correction included. + for (uint32_t e = 0; e < 2; e++) { + for (double v = 20; v < H - 20; v += 55) { + for (double u = 20; u < W - 20; u += 70) { + double x, y, u2, v2; + REQUIRE(u_stereo_rectify_map_point(&r1, e, u, v, &x, &y)); + REQUIRE(u_stereo_rectify_rect_from_raw(&r1, e, x, y, &u2, &v2)); + CHECK(u2 == Catch::Approx(u).margin(1e-6)); + CHECK(v2 == Catch::Approx(v).margin(1e-6)); + } + } + } + + // A pair whose UNCORRECTED rows differ by D = f (A + B y_n) lands on one row. + double worst = 0.0; + for (double v = 40; v < H - 40; v += 40) { + for (double u = 60; u < W - 60; u += 80) { + double yn = (v - r0.cy) / r0.f; + double vr = v + r0.f * (A + B * yn); + double lx, ly, rx, ry, ul, vl, ur, vr1; + REQUIRE(u_stereo_rectify_map_point(&r0, 0, u, v, &lx, &ly)); + REQUIRE(u_stereo_rectify_map_point(&r0, 1, u - 20.0, vr, &rx, &ry)); + REQUIRE(u_stereo_rectify_rect_from_raw(&r1, 0, lx, ly, &ul, &vl)); + REQUIRE(u_stereo_rectify_rect_from_raw(&r1, 1, rx, ry, &ur, &vr1)); + worst = std::max(worst, std::fabs(vr1 - vl)); + } + } + std::printf("[math] D = %.2f px + %.4f/px: corrected rows differ by at most %.4f px\n", A * r0.f, B, worst); + CHECK(worst < 0.02); +} + +TEST_CASE("stereo vrefine: measurement recovers an injected offset + slope", "[stereo_vrefine]") +{ + // The SR laptop's measured model, in the fake's virtual frame. + const double a_true = 2.6, b_true = -0.0047; + fake fk(a_true, b_true); + rectifier rc; + rc.in = input_from_truth(fk.d.truth); + std::memset(&rc.geo, 0, sizeof(rc.geo)); + REQUIRE(rc.build(0.0, 0.0)); + std::vector raw(2 * W * H), rect(2 * W * H); + sim_stereo_camera_render_gray_distorted(&fk.d, 7, raw.data(), 2 * W); + rc.apply(raw.data(), rect.data()); + + u_stereo_vrefine_measure_params mp; + u_stereo_vrefine_measure_defaults(&mp, 96); + std::vector s(512); + uint32_t n = u_stereo_vrefine_measure(rect.data(), 2 * W, W, H, &mp, s.data(), (uint32_t)s.size()); + u_stereo_vrefine_fit fit; + REQUIRE(u_stereo_vrefine_fit_samples(s.data(), n, rc.geo.cy, &fit)); + // The misalignment is defined in the virtual frame (ideal_fx); the + // rectified frame is that frame zoomed by f / ideal_fx about its centre. + const double k = rc.geo.f / fk.d.truth.ideal_fx; + const double a_exp = k * a_true, b_exp = b_true; + std::printf( + "[measure] %u matches (%u inliers): a %+.3f px (expect %+.3f), b %+.5f (expect %+.5f), sigma %.3f\n", n, + fit.inliers, fit.a, a_exp, fit.b, b_exp, fit.sigma); + CHECK(n >= 50); + CHECK(fit.slope_determined); + CHECK(fit.a == Catch::Approx(a_exp).margin(0.15)); + CHECK(fit.b == Catch::Approx(b_exp).margin(0.0008)); + for (uint32_t i = 0; i < n; i++) { + CHECK(s[i].d > 0.5f); + CHECK(s[i].ncc >= 0.9f); + } +} + +TEST_CASE("stereo vrefine: CLOSED LOOP on the fake converges to <= 0.2 px", "[stereo_vrefine]") +{ + const double a_true = 2.6, b_true = -0.0047; + fake fk(a_true, b_true); + rectifier rc; + rc.in = input_from_truth(fk.d.truth); + std::memset(&rc.geo, 0, sizeof(rc.geo)); + REQUIRE(rc.build(0.0, 0.0)); + std::vector raw(2 * W * H), rect(2 * W * H); + + u_stereo_vrefine_config cfg; + u_stereo_vrefine_config_defaults(&cfg, H, rc.geo.cy); + u_stereo_vrefine ref; + u_stereo_vrefine_init(&ref, &cfg); + u_stereo_vrefine_measure_params mp; + u_stereo_vrefine_measure_defaults(&mp, 96); + std::vector s(512); + + // Before: the probe sees the injected offset. + sim_stereo_camera_render_gray_distorted(&fk.d, 1, raw.data(), 2 * W); + rc.apply(raw.data(), rect.data()); + double before = median(probe_dy(rect.data(), W, H, 96)); + + // 60 frames at 30 Hz = 2 s of the settling phase. + int64_t now = 1000000000; + uint32_t updates = 0, measured = 0; + for (uint64_t frame = 0; frame < 60; frame++, now += 33333333) { + if (!u_stereo_vrefine_due(&ref, now)) { + continue; + } + sim_stereo_camera_render_gray_distorted(&fk.d, frame, raw.data(), 2 * W); + rc.apply(raw.data(), rect.data()); + uint32_t n = u_stereo_vrefine_measure(rect.data(), 2 * W, W, H, &mp, s.data(), (uint32_t)s.size()); + measured++; + if (u_stereo_vrefine_push(&ref, now, s.data(), n) == U_STEREO_VREFINE_UPDATED) { + updates++; + REQUIRE(rc.build(ref.a, ref.b)); + CHECK(std::fabs(ref.a) <= cfg.max_offset_px); + CHECK(std::fabs(ref.b) <= cfg.max_slope); + } + } + sim_stereo_camera_render_gray_distorted(&fk.d, 999, raw.data(), 2 * W); + rc.apply(raw.data(), rect.data()); + std::vector after = probe_dy(rect.data(), W, H, 96); + std::vector absd; + for (double v : after) { + absd.push_back(std::fabs(v)); + } + double med_after = median(after), abs_after = median(absd); + std::printf( + "[closed loop] %u frames measured, %u map rebuilds: applied a %+.3f px, b %+.5f; probe signed dy median " + "%+.3f -> %+.3f px (|dy| median %.3f, %zu blocks); initial window %+.3f px\n", + measured, updates, ref.a, ref.b, before, med_after, abs_after, after.size(), ref.initial_dy); + CHECK(before > 2.0); + CHECK(updates >= 1); + CHECK(after.size() > 40); + CHECK(std::fabs(med_after) <= 0.2); + CHECK(abs_after <= 0.25); + // Rows everywhere, not just at the centre: the slope was fitted. + CHECK(ref.b == Catch::Approx(b_true).margin(0.001)); + // Converged: further windows are STABLE (the deadband), no more rebuilds. + uint32_t late = 0; + for (uint64_t frame = 60; frame < 120; frame++, now += 33333333) { + if (!u_stereo_vrefine_due(&ref, now)) { + continue; + } + sim_stereo_camera_render_gray_distorted(&fk.d, frame, raw.data(), 2 * W); + rc.apply(raw.data(), rect.data()); + uint32_t n = u_stereo_vrefine_measure(rect.data(), 2 * W, W, H, &mp, s.data(), (uint32_t)s.size()); + if (u_stereo_vrefine_push(&ref, now, s.data(), n) == U_STEREO_VREFINE_UPDATED) { + late++; + REQUIRE(rc.build(ref.a, ref.b)); + } + } + CHECK(late == 0); + CHECK(std::fabs(ref.last_residual_dy) <= 0.2); +} + +TEST_CASE("stereo vrefine: degenerate frames never apply; clamps always hold", "[stereo_vrefine]") +{ + u_stereo_vrefine_config cfg; + u_stereo_vrefine_config_defaults(&cfg, H, 239.5); + u_stereo_vrefine_measure_params mp; + u_stereo_vrefine_measure_defaults(&mp, 96); + std::vector s(512); + std::vector img(2 * W * H); + + uint32_t lcg = 12345; + auto rnd = [&]() { + lcg = lcg * 1664525u + 1013904223u; + return (uint8_t)(lcg >> 24); + }; + for (int kind = 0; kind < 3; kind++) { + for (uint32_t y = 0; y < H; y++) { + for (uint32_t x = 0; x < 2 * W; x++) { + uint8_t v = 128; + if (kind == 1) { + v = rnd(); // uncorrelated noise between the eyes + } else if (kind == 2) { + v = ((x % W) / 7) % 2 ? 200 : 40; // vertical stripes: dy unobservable + } + img[y * 2 * W + x] = v; + } + } + u_stereo_vrefine ref; + u_stereo_vrefine_init(&ref, &cfg); + uint32_t total = 0; + int64_t now = 0; + for (int f = 0; f < 60; f++, now += 250000000) { + uint32_t n = + u_stereo_vrefine_measure(img.data(), 2 * W, W, H, &mp, s.data(), (uint32_t)s.size()); + total += n; + CHECK(u_stereo_vrefine_push(&ref, now, s.data(), n) != U_STEREO_VREFINE_UPDATED); + } + std::printf("[degenerate] kind %d: %u matches over 60 frames, applied %d\n", kind, total, ref.applied); + CHECK_FALSE(ref.applied); + CHECK(ref.a == 0.0); + CHECK(ref.b == 0.0); + } + + // Clamps: an absurd consistent residual saturates at the limits, never beyond. + u_stereo_vrefine ref; + u_stereo_vrefine_init(&ref, &cfg); + std::vector big(60); + for (size_t i = 0; i < big.size(); i++) { + big[i].x = 320; + big[i].y = (float)(20 + 7 * i); + big[i].d = 30; + big[i].ncc = 0.99f; + big[i].dy = (float)(15.0 + 0.05 * (big[i].y - 239.5)); + } + int64_t now = 0; + for (int w = 0; w < 10; w++) { + for (uint32_t f = 0; f < cfg.min_frames; f++, now += 250000000) { + u_stereo_vrefine_push(&ref, now, big.data(), 20); + } + CHECK(std::fabs(ref.a) <= cfg.max_offset_px); + CHECK(std::fabs(ref.b) <= cfg.max_slope); + } + CHECK(ref.a == cfg.max_offset_px); + CHECK(ref.b == cfg.max_slope); + + // Too few frames or matches: nothing applies. + u_stereo_vrefine_init(&ref, &cfg); + CHECK(u_stereo_vrefine_push(&ref, 0, big.data(), 60) == U_STEREO_VREFINE_ACCUMULATING); // 1 frame + CHECK(u_stereo_vrefine_push(&ref, 1, big.data(), 0) == U_STEREO_VREFINE_ACCUMULATING); // 2 frames + CHECK(u_stereo_vrefine_push(&ref, 2, big.data(), 0) == U_STEREO_VREFINE_UPDATED); // 3 frames, 60 matches + u_stereo_vrefine_init(&ref, &cfg); + for (int f = 0; f < 11; f++) { + CHECK(u_stereo_vrefine_push(&ref, f, big.data(), 4) == U_STEREO_VREFINE_ACCUMULATING); // 44 < 50 + } + // 12th frame: 48 < 50 matches when the window expires -> dropped, not applied. + CHECK(u_stereo_vrefine_push(&ref, 12, big.data(), 4) == U_STEREO_VREFINE_DROPPED); + CHECK_FALSE(ref.applied); +} + +TEST_CASE("stereo vrefine: duty cycle — settle fast, then every 30 s", "[stereo_vrefine]") +{ + u_stereo_vrefine_config cfg; + u_stereo_vrefine_config_defaults(&cfg, H, 239.5); + u_stereo_vrefine ref; + u_stereo_vrefine_init(&ref, &cfg); + std::vector ok(30); + for (size_t i = 0; i < ok.size(); i++) { + ok[i].x = 320; + ok[i].y = (float)(20 + 15 * i); + ok[i].d = 30; + ok[i].ncc = 0.99f; + ok[i].dy = (float)(0.01 * ((int)(i % 3) - 1)); // aligned rows: STABLE windows + } + const int64_t step = 16666667; // 60 Hz + uint32_t measured_fast = 0, measured_slow = 0; + for (int64_t t = 0; t < 65ll * 1000000000; t += step) { + if (u_stereo_vrefine_due(&ref, t)) { + u_stereo_vrefine_push(&ref, t, ok.data(), (uint32_t)ok.size()); + (t < cfg.fast_phase_ns ? measured_fast : measured_slow)++; + } + } + std::printf("[duty] measured %u frames in the first 5 s, %u in the next 60 s\n", measured_fast, measured_slow); + CHECK(measured_fast >= 15); + CHECK(measured_fast <= 21); + CHECK(measured_slow <= 3 * 3); // two or three windows of min_frames + CHECK_FALSE(ref.applied); +} + +TEST_CASE("stereo vrefine: REAL Leia SR frame — residual +1.7 px -> <= 0.5 px", "[stereo_vrefine]") +{ + int w = 0, h = 0, ch = 0; + uint8_t *raw = stbi_load(STEREO_FIXTURE_DIR "/stereo_vrefine_sr_raw.jpg", &w, &h, &ch, 1); + REQUIRE(raw != nullptr); + REQUIRE(w == 1280); + REQUIRE(h == 480); + + rectifier rc; + std::memset(&rc.in, 0, sizeof(rc.in)); + std::memset(&rc.geo, 0, sizeof(rc.geo)); + rc.in.width = 640; + rc.in.height = 480; + for (int e = 0; e < 2; e++) { + rc.in.eye[e].fx = k_sr_K[e][0]; + rc.in.eye[e].fy = k_sr_K[e][1]; + rc.in.eye[e].cx = k_sr_K[e][2]; + rc.in.eye[e].cy = k_sr_K[e][3]; + rc.in.eye[e].model = U_STEREO_RECTIFY_MODEL_RADTAN5; + for (int k = 0; k < 5; k++) { + rc.in.eye[e].d[k] = k_sr_D[e][k]; + } + } + std::memcpy(rc.in.R, k_sr_R, sizeof(rc.in.R)); + std::memcpy(rc.in.T, k_sr_T, sizeof(rc.in.T)); + REQUIRE(rc.build(0.0, 0.0)); + + auto sift_dy = [&](std::vector *ys) { + std::vector dys; + for (const stereo_vrefine_sr_match &m : k_sr_matches) { + double ul, vl, ur, vr; + if (u_stereo_rectify_rect_from_raw(&rc.geo, 0, m.lx, m.ly, &ul, &vl) && + u_stereo_rectify_rect_from_raw(&rc.geo, 1, m.rx, m.ry, &ur, &vr)) { + dys.push_back(vr - vl); + if (ys != nullptr) { + ys->push_back(vl); + } + } + } + return dys; + }; + std::vector dy0 = sift_dy(nullptr); + double before = median(dy0); + + std::vector rect(1280 * 480); + u_stereo_vrefine_config cfg; + u_stereo_vrefine_config_defaults(&cfg, 480, rc.geo.cy); + u_stereo_vrefine ref; + u_stereo_vrefine_init(&ref, &cfg); + u_stereo_vrefine_measure_params mp; + // A subject at 0.4 m on this 120 mm, f ~ 513 px pair: ~154 px. + u_stereo_vrefine_measure_defaults(&mp, (uint32_t)(rc.geo.f * rc.geo.baseline / 400.0)); + std::vector s(512); + uint32_t first_n = 0, updates = 0; + int64_t now = 0; + // One recorded frame stands in for the live stream (the service measures + // consecutive frames; the matcher is deterministic, so a repeated frame + // only repeats its matches). 30 frames = 7.5 s at the settling cadence. + for (int f = 0; f < 30; f++, now += cfg.fast_period_ns) { + rc.apply(raw, rect.data()); + uint32_t n = u_stereo_vrefine_measure(rect.data(), 1280, 640, 480, &mp, s.data(), (uint32_t)s.size()); + if (f == 0) { + first_n = n; + } + if (u_stereo_vrefine_push(&ref, now, s.data(), n) == U_STEREO_VREFINE_UPDATED) { + updates++; + REQUIRE(rc.build(ref.a, ref.b)); + } + } + std::vector dy1 = sift_dy(nullptr); + double after = median(dy1); + std::printf( + "[SR] %u refiner matches per frame, %u rebuilds: applied a %+.3f px, b %+.3f px/100 px; initial window " + "%+.3f px, last residual %+.3f px (%u inliers)\n" + "[SR] OpenCV SIFT reference (%zu matches): signed dy median %+.3f px (OpenCV's own rectification %+.3f) " + "-> %+.3f px\n", + first_n, updates, ref.a, ref.b * 100.0, ref.initial_dy, ref.last_residual_dy, ref.last_matches, dy1.size(), + before, k_sr_ocv_median_dy, after); + CHECK(before == Catch::Approx(k_sr_ocv_median_dy).margin(0.3)); // the runtime reproduces OpenCV + CHECK(before > 1.2); + CHECK(updates >= 1); + CHECK(std::fabs(after) <= 0.5); + CHECK(std::fabs(ref.a) <= cfg.max_offset_px); + CHECK(std::fabs(ref.b) <= cfg.max_slope); + stbi_image_free(raw); +} + +TEST_CASE("stereo vrefine: cost of one measurement", "[stereo_vrefine][.perf]") +{ + fake fk(2.6, -0.0047); + rectifier rc; + rc.in = input_from_truth(fk.d.truth); + std::memset(&rc.geo, 0, sizeof(rc.geo)); + REQUIRE(rc.build(0.0, 0.0)); + std::vector raw(2 * W * H), rect(2 * W * H); + sim_stereo_camera_render_gray_distorted(&fk.d, 3, raw.data(), 2 * W); + rc.apply(raw.data(), rect.data()); + u_stereo_vrefine_measure_params mp; + u_stereo_vrefine_measure_defaults(&mp, 154); + std::vector s(512); + const int N = 20; + uint32_t n = 0; + auto t0 = std::chrono::steady_clock::now(); + for (int i = 0; i < N; i++) { + n = u_stereo_vrefine_measure(rect.data(), 2 * W, W, H, &mp, s.data(), (uint32_t)s.size()); + } + double ms = std::chrono::duration(std::chrono::steady_clock::now() - t0).count() / N; + auto t1 = std::chrono::steady_clock::now(); + REQUIRE(rc.build(1.0, 0.001)); + double build_ms = std::chrono::duration(std::chrono::steady_clock::now() - t1).count(); + std::printf("[perf] measure %.2f ms per 1280x480 frame (%u matches, max disparity 154); map rebuild %.1f ms\n", + ms, n, build_ms); +}