From 5cbe37594a0a57d1f84bcf2fb292198b2a9114f4 Mon Sep 17 00:00:00 2001 From: David Date: Sat, 26 Sep 2026 11:04:31 -0700 Subject: [PATCH 01/15] =?UTF-8?q?feat(stereo-camera):=20u=5Fstereo=5Frecti?= =?UTF-8?q?fy=20=E2=80=94=20Bouguet=20rectification=20+=20LUT=20remap,=20n?= =?UTF-8?q?o=20OpenCV=20(R2)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Vendor-neutral rectifier for XR_DXR_stereo_camera (ADR-043 R2), in three layers so a GPU path can replace only the last one: - geometry: OpenCV's stereoRectify(CALIB_ZERO_DISPARITY, alpha = 0) in C (half rotations + baseline onto x, min-fy focal, corner-centroid principal point, 9x9 inner-rectangle zoom), plus a border check of the real maps that zooms further until no output pixel samples outside its raw image; - float maps (what a GPU backend uploads as an RG32F texture); - CPU backend: fixed-point bilinear taps over the whole SBS image, 1/2/4 channels (GRAY8, NV12 Y + half-res UV, BGRA8). RADTAN5 / RADTAN8 / KB4 lens models, calibration rescaled when it was taken at a different size than the frames. Also u_stereo_camera_estimate_offset: SSD block matching over dx AND dy, for row-alignment checks. Co-Authored-By: Claude Opus 5.5 --- src/xrt/auxiliary/util/CMakeLists.txt | 2 + src/xrt/auxiliary/util/u_stereo_camera.c | 76 +++ src/xrt/auxiliary/util/u_stereo_camera.h | 23 + src/xrt/auxiliary/util/u_stereo_rectify.c | 678 ++++++++++++++++++++++ src/xrt/auxiliary/util/u_stereo_rectify.h | 213 +++++++ 5 files changed, 992 insertions(+) create mode 100644 src/xrt/auxiliary/util/u_stereo_rectify.c create mode 100644 src/xrt/auxiliary/util/u_stereo_rectify.h diff --git a/src/xrt/auxiliary/util/CMakeLists.txt b/src/xrt/auxiliary/util/CMakeLists.txt index 4c6484c67d..be73fa7270 100644 --- a/src/xrt/auxiliary/util/CMakeLists.txt +++ b/src/xrt/auxiliary/util/CMakeLists.txt @@ -35,6 +35,8 @@ add_library( u_capture_dims.h u_stereo_camera.c u_stereo_camera.h + u_stereo_rectify.c + u_stereo_rectify.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..b08b5dca2d 100644 --- a/src/xrt/auxiliary/util/u_stereo_camera.c +++ b/src/xrt/auxiliary/util/u_stereo_camera.c @@ -9,6 +9,7 @@ #include "util/u_stereo_camera.h" #include +#include #include @@ -427,3 +428,78 @@ 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; +} diff --git a/src/xrt/auxiliary/util/u_stereo_camera.h b/src/xrt/auxiliary/util/u_stereo_camera.h index ab17c84ad2..1e7243db38 100644 --- a/src/xrt/auxiliary/util/u_stereo_camera.h +++ b/src/xrt/auxiliary/util/u_stereo_camera.h @@ -220,6 +220,29 @@ 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); + #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..418e95ce35 --- /dev/null +++ b/src/xrt/auxiliary/util/u_stereo_rectify.c @@ -0,0 +1,678 @@ +// 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. + * + */ + +//! 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; + 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; + } + *ou = r->f * q[0] / q[2] + r->cx; + *ov = r->f * q[1] / q[2] + r->cy; + return true; +} + +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, (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; + + // 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; + u_stereo_rectify_undistort_pixel(&out->raw[e], corners[k][0], corners[k][1], &x, &y); + double p[3] = {x, y, 1.0}, q[3]; + mat3_vec(M3(out->rot[e]), p, q); + ax += q[0] / q[2]; + ay += q[1] / q[2]; + } + 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..c9b49a3133 --- /dev/null +++ b/src/xrt/auxiliary/util/u_stereo_rectify.h @@ -0,0 +1,213 @@ +// 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]; +}; + +//! 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; +}; + +/*! + * 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); + +/*! + * 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 From 617c697358c25559b28f82cc13ea0d18bcd4227a Mon Sep 17 00:00:00 2001 From: David Date: Sat, 26 Sep 2026 11:04:31 -0700 Subject: [PATCH 02/15] feat(sim_display): distorted, misaligned fake stereo camera with ground truth (R2) SIM_DISPLAY_FAKE_STEREO_CAMERA_DISTORT=1 renders the fake's scene through two RAW cameras: different per-eye intrinsics, RADTAN5 barrel lenses, and each camera turned by half of (pitch 0.35, yaw 0.25, roll 0.45 deg) in opposite senses (~6 px vertical misalignment, 0.9 deg relative roll). The symmetric split makes Bouguet recover exactly the virtual parallel pair the scene is defined in, so the ground truth after rectification is exact: aligned rows and disparity f_rect * B / Z. The camera drops NATIVELY_RECTIFIED and its calibration slot returns the truth, so RECTIFIED output exercises the service's rectifier. The scene is sampled continuously (bilinear value noise), so resampling is honest. Co-Authored-By: Claude Opus 5.5 --- .../sim_display/sim_display_stereo_camera.c | 56 +++++- .../sim_display/sim_display_stereo_camera.h | 10 + .../sim_display_stereo_camera_pattern.c | 176 ++++++++++++++++++ .../sim_display_stereo_camera_pattern.h | 88 ++++++++- 4 files changed, 321 insertions(+), 9 deletions(-) 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..dbf03f3a2f 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,9 @@ 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; }; //! Completes the plug-in-owned opaque type. @@ -46,6 +49,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 +107,13 @@ 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'; 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)" : ""); } return &cfg; } @@ -135,7 +142,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 +176,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), FAKE_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 +233,12 @@ 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; + } 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 +248,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 +299,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 +343,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..1d038f9141 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,174 @@ 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_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..5a66133860 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,81 @@ 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); + +//! 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 From e32e55620fe1e3d290476d76f8415b70c3b37b3e Mon Sep 17 00:00:00 2001 From: David Date: Sat, 26 Sep 2026 11:04:31 -0700 Subject: [PATCH 03/15] test(stereo-camera): golden rectifier tests vs OpenCV + end to end on the distorted fake tests_stereo_rectify, with fixtures generated offline by tests/fixtures/gen_stereo_rectify_fixtures.py (OpenCV 4.10) for three calibrations (the sim fake, an SR-like pair with a non-axial baseline, a RADTAN8 pair calibrated at 2x the frame size): - R1/R2 equal OpenCV's to 1e-12; principal point equals the converged reference to 1e-4 px (OpenCV's own P1 within 0.75 px: it stops undistortPoints after 5 iterations); focal within 1 % (measured +0.05..0.13 %); - maps equal initUndistortRectifyMap to 3e-5 px; - alpha = 0 really leaves no black pixel (full and half resolution); - end to end: raw |dy| median 2.9 px -> rectified worst block 0.061 px; disparity 11.33 / 37.77 px ground truth, worst block error 0.063 px; - NV12 / BGRA8 planes agree with GRAY8; hidden [.perf] case. Co-Authored-By: Claude Opus 5.5 --- tests/CMakeLists.txt | 7 + tests/fixtures/gen_stereo_rectify_fixtures.py | 145 ++++++ tests/fixtures/stereo_rectify_fixtures.h | 50 ++ tests/tests_stereo_rectify.cpp | 482 ++++++++++++++++++ 4 files changed, 684 insertions(+) create mode 100644 tests/fixtures/gen_stereo_rectify_fixtures.py create mode 100644 tests/fixtures/stereo_rectify_fixtures.h create mode 100644 tests/tests_stereo_rectify.cpp diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt index f3c18c6555..15648a9fdd 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -67,6 +67,7 @@ set(tests tests_comp_color_policy tests_m_matrix_projection tests_stereo_camera + tests_stereo_rectify ) if(XRT_BUILD_DRIVER_QWERTY) list(APPEND tests tests_qwerty_camera_profile tests_qwerty_velocity) @@ -708,3 +709,9 @@ 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) 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/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/tests_stereo_rectify.cpp b/tests/tests_stereo_rectify.cpp new file mode 100644 index 0000000000..f3f3edce35 --- /dev/null +++ b/tests/tests_stereo_rectify.cpp @@ -0,0 +1,482 @@ +// 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; +} + +const double k_ideal_fx_640 = 320.0 / std::tan(34.0 * M_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) +{ + 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 (!u_stereo_camera_estimate_offset(img, pitch, eye_w, h, x, y, B, B, 64, 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); +} + +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); +} From d9772cfac0132b09b5ba8a4470c1bc5cf1b68f53 Mon Sep 17 00:00:00 2001 From: David Date: Sat, 26 Sep 2026 11:04:31 -0700 Subject: [PATCH 04/15] =?UTF-8?q?feat(stereo-camera):=20the=20service=20re?= =?UTF-8?q?ctifies=20=E2=80=94=20RECTIFIED=20frames=20+=20rectified=20cali?= =?UTF-8?q?bration=20(R2)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit A per-camera rectifier is built at manager create from the plug-in's RAW calibration when it is CALIBRATED and not NATIVELY_RECTIFIED. It runs once per source frame on the camera thread, outside the manager lock, and only while a started stream wants RECTIFIED; RAW streams read the original frame. A RECTIFIED stream never receives unrectified pixels (a frame produced before it started is skipped). get_calibration(RECTIFIED) returns the same geometry: one pinhole for both eyes (zero disparity at infinity), no distortion, rightFromLeft = identity + (+baseline, 0, 0). horizontalFovDeg is the rectified one. The browser (PRESENT_OWNER) is refused a camera the rectifier rejected instead of getting the RAW-flagged fallback native clients get. The seam is struct scam_rectifier (backend-neutral geometry + an apply() backend: CPU LUT today). Co-Authored-By: Claude Opus 5.5 --- src/xrt/ipc/server/ipc_server_stereo_camera.c | 294 +++++++++++++++++- 1 file changed, 281 insertions(+), 13 deletions(-) diff --git a/src/xrt/ipc/server/ipc_server_stereo_camera.c b/src/xrt/ipc/server/ipc_server_stereo_camera.c index 7c343449c1..d76bd2a115 100644 --- a/src/xrt/ipc/server/ipc_server_stereo_camera.c +++ b/src/xrt/ipc/server/ipc_server_stereo_camera.c @@ -25,6 +25,21 @@ * - 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. + * * 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 +76,11 @@ #include "util/u_logging.h" #include "util/u_misc.h" #include "util/u_stereo_camera.h" +#include "util/u_stereo_rectify.h" #include #include +#include #include #ifdef XRT_OS_WINDOWS @@ -201,6 +218,29 @@ 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; +}; + struct scam_camera { uint32_t index; //!< plug-in index @@ -210,6 +250,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 +406,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 +478,154 @@ 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]); + free(r->buf); + free(r); + *rp = NULL; +} + +/*! + * 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; + 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; +} + +//! 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 +702,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 +756,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 +775,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 +812,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)); } } @@ -681,10 +892,23 @@ 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; + os_mutex_unlock(&m->lock); + bool ok = rect->apply(rect, &f, &rf); + os_mutex_lock(&m->lock); + 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 +1014,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 +1055,7 @@ 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_destroy(&m->cams[i].rect); } for (uint32_t i = 0; i < MAX_STREAMS; i++) { if (m->streams[i].used) { @@ -944,7 +1172,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 +1218,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 +1330,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 +1366,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; From 0dce802ad16c473857abd03118e81f040cb735fa Mon Sep 17 00:00:00 2001 From: David Date: Sat, 26 Sep 2026 11:04:31 -0700 Subject: [PATCH 05/15] feat(cli): camera calib --rectified prints P1/P2; probe reports row alignment, --rectified gates it Every probe now 2-D block-matches the last frame (textured 32x32 blocks) and prints |dy| median / p90 / blocks above 1 px, plus the dominant disparities as depths Z = f*B/d from the RECTIFIED calibration. probe --rectified asks for RECTIFIED output and exits 5 when frames come back RAW or the median |dy| exceeds 0.5 px. Against the distorted fake: median 0.016 px, depths 2.004 m and 0.599 m (truth 2.0 / 0.6); --raw shows the 2.6 px it fixes. Co-Authored-By: Claude Opus 5.5 --- src/xrt/targets/cli/cli_cmd_camera.c | 171 ++++++++++++++++++++++++++- 1 file changed, 168 insertions(+), 3 deletions(-) diff --git a/src/xrt/targets/cli/cli_cmd_camera.c b/src/xrt/targets/cli/cli_cmd_camera.c index 1b2c20a492..20647dadc3 100644 --- a/src/xrt/targets/cli/cli_cmd_camera.c +++ b/src/xrt/targets/cli/cli_cmd_camera.c @@ -6,7 +6,7 @@ * * 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] * * `probe` is a real consumer: it creates and starts a stream (so it goes @@ -17,6 +17,16 @@ * 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 + * (median |dy| > 0.5 px). 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. + * * Connects as a DIAG client, like `clients`: on Windows run it NON-elevated. */ @@ -229,6 +239,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 +422,98 @@ 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 blocks; + float dy_median, dy_p90; //!< |dy|, px + int outliers; //!< blocks with |dy| > 1 px (mismatches: occlusion edges, flat or repeated texture) + int modes; + float mode_dx[2]; + int mode_count[2]; +}; + +/*! + * 2-D block matching (u_stereo_camera_estimate_offset) of a GRAY8 SBS frame on + * a 32 px grid, textured blocks only (a flat wall matches anywhere): |dy| + * statistics and the two dominant horizontal disparities (1 px clusters). + */ +static void +measure_rows(const uint8_t *gray, uint32_t pitch, uint32_t eye_w, uint32_t h, struct row_alignment *out) +{ + memset(out, 0, sizeof(*out)); + enum + { + B = 32, + MAXB = 1024 + }; + static float dxs[MAXB], ady[MAXB]; + int n = 0; + for (uint32_t y = 48; y + B + 12 <= h && n < MAXB; y += B) { + for (uint32_t x = 72; x + B <= eye_w && n < 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; + } + float dx, dy; + if (!u_stereo_camera_estimate_offset(gray, pitch, eye_w, h, x, y, B, B, 64, 10, &dx, &dy)) { + continue; + } + dxs[n] = dx; + ady[n] = dy < 0 ? -dy : dy; + n++; + } + } + out->blocks = n; + if (n == 0) { + return; + } + qsort(ady, (size_t)n, sizeof(float), cmp_float); + out->dy_median = ady[n / 2]; + out->dy_p90 = ady[(n * 9) / 10]; + 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 +524,13 @@ 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; 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 @@ -505,6 +621,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; @@ -569,6 +686,39 @@ cmd_probe(struct ipc_connection *ipc_c, int argc, const char **argv) report_disparity(gray, gl.pitch[0], last.width / 2, last.height, probe_max_disparity(&props, last.width / 2), 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, &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; + } + if (ra.blocks == 0) { + printf("row alignment: n/a (no textured block)\n"); + rows_ok = false; + } else { + printf( + "row alignment (%s, %d textured 32x32 blocks): |dy| median %.3f px, p90 %.3f px, " + "%d block(s) " + "> 1 px\n", + last.output == XRT_STEREO_CAMERA_OUTPUT_RECTIFIED ? "RECTIFIED" : "RAW", ra.blocks, + 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]); + } + } + rows_ok = ra.dy_median <= 0.5f; + } } if (out_dir != NULL) { char path[1024]; @@ -615,7 +765,21 @@ cmd_probe(struct ipc_connection *ipc_c, int argc, const char **argv) 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 (median |dy| > 0.5 px)\n"); + return 5; + } + printf("--rectified: PASS — rows aligned\n"); + } + return 0; } int @@ -639,7 +803,8 @@ 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] " + " probe [] [--raw|--rectified] [--format gray8|nv12|bgra8] [--fps F] [--frames N] " + "[--seconds S] " "[--out DIR]\n"); ret = 1; } From c5ba6e651092f5299ca1d6631e1ff1c0f67c5a05 Mon Sep 17 00:00:00 2001 From: David Date: Sat, 26 Sep 2026 11:04:31 -0700 Subject: [PATCH 06/15] =?UTF-8?q?docs(stereo-camera):=20R2=20as=20built=20?= =?UTF-8?q?=E2=80=94=20rectifier,=20distorted=20fake,=20probe,=20measured?= =?UTF-8?q?=20numbers?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-Authored-By: Claude Opus 5.5 --- docs/adr/ADR-043-stereo-camera-source.md | 2 +- docs/reference/xrt_plugin_iface.md | 2 +- docs/roadmap/stereo-camera-source.md | 41 ++++++++++++- docs/specs/extensions/XR_DXR_stereo_camera.md | 58 ++++++++++++++++++- 4 files changed, 95 insertions(+), 8 deletions(-) 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..2b38111571 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,40 @@ 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). +- **Not in R2:** the GPU backend (seam in place), per-frame recalibration (`calibrationGeneration` + stays 0), and a rectifier for a camera whose calibration changes at runtime. + ## 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..6fe939a185 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,28 @@ 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: |Δ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. +Against the DISTORTED fake (macOS, Debug service): + +``` +$ 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 --frames 150 +row alignment (RECTIFIED, 221 textured 32x32 blocks): |dy| median 0.016 px, p90 0.064 px, 9 block(s) > 1 px + disparity mode 1: 11.30 px (165 blocks) -> Z = f*B/d = 2.004 m + disparity mode 2: 37.81 px (48 blocks) -> Z = f*B/d = 0.599 m +--rectified: PASS — rows aligned +$ displayxr-cli camera probe --raw --format gray8 +row alignment (RAW, 221 textured 32x32 blocks): |dy| median 2.589 px, p90 5.292 px, 183 block(s) > 1 px +``` + +(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.) + 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 +561,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. | From 8c590f80821b9041ba620264b795bfc2a4e6d45d Mon Sep 17 00:00:00 2001 From: David Date: Sat, 26 Sep 2026 11:14:05 -0700 Subject: [PATCH 07/15] feat(stereo-camera): WARN when a plug-in's calibration puts the right camera at -x A swapped SBS pair still rectifies (Bouguet does not care), but every disparity comes out negative and rightFromLeft points the wrong way. The L1 Leia provider decides eye order from the sign of T; this makes a wrong decision visible in the service log instead of only in a probe. Co-Authored-By: Claude Opus 5.5 --- src/xrt/ipc/server/ipc_server_stereo_camera.c | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/xrt/ipc/server/ipc_server_stereo_camera.c b/src/xrt/ipc/server/ipc_server_stereo_camera.c index d76bd2a115..b2d39bfcac 100644 --- a/src/xrt/ipc/server/ipc_server_stereo_camera.c +++ b/src/xrt/ipc/server/ipc_server_stereo_camera.c @@ -600,6 +600,13 @@ rectifier_create(const struct scam_camera *cam) return NULL; } r->apply = rectify_apply_cpu; + 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", From 134d8976c3924296f90d6d8136b1c077cd045a38 Mon Sep 17 00:00:00 2001 From: dfattal Date: Sun, 27 Sep 2026 00:13:49 -0700 Subject: [PATCH 08/15] fix(stereo-camera): tests_stereo_rectify builds on MSVC (no M_PI) MSVC only defines M_PI when _USE_MATH_DEFINES precedes the first , which Catch may already have pulled in. Use a local constant. First MSVC build of R2; the test passes (716 assertions, 7 cases). Co-Authored-By: Claude Opus 5.5 (1M context) --- tests/tests_stereo_rectify.cpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/tests/tests_stereo_rectify.cpp b/tests/tests_stereo_rectify.cpp index f3f3edce35..98ed2b6b48 100644 --- a/tests/tests_stereo_rectify.cpp +++ b/tests/tests_stereo_rectify.cpp @@ -72,7 +72,10 @@ input_from(const stereo_rectify_fixture &f) return in; } -const double k_ideal_fx_640 = 320.0 / std::tan(34.0 * M_PI / 180.0); +// 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 From 8acb940e18dfe8a2a824c7d4b463cf2a3111cb76 Mon Sep 17 00:00:00 2001 From: David Date: Sun, 27 Sep 2026 11:11:33 -0700 Subject: [PATCH 09/15] fix(stereo-camera): probe row alignment = NCC, baseline-aware windows, signed dy, bound hits flagged MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit First SR-hardware run: a face at 0.6 m on the 120 mm tracker is ~95 px of disparity, past the probe's 0..64 search, and the SSD matcher's +-10 px dy window clamped too — it reported "d = 64, |dy| = 10.000" as if measured. - u_stereo_camera_match_block: zero-mean NCC (blind to the two sensors' gain/offset), coarse pass on every 2nd pixel + full-res hill-climb, parabolic sub-pixel on both axes, the searched window returned, and dx_at_edge / dy_at_edge when the integer peak sits ON a bound (a clamp, not a measurement). - camera probe: disparity window f*B/0.4 m from the camera (R1 helper), dy +-24 px, accept NCC >= 0.90; prints accepted / low-NCC / at-bound counts, warns when > 20 % of blocks hit a bound, reports the SIGNED dy median, and --rectified now also fails on a constant offset (|signed median| > 0.5 px). --max-disparity / --max-dy / --min-ncc override. - sim fake: the DISTORTED variant honours _BASELINE_MM too (truth + slot). - tests: large-baseline matcher (bar 94.9 px, signed dy +1.68 recovered to 0.07 px, gain/offset, flat refused, the old 64 px window is REJECTED not reported); END TO END at 120 mm (bar 90.64 px GT, measured 90.66); the [vshift] hypotheses for the field's +1.7 px residual (per-eye cy error = constant dy ~ -dcy, transposed R = large roll-shaped dy, calib-size rescale and the plug-in's half swap exact, identity = pure scale about cy). Co-Authored-By: Claude Opus 5.5 --- docs/specs/extensions/XR_DXR_stereo_camera.md | 50 ++- src/xrt/auxiliary/util/u_stereo_camera.c | 171 ++++++++ src/xrt/auxiliary/util/u_stereo_camera.h | 44 ++ .../sim_display/sim_display_stereo_camera.c | 2 +- src/xrt/targets/cli/cli_cmd_camera.c | 122 +++-- tests/tests_stereo_rectify.cpp | 415 +++++++++++++++++- 6 files changed, 763 insertions(+), 41 deletions(-) diff --git a/docs/specs/extensions/XR_DXR_stereo_camera.md b/docs/specs/extensions/XR_DXR_stereo_camera.md index 6fe939a185..7773f530d4 100644 --- a/docs/specs/extensions/XR_DXR_stereo_camera.md +++ b/docs/specs/extensions/XR_DXR_stereo_camera.md @@ -522,27 +522,57 @@ service stats: source 30.49 Hz, delivered 30.49 Hz, published 155, skipped 4, ac ``` R2 adds row alignment to every probe (2-D block matching of the last frame, textured 32×32 -blocks: |Δ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. -Against the DISTORTED fake (macOS, Debug service): +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 --frames 150 -row alignment (RECTIFIED, 221 textured 32x32 blocks): |dy| median 0.016 px, p90 0.064 px, 9 block(s) > 1 px - disparity mode 1: 11.30 px (165 blocks) -> Z = f*B/d = 2.004 m - disparity mode 2: 37.81 px (48 blocks) -> Z = f*B/d = 0.599 m +$ 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 -$ displayxr-cli camera probe --raw --format gray8 -row alignment (RAW, 221 textured 32x32 blocks): |dy| median 2.589 px, p90 5.292 px, 183 block(s) > 1 px ``` (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 diff --git a/src/xrt/auxiliary/util/u_stereo_camera.c b/src/xrt/auxiliary/util/u_stereo_camera.c index b08b5dca2d..3da7c9ee92 100644 --- a/src/xrt/auxiliary/util/u_stereo_camera.c +++ b/src/xrt/auxiliary/util/u_stereo_camera.c @@ -8,6 +8,7 @@ #include "util/u_stereo_camera.h" +#include #include #include #include @@ -503,3 +504,173 @@ u_stereo_camera_estimate_offset(const uint8_t *gray, *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 1e7243db38..f2d41db1e4 100644 --- a/src/xrt/auxiliary/util/u_stereo_camera.h +++ b/src/xrt/auxiliary/util/u_stereo_camera.h @@ -243,6 +243,50 @@ u_stereo_camera_estimate_offset(const uint8_t *gray, 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/drivers/sim_display/sim_display_stereo_camera.c b/src/xrt/drivers/sim_display/sim_display_stereo_camera.c index dbf03f3a2f..b2254e1b54 100644 --- a/src/xrt/drivers/sim_display/sim_display_stereo_camera.c +++ b/src/xrt/drivers/sim_display/sim_display_stereo_camera.c @@ -179,7 +179,7 @@ sim_display_stereo_camera_get_calibration(struct xrt_plugin_instance *inst, 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), FAKE_BASELINE_MM, + 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]; diff --git a/src/xrt/targets/cli/cli_cmd_camera.c b/src/xrt/targets/cli/cli_cmd_camera.c index 20647dadc3..d10ac11c78 100644 --- a/src/xrt/targets/cli/cli_cmd_camera.c +++ b/src/xrt/targets/cli/cli_cmd_camera.c @@ -8,6 +8,7 @@ * camera calib [--raw|--rectified] [--json] * 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 @@ -23,7 +24,13 @@ * 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 - * (median |dy| > 0.5 px). On the distorted sim fake + * (median |dy| > 0.5 px, or a SIGNED median dy beyond +-0.5 px: a constant + * vertical offset). 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. * @@ -431,21 +438,40 @@ cmp_float(const void *a, const void *b) struct row_alignment { - int blocks; + 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 - int outliers; //!< blocks with |dy| > 1 px (mismatches: occlusion edges, flat or repeated texture) + 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 block matching (u_stereo_camera_estimate_offset) of a GRAY8 SBS frame on - * a 32 px grid, textured blocks only (a flat wall matches anywhere): |dy| - * statistics and the two dominant horizontal disparities (1 px clusters). + * 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, struct row_alignment *out) +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 @@ -453,10 +479,10 @@ measure_rows(const uint8_t *gray, uint32_t pitch, uint32_t eye_w, uint32_t h, st B = 32, MAXB = 1024 }; - static float dxs[MAXB], ady[MAXB]; + static float dxs[MAXB], ady[MAXB], sdy[MAXB]; int n = 0; - for (uint32_t y = 48; y + B + 12 <= h && n < MAXB; y += B) { - for (uint32_t x = 72; x + B <= eye_w && n < MAXB; x += B) { + 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++) { @@ -470,12 +496,27 @@ measure_rows(const uint8_t *gray, uint32_t pitch, uint32_t eye_w, uint32_t h, st if (sq / (B * B) - mean * mean < 36.0) { continue; } - float dx, dy; - if (!u_stereo_camera_estimate_offset(gray, pitch, eye_w, h, x, y, B, B, 64, 10, &dx, &dy)) { + 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; } - dxs[n] = dx; - ady[n] = dy < 0 ? -dy : dy; + 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++; } } @@ -483,6 +524,8 @@ measure_rows(const uint8_t *gray, uint32_t pitch, uint32_t eye_w, uint32_t h, st 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]; @@ -525,6 +568,8 @@ cmd_probe(struct ipc_connection *ipc_c, int argc, const char **argv) 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}; for (int i = 3; i < argc; i++) { if (strcmp(argv[i], "--raw") == 0) { output = XRT_STEREO_CAMERA_OUTPUT_RAW; @@ -542,6 +587,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] != '-') { @@ -683,13 +734,13 @@ 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, &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 && @@ -697,16 +748,28 @@ cmd_probe(struct ipc_connection *ipc_c, int argc, const char **argv) &rc) == XRT_SUCCESS) { fb = rc.eye[0].fx * rc.baseline_mm / 1000.0; } - if (ra.blocks == 0) { - printf("row alignment: n/a (no textured block)\n"); + 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( - "row alignment (%s, %d textured 32x32 blocks): |dy| median %.3f px, p90 %.3f px, " - "%d block(s) " - "> 1 px\n", - last.output == XRT_STEREO_CAMERA_OUTPUT_RECTIFIED ? "RECTIFIED" : "RAW", ra.blocks, - ra.dy_median, ra.dy_p90, ra.outliers); + " 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( @@ -717,7 +780,9 @@ cmd_probe(struct ipc_connection *ipc_c, int argc, const char **argv) ra.mode_dx[k], ra.mode_count[k]); } } - rows_ok = ra.dy_median <= 0.5f; + // A constant vertical offset fails even when |dy| scatter is small. + float sm = ra.dy_signed_median < 0 ? -ra.dy_signed_median : ra.dy_signed_median; + rows_ok = ra.dy_median <= 0.5f && sm <= 0.5f; } } if (out_dir != NULL) { @@ -774,7 +839,9 @@ cmd_probe(struct ipc_connection *ipc_c, int argc, const char **argv) return 5; } if (!rows_ok) { - printf("--rectified: FAIL — rows do not align (median |dy| > 0.5 px)\n"); + printf( + "--rectified: FAIL — rows do not align (median |dy| or |signed median dy| > 0.5 px, or too " + "few matched blocks)\n"); return 5; } printf("--rectified: PASS — rows aligned\n"); @@ -805,7 +872,8 @@ cli_cmd_camera(int argc, const char **argv) "usage: displayxr-cli camera list [--json] | calib [--raw|--rectified] [--json] |\n" " probe [] [--raw|--rectified] [--format gray8|nv12|bgra8] [--fps F] [--frames N] " "[--seconds S] " - "[--out DIR]\n"); + "[--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/tests_stereo_rectify.cpp b/tests/tests_stereo_rectify.cpp index 98ed2b6b48..8637968f7c 100644 --- a/tests/tests_stereo_rectify.cpp +++ b/tests/tests_stereo_rectify.cpp @@ -282,8 +282,14 @@ max_abs_dev(const std::vector &v, float ref) */ 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) +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; @@ -305,7 +311,18 @@ match_blocks( continue; } float dx, dy; - if (!u_stereo_camera_estimate_offset(img, pitch, eye_w, h, x, y, B, B, 64, max_dy, &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); @@ -400,6 +417,398 @@ TEST_CASE("stereo rectify: END TO END on the distorted sim fake", "[stereo_recti 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; From e412fd4af80ee5af337b2f447bfb9f5ee850c31e Mon Sep 17 00:00:00 2001 From: David Date: Sun, 27 Sep 2026 12:14:19 -0700 Subject: [PATCH 10/15] feat(sim_display): uncalibrated vertical misalignment knob on the distorted fake (R2) SIM_DISPLAY_FAKE_STEREO_CAMERA_DY (px) and _DY_SLOPE (px per 100 px) shift the right eye's rows on top of the DISTORTED pair without telling the reported calibration, like a device whose stored calibration is slightly off (the Leia SR laptop: +2.6 px, -0.47 px / 100 px). Drives the online refinement's tests. Co-Authored-By: Claude Opus 5.5 --- .../sim_display/sim_display_stereo_camera.c | 21 +++++++++++++++++++ .../sim_display_stereo_camera_pattern.c | 19 +++++++++++++++++ .../sim_display_stereo_camera_pattern.h | 12 +++++++++++ 3 files changed, 52 insertions(+) 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 b2254e1b54..caf56718aa 100644 --- a/src/xrt/drivers/sim_display/sim_display_stereo_camera.c +++ b/src/xrt/drivers/sim_display/sim_display_stereo_camera.c @@ -42,6 +42,9 @@ struct fake_config //! 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. @@ -109,11 +112,25 @@ fake_config(void) } 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%s", cfg.eye_w, cfg.eye_h, cfg.fps, cfg.baseline_mm, cfg.format, 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; } @@ -239,6 +256,10 @@ sim_display_stereo_camera_open(struct xrt_plugin_instance *inst, 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) { 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 1d038f9141..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 @@ -279,6 +279,25 @@ sim_stereo_camera_distorted_init(struct sim_stereo_camera_distorted *d, 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) { 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 5a66133860..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 @@ -103,6 +103,18 @@ sim_stereo_camera_distorted_init(struct sim_stereo_camera_distorted *d, 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, From b747aef04319971aa06b5d659e60f43b5a504b25 Mon Sep 17 00:00:00 2001 From: David Date: Sun, 27 Sep 2026 12:14:19 -0700 Subject: [PATCH 11/15] feat(stereo-camera): online vertical-alignment refinement core (u_stereo_vrefine) On a Leia SR laptop the rectified rows stayed +1.71 px apart, and OpenCV's stereoRectify reproduces that on the same frame: the stored calibration is off, not the rectifier. u_stereo_vrefine measures the residual dy = a + b (y - cy) on rectified frames (Shi-Tomasi patches, half-res NCC search, full-res NCC refinement, gated on NCC, d > 0, unclamped peaks and vertical curvature), fits it robustly (median + Tukey IRLS) and runs the controller: duty cycle, >= 50 matches over >= 3 frames, 0.2 px deadband, |a| <= 6 px, |b| <= 2 px/100 px. u_stereo_rectify folds the correction in as v_offset = a/f, v_slope = b: a symmetric per-eye vertical affine in normalized rectified coordinates applied BEFORE the principal point and the alpha = 0 crop, so P1/P2 describe the corrected frames. Adds u_stereo_rectify_rect_from_raw (the inverse map). tests_stereo_vrefine: the fake with the SR model converges to +0.000 px in one rebuild; degenerate frames never apply; clamps hold; and a REAL SR raw frame (62 KB JPEG + calibration + 63 OpenCV SIFT matches as an independent judge): signed dy median +1.715 px -> +0.075 px. Co-Authored-By: Claude Opus 5.5 --- src/xrt/auxiliary/util/CMakeLists.txt | 2 + src/xrt/auxiliary/util/u_stereo_rectify.c | 63 +- src/xrt/auxiliary/util/u_stereo_rectify.h | 29 + src/xrt/auxiliary/util/u_stereo_vrefine.c | 592 ++++++++++++++++++ src/xrt/auxiliary/util/u_stereo_vrefine.h | 203 ++++++ tests/CMakeLists.txt | 8 + .../fixtures/gen_stereo_vrefine_sr_fixture.py | 104 +++ tests/fixtures/stereo_vrefine_sr_fixture.h | 89 +++ tests/fixtures/stereo_vrefine_sr_raw.jpg | Bin 0 -> 62426 bytes tests/tests_stereo_vrefine.cpp | 547 ++++++++++++++++ 10 files changed, 1627 insertions(+), 10 deletions(-) create mode 100644 src/xrt/auxiliary/util/u_stereo_vrefine.c create mode 100644 src/xrt/auxiliary/util/u_stereo_vrefine.h create mode 100644 tests/fixtures/gen_stereo_vrefine_sr_fixture.py create mode 100644 tests/fixtures/stereo_vrefine_sr_fixture.h create mode 100644 tests/fixtures/stereo_vrefine_sr_raw.jpg create mode 100644 tests/tests_stereo_vrefine.cpp diff --git a/src/xrt/auxiliary/util/CMakeLists.txt b/src/xrt/auxiliary/util/CMakeLists.txt index be73fa7270..12e3914326 100644 --- a/src/xrt/auxiliary/util/CMakeLists.txt +++ b/src/xrt/auxiliary/util/CMakeLists.txt @@ -37,6 +37,8 @@ add_library( 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_rectify.c b/src/xrt/auxiliary/util/u_stereo_rectify.c index 418e95ce35..25fcdd62b0 100644 --- a/src/xrt/auxiliary/util/u_stereo_rectify.c +++ b/src/xrt/auxiliary/util/u_stereo_rectify.c @@ -283,9 +283,27 @@ u_stereo_rectify_undistort_pixel( * */ -//! RAW pixel of eye @p e -> rectified pixel under (f, cx, cy). false = behind. +/* + * 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(const struct u_stereo_rectify_result *r, uint32_t e, double u, double v, double *ou, double *ov) +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); @@ -294,16 +312,36 @@ raw_to_rect(const struct u_stereo_rectify_result *r, uint32_t e, double u, doubl if (q[2] <= 1e-12) { return false; } - *ou = r->f * q[0] / q[2] + r->cx; - *ov = r->f * q[1] / q[2] + r->cy; + *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, (v - r->cy) / r->f, 1.0}, q[3]; + 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; @@ -352,6 +390,11 @@ u_stereo_rectify_compute(const struct u_stereo_rectify_input *in, struct u_stere 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; @@ -428,11 +471,11 @@ u_stereo_rectify_compute(const struct u_stereo_rectify_input *in, struct u_stere double ax = 0, ay = 0; for (int k = 0; k < 4; k++) { double x, y; - u_stereo_rectify_undistort_pixel(&out->raw[e], corners[k][0], corners[k][1], &x, &y); - double p[3] = {x, y, 1.0}, q[3]; - mat3_vec(M3(out->rot[e]), p, q); - ax += q[0] / q[2]; - ay += q[1] / q[2]; + 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; diff --git a/src/xrt/auxiliary/util/u_stereo_rectify.h b/src/xrt/auxiliary/util/u_stereo_rectify.h index c9b49a3133..dbcf09f10a 100644 --- a/src/xrt/auxiliary/util/u_stereo_rectify.h +++ b/src/xrt/auxiliary/util/u_stereo_rectify.h @@ -72,6 +72,24 @@ struct u_stereo_rectify_input //! 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. @@ -95,6 +113,8 @@ struct u_stereo_rectify_result //! 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; }; /*! @@ -115,6 +135,15 @@ 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 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/tests/CMakeLists.txt b/tests/CMakeLists.txt index 15648a9fdd..3e508da75c 100644 --- a/tests/CMakeLists.txt +++ b/tests/CMakeLists.txt @@ -68,6 +68,7 @@ set(tests 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) @@ -715,3 +716,10 @@ target_include_directories(tests_stereo_camera PRIVATE ${PROJECT_SOURCE_DIR}/src # 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_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_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 0000000000000000000000000000000000000000..d1ed289244b9212d27ae1217fee00e77d0a92d86 GIT binary patch literal 62426 zcmXWCe_WF1{y%;rD(VdTK?@Vn!ll?wiLwF(1$&1`35vBku9byN@sx>}DNY5=+VK!S z0#_L!9zn7*`?1-wQ!cD2+LkzQYHCJ;5jiszHO=gEpYP*)!++e5>xPT#x?ay;ua5&C zt-wkKEs+L5AOHXXU%*MSDiO(mWtbngx zxdOfl{>djRKV7+M4Px!uwZ4Aqke?!us}XAv9}U1KUJ$DXIP5CmyZBIKid= zjHBOVW^uFm`6mlb6&499PFJ2e``tgQ&MTDF-(R?RspjAHT3v(wYU8yZe`>z{^PQHv z|9$xA*Y*x$XV+u1rT5Ri{(+~1FGg+lv6qfl&ev1ZGp^aW`F9J89|r-i|4myQuyk8? z9@mitr@I#aIMZQUNZor1exLp0+i1HP(p(b9NNQ45)Ufk#!|Xug0K3uvDb?v;0O2P~IS=`L+Y5B7C|BX0S>9CV?R^4RZ&lQ9r1VDox?ppZd@ z%JL(_MF?!HqZ6G~rMa-wq|XDcnk=_B1Z^cdi?FKE8hC(&yUSKFY@X^6RC)c)czCe# zp&cR>q^etA4JQC6wHl1oRagrDeUVfD(S;BP*NUC7ZK)eA60AAuxwOLh@w`&`!+Xv z{m#RtBQX}y>Mc=K{|NggMD40{D^Ez3^j1|ysYle!YmM=7SH37-ecw_g@8<6g&r7tg z|JoqVjnm&5hZbJ7$?!DE%Z|dsC0s#C$S~9VlPbVB~O$z^Mbuna|+I8&YOGOBJIHgzZ zT4l`Me#*pPvWWEx6CP5|AG{E&^mWy1Oss55?7_1&drkhfL19Gna=uWAQy8;z$BHJB z#w{U&(F-o4F4!M)eMRUFW}C53hUQ5hD#M^PK8tzabJx5(a-o#$vE@xPXFO32TX+&0 z&(de_Z9|#W(Nphw;4m^w=Tm=c3EM6i8Mr!4H6ccy#VF{ASqGjI_}(&?kxAwXMdQ2V z^E<;=pH}5%{q3ImOicugII4v6odc;AveITV#Xn9*%tffuEfm7+Vrq3&**W*s)N#iC z?_K)Ce=izeYSgZ1{x$FEWKE%1mF}8g1Ac^Y>P}7#H>et4y`$VqH)1IHqbBJoX5*C> zl!WW=Ib0!SbCEXab*}D|@5%Np0kT4^yYGypPq@Un^@eLWV=MO{NS1gh%FiS*Cv^e8 zOs287iN={<27K3!9g`w9X9(W2k0Gs7)V-lqlyx}~Q4AWd({ww+C>20#z&FaOoLpR= z>g>;6bK0Q}zmLE*Sy(8>ozoPCiL3NCzVnHX`Uv!E-U?hFu%~!pUv_2#4L71xbO7zZ zbYTZU7w%=6@;L!T1x?}qnn3xoeZ`6=0srj!fTfb)A6})1de={t?cm>ITb5#<c+?6TBXMfHJCUO`r%(CoMr5m0 zZrA~B=u4PxQ=TASH*}s05ML@iI-+W`vn71XpmNTHu+8t>?jIx-;7pcQz?xx}Tzpwc zI%!+z;WeA{ZFudNz-KK@kn(&);qO|)vOL(nZxeDY#~nh3uj za-Cx%Fn3I57=v$44s~8PvncASaGM^lq>JsbP+F%sN&Ru z7p>fc;9TzZOpWyvzVAyyJ2n*Aj^FY^Wg|EK)*nc&B+T?KZ@jn%Vj|6H_!8Kydetfk zIm=X9ww^tt_`z|PQRK)w9n#m(zZ=>W8utbTOW96sEd0L4(G@f z#T|T4TLNi4FMS`e!Mc*Llr@r|fmV4h{4_kT3$7fMt-7=DQdTfA$=qBk$a37$b#~(z z6e7xpbx%!L?1@O&QJY44v}R&Fe(8B-xD}!hv*q%Mq#PJR<=Civ3v?u550=C>YkeWL z%dNp21GH+dJ1>c73*is5mA;W>jDv2jc9=V)hL9q!p(LI-krsbS%#o%ryX>3_3HP!7 z0hJaJ%^8^21|vpmcAzQ9*yfCCTMZP!DfpA-43`d2 z0Thj!_ut+noB3ki_^vj#AKJC{vmcqbgwJCSVs*6QDd(=~TJWszd|49>e{L3f5j>R} zOz)B*$tCYonmzo%!b?ks0Uv=%s9aogAj5XE!9`Bm6qETD|5y4+0=YfdehRP2ys0}p z6v)-^4H}i)pn>z3oOD~GQKQ9RYP&Bq_%WE;mtxM(TvpIIckGhKCV1398|$U@sk zRV-W5G59m?&_9DYQo)W6vh$IAc7pyi*~?@WuZ}X{!#~B}0|J@R7P^fL9(K10v-WRf z=v)}uWsTs`03^u_&*eXXe8-{h{|L-JK~DBq&IY4h!{>2^SmhYuVAA#gX;9OaZb(R3 zB|)qnwm@vL%D)s<{(jl<5iPpi9CBkP5Q=ot$8Jr$3Oh){Rg5zVM3TbB{(wYdY;%WI z4Xj-g1@MMiCgbgV3)y)f0)?uAF>>Q$Ue>&d1OkbgIon2=uQeZk7RXe6;G*mh=6elf zA(b9`U%=LrcY08A_@bWF?`-&s<0iZ?y}VFA;hq_aI4z!{z9MQG+g_J6LTrI7i!SU) zAx7snEixd)c@O$zV$sBiGT)fCO#++N1usJ%GzA+*Wk8Vwt!xP?USxYX+|B0^qoNtV z-DYs&M^#kJkoi*2D(6Tg`8%XW2KVF|&W`slrhat?7OOngH5b@OS)3V^xxxL$7);o8 zkL}l7_S6g9xmabz`J(Zpwl*VlMC}>3*eklTjmcwSg3H1Wci^a~Q;Vq`Ys=JO?;oj- z$WWp_>3?HJYpUp`rnnP9=9;?)rd)bY(=qfV?Ez6IY@lZ!&xLq+@2hdT?jm$2OgC;rWuOyIr0uq%;jE^OqC1VIZ`rbexatiDV;|^Za@GJA zB&ybtbl{{mFfu^x`ho7KNJ@?pLs^^elCcPpIFHvSTP;XW{ioqoVFR@C{e3~oy;ICs zC3c~;q@92}@uD3@u$SU1nbCetKJX&^3-PMjhe9E`Ir6+`G&|j6+;kf_H{jAN{R%PN z-q2|yET)tj)jZlT`av~RHEYssf){B6TB9Ztp2abUp}m~eU;?^91z=EY$S}GDOJl;u zDF>i^65Iq8qbGoiAE|@7UB072eJH`QHp{p>GJu3MntiIqlj3~W0}1U(X2L?JH0g@T z#hya^r8ZRRk6uKi76^KDOZW8M9~Cw66ldRsm}1cc#eGU;&ctiZbkH|dGd*$gN!p8^ zbRVk0HLneY^(j|b7E&Ioi_I%xx7|-^JWVhuBe{i4oYfvFA!jf_^IH^f` z)y&|<-&E5)CtWW&i|{?(Hd@V$o~xf+nQ)>{L8?8JeIjJ0ul$z^Lfu)E)HtH7keg+` zvznD>uNT<{%YQu|lQM(m)2TLev#td^^im(D`(`50Ug+AUFGM7Gc{x3`D%j(~ z@7yx1uvG1sRu9A~uWi4vo1b_Sezs=Yi?ies?C(5t&AwRGoJI_9d4gH(S@vO%_S^a( z9eyjRs?3MdjT`Q1G6ptovuCQatQBVI3y0WJ?)R%1@GNQkA%g$BPo+r@I@r5y^2R&1 zugI+YR{K)g+A}Kug&i3)t9AC*@W{d??6uN^g)b}pLl-S~QOSj&f$!NB>cVz+v>z<6 z(bLP$8R}Eq_IXP92y9Y$ePY{IZFMn_^Vy24GIOD=If3VxrAyBFHD=`CM8 zMTwDYo7Ei_4HPG@3*At*=*q|p@quBgkQwRrHMm=9R5zoI_^jDkO8 z5Zho45LjevqE^)8g&i`s2G9r&m~J~VoeEpR8b1dd2s4ZcATDBre8y1qe;otK?LXoc zl3_u=vul+r30k|aV+m`R>?jLQnU-d6Umb~~Gm&qjoU}(`1?3>c7*lW_OJ1_W%Bgh#RWe3dod;2_zW2-DHL32L%YY+lk8jmkLchDyUM{yxa1 z{Ggs10Nh>c*Qnj zJBpkPN?aPnllz#um`sc6PSnN~C{S?af2JjZ9vG^J&^9qaId%g9cv5wn4R8FMbWfrV zdgbXj{ap^RK8F_5gZO^J$k=xP{t-Bv&Ac)*t64oj#`0P+`>S$|!4*rd!jV2ZWVk+2 zIT^RBJMLl4N=Ki1WhMdfs9*BT`H0|b50e5Qa{lQ$c8o^E_m*fO3icN7kQpj4heTYo z+YqPWXI%|4$Q)jO`t{B)5)QDx?;tF3*G>&>Qa!13Cc(rfmzB@%N%)f9%SHD5N$~6p ze4pC6P_*=F7-vkKNc^iTx?l#RoyQidlJknlT)M${D4Vj)CTaec;yeMZZctiF5t(u4 zw!=imca{tOP6iNM<7GvqTfFCpIv5hBpr97bZ;ZWy)E!6f+K~G2Y5tSoHapaX_r;C3 zqmEniKXCU!Z^XSw!Z-+Z)D`Lq#0%4dzRQw}6O6mh6yPS)X96cM=m$F zI0y(0uZq@P_D}_zLYG%uu~mJ7GUpkWumv9ebrNqA6X((n{*R?wi6FW>?f5ya327?5 zViW)ySpJJWUOtg=;q1F(#TK|VDJH}(JA3t4AcTJrnMmSjw4d+r%@2I_xgYEC>0w(? z6Q(24Iz?f_^CGy3Fp^nF07^G0r|%*`;!EushHWe_0&=s$?|)A;9ZuF}pzp8$GKJD@ zt7SH?#F-9d$5W6?jfjlt#5r9I&v=NzO@GvgxYp`LY_NsGh+q|urx2_HbEU!V8^rK> z)-y#v6=7_O<9_m2Oe!B-)wgc*`tg?O>qNvA0bIG_SB~X&nET1m%jFTQoZj?R@*ay4 zh|Z9>dh8B~vi@aY2bb;}{TA!hW$*`auv&SktsZiA2cwuXUGM(>h>#WAd-=zrCtH>z z!elsvp!2scD_l2dOHev_%_=(P&TT5fk_QN_#I;HGI>QZ9o*qr&l+lXp?^I$R3X)E#sYqQ zau(6(RvzQmEhIV~2)qXG>OJN)LQlVJC|{z4Un%p+GF@MRMRrawyr2l7-68Yv_;03Y zurfWb1nU>OP&;WJ}ico|P4994!LQI>gu}Wp$qpUd>}X-Y-UDftVI2W8aAs1tnEJ zDD%yQ;O(jE;sTeJ(_C%TvqXHEoTq_(Yu}l>SxYKo{6zrJg-rxVTcUz$wuWV*L;JT(up#!OzeUN zVt*}<_R}{-9~K@x zDudSE5b$6^I9FpM)v$5pQHl{2LZZAFJVPCSQ_PmDPk$q%5=h^a-!%fsYxlgYKH#}% znLlP+FlbRTyM98#^l5Gg%=6o!rGi=p&plH;&`=(YZq}@9y&1--ZpR;{b`lVOh|NFQ zNTqml94WtUUtL=rJ+y|s4Gps;_D*w?rz}kvlp0s#?e6Db?hSs=CK3Y;PTKZWCcO3D z8FJRq!J=1j4DNXQ7G^YCq)?sIhM(e90rsG=1m9vAHd*3mau-jTZU@_~aHW_jV7s8V zeSjcG)7F@T9ZA8no&NgFn@i5M6qH2-MNTGlK?zG(%Cp{lEdl*+rLVQLc~yBCO8@~R z#1COxygE@;Nyfmd(9NBA=nMyn(5{a56|mDYk&YIf{iJ_;u+O;B#(Z@bX)VygPGDex z&ey*>R6d-o!80{`?Ni-l3_%?R|EDyRXw4W^3ZtdpR&9stT@&={xy-oeJk6jNs^+kX z`Vn}oFFGd|qmqdsDqjoAmvi^8u#}dS(Y@t3=SqJj%`E@s2jKmMzYyT;a`_}^d_=YpMih)$5hyN>XQ92B*1xM*A z3oigd$uNNz(zMY2#aFN;F8$4l4Fvou1r0J`uHGl<<-qzMn5z#FT)NLAyijl#+>@DG zoKoG{wiNcaNjf6a>7;)Z!h5%vqEd1EClca`x|{?8%EF>>4K_*d-S$l*Op!-D)d zjclJDA)*!OjzNtAYF8+!xaao-X7j~O%6#CYZd_y;rzRO{$7lvm781b`OA?y4yITu% zDW;q_1`l$URD#vz5s2+!7+6*8+m{Lqek*G9irh4<4R!@)89mE!5)hY+brj(N#D^^C zOHA#<%H)L7u%Yv~a&j06oj~F-61xl&eIORzBOu;FINN!^MLtbVLb|KcLJ*xx}%cb|7vx8;I@4i z_#-!9)oQ6Pz?xW^Sjz9go&CU2Uj_G(Z=Hk)PG!+o+`V4P8U zxL?lPEo!qS(;)a~_xR5jlRX1!*!4u{4ME=dieMgHpX+ZIPZzVLD#wl1jpwcvXSTvU zBO(iWQa|am&bjm%9t$)-26fe7L{Y>2qeuY^?U85%6If zR*A%D9yN}o3rkPfZ9bgf4&nz!ya5GgIg?_9C?6h7s>AzTR=gAZp}1$kx0o35+La#k zz90K&?+z13HRc&3%Rxu-BMMi$|5d@aB=znXNF% zU7g{u`VvC#QC#EQ;Wc{BfSYfS__c1{l3O&YjBCJ6(kN|DJkI2LgdLE;(*BXL%4w;J zbdz!U#sgp}`cRz8q|R=-(tp)zzS#jw8K)P8R)`W?IEwZ!y!8_y4uoY0s&9%xwn3%T z5fuszysUAI=}dt?W(^wL5)dr}XFsUs?q8+G#;+NaZcXiMtsT3pCbfT?h zchq*IK^y8RL%{_Cwp#=paeR{bWiUsLg^if9x5L~|X^%b|{t)Ivb;`jiZ5h1qu4u{W zM+j`?e&Q>z^q#@o(n}z=X$=5t3fTVM$ID<<2~xc~SWYHNWT+F8(#_!iYrX~@2Cyuk zic7R18$@?Vs?@H@niHg4P($zq!qRg+leYXJR+uk0zj;mnBDjMv_wv#m|4!S8u-@PJ z5jc1@BtPgwcB+wVn$ZTvnGnFx-Hlfyetc(A7jP8(-X}a&UV#w&aiR-RmzyxFkrbf> z>>GMS`iu}aktnvKe`$vH2-q@sR4*GHJ*r$Waz)TnU*l8h2vzs`9*-$K#(pzMKux`4 zhYgeCe0T?Zg}B}B>a!K&G~$aNS8Un7N(^uvCw6oNmN+RXB6;l?6;^xk_|m+# z?Qg2Ew%GJ&r zeCi*(jMT%`%gqH2q}D24^Uiz*CaT#s)9tP8@N$jQANhLWu{j@-+*M-u&P}J5@tSH_ z1D97ZKEd)q@}*5*$WDSa=ds4`kbt6N1!5y%rPu;{Zc!kQzD~?H;_D({1bl@%kAkYz z1r(urh@@O@fM`~mzD|`YL^>BNd5PLCSMYkl1A>vULo_J4a)vNLF9OyV9`w>=y5~|F ztXtW6#mOtNAwZV1zjmtb)}X3|5UUjOyHLr$eLs3xl}UQ zBL&}7y1SeoxYr}25)Dw4`QO!xmLK`1Z0->__Wi>-k|po&ExM(!1FM-4+;-!_WYVxV z233RL_3K|yO<%Wi-OiJ*?lh97Si^}>SA9r{U$)}64Fc&yd1BDkVD?#Lcu}nD_tQsk zT{{0~J)Dv1{kN>XAXVXPfkzjw&z_lN1IRp(XZ&Gws}PraAx^x))d~o+4XbYd=*jhV zh@mudvAk;Aj7x)pz7iy(&;)evs1Ty!wQnqs&Yy4Ob-)g##q);{t^ZA?1&Df6qaB#0 zEpWmF4IU*nS1#=gzlwo2;o3lqYhKlbK=F-QGxaW%>TiaJ`cc{|1#FNwE} zT;OcA^vSU8!LPl^*k5rdpGZ$gJw{TtHO;GQw9;zZsPg*K;nLbd7c7)6!pjSw^@`Rb z>n`Zi3Zi*W04&F3-HA=oi)*qvNshS3qVL@$*lnv!2L$GuR{kU<6jR=U7c;3!vmkjRmT0s_G8W`i$JZ`6N=a65JHZSg84&~B`^sy5eSRD?e8D^fZ1F3ondme{(FDYM<9IP?t+Gu z0yhuj(DR`hS~>-PDP&If7hy6f@09O)Pe|7|!!yPrxV5q7Dbn4g!vq2Z9W6wx<_^~$ zX52?*PB%st$YroPZAUI~DGXSdh`(eAMG4Gj7GiGv!u9``Tx>m6k3{!+e8#7=!BFx^ zx-oQVhS{;Kd>G;VWaDbes}kq0GP1Ld$!9$+_e0i`4nGLrJ!Qi5Njfgn&CTK~X_2tB z?CUUQtMf@);4ezg;RJ+^7M1vpEqQpRgY_ywfD>5=vzl{{Dj53$X@n(bTYS2jrEkTn z<|h+)DqElWqVZsf2ey-{;wKFP{voxjL)k#G0D8xKMw~7v3uiNCg!>Tx>R0*B2R6E0 z#mM;mLnErS`}8`(T2fBzQ*Bouh*PAHuWn%auJpWtRxk}6ytcrr&O3|du=Ko`DFuVF zkVuYd!wZk6^p-LB*rQNkoX-8sMD_AsWC!m~AaSn#6QBBdv@qfJ zMhTp^W1Bz}&jIOjDB9I$cihS55d*PWvAo|o2*wTIyf%%fIUljo=cU4<pRv%**194Dnr;%TLz3axUA%L8XM+1jK$;yz4e_0l>U8(Y8SuZ-{7J zU*r_+9uto5Dz6VLxf)dLRvu9jMZZSrh^om&c0v&mhQ(6lpPevuGf*(IXi>f>GbFd) z1JCuee^m4%G?}PXAHtAbo0J}*ji%Nu!&`{jqa(KF+Ns>dycPz3g&)o0Id zD~YhRg}-`-7lG!{-om-_u%H*zudl5;l_s|3Y_rHC6Q*6-++FEDM;aod#BB8?`v>ke zyW}}zhq1+lu$+e-W(I{pQI@Z2vv*22&(^QUWZtVN3*ygUYyL}R*x`ZvaXK9~Hv5|U{82}2ZQsR@02Vr{ z&kXkKzH}$Bcpf3hysFOt(b66FDFwaW-|(`U;7|I%rG75pO;CVfg0)~)ceI;HXz&;u zFnt8BUs2qXGL{PC}!<&{OEmUWb2je~qE@uxuV zI7e$ZiZ|3hqK?05WWxmCY`^6STuhElH5mJp-SxEdO94#_XgZ-=@FV6kLSKmF@_ z{c((F_+)(+sAUN?@I>Ooj&BDHq*CO+3BUQeTu+E+Jn2tx1K(SNcBhKc>qRnw&$$b^ zp$&zjvexS-wa)B6C_l)C!U_{Cb?hI7BLAr9Ii#-UpLZnOYYpZydTKMdHWILA43YBK(@T4cQ{eF+!=~JIY z*r?jw1D?X59N&&p})nVg}7M(ApLr^eI@8-*j5Oz+R!KlOC%vhY$2VQGw_j&JSBsgow(!vK| z3hAM=jPHCZdKdr9Om7LvsFK!A-Wky%p(MFl*+=55pJAbP8}v0d$Oq_7ia6P|R7t>w zg38Yi5yMb9V~=VEL?POtq1lETsLZCV;YFm>h-k;m#IClsB7rlOjt7ZG5k57B|iKP)aJIGFYLhX;GoJDuMRRJJ+=oLmKx(5gHLI{^|kw| zUBmE-n!W4<40J}LhRkY1D%v-Y`L((*AZ(IOaAyQ8*paQ){H3&ZGP)P|n z3%~{2InxbdbSrE|H&kA1=3vz7UX&2dqrq_%(zLJd{a0ZWr1-y~xG6n9F?3<7lIl-< zyI7Ma#=5@&VJ#pZs#5%x^1KOm)*q$=mKHFYNv22qjPd3qBbO5DYKT4$JeyQ7-E*Mg zsZM?M9@Nhe(0lPLf}(7LmwUG`sV}P$HnOydaD9&uX4`#8$y0m?-@Voy1o_7nQORfH zA~3It+3AXzRezOW8LQ6wk%_wQ9>9NguHk%vEvQ@8-tnSsUN;B;1+#kJw~-#fQdPnK zYAfH^KPXS>BL7kiI3Bpk93lBBueesVQ8MrbD;dCO|SJ_Zy(eaVEn&g7FU&+TuWH(XC35@LkDBAHB*x_%G&UdqolwdxMmT)lc ziocyxUKZ|V%_PRUPOnTHz2IJ9@l<4iK8SY+=wj)RMy6Uhwji3ENWAHrA8M9mpgjVZ zL~?P8#1G?4ROP?sQimg`qcxFmn1lJ6ZfM8IU?>pQQ@jp*1QuVRTa&hUqgL`ql)gb_ zj2Q7B=b0-4@*Y_%eOGR#&%L zwj)u~SUsWL*RMaY(JRi;6g~|@&FR7@_GNNo#TiUrt;FwGGVKjFI^PZ=LV;wdmdZ{L zXV@evw)7Hf_dGt_tHpJ;1pBTSRRz!PwizF!?YJu)X@{f~+Z8?R$g7>U zK>lQ+sNAc|DR0LDcSsN>yTxAHah3$_0e9@o0DsY(E*dDSBt{zh$9LU9Spn=1bZAZ( zRIFMv!B|8`u9IX$;XAN*TxSu~AZQqp)8%sT`z3Mpjj ztnO6=4U9HMp@r|wGyMhRR_?tB-uQTX*a0#6d6UP_Q({nuv_@c~SJL?7EW1PcShl7G zKGJ*-*XbCkxeu+$6FmsF`la#n^vJ6ZtK)1e$BATa({E4)&66i2CvC(M4InC(T*}!A zT_su2e>(h2xez{_&Rc4IHFVw&W$8dn6Y+l@Wd4Ji3z!I#yRI0{N1JmNa$&5rGgQkbL~C15Ajh@9&?~h^@1W z$pEKQ^R2$O*u3VvpN_y&h`+he*d5Kz@O4bx|gz)obU*lY`rToHbqPXXh{;<;IE z&=zv>8s^>*{vumK+rvROKl%--zJKIg%fijDSKB#N5wKoMy*=F)T8DpwyE1IyDP~=D zlGqeetGgZIWIdIuOV!Q)x=US(Hvc<48I+0w)31-|8V#$A`_b{|K3UYB$ph z`GyNIV4#~6`jC@>_beA;T3+n@qOEOm*PXFYNesI8+C>zqmde^|wOY*tgK>9h^Plb? z$j(z+a;ZyUaoeJ2de}192?OYB^Mw4R#yvTqaaTxNh@Hk6&9`ld6O7++12ZGSLQwwG zbq1?z8M}WjL>15Kj8CpSKdU47zBeED$m9Ch2qEv+3r@CUyJ=BUU9fw_YwT~b$ijMl z`gy{9J*iw^!L`llIw+2}^&yr6viOfcZ1bk*T-@LV?lbA{H8l5|k5cd75<992UWkP_ z=g76Ser*K8M6XnkwcExWUrhb^Itw{>wHA^P;^lE0lNCz~&~&-*+Eo*Ot=Q`Bn?HB;5rQ1`0sD7JUV?){G`#W?BqJQ^ zl2Tf{?evxtpnLh|Dt*H$unMWPUnOTRS~=5p1Y*>C%d`zgtRL0tTt*A3G_ z{z-yJSzt)yZLXg_J1eHQ;(Q5rAJ{-Na_#{Zn3^1C4)4i}_StbSlq97xBAZk;49N`q z{(CM(Z=9BShkkLq@K6(B+Z%$l=YDPW*FW4f61Cy}MMiVu;~%d3xm|ob%n|oMo~yB5 zyXTg;4N(fyA#;tv)0j<`I_bjmd=0u`pm|G}~6YW67ELB$!~UsAWVD zIt_uZUew5>7f(SKEO$`8v-O}bWRVGDIjXtF0;>zD8!Taxvj+3>o(r8x>`0>ul`>{u zO?X+oM^sUj1@$L?07KqugkJMHLN2_pq0t{XKJJ{P8er}pO0t4i;;J#Tx}fbh-X+sb z{voA!_1|F?3+r=w{KzaPgZ@h3$4Wy$ho=TF6!gUDf{Jo^PXY>T3I)R(69Pt@oss(m zbM@&%oy4G~09eYx`U2|m#uYK8kT+P+&83^BHNLZ?vgox$o7Blz_m4b&&2GC~a;8?y z5k1-b?XyCp?xCw2)`au{1K@es$3WmhT>@yhJS@M`^@I?_D43{6@yrVMQ>GRT*s=xh zT}kt@gB~Eb)U{b{`p?tB63C3g<=&Ad&`=rr>G>ji`|kNU?PquW+jI*;lwx-- z$h;X4Fm;?_2?VrIe{?gxY)jetZRtX$H&`cB$(}ZOHyImWNm3_8Sc8Mi>eGJ4yE-UF zNGD(Wy6!8{Py-hD3@0~^vj!sX|8K6ukpm9s%{o~a@)f8jkL}^Ltf!6L^V#Ve?N~@o z6;HHp{X`{cZ48)oY2Ob7UDXX)U^5L@0X25O&=I$O4RRKGdjn4>@JZ&YU8mQG%ql_` zN%Bx8QQ4dcP_YA%ncE!~%{xkjO5ZkAcXsnAND}`|){qJwQEV8vqD%kJ{s>6H%dC+UV4*oJmr11sg(Y<#=A!ltLum$LghCpW}#o750 zAX&QMxqc0R2HKpSI$elSNn9MVV760dY$RDyyoSLOv+lppr9<7S=cif_8*RjnlARku z39fo&x+mGmBJnM^BMC!GGJPk!VUQ*cMuf6-5lIme+2gBRAD2 z?*h|=`#R{K^Az-JUY*t+y}yZ3c!W1B=vni(4qTEPD!s@Vx%RuynqDa@?Vo2cWUs5g za|ms=yLzkjxC09IJA4HGq%tGF645Db#7;8V33Qo|ak>C_UTJc14+cp9VP&P8j6eG% z^k70+8qM80w`k@+mw(6t?7VlWw^XxeS4D+*P6zp}BcaE9wJXHt{1TR9Yx2$W%2D_B zc;?-AG(pEY5y!1pIxdYXOt-@bCfGPLp5Xxft68YjEIV4byr(iT*vx}{nAML6}&T)D`3 zetTnRs8}?p+}fM^U2wU_rZ#mHGk7g%)p+@L5ZZmzHiVYnu`~Fa(41(2d>lG?D zwi4DJMLEb+E#M~L+^qi(ihcai8sJN3Z=C`IbUxSWAk4qR-UqtqA(kHNo6JALgFlAma<4k-4$ErxWMh zYL=!z*)jplY0(04vaG?qKQebhfp1&6IFTup0yu7%WjX0!mtGc3U8?NaOZ7LlqAI%u z1$vLAs70{VsS9r{5Ld|seyPKPV~bXZr;@&$MO+%XBW6!@{so3>vXB^b zvqc@%v&h!_Bm|`urQTEJg_EkC6;*j+&Q+}s;$&B#lL=U*jXHN|2Q$p7N|!cmfCcrP z*`a18X!IVB)zK-%jfldpb$PJQ5x#qB_4#UZHBJcD^~|0gPtHya~tCsH(h{1 z{lN?&U;3)p{6k5Xb42~0a#quZju(W3-k%vlD@9aP zX+obWb$Cz42_j5KRK}rRUO;gq*wR-M@7Oc~X;!ZgN$y^~h!XRpX}JOawYgQFMFw(l z!bmWk_`|)kzqQ5b4yN4Gg^SgKDw;pR&V~%+gB^*q7?&Qk9V5e@f7F^6E%}p4Nx3zU znim-vRl~UjgNa^aeHue%r24eooDdfo?AY*O9Z0UCn25SyS)zm7c*AIxCtzwHpNUDh zr6q|4^Pu65@C*T)#~uM3&&$Fa&uhN)PwBGV23EqF_V0s@vxX#(#*;EStu|-UUQm}e ziav1baFWOpt^hjW91#No4n~LX;DIsv@)E4}I3`G_o$hjw-?OV9men$L9w|Ktd!P#& z@7L=vhi{G3yt_=)jxe0Cl{a3+W)iy4&7(r;4L*He4m=N*eIPTC3j}`z&SATt`nwI^ z2Mf$%MJIrEy}9xebMWsinhkAMG254K8eKy93A~mR+ zSJaQFG{=J?JjHo@u!o05w6kwKCb%h2ft^9&VrVz(c0hb>n$ML7X*&=26j=2?vk;MI z2%xFq_S9WL{cbkYzy2GV_fpsZ0FzgV*VJARu?PhB(mhYWXbvwm5xKel3szPT1+j@P`pd#y}~7R2HPHRm{2j81c#TL#b5r8WP{zNUr!Jer5YBO z@o*L3H&b_E^D7>ubI1tQebMm|cm#?rVl-@8pDR2W+NTIT@BIXG)G*!-yx>)N2Tv3K z3hORu-2d&bGs_69)wh)F)Fq&5*tEh9oLT4%hIe!Rl8a~BgAc4Ec#1s_-rZDwxv;zR zN@F|a;i9DZ7?@)#aW+--a1hrhivsp7n;v#|k`i*Vb51)LFkrdW{}L}6SP4q6*d41B zqV{l^QQfS|1=9YH4QEp7y1xQWBGxW=A+CMpPe8gfkZ1pFC!lk^+JW<79p3IQm3lpu zaRBZIcB)=LO)-QKwg*ifUaveh#kh3AHvJgA#klc`N<3QMQS>UyTNnct0KgNst9Nd( z=4-9Ewp)H`Wy0Z}KgzvKwjE)iWakgMBKOs0jC*Lh{u=Pe%vfAPpyO#z~ zsY5T3o*K1VLd&(5Hha1`BGs5J#y}))6^Xs}AcI-hk%SZ`ljV;XW%A^%25T%bk_Q3TS?YaGH8;L~v#Ic#&K3-Ln>W+62# zsyi%YX+H|ye2suAMU<^0IU2bB-LT)lE+{&N?ANr_J+~}8E0-rXHU*#C0|tzqHr|?+ zD<*2HvUUVxZlqmHJHSk9k==h+|j>xt= zFVp$l&wJ!dTK;^Bu?`xf!?Nqi35fEj-h9{9KLg}TjgQY-rgWy@>c)p$=SQW8@MQDx|E*9>G z%xtQ4XEwXU74IYUfYX4$OrhQqPGqN@ANc<%Zu(r4f{AiB2TlCkeS90We>hzljz>>A z&4NQ88`D-SnAeE`mj^-h+)M>-qQi{H2^bHg&N%u6EHCEAb=nww?{rG=^&W_jjVUU& zc$pkuPt>K52u6v|xg^_&dgUnsSBZ~VLhpL)HSH)5{n3}JsS*vw z3`t#UUNQOllj@Mu+~KEN=*YY>7z)#{)_q1#G0-ZbV4Ubj>C(h>CWO&n>O-;MD6K60 z$1wY=TWc$58`y8+051H`y0wm_YUPvq!HVjRzcT_WYTEVFI5E_=O_k;dfknmegjoN{ zM~x(83dvZ?y}@*0#-V3e#XVC z(^p}HIzS(HFFBtQNZitZXjhPHHclb+W$+qW5S&f`UQtWvw9Yl0dJn>Dm48QlPfv^6Kr-C>1NWo=wlbyXg&+IaPnZ@-&XX zm3H$2BDwU4&{y@EES(P})e12>w~Sv!>{u*H#j6F%KB&y$Ocf8;dMMC z=vA{XIvSf5I$4ph9sx7eNXZPOBWd~hX+TE=8e+k(v%dabJA+OTwODiXp|fH~d(3z? zpvz9s35!hAfob>dHgG3~kzBtmT2L*r$Vuf+X2d2JCs6rcE=6{kA)$Bf`j-pf-yqchPJu zXXT-iC-2V5kEI7=V4@?tv{$B&_XJ56pTxZ~?eU-|Li*AZBP`asPe8gNEp{eA^JTye zHNt#8lwSHB%=69~70`tsynMxPWHTfGayU%8N1WrsAWh>H{w8~AzTfc7(q%;|hectR z3&M$7xKODP#y4(CMj(UvfZ0b?jk&LH_Z?!{#mc;_tb!>jp+sOEeCsB!U z2A8T<+ykMEP&*nGMBeDL=zN_vmer-JYBCxkL26cJ|?c^w>zCRJ zzy+cJl4RbUEM>b`a9I;)pn(g;bP$P-{?^(R>h)Uax8v}hf>etOkCM=E*r%axqAe@M z6@4UN8{Kt>VVtP+)_+l2p@TLE@C=TcaWt|QP+8-Wrv38MO;~&A0IOKD$n#T>d`|7Pc4IgTE|rkm?x;6l;>s|8Z%yx4<+KUMVC8=J8+^% z@}fj+qp!7D(dYbefxcOhsM_peALuAMnpU|p@f1CHI76%Pxm89ZeT^wPaP%b}!bEqu zD?jQ>&HZfqlVtaxkjBT^JD$t`6Rp*Rp~o^lFaw+AL0)PA4o2}lm;f+vu0XsdVkc!y zUBK1ujQGr=#~(Djk=1GKm~j}o=ANNPaz<*$a7(tr-iKXO1Ver@x)HLt5NOIm6GD1< zE!l65$`VKsz^qark_t|oi-a2nTRJF#Rb~4@`Vxoc=DxkUfhKB2m=c5x^S^@aBEHc1 z*kBhJ+=yP%?g`hjc5-LuMur?n#1y?D2CjHK!dWC3UXQN}jU!NermzU1AcqxYDKS%B9HT?)i`0hfd4(w?+}sS|{zL2Ef>+lOsIHz^qFW-`2m;d+ z_C3;_HIXm;&Z0=n3fM5S#5{`e!q(U>Kstfd^NgWv%cxikum^d!$Onb2#m%c*U7|5M zWx8iNQpXQPN-=ugLPdFCKG!(eZMwD4GC2VH_Z!FZz;dy>@rMytK#93Z@knfF)e14> z{#bLPGN#O=OzrYK`l~q7XKzVMk=Yqta5CS~i6zvs_B}ngs&k-*8x#mjQ+7PYw_;~J z=?MbB39_PTUULSWnH>te#DED~4|dNO*!pk$(rzgD!FgWbt5f+*I)3u~rO@ zH|R(Mr3odDjF9>Ib&Kq%NNWy+cqtAoYum8C5&BmaGUJH<@tm`3wa%E3zg<{_*0y9Q z@1eFr5lxu=2zU}(&12`jdF)c*u>ox15M_zWwJIt&zxujfZKRMk-L!XqbP6r`;(I6uVYDkb2hl7aZkptbRU1z;PVr56CddNo53T!TU8s(Tcy4vT8h@993FW_>j~ zaO;X?BHGKL0f(M|Afu+#nh}=Zj0AQCIn`fh2R9+Y)?Be*V#k#hy12|$T|N@X=xM%3 zkoJlhX(otx~Vwyq0EtNIn79mo`5O@g458jWX6(eERS`+q-egWcFP6U-YqH0 z#H@krf>$d-0i*U=24l$_&O9|pJ@RU4B-;W`N_PJS1ad2_lW?#=yH_b?&JR8Y%!wTB z5hCX;nXQ(|3R5geA)*CG5`ZV2R;Y=+A6Wj&>q$C7cN%@ifCKF*D3r4ww<0Oj`U`PX z2B`fn#kK>c-BwA77Nky!w=ExJ_yeIuq4Imv=>oX@>KJ`wEZeqoxM4}c(SYKdH6zV~ z&1x}A)9t{m<#t0ze^n(W6|%he#3KR6ixMOMzlnh6wI#kK*^OVFd=yTZN`~3~OvuIg zz_d1;4?H}*oyO-}X#kcxSf)6^J!EazLIF<3)UmN?(otP43W z_Txg3V5Iec9(ASr1>#1JxEjQcsRR*wh%NbEVA)*{1S5N{EQrN3$1C9E`a;Z~1jA4b zB>3#$CfX<14p4RMLI!I{>BnD2V2bu-VUS)oHO8-+t>5av7GBT`03kitJ2{?LRRA&- z{qBt3R7%)CH%(?EF5<=%Tw=_A*wQ@+e9H|qMlbzG<>5pj*yjZ@f5t*VP+(G!uGJUzM|!&N0>MhFn!7U|z>ADUzHQMrkI2#kv>3q|pX4@H@C}oq|k{ zeWu;r!`%5-7?f0Ma|@(}W32RcvCsgfMGSM=4(R+3I4xtGoOw>TuVUinm4^Jr$*93a z4#6&8AGhGfycJ;THLzRU9NKJJ#qQlrW3mFZaOgX*B4f7X1LBRWoK{J}er$&Y*~Je+ zPdet94^|=F@GXnh4=hz+Q0j123*2_5ucRH0g-z&ijyBeS6F*0~j!RFzidCk@zK;oV zU}{Mp#wHKBsuRJ9Kt_(EWQrZ?rx=J?k`!u;FR3M7`BmM%)vy?3Wbkc#Ei5;SAuxd; zK$KU-Ncl6io_h;}ypWNgI=JHbiO!hJC|5SS+>F5#XlU__AVQX_Ep$)no)PQJJzb#X zrDT$|Lti^rBrCs}QPU9@XE4YL)=*P{G=oDTV9@cKP>LlFh3WW4tqYUDI3v223pRS+ zx&+U^x8g!eYHN_51({qZ-`3$lKFDK0pp}+S^a3%GLc*ZF7RYi3A?ahR)|+EFbh-o# zwbBDXc$QOVevKxeMAIZE7^bN zIvBR(k`{}^QSIh!Sz3gBbuWw0<09Z#;kEA*k#asT_7hbfGH&JVdd$+BJa@R=Db}V#D{l z&S~A!IVB>f!6ntw8)$`jWkJRWSjp2Sbg$))L+Tp}0Lq5pZa8IJS&;gGt5L+PmlROD z)17sb!~amO7R}Xi_hh@snyXM4l48PCii`8XAl>;083KZlmo?*o3a+wR+`X-P@tNEm z?Zw^A_z3jy36EAWftyImBW~6aia%~+5W~_hXd>rjFuT*)F9%s1qjs^xgkR3HuEUeq^@2)_z-IXnaDB$)cGQ{wR zlr@Nj+F$Cn(crtsp-G);cWzrHrH7(K_#w8e5*Us#A%jDA+O#tkpbxg1h$wb}nsR1scE;H|-IXwe z$QFPkd&y9QYMcm+&}^=IZKo^VoIGO-)dfFwv)n2tj{-Z&oTKXj=zGc$#K@A_@8>qw zJME!Ze+kvt9cWB5F}6LuSujobv@vkuOA=^7n|({!h!y;<=O;J@IOk6#+qjt63c*T` z0p_+19e!epTMUPL{Wt8t>FHhLkEZ)C@&@^rukGFY=6I07OcNQzpKk8zw|#WwQH0N~ zNB!zkJ192>E=_7?eM4+ZicKy{gqv@KmgY7YL22pWS3tA}fM#b|&AGI0>9ER}b>I{4@UoFsPQ>jqq4 zKBht~Q+%Bkx+T+&r`|g)aZA_f!fBs?PkE1I%j`9^jmYaIXH^vdFl2Jn??xk2TlhH0n{}x^JOm^WZ+POjx(tyd(_B^tWgrKX*0FT%sIpe}G1 z>Bk#%iN)V2&cC#kMb6Ami^)`w3HS9Dr(w-<$eoBHmVxkjomr+*wjHf*%M5MC83lV{ z^&x`}YP{^^bc_)k>8r9CMje8fMQ%P!TEeq|p>%HTW9g;j*WYO`hfyL9r zd1?nxNZ1UIIk?MvAl5@_`#KKY#zo*{Oj|w$@6IUiP8Tp8l)BBtwg1hcOY;L19(184 z;JiOtNCR=;u<^y>gMd}rsz|hwyrwxUz!wW8vk>u%wE5}+w7w*Wva&`Xw_cr!`ny~7c584DDp>EzI}C~ zEP1G}D8o%}b!c~<|Kq06ocj?!_9Zv=c=QUhbpGwF|6|&~xu!9`?T_*_p+p|WTjHsk zGjeBR-Jj08{P^hk)GAVzh|GS7dD)6JTcp3gagL>e{zD$lyxfBvALFWC_P;atBmm@1 z|9X5piKM$t0B~&6!Gf=MU~^CKU9zTxwt|ko z8^B=NkIm|9tAgN^3pM#iRKiYhKLxU#9V*_KU&xyfi|7>hr_4^ZoL3*`HbPckoGQcm zFdWvpe%@3ay4LRg&_vnaa37i4j)Hcd5ET*-`CW6t#28p+cB0$lie*0iB?PNE_5{wz5;+X612JFfYcnWEf}f$Q*aI zfnh|Tb5#VUH`_?`1!qtF=i!nGr1nZd$QG7|;}o4mxyI3zvHJp}GFq_FV`- z@$X;vKz%JVk(lMK`V-tZmKIpDz9f>8tZrx9qOxuBsnbs1(}$TkXOWAAO6<3j886&6 z)t*(cHzl)pKxeb$;UVe8^{~?g+V2*15n%ik5Vf?VO!I|yvm)>WGps)m#E?PX2hQby592%9^`KYWO`Qw96|CKlY7GB>|L~HC0^>7TX#@7$N zgq>;5@}%9GvZ^a_=HN%F^^w|=3BM}TbCLVqBsze%_KmiKgM0c|>el|s1EXg;7o=nJe5Da$y2pc>^CQJp zi;7<|G;^e5z2w7Deo1!1{yGy4!MUiB-rM(Hzy;(rH$BZ)I71kH_ej6-#B78S?r--Y zae#~QS~8`^9ow?V>wOhT@#w$ql&~oBikwQ0fnhdtL#6$uxZQoX{GOv0iY#sUm=<-% z6H#xB-PA22W6ZitjAE~ILRvMDJ?9;lLCj~r&bwm>fJFG}7eJo2OCz?^7omcbx8V3x z#$g1sR1pS@;7Z0hp|IkE`kMIAdZ0e}wGP%Y`^=d~07W20ZfOp+@1J(ojYxPzv8&B_ zXYNK1fG4r!TeW!Hbb8>AK00}58(5CAknSo(51M#5wOq&#JF_p&*$ni_FwiF=;)rUU z2GoA6*m-d930Umv^9trBvxgWgM#hJa*QRiF=)y#{}EqXKcr~^fVEaW6h&E^XCDkP7osaw^wu_ZnOPGq~5 z*3^2&#m|-}XaAfE2>-37jy<5F{_$XK$y9i=h79qN=T1+! z3)Aiy1CcJFUq6h1#Dx3xtLq^{*y;9z!u>`RFZq1q^ltT2;E-qQQAnp>@0P3^uUQdc zihmW148@%OU2@Q_-ul`)V@KKO`Zd9LF>XTy4BO6#Y@uc<_Sp90pk}O>3FxV|?=4d1 z*iUTh5=*3*mxEwdfjY(Nhlb#Iu!oS1DYPVsZ2WO4fi4|t$~~^0i#oBKJ!92w%^?HE zoz{ZvPYqDB?{8E3l4?faFiJp*^jyOqboCHs15JMntzHtlnFteW3JnN9WsLU9087uj znYSY-!oD|RY7`XdU3%yj00a*87K^E{m(>IqXEheDc}=k*>SOmu1ov&Rk; znh;`OI=qr+KO)xSLPt$o#Yf!|fA~+9?!FzK_rRi6n+1+iKB`d}aw3;44_9pfN_XEb z)-2h3cg23Pw|jrE6M6bLGFVX#_H~)dDgKp!az9wJw{>c;&gkUG`1qPa^lRO52Kshy zM097}!HShhJ30Zz(+W!u@t!jcoLUZ-%yH~)MH%=%{vfaf{s3&l*LDsX2#~D3cqx`P z?H3EFf=P-W8{x5?VF7k;71uVpU zM-gUZ7HzW&Z!o@%l?^nErP_ZbA8(LW6|jwzSI0OHmd3ZOcz)dWLn`~DBDC{KyDq8* z%UJ+0&2bwB4Q4kFMS~qnSW=vQe;vj^tGF2!j>el%z~G*o;#bKZ1M34Wpm9aRL3=YJ zK!Q~xb*FF+-uWbA^kjVp%LccqHk+R~UjD;1->obzfoc8T=i%AEoj=JX1D*~Lg`0v|#p;y5Frxmb>ju6%DR}vg^g4hQuUdX6V4!4+M+9j?? z)MA2ha9l`z%MANw6d3p#Ps7Q8nE>jhRJJD^jct~jCcM0TK_99nfI-thT0UP04yX4R zkr-&Fm}Xh$qvwQwgu?|FZ!pX$$!8N;5eYl%&R%4Evs$bQzH~&&_Tcj?s^K(U@7%hV zh7e@-_ijm7lLHd&aIkYF$L;WS8THl&R$frtX$-o|oay%Ejn< zzU0$|EY=u@fl9L1Se$m})Ea9gWD*{JTrb?VD0V-lVXxr2Fw|Q;x(@=?<|n^+xEdbA z+y0H5nxEiHeDzSDVfZ>TtMyH~)a&>@{h}Uj@3Oe%0Hz+43d#J6DcYgcNt__;@e7I)AD_AfO=)9>tFdD? zGv@li*}7NtRGA`xm=S~HnSPP|6-T)SFe<+{V0nxcbcvJ)a=Fm=eJp1B^h(c{a&vlY zcC;65$)O5>br~w)Dl+@QD;ZM_J$DwEZ))hUSo5&Uur?ZG{;1Q>@Pn^P zB)qNrn72B^bALte-&3VD@sj9o7*tpN?}Pao36Ll9^)_)JYwYJjYZc?wQTq=Ap>CQ8 zoid!ymDvTpFB1h#Z;stbDH^6(3kvU{sE8x-JP8mI*#Q2Ccds~#EJ4l{#-#5-lik0ud(HF^Xk0*v~wNJQXo?H&p00P@+C%GXV|%| zAJYYouH!%~#I72tsB^~FIY>7cnXN8|1@&Qs2B=j9{U(zIag4O#Q;ykTaE3uQWkkpK z+2G+2{Yu|gZ*+sTVkzFeoOfTkp*`N2g}~uYdChP3J%La6Td_0AzC8>kch&9Hy`nbS zkm9v+_(|m~Pfv7~lJu!<^&pvKAl=k^66Q zs?AuL{e_8W)UU9%lcqo^SYn3d?vs)-T~Kzp{ZNyf=ke3<{2LP^*TQW>YhKl6Lc~9J z^s70iWR=pvTZ>R%56OduE5j2iCNZ*EYZw1}(<>6>Gn#TrM%J<)SB!ZEQ>VSAUeDKq zX4}5UO4Ptasg2NoUKsq&UoWUum+p(U8MM_HT6)O5~zZ*8?1Rnx8+^u8=jie>e~qqKgaAY{pTgWrsyp$Q97?h!8T}P*1l4< zi`OBr6U1n~ivNjA6nO=|WV;@f@p}LEd)+TOd*^=^Z0BQNsa^moY*_aZxu!n*ZR($EU@9 zAuF5Ps$(g=dmo@TFDbG4b!312+0#{b#XGkYKZ+WH{lQI7&1l@P>bap4@qyrb@zA-z z$CN-pjn7SW#CWV%RLYg`xL^CsE*X0i!If`#d{_o8a`K?!Eno|?uh4tglR~*(94>y_ zrR)urde!zO{*QULModHdVdO%$Aa#lkZeY8JT3EB2fj!$CIkk)Vko1P4OP}0}$UoF| z5#y3)DyQcD#Mf)`E2zstC@ zD!15o?|j=o#+Y4x2hk^r_b5m5bwK4}Z3r%bqX{7c{Szu4Q-;iE^Q)I9@rde(ZFeD+PqM(CJ{X;)7%@XN6M zElQ*5#EJPjqXG_5O_zPlFInj;GRSK%HXJ@m^A4!eOU#dRK6%OxJDA5g`36k>9+>b+ zcZS4!wH~yj1e(zITMvuf{|B5u1fZBA=QvYcDOn=r2K> zUdK5fyjn0Z@kqP5ApJi?(o7Vn-HL`fza#1&DjIhwbmlp@mX=lL>oiWE zzE}C9jY@Mn&Y{-W%Tba?-kzO(wdKpd!|Wfy)w;R4S!?|n!=e3$4m)2aYb0O$B{w}Y z<5m7|B8r`R%=Vvm_FBen^xHq>)z4vl5-Hiak;Eg(P65oXn-%#1Nob$FeAI9TT}b2m zKDU%%*t-oBzcSE^#HEB$pESpmJla0#0)q|n4!(Xat=N6zwuJjZ8x2t}eO?DaMqu)3 zOh!c)7iA`7D2tp2KF&3vJ(9EcNf{46-+jyf8&0Gc`OHmJ}f(jom8rRIqCb(m{&a?99kEZStCT6AL%z+ch{2?H2h2RBSbw!f4Zq z%kJ+5&fIDyd09GNOZvAfiOI?zY^AUvFxx5{rom3bC(GbtLeG0B4H;2hq^4|+`mQH`1J@wV|QohM3 z4AG!|s`Wf`zEFv;v7F!7d64K%yZme<7_)iqy+>u4W}Gm$K`!G!-k!w6Mgb*pK#B5`%zaU+e~x*){i5MYO`_a(#gvN7c&hhWE?`D_ zv68OAd$5S#7Y~7Mg$FG7xVP-{(YuQVJJCN>xNG4?B}AtMEt{Mr`LF_ zF^IH1YqzBC-5T$$(bgOD#1jKdJ`-EK?j9(_3)wxh+~<}(_x0OpJ*4EU061;7P(l8? zSGoB9)&>W@>t2&%P|Nsh+lQU^bQNmX6u|W80j_wG*dZl8w0buSqg@dWLg{~`ICS9q zN*O}V&^%uKY=N}04O&xreAZypg#hbPxR_*rrcscUuur+~K`rR4N=)yc&xc1OC>T@u z2rF6()y6O40W$otgyR>O zYP=D4?0z?<-2tgbTQ3P24S*p3cDDtLDE55P7;HB0VSaRIKze2Udk>7Iyn0guV-P8s z|3WNNI=IIp0nG%%7jFs!BfxX~)kC^b)SS=d(jg;ZS=eCc{DEP=jf=KsztuGlr4PX? z<8jDa-x<+U>Se)CHGHIS;vZVi?3Cf^enDs)-rTGDD*mgKewJwm$vsaw4IuPt)REn| zw}WJ=43^e9Mb6h>bZ%JCn4i&ihc3VMjqfrTK^Ujs5&>;mDQKt zK1no*9eeywy7t?6b;XC8sT1U?zM3;?(L0E9PEy#O!6E9+Tnin_oX)mr{NE3c=53=( zXCH11jmEpBfgA7TD z>2CVGMY8uQ3`y?kjm>dG?~^^T=6&_07}Kz<4&6igWgcfJe|ACEC=#7_flp38^HNGq zF&^yaV{Qan#(U`kd1TRW(w!CZO12br`9DG6ztnb&HU^pPi9s}5RffZc0IiaMT9*@o zysT94KAjvFW7at^_2HC#W*-xToIUgc#?NI*mB(%HT4KQdlqYrx_ncetw4VSG9cb0N zSiwtu#*o|4(waxxI2fYHKDR5zthye>$PI0~`CNbQNe24&u`PhU{yZnG*D)=cS1RrR zK-lJuxU3$?7Th)g2291iZ&XanPe!E=RNmj86ZlV~@_(4l3BO~X-j;v*Pk?`eU3qr5;-MbV<)J9vc4!y8?$^idN-Bz<;eS32lyZvo zL30T6-><6myoWt(78u`id=oU69Q|gXre2EkR}~=9K@rG<-^=BINaVVaKKZreNmOH13GrS)%5`LPd|DrUfOnGuhC3w|K^TCb@9bNtErt?;*a}E{sTks~DeHW@~qV=gy zGUa7XTs`R7K-)0fChtAtBCUEG!J#XC)2hv8p>gkbx>wg1g`cG_u*ZsrPYNeB1}rFE zJ5E2KM-{0rt)opM03j$UiD_96J(L%2AqW}=Uonto1>EC=E%^aP>bll7wRcqU*;Npn4)*7ZC}zU zL%XrAtwnL*XM~fr3u)D(;C_0CGa)rqSxsUW zJ+*EaJ&MrHz7Yyi;k<&IX<$#&(trcU6GKz5xo0mwc;St`&)yBx+9(ME+a`^6Uq&kf z(7I@+gslqTUHMS)RG_Jw2@_QAlRW(_Yjx#{<3N?*aH8CnJm0>`uEGHI3>9UB_*7Ip zg1IukvU#mGc(ev}IzRBM!k5a~SnMaBULdruLO6SM)UFW4rg2Ua};X-;JT3(~1(-z$kRywRhEoi&rBTVg0f41V^>0NQrqUB-8kMgHZ^TtLc4tHiJ=N9Jpuvd*r}p zYjfhh!^C;R)49O3?R~wuMYB2ihI-LJ+L4R~o!I2svyfGr^=DCA@Ln2-wcf!Hf+ab* z_oZPwJrU=io-=lt;Ma*%ePNuxdUvsOkH$E1Q>`}zD@-5eoJiE*JF+n?2%+|(*`aav z9mZnlW#;V=#^5r$H-2w~%~&btMZKuW_x9)Kmo|=_NFfgW4=`#D3z5wD6D{G3mc9XA z`YZQD-cJNA(rh*DO2g2QXY)yo&kvf4l7bJcV~`58zq^Fz+>Y-WzmT1Mymo7%!LQ?< zks;&9A4b;ve;IsX^Y;kM|FKqyYT@1&=kdDm+mH8TT#gxt55G_Eu_qX>e;ekrc0MB) zNpA=hcS#mYFMFSgJv#hZ6{q=p)Gvx#UiS4o@%vHDB#ZvT(W#ZCe;}c7qi@U4zt{O3 zKY2y3O>vyOqVQzT{mRx4tj)~R6z5XP>tTBvV!l0g%*5>|uWLa8~)YXUUc>Lhk4qfJn zWpfX$ZwN2hx9MZVh8*JdeN}IGOR=-E8hEJBXY-jmGnN*-8YGpMcvb4t0qewhQcheRmgS?5t6?yA?$Gz(>zM23#-j-B&30;fp8 zAGk^hkzOt077`;feDCQYW~X}SzPi_9@23^#B4k2NiT(!|SU4<$dm8TkiTYODUT-B` zS!wEu_jmT~6Xhx+b1U9`IinAmcmAxNS?K#u%eql9`AE5XiYxYDP4H+yTj7UV`^ddd zyVrNrcZu>Uh}q44eXZ0N#gIz%rpd^oc%cvS9XW+IZDEx7+-lr*pS zH@QmrwiHV)S=e)p2fOW+TV>B$+RaJ%5Fao&F>Ce5!c(wv77sP>Qw%!ySpT+LJ*LB2 zp=47NU>VZ?dJ;!fcPr-mk@*zDx&dMb!86rD;l$hhyQEibjD>)wX^3L=Y80~zB6y3H zq#?ip^oP1*oYrF+iB0w_zsj+Z_bU%8eJJ0&gC14#T6A&W`8R-lnU}fWMNQO)@=~M0 zJ8u4m=XQ()UC&umQr^B$e%!{sZB?@TT&ea&o=Z*0{-?$rZ&m6U;P2p7v$n?7dY#k_ zASKlYXQMhSMnKV12Y`CX^C7vw{3R>A2v)-w z)@!|y-;WC?oBTXwA_}(+rw4d9>a;OyTA(_PbXn=^YMko;>`a&xfCDx{+>!$UylluZ zl!j=Ah%?`LMJhKX1Ia+!@ckwEvRY_Il{#fL=?1MxoT1mvk)W=ZNH%4eYZi#G&UMh9 zu-s~uF}MBH9($`nhT+FkLMM5|fxkTyGJwUP^~}ypl>jPhXCx!S;`%eyMpn=@UeilG zQg-gYQDS|NrbE2256k?Q?=o3-VF9L!>44=c7yuIu7RUt2mQ1gsZQocMUtLyz5L+beT(YttpN#8l|tB35fZVskH9wPzd%9$<~e5FPnY`s$~Jvge6y}*cG=N zI$XfWD+bEPswG)xP0cPXakkv<06)6AP7I9M6MG;#6`mWN&x2Juy#!4qyA-3>OFoj$ z+BRr^eJ1tKV4C4)HwYcB*T>o{FXD<_d|-yY7DT)9-zw^N#^fj-kMPKApqk!9XlJ@y z*&;Vj)VbqZ-)Ht$>(IV7YA(^{Vq@?ZRn0^jt6s-5@MAUa{3*0#p4H8}k;MS?{lN!T zw-293xjTplz?tKiiH*O-S}$61dHREJxcGDkgunKgnFMGV`zHuHX-ocQmCU!C+hWhm z#{lf$!kPOwWO&ggqH}fk=+3#X11C`H7g=da1-etXChusVUX)y}_Hq42eb29YHs!BJ zq69X_vG5{y38IkTj6pPd99qxkmbukdD76(s2a3&aA)wEldX8bO-eq(ZZu{zrg(jbE zh3Uq6yry;qxCf*ek-#gK8599{%^nE#0^qgsCo4h5(}#JO>!cHwTK=U<`^OQ&F0EI_ z!&zTut=}4`roKmO=x}uL`l)Efj+)k`d=*(hA2i;uv`yKWhQ%m-O9gio6}eBb9>>G= z09XxYuxGR43Aw7fC@yr&v}&*Qd_jFp~NKM|Y>UG+FW8=7V#KtUJ)xN$opck~N#&q;ftZ^&%+ zR&cq|Rc5@teNWqn+VYH7di~Ajj~^A#BeJ~rqgzzBx?8IIyO_57)Xd&U2f7XFHAW)C zSer_QKd0dn9r2fTe(bl@f7cf2=6R^QGFI!ud;FDPol6Ogs~*zHEA5y}g*Zd~#=wDs zt3F~c&YhIAo|B2Hf?~gV^8n?rj_Nxo402gg`YFd150pO330IbSRsI{AFJcbA3jdvH zD_khmY(I2pXs1!jklNYQV=*(yR$EsBPghl+Dl=5eq<(y6QUTc(l@08BIM)(F3U`6+S~Yf-zX1vv(`oz)-^(ogZ&HhR$qQMmp*YqPMg zZ2(s8GT>MPsfLdRarmBdzk0|5JIsp$e{B$FOksY#;@h5uy7q6>$ysn`(Q>D-d z%=mA)-eNEyWT(RBG<5*>2!BYAbZCh2h&mtg)wA zQRftf9#_UV5x*)kk1}+qZop%34#Er;C~WHoyFdgZT!mrZuyd5e|{Yc69_>GDh0; z%5zfy^S%*bTL*i?4}kRc)v4TdB@6jnI8p91blrN*KYN(q%&8ehnG;hM&)Fljt<*dK z{oeYJpaT&1Suozfr>R4yDV zAD-#5I$)D+1-I*Pi;1gm5oEWHN!)bH2ab%8Wdu6qTZn#do#=OQgJy)mrN1nd$mL2Z zRd#_nO5$A4*>$@}Qaw5>D#LWIaB?_mX(5OCz}T;&m``> zPMn%8(*FB;Ex1GYSG1RDT`N7+oJK?PK#vx)Q0(y=`XTc2%r(WPnUB8FexGgLK+m%@mLlqgvv@N zTTIg2fkY^ZN+;l~M7Z$;|EKFT*%j6I!n z&oi_qofQ#T?b9tv&_Ar}Jl;Q-a2%^j)KaCZCt;W2&hjclP*n&OVAR*ZBxK0pFICu6 zSlw`w_i>6vCRvsuhy7-B5{43Zy6!{+Rh{nQz(nC?KW*DK5DAsZ6d9Z z8Hpl5SVIl--ZR@)>zBR>7k4 zy*}7x{>f7#Bx7M^w|yoAm}!UKY>}L_^dc!+4ATKg56hc?9D(G@;6+L)F;z=R32Oz6 zg9YwMcpFnNA1}ZllIX*e>$aQ6;TeU6CGP_L2z|??rZ%SD(=tnCwZJd4QV-sRx07Ms zNCv&X5OXXnYb^MJ)>xEynHz~dUCzji#MNQkgnvlnQaA~0$TH*gR+i!NA01H>P^&5F zXmq8mg$?LdU<*Dbxd-|cM+X3lpZ8HfS28|R$PqsrXkI^k`7Zn4yQk2^^;^v9H#|2RGqRtBv(I|$5 zXOePP47%LOgDeMmuYH*tm~S27lADAlXN6((jowVSIA)+L7MjUhu;W8lShBYbc$D!` z6a^^T{(apC;Uk=A_I_Z;7>+2HBYzbuvTZ^Q36HON_P!LUs&P}YW-?&pTdHrCN>A%L z%W2!!{U&xiv8l$<{545cDv?5__(1W!(Nd&?=nn~^Siz6I#z*>1 zMB6L%kE(F8T#R?EJQbpGLr{YWgfsVTrDP|M7@9^;Ryh8pwYjf+>_^b@e(vHKl?My+ zq>aKo(DJ>X=*=24T@pl?p{q>%r0|7g@*t@GpBi!2e?a#VKfTxm-rB-wILlDKF{iXcg7+laG4PiH*Y^c*S^BO8d-{yhy&G<|FU2o8IGaO$&* z_=%-^(taYtGejDlS1?_~9c1v`PR+jfD4_T>hY50ev)xrLinB@ln<@Ks^k$$fcCe|^ zha>Es{&|#xwo@X?*v1#dg8f3lfCfvQ+`?++`6Lu2LZy4)2RAo0EUc@M8IId+^fWOW zQoA^%fk6d2k-17d20*qquJsnF|kA&L!-lD?ip6RaChQ1xZ8o3oqcpfE9Q2 zzctDaPTbHP?F&&xkT0?f)x4|1CqMZ`2wWF6I?+7FItW-1ChO z-u^JjD(|s}nVzsRsG}vTjMDF2HC~*5&~^hWI}1)IthE?_&k~V-C8rR+vFs>OPsHQXd&By*NQP^+0J7bN%%ZZD*3fxi~ z$zb*df_%to0TGaUEZS8KjJ@~IQE~+6H}`a_pJBfFE`A*j&DT`b)pXr~Jh;-+t2&YY zjx3T>6J2QFLKu1-qebE+_jP9?(Injxk`M$748+9_2LGzd=M6&H9v1FMjnv3Gu!o@VUoosBn%eIj zzF?UQ!*+EAv8JU_1kL$MleS1Zn;@QH;xuBT(M6*D&Z7OyQ}Kq#N80bhMC!8~r`)Q8 zh{*8twtryF*`EhF363O))oje58!45llGKiq*;L0@-cE#HY1M^(J)`N*g+SwfH_UMI zFzLy@X70r07=k&bEnrY=%PT=OefHniAA4pg!oXPNzaNzb#3}2u;r2rFES}u*k80+^ zJ||C%zdQK-s@|Bs`|Z{lm1FJi;Wk0=I+M14`o|h6fzKTN(a(G|D^}BJM=`E5TbH2(0#;1c%Mf zr{7>|NQ3 zO;yY_Q9d@0WnJzs{@ARd=J5ep`;-u~NgJB#Dh~;gH0YsPJQl*dDa5kiS4MF%v# zqG9VH)ztZyay+Wt@++wFhUJZv{$nzCUKI}nb4sf_lNxdAJ9WrTo_qxhmLO zVO_$-m*KeC3KjRL2?vaoDYOb`4K+DZ*~-antKR=WjvP>1H?NftB4%mEZ`@_r!pcRh z>n((rAFDLg^3xqBFOgd%{>Aar6+aeimFoF>2`z#L?;7h}{&66<=%xJ^!YtlmwxaNj zrF#^bjpMUZ-}2pU*&%@J8OJ{aL6D!NA@j9;>UEeJ%ucWM{+n`=C zH;hmGuMq0k^!NpZs^odeV=7+MGHG9P7#WpFMo)9?Y9a0CP-l)8(ut#2(BvTf3;vv& z7lK+LapWsEtwF5B6@(Wt2f79aBJ^crTDL;rt5M+rx$PmYpIjSyx}(PQyB>WIh6+}m z9U{S?&V&~Ahm66&j_PeA81a|$_xifuEzG7ojaeum%cGcxVLe1$jS))$*clo@4v(c4 zPVsQumYgHrDmHFL)-YH0xL+*)oje7Fdw>b@aw;;yG2dJsz{e&n2;DjYwuPiXo3HWe z;Mn(8>OI_KKx17`P#9R*C=ym1Lp}5WE9A>5v7*r%sVS?mo@9ZEhP-K@E2D$nw&Jd! z^HLHDI%+trr({ae!U(pko5V&=u2hxLU6D!Kaptvv@WK`8{Eu-{5@5e@c}m$fuap~K zZf27!yFB;RsBvCgY^??zE7Yan!dZ$g=KY?op`~y7^6^?JFKFJD9R>BzzIfTQy4K%o zL2)|kxcD&6H<8(+owG4hrt^N-3+BnO>5JF(2DT$Xfly`JzbXkV+`U$T9+u=WQ3JcY zc?9#MbF~Lsy>V*~_RaKhwTH_eHVPlGg@0g4l>~ty27mth3@Cfv7{tbUIs+XEg`19zy`*077J9J?FeR@ zsYklaFuNIpkb&5+xG;B*jIvgFAe8#HR;i)U)2U3C%IS~vS5_m!Qxn-k^E<{vs|v3& zCD#ttc>f*^{FXVy?pL>V^PFrkEZ2zN3NT+P7E_DaDax z=2R*XM5!5`c+Tv#$J?~WaU2_!7;(@Qi+L6WU)%UuK{Bh@S7K0ZgcrA1(4203k>kM< z>$=0A7Y?{~8B-uw!Hc?Q>Duqven*|{8#CABxUKBI*xL~M?VR6n)Z%3puoS&3t4mla zYx{fCIV^bGN@eBw{gK=SQG@aeiWmLy6f!Iw4H~<~I<)RWXq1W1A{z6MmaeS~`q@D? z^0X*(m>A4iGW^&0xJQXLVup}HvfxMJeg+~Qu&GeNfpt7}hRe33n#ZQ)XBCZ{HLt^!I@s~*5aW!OD&>SC`Ox`Mtf)UGb3r(X`YbWIl@ z6-R?@oN}Yivw?Bt|5X@${UKq+s5R>n>0}FVY_s0}#%J{-@L+v>&6*W=n#SvX5g~R0 z0x@H;d9J0)7HxSsyIrC$LTAMaHOhy5r$aK=8b+Q zz!(cP-UH_%F0#-KM;w~|thDi1{82vbeOmT88$TkP?mvk)md!mS{^>9tdpJHlDOJ|NqFFW=FG+W2 z6hm6SjlRfRsJHVjLJP5>j--Q~IQQoIeR`GAu%A!Z>sIws`?jYlY9xc2*Sua4`01qN zm3xdKc@`2!N98SIGoO-nVw}x&n82a1l%y`N-h86oahmy?&Ur&h=C!lDMKz8P8fr!i z2x5I_TO;!%5Cw2>;+m~95HMS$%RFooNezT zt%ZjrHYS^d-lBZow-*^EE^zt?1xH}d+ ze5CGpLPukqNN4GK@JL5vrh*~~Ro|=}pBcY2UA%9E+N((I9w?K+yJMi03 z-pd2H$J_C-o>>h6H=%(pNwAok_J!q&`k{BJ8zWvaBwoVy#>r4ErmNU<(Kqe*QpFiA!HUD0cW4(p>4;Z^~4NbN44)cTjQx z`_}Bx`j~}tsMI$QwR(Sfw98}dA0!^+Ye@^$S;FstJ|KHsniM03DnzB!YuIV8YV3qGXuvG7qu zSM8TESnd>MhqsTIb^A=ayOSIK;_7!6fN#I{q{g6m>N$Lh`ua~r^FLZi8~Dlr-b6g3 z_!F`UkbB_7pQJY1I*diEqFTP@4_gp~LHW5P`AGiruML3jrYn4Z3?Z%rtYvdh0|b9k zeI$i8Z~@oGP7Vz;@D0>h7NM5uHVh!yaL zS9BoS9=4xhv|ciXzeVzyjb4T^)t_OunX-%i^|p97T3<#aA6;EdK?|kgroiiG8%<1{ zxK(bW>+sno47=g&GRH1wQzoM!8PtO{L37XB#neQR;GGWNuQ4CNIXo$xWoxoK6HVlJ zjgRj`umv+^zQzBvaQ@rHx!x73SlawjW_y7TOs2LuuR9)X&@+fXjW}^VT~VOIXve0q zaB40GK&FstD|*Y+_qe8NEI)S<3|{6f09(+L_Wa(o9j!`xfrhJ7lne7^4gYwROT&ak z&>gwb6_Q+8uO?i|1bowGVc=hcrwV2fTBE9d{Kjb~b|PFbrJ8~q zR~vVoskfcMLhF^*=ay%_RwMUp!cH6wUI8jZn9RTeKK21cZW{Z=6%UW zC^&#%F`6-^MYTxdv^T)QQwHy9jL_PvTfn2ehls#&7iE}oj09iAB5MOej5*%E-jo#M zrni8E#%VvIp>*rPC|4g$pAA_cK)Dd+NzW(sWZ@T+eSJy@k8LPxa3mx+n9r<7iF2%% zuG_oOi;o$!+TwFXUCM@LM2FP;lNKv7O%d8f2WFf+CpRTyuRDZH6}&$qeQKhQbQiMv zl;_w9`>S!@?VDkel+8vYCz8C_c>7$Hkb5q?S8U9DM$NSz#PIX~%89-T?I(=6-Q)X7 z!F*wdgj|H)fWl5}BZTd%&GUzq{acHvt5)1t+sBj)sn(dO0ztmvQ*D!3?3R?Bb5e-K z`N|JjslSYFx@0AUAD63ENSkA|8+0l)>~O zC5iBn#ESa{vvYAxsp>dvHS0&05oMDkGAc!mtBFN@rOXcrnvS2cF4Pq?KfY-HG^^Xn z{eCc%gqAREr&C7TC~3Z4(l8<45IvpB@m#aJUT+Ld(o5TYqy6h{_`u}*S_(DgGusAP zX!woH*j+d0cK~TjMRUGUeDl5`P?y}4u1_OBt7KV%;3hQHdy$l%E5>=vbBL`ysq4t= zYN}{#v1xjUMf;esiE5wq!ZkF0+gGsDPr6MlxjwG`x?XH} z=|y5uo7A1p5UiLkC=6J@JRMo;uYCm0btkuLrI3OcGA5s#6wR&60<4RH3oBIMg!A+o zvm8$wfEgi2iXgm)Uez0-lH_=O5?{o_Pnd9bHYl%?g6gn%X#jNGMs}hwoFe%aHBqFq zC_T2j8sYUKjj4x7(rW~xP0xyV8(=uPeicmq3X|3w@KsnXAcK{X6Z6tt&CBF_N`-MB z<{k$b9)tk8>i#&MPX}w@i|{0Jn$5Ow;J^-jE3ITyXK(TL6(F$+y*eGoP0>wL2ucq5 zh(G3_Z9ge5>DV4LwgEQJw@+FuI6hUi)AgU7m-lKs7s|8KD zeTOfcoAw5x^fh1=8Im2AP2ZZIlN@iF` z@vxV<@71xSOv>ScuGybRbE3+JZiNbC-iBt5AkL1ivKPXA( zqEc6f2n1l+HMe`Mcv*WBuf|^$GA~Ai^N`|=XzFzxf!UA%{m~#8&YAz|3ze$kIp(wS zbyYNdDbXh;$B0$OSx?I1yleGgxzJ3p0{dkYr5Urcb^Kk*$4xicETzSW-^B16p6z^Amcgt)|zq0^STF|T`9#}l61>cj(M=z}JSG6V>{jXB`dlu237jF$|(0n%k4pZSl)^ z9$P~XmWZdc1qzqLr&9?K<0b)9Sb47Q)N_ve-wJG`BXYCG8u|EU?C&?ETpgN;{{M$< z6Sa?G_g&!$G zdS^}*mqtWY-~1l`Sg}UFmDJPIPo}|YfU>?}`wMzOfd2VbGxRcRP`swYvfl%V?1$ zw(4c<@`sAA9yHdz_T5f1@Lug#ZY?oe91v36&W%%k)tie9GiQ)HOQPk|1cdj`dmg1T zVWA)T&5^J{59jkoKh?bG+Lj&?#Qs{Y`1mt__ZI!V?PJ?}1j-uv9oH&fZA&Ly;&Abq zbn9e^aaPF0{6h+}yx~}r%fs%pWrwBi|pRd+4qRbaRo86*A_@|jR(8F^DxDzSRA zjXCHE=XI)GpHM|cbk|Gaw@}Yc^d}V($fxGIIaMS5f$HFBji<}Sg)NAqLs)NO^FZ13 zkOVWbmtneKY!0_lI}j2%q`i#hZT?j4y*9}>${teI2MP@UZe&*^jPxZ`U+7_4@-^Klof}Pi87(zKuOKJ!d56)=7Ay zD~0ul(&M$CIRAYVs>0T23AB7lt8-gtE{sBUd7p_PmfcocR1gkw#dLU65Wk#O+X0kK z28i%_EWrvru|C2iRs6S|y#sLT<639dU{bZOmmZv<5E+{}GCwPs|mi3@e z#wj-UxFCL|f8>OrPCg=Xu&W8VK>xnJr(HCLtg(hkCyA2&^W5^g*lCg~U3 zTon7Gii|SjTt&stLr0h;GkgRRBba_jGX1pjc$=eUHzh&685C10Aaob()cYThBMU`& z>EG94vI*5@eQX@h_0-E}SN0A!Z8vso5w=;!-KNoj^;ZM06XVr1Ywj=fXdFX`?|k5l zeG`oXTU}7)&*sUIg3|r?YCyPHOHTAykBof_S%&LwcV8Xqi<{^ zz7E7sZ(oez1u-%1x<=B?l56(h>{PUhzf}Iy7S}Q_>g(EJyxqU6QDR$E{cGl^`5#bA zw~`O+=sDTo9`A=*!Uy4yqTH~^Yv`GCS{sSxzKjl~*~T3Q-2U9C zYTV^yJ5)^HPtvNVx#zkq{h;c&tT#jChaP<}4VIf$c=iDv&g=2K3DQ zn*tVHg`fbh8hoq_?Lex6nQ&-TRZ6;oYHN=yn99@p)+;4eg2H|!ZPdE98mBkK{{-QX z!#>Q#(z?aN%xq7YNv#`MxNB-=l|OLzO7e{8=q<(%KDg)T*0@~w^gTr40r*CzA@_C) zihH!f7B;4yKvFLtL!S&d2%9vV6tl-tDIHB@=^K^__Vg&FzKuk7*|eZLcIVV;5;a`@ zBz(z&t;m{+o-@Su>={2Srwk#7R@c_OX=eJgM>l!xVgISRqy5+7_SNKtZC&cU`K??K zu`RY`>C9{*GV1}(FlmD|ski)U8@0l?>enYasTbw+ogeYc3 zk#2cc5SbO=WhjDG62dcp92l7j*;nd5qyK>50L>USv-`r|tFKAu=1qQ}TI7wkC^vFm z-OfPf&H@@pb~w-aep>LX`2GKW6)Kzc1ho=5{a8x&g){1%i`zNaIvK!V@7F$|}v5Xd0lccq7D+KCdz*lxU z$erbu60s^=ev(nu50L6sfDftEG%KE|(Get=mGvnO`jlM{s~gmFwdf_{Z%UPmtc2v> zrY_6D>^$QSF5g3oIi`lQD0iPW)S&-<%`vvl=|Ds-7G8v}+#jLf|FG}FKOronhycIx z!C7*}EI-L<)-(QKe&^cocC3Y3%%>(w9TWwz#XKv}e+ZhM7gRi{P&iBox1LaJ`oVTT z^%5<Yf&Ycq~>4>}sWr=xT26{hi??w}XN00{tN;W$D`h#Ii0_GtMJR zPcC3Anm`_19Ya6DE}Ge(z^`72A_L~Ove5)}u-ezQTwvyu(bwqgP|alb-d7nmS?P(I z@fv+S+$OS{vR79|U)vydfn%kRuo7Xp0He>D4U!AHIxpJn&|r*u4B~JEXF|1^v&zK# za#-#b_S@DCn;%!diS#c$QJhu;-ITDm%AT+tyK^$Q`>~ACk#1Fv$Y<6SgA}jb zu}uio-1W{3QdxJEA_uxI54#eJ8Bw<7e9eDujA5GoNl}s`*v7f%)VSkvqOl~oKof)n z_BhP;E91XOoByezz|ZFM6_tk6r-M|a5a>wAlRsg|pXA%yw|pbbYK2m@@9IFjNnNW+ zeEqGgIE4e`g{@u{SK7V7ULUFAoUl!#R z`CGn{aTnDrTQ@w(Cm`D`U9q9LD6H%|f0^fdJGn;_;i!y%ovzzSJTC-9+L=$rit9c4 zaEb`2%foHt5eeh6~0*fus5_PcNi|q zvZ+2mY4+LCM08ogGax|E$j(p|Ch zl3goB5Q#Ilf*>!G!+~y!Ssh-3tQYRJFlsPN$e@JyGC_Xg7xxo*@q9sky)QsKvT~3zA6l0p3kzGG_)X}Nj8rN`>fAHgdQ2F5>Nc}vYEBue z4SNIIdeki#h~(R{?&10SPF96mk~vf8YRcR};3+F}bR#1Pi1qaRq&#A5+mU&4+%f&m z5Fj3JTcQ(7pASgbj!tF2XMXy+^%w*vWR7k!m-$!0PKFSf4=~R7pMTYM@ocI#?w}$6 z%~+pe0cF~!9zAj@{t6!jv<8%^UuOY%e$gcw9Dj#}b@FHq&OmD=PAuQXp~D@I)E`4G zem7^Rdi@M-^PD4-7pAkV1lOi1cXd>LH$pJjD+E>qe!BbY1=IzqyN}^Pw&~@ z(iBPTP~)MuuEb!06_uQkG2bs*B^qi?tOR#S{+CJ!p4FQ}Xpi{S<_G88Y7Iiqc}N=F zFkjSxGj}7|hvLO4GIhCz@`tM~7s2LQRTkf0e49ppNVfWXaD_W9hYn(8zjp*kI-tO9bQRbT9PNgxM>Gr;A-vPo{htCu05yrM z?O2{ET}3`Ovi2UUZB=1}DlqN*wn01mn!(%C8Xum2d~gr!S0iUfAqk1^tkHy&Ab<_Z z(aOuFWP`Lt(9Q8I3g|5m+Sa_nR9Yu=$9;wx0qS(}G3oLC2d<_aG zk!tH@^|0jp7TVV?qa)I*8#JZl7RYDar9*S1CKd#*sh-a8wXsz7MWdjBf1lIOI+>~D zk0zRE;LYb&foW$P8#~0uATIQG0+SQ(uhv8c6her>6`H#zTFD~Tn)let1E&0Pa8SQT zTgpXB^BG(G&TEm31<&NbnhpQgZiHsuD1O^D)S#w%QSl~30R!)8d9=x`(-xZVe^(xD zt@u=(te*LFHDJ7X1O5ULS(#^#aKa#5Fuk+OOxX5VtVf>$e;_`0)3AG4%Gy zbj9Cqe`4hgJJ!znlTRvCE))&}+~NZM)oSy2|HJ%(Z~A(w!OlNl_hB#&^+^20a>a83 z0QYY3=7ZR0H{SL9hFHeZI^mBqVKyQr6|JlnZfcpB{U9Y<7wLqrPgC$kj1#(l5%W?9 z85V>uId5s)!m58UG?RxFuVl&!?Tu0JVn%u?iFzMl5BU>0Zko(WmTul2WFmPyR`_1d z=cKr8@!^NY%aa*U-f)D`xXn8+)yUy9L4Rn&2>AvO~Pp-IAvPmb6 zbl1+VcUaA7H%^oBuuGLnnAN3H`ABgr6D)~kiI=^Kai?P|(kgx~Wi5Rw=foMfmZUe( zb%mIucl!^>KeOvL5J*K?y>j^PU-%!;Cjs$l$0n)Ek0<5wTQ7!nymoj~D$u&j8%0e# z9H&FpmI7R!=Eb|SrK{`}%qem4ndCw(s+7*-G)T&jKm()_0Lgj+v6YQ|Nm#jU00~d9 zn)uRjWw773i8#xJlM;*Qr7xq5tRpn=ODrJjQnaSWO$j;^4%vWdJfUSotj={kM+wN> z$b83#oR|HHvVr$vWGexd{?`#Ek!kEm^ctTZ8?~U}FO2hWUQa-BJPIo3F7o6buzwbu zeMaLtOtBX@QOdb46EKx`p}8tgUq?T`)g@+dXzo%+M4@Zhiat#y;2d2rPv|Ikxc`nk zf)ZfUeDM$0VDuWP@d?47jETA9xUf3+Ws_hwXR%Sdvv0*FDcYqJpURr-rv4eAnx}K^kb&*vTCe}3UF00 z#?MUc6GIS5W!n{0g{D-P*bwP>0`#Mdge^D>)gD4bzs}01s#!D~xeQQ{&=2`TKf1M( zAA@w%Hm%;3TU3`1V2%Ug=ch(|XES5F%8;r9UVnJIq*HSnW5V^-f+K(ZHM4@mikokL zRD4h!4h6+e`Wa0=t4^)a>RvpjtYM#}SmAj(CHik&5vyLPa5M~T7tC#+d1}6Nrv`^U zBQ;#nGW{_&C)o=rQW(p2qwZmWnbjMPgUIeu&E7oRL#|8_Ojc#B8@tLX>T=Fg>)-(( z=wi+~-Xt<>ggd#t21X~spvrYb3C#yKoh)psL@ydmKYjC@Z(7Vhv}>1*{L#|Oaclk~ z9^ZMVm`KA3QFBKV+PKK1?Pk9cA%(dwi(84EqWkB{0`78r%?IQ^XD$K+2Oe-_=97Zj z=s}^pxWYy-`!BOxj{r5IN^Q?p$%!;MikF1@0yHl2HJgw9UKWFvZRuz~`MV2Jh6c2? z$n=X1Z}AdtMxyNg1B%jjZTJVqs6y#BF``l3irwedhAaW+8%PrIWMpv3O95g(?>(Nj zRxfl5&z#-JBG+sL3m6s5lO<*0D#di`$oRY0%F#A6VmZk@ly@Y<^pV3mU!un42}jvd zv`aZtChP3z2&e-%uU=sU04e^R;=ETWtJ>-(QM|R_j8Rg*2Q3Be<)UMR+T8=-C5pS z=&UD^`)naX3_J14_k6?!5=5xSJ6RWdqfHnX-MAd;ZGy5AKNN)vZI_o$gw30Dg;L9! z=g-MjI_WHfeoM+i$3qC3m_-gAB@`|EXg+_yB33nVd^Is^^|@%KU#z8UsUbo+63{`5 zjZMG5+HtL5`u**PNA$=g?VO|d?7RkpMv&@`S9rJYEMeo&PvXaR$vB)<`*Ic?4Ey$x zKfz{^>q<+YI@V^=+Qj172Rn@8|Mh0Yh9|j3q3Q4+*370tgT5?QHESX4-vqkcNqTf) z#|yBv?hV@hx1)hm=}tV1+flwnYI;L;O`f5dAwjcn1+$E?=X|=UDTruUf{8J$(^K7@%xtZ{BNSTh_(xHqVc#mi8sT}&=0v^9>yOg=qLPp7V&?+(YWxTx#QsJW}$cC<@+ z8A~Jc0C!sU@Gp}&!6VS>pQz_I>blzX^*3cdB)bbU_y9Wb~U#6Y$2N|Z*7%;iI>N92(D^TpxRB~+w*!Tx>w(=2pr)*6)fzXEFVWSRLm|rf!H6@xNWCX)wFhuV`SG0{ME zw| z)3$HP37HE$iwKm;{SV00iFe>vv_eN~boFgcF8l%K_|owEI~zFnvt?DJQ(@sexf zd#^GnbPKS(V5UAdFtkaZ1IM9yb z6skj`_gvW!O8%4??S6gDq-TSc8jMaVPq9nFdb7xUqSvQ<|dVd=oB zocGct%}S>Ay#Y&LcF}5J!a8(;UB+S0pxa|4Q-0Tl6-Pxu`oWOeO3k>`w_@Y!Ep(e_o!YTx0ISFe736n^<{! zn2*vp+((y1iScIiwbBMvfGZ5EWo+NCr+ZXvTWdT#asOmr=lYpLQF~sX4_{nv{6*15 zP27a5UPz6Bma!fj1o7^+UnGh$<4V&iq*RdGqtGe&mQ0`xLEOSB6z`g$AUq*2Y1l zt-sszocYn%i?`YQR9p{|d&i2c{sr)<(Y%?dOx5{tTGFvW$4){W6(_${fi2YzqGV6N zI9OUe;wTrfNJo?>%vitJBG71v4leyvpXJCQ?lo$nvNlf@h?V<2m%y}5dYOO@co=T9 zpl*F{d-jVVQ}5WgXrznOsXZe`)rmf$;AW`>z#*??ms$m8|LC8>q}(t8>ZhBV=BK^+g1ll~79wdJ}z z+zOb0-GikjH<<~B<6Xs%H6+BIU3b8?1uy=2p?R*zk6K#w9w)O!6<(kLgvchCZAIT^ zi21Z$*tvXmO&Z3n_GU}6wfJdzoDD|N4YDngAEGxZ+`C=x6^f}{}TXb&ig_@hbNS|`HdZl+W zl+9&b>#VtQEDMzkt?(OpikT;7nH2m7)S*=WiQLz4;r7 z)Zu#+`T!*heXnz@n7P_q`LY-J%)yy%b7r5u#QR=GXS z;-TpDaFDOrul4-0qMuy%ep}ZGNf%N*W6wo*{XHj1aczPk#4viLG>sH2J0Y-^a8T|C zwNF~}6fa3Mb=Ispk=sfgCscL)vPt_@Y;-z!`^g$nrOX((t#@1dnb0&~8H$h&k+1m% zzt=xZWcWQy^JE3ZVJ-7#qmoG@pIVGN^@}^;hx$4(=8r*){_ggd4Ip)RWf*`?S>IAf zb)HRnf}jrY+t+Y6$`M0#wStN|!gL(}2!gM$@o}6(FNAdoZ8LBj$?LLY0RbjNM{ye^ z5sKite6e}r)#81_In7`-+9^2x!RNW@en#;K;lPXqY*_sO3W9ORd96LxUh?@kBvyVv zDO*FeWiZ?KLshe~^zh=xjR;jAF<5`BeC}dgpQ6l^F@B}IbJcvs|EQlS78_NOAV!1~ zdVRPgmILn6Ruv9;$$`!+b$E4=#VyQn7(w|0to%kbh~>g_QDM}j#{Bak4Vj<+d*OM8 ztE^Xz*Zi5yLPBq;3CTLg0bQ=8KvvLwd~+~Y%6LDMo z)Bk{46>0n7(kn7EVlLXptt)Q)5~1!;^*ka%f+5k&1wdqoCm)mW8;AlZ)?2P;)f42a zh^-WHM7T?WOyoAoViLYBMNW-Xo|3CyKI?vMVh$rR^c~YE57D+PoJ6RDUXODaK{F~5 zsr<1$jS~k@Czc?r&thp634xc%tGchRBJfHuMZ|P{;KXDj<_m7OFCk3Z$isJI8h+G% z#{9|q{{d~4oBaJG&~@0uC*U_<2+)vIk4GUCcQr90LeF$P116r$RR#yk@ib{{o)(DM z)++Sm0y45AN5%Hj+R>+uj9x6}(uO@H>R-O)?a+?eJW96jvTzE6_~L4>s>T6L8lnW=C} zy>ABrw=01i8h&&y*^fB?Aq^{VD}FNdqVpnux(a_UpRP}WPdj)A9pbET*F0MtJk1&f zt*RUv69E)oD{^*U7Zj3^$88N}nQc{XcXS;HO4~Hvzu#QJfiK5rGrN;UC&w2uqUTRC@zkIaM_=74$FH zG=hFrvZm>8J8!QT-Y)u3tFA_n3OZZEl8^Hi&s<&`wLYp!HaH|l@HRYcm+xqe(|-E_ zmKb@RKi#k$-t944TQK#|g4jliC-J5<;i9V4pP9b$hvmReani^MTZQ4~cvHa^Nv5rz zH`AMiUfS*Um;MA}wI}@{{n;?6wM*hxn-_r_RfevkLXo=wMK~bHS(R9Ucl!=9Bi2(r z_T{|H+TCf!#G*(Kq#rL=qQT^gpa^!1lc2*HKXuPL#^wnB)y;n~vQ$}g)6s?RfjZu` zSYONTX9Y_-dl@l(Tst`_fwsT0L;W|zGCw$BhiBCM8!dfxH6}E#xQbJ9766MPW`L0c zMz5)-bNi+QiPKUQR*?kh2=!tlRl@H?n}j8Mc$(B%^-AO4v(xA6N1Yc5h(*Y_4 z09xW~CeFXQS!bq&qG9`~_0}=_otI9JD!9gCJmu+btJKr5jYhvVhbt^ac8;CYTA(p! zP&t?xpieIG^!VTM`rOWkr2@w)^E-+LV=S}yUo!=4=X^VE~$o_hR*+^g(jBWmNb(!p+$V_6WSwe*l>0ZQCkw%g^P6 zu5Vey@FuOAx?ic@Gxo37qT=-&2*l1)Tl%R>mLbcBm)Lbwd>fgUC1=`GWL)INO4A!k zTtMaus8WdIF!uBp_`P4+gGWuwR%#?@r&3@NRFZ!7V98vY9z;)7te|hjo)3Qr{fYJA zfA)eN@A$#>gV*S%2XUw2NQ)Qk>EtNrx2=Fcj)bF1&}PO>kVZs>1od(JtC)9{y|rul}_=;}eJ28`TVn==}mRvN5&wqH|g3*Vh+|{{iV@AHJ$p?`}zL ztd4Ord27YK$}&gUW9Ifr?7p_pP7;=$491Zl$sW7Ga@Ko+IC6P>rYzUR9*ek}Qh3mr z_WuGeEzr{b(z9cZf+58=+xDmUjj8Gu_U);gWoXLL78<3!2))cN;NF>z>v%5Tznx#7st-%-^3c-<&Q~4TZeqJ$* z&yG)PX%`^p z@Tg>yDsj|P6tG-zROM2S&VIFAbIGS}^zTa%#EZ{yT@ltf;ya_2@=Y#YW<@(jj$t;CV z6#CZ_s@w(-xsD0P;apy|a-)93gkCovB+2F zSFxy-4FeX5v_r+Pu@^UaZ&p z>~Ns{+$u*M25XOUF`U!^Ixrar98-doR>!C0YhP7XwD9$npcr{in85m0U`?@ukHVo5 zW$JW`<^T>W(Eb_NyF~W@6%OTh9`)#Pl?#A#j-CfJ9~k80*FZ;_;RW6<`h z#_}+E&MBt+1He6MSgt(9^gfj#VYK3?U6ZtDy;kW(Dr0v(l;Xf(XRSyIp4j)GNTi%} zrQi+^G~6yT*mH_Oz{l%IMik zuL}dWa3h~zO6nfuup>N@dRH&2f|pKvo=>@~yp=g8pGv=K%Opntk`6wVCd?h9C)3)b zM8O5B%xW+Xa(K;Kh%P#e`&Gn|5aqiJ^HNI4ZT!Vmr6y8BcLeqM7BdnZIb?jBf9bN~Npl@M|{B6v{J<2K286 z@y~)^NV${b0S|v#{7N1n@NVeQ{5y0x#}a$j*q;U3+rs_Qq-TM6uXew4mC z@nlz*HdY0|QvQ34^ij=lHhwC%(lk#HYga5v%*}$ws9b<6=F9IE_+siwCYwN#S!80( zAd{&ioc^^!UlMpx5Bz{6Ip_qO(4HmmssdBPbFpr_q~rP4*M&9j4@<2_=P?9pWX-i-1Vww#kxA8)=dHt{?7iNTF}t7 zyF2|>)&Zfyk~4gzSnzAzEbmRFq%jUcuif;dn(WE}B#ibIN=t&xoy1fxAuH2xH3@P+ z;}qAKM+E%GwN{*xiWGh|$?6k>bvps~nDy*^D>_$J>UsW^O6L*ANIB-RuBXgx$rtBQ?(wHtzbI3H3NLc~=Y6dtU4@z!ueX2

