diff --git a/openpilot/selfdrive/locationd/calibrationd.py b/openpilot/selfdrive/locationd/calibrationd.py index ec9a22563..ccafb588b 100755 --- a/openpilot/selfdrive/locationd/calibrationd.py +++ b/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, diff --git a/openpilot/selfdrive/modeld/helpers.py b/openpilot/selfdrive/modeld/helpers.py index 5e7f413c2..24e830103 100644 --- a/openpilot/selfdrive/modeld/helpers.py +++ b/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() diff --git a/openpilot/selfdrive/modeld/reproject_c4/__init__.py b/openpilot/selfdrive/modeld/reproject_c4/__init__.py new file mode 100644 index 000000000..eefc8b4da --- /dev/null +++ b/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 diff --git a/openpilot/selfdrive/modeld/reproject_c4/cameras.py b/openpilot/selfdrive/modeld/reproject_c4/cameras.py new file mode 100644 index 000000000..64d96d998 --- /dev/null +++ b/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 diff --git a/openpilot/selfdrive/modeld/reproject_c4/fit.py b/openpilot/selfdrive/modeld/reproject_c4/fit.py new file mode 100644 index 000000000..dce69f440 --- /dev/null +++ b/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 diff --git a/openpilot/selfdrive/modeld/reproject_c4/geometry.py b/openpilot/selfdrive/modeld/reproject_c4/geometry.py new file mode 100644 index 000000000..8ab5a7eb8 --- /dev/null +++ b/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 diff --git a/openpilot/selfdrive/modeld/reproject_c4/kernel.py b/openpilot/selfdrive/modeld/reproject_c4/kernel.py new file mode 100644 index 000000000..cacee6c53 --- /dev/null +++ b/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"]) diff --git a/openpilot/selfdrive/modeld/reproject_c4/meter.py b/openpilot/selfdrive/modeld/reproject_c4/meter.py new file mode 100644 index 000000000..39abdc163 --- /dev/null +++ b/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 diff --git a/openpilot/selfdrive/modeld/reproject_c4/rotation.py b/openpilot/selfdrive/modeld/reproject_c4/rotation.py new file mode 100644 index 000000000..71a57e4d2 --- /dev/null +++ b/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) diff --git a/openpilot/selfdrive/modeld/reproject_c4/tables.py b/openpilot/selfdrive/modeld/reproject_c4/tables.py new file mode 100644 index 000000000..054af3211 --- /dev/null +++ b/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' diff --git a/openpilot/selfdrive/modeld/reprojectcalibd.py b/openpilot/selfdrive/modeld/reprojectcalibd.py new file mode 100644 index 000000000..33d8e258e --- /dev/null +++ b/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() diff --git a/openpilot/selfdrive/modeld/reprojectd.py b/openpilot/selfdrive/modeld/reprojectd.py new file mode 100644 index 000000000..c85009fbe --- /dev/null +++ b/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() diff --git a/openpilot/sunnypilot/modeld_v2/camera_offset_helper.py b/openpilot/sunnypilot/modeld_v2/camera_offset_helper.py index bcf3a20c0..4fdd32319 100644 --- a/openpilot/sunnypilot/modeld_v2/camera_offset_helper.py +++ b/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 diff --git a/openpilot/sunnypilot/modeld_v2/modeld.py b/openpilot/sunnypilot/modeld_v2/modeld.py index fdfdaf42e..7a2585e25 100755 --- a/openpilot/sunnypilot/modeld_v2/modeld.py +++ b/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) diff --git a/openpilot/system/manager/process_config.py b/openpilot/system/manager/process_config.py index 4be6c5062..b9b4f93f3 100644 --- a/openpilot/system/manager/process_config.py +++ b/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