sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Combine update_calibration/get_warp_matrix (#29719)

Browse Source 
old-commit-hash: 151ac4bf76
beeps
Mitchell Goff 2 years ago committed by GitHub
parent b580f19448
commit fa770f1ccc
2 changed files with 19 additions and 71 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 71
      common/transformations/model.py
  2. 19
      selfdrive/modeld/modeld.py

71
common/transformations/model.py
Unescape View File

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

19
selfdrive/modeld/modeld.py
Unescape 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)

Write Preview
Loading…
Cancel
Save