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
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 94
      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

94
common/transformations/camera.py
Unescape View File

@ -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)
@dataclass(frozen=True)
class CameraConfig:
width: int
height: int
focal_length: float
@property
def intrinsics(self):
# 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
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
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]
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))

21
common/transformations/model.py
Unescape View File

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

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.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)

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

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

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

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

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, \
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(

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

@ -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),
]

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.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')

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.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")

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

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

26
tools/replay/ui.py
Unescape View File

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

Write Preview
Loading…
Cancel
Save