Don't import from openpilot

rednose/helpers/feature_handler.py

@@ -5,18 +5,8 @@
 
 import numpy as np
 
-from rednose.helpers import (TEMPLATE_DIR, load_code, write_code)
-from rednose.helpers.sympy_helpers import quat_matrix_l
-
-
-def rot_matrix(roll, pitch, yaw):
-  cr, sr = np.cos(roll), np.sin(roll)
-  cp, sp = np.cos(pitch), np.sin(pitch)
-  cy, sy = np.cos(yaw), np.sin(yaw)
-  rr = np.array([[1,0,0],[0, cr,-sr],[0, sr, cr]])
-  rp = np.array([[cp,0,sp],[0, 1,0],[-sp, 0, cp]])
-  ry = np.array([[cy,-sy,0],[sy, cy,0],[0, 0, 1]])
-  return ry.dot(rp.dot(rr))
+from rednose.helpers import TEMPLATE_DIR, load_code, write_code
+from rednose.helpers.sympy_helpers import quat_matrix_l, rot_matrix
 
 
 def sane(track):

rednose/helpers/lst_sq_computer.py

@@ -5,17 +5,16 @@
 import numpy as np
 import sympy as sp
 
-import common.transformations.orientation as orient
-from rednose.helpers import (TEMPLATE_DIR, load_code, write_code)
-from rednose.helpers.sympy_helpers import (quat_rotate, sympy_into_c)
+from rednose.helpers import TEMPLATE_DIR, load_code, write_code
+from rednose.helpers.sympy_helpers import quat_rotate, sympy_into_c, rot_matrix, rotations_from_quats
 
 
 def generate_residual(K):
   x_sym = sp.MatrixSymbol('abr', 3, 1)
   poses_sym = sp.MatrixSymbol('poses', 7 * K, 1)
   img_pos_sym = sp.MatrixSymbol('img_positions', 2 * K, 1)
   alpha, beta, rho = x_sym
-  to_c = sp.Matrix(orient.rot_matrix(-np.pi / 2, -np.pi / 2, 0))
+  to_c = sp.Matrix(rot_matrix(-np.pi / 2, -np.pi / 2, 0))
   pos_0 = sp.Matrix(np.array(poses_sym[K * 7 - 7:K * 7 - 4])[:, 0])
   q = poses_sym[K * 7 - 4:K * 7]
   quat_rot = quat_rotate(*q)
@@ -62,7 +61,7 @@ def generate_code(generated_dir, K=4):
     write_code(generated_dir, filename, code, header)
 
   def __init__(self, generated_dir, K=4, MIN_DEPTH=2, MAX_DEPTH=500):
-    self.to_c = orient.rot_matrix(-np.pi / 2, -np.pi / 2, 0)
+    self.to_c = rot_matrix(-np.pi / 2, -np.pi / 2, 0)
     self.MAX_DEPTH = MAX_DEPTH
     self.MIN_DEPTH = MIN_DEPTH
 
@@ -137,7 +136,7 @@ def compute_pos_python(self, poses, img_positions, check_quality=False):
     # res = self.gauss_newton(self.residual, self.residual_jac, x, (poses, img_positions)) # diy gauss_newton
 
     alpha, beta, rho = res[0]
-    rot_0_to_g = (orient.rotations_from_quats(poses[-1, 3:])).dot(self.to_c.T)
+    rot_0_to_g = (rotations_from_quats(poses[-1, 3:])).dot(self.to_c.T)
     return (rot_0_to_g.dot(np.array([alpha, beta, 1]))) / rho + poses[-1, :3]
 
 
@@ -163,7 +162,7 @@ def unroll_shutter(img_positions, poses, v, rot_rates, ecef_pos):
 def project(poses, ecef_pos):
   img_positions = np.zeros((len(poses), 2))
   for i, p in enumerate(poses):
