mirror of
https://github.com/firestar5683/StarPilot.git
synced 2026-08-21 08:14:00 +08:00
refactor camera transformations (#31818)
* refactor camera transormations * update users * more stuff * more fix * swap * tici * lil shorter old-commit-hash: e3589e4b5cc783001c95b52cce066647b4ae0c6c
This commit is contained in:
Adeeb Shihadeh
@@ -1,55 +1,55 @@
|
||||
import itertools
|
||||
import numpy as np
|
||||
from dataclasses import dataclass
|
||||
|
||||
import openpilot.common.transformations.orientation as orient
|
||||
|
||||
## -- hardcoded hardware params --
|
||||
eon_f_focal_length = 910.0
|
||||
eon_d_focal_length = 650.0
|
||||
tici_f_focal_length = 2648.0
|
||||
tici_e_focal_length = tici_d_focal_length = 567.0 # probably wrong? magnification is not consistent across frame
|
||||
@dataclass(frozen=True)
|
||||
class CameraConfig:
|
||||
width: int
|
||||
height: int
|
||||
focal_length: float
|
||||
|
||||
eon_f_frame_size = (1164, 874)
|
||||
eon_d_frame_size = (816, 612)
|
||||
tici_f_frame_size = tici_e_frame_size = tici_d_frame_size = (1928, 1208)
|
||||
@property
|
||||
def intrinsics(self):
|
||||
# aka 'K' aka camera_frame_from_view_frame
|
||||
return np.array([
|
||||
[self.focal_length, 0.0, float(self.width)/2],
|
||||
[0.0, self.focal_length, float(self.height)/2],
|
||||
[0.0, 0.0, 1.0]
|
||||
])
|
||||
|
||||
# aka 'K' aka camera_frame_from_view_frame
|
||||
eon_fcam_intrinsics = np.array([
|
||||
[eon_f_focal_length, 0.0, float(eon_f_frame_size[0])/2],
|
||||
[0.0, eon_f_focal_length, float(eon_f_frame_size[1])/2],
|
||||
[0.0, 0.0, 1.0]])
|
||||
eon_intrinsics = eon_fcam_intrinsics # xx
|
||||
@property
|
||||
def intrinsics_inv(self):
|
||||
# aka 'K_inv' aka view_frame_from_camera_frame
|
||||
return np.linalg.inv(self.intrinsics)
|
||||
|
||||
eon_dcam_intrinsics = np.array([
|
||||
[eon_d_focal_length, 0.0, float(eon_d_frame_size[0])/2],
|
||||
[0.0, eon_d_focal_length, float(eon_d_frame_size[1])/2],
|
||||
[0.0, 0.0, 1.0]])
|
||||
@dataclass(frozen=True)
|
||||
class DeviceCameraConfig:
|
||||
fcam: CameraConfig
|
||||
dcam: CameraConfig
|
||||
ecam: CameraConfig
|
||||
|
||||
tici_fcam_intrinsics = np.array([
|
||||
[tici_f_focal_length, 0.0, float(tici_f_frame_size[0])/2],
|
||||
[0.0, tici_f_focal_length, float(tici_f_frame_size[1])/2],
|
||||
[0.0, 0.0, 1.0]])
|
||||
ar_ox_fisheye = CameraConfig(1928, 1208, 567.0) # focal length probably wrong? magnification is not consistent across frame
|
||||
ar_ox_config = DeviceCameraConfig(CameraConfig(1928, 1208, 2648.0), ar_ox_fisheye, ar_ox_fisheye)
|
||||
os_fisheye = CameraConfig(2688, 1520, 567.0 / 2 * 3)
|
||||
os_config = DeviceCameraConfig(CameraConfig(2688, 1520, 2648.0 * 2 / 3), os_fisheye, os_fisheye)
|
||||
|
||||
tici_dcam_intrinsics = np.array([
|
||||
[tici_d_focal_length, 0.0, float(tici_d_frame_size[0])/2],
|
||||
[0.0, tici_d_focal_length, float(tici_d_frame_size[1])/2],
|
||||
[0.0, 0.0, 1.0]])
|
||||
DEVICE_CAMERAS = {
|
||||
# A "device camera" is defined by a device type and sensor
|
||||
|
||||
tici_ecam_intrinsics = tici_dcam_intrinsics
|
||||
|
||||
# aka 'K_inv' aka view_frame_from_camera_frame
|
||||
eon_fcam_intrinsics_inv = np.linalg.inv(eon_fcam_intrinsics)
|
||||
eon_intrinsics_inv = eon_fcam_intrinsics_inv # xx
|
||||
|
||||
tici_fcam_intrinsics_inv = np.linalg.inv(tici_fcam_intrinsics)
|
||||
tici_ecam_intrinsics_inv = np.linalg.inv(tici_ecam_intrinsics)
|
||||
|
||||
|
||||
FULL_FRAME_SIZE = tici_f_frame_size
|
||||
FOCAL = tici_f_focal_length
|
||||
fcam_intrinsics = tici_fcam_intrinsics
|
||||
|
||||
W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1]
|
||||
# sensor type was never set on eon/neo/two
|
||||
("neo", "unknown"): DeviceCameraConfig(CameraConfig(1164, 874, 910.0), CameraConfig(816, 612, 650.0), CameraConfig(0, 0, 0.)),
|
||||
# unknown here is AR0231, field was added with OX03C10 support
|
||||
("tici", "unknown"): ar_ox_config,
|
||||
|
||||
# before deviceState.deviceType was set, assume tici AR config
|
||||
("unknown", "ar0231"): ar_ox_config,
|
||||
("unknown", "ox03c10"): ar_ox_config,
|
||||
}
|
||||
prods = itertools.product(('tici', 'tizi', 'mici'), (('ar0231', ar_ox_config), ('ox03c10', ar_ox_config), ('os04c10', os_config)))
|
||||
DEVICE_CAMERAS.update({(d, c[0]): c[1] for d, c in prods})
|
||||
|
||||
# device/mesh : x->forward, y-> right, z->down
|
||||
# view : x->right, y->down, z->forward
|
||||
@@ -93,7 +93,7 @@ def roll_from_ke(m):
|
||||
-(m[0, 0] - m[0, 1] * m[2, 0] / m[2, 1]))
|
||||
|
||||
|
||||
def normalize(img_pts, intrinsics=fcam_intrinsics):
|
||||
def normalize(img_pts, intrinsics):
|
||||
# normalizes image coordinates
|
||||
# accepts single pt or array of pts
|
||||
intrinsics_inv = np.linalg.inv(intrinsics)
|
||||
@@ -106,7 +106,7 @@ def normalize(img_pts, intrinsics=fcam_intrinsics):
|
||||
return img_pts_normalized[:, :2].reshape(input_shape)
|
||||
|
||||
|
||||
def denormalize(img_pts, intrinsics=fcam_intrinsics, width=np.inf, height=np.inf):
|
||||
def denormalize(img_pts, intrinsics, width=np.inf, height=np.inf):
|
||||
# denormalizes image coordinates
|
||||
# accepts single pt or array of pts
|
||||
img_pts = np.array(img_pts)
|
||||
@@ -123,7 +123,7 @@ def denormalize(img_pts, intrinsics=fcam_intrinsics, width=np.inf, height=np.inf
|
||||
return img_pts_denormalized[:, :2].reshape(input_shape)
|
||||
|
||||
|
||||
def get_calib_from_vp(vp, intrinsics=fcam_intrinsics):
|
||||
def get_calib_from_vp(vp, intrinsics):
|
||||
vp_norm = normalize(vp, intrinsics)
|
||||
yaw_calib = np.arctan(vp_norm[0])
|
||||
pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib))
|
||||
|
||||
@@ -1,19 +1,11 @@
|
||||
import numpy as np
|
||||
|
||||
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)
|
||||
from openpilot.common.transformations.camera import get_view_frame_from_calib_frame, view_frame_from_device_frame
|
||||
|
||||
# segnet
|
||||
SEGNET_SIZE = (512, 384)
|
||||
|
||||
def get_segnet_frame_from_camera_frame(segnet_size=SEGNET_SIZE, full_frame_size=FULL_FRAME_SIZE):
|
||||
return np.array([[float(segnet_size[0]) / full_frame_size[0], 0.0],
|
||||
[0.0, float(segnet_size[1]) / full_frame_size[1]]])
|
||||
segnet_frame_from_camera_frame = get_segnet_frame_from_camera_frame() # xx
|
||||
|
||||
|
||||
# MED model
|
||||
MEDMODEL_INPUT_SIZE = (512, 256)
|
||||
MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2)
|
||||
@@ -63,16 +55,9 @@ 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 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:
|
||||
cam_intrinsics = tici_fcam_intrinsics
|
||||
else:
|
||||
cam_intrinsics = eon_fcam_intrinsics
|
||||
|
||||
def get_warp_matrix(device_from_calib_euler: np.ndarray, intrinsics: np.ndarray, bigmodel_frame: bool = False) -> np.ndarray:
|
||||
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
|
||||
camera_from_calib = intrinsics @ view_frame_from_device_frame @ device_from_calib
|
||||
warp_matrix: np.ndarray = camera_from_calib @ calib_from_model
|
||||
return warp_matrix
|
||||
|
||||
Reference in New Issue
Block a user