From 0fb6cb068a8d33bd9bb2bb96888e7af17f69d508 Mon Sep 17 00:00:00 2001
From: dekerr <dkerr2@mix.wvu.edu>
Date: Tue, 6 Nov 2018 16:51:37 -0500
Subject: [PATCH] Faster calibration filtering (#421)

* separable filter

* add missing args

* formatting

* fix casing

old-commit-hash: 7db592d3b8982698c0e5206faa04d014d7a7a73f
---
 selfdrive/locationd/calibrationd.py | 16 ++++++++++++----
 1 file changed, 12 insertions(+), 4 deletions(-)

diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py
index 523302f11a..05ba209ed9 100755
--- a/selfdrive/locationd/calibrationd.py
+++ b/selfdrive/locationd/calibrationd.py
@@ -72,8 +72,15 @@ def gaussian_kernel(sizex, sizey, stdx, stdy, dx, dy):
   g = np.exp(-((x - dx)**2 / (2. * stdx**2) + (y - dy)**2 / (2. * stdy**2)))
   return g / g.sum()
 
-def blur_image(img, kernel):
-  return cv2.filter2D(img.astype(np.uint16), -1, kernel)
+def gaussian_kernel_1d(kernel):
+  #creates separable gaussian filter
+  u,s,v = np.linalg.svd(kernel)
+  x = u[:,0]*np.sqrt(s[0])
+  y = np.sqrt(s[0])*v[0,:]
+  return x, y
+
+def blur_image(img, kernel_x, kernel_y):
+  return cv2.sepFilter2D(img.astype(np.uint16), -1, kernel_x, kernel_y)
 
 def is_calibration_valid(vp):
   return vp[0] > VP_VALIDITY_CORNERS[0,0] and vp[0] < VP_VALIDITY_CORNERS[1,0] and \
@@ -89,6 +96,7 @@ class Calibrator(object):
     self.l100_last_updated = 0
     self.prev_orbs = None
     self.kernel = gaussian_kernel(11, 11, 2.35, 2.35, 0, 0)
+    self.kernel_x, self.kernel_y = gaussian_kernel_1d(self.kernel)
 
     self.vp = copy.copy(VP_INIT)
     self.cal_status = Calibration.UNCALIBRATED
@@ -136,7 +144,7 @@ class Calibrator(object):
     increment_grid_c(self.grid, lines, len(lines))
     self.frame_counter += 1
     if (self.frame_counter % FRAMES_NEEDED) == 0:
-      grid = blur_image(self.grid, self.kernel)
+      grid = blur_image(self.grid, self.kernel_x, self.kernel_y)
       argmax_vp = np.unravel_index(np.argmax(grid), grid.shape)[::-1]
       self.rescale_grid()
       self.vp_unfilt = np.array(argmax_vp)
@@ -189,7 +197,7 @@ class Calibrator(object):
     self.yaw_rate = log.live100.curvature * self.speed
 
   def handle_debug(self):
-    grid_blurred = blur_image(self.grid, self.kernel)
+    grid_blurred = blur_image(self.grid, self.kernel_x, self.kernel_y)
     grid_grey = np.clip(grid_blurred/(0.1 + np.max(grid_blurred))*255, 0, 255)
     grid_color = np.repeat(grid_grey[:,:,np.newaxis], 3, axis=2)
     grid_color[:,:,0] = 0