refactor camera transformations (#31818)

* refactor camera transormations

* update users

* more stuff

* more fix

* swap

* tici

* lil shorter
old-commit-hash: e3589e4b5cc783001c95b52cce066647b4ae0c6c

common/transformations/camera.py

@@ -1,55 +1,55 @@
+import itertools
 import numpy as np
+from dataclasses import dataclass
 
 import openpilot.common.transformations.orientation as orient
 
 ## -- hardcoded hardware params --
-eon_f_focal_length = 910.0
-eon_d_focal_length = 650.0
-tici_f_focal_length = 2648.0
-tici_e_focal_length = tici_d_focal_length = 567.0 # probably wrong? magnification is not consistent across frame
+@dataclass(frozen=True)
+class CameraConfig:
+  width: int
+  height: int
+  focal_length: float
 
-eon_f_frame_size = (1164, 874)
-eon_d_frame_size = (816, 612)
-tici_f_frame_size = tici_e_frame_size = tici_d_frame_size = (1928, 1208)
+  @property
+  def intrinsics(self):
+    # aka 'K' aka camera_frame_from_view_frame
+    return np.array([
+      [self.focal_length,  0.0, float(self.width)/2],
+      [0.0, self.focal_length, float(self.height)/2],
+      [0.0,  0.0, 1.0]
+    ])
 
-# aka 'K' aka camera_frame_from_view_frame
-eon_fcam_intrinsics = np.array([
-  [eon_f_focal_length,  0.0,  float(eon_f_frame_size[0])/2],
-  [0.0,  eon_f_focal_length,  float(eon_f_frame_size[1])/2],
-  [0.0,  0.0,                                          1.0]])
-eon_intrinsics = eon_fcam_intrinsics # xx
+  @property
+  def intrinsics_inv(self):
+    # aka 'K_inv' aka view_frame_from_camera_frame
+    return np.linalg.inv(self.intrinsics)
 
-eon_dcam_intrinsics = np.array([
-  [eon_d_focal_length,  0.0,  float(eon_d_frame_size[0])/2],
-  [0.0,  eon_d_focal_length,  float(eon_d_frame_size[1])/2],
-  [0.0,  0.0,                                          1.0]])
+@dataclass(frozen=True)
+class DeviceCameraConfig:
+  fcam: CameraConfig
+  dcam: CameraConfig
+  ecam: CameraConfig
 
-tici_fcam_intrinsics = np.array([
-  [tici_f_focal_length,  0.0,  float(tici_f_frame_size[0])/2],
-  [0.0,  tici_f_focal_length,  float(tici_f_frame_size[1])/2],
-  [0.0,  0.0,                                            1.0]])
+ar_ox_fisheye = CameraConfig(1928, 1208, 567.0)  # focal length probably wrong? magnification is not consistent across frame
+ar_ox_config = DeviceCameraConfig(CameraConfig(1928, 1208, 2648.0), ar_ox_fisheye, ar_ox_fisheye)
+os_fisheye = CameraConfig(2688, 1520, 567.0 / 2 * 3)
+os_config = DeviceCameraConfig(CameraConfig(2688, 1520, 2648.0 * 2 / 3), os_fisheye, os_fisheye)
 
-tici_dcam_intrinsics = np.array([
-  [tici_d_focal_length,  0.0,  float(tici_d_frame_size[0])/2],
-  [0.0,  tici_d_focal_length,  float(tici_d_frame_size[1])/2],
-  [0.0,  0.0,                                            1.0]])
+DEVICE_CAMERAS = {
+  # A "device camera" is defined by a device type and sensor
 
-tici_ecam_intrinsics = tici_dcam_intrinsics
-
-# aka 'K_inv' aka view_frame_from_camera_frame
-eon_fcam_intrinsics_inv = np.linalg.inv(eon_fcam_intrinsics)
-eon_intrinsics_inv = eon_fcam_intrinsics_inv # xx
-
-tici_fcam_intrinsics_inv = np.linalg.inv(tici_fcam_intrinsics)
-tici_ecam_intrinsics_inv = np.linalg.inv(tici_ecam_intrinsics)
-
-
-FULL_FRAME_SIZE = tici_f_frame_size
-FOCAL = tici_f_focal_length
-fcam_intrinsics = tici_fcam_intrinsics
-
-W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1]
+  # sensor type was never set on eon/neo/two
+  ("neo", "unknown"): DeviceCameraConfig(CameraConfig(1164, 874, 910.0), CameraConfig(816, 612, 650.0), CameraConfig(0, 0, 0.)),
+  # unknown here is AR0231, field was added with OX03C10 support
+  ("tici", "unknown"): ar_ox_config,
 
+  # before deviceState.deviceType was set, assume tici AR config
+  ("unknown", "ar0231"): ar_ox_config,
+  ("unknown", "ox03c10"): ar_ox_config,
+}
+prods = itertools.product(('tici', 'tizi', 'mici'), (('ar0231', ar_ox_config), ('ox03c10', ar_ox_config), ('os04c10', os_config)))
+DEVICE_CAMERAS.update({(d, c[0]): c[1] for d, c in prods})
 
 # device/mesh : x->forward, y-> right, z->down
 # view : x->right, y->down, z->forward
