diff --git a/common/transformations/camera.py b/common/transformations/camera.py index c643cb5702..7a8495a9b2 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -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 - -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) - -# 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 - -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]]) - -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]]) - -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]]) - -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] - +@dataclass(frozen=True) +class CameraConfig: + width: int + height: int + focal_length: float + + @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] + ]) + + @property + def intrinsics_inv(self): + # aka 'K_inv' aka view_frame_from_camera_frame + return np.linalg.inv(self.intrinsics) + +@dataclass(frozen=True) +class DeviceCameraConfig: + fcam: CameraConfig + dcam: CameraConfig + ecam: CameraConfig + +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) + +DEVICE_CAMERAS = { + # A "device camera" is defined by a device type and sensor + + # 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)) diff --git a/common/transformations/model.py b/common/transformations/model.py index 7e40767f63..aaa12d776a 100644 --- a/common/transformations/model.py +++ b/common/transformations/model.py @@ -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 diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index fe523dbd35..c3b3918903 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -13,6 +13,7 @@ from openpilot.common.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.camera import DEVICE_CAMERAS from openpilot.common.transformations.model import get_warp_matrix from openpilot.selfdrive import sentry from openpilot.selfdrive.car.car_helpers import get_demo_car_params @@ -227,8 +228,9 @@ def main(demo=False): lateral_control_params = np.array([sm["carState"].vEgo, steer_delay], dtype=np.float32) if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']: device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) - 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) + dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))] + model_transform_main = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics if main_wide_camera else dc.fcam.intrinsics, False).astype(np.float32) + model_transform_extra = get_warp_matrix(device_from_calib_euler, dc.ecam.intrinsics, True).astype(np.float32) live_calib_seen = True traffic_convention = np.zeros(2) diff --git a/selfdrive/modeld/tests/test_modeld.py b/selfdrive/modeld/tests/test_modeld.py index 257a9bc878..67c6f71038 100755 --- a/selfdrive/modeld/tests/test_modeld.py +++ b/selfdrive/modeld/tests/test_modeld.py @@ -5,13 +5,14 @@ import random import cereal.messaging as messaging from cereal.visionipc import VisionIpcServer, VisionStreamType -from openpilot.common.transformations.camera import tici_f_frame_size +from openpilot.common.transformations.camera import DEVICE_CAMERAS from openpilot.common.realtime import DT_MDL from openpilot.selfdrive.car.car_helpers import write_car_param from openpilot.selfdrive.manager.process_config import managed_processes from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state -IMG = np.zeros(int(tici_f_frame_size[0]*tici_f_frame_size[1]*(3/2)), dtype=np.uint8) +CAM = DEVICE_CAMERAS[("tici", "ar0231")].fcam +IMG = np.zeros(int(CAM.width*CAM.height*(3/2)), dtype=np.uint8) IMG_BYTES = IMG.flatten().tobytes() @@ -19,9 +20,9 @@ class TestModeld(unittest.TestCase): def setUp(self): self.vipc_server = VisionIpcServer("camerad") - self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, *tici_f_frame_size) - self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *tici_f_frame_size) - self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *tici_f_frame_size) + self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, CAM.width, CAM.height) + self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, CAM.width, CAM.height) + self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, CAM.width, CAM.height) self.vipc_server.start_listener() write_car_param() diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 2279002f35..7c1c297fff 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -5,7 +5,7 @@ from openpilot.common.numpy_fast import interp from openpilot.common.realtime import DT_DMON from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.stat_live import RunningStatFilter -from openpilot.common.transformations.camera import tici_d_frame_size +from openpilot.common.transformations.camera import DEVICE_CAMERAS EventName = car.CarEvent.EventName @@ -71,9 +71,11 @@ class DRIVER_MONITOR_SETTINGS(): self._MAX_TERMINAL_DURATION = int(30 / self._DT_DMON) # not allowed to engage after 30s of terminal alerts +# TODO: get these live # model output refers to center of undistorted+leveled image EFL = 598.0 # focal length in K -W, H = tici_d_frame_size # corrected image has same size as raw +cam = DEVICE_CAMERAS[("tici", "ar0231")] # corrected image has same size as raw +W, H = (cam.dcam.width, cam.dcam.height) # corrected image has same size as raw class DistractedType: NOT_DISTRACTED = 0 diff --git a/selfdrive/test/process_replay/migration.py b/selfdrive/test/process_replay/migration.py index d480309169..afcf705ff9 100644 --- a/selfdrive/test/process_replay/migration.py +++ b/selfdrive/test/process_replay/migration.py @@ -12,6 +12,7 @@ def migrate_all(lr, old_logtime=False, manager_states=False, panda_states=False, msgs = migrate_sensorEvents(lr, old_logtime) msgs = migrate_carParams(msgs, old_logtime) msgs = migrate_gpsLocation(msgs) + msgs = migrate_deviceState(msgs) if manager_states: msgs = migrate_managerState(msgs) if panda_states: @@ -52,6 +53,21 @@ def migrate_gpsLocation(lr): return all_msgs +def migrate_deviceState(lr): + all_msgs = [] + dt = None + for msg in lr: + if msg.which() == 'initData': + dt = msg.initData.deviceType + if msg.which() == 'deviceState': + n = msg.as_builder() + n.deviceState.deviceType = dt + all_msgs.append(n.as_reader()) + else: + all_msgs.append(msg) + return all_msgs + + def migrate_pandaStates(lr): all_msgs = [] # TODO: safety param migration should be handled automatically diff --git a/selfdrive/test/process_replay/regen.py b/selfdrive/test/process_replay/regen.py index 8e882207b5..ec3023c5dc 100755 --- a/selfdrive/test/process_replay/regen.py +++ b/selfdrive/test/process_replay/regen.py @@ -10,7 +10,7 @@ from collections.abc import Iterable from openpilot.selfdrive.test.process_replay.process_replay import CONFIGS, FAKEDATA, ProcessConfig, replay_process, get_process_config, \ check_openpilot_enabled, get_custom_params_from_lr -from openpilot.selfdrive.test.process_replay.vision_meta import DRIVER_FRAME_SIZES +from openpilot.selfdrive.test.process_replay.vision_meta import DRIVER_CAMERA_FRAME_SIZES from openpilot.selfdrive.test.update_ci_routes import upload_route from openpilot.tools.lib.route import Route from openpilot.tools.lib.framereader import FrameReader, BaseFrameReader, FrameType @@ -37,7 +37,7 @@ class DummyFrameReader(BaseFrameReader): @staticmethod def zero_dcamera(): - return DummyFrameReader(*DRIVER_FRAME_SIZES["tici"], 1200, 0) + return DummyFrameReader(*DRIVER_CAMERA_FRAME_SIZES[("tici", "ar0231")], 1200, 0) def regen_segment( diff --git a/selfdrive/test/process_replay/vision_meta.py b/selfdrive/test/process_replay/vision_meta.py index b3c3dc0c9c..9bfe214c1e 100644 --- a/selfdrive/test/process_replay/vision_meta.py +++ b/selfdrive/test/process_replay/vision_meta.py @@ -1,17 +1,17 @@ from collections import namedtuple from cereal.visionipc import VisionStreamType from openpilot.common.realtime import DT_MDL, DT_DMON -from openpilot.common.transformations.camera import tici_f_frame_size, tici_d_frame_size, tici_e_frame_size, eon_f_frame_size, eon_d_frame_size +from openpilot.common.transformations.camera import DEVICE_CAMERAS VideoStreamMeta = namedtuple("VideoStreamMeta", ["camera_state", "encode_index", "stream", "dt", "frame_sizes"]) -ROAD_CAMERA_FRAME_SIZES = {"tici": tici_f_frame_size, "tizi": tici_f_frame_size, "neo": eon_f_frame_size} -WIDE_ROAD_CAMERA_FRAME_SIZES = {"tici": tici_e_frame_size, "tizi": tici_e_frame_size} -DRIVER_FRAME_SIZES = {"tici": tici_d_frame_size, "tizi": tici_d_frame_size, "neo": eon_d_frame_size} +ROAD_CAMERA_FRAME_SIZES = {k: (v.dcam.width, v.dcam.height) for k, v in DEVICE_CAMERAS.items()} +WIDE_ROAD_CAMERA_FRAME_SIZES = {k: (v.ecam.width, v.ecam.height) for k, v in DEVICE_CAMERAS.items() if v.ecam is not None} +DRIVER_CAMERA_FRAME_SIZES = {k: (v.dcam.width, v.dcam.height) for k, v in DEVICE_CAMERAS.items()} VIPC_STREAM_METADATA = [ # metadata: (state_msg_type, encode_msg_type, stream_type, dt, frame_sizes) ("roadCameraState", "roadEncodeIdx", VisionStreamType.VISION_STREAM_ROAD, DT_MDL, ROAD_CAMERA_FRAME_SIZES), ("wideRoadCameraState", "wideRoadEncodeIdx", VisionStreamType.VISION_STREAM_WIDE_ROAD, DT_MDL, WIDE_ROAD_CAMERA_FRAME_SIZES), - ("driverCameraState", "driverEncodeIdx", VisionStreamType.VISION_STREAM_DRIVER, DT_DMON, DRIVER_FRAME_SIZES), + ("driverCameraState", "driverEncodeIdx", VisionStreamType.VISION_STREAM_DRIVER, DT_DMON, DRIVER_CAMERA_FRAME_SIZES), ] diff --git a/selfdrive/ui/tests/test_ui/run.py b/selfdrive/ui/tests/test_ui/run.py index 7a2ac9a110..c834107780 100644 --- a/selfdrive/ui/tests/test_ui/run.py +++ b/selfdrive/ui/tests/test_ui/run.py @@ -18,7 +18,7 @@ from cereal.messaging import SubMaster, PubMaster from openpilot.common.mock import mock_messages from openpilot.common.params import Params from openpilot.common.realtime import DT_MDL -from openpilot.common.transformations.camera import tici_f_frame_size +from openpilot.common.transformations.camera import DEVICE_CAMERAS from openpilot.selfdrive.test.helpers import with_processes from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state from openpilot.tools.webcam.camera import Camera @@ -69,15 +69,16 @@ def setup_onroad(click, pm: PubMaster): pm.send("pandaStates", dat) + d = DEVICE_CAMERAS[("tici", "ar0231")] server = VisionIpcServer("camerad") - server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, *tici_f_frame_size) - server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *tici_f_frame_size) - server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, *tici_f_frame_size) + server.create_buffers(VisionStreamType.VISION_STREAM_ROAD, 40, False, d.fcam.width, d.fcam.height) + server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, d.dcam.width, d.dcam.height) + server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, d.fcam.width, d.fcam.height) server.start_listener() time.sleep(0.5) # give time for vipc server to start - IMG = Camera.bgr2nv12(np.random.randint(0, 255, (*tici_f_frame_size,3), dtype=np.uint8)) + IMG = Camera.bgr2nv12(np.random.randint(0, 255, (d.fcam.width, d.fcam.height, 3), dtype=np.uint8)) IMG_BYTES = IMG.flatten().tobytes() cams = ('roadCameraState', 'wideRoadCameraState') diff --git a/system/loggerd/tests/test_loggerd.py b/system/loggerd/tests/test_loggerd.py index c80dc19fce..fdea60a282 100755 --- a/system/loggerd/tests/test_loggerd.py +++ b/system/loggerd/tests/test_loggerd.py @@ -24,7 +24,7 @@ from openpilot.system.version import get_version from openpilot.tools.lib.helpers import RE from openpilot.tools.lib.logreader import LogReader from cereal.visionipc import VisionIpcServer, VisionStreamType -from openpilot.common.transformations.camera import tici_f_frame_size, tici_d_frame_size, tici_e_frame_size +from openpilot.common.transformations.camera import DEVICE_CAMERAS SentinelType = log.Sentinel.SentinelType @@ -142,10 +142,11 @@ class TestLoggerd: os.environ["LOGGERD_TEST"] = "1" Params().put("RecordFront", "1") + d = DEVICE_CAMERAS[("tici", "ar0231")] expected_files = {"rlog", "qlog", "qcamera.ts", "fcamera.hevc", "dcamera.hevc", "ecamera.hevc"} - streams = [(VisionStreamType.VISION_STREAM_ROAD, (*tici_f_frame_size, 2048*2346, 2048, 2048*1216), "roadCameraState"), - (VisionStreamType.VISION_STREAM_DRIVER, (*tici_d_frame_size, 2048*2346, 2048, 2048*1216), "driverCameraState"), - (VisionStreamType.VISION_STREAM_WIDE_ROAD, (*tici_e_frame_size, 2048*2346, 2048, 2048*1216), "wideRoadCameraState")] + streams = [(VisionStreamType.VISION_STREAM_ROAD, (d.fcam.width, d.fcam.height, 2048*2346, 2048, 2048*1216), "roadCameraState"), + (VisionStreamType.VISION_STREAM_DRIVER, (d.dcam.width, d.dcam.height, 2048*2346, 2048, 2048*1216), "driverCameraState"), + (VisionStreamType.VISION_STREAM_WIDE_ROAD, (d.ecam.width, d.ecam.height, 2048*2346, 2048, 2048*1216), "wideRoadCameraState")] pm = messaging.PubMaster(["roadCameraState", "driverCameraState", "wideRoadCameraState"]) vipc_server = VisionIpcServer("camerad") diff --git a/tools/replay/lib/ui_helpers.py b/tools/replay/lib/ui_helpers.py index 23f3563084..11b5182a6b 100644 --- a/tools/replay/lib/ui_helpers.py +++ b/tools/replay/lib/ui_helpers.py @@ -7,9 +7,7 @@ import pygame from matplotlib.backends.backend_agg import FigureCanvasAgg -from openpilot.common.transformations.camera import (eon_f_frame_size, eon_f_focal_length, - tici_f_frame_size, tici_f_focal_length, - get_view_frame_from_calib_frame) +from openpilot.common.transformations.camera import get_view_frame_from_calib_frame from openpilot.selfdrive.controls.radard import RADAR_TO_CAMERA @@ -20,9 +18,6 @@ YELLOW = (255, 255, 0) BLACK = (0, 0, 0) WHITE = (255, 255, 255) -_FULL_FRAME_SIZE = { -} - class UIParams: lidar_x, lidar_y, lidar_zoom = 384, 960, 6 lidar_car_x, lidar_car_y = lidar_x / 2., lidar_y / 1.1 @@ -32,45 +27,13 @@ class UIParams: car_color = 110 UP = UIParams -_BB_TO_FULL_FRAME = {} -_CALIB_BB_TO_FULL = {} -_FULL_FRAME_TO_BB = {} -_INTRINSICS = {} - -eon_f_qcam_frame_size = (480, 360) -tici_f_qcam_frame_size = (528, 330) - -cams = [(eon_f_frame_size, eon_f_focal_length, eon_f_frame_size), - (tici_f_frame_size, tici_f_focal_length, tici_f_frame_size), - (eon_f_qcam_frame_size, eon_f_focal_length, eon_f_frame_size), - (tici_f_qcam_frame_size, tici_f_focal_length, tici_f_frame_size)] -for size, focal, full_size in cams: - sz = size[0] * size[1] - _BB_SCALE = size[0] / 640. - _BB_TO_FULL_FRAME[sz] = np.asarray([ - [_BB_SCALE, 0., 0.], - [0., _BB_SCALE, 0.], - [0., 0., 1.]]) - calib_scale = full_size[0] / 640. - _CALIB_BB_TO_FULL[sz] = np.asarray([ - [calib_scale, 0., 0.], - [0., calib_scale, 0.], - [0., 0., 1.]]) - _FULL_FRAME_TO_BB[sz] = np.linalg.inv(_BB_TO_FULL_FRAME[sz]) - _FULL_FRAME_SIZE[sz] = (size[0], size[1]) - _INTRINSICS[sz] = np.array([ - [focal, 0., full_size[0] / 2.], - [0., focal, full_size[1] / 2.], - [0., 0., 1.]]) - - METER_WIDTH = 20 class Calibration: - def __init__(self, num_px, rpy, intrinsic): + def __init__(self, num_px, rpy, intrinsic, calib_scale): self.intrinsic = intrinsic self.extrinsics_matrix = get_view_frame_from_calib_frame(rpy[0], rpy[1], rpy[2], 0.0)[:,:3] - self.zoom = _CALIB_BB_TO_FULL[num_px][0, 0] + self.zoom = calib_scale def car_space_to_ff(self, x, y, z): car_space_projective = np.column_stack((x, y, z)).T diff --git a/tools/replay/ui.py b/tools/replay/ui.py index be80166e76..126340afc8 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -10,8 +10,9 @@ import pygame import cereal.messaging as messaging from openpilot.common.numpy_fast import clip from openpilot.common.basedir import BASEDIR -from openpilot.tools.replay.lib.ui_helpers import (_BB_TO_FULL_FRAME, UP, - _INTRINSICS, BLACK, GREEN, +from openpilot.common.transformations.camera import DEVICE_CAMERAS +from openpilot.tools.replay.lib.ui_helpers import (UP, + BLACK, GREEN, YELLOW, Calibration, get_blank_lid_overlay, init_plots, maybe_update_radar_points, plot_lead, @@ -55,7 +56,7 @@ def ui_thread(addr): top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y), 0, 8) sm = messaging.SubMaster(['carState', 'longitudinalPlan', 'carControl', 'radarState', 'liveCalibration', 'controlsState', - 'liveTracks', 'modelV2', 'liveParameters'], addr=addr) + 'liveTracks', 'modelV2', 'liveParameters', 'roadCameraState'], addr=addr) img = np.zeros((480, 640, 3), dtype='uint8') imgff = None @@ -112,20 +113,27 @@ def ui_thread(addr): vipc_client.connect(True) yuv_img_raw = vipc_client.recv() - if yuv_img_raw is None or not yuv_img_raw.data.any(): continue + sm.update(0) + + camera = DEVICE_CAMERAS[("three", str(sm['roadCameraState'].sensor))] + imgff = np.frombuffer(yuv_img_raw.data, dtype=np.uint8).reshape((len(yuv_img_raw.data) // vipc_client.stride, vipc_client.stride)) num_px = vipc_client.width * vipc_client.height rgb = cv2.cvtColor(imgff[:vipc_client.height * 3 // 2, :vipc_client.width], cv2.COLOR_YUV2RGB_NV12) - zoom_matrix = _BB_TO_FULL_FRAME[num_px] + qcam = "QCAM" in os.environ + bb_scale = (528 if qcam else camera.fcam.width) / 640. + calib_scale = camera.fcam.width / 640. + zoom_matrix = np.asarray([ + [bb_scale, 0., 0.], + [0., bb_scale, 0.], + [0., 0., 1.]]) cv2.warpAffine(rgb, zoom_matrix[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP) - intrinsic_matrix = _INTRINSICS[num_px] - - sm.update(0) + intrinsic_matrix = camera.fcam.intrinsics w = sm['controlsState'].lateralControlState.which() if w == 'lqrStateDEPRECATED': @@ -165,7 +173,7 @@ def ui_thread(addr): if sm.updated['liveCalibration'] and num_px: rpyCalib = np.asarray(sm['liveCalibration'].rpyCalib) - calibration = Calibration(num_px, rpyCalib, intrinsic_matrix) + calibration = Calibration(num_px, rpyCalib, intrinsic_matrix, calib_scale) # *** blits *** pygame.surfarray.blit_array(camera_surface, img.swapaxes(0, 1))