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

refactor camera transformations (#31818)

Browse Source 
* refactor camera transormations

* update users

* more stuff

* more fix

* swap

* tici

* lil shorter
pull/31823/head
Adeeb Shihadeh 1 year ago committed by GitHub
parent 6a7a998058
commit e3589e4b5c
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
12 changed files with 121 additions and 142 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 100
      common/transformations/camera.py
  2. 21
      common/transformations/model.py
  3. 6
      selfdrive/modeld/modeld.py
  4. 11
      selfdrive/modeld/tests/test_modeld.py
  5. 6
      selfdrive/monitoring/driver_monitor.py
  6. 16
      selfdrive/test/process_replay/migration.py
  7. 4
      selfdrive/test/process_replay/regen.py
  8. 10
      selfdrive/test/process_replay/vision_meta.py
  9. 11
      selfdrive/ui/tests/test_ui/run.py
  10. 9
      system/loggerd/tests/test_loggerd.py
  11. 43
      tools/replay/lib/ui_helpers.py
  12. 26
      tools/replay/ui.py

100
common/transformations/camera.py
Unescape View File

@ -1,55 +1,55 @@
import itertools
import numpy as np import numpy as np
from dataclasses import dataclass
import openpilot.common.transformations.orientation as orient import openpilot.common.transformations.orientation as orient
## -- hardcoded hardware params -- ## -- hardcoded hardware params --
eon_f_focal_length = 910.0 @dataclass(frozen=True)
eon_d_focal_length = 650.0 class CameraConfig:
tici_f_focal_length = 2648.0 width: int
tici_e_focal_length = tici_d_focal_length = 567.0 # probably wrong? magnification is not consistent across frame height: int
focal_length: float
eon_f_frame_size = (1164, 874)
eon_d_frame_size = (816, 612) @property
tici_f_frame_size = tici_e_frame_size = tici_d_frame_size = (1928, 1208) def intrinsics(self):
# aka 'K' aka camera_frame_from_view_frame
# aka 'K' aka camera_frame_from_view_frame return np.array([
eon_fcam_intrinsics = np.array([ [self.focal_length, 0.0, float(self.width)/2],
[eon_f_focal_length, 0.0, float(eon_f_frame_size[0])/2], [0.0, self.focal_length, float(self.height)/2],
[0.0, eon_f_focal_length, float(eon_f_frame_size[1])/2], [0.0, 0.0, 1.0]
[0.0, 0.0, 1.0]]) ])
eon_intrinsics = eon_fcam_intrinsics # xx
@property
eon_dcam_intrinsics = np.array([ def intrinsics_inv(self):
[eon_d_focal_length, 0.0, float(eon_d_frame_size[0])/2], # aka 'K_inv' aka view_frame_from_camera_frame
[0.0, eon_d_focal_length, float(eon_d_frame_size[1])/2], return np.linalg.inv(self.intrinsics)
[0.0, 0.0, 1.0]])
@dataclass(frozen=True)
tici_fcam_intrinsics = np.array([ class DeviceCameraConfig:
[tici_f_focal_length, 0.0, float(tici_f_frame_size[0])/2], fcam: CameraConfig
[0.0, tici_f_focal_length, float(tici_f_frame_size[1])/2], dcam: CameraConfig
[0.0, 0.0, 1.0]]) ecam: CameraConfig
tici_dcam_intrinsics = np.array([ ar_ox_fisheye = CameraConfig(1928, 1208, 567.0) # focal length probably wrong? magnification is not consistent across frame
[tici_d_focal_length, 0.0, float(tici_d_frame_size[0])/2], ar_ox_config = DeviceCameraConfig(CameraConfig(1928, 1208, 2648.0), ar_ox_fisheye, ar_ox_fisheye)
[0.0, tici_d_focal_length, float(tici_d_frame_size[1])/2], os_fisheye = CameraConfig(2688, 1520, 567.0 / 2 * 3)
[0.0, 0.0, 1.0]]) os_config = DeviceCameraConfig(CameraConfig(2688, 1520, 2648.0 * 2 / 3), os_fisheye, os_fisheye)
tici_ecam_intrinsics = tici_dcam_intrinsics DEVICE_CAMERAS = {
# A "device camera" is defined by a device type and sensor
# aka 'K_inv' aka view_frame_from_camera_frame
eon_fcam_intrinsics_inv = np.linalg.inv(eon_fcam_intrinsics) # sensor type was never set on eon/neo/two
eon_intrinsics_inv = eon_fcam_intrinsics_inv # xx ("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_fcam_intrinsics_inv = np.linalg.inv(tici_fcam_intrinsics) ("tici", "unknown"): ar_ox_config,
tici_ecam_intrinsics_inv = np.linalg.inv(tici_ecam_intrinsics)
# before deviceState.deviceType was set, assume tici AR config
("unknown", "ar0231"): ar_ox_config,
FULL_FRAME_SIZE = tici_f_frame_size ("unknown", "ox03c10"): ar_ox_config,
FOCAL = tici_f_focal_length }
fcam_intrinsics = tici_fcam_intrinsics 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})
W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1]
# device/mesh : x->forward, y-> right, z->down # device/mesh : x->forward, y-> right, z->down
# view : x->right, y->down, z->forward # 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])) -(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 # normalizes image coordinates
# accepts single pt or array of pts # accepts single pt or array of pts
intrinsics_inv = np.linalg.inv(intrinsics) 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) 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 # denormalizes image coordinates
# accepts single pt or array of pts # accepts single pt or array of pts
img_pts = np.array(img_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) 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) vp_norm = normalize(vp, intrinsics)
yaw_calib = np.arctan(vp_norm[0]) yaw_calib = np.arctan(vp_norm[0])
pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib)) pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib))

