generalize camera assumptions (commaai#2423)

* clean up 1

* complain

* cleanup

* make models api generic

* clean up 2

* cleanup

* remove unused

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,17 +132,17 @@ 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)
   input_shape = img_pts.shape
   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)

common/transformations/model.py

@@ -1,80 +1,79 @@
 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
 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 @@
 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

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<float, 3, 3> fcam_intrinsics;
 #ifndef QCOM2
-  Eigen::Matrix<float, 3, 3> 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<float, 3, 3> 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<float, 3, 3> 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);

tools/replay/lib/ui_helpers.py

@@ -6,6 +6,8 @@
 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 @@
 _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([

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