-    cam_frame = orient.rotations_from_quats(p[3:]).T.dot(ecef_pos - p[:3])
+    cam_frame = rotations_from_quats(p[3:]).T.dot(ecef_pos - p[:3])
     img_positions[i] = np.array([cam_frame[1] / cam_frame[0], cam_frame[2] / cam_frame[0]])
   return img_positions
 

rednose/helpers/sympy_helpers.py

@@ -2,6 +2,62 @@
 import sympy as sp
 import numpy as np
 
+# TODO: remove code duplication between openpilot.common.orientation
+def quat2rot(quats):
+  quats = np.array(quats)
+  input_shape = quats.shape
+  quats = np.atleast_2d(quats)
+  Rs = np.zeros((quats.shape[0], 3, 3))
+  q0 = quats[:, 0]
+  q1 = quats[:, 1]
+  q2 = quats[:, 2]
+  q3 = quats[:, 3]
+  Rs[:, 0, 0] = q0 * q0 + q1 * q1 - q2 * q2 - q3 * q3
+  Rs[:, 0, 1] = 2 * (q1 * q2 - q0 * q3)
+  Rs[:, 0, 2] = 2 * (q0 * q2 + q1 * q3)
+  Rs[:, 1, 0] = 2 * (q1 * q2 + q0 * q3)
+  Rs[:, 1, 1] = q0 * q0 - q1 * q1 + q2 * q2 - q3 * q3
+  Rs[:, 1, 2] = 2 * (q2 * q3 - q0 * q1)
+  Rs[:, 2, 0] = 2 * (q1 * q3 - q0 * q2)
+  Rs[:, 2, 1] = 2 * (q0 * q1 + q2 * q3)
+  Rs[:, 2, 2] = q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3
+
+  if len(input_shape) < 2:
+    return Rs[0]
+  else:
+    return Rs
+
+
+def euler2quat(eulers):
+  eulers = np.array(eulers)
+  if len(eulers.shape) > 1:
+    output_shape = (-1,4)
+  else:
+    output_shape = (4,)
+  eulers = np.atleast_2d(eulers)
+  gamma, theta, psi = eulers[:,0],  eulers[:,1],  eulers[:,2]
+
+  q0 = np.cos(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) + \
+       np.sin(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2)
+  q1 = np.sin(gamma / 2) * np.cos(theta / 2) * np.cos(psi / 2) - \
+       np.cos(gamma / 2) * np.sin(theta / 2) * np.sin(psi / 2)
+  q2 = np.cos(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2) + \
+       np.sin(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2)
+  q3 = np.cos(gamma / 2) * np.cos(theta / 2) * np.sin(psi / 2) - \
+       np.sin(gamma / 2) * np.sin(theta / 2) * np.cos(psi / 2)
+
+  quats = np.array([q0, q1, q2, q3]).T
+  for i in range(len(quats)):
+    if quats[i,0] < 0:
+      quats[i] = -quats[i]
+  return quats.reshape(output_shape)
+
+
+def euler2rot(eulers):
+  return quat2rot(euler2quat(eulers))
+
+rotations_from_quats = quat2rot
+
 
 def cross(x):
   ret = sp.Matrix(np.zeros((3, 3)))
@@ -11,6 +67,16 @@ def cross(x):
   return ret
 
 
+def rot_matrix(roll, pitch, yaw):
+  cr, sr = np.cos(roll), np.sin(roll)
+  cp, sp = np.cos(pitch), np.sin(pitch)
+  cy, sy = np.cos(yaw), np.sin(yaw)
+  rr = np.array([[1,0,0],[0, cr,-sr],[0, sr, cr]])
+  rp = np.array([[cp,0,sp],[0, 1,0],[-sp, 0, cp]])
+  ry = np.array([[cy,-sy,0],[sy, cy,0],[0, 0, 1]])
+  return ry.dot(rp.dot(rr))
+
+
 def euler_rotate(roll, pitch, yaw):
   # make symbolic rotation matrix from eulers
   matrix_roll = sp.Matrix([[1, 0, 0],