openpilot v0.5.12 release

old-commit-hash: 3f9059fea8

common/kalman/Makefile

@@ -0,0 +1,10 @@
+all: simple_kalman_impl.so
+
+simple_kalman_impl.so: simple_kalman_impl.pyx simple_kalman_impl.pxd simple_kalman_setup.py
+	python simple_kalman_setup.py build_ext --inplace
+	rm -rf build
+	rm simple_kalman_impl.c
+
+.PHONY: clean
+clean:
+	rm -f simple_kalman_impl.so

common/kalman/simple_kalman.py

@@ -1,23 +1,10 @@
-import numpy as np
+# pylint: skip-file
+import os
+import subprocess
 
+kalman_dir = os.path.dirname(os.path.abspath(__file__))
+subprocess.check_call(["make", "simple_kalman_impl.so"], cwd=kalman_dir)
 
-class KF1D:
-  # this EKF assumes constant covariance matrix, so calculations are much simpler
-  # the Kalman gain also needs to be precomputed using the control module
-
-  def __init__(self, x0, A, C, K):
-    self.x = x0
-    self.A = A
-    self.C = C
-    self.K = K
-
-    self.A_K = self.A - np.dot(self.K, self.C)
-
-    # K matrix needs to  be pre-computed as follow:
-    # import control
-    # (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R)
-    # self.K = np.transpose(K)
-
-  def update(self, meas):
-    self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas)
-    return self.x
+from simple_kalman_impl import KF1D as KF1D
+# Silence pyflakes
+assert KF1D

common/kalman/simple_kalman_impl.pxd

@@ -0,0 +1,16 @@
+cdef class KF1D:
+  cdef public:
+    double x0_0
+    double x1_0
+    double K0_0
+    double K1_0
+    double A0_0
+    double A0_1
+    double A1_0
+    double A1_1
+    double C0_0
+    double C0_1
+    double A_K_0
+    double A_K_1
+    double A_K_2
+    double A_K_3

common/kalman/simple_kalman_impl.pyx

@@ -0,0 +1,35 @@
+
+cdef class KF1D:
+  def __init__(self, x0, A, C, K):
+    self.x0_0 = x0[0][0]
+    self.x1_0 = x0[1][0]
+    self.A0_0 = A[0][0]
+    self.A0_1 = A[0][1]
+    self.A1_0 = A[1][0]
+    self.A1_1 = A[1][1]
+    self.C0_0 = C[0]
+    self.C0_1 = C[1]
+    self.K0_0 = K[0][0]
+    self.K1_0 = K[1][0]
+
+    self.A_K_0 = self.A0_0 - self.K0_0 * self.C0_0
+    self.A_K_1 = self.A0_1 - self.K0_0 * self.C0_1
+    self.A_K_2 = self.A1_0 - self.K1_0 * self.C0_0
+    self.A_K_3 = self.A1_1 - self.K1_0 * self.C0_1
+
+  def update(self, meas):
+    cdef double x0_0 = self.A_K_0 * self.x0_0 + self.A_K_1 * self.x1_0 + self.K0_0 * meas
+    cdef double x1_0 = self.A_K_2 * self.x0_0 + self.A_K_3 * self.x1_0 + self.K1_0 * meas
+    self.x0_0 = x0_0
+    self.x1_0 = x1_0
+
+    return [self.x0_0, self.x1_0]
+
+  @property
+  def x(self):
+    return [[self.x0_0], [self.x1_0]]
+
+  @x.setter
+  def x(self, x):
+    self.x0_0 = x[0][0]
+    self.x1_0 = x[1][0]

common/kalman/simple_kalman_old.py

