Combine update_calibration/get_warp_matrix (#29719)

common/transformations/model.py

@@ -1,7 +1,9 @@
 import numpy as np
 
-from openpilot.common.transformations.camera import (FULL_FRAME_SIZE,
-                                           get_view_frame_from_calib_frame)
+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)
 
 # segnet
 SEGNET_SIZE = (512, 384)
@@ -57,61 +59,20 @@ medmodel_frame_from_calib_frame = np.dot(medmodel_intrinsics,
 
 medmodel_frame_from_bigmodel_frame = np.dot(medmodel_intrinsics, np.linalg.inv(bigmodel_intrinsics))
 
+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 mimics the update_calibration logic in modeld.cc
-### Manually verified to give similar results to xx.uncommon.utils.transform_img
-def get_warp_matrix(rpy_calib, wide_cam=False, big_model=False, tici=True):
-  from openpilot.common.transformations.orientation import rot_from_euler
-  from openpilot.common.transformations.camera import view_frame_from_device_frame, eon_fcam_intrinsics, tici_ecam_intrinsics, tici_fcam_intrinsics
-
-  if tici and wide_cam:
-    intrinsics = tici_ecam_intrinsics
-  elif tici:
-    intrinsics = tici_fcam_intrinsics
-  else:
-    intrinsics = eon_fcam_intrinsics
-
-  if big_model:
-    sbigmodel_from_calib = sbigmodel_frame_from_calib_frame[:, (0,1,2)]
-    calib_from_model = np.linalg.inv(sbigmodel_from_calib)
-  else:
-    medmodel_from_calib = medmodel_frame_from_calib_frame[:, (0,1,2)]
-    calib_from_model = np.linalg.inv(medmodel_from_calib)
-  device_from_calib = rot_from_euler(rpy_calib)
-  camera_from_calib = intrinsics.dot(view_frame_from_device_frame.dot(device_from_calib))
-  warp_matrix = camera_from_calib.dot(calib_from_model)
-  return warp_matrix
-
-
-### This is old, just for debugging
-def get_warp_matrix_old(rpy_calib, wide_cam=False, big_model=False, tici=True):
-  from openpilot.common.transformations.orientation import rot_from_euler
-  from openpilot.common.transformations.camera import view_frame_from_device_frame, eon_fcam_intrinsics, tici_ecam_intrinsics, tici_fcam_intrinsics
-
-
-  def get_view_frame_from_road_frame(roll, pitch, yaw, height):
-    device_from_road = rot_from_euler([roll, pitch, yaw]).dot(np.diag([1, -1, -1]))
-    view_from_road = view_frame_from_device_frame.dot(device_from_road)
-    return np.hstack((view_from_road, [[0], [height], [0]]))
-
-  if tici and wide_cam:
-    intrinsics = tici_ecam_intrinsics
+# 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:
-    intrinsics = tici_fcam_intrinsics
+    cam_intrinsics = tici_fcam_intrinsics
   else:
-    intrinsics = eon_fcam_intrinsics
+    cam_intrinsics = eon_fcam_intrinsics
 
-  model_height = 1.22
-  if big_model:
-    model_from_road = np.dot(sbigmodel_intrinsics,
-             get_view_frame_from_road_frame(0, 0, 0, model_height))
-  else:
-    model_from_road = np.dot(medmodel_intrinsics,
-             get_view_frame_from_road_frame(0, 0, 0, model_height))
-  ground_from_model = np.linalg.inv(model_from_road[:, (0, 1, 3)])
-
-  E = get_view_frame_from_road_frame(*rpy_calib, 1.22)
-  camera_frame_from_road_frame = intrinsics.dot(E)
-  camera_frame_from_ground = camera_frame_from_road_frame[:,(0,1,3)]
-  warp_matrix = camera_frame_from_ground .dot(ground_from_model)
+  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
+  warp_matrix: np.ndarray = camera_from_calib @ calib_from_model
   return warp_matrix

selfdrive/modeld/modeld.py

@@ -13,9 +13,7 @@ from openpilot.system.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.orientation import rot_from_euler
-from openpilot.common.transformations.model import medmodel_frame_from_calib_frame, sbigmodel_frame_from_calib_frame
-from openpilot.common.transformations.camera import view_frame_from_device_frame, tici_fcam_intrinsics, tici_ecam_intrinsics
+from openpilot.common.transformations.model import get_warp_matrix
 from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext, Runtime
 from openpilot.selfdrive.modeld.models.driving_pyx import (
   PublishState, create_model_msg, create_pose_msg,
@@ -30,17 +28,6 @@ else:
 
 MODEL_PATH = str(Path(__file__).parent / f"models/supercombo.{'thneed' if USE_THNEED else 'onnx'}")
 
-calib_from_medmodel = np.linalg.inv(medmodel_frame_from_calib_frame[:, :3])
-calib_from_sbigmodel = np.linalg.inv(sbigmodel_frame_from_calib_frame[:, :3])
-
-def update_calibration(device_from_calib_euler: np.ndarray, wide_camera: bool, bigmodel_frame: bool) -> np.ndarray:
-  cam_intrinsics = tici_ecam_intrinsics if wide_camera else tici_fcam_intrinsics
-  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
-  warp_matrix: np.ndarray = camera_from_calib @ calib_from_model
-  return warp_matrix.astype(np.float32)
-
 class FrameMeta:
   frame_id: int = 0
   timestamp_sof: int = 0
@@ -202,8 +189,8 @@ def main():
     frame_id = sm["roadCameraState"].frameId
     if sm.updated["liveCalibration"]:
       device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32)
-      model_transform_main = update_calibration(device_from_calib_euler, main_wide_camera, False)
-      model_transform_extra = update_calibration(device_from_calib_euler, True, True)
+      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)
       live_calib_seen = True
 
     traffic_convention = np.zeros(2)