mawei/sunnypilot
1
0
Fork 0
mirror of https://github.com/sunnypilot/sunnypilot.git synced 2026-02-18 22:23:56 +08:00
Code Issues Packages Projects Releases Wiki Activity

Combine update_calibration/get_warp_matrix (#29719)

Browse Source 
old-commit-hash: 151ac4bf76
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

71
common/transformations/model.py
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
# 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
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
elif tici:
intrinsics = tici_fcam_intrinsics
else:
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