@@ -93,7 +93,7 @@ def roll_from_ke(m):
                     -(m[0, 0] - m[0, 1] * m[2, 0] / m[2, 1]))
 
 
-def normalize(img_pts, intrinsics=fcam_intrinsics):
+def normalize(img_pts, intrinsics):
   # normalizes image coordinates
   # accepts single pt or array of pts
   intrinsics_inv = np.linalg.inv(intrinsics)
@@ -106,7 +106,7 @@ def normalize(img_pts, intrinsics=fcam_intrinsics):
   return img_pts_normalized[:, :2].reshape(input_shape)
 
 
-def denormalize(img_pts, intrinsics=fcam_intrinsics, width=np.inf, height=np.inf):
+def denormalize(img_pts, intrinsics, width=np.inf, height=np.inf):
   # denormalizes image coordinates
   # accepts single pt or array of pts
   img_pts = np.array(img_pts)
@@ -123,7 +123,7 @@ def denormalize(img_pts, intrinsics=fcam_intrinsics, width=np.inf, height=np.inf
   return img_pts_denormalized[:, :2].reshape(input_shape)
 
 
-def get_calib_from_vp(vp, intrinsics=fcam_intrinsics):
+def get_calib_from_vp(vp, intrinsics):
   vp_norm = normalize(vp, intrinsics)
   yaw_calib = np.arctan(vp_norm[0])
   pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib))

common/transformations/model.py

@@ -1,19 +1,11 @@
 import numpy as np
 
 from openpilot.common.transformations.orientation import rot_from_euler
-from openpilot.common.transformations.camera import (
-  FULL_FRAME_SIZE, get_view_frame_from_calib_frame, view_frame_from_device_frame,
-  eon_fcam_intrinsics, tici_ecam_intrinsics, tici_fcam_intrinsics)
+from openpilot.common.transformations.camera import get_view_frame_from_calib_frame, view_frame_from_device_frame
 
 # segnet
 SEGNET_SIZE = (512, 384)
 
-def get_segnet_frame_from_camera_frame(segnet_size=SEGNET_SIZE, full_frame_size=FULL_FRAME_SIZE):
-  return np.array([[float(segnet_size[0]) / full_frame_size[0],  0.0],
-                   [0.0,  float(segnet_size[1]) / full_frame_size[1]]])
-segnet_frame_from_camera_frame = get_segnet_frame_from_camera_frame() # xx
-
-
 # MED model
 MEDMODEL_INPUT_SIZE = (512, 256)
 MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2)
@@ -63,16 +55,9 @@ calib_from_medmodel = np.linalg.inv(medmodel_frame_from_calib_frame[:, :3])
 calib_from_sbigmodel = np.linalg.inv(sbigmodel_frame_from_calib_frame[:, :3])
 
 # This function is verified to give similar results to xx.uncommon.utils.transform_img