21
common/transformations/model.py
Unescape View File

@ -1,19 +1,11 @@
import numpy as np import numpy as np
from openpilot.common.transformations.orientation import rot_from_euler from openpilot.common.transformations.orientation import rot_from_euler
from openpilot.common.transformations.camera import ( from openpilot.common.transformations.camera import get_view_frame_from_calib_frame, view_frame_from_device_frame
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
SEGNET_SIZE = (512, 384) 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 # MED model
MEDMODEL_INPUT_SIZE = (512, 256) MEDMODEL_INPUT_SIZE = (512, 256)
MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2) 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]) 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 # 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: def get_warp_matrix(device_from_calib_euler: np.ndarray, intrinsics: np.ndarray, bigmodel_frame: bool = False) -> 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
calib_from_model = calib_from_sbigmodel if bigmodel_frame else calib_from_medmodel calib_from_model = calib_from_sbigmodel if bigmodel_frame else calib_from_medmodel
device_from_calib = rot_from_euler(device_from_calib_euler) 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 warp_matrix: np.ndarray = camera_from_calib @ calib_from_model
return warp_matrix return warp_matrix

6
selfdrive/modeld/modeld.py
Unescape View File

@ -13,6 +13,7 @@ from openpilot.common.swaglog import cloudlog
from openpilot.common.params import Params from openpilot.common.params import Params
from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.realtime import config_realtime_process 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.common.transformations.model import get_warp_matrix
from openpilot.selfdrive import sentry from openpilot.selfdrive import sentry
from openpilot.selfdrive.car.car_helpers import get_demo_car_params 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) 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']: if sm.updated["liveCalibration"] and sm.seen['roadCameraState'] and sm.seen['deviceState']:
device_from_calib_euler = np.array(sm["liveCalibration"].rpyCalib, dtype=np.float32) 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) dc = DEVICE_CAMERAS[(str(sm['deviceState'].deviceType), str(sm['roadCameraState'].sensor))]
model_transform_extra = get_warp_matrix(device_from_calib_euler, True, True).astype(np.float32) 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 live_calib_seen = True
traffic_convention = np.zeros(2) traffic_convention = np.zeros(2)

11
selfdrive/modeld/tests/test_modeld.py
Unescape View File

