From fa770f1ccc26238c8da3a070389b2aef4f79a8a7 Mon Sep 17 00:00:00 2001 From: Mitchell Goff Date: Wed, 30 Aug 2023 15:16:29 -0700 Subject: [PATCH] Combine update_calibration/get_warp_matrix (#29719) old-commit-hash: 151ac4bf7631a60e05c7490515e58c3309de445b --- common/transformations/model.py | 71 ++++++++------------------------- selfdrive/modeld/modeld.py | 19 ++------- 2 files changed, 19 insertions(+), 71 deletions(-) diff --git a/common/transformations/model.py b/common/transformations/model.py index 114f8eae38..7e40767f63 100644 --- a/common/transformations/model.py +++ b/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 diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 1f30f52ed0..356ae64866 100755 --- a/selfdrive/modeld/modeld.py +++ b/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)