-def get_warp_matrix(device_from_calib_euler: np.ndarray, wide_camera: bool = False, bigmodel_frame: bool = False, tici: bool = True) -> np.ndarray:
-  if tici and wide_camera:
-    cam_intrinsics = tici_ecam_intrinsics
-  elif tici:
-    cam_intrinsics = tici_fcam_intrinsics
-  else:
-    cam_intrinsics = eon_fcam_intrinsics
-
+def get_warp_matrix(device_from_calib_euler: np.ndarray, intrinsics: np.ndarray, bigmodel_frame: bool = False) -> np.ndarray:
   calib_from_model = calib_from_sbigmodel if bigmodel_frame else calib_from_medmodel
   device_from_calib = rot_from_euler(device_from_calib_euler)
-  camera_from_calib = cam_intrinsics @ view_frame_from_device_frame @ device_from_calib
+  camera_from_calib = intrinsics @ view_frame_from_device_frame @ device_from_calib
   warp_matrix: np.ndarray = camera_from_calib @ calib_from_model
   return warp_matrix

selfdrive/modeld/modeld.py

@@ -13,6 +13,7 @@ from openpilot.common.swaglog import cloudlog
 from openpilot.common.params import Params
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import config_realtime_process
+from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.common.transformations.model import get_warp_matrix
 from openpilot.selfdrive import sentry
 from openpilot.selfdrive.car.car_helpers import get_demo_car_params
@@ -227,8 +228,9 @@ def main(demo=False):
     lateral_control_params = np.array([sm["carState"].vEgo, steer_delay], dtype=np.float32)
     if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
       device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
-      model_transform_main = get_warp_matrix(device_from_calib_euler, main_wide_camera, False).astype(np.float32)
-      model_transform_extra = get_warp_matrix(device_from_calib_euler, True, True).astype(np.float32)
+      dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
+      model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics, False).astype(np.float32)
+      model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics, True).astype(np.float32)
       live_calib_seen = True
 
     traffic_convention = np.zeros(2)

selfdrive/modeld/tests/test_modeld.py

@@ -5,13 +5,14 @@ import random
 
 import cereal.messaging as messaging
 from cereal.visionipc import VisionIpcServer, VisionStreamType
-from openpilot.common.transformations.camera import tici_f_frame_size
+from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.common.realtime import DT_MDL
 from openpilot.selfdrive.car.car_helpers import write_car_param
 from openpilot.selfdrive.manager.process_config import managed_processes
 from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state
 
-IMG = np.zeros(int(tici_f_frame_size[0]*tici_f_frame_size[1]*(3/2)), dtype=np.uint8)
+CAM = DEVICE_CAMERAS[("tici", "ar0231")].fcam
+IMG = np.zeros(int(CAM.width*CAM.height*(3/2)), dtype=np.uint8)
 IMG_BYTES = IMG.flatten().tobytes()
 
 
@@ -19,9 +20,9 @@ class TestModeld(unittest.TestCase):
 
   def setUp(self):
     self.vipc_server = VisionIpcServer("camerad")
-    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, *tici_f_frame_size)
-    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *tici_f_frame_size)
-    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *tici_f_frame_size)
+    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, CAM.width, CAM.height)
+    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, CAM.width, CAM.height)
+    self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, CAM.width, CAM.height)
     self.vipc_server.start_listener()
     write_car_param()
 

selfdrive/monitoring/driver_monitor.py

@@ -5,7 +5,7 @@ from openpilot.common.numpy_fast import interp
 from openpilot.common.realtime import DT_DMON
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.stat_live import RunningStatFilter
-from openpilot.common.transformations.camera import tici_d_frame_size
+from openpilot.common.transformations.camera import DEVICE_CAMERAS
 
 EventName = car.CarEvent.EventName
 