@ -5,13 +5,14 @@ import random
import cereal.messaging as messaging import cereal.messaging as messaging
from cereal.visionipc import VisionIpcServer, VisionStreamType 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.common.realtime import DT_MDL
from openpilot.selfdrive.car.car_helpers import write_car_param from openpilot.selfdrive.car.car_helpers import write_car_param
from openpilot.selfdrive.manager.process_config import managed_processes from openpilot.selfdrive.manager.process_config import managed_processes
from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state 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() IMG_BYTES = IMG.flatten().tobytes()
@ -19,9 +20,9 @@ class TestModeld(unittest.TestCase):
def setUp(self): def setUp(self):
self.vipc_server = VisionIpcServer("camerad") 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_ROAD, 40, False, CAM.width, CAM.height)
self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_DRIVER, 40, False, *tici_f_frame_size) 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, *tici_f_frame_size) self.vipc_server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, CAM.width, CAM.height)
self.vipc_server.start_listener() self.vipc_server.start_listener()
write_car_param() write_car_param()

6
selfdrive/monitoring/driver_monitor.py
Unescape View File

@ -5,7 +5,7 @@ from openpilot.common.numpy_fast import interp
from openpilot.common.realtime import DT_DMON from openpilot.common.realtime import DT_DMON
from openpilot.common.filter_simple import FirstOrderFilter from openpilot.common.filter_simple import FirstOrderFilter
from openpilot.common.stat_live import RunningStatFilter 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 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 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 # model output refers to center of undistorted+leveled image
EFL = 598.0 # focal length in K 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: class DistractedType:
NOT_DISTRACTED = 0 NOT_DISTRACTED = 0

16
selfdrive/test/process_replay/migration.py
Unescape View File

@ -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_sensorEvents(lr, old_logtime)
msgs = migrate_carParams(msgs, old_logtime) msgs = migrate_carParams(msgs, old_logtime)
msgs = migrate_gpsLocation(msgs) msgs = migrate_gpsLocation(msgs)
msgs = migrate_deviceState(msgs)
if manager_states: if manager_states:
msgs = migrate_managerState(msgs) msgs = migrate_managerState(msgs)
if panda_states: if panda_states:
@ -52,6 +53,21 @@ def migrate_gpsLocation(lr):
return all_msgs 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): def migrate_pandaStates(lr):
all_msgs = [] all_msgs = []
# TODO: safety param migration should be handled automatically # TODO: safety param migration should be handled automatically

4
selfdrive/test/process_replay/regen.py
Unescape View File