@@ -0,0 +1,23 @@
+import numpy as np
+
+
+class KF1D:
+  # this EKF assumes constant covariance matrix, so calculations are much simpler
+  # the Kalman gain also needs to be precomputed using the control module
+
+  def __init__(self, x0, A, C, K):
+    self.x = x0
+    self.A = A
+    self.C = C
+    self.K = K
+
+    self.A_K = self.A - np.dot(self.K, self.C)
+
+    # K matrix needs to  be pre-computed as follow:
+    # import control
+    # (x, l, K) = control.dare(np.transpose(self.A), np.transpose(self.C), Q, R)
+    # self.K = np.transpose(K)
+
+  def update(self, meas):
+    self.x = np.dot(self.A_K, self.x) + np.dot(self.K, meas)
+    return self.x

common/kalman/simple_kalman_setup.py

@@ -0,0 +1,5 @@
+from distutils.core import setup, Extension
+from Cython.Build import cythonize
+
+setup(name='Simple Kalman Implementation',
+      ext_modules=cythonize(Extension("simple_kalman_impl", ["simple_kalman_impl.pyx"])))

common/kalman/tests/test_ekf.py

@@ -0,0 +1,116 @@
+import numpy as np
+import numpy.matlib
+import unittest
+import timeit
+
+from common.kalman.ekf import EKF, SimpleSensor, FastEKF1D
+
+class TestEKF(EKF):
+  def __init__(self, var_init, Q):
+    super(TestEKF, self).__init__(False)
+    self.identity = numpy.matlib.identity(2)
+    self.state = numpy.matlib.zeros((2, 1))
+    self.covar = self.identity * var_init
+
+    self.process_noise = numpy.matlib.diag(Q)
+
+  def calc_transfer_fun(self, dt):
+    tf = numpy.matlib.identity(2)
+    tf[0, 1] = dt
+    return tf, tf
+
+
+class EKFTest(unittest.TestCase):
+  def test_update_scalar(self):
+    ekf = TestEKF(1e3, [0.1, 1])
+    dt = 1. / 100
+
+    sensor = SimpleSensor(0, 1, 2)
+    readings = map(sensor.read, np.arange(100, 300))
+
+    for reading in readings:
+      ekf.update_scalar(reading)
+      ekf.predict(dt)
+
+    np.testing.assert_allclose(ekf.state, [[300], [100]], 1e-4)
+    np.testing.assert_allclose(
+      ekf.covar,
+      np.asarray([[0.0563, 0.10278], [0.10278, 0.55779]]),
+      atol=1e-4)
+
+  def test_unbiased(self):
+    ekf = TestEKF(1e3, [0., 0.])
+    dt = np.float64(1. / 100)
+
+    sensor = SimpleSensor(0, 1, 2)
+    readings = map(sensor.read, np.arange(1000))
+
+    for reading in readings:
+      ekf.update_scalar(reading)
+      ekf.predict(dt)
+
+    np.testing.assert_allclose(ekf.state, [[1000.], [100.]], 1e-4)
+
+
+class FastEKF1DTest(unittest.TestCase):
+  def test_correctness(self):
+    dt = 1. / 100
+    reading = SimpleSensor(0, 1, 2).read(100)
+
+    ekf = TestEKF(1e3, [0.1, 1])
+    fast_ekf = FastEKF1D(dt, 1e3, [0.1, 1])
+
+    ekf.update_scalar(reading)
+    fast_ekf.update_scalar(reading)
+    self.assertAlmostEqual(ekf.state[0]   , fast_ekf.state[0])
+    self.assertAlmostEqual(ekf.state[1]   , fast_ekf.state[1])
+    self.assertAlmostEqual(ekf.covar[0, 0], fast_ekf.covar[0])
+    self.assertAlmostEqual(ekf.covar[0, 1], fast_ekf.covar[2])
+    self.assertAlmostEqual(ekf.covar[1, 1], fast_ekf.covar[1])
+
+    ekf.predict(dt)
+    fast_ekf.predict(dt)
+    self.assertAlmostEqual(ekf.state[0]   , fast_ekf.state[0])
+    self.assertAlmostEqual(ekf.state[1]   , fast_ekf.state[1])
+    self.assertAlmostEqual(ekf.covar[0, 0], fast_ekf.covar[0])
+    self.assertAlmostEqual(ekf.covar[0, 1], fast_ekf.covar[2])
+    self.assertAlmostEqual(ekf.covar[1, 1], fast_ekf.covar[1])
+
+  def test_speed(self):
+    setup = """
+import numpy as np
+from common.kalman.tests.test_ekf import TestEKF
+from common.kalman.ekf import SimpleSensor, FastEKF1D
+
+dt = 1. / 100
+reading = SimpleSensor(0, 1, 2).read(100)
+
+var_init, Q = 1e3, [0.1, 1]
+ekf = TestEKF(var_init, Q)
+fast_ekf = FastEKF1D(dt, var_init, Q)
+    """
+
+    timeit.timeit("""
+ekf.update_scalar(reading)
+ekf.predict(dt)
+    """, setup=setup, number=1000)
+
+    ekf_speed = timeit.timeit("""
+ekf.update_scalar(reading)
+ekf.predict(dt)
+    """, setup=setup, number=20000)
+
+    timeit.timeit("""
+fast_ekf.update_scalar(reading)
+fast_ekf.predict(dt)
+    """, setup=setup, number=1000)
+
+    fast_ekf_speed = timeit.timeit("""
+fast_ekf.update_scalar(reading)
+fast_ekf.predict(dt)
+    """, setup=setup, number=20000)
+
+    assert fast_ekf_speed < ekf_speed / 4
+
+if __name__ == "__main__":
+  unittest.main()