@@ -71,9 +71,11 @@ class DRIVER_MONITOR_SETTINGS():
     self._MAX_TERMINAL_DURATION = int(30 / self._DT_DMON)  # not allowed to engage after 30s of terminal alerts
 
 
+# TODO: get these live
 # model output refers to center of undistorted+leveled image
 EFL = 598.0 # focal length in K
-W, H = tici_d_frame_size # corrected image has same size as raw
+cam = DEVICE_CAMERAS[("tici", "ar0231")] # corrected image has same size as raw
+W, H = (cam.dcam.width, cam.dcam.height)  # corrected image has same size as raw
 
 class DistractedType:
   NOT_DISTRACTED = 0

selfdrive/test/process_replay/migration.py

@@ -12,6 +12,7 @@ def migrate_all(lr, old_logtime=False, manager_states=False, panda_states=False,
   msgs = migrate_sensorEvents(lr, old_logtime)
   msgs = migrate_carParams(msgs, old_logtime)
   msgs = migrate_gpsLocation(msgs)
+  msgs = migrate_deviceState(msgs)
   if manager_states:
     msgs = migrate_managerState(msgs)
   if panda_states:
@@ -52,6 +53,21 @@ def migrate_gpsLocation(lr):
   return all_msgs
 
 
+def migrate_deviceState(lr):
+  all_msgs = []
+  dt = None
+  for msg in lr:
+    if msg.which() == 'initData':
+      dt = msg.initData.deviceType
+    if msg.which() == 'deviceState':
+      n = msg.as_builder()
+      n.deviceState.deviceType = dt
+      all_msgs.append(n.as_reader())
+    else:
+      all_msgs.append(msg)
+  return all_msgs
+
+
 def migrate_pandaStates(lr):
   all_msgs = []
   # TODO: safety param migration should be handled automatically

selfdrive/test/process_replay/regen.py

@@ -10,7 +10,7 @@ from collections.abc import Iterable
 
 from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, FAKEDATA, ProcessConfig, replay_process, get_process_config, \
                                                                    check_openpilot_enabled, get_custom_params_from_lr
-from openpilot.selfdrive.test.process_replay.vision_meta import DRIVER_FRAME_SIZES
+from openpilot.selfdrive.test.process_replay.vision_meta import DRIVER_CAMERA_FRAME_SIZES
 from openpilot.selfdrive.test.update_ci_routes import upload_route
 from openpilot.tools.lib.route import Route
 from openpilot.tools.lib.framereader import FrameReader, BaseFrameReader, FrameType
@@ -37,7 +37,7 @@ class DummyFrameReader(BaseFrameReader):
 
   @staticmethod
   def zero_dcamera():
-    return DummyFrameReader(*DRIVER_FRAME_SIZES["tici"], 1200, 0)
+    return DummyFrameReader(*DRIVER_CAMERA_FRAME_SIZES[("tici", "ar0231")], 1200, 0)
 
 
 def regen_segment(

selfdrive/test/process_replay/vision_meta.py

@@ -1,17 +1,17 @@
 from collections import namedtuple
 from cereal.visionipc import VisionStreamType
 from openpilot.common.realtime import DT_MDL, DT_DMON
-from openpilot.common.transformations.camera import tici_f_frame_size, tici_d_frame_size, tici_e_frame_size, eon_f_frame_size, eon_d_frame_size
+from openpilot.common.transformations.camera import DEVICE_CAMERAS
 
 VideoStreamMeta = namedtuple("VideoStreamMeta", ["camera_state", "encode_index", "stream", "dt", "frame_sizes"])
-ROAD_CAMERA_FRAME_SIZES = {"tici": tici_f_frame_size, "tizi": tici_f_frame_size, "neo": eon_f_frame_size}
-WIDE_ROAD_CAMERA_FRAME_SIZES = {"tici": tici_e_frame_size, "tizi": tici_e_frame_size}
-DRIVER_FRAME_SIZES = {"tici": tici_d_frame_size, "tizi": tici_d_frame_size, "neo": eon_d_frame_size}
+ROAD_CAMERA_FRAME_SIZES = {k: (v.dcam.width, v.dcam.height) for k, v in DEVICE_CAMERAS.items()}
+WIDE_ROAD_CAMERA_FRAME_SIZES = {k: (v.ecam.width, v.ecam.height) for k, v in DEVICE_CAMERAS.items() if v.ecam is not None}
+DRIVER_CAMERA_FRAME_SIZES = {k: (v.dcam.width, v.dcam.height) for k, v in DEVICE_CAMERAS.items()}
 VIPC_STREAM_METADATA = [
   # metadata: (state_msg_type, encode_msg_type, stream_type, dt, frame_sizes)
   ("roadCameraState", "roadEncodeIdx", VisionStreamType.VISION_STREAM_ROAD, DT_MDL, ROAD_CAMERA_FRAME_SIZES),
   ("wideRoadCameraState", "wideRoadEncodeIdx", VisionStreamType.VISION_STREAM_WIDE_ROAD, DT_MDL, WIDE_ROAD_CAMERA_FRAME_SIZES),
-  ("driverCameraState", "driverEncodeIdx", VisionStreamType.VISION_STREAM_DRIVER, DT_DMON, DRIVER_FRAME_SIZES),
+  ("driverCameraState", "driverEncodeIdx", VisionStreamType.VISION_STREAM_DRIVER, DT_DMON, DRIVER_CAMERA_FRAME_SIZES),
 ]
 
 

selfdrive/ui/tests/test_ui/run.py

@@ -18,7 +18,7 @@ from cereal.messaging import SubMaster, PubMaster
 from openpilot.common.mock import mock_messages
 from openpilot.common.params import Params
 from openpilot.common.realtime import DT_MDL
-from openpilot.common.transformations.camera import tici_f_frame_size
+from openpilot.common.transformations.camera import DEVICE_CAMERAS
 from openpilot.selfdrive.test.helpers import with_processes
 from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state
 from openpilot.tools.webcam.camera import Camera
@@ -69,15 +69,16 @@ def setup_onroad(click, pm: PubMaster):
 
   pm.send("pandaStates", dat)
 
+  d = DEVICE_CAMERAS[("tici", "ar0231")]
   server = VisionIpcServer("camerad")
-  server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, *tici_f_frame_size)
-  server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *tici_f_frame_size)
-  server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *tici_f_frame_size)
+  server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, d.fcam.width, d.fcam.height)
+  server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, d.dcam.width, d.dcam.height)
+  server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, d.fcam.width, d.fcam.height)
   server.start_listener()
 
   time.sleep(0.5) # give time for vipc server to start
 
