diff --git a/common/transformations/camera.py b/common/transformations/camera.py index e56a6f34bf..b406c33fd7 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -52,6 +52,13 @@ def get_view_frame_from_road_frame(roll, pitch, yaw, height): return np.hstack((view_from_road, [[0], [height], [0]])) +# aka 'extrinsic_matrix' +def get_view_frame_from_calib_frame(roll, pitch, yaw, height): + device_from_calib= orient.rot_from_euler([roll, pitch, yaw]) + view_from_calib = view_frame_from_device_frame.dot(device_from_calib) + return np.hstack((view_from_calib, [[0], [height], [0]])) + + def vp_from_ke(m): """ Computes the vanishing point from the product of the intrinsic and extrinsic diff --git a/common/transformations/model.py b/common/transformations/model.py index 070d174711..221bebae69 100644 --- a/common/transformations/model.py +++ b/common/transformations/model.py @@ -2,6 +2,7 @@ import numpy as np from common.transformations.camera import (FULL_FRAME_SIZE, eon_focal_length, get_view_frame_from_road_frame, + get_view_frame_from_calib_frame, vp_from_ke) # segnet @@ -73,6 +74,9 @@ bigmodel_frame_from_road_frame = np.dot(bigmodel_intrinsics, medmodel_frame_from_road_frame = np.dot(medmodel_intrinsics, get_view_frame_from_road_frame(0, 0, 0, model_height)) +medmodel_frame_from_calib_frame = np.dot(medmodel_intrinsics, + get_view_frame_from_calib_frame(0, 0, 0, model_height)) + 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))