refactor camera transformations (#31818)

* refactor camera transormations

* update users

* more stuff

* more fix

* swap

* tici

* lil shorter
old-commit-hash: e3589e4b5cc783001c95b52cce066647b4ae0c6c

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))