-  IMG = Camera.bgr2nv12(np.random.randint(0, 255, (*tici_f_frame_size,3), dtype=np.uint8))
+  IMG = Camera.bgr2nv12(np.random.randint(0, 255, (d.fcam.width, d.fcam.height, 3), dtype=np.uint8))
   IMG_BYTES = IMG.flatten().tobytes()
 
   cams = ('roadCameraState', 'wideRoadCameraState')

system/loggerd/tests/test_loggerd.py

@@ -24,7 +24,7 @@ from openpilot.system.version import get_version
 from openpilot.tools.lib.helpers import RE
 from openpilot.tools.lib.logreader import LogReader
 from cereal.visionipc import VisionIpcServer, VisionStreamType
-from openpilot.common.transformations.camera import tici_f_frame_size, tici_d_frame_size, tici_e_frame_size
+from openpilot.common.transformations.camera import DEVICE_CAMERAS
 
 SentinelType = log.Sentinel.SentinelType
 
@@ -142,10 +142,11 @@ class TestLoggerd:
     os.environ["LOGGERD_TEST"] = "1"
     Params().put("RecordFront", "1")
 
+    d = DEVICE_CAMERAS[("tici", "ar0231")]
     expected_files = {"rlog", "qlog", "qcamera.ts", "fcamera.hevc", "dcamera.hevc", "ecamera.hevc"}