@ -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, \ 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 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.selfdrive.test.update_ci_routes import upload_route
from openpilot.tools.lib.route import Route from openpilot.tools.lib.route import Route
from openpilot.tools.lib.framereader import FrameReader, BaseFrameReader, FrameType from openpilot.tools.lib.framereader import FrameReader, BaseFrameReader, FrameType
@ -37,7 +37,7 @@ class DummyFrameReader(BaseFrameReader):
@staticmethod @staticmethod
def zero_dcamera(): def zero_dcamera():
return DummyFrameReader(*DRIVER_FRAME_SIZES["tici"], 1200, 0) return DummyFrameReader(*DRIVER_CAMERA_FRAME_SIZES[("tici", "ar0231")], 1200, 0)
def regen_segment( def regen_segment(

10
selfdrive/test/process_replay/vision_meta.py
Unescape View File

@ -1,17 +1,17 @@
from collections import namedtuple from collections import namedtuple
from cereal.visionipc import VisionStreamType from cereal.visionipc import VisionStreamType
from openpilot.common.realtime import DT_MDL, DT_DMON 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"]) 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} ROAD_CAMERA_FRAME_SIZES = {k: (v.dcam.width, v.dcam.height) for k, v in DEVICE_CAMERAS.items()}
WIDE_ROAD_CAMERA_FRAME_SIZES = {"tici": tici_e_frame_size, "tizi": tici_e_frame_size} 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_FRAME_SIZES = {"tici": tici_d_frame_size, "tizi": tici_d_frame_size, "neo": eon_d_frame_size} DRIVER_CAMERA_FRAME_SIZES = {k: (v.dcam.width, v.dcam.height) for k, v in DEVICE_CAMERAS.items()}
VIPC_STREAM_METADATA = [ VIPC_STREAM_METADATA = [
# metadata: (state_msg_type, encode_msg_type, stream_type, dt, frame_sizes) # metadata: (state_msg_type, encode_msg_type, stream_type, dt, frame_sizes)
("roadCameraState", "roadEncodeIdx", VisionStreamType.VISION_STREAM_ROAD, DT_MDL, ROAD_CAMERA_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), ("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),
] ]

11
selfdrive/ui/tests/test_ui/run.py
Unescape View File

@ -18,7 +18,7 @@ from cereal.messaging import SubMaster, PubMaster
from openpilot.common.mock import mock_messages from openpilot.common.mock import mock_messages
from openpilot.common.params import Params from openpilot.common.params import Params
from openpilot.common.realtime import DT_MDL 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.helpers import with_processes
from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state from openpilot.selfdrive.test.process_replay.vision_meta import meta_from_camera_state
from openpilot.tools.webcam.camera import Camera from openpilot.tools.webcam.camera import Camera
@ -69,15 +69,16 @@ def setup_onroad(click, pm: PubMaster):
pm.send("pandaStates", dat) pm.send("pandaStates", dat)
d = DEVICE_CAMERAS[("tici", "ar0231")]
server = VisionIpcServer("camerad") server = VisionIpcServer("camerad")
server.create_buffers(VisionStreamType.VISION_STREAM_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, *tici_f_frame_size) 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, *tici_f_frame_size) server.create_buffers(VisionStreamType.VISION_STREAM_WIDE_ROAD, 40, False, d.fcam.width, d.fcam.height)
server.start_listener() server.start_listener()
time.sleep(0.5) # give time for vipc server to start 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() IMG_BYTES = IMG.flatten().tobytes()
cams = ('roadCameraState', 'wideRoadCameraState') cams = ('roadCameraState', 'wideRoadCameraState')

9
system/loggerd/tests/test_loggerd.py
Unescape View File

@ -24,7 +24,7 @@ from openpilot.system.version import get_version
from openpilot.tools.lib.helpers import RE from openpilot.tools.lib.helpers import RE
from openpilot.tools.lib.logreader import LogReader from openpilot.tools.lib.logreader import LogReader
from cereal.visionipc import VisionIpcServer, VisionStreamType 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 SentinelType = log.Sentinel.SentinelType
@ -142,10 +142,11 @@ class TestLoggerd:
os.environ["LOGGERD_TEST"] = "1" os.environ["LOGGERD_TEST"] = "1"
Params().put("RecordFront", "1") Params().put("RecordFront", "1")
d = DEVICE_CAMERAS[("tici", "ar0231")]
expected_files = {"rlog", "qlog", "qcamera.ts", "fcamera.hevc", "dcamera.hevc", "ecamera.hevc"} 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"), streams = [(VisionStreamType.VISION_STREAM_ROAD, (d.fcam.width, d.fcam.height, 2048*2346, 2048, 2048*1216), "roadCameraState"),
(VisionStreamType.VISION_STREAM_DRIVER, (*tici_d_frame_size, 2048*2346, 2048, 2048*1216), "driverCameraState"), (VisionStreamType.VISION_STREAM_DRIVER, (d.dcam.width, d.dcam.height, 2048*2346, 2048, 2048*1216), "driverCameraState"),
(VisionStreamType.VISION_STREAM_WIDE_ROAD, (*tici_e_frame_size, 2048*2346, 2048, 2048*1216), "wideRoadCameraState")] (VisionStreamType.VISION_STREAM_WIDE_ROAD, (d.ecam.width, d.ecam.height, 2048*2346, 2048, 2048*1216), "wideRoadCameraState")]
pm = messaging.PubMaster(["roadCameraState", "driverCameraState", "wideRoadCameraState"]) pm = messaging.PubMaster(["roadCameraState", "driverCameraState", "wideRoadCameraState"])
vipc_server = VisionIpcServer("camerad") vipc_server = VisionIpcServer("camerad")

43
tools/replay/lib/ui_helpers.py
Unescape View File