?r25u*I2`ajX#-?~=~6E}`Sz}h!f%ATL&iuym1y1pf=DEK z*E=h6PfBj{o=-{<@Z2A*SrToG;Ck1kcw+F}cy7${03N>ek>bWIvB~@qwhd#O@F5B^ zGv75)?#aO1aauZ8iA98B7mIR^hd#Bd2aF(=Mpw3AqZlBLYUB9Q-ax7LM%cY@MQB?1 z=Ee{~u$9Lc&1hTr_Q@0nvno9S=NYYQPl!5Pn^WwpjE($N)p&2j_WJGIR<>}fTu9r#6f<}I`!&n+HCJ~Re|YU4x6g-GHbWdb-lZC_&ttm z(EK^BDP=}(AA%39b)e>!iYTU*3NlVUwdMEs{{Yy35HFx+DIMYhFnjf`TgCT>b1(#x zo@>nYTfZ?$2N*s3*BPr^0_A~iy(^W~)z>rpLXnHhV!-m{>89=fu$V++B-eL{JfIO(AKz8+^HP(_o3h+NzQXo ztY6+Z#YV~U^V*?h8@kg(WFEX#JxTnVc5--Lzn&|bXUM=momP{8eNAL)k{z2)2lcKl zMgiC!%z5>xRL9VcF-pxJKq!hp7y-ptGBFtG?kTb&G+;5o^)*?o&;#@56xN6Vklg#y zY*k$D=A)(Mz41x(rz;XiKaD9Y4JPA|4LQ#rT419dn8hLdD?-Jy2+8PbX}~ILEaN0% zkw!Dpl|jcmd(`l;7(MgH=Uqzt!FKlpuhO!$MI*mXwU{^@5NVsZJY(EajP3eWy9NSy znDA8ob<*z`Z2fB^S0SY=Ka2FNgO(oO(ywW!cJv%@59?Nm0G@d?hE~Vj$)_dG9aN5b zjB{F6a|UC;#wzRVXvtxWVvBPekus|1oEp>8q$bIs9kMI*Gxk>2*IW2ErF{G3wvnJ? z>4RTMct1>&QMg1bnAPIux{2d=J|(`-A#3|$g-ehV9I(CPpIaVZQ^Mh z;YU;HPX{%T;$0%!Skuf`0CgK!_BH(4=$7Bw8s?p7s!5ty_-CeweaJw=;m2saFJU}%600RF2XU2KCV4xnP9!akYEwMtDB=)NE zxgebM?kTG=z~_us=7V(hT4t+y>5wB3sUEHGSTT~3B97gu-e)+$%G1(LI3pF;d^3pM zc&ai~5gO;7*{^PUfL*^XDkC=B0#7u@lMl{0sHBvJJx8@d(h|dvP7OsQjkGo~?tLl; zmjmw}Ip(=bpm#VOGJ5yxRG$C?)7%Q7sY({#<>xKbeJc)matXoWsIz1ooCDgIuTj_5 zkPZRG>^ya7#Kt1UddUnl9zblMZ$A(;sn}p{Z z2lK6a#wiiUJan#hcFeyqz^JBj0KusuU_nv{?Z1aq3R48JydlloSKEMFWC z#;iw$Z0Da}O5X6EruuHF7NKiDlXq(%btk5C-oE$v74b&n;zxs;?)hR%%UL{`3}^3g z*1cxZ>oKSwoo-l4`$0MFgIz|OJM9HQgRcdFHKS zEG0vs0QaspTZi{g!2>c<2loN`E`E%u`2OyuPO5#2vc#7{% zirUr*OiukuoE%pazA4p4)fdtgUc~1$p`mN{($3LIbjplD;Nvyunq~crTLsP1qL4GW zxvtAez3{WbME5Y0*ttC`&^$dKfJ9L-p2}Gqav6!QPVm=<{vX|i#5QCeuG!+fSHqtU zeje(G3_5O^bm}=Oz?$VgJA5hdewE-IO8VDDwzj&ADUG7QIj@SwB$j93DPkAcR(`Xs z$D&xY(qw_@-nm%5DZ#XdwUjVCZKqs(REVfyX8AsctyOP}NsK*?1kgmg5z#ajQO|r^d|?K_Z62)^kQnWK>K_gOObBxgl7Xlfu_2kWP5| z8hWyvWarwZJ3!!56R16Xs(sr>UTU?$`Gy-O6%Z%X(TaRX7##OB?-gm;*@9+nKuiDV`G7u{V4FV+v%`E(2zr@VE)7k^bI#%3KfHLJq>kMI_QxX zaL6^U9O*KGtOzv*t8{F%>zjq^8W#M8MHR~Gx-Xp(3@3n<$GX?UgW|3H7uQQN7=#nN zOv5B+nr@Th&l&i$Qu`i+o9124tiz~LUsrqv@uT?D;q{!?L=8&Vv=>M?D&U^g==Dch zh|i>6qMR{_vGv7J@P4-*x8d7ckl?bYNj;cx{(iL{xvGT(E;g^~Yx&#wR~Mb)$ftF{ zwhYQo_pyrhDRm;MU-*|jeR|g~JkLH6#!DT?>T7`byR93YPQuX_1{jHBp685Wyz)V~ zjz~2SlL`PEsEmdgBc6W>*STgMxVX{c5ptr?4YD(b@pSYWM(x^!$GlDVcS$d3xQ>O;7XO*3HudP;gWC!}vEaQW_H7dvm z;Qmz{(-5EiYNL4Jqrn~Xnx{xl6(no94?l%SqmH;0AW~QQQWeSPk6}PTgYDjvqjdZ! zr9d9Tx1|>cD7<480(bSN5H^qJNwYbv*ot{b7oY?A3axpso#tAm7Liapj(swQ}H(QHtm8!uh3FILCU6rp`_C(mPAW5n z9(k)#$L25@z$XHor$)(jJU9Tk9Ok~G{f#wMPZjHWBRB5lwiBxRj12y@_AZqeDikhr zj2~+4G+jAmbzc3m-o0PJ9uSK1KwdD)xa9N|>UNeeSj4fz8lVHPt29$tf`UoF^)xF6 zP*mg|;+;i7cWD*$4GhR6jk>9(O{ZJUZXtM;qXTFd>0V9a{{R8~n0c{Bx&xkr*R^;@ ziS&Oa-N1~73^^UwHRn40qQtw9V4j}j*C=6WH6$glr#yZJzb@>bHt0K#@vlbsb*AsI zd7A<5B@2(I`Neu;KuYagouCdoty{F#qKZOZ9o!F;HELy+K?M(#oVi;aE2xwC$FmG?N_$T`|2Zrz85*gX2B#jjX4$hUZWgi_0a% z#ks6-GMLZT>s)ul`I_5Ff;^Ajw*Y@S^B*_o2RZhpOK^HrVY0c;=Sp0a%}SDIY-8^; z0rbs!e8p1U1RQORc>D!c-b;U;DhY=j^IX4(B-!R6Aok|Du%*8Wk|>B^NaCfLf~H3Y z6%;3)qO9bE=f8TXAqI?=BomyzG-2TY?<`$SKz*yAo~az#{_X!hPQN} z{`VO@G3izpT%RF}FCCZp)a$NXI;a~}N2Oi1)+P#mU`{%kw-&!T;G(8`=Cv&RM{^hw zFi(`^V05pxJ_P>HJ|FRigfxE?>RN@=lH7jqMIQhrCr{zEhMsS1op*tS63jy-zWpF zPpxO#NxtaF+#-J}@h=u$wdaZT3yA~mG7!Hn7|mk&>dlZ}nvDiHYE6NW5b*l0sHdi(s%URET#VEWQY0a89{v2QU{!R_DLtTSYNYPG~BLC4b-RNBZp98*oa zi;tH%pvLizL8fHqu4%?r?Mjj!=QS?>0GAlg(wZNfkbacl!G|A}HdB-p1}RS@<@S*O04e0{;8m!vVRD5@Ae>_rwQd6^YHK1zDlwFy zX4|y%J*v`!72IGDTRVQa2ED8HTGtMR@s~p`cBR()SqU9O4nJD`5z^o(I0{vOW1Ls0 zcpJm=>emsSn&dzeC$#_GdSpJk1?WenIG$UJwiUTya(cE zf!j!yMv05tXCR){;T|W__e{Q0l0n`1X1x8a{LMvhI0c4%a1DM`**OjQ!>q~Kzqk@ANppeH{{;`O;q@&ZREl7G+AveCkU#Ynd$M@npy znR<+S3P|udJXGZ559iveH{Ln=QAH>RZhLp8j+BSNCNut8WqH47L=17qwt~Kk-uN zzY?`;9pAdVkIj#s;Mb7;*j^#iw0o^))_WC_ZGPQ+yMUuA$_0EYYiJ>sNJ|ymak;yD z*VJAX{h>8Uej3<#QsT-cmc+*CcE_A8ev0@<#%ZtkOGni36YW;XBL(LKfyGIv>%ij= zv<|iN?z^j<3*!T9VzJz=ay#|nw{<;Pqz%1}52tZok-BmVJvkzcRaMx(0{~~!{P(CX zTO_L=m($v^JVWBEFAQ5mCRUP3jsi#5r&{M%;--j@h$PXZl$`EnQT`PzkHl>*>QzYe zh}51i$U1&i(b!%`s>#loZ476CSwS_@>DnfvKm2+}Z5ZjCV~@(ZeIwz1wW~8SwTmh0 zAJ)4a2jE7teD4sGONDsO=<}b_x@!;FJ6zQ6JhjxccsK_l9R78k@e}rF*M1)OX6IU; zRnu;*U?a>(VcON;d%%)yt&mvYkol6!r%@7@9S0WE?FDps72&~aY)xL*ykAUPjK=t*R?QPw^joJq&C~vuS#!` zgMfauI$P}n2Aa`v%WyqA(?m)FoZz2Ij#dkRIrgYW9P>v$rj#Z}>00nM=#!JhLv}xQ z$F4e7c1iT0ag5`ddOtkp6?aaUE$l@`4srFag`Iih1a{3~>P)6qz~ZVF+&BZ8X<|DA zk4kWT+jGV%S4DF7A$jB;^{&zqF`C1^C?{+W!l;JY`RF|>O4#p(QCMf7&%IuUM!Q!~ z?GtCXCyJElmunoG<&2g+2WofxBezk+0?8X5ryl0FEbJO6kTa<`?M}VWE_Dry4ACyq zALc%lW-kxlNgxd@8-dP48tr@^4x{0p5$RgEF*vfFZZ@kQn;hc5OTGwr%SiaEr8cLh zvxkI%JPa^Z>*)Ug4c?`dxKubEg1e0~#0w%Qe*NoeDbi31usNw}fz%Z>f3Y_V@p*~w zgM(g6`$Ks8Ujs>_IC)YVo$B6M<0IC*X5-_B*vPmUkV()+4&rOQ_#N@0_g?W$wwsvPy3wT-1IIlnPMeH*{aTKfOf)50D zHS_n5^e-hPp4%HRenlLL@#Tm^ew&*lmLrU0;Me5$hb&)EyR(5C8|3HNM_T$DO0!6< zrHxwzU@kj=F;3Lf$8Bz^Sy|Zb2e`#^o;AIFGs1VJSBU^p!`{3`Xt%PENcQbfI)(X3 z&%HyDpF!T5_}!7tDbYUn0B4@{pxRu!LlwgDf=~3S&ZS=fjMRb1Oy?{(>(rX{{{RTs ziysf#ry#yrf$nR193tdokxxJSngb8 z*Hd{Qd$}BOo-y>Qj^8$O#aFpR(Do`R`>Ta0Fnr|I>hVb4#;G^!4H zCJ&*dDo1KcD58oeqKYV@iYTP-j8dwT&>U3g3a~wTaro8q2~q(g9-fswwWucu z!#?$YP`2{pjfoip1cTPG?pc_kfN*oqy=rN9C16;jf0+*i_sw)L<6_`sA6oi{_Al3k zv|84h(hMJ&LFx!StLlh#Y(xsSN1&}6ZDk}~%>G_Kz22`yt*Zft=N)s_wJf#G`33UA zufgIoji=@3schs|E3EjF`wzz+AJv47wWj+#gbvk>`(wiXA<=w2b#EV-$vei(djh-) z_Y3L!4>Gg3}R0Z+21ww$+BB?+_*R=gPQxd_E-3ZHQ$7ATeQzC z`ecm*7CeFcHR=~S>63TOfsUl+yjSCm$+fxB^@y7cvn*}+bD!r|=GH~ctWrfT_?!j+ zA6op*MB1vq!Uh1W>#$^(RI|-9a9Df;t-MG~bRsEwH&vU&3M> z;Zz#>Tj2iy#Qh6a(wggD_?>N_UU9Y?m=^@sO&{$KdExCvzq7nY;r{>=+X6t7a-m_K zqZQ8lV*R79e1G8C^oG-I>|%CSmP>iWn}DyJB7L!dNp5qV^~HQty~dAeZyPd6B$;3s z@}t_k!zv^vY2E9e{=GOj+Esx7`kI7nE_f^lsiWivECBjdy*@D4Fsk8Mflv+z_NB=kF-e{=`qb95{hD-> zk`(mytuwC1#O5aF(2lzhi7^ukNwZFHm z4geCU$MFizy1z?_K1_gnkOY9c!imi`8!K>8@g7kJ|QUi7U76I65YKk z(0&)$U3^OMRgZ^s;}xyd&mLsNkhS^=`#gLnpWv;grjzR{sY!ZMdPfmOTxPx2&sQS_ z8yz}VL20j)U`w1H>s=<-nIqk1|Hluse9^ zkHWo2{uhUZd}X8fdf!LXe!+jCjq%80U{4~x%kU41pz)uC^!-v9qnOJW5*W@vanycw z(N~##QPiKqz9P`IL?u^$mgIK?*5HXeStugNckI&itv!s=AfQmr_W`#Bs;+mFJWH0N1ZS_`P!{*sSc4vF7~e2d)iyjl=mkQm3a{&ymX< z*n{(P zj-=ID{$|4Qo|R7eMo3R^Ju8d9ZPzdOaZm;*ayrtPGCB>Z0FpXW$siN_>LJpW zig_c+T936`vr?$d$p`eQQG%{>(wQSOE_&5A7;mi$!4*12TaIy4q-&AE#VaC*1Eo;u zMHEp*6j4PKQAHIj2pc?S6(hyYFhHh|7oOd5S0p8ZvYw+qol9v9^RXy6PDux)T)UU$ zU`Z{-K&$3PxKK_ynn_`3*d7NRt3yG(U$z*_jjX%|J^icK{t|pkhvA2b^y`UXGTP5@ zt8(AnQgL5!%c|MD>ot_3ds$RSg%41AREJdnjjBh~)YiJt^LEL#bI^*t4!nxs0y9^R zyg>k904tUFiL5lbDoBs>TgXFxU#W1K0d3;yJ*MH+CO!bAeQ1 zSc1tT9hg$vk4~AdpS7E|)O9P%w_?pCje-whz^vz(#!d(Tf!3l~43G04%B&>&6nxpF zTaxNQIn7NR`K{dXxS&G!rTwI9}UMF0T*_pdGZrFtaSBDWtfiavw2aYLyc>CWr`8O}difM<6jpGs1{Fys;MTV4>i`%j4M zmCjWd$&YX=xxJLjbs-?&vE)@^yb=!{m0oS65uPw}?OaZ+F6)4}!0V4ntpQOP86ckd zsZ_$b+{EUqJ0rGNG^w4Zr{hyD005uE(y7QnbIyLW(Sk8ZKl;@7QWO)OX}vu;r5?Y^ zg(E#b8V@J1_n-`qr9D&SIi@m|Jolvc?TQBimWn8%iYTIrD58o~IAKyS%7aoX6Bz@i zrCYa<$EGus84_bY)WLF2JBqnvvRuf(bX6=t?lD#&U{@gZ=xWpoWN(+K>IG@U%(xiO z^{+_%pS(!^75K&B%l1+u`$9&<`V)`jYxN2}PZPSS%I@8tV_KHl&eb@4e?wZ9y8CQU z?f8!Mvt_QZar?km*w*c@i5~zoo?bigSf3HKzwI9kOn05u>?T9*D^thXjgODM2eV&5 zE}~cxW>)9!k)P7PH9j$1X_9!p>Rky$8g#0qB3=L^gWkID+1ug`pTl1fM|-N+CZl6= zncpR8(rE7PqYHg@fC_Pc9tF^c*UeQq^~l>61>{{S7XTK@pSzI0&k zwv;&|u=T8M55&V)@W!8TWZdR3tZ%XJpIVOi(RI4udc+L!!X;o7dJkcW$rzeuehGH({Hw0E{SmXH==ayazGR3L5isF-a$QiaItfr^z70g_MBn%uV;9@Qjlp+F#K z(zpB{Znxefu|Nsf-ew1S_4yk&E^&@&9kRHp(x8!gk%QaQ(ws`P0SM@Goev(u@(00S40fY3bR}2h%fyX(ldF0EHjyh(moxSr}n#ISHZ6HJQ z5O_7k$mQQHjGszw53LyVpp4U${M7Q}80k^bMLe$P#yQ0xjfnL6)ysd~#6dlK3WGfT z>}S17SrIU;xQ_U!88`gd0-CXq*x*&98wPStG18~XuP2OCWEnhn@BJ$@@J~OD6j4PK zQAHF{MHEz)&$PEp5&G4l;lV#nY9>PG1JbnZ^!cZT21Z^~0vPtKeI_^4H2a#cr;xg#&}0 z`8?B?P%OE~1KZZ6iv9Q?^{WZ0Is(p1HaHcZu3sCyKI$9=5=3#y>x|XEgZ?gCpA1Q7 zCgbF07aqCqULo<*;bV9*89Ys=NN)7$UlPE_4HkM4UTN^&)?XHQ*2_sZ0?I4CxLo6p z@UOLe1>w6NhCUX*f(xTOuB{}pu;7q56=%VICDS}hqfX{u^ytB5a87b_f!o@)Exb^5 zpASkVUGF4M8y3$51O9VdCxJC-?IpXic2-#2vhL@K`D*wB5JhA2<0IOzyk&B9?+9N^ zpcVikpZ18r{{R~BFb9?w1x5!n$rK)P4tT{yv?~Kh+Xr3_J9MWj4WrW?Y0I~02atVg zNRM39s7I9i9{oF4vG{ToweaSb4%Sd(-_9#)B{DEy?;Z_!w~VeD^Tk?&oCQ}3KLLuw z@`-LZBi4^AB>I6@bgvKG>W*;UTwsOX-T9={Eh2*9p^3zTGC^-rD{H_}Y}X`}mne{* z^utw|Ze@z1p+ z%o_(7#YfcUfzpo&wLgCrA@rk>Wr3xxxk+t6%o zeEreenHO#tf;g<*tq$e;r3q8@Zb!-h?bfw?H+A+cYRXa@ zbaNwQ4uZc-^c``ow2KS3c1D)j8Hnz11#R2w3j0|YvL5wrEoqsEL9`5V)unH$vXH|E z>+4!@Yf@k^f1kcaX?1;M-D&g0;1w}2I3CpBggztw*!XdCr_b{QGTR?gF<%4t=;@kv zuc$PVOBzEok*Euvjf%n2d_iv@hB%$2iJ4J%0x)x5U;fXHsr)SXx1_;&a)upB$&{j< z*ap7GlUy=x!wcV literal 0 HcmV?d00001 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); +} From 3e07b7c5f39aa5fc81c4eda1ed7574a12627c8ab Mon Sep 17 00:00:00 2001 From: David Date: Sun, 27 Sep 2026 12:14:19 -0700 Subject: [PATCH 12/15] feat(stereo-camera): the service refines row alignment online; probe reports it A per-camera worker measures rectified frames the camera thread parks for it (every 250 ms for 5 s after an open or an update, then every 30 s; 2.7 ms per measurement at -O2, off the frame path), rebuilds geometry + LUTs when the correction moves, and the camera thread swaps them in between frames and bumps calibration_generation. WARN on first application, INFO after, re-verified on re-open, DXR_STEREO_CAMERA_REFINE=0 kill switch. Stream stats carry the state, applied a/b, match count and dy before/after; camera probe prints them. Co-Authored-By: Claude Opus 5.5 --- src/xrt/include/xrt/xrt_stereo_camera.h | 22 ++ src/xrt/ipc/server/ipc_server_stereo_camera.c | 300 ++++++++++++++++++ src/xrt/targets/cli/cli_cmd_camera.c | 30 ++ 3 files changed, 352 insertions(+) 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_stereo_camera.c b/src/xrt/ipc/server/ipc_server_stereo_camera.c index b2d39bfcac..d3ca57ff22 100644 --- a/src/xrt/ipc/server/ipc_server_stereo_camera.c +++ b/src/xrt/ipc/server/ipc_server_stereo_camera.c @@ -40,6 +40,22 @@ * 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: @@ -77,6 +93,7 @@ #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 @@ -93,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) @@ -100,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 /* @@ -239,6 +258,34 @@ struct scam_rectifier // 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 @@ -550,11 +597,36 @@ rectifier_destroy(struct scam_rectifier **rp) } 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 @@ -600,6 +672,34 @@ rectifier_create(const struct scam_camera *cam) 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( @@ -615,6 +715,176 @@ rectifier_create(const struct scam_camera *cam) 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) @@ -886,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); } @@ -906,9 +1180,17 @@ camera_thread(void *ptr) 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; } @@ -1062,6 +1344,7 @@ 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++) { @@ -1612,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 d10ac11c78..2755a72bbd 100644 --- a/src/xrt/targets/cli/cli_cmd_camera.c +++ b/src/xrt/targets/cli/cli_cmd_camera.c @@ -34,6 +34,14 @@ * (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. */ @@ -570,6 +578,8 @@ cmd_probe(struct ipc_connection *ipc_c, int argc, const char **argv) 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; @@ -780,6 +790,9 @@ cmd_probe(struct ipc_connection *ipc_c, int argc, const char **argv) 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. float sm = ra.dy_signed_median < 0 ? -ra.dy_signed_median : ra.dy_signed_median; rows_ok = ra.dy_median <= 0.5f && sm <= 0.5f; @@ -824,6 +837,23 @@ 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); From 558ce0c4f6fcb990b6ffd7af641b27469af0b9e5 Mon Sep 17 00:00:00 2001 From: David Date: Sun, 27 Sep 2026 12:14:19 -0700 Subject: [PATCH 13/15] docs(stereo-camera): R2 online vertical-alignment refinement as built Co-Authored-By: Claude Opus 5.5 --- docs/roadmap/stereo-camera-source.md | 28 ++++++++++++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/docs/roadmap/stereo-camera-source.md b/docs/roadmap/stereo-camera-source.md index 2b38111571..859e3fbe4d 100644 --- a/docs/roadmap/stereo-camera-source.md +++ b/docs/roadmap/stereo-camera-source.md @@ -262,8 +262,32 @@ against the sim fake). 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). -- **Not in R2:** the GPU backend (seam in place), per-frame recalibration (`calibrationGeneration` - stays 0), and a rectifier for a camera whose calibration changes at runtime. +- **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 From f932a512c46debc58f9dd0b2bfc0b340bcfea228 Mon Sep 17 00:00:00 2001 From: David Date: Sun, 27 Sep 2026 13:20:23 -0700 Subject: [PATCH 14/15] fix(cli): probe row gate = |signed median dy| + MAD, not the unsigned |dy| median On the SR laptop with a face at 0.6 m the refined rows were aligned (signed median -0.10/-0.18 px) but --rectified FAILED: block-match noise alone put the unsigned |dy| median at 1.03/1.10 px. Gate on |signed median| <= 0.5 px, MAD about it <= 1.5 px and >= 20 blocks; print the gate numbers; keep |dy| stats informational. Co-Authored-By: Claude Opus 5.5 --- src/xrt/targets/cli/cli_cmd_camera.c | 30 +++++++++++++++++++++------- 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/src/xrt/targets/cli/cli_cmd_camera.c b/src/xrt/targets/cli/cli_cmd_camera.c index 2755a72bbd..cf5b71986c 100644 --- a/src/xrt/targets/cli/cli_cmd_camera.c +++ b/src/xrt/targets/cli/cli_cmd_camera.c @@ -23,9 +23,10 @@ * 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 - * (median |dy| > 0.5 px, or a SIGNED median dy beyond +-0.5 px: a constant - * vertical offset). Matching is zero-mean NCC with sub-pixel refinement; the + * (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 — @@ -452,7 +453,8 @@ struct row_alignment 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 + 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]; @@ -537,6 +539,13 @@ measure_rows(const uint8_t *gray, 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; } @@ -794,8 +803,15 @@ cmd_probe(struct ipc_connection *ipc_c, int argc, const char **argv) 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 = ra.dy_median <= 0.5f && sm <= 0.5f; + 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) { @@ -870,8 +886,8 @@ cmd_probe(struct ipc_connection *ipc_c, int argc, const char **argv) } if (!rows_ok) { printf( - "--rectified: FAIL — rows do not align (median |dy| or |signed median dy| > 0.5 px, or too " - "few matched blocks)\n"); + "--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"); From 0191f548108395d80edff0f969c781ca68c6646f Mon Sep 17 00:00:00 2001 From: dfattal Date: Sun, 27 Sep 2026 15:18:47 -0700 Subject: [PATCH 15/15] fix(ipc): PRESENT_OWNER quota counts owners (executables), not connections On the SR laptop the B1 browser's "3D Camera (DisplayXR)" never appeared: [CAP] refusing client pid=... ('ChromiumStereoCamera'): class PRESENT_OWNER quota 2/2 exhausted (#960). One browser already takes both slots (GPU process weaves, browser process binds the window), so its video-capture utility - the XR_DXR_stereo_camera client, which declares PRESENT_OWNER by enabling XR_DXR_weave - was the third connection and was refused (xrCreateInstance -10 in the capture service). A connection from the same executable as an admitted PRESENT_OWNER is now a sibling and takes no new slot; the quota is compared against DISTINCT owner executables, so two different browsers still fill it. A peer whose image path is unreadable is never a sibling (fails closed). Verified on the SR laptop: all three Chromium processes admitted, the page lists and opens "3D Camera (DisplayXR)" (sbs 1280x480), stream RECTIFIED. Co-Authored-By: Claude Opus 5.5 (1M context) --- src/xrt/ipc/server/ipc_server_handler.c | 45 ++++++++++++++++++++++++- 1 file changed, 44 insertions(+), 1 deletion(-) 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) {