openpilot v0.5.8 release

common/transformations/camera.py

@@ -1,5 +1,6 @@
 import numpy as np
 import common.transformations.orientation as orient
+import cv2
 
 FULL_FRAME_SIZE = (1164, 874)
 W, H = FULL_FRAME_SIZE[0], FULL_FRAME_SIZE[1]
@@ -62,31 +63,35 @@ def roll_from_ke(m):
   return np.arctan2(-(m[1, 0] - m[1, 1] * m[2, 0] / m[2, 1]),
                     -(m[0, 0] - m[0, 1] * m[2, 0] / m[2, 1]))
 
-def normalize(img_pts):
+
+def normalize(img_pts, intrinsics=eon_intrinsics):
   # normalizes image coordinates
   # accepts single pt or array of pts
+  intrinsics_inv = np.linalg.inv(intrinsics)
   img_pts = np.array(img_pts)
   input_shape = img_pts.shape
   img_pts = np.atleast_2d(img_pts)
   img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0],1))))
-  img_pts_normalized = eon_intrinsics_inv.dot(img_pts.T).T
+  img_pts_normalized = intrinsics_inv.dot(img_pts.T).T
   img_pts_normalized[(img_pts < 0).any(axis=1)] = np.nan
   return img_pts_normalized[:,:2].reshape(input_shape)
 
-def denormalize(img_pts):
+
+def denormalize(img_pts, intrinsics=eon_intrinsics):
   # denormalizes image coordinates
   # accepts single pt or array of pts
   img_pts = np.array(img_pts)
   input_shape = img_pts.shape
   img_pts = np.atleast_2d(img_pts)
   img_pts = np.hstack((img_pts, np.ones((img_pts.shape[0],1))))
-  img_pts_denormalized = eon_intrinsics.dot(img_pts.T).T
+  img_pts_denormalized = intrinsics.dot(img_pts.T).T
   img_pts_denormalized[img_pts_denormalized[:,0] > W] = 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] < 0] = np.nan
   return img_pts_denormalized[:,:2].reshape(input_shape)
 
+
 def device_from_ecef(pos_ecef, orientation_ecef, pt_ecef):
   # device from ecef frame
   # device frame is x -> forward, y-> right, z -> down
@@ -99,6 +104,7 @@ def device_from_ecef(pos_ecef, orientation_ecef, pt_ecef):
   pt_device = np.einsum('jk,ik->ij', device_from_ecef_rot, pt_ecef_rel)
   return pt_device.reshape(input_shape)
 
+
 def img_from_device(pt_device):
   # img coordinates from pts in device frame
   # first transforms to view frame, then to img coords
@@ -113,3 +119,30 @@ def img_from_device(pt_device):
   pt_img = pt_view/pt_view[:,2:3]
   return pt_img.reshape(input_shape)[:,:2]
 
+
+def rotate_img(img, eulers, crop=None, intrinsics=eon_intrinsics):
+  size = img.shape[:2]
+  rot = orient.rot_from_euler(eulers)
+  quadrangle = np.array([[0, 0],
+                         [size[1]-1, 0],
+                         [0, size[0]-1],
+                         [size[1]-1, size[0]-1]], dtype=np.float32)
+  quadrangle_norm = np.hstack((normalize(quadrangle, intrinsics=intrinsics), np.ones((4,1))))
+  warped_quadrangle_full = np.einsum('ij, kj->ki', intrinsics.dot(rot), quadrangle_norm)
+  warped_quadrangle = np.column_stack((warped_quadrangle_full[:,0]/warped_quadrangle_full[:,2],
+                                       warped_quadrangle_full[:,1]/warped_quadrangle_full[:,2])).astype(np.float32)
+  if crop:
+    W_border = (size[1] - crop[0])/2
+    H_border = (size[0] - crop[1])/2
+    outside_crop = (((warped_quadrangle[:,0] < W_border) |
+                     (warped_quadrangle[:,0] >= size[1] - W_border)) &
+                    ((warped_quadrangle[:,1] < H_border) |
+                     (warped_quadrangle[:,1] >= size[0] - H_border)))
+    if not outside_crop.all():
+      raise ValueError("warped image not contained inside crop")
+  else:
+    H_border, W_border = 0, 0
+  M = cv2.getPerspectiveTransform(quadrangle, warped_quadrangle)
+  img_warped = cv2.warpPerspective(img, M, size[::-1])
+  return img_warped[H_border: size[0] - H_border,
+                    W_border: size[1] - W_border]

common/transformations/orientation.py

@@ -221,6 +221,8 @@ def ned_euler_from_ecef(ned_ecef_init, ecef_poses):
   ecef_poses = array(ecef_poses)
   output_shape = ecef_poses.shape
   ned_ecef_init = np.atleast_2d(ned_ecef_init)
+  if ned_ecef_init.shape[0] == 1:
+    ned_ecef_init = np.tile(ned_ecef_init[0], (output_shape[0], 1))
   ecef_poses = np.atleast_2d(ecef_poses)
 
   ned_poses = np.zeros(ecef_poses.shape)