@ -7,9 +7,7 @@ import pygame
from matplotlib.backends.backend_agg import FigureCanvasAgg from matplotlib.backends.backend_agg import FigureCanvasAgg
from openpilot.common.transformations.camera import (eon_f_frame_size, eon_f_focal_length, from openpilot.common.transformations.camera import get_view_frame_from_calib_frame
tici_f_frame_size, tici_f_focal_length,
get_view_frame_from_calib_frame)
from openpilot.selfdrive.controls.radard import RADAR_TO_CAMERA from openpilot.selfdrive.controls.radard import RADAR_TO_CAMERA
@ -20,9 +18,6 @@ YELLOW = (255, 255, 0)
BLACK = (0, 0, 0) BLACK = (0, 0, 0)
WHITE = (255, 255, 255) WHITE = (255, 255, 255)
_FULL_FRAME_SIZE = {
}
class UIParams: class UIParams:
lidar_x, lidar_y, lidar_zoom = 384, 960, 6 lidar_x, lidar_y, lidar_zoom = 384, 960, 6
lidar_car_x, lidar_car_y = lidar_x / 2., lidar_y / 1.1 lidar_car_x, lidar_car_y = lidar_x / 2., lidar_y / 1.1
@ -32,45 +27,13 @@ class UIParams:
car_color = 110 car_color = 110
UP = UIParams 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 METER_WIDTH = 20
class Calibration: class Calibration:
def __init__(self, num_px, rpy, intrinsic): def __init__(self, num_px, rpy, intrinsic, calib_scale):
self.intrinsic = intrinsic self.intrinsic = intrinsic
self.extrinsics_matrix = get_view_frame_from_calib_frame(rpy[0], rpy[1], rpy[2], 0.0)[:,:3] 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): def car_space_to_ff(self, x, y, z):
car_space_projective = np.column_stack((x, y, z)).T car_space_projective = np.column_stack((x, y, z)).T

26
tools/replay/ui.py
Unescape View File

@ -10,8 +10,9 @@ import pygame
import cereal.messaging as messaging import cereal.messaging as messaging
from openpilot.common.numpy_fast import clip from openpilot.common.numpy_fast import clip
from openpilot.common.basedir import BASEDIR from openpilot.common.basedir import BASEDIR
from openpilot.tools.replay.lib.ui_helpers import (_BB_TO_FULL_FRAME, UP, from openpilot.common.transformations.camera import DEVICE_CAMERAS
_INTRINSICS, BLACK, GREEN, from openpilot.tools.replay.lib.ui_helpers import (UP,
BLACK, GREEN,
YELLOW, Calibration, YELLOW, Calibration,
get_blank_lid_overlay, init_plots, get_blank_lid_overlay, init_plots,
maybe_update_radar_points, plot_lead, 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) top_down_surface = pygame.surface.Surface((UP.lidar_x, UP.lidar_y), 0, 8)
sm = messaging.SubMaster(['carState', 'longitudinalPlan', 'carControl', 'radarState', 'liveCalibration', 'controlsState', 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') img = np.zeros((480, 640, 3), dtype='uint8')
imgff = None imgff = None
@ -112,20 +113,27 @@ def ui_thread(addr):
vipc_client.connect(True) vipc_client.connect(True)
yuv_img_raw = vipc_client.recv() yuv_img_raw = vipc_client.recv()
if yuv_img_raw is None or not yuv_img_raw.data.any(): if yuv_img_raw is None or not yuv_img_raw.data.any():
continue 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)) 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 num_px = vipc_client.width * vipc_client.height
rgb = cv2.cvtColor(imgff[:vipc_client.height * 3 // 2, :vipc_client.width], cv2.COLOR_YUV2RGB_NV12) 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) cv2.warpAffine(rgb, zoom_matrix[:2], (img.shape[1], img.shape[0]), dst=img, flags=cv2.WARP_INVERSE_MAP)
intrinsic_matrix = _INTRINSICS[num_px] intrinsic_matrix = camera.fcam.intrinsics
sm.update(0)
w = sm['controlsState'].lateralControlState.which() w = sm['controlsState'].lateralControlState.which()
if w == 'lqrStateDEPRECATED': if w == 'lqrStateDEPRECATED':
@ -165,7 +173,7 @@ def ui_thread(addr):
if sm.updated['liveCalibration'] and num_px: if sm.updated['liveCalibration'] and num_px:
rpyCalib = np.asarray(sm['liveCalibration'].rpyCalib) rpyCalib = np.asarray(sm['liveCalibration'].rpyCalib)
calibration = Calibration(num_px, rpyCalib, intrinsic_matrix) calibration = Calibration(num_px, rpyCalib, intrinsic_matrix, calib_scale)
# *** blits *** # *** blits ***
pygame.surfarray.blit_array(camera_surface, img.swapaxes(0, 1)) pygame.surfarray.blit_array(camera_surface, img.swapaxes(0, 1))

Write Preview
Loading…
Cancel
Save