diff --git a/common/transformations/camera.py b/common/transformations/camera.py index b20ed5c64b..eaed0e7afe 100644 --- a/common/transformations/camera.py +++ b/common/transformations/camera.py @@ -61,14 +61,6 @@ device_frame_from_view_frame = np.array([ view_frame_from_device_frame = device_frame_from_view_frame.T -def get_calib_from_vp(vp): - vp_norm = normalize(vp) - yaw_calib = np.arctan(vp_norm[0]) - pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib)) - roll_calib = 0 - return roll_calib, pitch_calib, yaw_calib - - # aka 'extrinsic_matrix' # road : x->forward, y -> left, z->up def get_view_frame_from_road_frame(roll, pitch, yaw, height): @@ -131,6 +123,14 @@ def denormalize(img_pts, intrinsics=fcam_intrinsics, width=np.inf, height=np.inf return img_pts_denormalized[:, :2].reshape(input_shape) +def get_calib_from_vp(vp, intrinsics=fcam_intrinsics): + vp_norm = normalize(vp, intrinsics) + yaw_calib = np.arctan(vp_norm[0]) + pitch_calib = -np.arctan(vp_norm[1]*np.cos(yaw_calib)) + roll_calib = 0 + return roll_calib, pitch_calib, yaw_calib + + def device_from_ecef(pos_ecef, orientation_ecef, pt_ecef): # device from ecef frame # device frame is x -> forward, y-> right, z -> down