common/kalman/tests/test_simple_kalman.py

@@ -0,0 +1,85 @@
+import unittest
+import random
+import timeit
+import numpy as np
+
+from common.kalman.simple_kalman import KF1D
+from common.kalman.simple_kalman_old import KF1D as KF1D_old
+
+
+class TestSimpleKalman(unittest.TestCase):
+  def setUp(self):
+    dt = 0.01
+    x0_0 = 0.0
+    x1_0 = 0.0
+    A0_0 = 1.0
+    A0_1 = dt
+    A1_0 = 0.0
+    A1_1 = 1.0
+    C0_0 = 1.0
+    C0_1 = 0.0
+    K0_0 = 0.12287673
+    K1_0 = 0.29666309
+
+    self.kf_old = KF1D_old(x0=np.matrix([[x0_0], [x1_0]]),
+                           A=np.matrix([[A0_0, A0_1], [A1_0, A1_1]]),
+                           C=np.matrix([C0_0, C0_1]),
+                           K=np.matrix([[K0_0], [K1_0]]))
+
+    self.kf = KF1D(x0=[[x0_0], [x1_0]],
+                   A=[[A0_0, A0_1], [A1_0, A1_1]],
+                   C=[C0_0, C0_1],
+                   K=[[K0_0], [K1_0]])
+
+  def test_getter_setter(self):
+    self.kf.x = [[1.0], [1.0]]
+    self.assertEqual(self.kf.x, [[1.0], [1.0]])
+
+  def update_returns_state(self):
+      x = self.kf.update(100)
+      self.assertEqual(x, self.kf.x)
+
+  def test_old_equal_new(self):
+    for _ in range(1000):
+      v_wheel = random.uniform(0, 200)
+
+      x_old = self.kf_old.update(v_wheel)
+      x = self.kf.update(v_wheel)
+
+      # Compare the output x, verify that the error is less than 1e-4
+      self.assertAlmostEqual(x_old[0], x[0])
+      self.assertAlmostEqual(x_old[1], x[1])
+
+
+  def test_new_is_faster(self):
+    setup = """
+import numpy as np
+
+from common.kalman.simple_kalman import KF1D
+from common.kalman.simple_kalman_old import KF1D as KF1D_old
+
+dt = 0.01
+x0_0 = 0.0
+x1_0 = 0.0
+A0_0 = 1.0
+A0_1 = dt
+A1_0 = 0.0
+A1_1 = 1.0
+C0_0 = 1.0
+C0_1 = 0.0
+K0_0 = 0.12287673
+K1_0 = 0.29666309
+
+kf_old = KF1D_old(x0=np.matrix([[x0_0], [x1_0]]),
+                  A=np.matrix([[A0_0, A0_1], [A1_0, A1_1]]),
+                  C=np.matrix([C0_0, C0_1]),
+                  K=np.matrix([[K0_0], [K1_0]]))
+
+kf = KF1D(x0=[[x0_0], [x1_0]],
+          A=[[A0_0, A0_1], [A1_0, A1_1]],
+          C=[C0_0, C0_1],
+          K=[[K0_0], [K1_0]])
+    """
+    kf_speed = timeit.timeit("kf.update(1234)", setup=setup, number=10000)
+    kf_old_speed = timeit.timeit("kf_old.update(1234)", setup=setup, number=10000)
+    self.assertTrue(kf_speed < kf_old_speed / 4)

