reproject: test C3X cameras through comma 4 geometry

openpilot/selfdrive/locationd/calibrationd.py

@@ -20,6 +20,8 @@ from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process
 from openpilot.common.transformations.orientation import rot_from_euler, euler_from_rot
 from openpilot.common.swaglog import cloudlog
+from openpilot.selfdrive.modeld.helpers import reproject_expected
+from openpilot.selfdrive.modeld.reproject_c4.rotation import read_rotation
 
 MIN_SPEED_FILTER = 15 * CV.MPH_TO_MS
 MAX_VEL_ANGLE_STD = np.radians(0.25)
@@ -270,12 +272,13 @@ def main() -> NoReturn:
 
   calibrator = Calibrator(param_put=True)
   calibrator.not_car = CP.notCar
+  hold_reproject = reproject_expected()
 
   while 1:
     timeout = 0 if sm.frame == -1 else 100
     sm.update(timeout)
 
-    if sm.updated['cameraOdometry']:
+    if sm.updated['cameraOdometry'] and (not hold_reproject or read_rotation()):
       calibrator.handle_v_ego(sm['carState'].vEgo)
       new_rpy = calibrator.handle_cam_odom(sm['cameraOdometry'].trans,
                                            sm['cameraOdometry'].rot,

openpilot/selfdrive/modeld/helpers.py

@@ -1,9 +1,11 @@
+import functools
 import io
 import pickle
 import struct
 from pathlib import Path
 
 from openpilot.common.file_chunker import get_manifest_path
+from openpilot.common.hardware import HARDWARE
 from openpilot.common.hardware.usb import CHESTNUT_USB_PRODUCT, USB_DEVICES_PATH, is_chestnut_usb_id
 
 MODELS_DIR = Path(__file__).resolve().parent / 'models'
@@ -38,3 +40,9 @@ def chestnut_compiled() -> bool:
   path = modeld_pkl_path(chestnut=True)
   return (path.is_file() or Path(get_manifest_path(path)).is_file()) and all(
     (MODELS_DIR / f'big_driving_warp_{size}_tinygrad.pkl').is_file() for size in ('1344x760', '1928x1208'))
+
+
+@functools.cache
+def reproject_expected() -> bool:
+  """Serve the C3X cameras through the C4 reprojection stage when a chestnut is attached."""
+  return HARDWARE.get_device_type() in ("tici", "tizi") and chestnut_present()

openpilot/selfdrive/modeld/reproject_c4/__init__.py

@@ -0,0 +1,19 @@
+"""Reproject the comma 3X cameras into comma 4 camera geometry, on the GPU, in front of an untouched comma 4 model.
+
+Every comma 4 output pixel is traced through the comma 4 lens to a ray, rotated onto the 3X device axis and looked up in
+the 3X wide (fisheye) or, for the narrow output, composited from the 3X narrow inset and the 3X wide surround. None of it
+depends on the calibration, so the lookup tables are constants built once per process and the per-frame work is one
+gather per output byte (two plus a blend for the composite) - the same cost as the model's own crop warp.
+
+Outputs are NV12 buffers in the comma 4 camerad layout so the model pkl (keyed by its camera size) sees exactly what a
+comma 4 would have given it.
+
+geometry: lenses, rotations, sample coordinates. tables: the gather tables. meter: the seam exposure match. fit: the
+rotation fit from frame pairs. rotation: the fitted rotation across boots. cameras: camerad's frame pairs. kernel (the
+GPU stage, tinygrad) is imported by the stage alone."""
+from .geometry import *  # noqa: F403
+from .meter import *  # noqa: F403
+from .tables import *  # noqa: F403
+from .rotation import *  # noqa: F403
+from .fit import *  # noqa: F403
+from .cameras import *  # noqa: F403

openpilot/selfdrive/modeld/reproject_c4/cameras.py

@@ -0,0 +1,22 @@
+"""camerad's narrow and wide frames paired by start-of-frame time, as modeld pairs them on a two-camera device."""
+
+from msgq.visionipc import VisionIpcClient
+from openpilot.common.swaglog import cloudlog
+
+
+def recv_pair(narrow: VisionIpcClient, wide: VisionIpcClient):
+  """The newest narrow frame and the wide frame taken with it, or None. The narrow client conflates; the wide must not,
+  so the frames it queued can be drained up to the narrow's."""
+  buf_n = narrow.recv()
+  if buf_n is None:
+    return None
+  while True:
+    buf_w = wide.recv()
+    if buf_w is None or narrow.timestamp_sof < wide.timestamp_sof + 25000000:
+      break
+  if buf_w is None:
+    return None
+  if abs(narrow.timestamp_sof - wide.timestamp_sof) > 10000000:
+    cloudlog.error(f"reproject: frames out of sync! narrow {narrow.frame_id} ({narrow.timestamp_sof / 1e9:.5f}), " +
+                   f"wide {wide.frame_id} ({wide.timestamp_sof / 1e9:.5f})")
+  return buf_n, buf_w

openpilot/selfdrive/modeld/reproject_c4/fit.py

@@ -0,0 +1,143 @@
+"""The rotation fit of one frame pair: phase-correlation matches between the narrow inset and the wide surround, Kabsch on
+the matched rays. numpy only: it runs on the device CPU during the calibration phase."""
+
+import numpy as np
+
+from .geometry import C4_NARROW_K, calib_from_rotvec, matrix_to_rotvec, sample_coords, unproject_fisheye, unproject_pinhole
+
+MIN_MATCHES = 15  # matched patches a frame's fit must keep after kabsch's trimming
+MAX_RMS_DEG = 0.25  # per-frame residual after trimming (good frames 0.10-0.17 on the 3X)
+N_FRAMES = 12  # per fit: every logged session had converged by then, the median of 12 within 0.03 deg of the 40-frame mean
+
+def render_layers(narrow_y, wide_y, calib, dst_wh=(1344, 760)):
+  """The comma 4 narrow view's luma twice, nearest-sampled: inset from the 3X narrow, surround from the 3X wide, plus the
+  wide sample coordinates and the mask where both exist. With the right rotation the two layers coincide."""
+  sh, sw = narrow_y.shape
+  mw, mn = sample_coords("narrow", dst_wh[0], dst_wh[1], 1.0, calib)
+  xn = np.round(mn[..., 0] - 0.5).astype(int)
+  yn = np.round(mn[..., 1] - 0.5).astype(int)
+  xw = np.round(mw[..., 0] - 0.5).astype(int)
+  yw = np.round(mw[..., 1] - 0.5).astype(int)
+  vn = (xn >= 0) & (xn < sw) & (yn >= 0) & (yn < sh)
+  vw = (xw >= 0) & (xw < sw) & (yw >= 0) & (yw < sh)
+  inset = np.where(vn, narrow_y[yn.clip(0, sh - 1), xn.clip(0, sw - 1)], 0).astype(np.float32)
+  surround = np.where(vw, wide_y[yw.clip(0, sh - 1), xw.clip(0, sw - 1)], 0).astype(np.float32)
+  return inset, surround, mw, vn & vw
+
+def phase_shift(a, b):
+  """Shift of b relative to a (float32 patches, same shape) by phase correlation: (dx, dy, peak-to-sidelobe ratio)."""
+  h, w = a.shape
+  win = np.outer(np.hanning(h), np.hanning(w)).astype(np.float32)
+  A = np.fft.rfft2((a - a.mean()) * win)
+  B = np.fft.rfft2((b - b.mean()) * win)
+  R = A * np.conj(B)
+  R /= np.abs(R) + 1e-6
+  r = np.fft.irfft2(R, s=(h, w))
+  py, px = divmod(int(np.argmax(r)), w)
+  peak = r[py, px]
+  m = np.ones_like(r, bool)
+  m[max(0, py - 5):py + 6, max(0, px - 5):px + 6] = False
+  side = r[m]
+  psr = (peak - side.mean()) / (side.std() + 1e-9)
+  def sub(c, l, rr):  # parabolic sub-pixel peak
+    d = l - 2 * c + rr
+    return 0.0 if d >= 0 else float(0.5 * (l - rr) / d)
+  dx = px + sub(peak, r[py, (px - 1) % w], r[py, (px + 1) % w])
+  dy = py + sub(peak, r[(py - 1) % h, px], r[(py + 1) % h, px])
+  if dx > w / 2:
+    dx -= w
+  if dy > h / 2:
+    dy -= h
+  return -dx, -dy, float(psr)
+
+def match_rays(narrow_y, wide_y, calib, dst_wh=(1344, 760), patch=96, stride=64, min_psr=5.0, max_shift=None):
+  """Correspondences between the inset and the surround (patch center -> center + its measured shift) as narrow and wide
+  rays in the current calibration's geometry, or None when the frame has too few usable patches. min_psr 5: at dusk only
+  a third of the textured patches reach 6 and frames fell under the match floor; 5 doubles them at the same per-frame
+  accuracy (kabsch trims the rest)."""
+  inset, surround, mw, valid = render_layers(narrow_y, wide_y, calib, dst_wh)
+  dw, dh = dst_wh
+  max_shift = max_shift or patch / 3
+  pa, pb = [], []
+  for y in range(0, dh - patch + 1, stride):
+    for x in range(0, dw - patch + 1, stride):
+      if valid[y:y + patch, x:x + patch].mean() < 0.98:
+        continue
+      a = inset[y:y + patch, x:x + patch]
+      b = surround[y:y + patch, x:x + patch]
+      if a.std() < 4 or b.std() < 4:  # flat: sky, bonnet, night
+        continue
+      dx, dy, psr = phase_shift(a, b)
+      if psr < min_psr or abs(dx) > max_shift or abs(dy) > max_shift:
+        continue
+      pa.append((x + patch / 2, y + patch / 2))
+      pb.append((x + patch / 2 + dx, y + patch / 2 + dy))
+  if len(pa) < 4:
+    return None
+  pa, pb = np.array(pa, np.float32), np.array(pb, np.float32)
+  rays_n = unproject_pinhole(pa, C4_NARROW_K[0, 0], C4_NARROW_K[0, 2], C4_NARROW_K[1, 2])
+  xb, yb = np.round(pb[:, 0] - 0.5).astype(int).clip(0, dw - 1), np.round(pb[:, 1] - 0.5).astype(int).clip(0, dh - 1)
+  rays_w = unproject_fisheye(mw[yb, xb], calib["wide"])
+  return rays_n, rays_w
+
+def kabsch(rays_n, rays_w):
+  """Rotation taking narrow rays onto wide rays, trimmed twice at the 80th percentile: (rotvec, n_kept, rms_deg)."""
+  for _ in range(2):
+    U, _, Vt = np.linalg.svd(rays_w.T @ rays_n)
+    d = np.sign(np.linalg.det(U @ Vt))
+    Rm = U @ np.diag([1, 1, d]) @ Vt
+    res = np.degrees(np.arccos(np.clip((rays_n @ Rm.T * rays_w).sum(1), -1, 1)))
+    keep = res <= np.percentile(res, 80)
+    rays_n, rays_w = rays_n[keep], rays_w[keep]
+  return matrix_to_rotvec(Rm), int(len(rays_n)), float(np.sqrt(np.mean(res[keep] ** 2)))
+
+def fit_rotation(narrow_y, wide_y, calib0, dst_wh=(1344, 760), iters=2, coarse=True):
+  """One frame pair -> (rotvec, n_matches, rms_deg), or None when too few patches matched. With `coarse` the first pass
+  uses big patches (a fleet-median seed can be tens of px off) and the second re-renders with its fit and refines with
+  small ones. From a good seed (the running estimate of earlier frames) coarse=False, iters=1 is enough."""
+  calib = dict(calib0)
+  R = np.asarray(calib0["R"], float)
+  for it in range(iters):
+    calib["R"] = tuple(R)
+    m = match_rays(narrow_y, wide_y, calib, dst_wh, patch=192, stride=96, max_shift=64) if (it == 0 and coarse) else match_rays(narrow_y, wide_y, calib, dst_wh)
+    if m is None:
+      return None
+    R, n, rms = kabsch(m[0], m[1])
+  if n < MIN_MATCHES:
+    return None
+  return tuple(float(v) for v in R), n, rms
+
+
+class Fit:
+  """The running fit: the rotation it started from (what reprojectd applies), the accepted per-frame fits and their
+  component-wise median, and the last frame it looked at."""
+  def __init__(self, applied):
+    self.applied = tuple(float(v) for v in applied)
+    self.calib = calib_from_rotvec(self.applied)
+    self.fits: list[tuple] = []
+    self.mean = self.applied
+    self.last_id = 0
+    self.last_ok = False
+
+  @property
+  def n(self) -> int:
+    return len(self.fits)
+
+  @property
+  def spread(self) -> float:
+    return float(np.degrees(np.abs(np.array(self.fits) - self.mean).max())) if self.fits else 0.0
+
+  def frame(self, narrow_y, wide_y, frame_id: int):
+    """One frame pair: the first from the applied rotation with the coarse pass, later ones refined from the running median
+    (half the work, ~1 frame/s on the 3X). Returns the frame's fit (rotvec, matches, rms) or None when rejected."""
+    if not self.fits:
+      r = fit_rotation(narrow_y, wide_y, self.calib)
+    else:
+      r = fit_rotation(narrow_y, wide_y, calib_from_rotvec(self.mean), iters=1, coarse=False)
+    self.last_id = frame_id
+    self.last_ok = r is not None and r[2] <= MAX_RMS_DEG
+    if not self.last_ok:
+      return None
+    self.fits.append(r[0])
+    self.mean = tuple(float(v) for v in np.median(self.fits, axis=0))
+    return r

openpilot/selfdrive/modeld/reproject_c4/geometry.py

@@ -0,0 +1,116 @@
+"""Lens models, the narrow->wide rotation as a rotation vector, and where each comma 4 output pixel samples the 3X frames."""
+
+import numpy as np
+
+from openpilot.common.transformations.camera import DEVICE_CAMERAS
+
+
+# measured lenses (self-calibrated comma 4 population, 3X bench + fleet); f/cx/cy in px, angles in rad
+X3_NARROW = {"f": 2600.85, "cx": 964.0, "cy": 604.0, "k1": -0.36400}  # pinhole with radial barrel: r=f·x(1+k1·x²)
+
+C4_WIDE = {"f": 442.555, "cx": 672.380, "cy": 378.718, "k": (0.0089611, 0.029156, -0.015066), "tc": 1.39626}  # fisheye r=f·θ(1+k1θ²+k2θ⁴+k3θ⁶), linear past tc
+
+# fleet median of 40 self-calibrated 3X units: the lens a unit gets when only its rotation is calibrated (the model's and the
+# self-cal's rotations pair with this nominal center, not with a board-calibrated lens)
+X3_WIDE_POP = {"f": 597.732, "cx": 963.936, "cy": 603.959, "k": (-0.011968, 0.024043, -0.0091132), "tc": 1.51354}
+
+POP_ROTATION = (0.0154781, -0.0225994, -0.00068013)  # rotvec: the 3X wide points ~1° above the narrow
+
+C4_NARROW_K = DEVICE_CAMERAS[("mici", "os04c10")].narrow_road.intrinsics
+
+def calib_from_rotvec(rotvec):
+  return {"wide": X3_WIDE_POP, "narrow": X3_NARROW, "R": tuple(float(v) for v in rotvec)}
+
+def rotvec_from_wide_from_device_euler(euler):
+  """The model's wide_from_device_euler (device frame: roll, pitch, yaw) as our camera-axes rotvec (x=pitch, y=yaw, z=roll)."""
+  r, p, y = euler
+  return (float(p), float(y), float(r))
+
+def matrix_to_rotvec(M):
+  a = np.arccos(np.clip((np.trace(M) - 1) / 2, -1, 1))
+  if a < 1e-9:
+    return np.zeros(3)
+  return a / (2 * np.sin(a)) * np.array([M[2, 1] - M[1, 2], M[0, 2] - M[2, 0], M[1, 0] - M[0, 1]])
+
+ZMIN = np.cos(np.radians(88.0))  # rays further off-axis than this have no 3X wide pixel
+
+FEATHER_PX = 50  # composite seam width in comma 4 narrow px (50 over 24, by eye on drive clips)
+
+UV_FILL = 128
+
+def _dtheta_d(t, k):
+  return 1 + 3 * k[0] * t**2 + 5 * k[1] * t**4 + 7 * k[2] * t**6
+
+def _theta_d(th, L):
+  k, tc = L["k"], L["tc"]
+
+  def p(t):
+    return t * (1 + k[0] * t**2 + k[1] * t**4 + k[2] * t**6)
+
+  return np.where(th <= tc, p(th), p(tc) + _dtheta_d(tc, k) * (th - tc))
+
+def unproject_fisheye(px, L):
+  dx, dy = px[..., 0] - L["cx"], px[..., 1] - L["cy"]
+  r = np.hypot(dx, dy)
+  td = r / L["f"]
+  th = td
+  for _ in range(4):  # Newton from θ=θd: at double precision by the third step for every lens here
+    th = th - (_theta_d(th, L) - td) / _dtheta_d(np.minimum(th, L["tc"]), L["k"])
+  s = np.sin(th)
+  rr = np.where(r > 0, r, 1)
+  return np.stack([s * dx / rr, s * dy / rr, np.cos(th)], -1)
+
+def unproject_pinhole(px, f, cx, cy):
+  d = np.stack([(px[..., 0] - cx) / f, (px[..., 1] - cy) / f, np.ones(px.shape[:-1])], -1)
+  return d / np.linalg.norm(d, axis=-1, keepdims=True)
+
+def project_fisheye(rays, L):
+  rho = np.hypot(rays[..., 0], rays[..., 1])
+  th = np.arctan2(rho, rays[..., 2])
+  r = L["f"] * _theta_d(th, L)
+  rr = np.where(rho > 0, rho, 1)
+  return np.stack([L["cx"] + r * rays[..., 0] / rr, L["cy"] + r * rays[..., 1] / rr], -1)
+
+def project_pinhole_k1(rays, L):
+  x, y = rays[..., 0] / rays[..., 2], rays[..., 1] / rays[..., 2]
+  s = 1 + L["k1"] * (x * x + y * y)
+  return np.stack([L["cx"] + L["f"] * x * s, L["cy"] + L["f"] * y * s], -1)
+
+def rotvec_to_matrix(v):
+  v = np.asarray(v, dtype=np.float64)
+  a = np.linalg.norm(v)
+  if a == 0:
+    return np.eye(3)
+  k = v / a
+  K = np.array([[0, -k[2], k[1]], [k[2], 0, -k[0]], [-k[1], k[0], 0]])
+  return np.eye(3) + np.sin(a) * K + (1 - np.cos(a)) * K @ K
+
+def sample_coords(out_cam, dst_w, dst_h, scale, calib):
+  """Float 3X wide / narrow coordinates for every comma 4 `out_cam` pixel (scale 0.5 = the half-res chroma plane)."""
+  xs, ys = np.meshgrid((np.arange(dst_w) + 0.5) / scale, (np.arange(dst_h) + 0.5) / scale)
+  px = np.stack([xs, ys], -1)
+  if out_cam == "wide":
+    rays = unproject_fisheye(px, C4_WIDE)
+  else:
+    rays = unproject_pinhole(px, C4_NARROW_K[0, 0], C4_NARROW_K[0, 2], C4_NARROW_K[1, 2])
+  rays_w = rays @ rotvec_to_matrix(calib["R"]).T
+  mw = project_fisheye(rays_w, calib["wide"]) * scale
+  mw[rays_w[..., 2] < ZMIN] = -1
+  mn = project_pinhole_k1(rays, calib["narrow"]) * scale
+  mn[rays[..., 2] <= 0] = -1
+  return mw, mn
+
+def _nv12_index(xy, src_w, src_h, stride, uv_offset, chroma):
+  """Nearest-neighbor byte index into a source NV12 buffer for float sample coords; chroma coords are in the half-res plane.
+  Returns (index, valid). For chroma the caller adds +1 for the V byte."""
+  xy = np.clip(np.nan_to_num(xy, nan=-1.0), -1e6, 1e6)  # grazing rays project to ±inf
+  x = np.round(xy[..., 0] - 0.5).astype(np.int64)
+  y = np.round(xy[..., 1] - 0.5).astype(np.int64)
+  w, h = (src_w // 2, src_h // 2) if chroma else (src_w, src_h)
+  valid = (x >= 0) & (x < w) & (y >= 0) & (y < h)
+  x = np.clip(x, 0, w - 1)
+  y = np.clip(y, 0, h - 1)
+  idx = (uv_offset + y * stride + 2 * x) if chroma else (y * stride + x)
+  return idx, valid
+
+C4_CAM = (1344, 760)  # the comma 4 narrow/wide frame the stage produces, the size the big model's pkl is keyed by

openpilot/selfdrive/modeld/reproject_c4/kernel.py

@@ -0,0 +1,90 @@
+"""The GPU stage: one gather per output byte through the tables, the composite blended in the seam band."""
+
+from tinygrad import Tensor, TinyJit
+import numpy as np
+
+from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
+from .geometry import UV_FILL
+from .meter import SeamMeter
+from .tables import ALPHA_SHIFT, IDX_BITS, INVALID_BIT
+
+
+class Reprojector:
+  """Holds the tables `T` (load_tables) on `device` and runs the gathers. Call once per model run with the two 3X NV12 buffers."""
+
+  def __init__(self, T, dst_wh, device):
+    self.stride, yh, uvh, _ = get_nv12_info(*dst_wh)
+    self.dw, self.dh = dst_wh
+    self.uv_offset, self.body = self.stride * yh, self.stride * (yh + uvh)
+    assert self.body == 3 * (self.body - self.uv_offset)  # NV12: the UV plane is the last third of the body
+    self.device = device
+    self.reload(T)
+    # per-frame match parameters as SeamMeter.update() returns them; on QCOM the jit reads them straight from host memory
+    # (a per-call upload costs a scheduled copy, ~4 ms of Python)
+    self.params_np = SeamMeter.IDENTITY.copy()
+    if str(device or "").startswith("QCOM"):
+      self.gains = Tensor.from_blob(self.params_np.ctypes.data, self.params_np.shape, dtype="float32", device=device)
+      self.host_params = True
+    else:
+      self.gains = Tensor(self.params_np, device=device).contiguous().realize()
+      self.host_params = False
+    self.plane = Tensor([0, 0, 1], dtype="uint8", device=device).realize()
+    # pixel position for the gain gradient from the byte index (x = i % stride, y = i // stride): a plain sequential read;
+    # broadcast row/column vectors cost 4 ms on the Adreno, per-pixel gathers 1.5 ms
+    self.idx = Tensor.arange(self.body, dtype="int32").to(device).realize()
+    self.dst = None
+    self._run = TinyJit(self._both)
+
+  def bind(self, host_wide: np.ndarray, host_narrow: np.ndarray):
+    """Write the outputs straight into these host arrays (each `body` bytes, e.g. modeld's packed frame slots) instead of
+    returning device tensors: a GPU->host readback costs ~40 ms on the 3X, a direct write ~10 ms."""
+    assert host_wide.nbytes == self.body and host_narrow.nbytes == self.body and host_wide.dtype == host_narrow.dtype == np.uint8
+    self.dst = (Tensor.from_blob(host_wide.ctypes.data, (self.body,), dtype="uint8", device=self.device),
+                Tensor.from_blob(host_narrow.ctypes.data, (self.body,), dtype="uint8", device=self.device))
+    self._run = TinyJit(self._both)
+
+  def reload(self, T):
+    """Swap in the tables of another rotation between runs: they are jit inputs, so one upload and no re-capture (~1.3 s on the 3X)."""
+    assert len(T["wide"]["pw"]) == self.body
+    self.t = {cam: {k: Tensor(v, device=self.device).realize() for k, v in T[cam].items()} for cam in ("wide", "narrow")}
+
+  def _chroma(self):
+    return self.plane.reshape(3, 1).expand(3, self.body // 3).reshape(self.body).bool()  # a broadcast: no table read
+
+  def _wide(self, wide, pw):
+    return (pw < INVALID_BIT).where(wide[pw & IDX_BITS], self._chroma().cast("uint8") * UV_FILL)
+
+  def _narrow(self, wide, narrow, gains, pw, pn):
+    chroma = self._chroma()
+    # match the wide surround to the narrow: luma times a gain per brightness band (the two ISPs' tone curves; piecewise
+    # linear between the band centers, flat beyond) times a gain gradient over the frame (lens shading); chroma gain about
+    # the 128 midpoint plus a U/V offset (independent white balance), the offset a parity broadcast: U and V bytes alternate
+    off = gains[1:3].reshape(1, 2).expand(self.body // 2, 2).reshape(self.body)
+    w = wide[pw & IDX_BITS].float()
+    c = [0.5 * (lo + hi) for lo, hi in SeamMeter.BANDS]
+    tone = gains[5]
+    for i in range(1, len(c)):
+      tone = tone + (gains[5 + i] - gains[4 + i]) * ((w - c[i - 1]) * (1 / (c[i] - c[i - 1]))).clip(0, 1)
+    x = (self.idx % self.stride).float() * (2.0 / self.dw) - 1
+    y = (self.idx // self.stride).float() * (2.0 / self.dh) - 1
+    w = chroma.where((w - UV_FILL) * gains[0] + UV_FILL + off, w * tone * (1 + gains[3] * x + gains[4] * y)).clip(0, 255)
+    w = (pw < INVALID_BIT).where(w, chroma.cast("float32") * UV_FILL)
+    a = ((pw >> ALPHA_SHIFT) & 0xff).float() * (1 / 255)
+    # the mask looks redundant but bounds the index; without it every gather is guarded (0.8 ms on the 3X)
+    return a * narrow[pn & IDX_BITS].float() + (1 - a) * w
+
+  def _both(self, wide, narrow, gains, pw_w, pw_n, pn):
+    out_w = self._wide(wide, pw_w)
+    out_n = self._narrow(wide, narrow, gains, pw_n, pn).round().cast("uint8")
+    if self.dst is not None:
+      out_w, out_n = self.dst[0].assign(out_w), self.dst[1].assign(out_n)
+    return out_w.realize(), out_n.realize()
+
+  def __call__(self, wide, narrow, match=None):
+    """wide/narrow: flat uint8 3X NV12 tensors (the same buffers every call, e.g. the VisionIPC ring, so the jit replays).
+    match: the seam match vector from SeamMeter.update() (None = no match). Returns (c4_wide, c4_narrow): flat uint8 NV12
+    tensors of `body` bytes; with bind() they are views of the bound host arrays."""
+    self.params_np[:] = SeamMeter.IDENTITY if match is None else match
+    if not self.host_params:
+      self.gains.assign(Tensor(self.params_np, device=self.device)).realize()
+    return self._run(wide, narrow, self.gains, self.t["wide"]["pw"], self.t["narrow"]["pw"], self.t["narrow"]["pn"])

openpilot/selfdrive/modeld/reproject_c4/meter.py

@@ -0,0 +1,127 @@
+"""The seam exposure match: the wide surround's luma, tone bands, shading gradient and color offsets against the narrow inset,
+measured live in the seam ring, on top of the exposure model from the two camera states."""
+
+import numpy as np
+
+from .geometry import UV_FILL
+
+
+# encoded-domain luma ratio narrow/wide vs the sensors' exposure ratio (gain*integLines), fitted in the seam ring on a
+# day-to-night drive (1607 frame pairs, 4 % residual): the exponent is the ISP tone curve, so a plain constant can't fit
+# both dusk and night; the ring (not the whole overlap) because the wide lens falls off towards the seam
+EXPOSURE_GAIN_A, EXPOSURE_GAIN_P = 0.868, 0.708
+
+def exposure_gain(narrow_exposure, wide_exposure, lo=0.25, hi=4.0):
+  """Gain to apply to the 3X wide surround so it matches the narrow inset; exposures are gain*integLines from the camera states."""
+  if not (narrow_exposure > 0 and wide_exposure > 0):
+    return 1.0
+  return float(np.clip(EXPOSURE_GAIN_A * (narrow_exposure / wide_exposure) ** EXPOSURE_GAIN_P, lo, hi))
+
+class SeamMeter:
+  """Measures the narrow-inset / wide-surround match in the composite's seam ring straight from the two source NV12 buffers
+  (host memory, a few thousand nearest-neighbor pixel pairs). The two cameras expose and white-balance independently and
+  their constant differs per unit (fleet: +-8 % luma, +-2 U/V steps), so the match is measured live and low-pass filtered;
+  the exposure model is the fallback when the ring is too dark/saturated. Beyond one luma gain: the wide lens's shading is
+  uneven per unit (25 % side to side on some), measured as a gain gradient across the frame; and the two ISPs' tone curves
+  differ (+-10 Y steps between shadows and highlights after the best single gain), measured as a gain per brightness band
+  and applied as a lookup on the surround luma."""
+  BANDS = ((16, 50), (50, 100), (100, 160), (160, 235))
+  IDENTITY = np.array([1, 0, 0, 0, 0] + [1] * len(BANDS), np.float32)  # [gain, u_off, v_off, gx, gy, band gains]: no match
+
+  def __init__(self, geometry, alpha=0.3, every=2):
+    """The luma gains are filtered as a correction on top of the exposure model (from the camera states) and applied times
+    the model's live value, so a jump in either camera's exposure is followed the same frame.
+    geometry: the pixel pairs from `geometry`, kept with the tables."""
+    self.y_w, self.y_n, self.pos, self.cell, self.uv_w, self.uv_n = (geometry[k] for k in ("y_w", "y_n", "pos", "cell", "uv_w", "uv_n"))
+    self.cell_bad = np.zeros(32 * 18, np.float32)
+    self.CELL_ALPHA, self.CELL_LIMIT = 0.02, 0.2
+    self.alpha, self.every, self.n_calls, self.moving = alpha, every, 0, False  # measuring every frame is ~0.7 ms of CPU on a PC
+    self.state = None  # filtered [gain_y, u_off, v_off, gx, gy, band gains...]
+
+  @staticmethod
+  def geometry(dw, dh, luma, chroma, n_pairs=2048, ring=(30.0, 130.0)) -> dict:
+    """The NV12 byte indices of the sampled wide/narrow pairs in the seam ring, their comma 4 positions and ring cells, from
+    build_tables' (wide index, wide valid, narrow index, narrow valid, distance to the inset edge) of the two planes."""
+    iw, vw, inn, vn, dist = luma
+    in_ring = vw & vn & (dist > ring[0]) & (dist < ring[1])
+    sel = np.flatnonzero(in_ring)[:: max(1, int(in_ring.sum()) // n_pairs)][:n_pairs]
+    g = {"y_w": iw.ravel()[sel], "y_n": inn.ravel()[sel]}
+    g["pos"] = np.stack([(sel % dw + 0.5) / dw * 2 - 1, (sel // dw + 0.5) / dh * 2 - 1], 1).astype(np.float32)  # c4 pixel, -1..1
+    # the ring in 32x18 cells: a cell whose pairs persistently disagree with the fit (an obstruction on one lens, dirt, a
+    # wiper) is excluded from the fit rather than dragged along; passing objects average out before they count
+    cell = (np.floor((g["pos"] + 1) / 2 * [32, 18])).astype(int)
+    g["cell"] = cell[:, 0] * 18 + cell[:, 1]
+    iw2, vw2, inn2, vn2, _ = chroma
+    in_ring2 = in_ring[::2, ::2] & vw2 & vn2
+    sel2 = np.flatnonzero(in_ring2)[:: max(1, int(in_ring2.sum()) // (n_pairs // 2))][: n_pairs // 2]
+    g["uv_w"], g["uv_n"] = iw2.ravel()[sel2], inn2.ravel()[sel2]  # U byte; V is the next one
+    return g
+
+  def measure(self, wide, narrow):
+    """One frame's raw state vector, or None when fewer than 256 usable luma pairs (night, glare)."""
+    yw = wide[self.y_w].astype(np.float32)
+    yn = narrow[self.y_n].astype(np.float32)
+    good = (yw > 16) & (yw < 235) & (yn > 16) & (yn < 235)
+    if good.sum() < 256:
+      return None
+    yw, yn, pos = yw[good], yn[good], self.pos[good]
+    ratio = yn / yw
+    gy = float(np.median(ratio))
+    # tone (a gain per brightness band of the wide) and lens shading (a gain gradient over the frame) are confounded in
+    # one frame - the sky is always at the top of the ring - so they are fitted jointly: log ratio = band gain + gx*x + gy*y
+    band = np.searchsorted([hi for _, hi in self.BANDS[:-1]], yw)
+    A = np.zeros((len(yw), len(self.BANDS) + 2), np.float32)
+    A[np.arange(len(yw)), band] = 1
+    A[:, -2:] = pos
+    lr = np.log(np.clip(ratio, 0.25, 4.0))
+    cells = self.cell[good]
+    w = (self.cell_bad[cells] < self.CELL_LIMIT).astype(np.float32)
+    if w.sum() < 256:
+      w[:] = 1  # too much excluded: fit everything rather than nothing
+    # weighted least squares by the normal equations (6x6, ridge 1e-3 so an empty band stays solvable): the general solver
+    # costs 1.3 ms per call on the device CPU, this ~0.1 ms
+    def solve(wt):
+      Aw = A * wt[:, None]
+      return np.linalg.solve(A.T @ Aw + 1e-3 * np.eye(A.shape[1], dtype=np.float32), Aw.T @ lr)
+    c = solve(w)
+    res = lr - A @ c
+    wr = w * np.clip(1 - (res / 0.3) ** 2, 0, 1) ** 2  # reweight once: a pair's pull falls off with its residual (Tukey-like)
+    if wr.sum() >= 256:
+      c = solve(wr)
+    n = np.bincount(cells, minlength=len(self.cell_bad))
+    bad = np.bincount(cells, weights=np.abs(lr - A @ c), minlength=len(self.cell_bad))
+    seen = n > 0
+    self.cell_bad[seen] += self.CELL_ALPHA * (bad[seen] / n[seen] - self.cell_bad[seen])
+    counts = np.bincount(band, minlength=len(self.BANDS))
+    bands = [float(np.exp(c[i])) if counts[i] >= 100 else gy for i in range(len(self.BANDS))]
+    gx, gyp = float(np.clip(c[-2], -0.5, 0.5)), float(np.clip(c[-1], -0.5, 0.5))
+    uw, un = wide[self.uv_w].astype(np.float32), narrow[self.uv_n].astype(np.float32)
+    vw, vn = wide[self.uv_w + 1].astype(np.float32), narrow[self.uv_n + 1].astype(np.float32)
+    du, dv = float(np.median(un - UV_FILL - gy * (uw - UV_FILL))), float(np.median(vn - UV_FILL - gy * (vw - UV_FILL)))
+    return np.array([gy, du, dv, gx, gyp, *bands], np.float32)
+
+  def update(self, wide, narrow, model_gain=1.0):
+    """Filtered match for this frame as the Reprojector's parameter vector; measured when the ring is usable, else the luma
+    gains decay to the exposure model (a flat curve) while the color offsets and the shading gradient hold: those are
+    per-unit constants, and the ring is unmeasurable for minutes at a time at night."""
+    self.n_calls += 1
+    if self.state is None or self.moving or self.n_calls % self.every == 0:
+      target = self.measure(wide, narrow)
+      if target is None:
+        held = self.state[1:5] if self.state is not None else (0, 0, 0, 0)
+        target = np.array([model_gain, *held] + [model_gain] * len(self.BANDS), np.float32)
+      target = target.copy()
+      target[0] /= model_gain
+      target[5:] /= model_gain  # the luma entries are kept relative to the exposure model
+      if self.state is None:
+        self.state = target
+      else:
+        # the two auto-exposures diverge for a second at a time (one brightens while the other darkens): each measurement
+        # is a median over thousands of pairs, so when the match moves, follow it almost at once and measure every frame
+        jump = abs(float(target[0] - self.state[0])) / max(float(self.state[0]), 0.1)
+        self.state += min(self.alpha + 3 * jump, 0.9) * (target - self.state)
+        self.moving = jump > 0.03
+    s = self.state.copy()
+    s[0] *= model_gain
+    s[5:] *= model_gain
+    return s

openpilot/selfdrive/modeld/reproject_c4/rotation.py

@@ -0,0 +1,45 @@
+"""The unit's fitted narrow->wide rotation, kept across boots in /data."""
+import json
+import os
+from pathlib import Path
+
+import numpy as np
+
+from .geometry import POP_ROTATION, rotvec_from_wide_from_device_euler
+
+ROTATION_PATH = Path('/data/reproject_rotation.json')
+
+
+def read_rotation() -> dict:
+  """{'rotvec', ...} as the fitter/reprojector saved it, or {} when not fitted."""
+  try:
+    d = json.loads(ROTATION_PATH.read_text(encoding='utf-8'))
+    if len(d['rotvec']) == 3 and np.isfinite(d['rotvec']).all():
+      return d
+  except (OSError, KeyError, TypeError, ValueError, json.JSONDecodeError):
+    pass
+  return {}
+
+
+def load_rotation():
+  """The fitted value, else the stock calibration seed, else the fleet median."""
+  d = read_rotation()
+  if d:
+    return tuple(float(v) for v in d['rotvec'])
+  try:
+    from openpilot.cereal import log
+    from openpilot.common.params import Params
+    with log.Event.from_bytes(Params().get('CalibrationParams')) as msg:
+      e = list(msg.extrinsicsCalibration.wideFromDeviceEuler)
+    if len(e) == 3 and np.isfinite(e).all():
+      return rotvec_from_wide_from_device_euler(e)
+  except Exception:
+    pass
+  return POP_ROTATION
+
+
+def save_rotation(rotvec, **extra) -> None:
+  data = {'rotvec': [float(v) for v in rotvec], **extra}
+  tmp = ROTATION_PATH.with_name(f'.{ROTATION_PATH.name}.tmp')
+  tmp.write_text(json.dumps(data), encoding='utf-8')
+  os.replace(tmp, ROTATION_PATH)

openpilot/selfdrive/modeld/reproject_c4/tables.py

@@ -0,0 +1,99 @@
+"""The gather tables that map every comma 4 output byte to a 3X source byte, built once per rotation and cached on disk."""
+import contextlib
+import glob
+import hashlib
+import json
+import os
+
+import numpy as np
+
+from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
+from .geometry import C4_NARROW_K, FEATHER_PX, _nv12_index, sample_coords
+from .meter import SeamMeter
+
+
+IDX_BITS, ALPHA_SHIFT, INVALID_BIT = 0x3fffff, 22, 1 << 30  # one int32 per output byte: wide index | alpha << 22 | invalid
+
+def build_tables(src_wh, dst_wh, calib):
+  """Flat gather tables over the destination NV12 body (comma 4 layout). Byte b of the wide output = src[pw[b]] (the fill
+  value where invalid); of the narrow composite, alpha[b]*narrow[pn[b]] + (1-alpha[b])*gain*wide[pw[b]]. The kernel is
+  memory-bound, so the wide index, the blend weight and the validity share one int32 (`pw`)."""
+  sw, sh = src_wh
+  dw, dh = dst_wh
+  s_stride, s_yh, s_uvh, _ = get_nv12_info(sw, sh)
+  s_uv = s_stride * s_yh
+  assert s_stride * (s_yh + s_uvh) <= IDX_BITS + 1
+  d_stride, d_yh, d_uvh, _ = get_nv12_info(dw, dh)
+  d_uv = d_stride * d_yh
+  n_body = d_stride * (d_yh + d_uvh)
+  sc = calib["narrow"]["f"] / C4_NARROW_K[0, 0]  # 3X narrow px per comma 4 narrow px
+  out = {}
+  planes = {}
+  for cam in ("wide", "narrow"):
+    idx_w = np.zeros(n_body, np.int64)
+    idx_n = np.zeros(n_body, np.int64)
+    val_w = np.zeros(n_body, bool)
+    val_n = np.zeros(n_body, bool)
+    dist = np.zeros(n_body, np.float32)
+    for chroma in (False, True):
+      s = 0.5 if chroma else 1.0
+      mw, mn = sample_coords(cam, dw // 2 if chroma else dw, dh // 2 if chroma else dh, s, calib)
+      iw, vw = _nv12_index(mw, sw, sh, s_stride, s_uv, chroma)
+      inn, vn = _nv12_index(mn, sw, sh, s_stride, s_uv, chroma)
+      # distance to the narrow frame edge, in destination px: the composite feathers over FEATHER_PX of it
+      d = np.minimum(np.minimum(mn[..., 0], sw * s - mn[..., 0]), np.minimum(mn[..., 1], sh * s - mn[..., 1])) / (s * sc)
+      if cam == "narrow":
+        planes[chroma] = (iw, vw, inn, vn, d)
+      if chroma:
+        rows = np.arange(dh // 2)[:, None]
+        cols = np.arange(dw // 2)[None, :]
+        for plane in (0, 1):
+          flat = d_uv + rows * d_stride + 2 * cols + plane
+          idx_w[flat] = iw + plane
+          idx_n[flat] = inn + plane
+          val_w[flat] = vw
+          val_n[flat] = vn
+          dist[flat] = d
+      else:
+        flat = np.arange(dh)[:, None] * d_stride + np.arange(dw)[None, :]
+        idx_w[flat] = iw
+        idx_n[flat] = inn
+        val_w[flat] = vw
+        val_n[flat] = vn
+        dist[flat] = d
+    alpha = np.round(np.clip(dist / FEATHER_PX, 0, 1) * val_n * 255).astype(np.int64)
+    out[cam] = {"pw": (idx_w | (alpha << ALPHA_SHIFT) | (~val_w * INVALID_BIT)).astype(np.int32)}
+  out["narrow"]["pn"] = idx_n.astype(np.int32)
+  out["meter"] = SeamMeter.geometry(dw, dh, planes[False], planes[True])  # ~1 s of numpy on the 3X: not for modeld's swap
+  return out
+
+TABLE_VERSION = 2  # bump when the lens numbers, the feather or the table layout change
+
+def table_path(src_wh, dst_wh, cache_dir, calib):
+  tag = hashlib.sha1(json.dumps(calib, sort_keys=True, default=float).encode()).hexdigest()[:10]
+  return os.path.join(cache_dir, f"reproject_c4_v{TABLE_VERSION}_{src_wh[0]}x{src_wh[1]}_{dst_wh[0]}x{dst_wh[1]}_{tag}.npz")
+
+def load_tables(src_wh, dst_wh, cache_dir, calib):
+  """build_tables takes 5-8 s of numpy on the 3X CPU, so keep a copy on disk (19 MB, uncompressed: zlib was half the build on the 3X)."""
+  os.makedirs(cache_dir, exist_ok=True)
+  p = table_path(src_wh, dst_wh, cache_dir, calib)
+  if os.path.exists(p):
+    z = np.load(p)
+    T = {"wide": {}, "narrow": {}, "meter": {}}
+    for key in z.files:
+      cam, k = key.split("_", 1)
+      T[cam][k] = z[key]
+    return T
+  T = build_tables(src_wh, dst_wh, calib)
+  np.savez(p + ".tmp.npz", **{f"{cam}_{k}": v for cam, tab in T.items() for k, v in tab.items()})
+  os.replace(p + ".tmp.npz", p)
+  for f in sorted(glob.glob(os.path.join(cache_dir, "reproject_c4_*.npz")), key=os.path.getmtime)[:-4]:  # every refit leaves a file
+    with contextlib.suppress(OSError):
+      os.remove(f)
+  return T
+
+def table_cache_dir() -> str:
+  """Where the gather tables are kept: the device's data partition (its home is wiped at boot), the openpilot home on a PC."""
+  from openpilot.common.hardware import PC
+  from openpilot.common.hardware.hw import Paths
+  return os.path.join(Paths.comma_home(), 'reproject_c4') if PC else '/data/reproject_c4'

openpilot/selfdrive/modeld/reprojectcalibd.py

@@ -0,0 +1,152 @@
+#!/usr/bin/env python3
+"""Fits this unit's narrow->wide camera rotation for the 3X->comma 4 reprojection stage: a one-off, the median of twelve
+frame pairs taken under calibrationd's own conditions (straight road, above 15 mph), about one frame a second.
+The tables are built here and the result is published as ready; reprojectd swaps it in and keeps it in ReprojectRotation,
+and only then is it published as fitted (calibrationd holds until then, so the car cannot be engaged around the swap);
+afterwards this process idles. The rotation
+is board geometry: it is fitted once and only a calibration reset, which clears it, fits it again. Numpy only, low
+priority, one frame pair every few seconds."""
+import json
+import os
+import time
+
+import numpy as np
+
+import openpilot.cereal.messaging as messaging
+from openpilot.cereal.visionipc import VisionStreamType
+from msgq.visionipc import VisionIpcClient
+from openpilot.common.params import Params
+from openpilot.common.swaglog import cloudlog
+from openpilot.selfdrive.locationd.calibrationd import MIN_SPEED_FILTER, MAX_YAW_RATE_FILTER
+from openpilot.selfdrive.modeld import reproject_c4 as RC
+from openpilot.selfdrive.modeld.reproject_c4 import N_FRAMES, Fit
+
+C4_CAM = RC.C4_CAM
+NARROW, WIDE = VisionStreamType.VISION_STREAM_NARROW_ROAD, VisionStreamType.VISION_STREAM_WIDE_ROAD
+CALIB_PATH = '/data/reproject_calibration.json'
+
+
+def luma(buf) -> np.ndarray:
+  return np.array(buf.data[:buf.uv_offset], dtype=np.uint8).reshape(-1, buf.stride)[:buf.height, :buf.width]
+
+
+class FitState:
+  """The fit state written atomically for reprojectd and calibrationd."""
+  def __init__(self):
+    self.last = None
+    self.t = 0.0
+
+  def publish(self, status: str, fit: Fit, why: str | None = None, target: tuple = ()) -> None:
+    pct = 100 if status in ('building', 'ready', 'fitted') else 100 * fit.n // N_FRAMES
+    mean = [float(v) for v in (fit.applied if status == 'fitted' else fit.mean)]
+    data = {'status': status, 'why': why or 'none', 'pct': pct, 'mean': mean,
+            'lastFrameId': int(fit.last_id), 'lastAccepted': bool(fit.last_ok),
+            'applied': [float(v) for v in fit.applied], 'target': [float(v) for v in target]}
+    key = json.dumps(data, sort_keys=True)
+    if key == self.last and time.monotonic() - self.t < 0.5:
+      return
+    self.last, self.t = key, time.monotonic()
+    tmp = CALIB_PATH + '.tmp'
+    with open(tmp, 'w', encoding='utf-8') as f:
+      json.dump(data, f)
+    os.replace(tmp, CALIB_PATH)
+
+
+def main():
+  os.nice(10)
+  sm = messaging.SubMaster(['carState', 'extrinsicsCalibration', 'cameraOdometry'])
+  narrow = VisionIpcClient("camerad", NARROW, True)
+  wide = VisionIpcClient("camerad", WIDE, False)
+  params = Params()
+  state = FitState()
+  fit = Fit(RC.load_rotation())  # the rotation reprojectd applies: the same param, else the same seed
+  fitted = bool(RC.read_rotation())
+  if fitted and params.get("CalibrationParams") is None:
+    # a calibration reset from outside the ui (which clears the rotation too) cleared the calibration only: fit again first
+    cloudlog.warning("reprojectcalibd: calibration was reset: refitting the rotation")
+    fitted = False
+  cloudlog.warning(f"reprojectcalibd: applied rotation {np.degrees(fit.applied).round(3)} deg, {'fitted' if fitted else 'not fitted yet'}")
+
+  def done(final) -> None:
+    while True:
+      state.publish('fitted', Fit(final))
+      time.sleep(0.5)
+
+  if fitted:
+    done(fit.applied)
+  state.publish('waiting', fit)
+  why = None
+  why_t = log_t = time.monotonic()
+
+  def waiting(reason: str | None) -> None:
+    """What the fit is waiting for (cameras, model, speed, straight road, a matched frame pair), for the bar and the tile;
+    a stretch of it longer than 5 s is logged, and one that never ends every 30 s, so a stalled bar explains itself in the rlog."""
+    nonlocal why, why_t, log_t
+    now = time.monotonic()
+    if reason != why:
+      if why is not None and now - why_t > 5.0:
+        cloudlog.warning(f"reprojectcalibd: waited {now - why_t:.0f} s for {why} at {fit.n} fits")
+      why, why_t, log_t = reason, now, now
+    elif reason is not None and now - log_t > 30.0:
+      cloudlog.warning(f"reprojectcalibd: still waiting for {reason} after {now - why_t:.0f} s at {fit.n} fits")
+      log_t = now
+    state.publish('fitting' if fit.fits else 'waiting', fit, why=reason)
+
+  while True:
+    sm.update(100)
+    if not (narrow.is_connected() and wide.is_connected()):
+      narrow.connect(False)
+      wide.connect(False)
+      waiting('cameras')
+      continue
+    if not (sm.alive['extrinsicsCalibration'] and sm.valid['extrinsicsCalibration']):
+      # calibrationd is valid once modeld publishes cameraOdometry: the model is still loading. alive+valid, not
+      # all_checks: its frequency check fails for a second after a 3.5 s coarse fit, which showed as 'Loading Model'
+      waiting('model')
+      continue
+    if not all(sm.alive[k] and sm.valid[k] for k in ('carState', 'cameraOdometry')):
+      waiting('model')
+      continue
+    if sm['carState'].vEgo <= MIN_SPEED_FILTER:
+      waiting('speed')
+      continue
+    if abs(sm['cameraOdometry'].rot[2]) >= MAX_YAW_RATE_FILTER:
+      waiting('straight')
+      continue
+    pair = RC.recv_pair(narrow, wide)
+    if pair is None:
+      waiting('pair')
+      continue
+    waiting(None)
+    bn, bw = pair
+    narrow_y, wide_y = luma(bn), luma(bw)
+    t0 = time.monotonic()
+    r = fit.frame(narrow_y, wide_y, narrow.frame_id)
+    if r is None:
+      cloudlog.warning(f"reprojectcalibd: frame {narrow.frame_id} rejected, {time.monotonic() - t0:.1f} s")
+      # too few patches with texture (dusk, plain road): the bar says so instead of sitting at the same number
+      state.publish('fitting' if fit.fits else 'waiting', fit, why='features')
+      continue
+    cloudlog.warning(f"reprojectcalibd: fit {fit.n}: {np.degrees(r[0]).round(3)} deg, {r[1]} matches, rms {r[2]:.3f} deg, {time.monotonic() - t0:.1f} s; "
+                     + f"median {np.degrees(fit.mean).round(3)} spread {fit.spread:.3f} deg")
+    state.publish('fitting', fit)
+    if fit.n < N_FRAMES:
+      continue
+    final = tuple(float(v) for v in fit.mean)
+    # build the tables here (~6 s of numpy at low priority) so reprojectd's swap is just a load; then publish the fit
+    t0 = time.monotonic()
+    state.publish('building', fit, target=final)
+    RC.load_tables((bn.width, bn.height), C4_CAM, RC.table_cache_dir(), RC.calib_from_rotvec(final))
+    cloudlog.warning(f"reprojectcalibd: tables built in {time.monotonic() - t0:.0f} s")
+    # calibrationd held at uncalibrated through the fit and calibrates afresh after the swap, but first writes that ~25 s
+    # in: the calibration on disk is from other frames (stock, or another rotation) and must not outlive a reboot before then
+    params.remove("CalibrationParams")
+    while RC.read_rotation().get("rotvec") != list(final):  # reprojectd saves it once swapped in
+      state.publish('ready', fit, target=final)
+      time.sleep(0.1)
+    cloudlog.warning(f"reprojectcalibd: rotation fitted {np.degrees(final).round(3)} deg (was {np.degrees(fit.applied).round(3)}, spread {fit.spread:.3f} deg)")
+    done(final)
+
+
+if __name__ == "__main__":
+  main()

openpilot/selfdrive/modeld/reprojectd.py

@@ -0,0 +1,162 @@
+#!/usr/bin/env python3
+"""The 3X's cameras as a comma 4's. Reprojects camerad's narrow and wide frames into the comma 4 geometry on the QCOM GPU
+(reproject_c4: the wide as the surround, the narrow as the sharp inset) and serves the pair as VisionIPC streams "reproject"
+(narrow = the composite, wide), with camerad's frame ids and timestamps, so modeld runs comma's big model on them as it would
+on a mici. The seam exposure meter lives here, and a rotation reprojectcalibd publishes as ready is swapped in here. It
+publishes nothing: reprojectCalibration carries the rotation, and the stage's timing goes to the log as a summary a minute
+(reprojectd.stage)."""
+import os
+os.environ.setdefault('QCOM_PRIORITY', '1')  # KGSL context priority: the stage preempts the driver-monitoring model's 20 ms
+os.environ.setdefault('DEV', 'QCOM')  # tinygrad's default device: never probe the eGPU, modeld owns it (its USB lock)
+import json
+import threading
+import time
+
+import numpy as np
+from tinygrad import Tensor, Device
+
+from openpilot.cereal import messaging
+from openpilot.cereal.visionipc import VisionStreamType
+from msgq.visionipc import VisionIpcClient, VisionIpcServer
+from openpilot.common.realtime import config_realtime_process
+from openpilot.common.swaglog import cloudlog
+from openpilot.selfdrive.modeld import reproject_c4 as RC
+from openpilot.selfdrive.modeld.reproject_c4.kernel import Reprojector
+from openpilot.system.camerad.cameras.nv12_info import get_nv12_info
+
+C4_CAM = RC.C4_CAM
+NARROW, WIDE = VisionStreamType.VISION_STREAM_NARROW_ROAD, VisionStreamType.VISION_STREAM_WIDE_ROAD
+CALIB_PATH = '/data/reproject_calibration.json'
+
+
+def read_calibration():
+  try:
+    with open(CALIB_PATH, encoding='utf-8') as f:
+      return json.load(f)
+  except (OSError, ValueError, TypeError):
+    return None
+
+
+class Stage:
+  def __init__(self, src_wh: tuple[int, int]):
+    self.src_wh, self.cache_dir = src_wh, RC.table_cache_dir()
+    self.src_size = get_nv12_info(*src_wh)[3]
+    self.rotation = RC.load_rotation()
+    self.fitted = bool(RC.read_rotation())
+    cloudlog.warning(f"reprojectd: rotation {np.degrees(self.rotation).round(3)} deg, "
+                     + ('fitted' if self.fitted else 'seeded from CalibrationParams / the fleet median'))
+    self.calib = RC.calib_from_rotvec(self.rotation)
+    T = RC.load_tables(src_wh, C4_CAM, self.cache_dir, self.calib)
+    self.rp = Reprojector(T, C4_CAM, 'QCOM')
+    self.meter = RC.SeamMeter(T["meter"])
+    self.pending = None
+    self.loader: threading.Thread | None = None
+    self.saver: threading.Thread | None = None
+    self._src_tensors: dict[int, Tensor] = {}
+    self.out = np.zeros((2, self.rp.body), np.uint8)  # composite, wide: the kernel writes here, the server copies into its ring
+    self.rp.bind(self.out[1], self.out[0])
+    self.time = 0.0
+
+  def src_tensor(self, buf) -> Tensor:
+    ptr = np.frombuffer(buf.data, dtype=np.uint8).ctypes.data
+    if ptr not in self._src_tensors:  # one mapping per VisionIPC ring slot
+      self._src_tensors[ptr] = Tensor.from_blob(ptr, (self.src_size,), dtype='uint8', device='QCOM')
+    return self._src_tensors[ptr]
+
+  def run(self, wide, narrow, match: np.ndarray) -> None:
+    t0 = time.perf_counter()
+    self.rp(self.src_tensor(wide), self.src_tensor(narrow), match)
+    Device['QCOM'].synchronize()
+    self.time = time.perf_counter() - t0
+
+  def follow_fit(self, fit) -> None:
+    """A rotation reprojectcalibd has published as ready (its tables already built) is loaded in a thread and swapped in
+    between frames (one upload), then kept in ReprojectRotation: that is what tells reprojectcalibd it is applied, so it
+    is saved again while reprojectcalibd still says ready. calibrationd holds meanwhile, so the car cannot be engaged
+    around the swap."""
+    if self.pending is not None:
+      T, calib, meter, rot = self.pending
+      self.pending = None
+      self.loader = None
+      t0 = time.perf_counter()
+      self.rp.reload(T)
+      self.calib, self.meter, self.rotation, self.fitted = calib, meter, rot, True
+      cloudlog.warning(f"reprojectd: fitted rotation {np.degrees(rot).round(3)} deg swapped in ({(time.perf_counter() - t0) * 1e3:.0f} ms)")
+      return
+    if self.loader is not None or fit is None or fit.get('status') != 'ready' or len(fit.get('target', [])) != 3:
+      return
+    rot = tuple(float(v) for v in fit['target'])
+    if rot == self.rotation:
+      if self.saver is None or not self.saver.is_alive():
+        def save():  # a param write, off the frame loop
+          os.sched_setscheduler(0, os.SCHED_OTHER, os.sched_param(0))  # inherited SCHED_FIFO otherwise
+          RC.save_rotation(rot)
+        self.saver = threading.Thread(target=save, daemon=True)
+        self.saver.start()
+      return
+    calib = RC.calib_from_rotvec(rot)
+    def load():  # one 19 MB read off the frame loop (a build, if the fitter's tables are gone)
+      os.sched_setscheduler(0, os.SCHED_OTHER, os.sched_param(0))
+      T = RC.load_tables(self.src_wh, C4_CAM, self.cache_dir, calib)
+      self.pending = (T, calib, RC.SeamMeter(T["meter"]), rot)
+    self.loader = threading.Thread(target=load, daemon=True)
+    self.loader.start()
+
+
+def main():
+  sm = messaging.SubMaster(['narrowRoadCameraState', 'wideRoadCameraState'])
+  narrow = VisionIpcClient("camerad", NARROW, True)
+  wide = VisionIpcClient("camerad", WIDE, False)
+  while not narrow.connect(False):
+    time.sleep(0.1)
+  while not wide.connect(False):
+    time.sleep(0.1)
+  src_wh = (narrow.width, narrow.height)
+  cloudlog.warning(f"reprojectd: cameras {src_wh[0]}x{src_wh[1]} -> comma 4 {C4_CAM[0]}x{C4_CAM[1]}")
+  t0 = time.monotonic()
+  stage = Stage(src_wh)
+  blank = [Tensor.zeros(stage.src_size, dtype='uint8', device='QCOM').contiguous().realize() for _ in range(2)]
+  for _ in range(3):  # jit capture before the first real frame
+    stage.rp(blank[0], blank[1])
+    Device['QCOM'].synchronize()
+  stride, y_height, _, _ = get_nv12_info(*C4_CAM)
+  server = VisionIpcServer("reproject")
+  for tp in (NARROW, WIDE):
+    server.create_buffers_with_sizes(tp, 4, C4_CAM[0], C4_CAM[1], stage.rp.body, stride, stride * y_height)
+  server.start_listener()
+  cloudlog.warning(f"reprojectd: serving after {time.monotonic() - t0:.1f} s")
+  # real-time only from here: the table load and the jit capture above are seconds of CPU, and a real-time task holding a
+  # core for a second trips the kernel's throttle, which this kernel turns into a panic (CONFIG_PANIC_ON_RT_THROTTLING)
+  config_realtime_process(6, 53)
+
+  n = 0
+  stage_ms: list[float] = []
+  summary_t = time.monotonic()
+  while True:
+    pair = RC.recv_pair(narrow, wide)
+    if pair is None:
+      continue
+    buf_n, buf_w = pair
+    sm.update(0)
+    # match the wide surround to the narrow inset: measured in the seam ring of these frames, the sensors' exposure
+    # settings as the fallback when the ring is unusable
+    ncs, wcs = sm['narrowRoadCameraState'], sm['wideRoadCameraState']
+    g = RC.exposure_gain(ncs.gain * ncs.integLines, wcs.gain * wcs.integLines) if sm.seen['narrowRoadCameraState'] and sm.seen['wideRoadCameraState'] else 1.0
+    match = stage.meter.update(np.frombuffer(buf_w.data, dtype=np.uint8), np.frombuffer(buf_n.data, dtype=np.uint8), g)
+    stage.run(buf_w, buf_n, match)
+    server.send(NARROW, stage.out[0], narrow.frame_id, narrow.timestamp_sof, narrow.timestamp_eof)
+    server.send(WIDE, stage.out[1], wide.frame_id, wide.timestamp_sof, wide.timestamp_eof)
+    n += 1
+    stage_ms.append(stage.time * 1e3)
+    if time.monotonic() - summary_t >= 60.0:
+      ms = np.array(stage_ms)
+      cloudlog.event("reprojectd.stage", frames=len(ms), p50_ms=round(float(np.percentile(ms, 50)), 2),
+                     p95_ms=round(float(np.percentile(ms, 95)), 2), max_ms=round(float(ms.max()), 2), over_8ms=int((ms > 8).sum()))
+      stage_ms.clear()
+      summary_t = time.monotonic()
+    if n % 20 == 0:
+      stage.follow_fit(read_calibration())
+
+
+if __name__ == "__main__":
+  main()

openpilot/sunnypilot/modeld_v2/camera_offset_helper.py

@@ -7,6 +7,7 @@ See the LICENSE.md file in the root directory for more details.
 import numpy as np
 
 from openpilot.common.transformations.camera import DEVICE_CAMERAS
+from openpilot.selfdrive.modeld.helpers import reproject_expected
 
 
 class CameraOffsetHelper:
@@ -36,7 +37,7 @@ class CameraOffsetHelper:
 
   def update(self, model_transform_main, model_transform_extra, sm, main_wide_camera):
     self.actual_camera_offset = (0.9 * self.actual_camera_offset) + (0.1 * self.camera_offset)
-    dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['narrowRoadCameraState'].sensor))]
+    dc = DEVICE_CAMERAS[("mici", "os04c10") if reproject_expected() else (str(sm['deviceState'].deviceType), str(sm['narrowRoadCameraState'].sensor))]
     height = sm["extrinsicsCalibration"].height[0] if sm['extrinsicsCalibration'].height else 1.22
     rpy_calib = sm['extrinsicsCalibration'].rpyCalib
 

openpilot/sunnypilot/modeld_v2/modeld.py

@@ -16,7 +16,7 @@ from setproctitle import setproctitle
 
 import openpilot.cereal.messaging as messaging
 from openpilot.common.hardware import COMMA_HARDWARE
-from openpilot.selfdrive.modeld.helpers import chestnut_present
+from openpilot.selfdrive.modeld.helpers import chestnut_present, reproject_expected
 from openpilot.cereal import log
 from opendbc.car.structs import car
 from openpilot.cereal.services import SERVICE_LIST
@@ -283,8 +283,9 @@ def main(demo=False):
   params.remove("ChestnutActive")
 
   # visionipc clients
+  camera_server = "reproject" if reproject_expected() else "camerad"
   while True:
-    available_streams = VisionIpcClient.available_streams("camerad", block=False)
+    available_streams = VisionIpcClient.available_streams(camera_server, block=False)
     if available_streams:
       use_extra_client = VisionStreamType.VISION_STREAM_WIDE_ROAD in available_streams and VisionStreamType.VISION_STREAM_NARROW_ROAD in available_streams
       main_wide_camera = VisionStreamType.VISION_STREAM_NARROW_ROAD not in available_streams
@@ -292,8 +293,8 @@ def main(demo=False):
     time.sleep(.1)
 
   vipc_client_main_stream = VisionStreamType.VISION_STREAM_WIDE_ROAD if main_wide_camera else VisionStreamType.VISION_STREAM_NARROW_ROAD
-  vipc_client_main = VisionIpcClient("camerad", vipc_client_main_stream, True)
-  vipc_client_extra = VisionIpcClient("camerad", VisionStreamType.VISION_STREAM_WIDE_ROAD, False)
+  vipc_client_main = VisionIpcClient(camera_server, vipc_client_main_stream, True)
+  vipc_client_extra = VisionIpcClient(camera_server, VisionStreamType.VISION_STREAM_WIDE_ROAD, False)
   cloudlog.warning(f"vision stream set up, main_wide_camera: {main_wide_camera}, use_extra_client: {use_extra_client}")
 
   while not vipc_client_main.connect(False):
@@ -414,7 +415,7 @@ def main(demo=False):
     lat_delay = model.lat_delay + model.LAT_SMOOTH_SECONDS
     if sm.updated["extrinsicsCalibration"] and sm.seen['narrowRoadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["extrinsicsCalibration"].rpyCalib, dtype=np.float32)
-      dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['narrowRoadCameraState'].sensor))]
+      dc = DEVICE_CAMERAS[("mici", "os04c10") if camera_server == "reproject" else (str(sm['deviceState'].deviceType), str(sm['narrowRoadCameraState'].sensor))]
       main_intrinsics = dc.wide_road.intrinsics if main_wide_camera else dc.narrow_road.intrinsics
       model_transform_main = get_warp_matrix(device_from_calib_euler, main_intrinsics, False).astype(np.float32)
       model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.wide_road.intrinsics, True).astype(np.float32)

openpilot/system/manager/process_config.py

@@ -6,6 +6,7 @@ from opendbc.car.structs import car
 from openpilot.cereal import custom
 from openpilot.common.params import Params
 from openpilot.common.hardware import PC, COMMA_HARDWARE
+from openpilot.selfdrive.modeld.helpers import reproject_expected
 from openpilot.system.manager.process import PythonProcess, NativeProcess, DaemonProcess
 from openpilot.common.hardware.hw import Paths
 
@@ -65,6 +66,9 @@ def only_onroad(started: bool, params: Params, CP: car.CarParams) -> bool:
 def only_offroad(started: bool, params: Params, CP: car.CarParams) -> bool:
   return not started
 
+def reproject(started: bool, params: Params, CP: car.CarParams) -> bool:
+  return started and reproject_expected()
+
 def livestream(started: bool, params: Params, CP: car.CarParams) -> bool:
   return params.get_bool("IsLiveStreaming")
 
@@ -172,6 +176,8 @@ procs = [
 procs += [
   # Models
   PythonProcess("models_manager", "openpilot.sunnypilot.models.manager", always_run),
+  PythonProcess("reprojectd", "openpilot.selfdrive.modeld.reprojectd", reproject),
+  PythonProcess("reprojectcalibd", "openpilot.selfdrive.modeld.reprojectcalibd", reproject),
   NativeProcess("modeld_tinygrad", "openpilot/sunnypilot/modeld_v2", ["./modeld"], and_(only_onroad, is_tinygrad_model)),
 
   # Backup