mawei/onepilot
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: c0946c8bdbaac0029c67e0b2ba4e591f591790eb
This commit is contained in:
ZwX1616
2020-11-05 13:22:28 -08:00
committed by GitHub
parent fcb995e19c
commit 4dc3fb994e
5 changed files with 120 additions and 86 deletions
Download Patch File Download Diff File Expand all files Collapse all files

94
common/transformations/camera.py
View File

@@ -1,29 +1,67 @@
import numpy as np
import common.transformations.orientation as orient
FULL_FRAME_SIZE = (1164, 874)
W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1]
eon_focal_length = FOCAL = 910.0
import common.transformations.orientation as orient
from common.hardware import TICI
## -- 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
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_intrinsics = np.array([
[FOCAL, 0., W/2.],
[ 0., FOCAL, H/2.],
[ 0., 0., 1.]])
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
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)
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
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
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
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