mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-09-14 19:44:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

openpilot v0.5.10 release

Browse Source
This commit is contained in:
Vehicle Researcher
2019-03-26 01:09:18 -07:00
parent be5c2aef3a
commit f74a201edc
102 changed files with 6394 additions and 5433 deletions
Download Patch File Download Diff File Expand all files Collapse all files
common/transformations/camera.py
+34
View File
@@ -112,6 +112,7 @@ def img_from_device(pt_device):
return pt_img.reshape(input_shape)[:,:2]
#TODO please use generic img transform below
def rotate_img(img, eulers, crop=None, intrinsics=eon_intrinsics):
size = img.shape[:2]
rot = orient.rot_from_euler(eulers)
@@ -138,3 +139,36 @@ def rotate_img(img, eulers, crop=None, intrinsics=eon_intrinsics):
img_warped = cv2.warpPerspective(img, M, size[::-1])
return img_warped[H_border: size[0] - H_border,
W_border: size[1] - W_border]
def transform_img(base_img,
augment_trans=np.array([0,0,0]),
augment_eulers=np.array([0,0,0]),
from_intr=eon_intrinsics,
to_intr=eon_intrinsics,
calib_rot_view=None,
output_size=None):
cy = from_intr[1,2]
size = base_img.shape[:2]
if not output_size:
output_size = size[::-1]
h = 1.22
quadrangle = np.array([[0, cy + 20],
[size[1]-1, cy + 20],
[0, size[0]-1],
[size[1]-1, size[0]-1]], dtype=np.float32)
quadrangle_norm = np.hstack((normalize(quadrangle, intrinsics=from_intr), np.ones((4,1))))
quadrangle_world = np.column_stack((h*quadrangle_norm[:,0]/quadrangle_norm[:,1],
h*np.ones(4),
h/quadrangle_norm[:,1]))
rot = orient.rot_from_euler(augment_eulers)
if calib_rot_view is not None:
rot = calib_rot_view.dot(rot)
to_extrinsics = np.hstack((rot.T, -augment_trans[:,None]))
to_KE = to_intr.dot(to_extrinsics)
warped_quadrangle_full = np.einsum('jk,ik->ij', to_KE, np.hstack((quadrangle_world, np.ones((4,1)))))
warped_quadrangle = np.column_stack((warped_quadrangle_full[:,0]/warped_quadrangle_full[:,2],
warped_quadrangle_full[:,1]/warped_quadrangle_full[:,2])).astype(np.float32)
M = cv2.getPerspectiveTransform(quadrangle, warped_quadrangle.astype(np.float32))
augmented_rgb = cv2.warpPerspective(base_img, M, output_size, borderMode=cv2.BORDER_REPLICATE)
return augmented_rgb
common/transformations/model.py
+29
View File
@@ -31,6 +31,18 @@ model_intrinsics = np.array(
[ 0. , 0. , 1.]])
# MED model
MEDMODEL_INPUT_SIZE = (640, 240)
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)
@@ -57,6 +69,9 @@ model_frame_from_road_frame = np.dot(model_intrinsics,
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'
@@ -110,3 +125,17 @@ def get_camera_frame_from_bigmodel_frame(camera_frame_from_road_frame):
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