common/timeout.py

@@ -0,0 +1,28 @@
+import signal
+
+class TimeoutException(Exception):
+  pass
+
+class Timeout:
+  """
+  Timeout context manager.
+  For example this code will raise a TimeoutException:
+  with Timeout(seconds=5, error_msg="Sleep was too long"):
+    time.sleep(10)
+  """
+  def __init__(self, seconds, error_msg=None):
+    if error_msg is None:
+      error_msg = 'Timed out after {} seconds'.format(seconds)
+    self.seconds = seconds
+    self.error_msg = error_msg
+
+  def handle_timeout(self, signume, frame):
+    raise TimeoutException(self.error_msg)
+
+  def __enter__(self):
+    signal.signal(signal.SIGALRM, self.handle_timeout)
+    signal.alarm(self.seconds)
+
+  def __exit__(self, exc_type, exc_val, exc_tb):
+    signal.alarm(0)
+

common/transformations/camera.py

@@ -200,3 +200,14 @@ def transform_img(base_img,
     augmented_rgb[:cyy] = cv2.warpPerspective(base_img, M, (output_size[0], cyy), borderMode=cv2.BORDER_REPLICATE)
 
   return augmented_rgb
+
+def yuv_crop(frame, output_size, center=None):
+  # output_size in camera coordinates so u,v
+  # center in array coordinates so row, column
+  rgb = cv2.cvtColor(frame, cv2.COLOR_YUV2RGB_I420)
+  if not center:
+    center = (rgb.shape[0]/2, rgb.shape[1]/2)
+  rgb_crop = rgb[center[0] - output_size[1]/2: center[0] + output_size[1]/2,
+                 center[1] - output_size[0]/2: center[1] + output_size[0]/2]
+  return cv2.cvtColor(rgb_crop, cv2.COLOR_RGB2YUV_I420)
+

common/transformations/model.py

@@ -1,8 +1,7 @@
 import numpy as np
 
 from common.transformations.camera import eon_focal_length, \
-        vp_from_ke, \
-        get_view_frame_from_road_frame, \
+        vp_from_ke, get_view_frame_from_road_frame, \
         FULL_FRAME_SIZE
 
 # segnet
@@ -117,6 +116,16 @@ def get_camera_frame_from_model_frame(camera_frame_from_road_frame, height=model
   return np.dot(camera_from_model_camera, model_camera_from_model_frame)
 
 
+def get_camera_frame_from_medmodel_frame(camera_frame_from_road_frame):
+  camera_frame_from_ground = camera_frame_from_road_frame[:, (0, 1, 3)]
+  medmodel_frame_from_ground = medmodel_frame_from_road_frame[:, (0, 1, 3)]
+
+  ground_from_medmodel_frame = np.linalg.inv(medmodel_frame_from_ground)
+  camera_frame_from_medmodel_frame = np.dot(camera_frame_from_ground, ground_from_medmodel_frame)
+
+  return camera_frame_from_medmodel_frame
+
+
 def get_camera_frame_from_bigmodel_frame(camera_frame_from_road_frame):
   camera_frame_from_ground = camera_frame_from_road_frame[:, (0, 1, 3)]
   bigmodel_frame_from_ground = bigmodel_frame_from_road_frame[:, (0, 1, 3)]