-    streams = [(VisionStreamType.VISION_STREAM_ROAD, (*tici_f_frame_size, 2048*2346, 2048, 2048*1216), "roadCameraState"),
-               (VisionStreamType.VISION_STREAM_DRIVER, (*tici_d_frame_size, 2048*2346, 2048, 2048*1216), "driverCameraState"),
-               (VisionStreamType.VISION_STREAM_WIDE_ROAD, (*tici_e_frame_size, 2048*2346, 2048, 2048*1216), "wideRoadCameraState")]
+    streams = [(VisionStreamType.VISION_STREAM_ROAD, (d.fcam.width, d.fcam.height, 2048*2346, 2048, 2048*1216), "roadCameraState"),
+               (VisionStreamType.VISION_STREAM_DRIVER, (d.dcam.width, d.dcam.height, 2048*2346, 2048, 2048*1216), "driverCameraState"),
+               (VisionStreamType.VISION_STREAM_WIDE_ROAD, (d.ecam.width, d.ecam.height, 2048*2346, 2048, 2048*1216), "wideRoadCameraState")]
 
     pm = messaging.PubMaster(["roadCameraState", "driverCameraState", "wideRoadCameraState"])
     vipc_server = VisionIpcServer("camerad")

tools/replay/lib/ui_helpers.py

@@ -7,9 +7,7 @@ import pygame
 
 from matplotlib.backends.backend_agg import FigureCanvasAgg
 
-from openpilot.common.transformations.camera import (eon_f_frame_size, eon_f_focal_length,
-                                           tici_f_frame_size, tici_f_focal_length,
-                                           get_view_frame_from_calib_frame)
+from openpilot.common.transformations.camera import get_view_frame_from_calib_frame
 from openpilot.selfdrive.controls.radard import RADAR_TO_CAMERA
 
 
@@ -20,9 +18,6 @@ YELLOW = (255, 255, 0)
 BLACK = (0, 0, 0)
 WHITE = (255, 255, 255)
 
-_FULL_FRAME_SIZE = {
-}
-
 class UIParams:
   lidar_x, lidar_y, lidar_zoom = 384, 960, 6
   lidar_car_x, lidar_car_y = lidar_x / 2., lidar_y / 1.1
@@ -32,45 +27,13 @@ class UIParams:
   car_color = 110
 UP = UIParams
 
-_BB_TO_FULL_FRAME = {}
-_CALIB_BB_TO_FULL = {}
-_FULL_FRAME_TO_BB = {}
-_INTRINSICS = {}
-
-eon_f_qcam_frame_size = (480, 360)
-tici_f_qcam_frame_size = (528, 330)
-
-cams = [(eon_f_frame_size, eon_f_focal_length, eon_f_frame_size),
-        (tici_f_frame_size, tici_f_focal_length, tici_f_frame_size),
-        (eon_f_qcam_frame_size, eon_f_focal_length, eon_f_frame_size),
-        (tici_f_qcam_frame_size, tici_f_focal_length, tici_f_frame_size)]
-for size, focal, full_size in cams:
-  sz = size[0] * size[1]
-  _BB_SCALE = size[0] / 640.
-  _BB_TO_FULL_FRAME[sz] = np.asarray([
-      [_BB_SCALE, 0., 0.],
-      [0., _BB_SCALE, 0.],
-      [0., 0., 1.]])
-  calib_scale = full_size[0] / 640.
-  _CALIB_BB_TO_FULL[sz] = np.asarray([
-      [calib_scale, 0., 0.],
-      [0., calib_scale, 0.],
-      [0., 0., 1.]])
-  _FULL_FRAME_TO_BB[sz] = np.linalg.inv(_BB_TO_FULL_FRAME[sz])
-  _FULL_FRAME_SIZE[sz] = (size[0], size[1])
-  _INTRINSICS[sz] = np.array([
-    [focal, 0., full_size[0] / 2.],
-    [0., focal, full_size[1] / 2.],
-    [0., 0., 1.]])
-
-
 METER_WIDTH = 20
 
 class Calibration:
