def update(self, uvs, yaw_rate, speed):
if len(uvs) < 10 or \
abs(yaw_rate) > Filter.MAX_YAW_RATE or \
speed < Filter.MIN_SPEED:
return
rot_speeds = np.array([0.,0.,-yaw_rate])
uvs[:,1,:] = denormalize(correct_pts(normalize(uvs[:,1,:]), rot_speeds, self.dt))
# exclude tracks where:
# - pixel movement was less than 10 pixels
# - tracks are in the "hood region"
good_tracks = np.all([np.linalg.norm(uvs[:,1,:] - uvs[:,0,:], axis=1) > 10,
uvs[:,0,1] < HOOD_HEIGHT,
uvs[:,1,1] < HOOD_HEIGHT], axis = 0)
uvs = uvs[good_tracks]
if uvs.shape[0] > MAX_LINES:
uvs = uvs[np.random.choice(uvs.shape[0], MAX_LINES, replace=False), :]
lines = get_lines(uvs)
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_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)
self.vp_cycles += 1
if (self.vp_cycles - VP_CYCLES_NEEDED) % 10 == 0: # update file every 10
# skip rate_lim before writing the file to avoid writing a lagged vp
if self.cal_status != Calibration.CALIBRATED:
self.vp = self.vp_unfilt
if self.param_put:
cal_params = {"vanishing_point": list(self.vp), "angle_offset2": self.angle_offset}
self.params.put("CalibrationParams", json.dumps(cal_params))
return self.vp_unfilt
def parse_orb_features(self, log):
matches = np.array(log.orbFeatures.matches)
n = len(log.orbFeatures.matches)
t = float(log.orbFeatures.timestampLastEof)*1e-9
if t == 0 or n == 0:
return t, np.zeros((0,2,2))
orbs = denormalize(np.column_stack((log.orbFeatures.xs,
log.orbFeatures.ys)))
if self.prev_orbs is not None:
valid_matches = (matches > -1) & (matches < len(self.prev_orbs))
tracks = np.hstack((orbs[valid_matches], self.prev_orbs[matches[valid_matches]])).reshape((-1,2,2))
else:
tracks = np.zeros((0,2,2))
self.prev_orbs = orbs
return t, tracks