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

generalize camera assumptions (#2423)

Browse Source 
* clean up 1

* complain

* cleanup

* make models api generic

* clean up 2

* cleanup

* remove unused
old-commit-hash: c0946c8bdb
commatwo_master
ZwX1616 5 years ago committed by GitHub
parent fcb995e19c
commit 4dc3fb994e
5 changed files with 119 additions and 85 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. 92
      common/transformations/camera.py
  2. 95
      common/transformations/model.py
  3. 9
      selfdrive/modeld/modeld.cc
  4. 6
      tools/replay/lib/ui_helpers.py
  5. 2
      tools/replay/ui.py

92
common/transformations/camera.py
Unescape View File

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

95
common/transformations/model.py
Unescape View File

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

9
selfdrive/modeld/modeld.cc
Unescape View File

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

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

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

2
tools/replay/ui.py
Unescape View File

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

Write Preview
Loading…
Cancel
Save