-  def __init__(self, num_px, rpy, intrinsic):
+  def __init__(self, num_px, rpy, intrinsic, calib_scale):
     self.intrinsic = intrinsic
     self.extrinsics_matrix = get_view_frame_from_calib_frame(rpy[0], rpy[1], rpy[2], 0.0)[:,:3]
-    self.zoom = _CALIB_BB_TO_FULL[num_px][0, 0]
+    self.zoom = calib_scale
 
   def car_space_to_ff(self, x, y, z):
     car_space_projective = np.column_stack((x, y, z)).T

tools/replay/ui.py

@@ -10,8 +10,9 @@ import pygame
 import cereal.messaging as messaging
 from openpilot.common.numpy_fast import clip
 from openpilot.common.basedir import BASEDIR
-from openpilot.tools.replay.lib.ui_helpers import (_BB_TO_FULL_FRAME, UP,
-                                         _INTRINSICS, BLACK, GREEN,
+from openpilot.common.transformations.camera import DEVICE_CAMERAS
+from openpilot.tools.replay.lib.ui_helpers import (UP,
+                                         BLACK, GREEN,
                                          YELLOW, Calibration,
                                          get_blank_lid_overlay, init_plots,
                                          maybe_update_radar_points, plot_lead,
@@ -55,7 +56,7 @@ def ui_thread(addr):
   top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y), 0, 8)
 
   sm = messaging.SubMaster(['carState', 'longitudinalPlan', 'carControl', 'radarState', 'liveCalibration', 'controlsState',
-                            'liveTracks', 'modelV2', 'liveParameters'], addr=addr)
+                            'liveTracks', 'modelV2', 'liveParameters', 'roadCameraState'], addr=addr)
 
   img = np.zeros((480, 640, 3), dtype='uint8')
   imgff = None
@@ -112,20 +113,27 @@ def ui_thread(addr):
       vipc_client.connect(True)
 
     yuv_img_raw = vipc_client.recv()
-
     if yuv_img_raw is None or not yuv_img_raw.data.any():
       continue
 
+    sm.update(0)
+
+    camera = DEVICE_CAMERAS[("three", str(sm['roadCameraState'].sensor))]
+
     imgff = np.frombuffer(yuv_img_raw.data, dtype=np.uint8).reshape((len(yuv_img_raw.data) // vipc_client.stride, vipc_client.stride))
     num_px = vipc_client.width * vipc_client.height
     rgb = cv2.cvtColor(imgff[:vipc_client.height * 3 // 2, :vipc_client.width], cv2.COLOR_YUV2RGB_NV12)
 
-    zoom_matrix = _BB_TO_FULL_FRAME[num_px]
+    qcam = "QCAM" in os.environ
+    bb_scale = (528 if qcam else camera.fcam.width) / 640.
+    calib_scale = camera.fcam.width / 640.
+    zoom_matrix = np.asarray([
+        [bb_scale, 0., 0.],
+        [0., bb_scale, 0.],
+        [0., 0., 1.]])
     cv2.warpAffine(rgb, zoom_matrix[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)
 
-    intrinsic_matrix = _INTRINSICS[num_px]
-
-    sm.update(0)
+    intrinsic_matrix = camera.fcam.intrinsics
 
     w = sm['controlsState'].lateralControlState.which()
     if w == 'lqrStateDEPRECATED':
@@ -165,7 +173,7 @@ def ui_thread(addr):
 
     if sm.updated['liveCalibration'] and num_px:
       rpyCalib = np.asarray(sm['liveCalibration'].rpyCalib)
-      calibration = Calibration(num_px, rpyCalib, intrinsic_matrix)
+      calibration = Calibration(num_px, rpyCalib, intrinsic_matrix, calib_scale)
 
     # *** blits ***
     pygame.surfarray.blit_array(camera_surface, img.swapaxes(0, 1))