Whitespace

old-commit-hash: 1716f0b11b

selfdrive/locationd/kalman/helpers/chi2_lookup.py

@@ -1,13 +1,14 @@
-import numpy as np
 import os
 
+import numpy as np
+
 
 def gen_chi2_ppf_lookup(max_dim=200):
   from scipy.stats import chi2
   table = np.zeros((max_dim, 98))
   for dim in range(1, max_dim):
     table[dim] = chi2.ppf(np.arange(.01, .99, .01), dim)
-  #outfile = open('chi2_lookup_table', 'w')
+
   np.save('chi2_lookup_table', table)
 
 

selfdrive/locationd/kalman/helpers/ekf_sym.py

@@ -14,7 +14,6 @@ from selfdrive.locationd.kalman.helpers.chi2_lookup import chi2_ppf
 
 def solve(a, b):
   if a.shape[0] == 1 and a.shape[1] == 1:
-    #assert np.allclose(b/a[0][0], np.linalg.solve(a, b))
     return b / a[0][0]
   else:
     return np.linalg.solve(a, b)
@@ -144,15 +143,8 @@ def gen_code(name, f_sym, dt_sym, x_sym, obs_eqs, dim_x, dim_err, eskf_params=No
 class EKF_sym():
   def __init__(self, name, Q, x_initial, P_initial, dim_main, dim_main_err,
                N=0, dim_augment=0, dim_augment_err=0, maha_test_kinds=[]):
-    '''
-    Generates process function and all
-    observation functions for the kalman
-    filter.
-    '''
-    if N > 0:
-      self.msckf = True
-    else:
-      self.msckf = False
+    """Generates process function and all observation functions for the kalman filter."""
+    self.msckf = N > 0
     self.N = N
     self.dim_augment = dim_augment
     self.dim_augment_err = dim_augment_err
@@ -191,19 +183,24 @@ class EKF_sym():
     # wrap all the sympy functions
     def wrap_1lists(name):
       func = eval("lib.%s" % name, {"lib": lib})
+
       def ret(lst1, out):
         func(ffi.cast("double *", lst1.ctypes.data),
              ffi.cast("double *", out.ctypes.data))
       return ret
+
     def wrap_2lists(name):
       func = eval("lib.%s" % name, {"lib": lib})
+
       def ret(lst1, lst2, out):
         func(ffi.cast("double *", lst1.ctypes.data),
              ffi.cast("double *", lst2.ctypes.data),
              ffi.cast("double *", out.ctypes.data))
       return ret
+
     def wrap_1list_1float(name):
       func = eval("lib.%s" % name, {"lib": lib})
+
       def ret(lst1, fl, out):
         func(ffi.cast("double *", lst1.ctypes.data),
              ffi.cast("double", fl),
@@ -235,6 +232,7 @@ class EKF_sym():
     # wrap the C++ update function
     def fun_wrapper(f, kind):
       f = eval("lib.%s" % f, {"lib": lib})
+
       def _update_inner_blas(x, P, z, R, extra_args):
         f(ffi.cast("double *", x.ctypes.data),
           ffi.cast("double *", P.ctypes.data),
@@ -261,7 +259,6 @@ class EKF_sym():
     self._update = _update_blas
     # self._update = self._update_python
 
-
   def init_state(self, state, covs, filter_time):
     self.x = np.array(state.reshape((-1, 1))).astype(np.float64)
     self.P = np.array(covs).astype(np.float64)
@@ -272,19 +269,19 @@ class EKF_sym():
     self.rewind_states = []
 
   def augment(self):
-    # TODO this is not a generalized way of doing
-    # this and implies that the augmented states
-    # are simply the first (dim_augment_state)
-    # elements of the main state.
+    # TODO this is not a generalized way of doing this and implies that the augmented states
+    # are simply the first (dim_augment_state) elements of the main state.
     assert self.msckf
     d1 = self.dim_main
     d2 = self.dim_main_err
     d3 = self.dim_augment
     d4 = self.dim_augment_err
+
     # push through augmented states
     self.x[d1:-d3] = self.x[d1 + d3:]
     self.x[-d3:] = self.x[:d3]
     assert self.x.shape == (self.dim_x, 1)
+
     # push through augmented covs
     assert self.P.shape == (self.dim_err, self.dim_err)
     P_reduced = self.P

selfdrive/locationd/kalman/helpers/feature_handler.py

@@ -1,13 +1,13 @@
 #!/usr/bin/env python3
 
 import os
-import time
 
 import numpy as np
 
 import common.transformations.orientation as orient
+from selfdrive.locationd.kalman.helpers import (TEMPLATE_DIR, load_code,
+                                                write_code)
 from selfdrive.locationd.kalman.helpers.sympy_helpers import quat_matrix_l
-from selfdrive.locationd.kalman.helpers import TEMPLATE_DIR, write_code, load_code
 
 
 def sane(track):
@@ -15,11 +15,11 @@ def sane(track):
   diffs_x = abs(img_pos[1:, 0] - img_pos[:-1, 0])
   diffs_y = abs(img_pos[1:, 1] - img_pos[:-1, 1])
   for i in range(1, len(diffs_x)):
-    if ((diffs_x[i] > 0.05 or diffs_x[i-1] > 0.05) and \
-       (diffs_x[i] > 2*diffs_x[i-1] or \
+    if ((diffs_x[i] > 0.05 or diffs_x[i - 1] > 0.05) and
+        (diffs_x[i] > 2 * diffs_x[i - 1] or
          diffs_x[i] < .5 * diffs_x[i - 1])) or \
-       ((diffs_y[i] > 0.05 or diffs_y[i-1] > 0.05) and \
-       (diffs_y[i] > 2*diffs_y[i-1] or \
+       ((diffs_y[i] > 0.05 or diffs_y[i - 1] > 0.05) and
+        (diffs_y[i] > 2 * diffs_y[i - 1] or
          diffs_y[i] < .5 * diffs_y[i - 1])):
       return False
   return True
@@ -45,10 +45,8 @@ class FeatureHandler():
     self.MAX_TRACKS = 6000
     self.K = K
 
-    #Array of tracks, each track
-    #has K 5D features preceded
-    #by 5 params that inidicate
-    #[f_idx, last_idx, updated, complete, valid]
+    # Array of tracks, each track has K 5D features preceded
+    # by 5 params that inidicate [f_idx, last_idx, updated, complete, valid]
     # f_idx: idx of current last feature in track
     # idx of of last feature in frame
     # bool for whether this track has been update
@@ -95,13 +93,14 @@ class FeatureHandler():
         empty_idx += 1
 
   def update_tracks(self, features):
-    t0 = time.time()
     last_idxs = np.copy(self.tracks[:, 0, 1])
     real = np.isfinite(last_idxs)
     self.tracks[last_idxs[real].astype(int)] = self.tracks[real]
+
     mask = np.ones(self.MAX_TRACKS, np.bool)
     mask[last_idxs[real].astype(int)] = 0
     empty_idxs = np.arange(self.MAX_TRACKS)[mask]
+
     self.tracks[empty_idxs] = np.nan
     self.tracks[:, 0, 2] = 0
     self.merge_features(self.tracks, features, empty_idxs)
@@ -117,10 +116,10 @@ class FeatureHandler():
     return valid_tracks[:, 1:, :4].reshape((len(valid_tracks), self.K * 4))
 
 
-
 def generate_orient_error_jac(K):
   import sympy as sp
   from common.sympy_helpers import quat_rotate
+
   x_sym = sp.MatrixSymbol('abr', 3, 1)
   dtheta = sp.MatrixSymbol('dtheta', 3, 1)
   delta_quat = sp.Matrix(np.ones(4))

selfdrive/locationd/kalman/helpers/lst_sq_computer.py

@@ -4,7 +4,6 @@ import os
 
 import numpy as np
 import sympy as sp
-#import scipy.optimize as opt
 
 import common.transformations.orientation as orient
 from selfdrive.locationd.kalman.helpers import (TEMPLATE_DIR, load_code,
@@ -24,12 +23,13 @@ def generate_residual(K):
   quat_rot = quat_rotate(*q)
   rot_g_to_0 = to_c * quat_rot.T
   rows = []
+
   for i in range(K):
     pos_i = sp.Matrix(np.array(poses_sym[i * 7:i * 7 + 3])[:, 0])
     q = poses_sym[7 * i + 3:7 * i + 7]
     quat_rot = quat_rotate(*q)
     rot_g_to_i = to_c * quat_rot.T
-    rot_0_to_i = rot_g_to_i*(rot_g_to_0.T)
+    rot_0_to_i = rot_g_to_i * rot_g_to_0.T
     trans_0_to_i = rot_g_to_i * (pos_0 - pos_i)
     funct_vec = rot_0_to_i * sp.Matrix([alpha, beta, 1]) + rho * trans_0_to_i
     h1, h2, h3 = funct_vec
@@ -106,10 +106,12 @@ class LstSqComputer():
   def compute_pos(self, poses, img_positions, debug=False):
     pos, param = self.compute_pos_c(poses, img_positions)
     # pos, param = self.compute_pos_python(poses, img_positions)
+
     depth = 1 / param[2]
     if debug:
       if not self.debug:
         raise NotImplementedError("This is not a debug computer")
+
       # orient_err_jac = self.orient_error_jac(param, poses, img_positions, np.zeros(3)).reshape((-1,2,3))
       jac = self.residual_jac(param, poses, img_positions).reshape((-1, 2, 3))
       res = self.residual(param, poses, img_positions).reshape((-1, 2))
@@ -130,6 +132,8 @@ class LstSqComputer():
     return [x]
 
   def compute_pos_python(self, poses, img_positions, check_quality=False):
+    import scipy.optimize as opt
+
     # This procedure is also described
     # in the MSCKF paper (Mourikis et al. 2007)
     x = np.array([img_positions[-1][0],
@@ -142,11 +146,7 @@ class LstSqComputer():
     return (rot_0_to_g.dot(np.array([alpha, beta, 1]))) / rho + poses[-1, :3]
 
 
-
-
-'''
-EXPERIMENTAL CODE
-'''
+# EXPERIMENTAL CODE
 def unroll_shutter(img_positions, poses, v, rot_rates, ecef_pos):
   # only speed correction for now
   t_roll = 0.016  # 16ms rolling shutter?
@@ -164,6 +164,7 @@ def unroll_shutter(img_positions, poses, v, rot_rates, ecef_pos):
   errs = project(poses, ecef_pos) - project(poses + np.atleast_2d(dt).T.dot(np.atleast_2d(v)), ecef_pos)
   return img_positions - errs
 
+
 def project(poses, ecef_pos):
   img_positions = np.zeros((len(poses), 2))
   for i, p in enumerate(poses):

selfdrive/locationd/kalman/helpers/sympy_helpers.py

@@ -2,6 +2,7 @@
 import sympy as sp
 import numpy as np
 
+
 def cross(x):
   ret = sp.Matrix(np.zeros((3, 3)))
   ret[0, 1], ret[0, 2] = -x[2], x[1]
@@ -9,6 +10,7 @@ def cross(x):
   ret[2, 0], ret[2, 1] = -x[1], x[0]
   return ret
 
+
 def euler_rotate(roll, pitch, yaw):
   # make symbolic rotation matrix from eulers
   matrix_roll = sp.Matrix([[1, 0, 0],
@@ -22,18 +24,21 @@ def euler_rotate(roll, pitch, yaw):
                           [0, 0, 1]])
   return matrix_yaw * matrix_pitch * matrix_roll
 
+
 def quat_rotate(q0, q1, q2, q3):
   # make symbolic rotation matrix from quat
   return sp.Matrix([[q0**2 + q1**2 - q2**2 - q3**2, 2 * (q1 * q2 + q0 * q3), 2 * (q1 * q3 - q0 * q2)],
                     [2 * (q1 * q2 - q0 * q3), q0**2 - q1**2 + q2**2 - q3**2, 2 * (q2 * q3 + q0 * q1)],
                     [2 * (q1 * q3 + q0 * q2), 2 * (q2 * q3 - q0 * q1), q0**2 - q1**2 - q2**2 + q3**2]]).T
 
+
 def quat_matrix_l(p):
   return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
                     [p[1],  p[0], -p[3],  p[2]],
                     [p[2],  p[3],  p[0], -p[1]],
                     [p[3], -p[2],  p[1],  p[0]]])
 
+
 def quat_matrix_r(p):
   return sp.Matrix([[p[0], -p[1], -p[2], -p[3]],
                     [p[1],  p[0],  p[3], -p[2]],
@@ -49,6 +54,7 @@ def sympy_into_c(sympy_functions):
 
     # argument ordering input to sympy is broken with function with output arguments
     nargs = []
+
     # reorder the input arguments
     for aa in args:
       if aa is None:
@@ -63,6 +69,7 @@ def sympy_into_c(sympy_functions):
       if not found:
         # [1,1] is a hack for Matrices
         nargs.append(codegen.InputArgument(aa, dimensions=[1, 1]))
+
     # add the output arguments
     for a in r.arguments:
       if type(a) == codegen.OutputArgument:
@@ -75,9 +82,9 @@ def sympy_into_c(sympy_functions):
     routines.append(r)
 
   [(c_name, c_code), (h_name, c_header)] = codegen.get_code_generator('C', 'ekf', 'C99').write(routines, "ekf")
-  c_code = '\n'.join(x for x in c_code.split("\n") if len(x) > 0 and x[0] != '#')
   c_header = '\n'.join(x for x in c_header.split("\n") if len(x) > 0 and x[0] != '#')
 
+  c_code = '\n'.join(x for x in c_code.split("\n") if len(x) > 0 and x[0] != '#')
   c_code = 'extern "C" {\n#include <math.h>\n' + c_code + "\n}\n"
 
   return c_header, c_code