mirror of
https://github.com/dragonpilot/dragonpilot.git
synced 2026-02-19 11:13:55 +08:00
9dae0bfac4
* tabs to spaces python 2 to 3: https://portingguide.readthedocs.io/en/latest/syntax.html#tabs-and-spaces * use the new except syntax python 2 to 3: https://portingguide.readthedocs.io/en/latest/exceptions.html#the-new-except-syntax * make relative imports absolute python 2 to 3: https://portingguide.readthedocs.io/en/latest/imports.html#absolute-imports * Queue renamed to queue in python 3 Use the six compatibility library to support both python 2 and 3: https://portingguide.readthedocs.io/en/latest/stdlib-reorg.html#renamed-modules * replace dict.has_key() with in python 2 to 3: https://portingguide.readthedocs.io/en/latest/dicts.html#removed-dict-has-key * make dict views compatible with python 3 python 2 to 3: https://portingguide.readthedocs.io/en/latest/dicts.html#dict-views-and-iterators Where needed, wrapping things that will be a view in python 3 with a list(). For example, if it's accessed with [] Python 3 has no iter*() methods, so just using the values() instead of itervalues() as long as it's not too performance intensive. Note that any minor performance hit of using a list instead of a view will go away when switching to python 3. If it is intensive, we could use the six version. * Explicitly use truncating division python 2 to 3: https://portingguide.readthedocs.io/en/latest/numbers.html#division python 3 treats / as float division. When we want the result to be an integer, use // * replace map() with list comprehension where a list result is needed. In python 3, map() returns an iterator. python 2 to 3: https://portingguide.readthedocs.io/en/latest/iterators.html#new-behavior-of-map-and-filter * replace filter() with list comprehension In python 3, filter() returns an interatoooooooooooor. python 2 to 3: https://portingguide.readthedocs.io/en/latest/iterators.html#new-behavior-of-map-and-filter * wrap zip() in list() where we need the result to be a list python 2 to 3: https://portingguide.readthedocs.io/en/latest/iterators.html#new-behavior-of-zip * clean out some lint Removes these pylint warnings: ************* Module selfdrive.car.chrysler.chryslercan W: 15, 0: Unnecessary semicolon (unnecessary-semicolon) W: 16, 0: Unnecessary semicolon (unnecessary-semicolon) W: 25, 0: Unnecessary semicolon (unnecessary-semicolon) ************* Module common.dbc W:101, 0: Anomalous backslash in string: '\?'. String constant might be missing an r prefix. (anomalous-backslash-in-string) ************* Module selfdrive.car.gm.interface R:102, 6: Redefinition of ret.minEnableSpeed type from float to int (redefined-variable-type) R:103, 6: Redefinition of ret.mass type from int to float (redefined-variable-type) ************* Module selfdrive.updated R: 20, 6: Redefinition of r type from int to str (redefined-variable-type)
142 lines
4.7 KiB
Python
142 lines
4.7 KiB
Python
import numpy as np
|
|
|
|
from common.transformations.camera import eon_focal_length, \
|
|
vp_from_ke, \
|
|
get_view_frame_from_road_frame, \
|
|
FULL_FRAME_SIZE
|
|
|
|
# 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]]])
|
|
|
|
|
|
# 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_CY = 21.
|
|
|
|
model_zoom = 1.25
|
|
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.]])
|
|
|
|
|
|
# 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.]])
|
|
|
|
|
|
# BIG model
|
|
|
|
BIGMODEL_INPUT_SIZE = (864, 288)
|
|
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, 0.2 * BIGMODEL_INPUT_SIZE[1]],
|
|
[ 0. , 0. , 1.]])
|
|
|
|
|
|
bigmodel_border = np.array([
|
|
[0,0,1],
|
|
[BIGMODEL_INPUT_SIZE[0], 0, 1],
|
|
[BIGMODEL_INPUT_SIZE[0], BIGMODEL_INPUT_SIZE[1], 1],
|
|
[0, BIGMODEL_INPUT_SIZE[1], 1],
|
|
])
|
|
|
|
|
|
model_frame_from_road_frame = np.dot(model_intrinsics,
|
|
get_view_frame_from_road_frame(0, 0, 0, model_height))
|
|
|
|
bigmodel_frame_from_road_frame = np.dot(bigmodel_intrinsics,
|
|
get_view_frame_from_road_frame(0, 0, 0, model_height))
|
|
|
|
medmodel_frame_from_road_frame = np.dot(medmodel_intrinsics,
|
|
get_view_frame_from_road_frame(0, 0, 0, model_height))
|
|
|
|
model_frame_from_bigmodel_frame = np.dot(model_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],
|
|
]))
|
|
|
|
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],
|
|
]))
|
|
|
|
road_high_from_camera_frame = np.linalg.inv(camera_frame_from_road_high)
|
|
high_camera_from_low_camera = np.dot(camera_frame_from_road_ground, road_high_from_camera_frame)
|
|
|
|
return high_camera_from_low_camera
|
|
|
|
|
|
# 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):
|
|
vp = vp_from_ke(camera_frame_from_road_frame)
|
|
|
|
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.],
|
|
])
|
|
|
|
# This function is super slow, so skip it if height is very close to canonical
|
|
# TODO: speed it up!
|
|
if abs(height - model_height) > 0.001: #
|
|
camera_from_model_camera = get_model_height_transform(camera_frame_from_road_frame, height)
|
|
else:
|
|
camera_from_model_camera = np.eye(3)
|
|
|
|
return np.dot(camera_from_model_camera, model_camera_from_model_frame)
|
|
|
|
|
|
def get_camera_frame_from_bigmodel_frame(camera_frame_from_road_frame):
|
|
camera_frame_from_ground = camera_frame_from_road_frame[:, (0, 1, 3)]
|
|
bigmodel_frame_from_ground = bigmodel_frame_from_road_frame[:, (0, 1, 3)]
|
|
|
|
ground_from_bigmodel_frame = np.linalg.inv(bigmodel_frame_from_ground)
|
|
camera_frame_from_bigmodel_frame = np.dot(camera_frame_from_ground, ground_from_bigmodel_frame)
|
|
|
|
return camera_frame_from_bigmodel_frame
|
|
|
|
|
|
def get_model_frame(snu_full, camera_frame_from_model_frame, size):
|
|
idxs = camera_frame_from_model_frame.dot(np.column_stack([np.tile(np.arange(size[0]), size[1]),
|
|
np.tile(np.arange(size[1]), (size[0],1)).T.flatten(),
|
|
np.ones(size[0] * size[1])]).T).T.astype(int)
|
|
calib_flat = snu_full[idxs[:,1], idxs[:,0]]
|
|
if len(snu_full.shape) == 3:
|
|
calib = calib_flat.reshape((size[1], size[0], 3))
|
|
elif len(snu_full.shape) == 2:
|
|
calib = calib_flat.reshape((size[1], size[0]))
|
|
else:
|
|
raise ValueError("shape of input img is weird")
|
|
return calib
|