mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-21 00:03:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Combine update_calibration/get_warp_matrix (#29719)

Browse Source 
old-commit-hash: 151ac4bf7631a60e05c7490515e58c3309de445b
This commit is contained in:
Mitchell Goff
2023-08-30 15:16:29 -07:00
committed by GitHub
parent b580f19448
commit fa770f1ccc
2 changed files with 19 additions and 71 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/modeld/modeld.py
+3 -16
View File
@@ -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)