diff --git a/common/transformations/camera.py b/common/transformations/camera.py index b406c33fd7..e1fbcb5460 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -1,29 +1,67 @@ import numpy as np + import common.transformations.orientation as orient +from common.hardware import TICI -FULL_FRAME_SIZE = (1164, 874) -W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1] -eon_focal_length = FOCAL = 910.0 +## -- hardcoded hardware params -- +eon_f_focal_length = 910.0 +eon_d_focal_length = 860.0 +leon_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 -# aka 'K' aka camera_frame_from_view_frame -eon_intrinsics = np.array([ - [FOCAL, 0., W/2.], - [ 0., FOCAL, H/2.], - [ 0., 0., 1.]]) +eon_f_frame_size = (1164, 874) +eon_d_frame_size = (1152, 864) +leon_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 leon_dcam_intrinsics = np.array([ - [650, 0, 816//2], - [ 0, 650, 612//2], - [ 0, 0, 1]]) + [leon_d_focal_length, 0.0, float(leon_d_frame_size[0])/2], + [0.0, leon_d_focal_length, float(leon_d_frame_size[1])/2], + [0.0, 0.0, 1.0]]) eon_dcam_intrinsics = np.array([ - [860, 0, 1152//2], - [ 0, 860, 864//2], - [ 0, 0, 1]]) + [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_intrinsics_inv = np.linalg.inv(eon_intrinsics) +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) + + +if not TICI: + FULL_FRAME_SIZE = eon_f_frame_size + FOCAL = eon_f_focal_length + fcam_intrinsics = eon_fcam_intrinsics +else: + 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] # device/mesh : x->forward, y-> right, z->down @@ -69,9 +107,9 @@ def vp_from_ke(m): return (m[0, 0]/m[2, 0], m[1, 0]/m[2, 0]) -def vp_from_rpy(rpy): +def vp_from_rpy(rpy, intrinsics=fcam_intrinsics): e = get_view_frame_from_road_frame(rpy[0], rpy[1], rpy[2], 1.22) - ke = np.dot(eon_intrinsics, e) + ke = np.dot(intrinsics, e) return vp_from_ke(ke) @@ -81,7 +119,7 @@ def roll_from_ke(m): -(m[0, 0] - m[0, 1] * m[2, 0] / m[2, 1])) -def normalize(img_pts, intrinsics=eon_intrinsics): +def normalize(img_pts, intrinsics=fcam_intrinsics): # normalizes image coordinates # accepts single pt or array of pts intrinsics_inv = np.linalg.inv(intrinsics) @@ -94,7 +132,7 @@ def normalize(img_pts, intrinsics=eon_intrinsics): return img_pts_normalized[:, :2].reshape(input_shape) -def denormalize(img_pts, intrinsics=eon_intrinsics): +def denormalize(img_pts, intrinsics=fcam_intrinsics, width=W, height=H): # denormalizes image coordinates # accepts single pt or array of pts img_pts = np.array(img_pts) @@ -102,9 +140,9 @@ def denormalize(img_pts, intrinsics=eon_intrinsics): img_pts = np.atleast_2d(img_pts) img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0], 1)))) img_pts_denormalized = img_pts.dot(intrinsics.T) - img_pts_denormalized[img_pts_denormalized[:, 0] > W] = np.nan + img_pts_denormalized[img_pts_denormalized[:, 0] > width] = np.nan img_pts_denormalized[img_pts_denormalized[:, 0] < 0] = np.nan - img_pts_denormalized[img_pts_denormalized[:, 1] > H] = np.nan + img_pts_denormalized[img_pts_denormalized[:, 1] > height] = np.nan img_pts_denormalized[img_pts_denormalized[:, 1] < 0] = np.nan return img_pts_denormalized[:, :2].reshape(input_shape) @@ -137,18 +175,10 @@ def img_from_device(pt_device): return pt_img.reshape(input_shape)[:, :2] -def get_camera_frame_from_calib_frame(camera_frame_from_road_frame): +def get_camera_frame_from_calib_frame(camera_frame_from_road_frame, intrinsics=fcam_intrinsics): camera_frame_from_ground = camera_frame_from_road_frame[:, (0, 1, 3)] - calib_frame_from_ground = np.dot(eon_intrinsics, + calib_frame_from_ground = np.dot(intrinsics, get_view_frame_from_road_frame(0, 0, 0, 1.22))[:, (0, 1, 3)] ground_from_calib_frame = np.linalg.inv(calib_frame_from_ground) camera_frame_from_calib_frame = np.dot(camera_frame_from_ground, ground_from_calib_frame) return camera_frame_from_calib_frame - - -def pretransform_from_calib(calib): - roll, pitch, yaw, height = calib - view_frame_from_road_frame = get_view_frame_from_road_frame(roll, pitch, yaw, height) - camera_frame_from_road_frame = np.dot(eon_intrinsics, view_frame_from_road_frame) - camera_frame_from_calib_frame = get_camera_frame_from_calib_frame(camera_frame_from_road_frame) - return np.linalg.inv(camera_frame_from_calib_frame) diff --git a/common/transformations/model.py b/common/transformations/model.py index f4fb803eb9..481240f999 100644 --- a/common/transformations/model.py +++ b/common/transformations/model.py @@ -1,34 +1,33 @@ import numpy as np -from common.transformations.camera import (FULL_FRAME_SIZE, eon_focal_length, +from common.transformations.camera import (FULL_FRAME_SIZE, + FOCAL, get_view_frame_from_road_frame, get_view_frame_from_calib_frame, vp_from_ke) # segnet - SEGNET_SIZE = (512, 384) -segnet_frame_from_camera_frame = np.array([ - [float(SEGNET_SIZE[0])/FULL_FRAME_SIZE[0], 0., ], - [ 0., float(SEGNET_SIZE[1])/FULL_FRAME_SIZE[1]]]) - +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 # model - MODEL_INPUT_SIZE = (320, 160) MODEL_YUV_SIZE = (MODEL_INPUT_SIZE[0], MODEL_INPUT_SIZE[1] * 3 // 2) -MODEL_CX = MODEL_INPUT_SIZE[0]/2. +MODEL_CX = MODEL_INPUT_SIZE[0] / 2. MODEL_CY = 21. -model_zoom = 1.25 +model_fl = 728.0 model_height = 1.22 # canonical model transform -model_intrinsics = np.array( - [[ eon_focal_length / model_zoom, 0. , MODEL_CX], - [ 0. , eon_focal_length / model_zoom, MODEL_CY], - [ 0. , 0. , 1.]]) +model_intrinsics = np.array([ + [model_fl, 0.0, MODEL_CX], + [0.0, model_fl, MODEL_CY], + [0.0, 0.0, 1.0]]) # MED model @@ -36,45 +35,45 @@ MEDMODEL_INPUT_SIZE = (512, 256) MEDMODEL_YUV_SIZE = (MEDMODEL_INPUT_SIZE[0], MEDMODEL_INPUT_SIZE[1] * 3 // 2) MEDMODEL_CY = 47.6 -medmodel_zoom = 1. -medmodel_intrinsics = np.array( - [[ eon_focal_length / medmodel_zoom, 0. , 0.5 * MEDMODEL_INPUT_SIZE[0]], - [ 0. , eon_focal_length / medmodel_zoom, MEDMODEL_CY], - [ 0. , 0. , 1.]]) +medmodel_fl = 910.0 +medmodel_intrinsics = np.array([ + [medmodel_fl, 0.0, 0.5 * MEDMODEL_INPUT_SIZE[0]], + [0.0, medmodel_fl, MEDMODEL_CY], + [0.0, 0.0, 1.0]]) + # CAL model CALMODEL_INPUT_SIZE = (512, 256) CALMODEL_YUV_SIZE = (CALMODEL_INPUT_SIZE[0], CALMODEL_INPUT_SIZE[1] * 3 // 2) CALMODEL_CY = 47.6 -calmodel_zoom = 1.5 -calmodel_intrinsics = np.array( - [[ eon_focal_length / calmodel_zoom, 0. , 0.5 * CALMODEL_INPUT_SIZE[0]], - [ 0. , eon_focal_length / calmodel_zoom, CALMODEL_CY], - [ 0. , 0. , 1.]]) +calmodel_fl = 606.7 +calmodel_intrinsics = np.array([ + [calmodel_fl, 0.0, 0.5 * CALMODEL_INPUT_SIZE[0]], + [0.0, calmodel_fl, CALMODEL_CY], + [0.0, 0.0, 1.0]]) # BIG model - BIGMODEL_INPUT_SIZE = (1024, 512) BIGMODEL_YUV_SIZE = (BIGMODEL_INPUT_SIZE[0], BIGMODEL_INPUT_SIZE[1] * 3 // 2) -bigmodel_zoom = 1. -bigmodel_intrinsics = np.array( - [[ eon_focal_length / bigmodel_zoom, 0. , 0.5 * BIGMODEL_INPUT_SIZE[0]], - [ 0. , eon_focal_length / bigmodel_zoom, 256+MEDMODEL_CY], - [ 0. , 0. , 1.]]) +bigmodel_fl = 910.0 +bigmodel_intrinsics = np.array([ + [bigmodel_fl, 0.0, 0.5 * BIGMODEL_INPUT_SIZE[0]], + [0.0, bigmodel_fl, 256 + MEDMODEL_CY], + [0.0, 0.0, 1.0]]) -# SBIG model (big model with the size of small model) +# SBIG model (big model with the size of small model) SBIGMODEL_INPUT_SIZE = (512, 256) SBIGMODEL_YUV_SIZE = (SBIGMODEL_INPUT_SIZE[0], SBIGMODEL_INPUT_SIZE[1] * 3 // 2) -sbigmodel_zoom = 2. -sbigmodel_intrinsics = np.array( - [[ eon_focal_length / sbigmodel_zoom, 0. , 0.5 * SBIGMODEL_INPUT_SIZE[0]], - [ 0. , eon_focal_length / sbigmodel_zoom, 0.5 * (256+MEDMODEL_CY)], - [ 0. , 0. , 1.]]) +sbigmodel_fl = 455.0 +sbigmodel_intrinsics = np.array([ + [sbigmodel_fl, 0.0, 0.5 * SBIGMODEL_INPUT_SIZE[0]], + [0.0, sbigmodel_fl, 0.5 * (256 + MEDMODEL_CY)], + [0.0, 0.0, 1.0]]) model_frame_from_road_frame = np.dot(model_intrinsics, get_view_frame_from_road_frame(0, 0, 0, model_height)) @@ -91,20 +90,21 @@ medmodel_frame_from_calib_frame = np.dot(medmodel_intrinsics, model_frame_from_bigmodel_frame = np.dot(model_intrinsics, np.linalg.inv(bigmodel_intrinsics)) medmodel_frame_from_bigmodel_frame = np.dot(medmodel_intrinsics, np.linalg.inv(bigmodel_intrinsics)) + # 'camera from model camera' def get_model_height_transform(camera_frame_from_road_frame, height): camera_frame_from_road_ground = np.dot(camera_frame_from_road_frame, np.array([ - [1, 0, 0], - [0, 1, 0], - [0, 0, 0], - [0, 0, 1], + [1, 0, 0], + [0, 1, 0], + [0, 0, 0], + [0, 0, 1], ])) camera_frame_from_road_high = np.dot(camera_frame_from_road_frame, np.array([ - [1, 0, 0], - [0, 1, 0], - [0, 0, height - model_height], - [0, 0, 1], + [1, 0, 0], + [0, 1, 0], + [0, 0, height - model_height], + [0, 0, 1], ])) road_high_from_camera_frame = np.linalg.inv(camera_frame_from_road_high) @@ -115,13 +115,14 @@ def get_model_height_transform(camera_frame_from_road_frame, height): # camera_frame_from_model_frame aka 'warp matrix' # was: calibration.h/CalibrationTransform -def get_camera_frame_from_model_frame(camera_frame_from_road_frame, height=model_height): +def get_camera_frame_from_model_frame(camera_frame_from_road_frame, height=model_height, camera_fl=FOCAL): vp = vp_from_ke(camera_frame_from_road_frame) + model_zoom = camera_fl / model_fl model_camera_from_model_frame = np.array([ - [model_zoom, 0., vp[0] - MODEL_CX * model_zoom], - [ 0., model_zoom, vp[1] - MODEL_CY * model_zoom], - [ 0., 0., 1.], + [model_zoom, 0.0, vp[0] - MODEL_CX * model_zoom], + [0.0, model_zoom, vp[1] - MODEL_CY * model_zoom], + [0.0, 0.0, 1.0], ]) # This function is super slow, so skip it if height is very close to canonical diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index c8c637b3f2..63ff164e35 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -38,16 +38,15 @@ void* live_thread(void *arg) { -1.09890110e-03, 0.00000000e+00, 2.81318681e-01, -1.84808520e-20, 9.00738606e-04,-4.28751576e-02; + Eigen::Matrix fcam_intrinsics; #ifndef QCOM2 - Eigen::Matrix eon_intrinsics; - eon_intrinsics << + fcam_intrinsics << 910.0, 0.0, 582.0, 0.0, 910.0, 437.0, 0.0, 0.0, 1.0; float db_s = 0.5; // debayering does a 2x downscale #else - Eigen::Matrix eon_intrinsics; - eon_intrinsics << + fcam_intrinsics << 2648.0, 0.0, 1928.0/2, 0.0, 2648.0, 1208.0/2, 0.0, 0.0, 1.0; @@ -69,7 +68,7 @@ void* live_thread(void *arg) { extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i]; } - auto camera_frame_from_road_frame = eon_intrinsics * extrinsic_matrix_eigen; + auto camera_frame_from_road_frame = fcam_intrinsics * extrinsic_matrix_eigen; Eigen::Matrix camera_frame_from_ground; camera_frame_from_ground.col(0) = camera_frame_from_road_frame.col(0); camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1); diff --git a/tools/replay/lib/ui_helpers.py b/tools/replay/lib/ui_helpers.py index bb6020c801..e52c7fcc20 100644 --- a/tools/replay/lib/ui_helpers.py +++ b/tools/replay/lib/ui_helpers.py @@ -6,6 +6,8 @@ import matplotlib.pyplot as plt import numpy as np import pygame # pylint: disable=import-error +from common.transformations.camera import (eon_f_frame_size, eon_f_focal_length, + tici_f_frame_size, tici_f_focal_length) from selfdrive.config import RADAR_TO_CAMERA from selfdrive.config import UIParams as UP from selfdrive.controls.lib.lane_planner import (compute_path_pinv, @@ -29,7 +31,9 @@ _FULL_FRAME_SIZE = { _BB_TO_FULL_FRAME = {} _FULL_FRAME_TO_BB = {} _INTRINSICS = {} -for width, height, focal in [(1164, 874, 910), (1928, 1208, 2648)]: +cams = [(eon_f_frame_size[0], eon_f_frame_size[1], eon_f_focal_length), + (tici_f_frame_size[0], tici_f_frame_size[1], tici_f_focal_length)] +for width, height, focal in cams: sz = width * height _BB_SCALE = width / 640. _BB_TO_FULL_FRAME[sz] = np.asarray([ diff --git a/tools/replay/ui.py b/tools/replay/ui.py index 2a4faa1f02..90ec22e21c 100755 --- a/tools/replay/ui.py +++ b/tools/replay/ui.py @@ -196,7 +196,7 @@ def ui_thread(addr, frame_address): if sm.updated['liveCalibration'] and num_px: extrinsic_matrix = np.asarray(sm['liveCalibration'].extrinsicMatrix).reshape(3, 4) ke = intrinsic_matrix.dot(extrinsic_matrix) - warp_matrix = get_camera_frame_from_model_frame(ke) + warp_matrix = get_camera_frame_from_model_frame(ke, camera_fl=intrinsic_matrix[0][0]) calibration = CalibrationTransformsForWarpMatrix(num_px, warp_matrix, intrinsic_matrix, extrinsic_matrix) # draw red pt for lead car in the main img