mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-03-01 07:03:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

Faster calibration filtering (#421)

Browse Source 
* separable filter

* add missing args

* formatting

* fix casing
This commit is contained in:
dekerr
2018-11-06 16:51:37 -05:00
committed by rbiasini
parent 736cf2fe32
commit 2b8e62a0d6
1 changed files with 12 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
selfdrive/locationd/calibrationd.py
View File

@@ -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