Use skimage masked corr

selfdrive/locationd/lagd.py

@@ -6,46 +6,35 @@ import cereal.messaging as messaging
 from cereal import car
 from openpilot.common.params import Params
 from openpilot.common.realtime import config_realtime_process, DT_CTRL
-from openpilot.selfdrive.locationd.helpers import ParameterEstimator
+from openpilot.selfdrive.locationd.helpers import ParameterEstimator, PoseCalibrator, Pose
 
 MIN_LAG_VEL = 15.0
 MAX_SANE_LAG = 3.0
-MAX_LAG_HIST_LEN_SEC = 120
-MOVING_CORR_WINDOW = 60
+MIN_ABS_YAW_RATE_DEG = 1
+MAX_LAG_HIST_LEN_SEC = 600
+MOVING_CORR_WINDOW = 300
+MIN_OKAY_WINDOW = 60
 
 
-class LagEstimator(ParameterEstimator):
-  def __init__(self, CP, dt, pub_dt, max_lag_hist_len_sec, moving_corr_window):
+class BaseLagEstimator:
+  def __init__(self, CP, dt, moving_corr_window, min_okay_window):
     self.dt = dt
     self.window_len = int(moving_corr_window / self.dt)
+    self.min_okay_window_len = int(min_okay_window / self.dt)
     self.initial_lag = CP.steerActuatorDelay
-    self.current_lag = self.initial_lag
 
-    self.lat_active = False
-    self.steering_pressed = False
-    self.v_ego = 0.0
-    self.lags = deque(maxlen= int(max_lag_hist_len_sec / pub_dt))
+    self.calibrator = PoseCalibrator()
+
+    lag_limit = int(moving_corr_window / (self.dt * 25))
+    self.lags = deque(maxlen=lag_limit)
+    self.correlations = deque(maxlen=lag_limit)
+    self.times = deque(maxlen=int(moving_corr_window / self.dt))
     self.curvature = deque(maxlen=int(moving_corr_window / self.dt))
     self.desired_curvature = deque(maxlen=int(moving_corr_window / self.dt))
-    self.frame = 0
-
-  def correlation_lags(self, sig_len, dt):
-    return np.arange(0, sig_len) * dt
+    self.okay = deque(maxlen=int(moving_corr_window / self.dt))
 
-  def actuator_delay(self, expected_sig, actual_sig, dt, max_lag=MAX_SANE_LAG):
-    assert len(expected_sig) == len(actual_sig)
-    correlations = np.correlate(expected_sig, actual_sig, mode='full')
-    lags = self.correlation_lags(len(expected_sig), dt)
-
-    # only consider negative time shifts within the max_lag
-    n_frames_max_delay = int(max_lag / dt)
-    correlations = correlations[len(expected_sig) - 1: len(expected_sig) - 1 + n_frames_max_delay]
-    lags = lags[:n_frames_max_delay]
-
-    max_corr_index = np.argmax(correlations)
-
-    lag, corr = lags[max_corr_index], correlations[max_corr_index]
-    return lag, corr
+  def actuator_delay(self, expected_sig, actual_sig, is_okay, dt, max_lag):
+    raise NotImplementedError
 
   def handle_log(self, t, which, msg) -> None:
     if which == "carControl":
@@ -56,27 +45,39 @@ class LagEstimator(ParameterEstimator):
     elif which == "controlsState":
       curvature = msg.curvature
       desired_curvature = msg.desiredCurvature
-      if self.lat_active and not self.steering_pressed and self.v_ego > MIN_LAG_VEL:
-        self.curvature.append((t, curvature))
-        self.desired_curvature.append((t, desired_curvature))
-      self.frame += 1
+      okay = self.lat_active and not self.steering_pressed and self.v_ego > MIN_LAG_VEL and abs(self.yaw_rate) > np.radians(MIN_ABS_YAW_RATE_DEG)
+      self.times.append(t)
+      self.okay.append(okay)
+      self.curvature.append(curvature)
+      self.desired_curvature.append(desired_curvature)
+    elif which == "livePose":
+      device_pose = Pose.from_live_pose(msg)
+      calibrated_pose = self.calibrator.build_calibrated_pose(device_pose)
+
+      self.yaw_rate = calibrated_pose.angular_velocity.z
+    elif which == 'liveCalibration':
+      self.calibrator.feed_live_calib(msg)
 
   def get_msg(self, valid: bool, with_points: bool):
-    if len(self.curvature) >= self.window_len:
-      _, curvature = zip(*self.curvature)
-      _, desired_curvature = zip(*self.desired_curvature)
-      delay_curvature, _ = self.actuator_delay(curvature, desired_curvature, self.dt)
-      if delay_curvature != 0.0:
+    okay_count = np.count_nonzero(self.okay)
+    if len(self.curvature) >= self.window_len and okay_count >= self.min_okay_window_len:
+      curvature = np.array(self.curvature)
+      desired_curvature = np.array(self.desired_curvature)
+      okay = np.array(self.okay)
+      try:
+        delay_curvature, correlation = self.actuator_delay(desired_curvature, curvature, okay, self.dt, MAX_SANE_LAG)
         self.lags.append(delay_curvature)
-      # FIXME: this is fragile and ugly, refactor this
-      if len(self.lags) > 0:
-        steer_actuation_delay = float(np.mean(self.lags))
-        is_estimated = True
-      else:
-        steer_actuation_delay = self.initial_lag
-        is_estimated = False
+        self.correlations.append(correlation)
+      except ValueError:
+        pass
+
+    if len(self.lags) > 0:
+      steer_actuation_delay = np.mean(self.lags)
+      steer_correlation = np.mean(self.correlations)
+      is_estimated = True
     else:
       steer_actuation_delay = self.initial_lag
+      steer_correlation = np.nan
       is_estimated = False
 
     msg = messaging.new_message('liveActuatorDelay')
@@ -88,25 +89,49 @@ class LagEstimator(ParameterEstimator):
     liveActuatorDelay.isEstimated = is_estimated
 
     if with_points:
-      liveActuatorDelay.points = [[c, dc] for ((_, c), (_, dc)) in zip(self.curvature, self.desired_curvature)]
+      liveActuatorDelay.points = [p for p in zip(self.curvature, self.desired_curvature)]
+
+    return steer_actuation_delay, steer_correlation, okay_count, is_estimated
+
 
-    return msg
+class LagEstimator(ParameterEstimator):
+  def correlation_lags(self, sig_len, dt):
+    return np.arange(0, sig_len) * dt
+
+  def actuator_delay(self, expected_sig, actual_sig, is_okay, dt, max_lag):
+    from skimage.registration._masked_phase_cross_correlation import cross_correlate_masked
+    # masked (gated) normalized cross-correlation
+    # normalized, can be used for anything, like comparsion
+
+    assert len(expected_sig) == len(actual_sig)
+
+    xcorr = cross_correlate_masked(actual_sig, expected_sig, is_okay, is_okay, axes=tuple(range(actual_sig.ndim)),)
+    lags = self.correlation_lags(len(expected_sig), dt)
+
+    n_frames_max_delay = int(max_lag / dt)
+    xcorr = xcorr[len(expected_sig) - 1: len(expected_sig) - 1 + n_frames_max_delay]
+    lags = lags[:n_frames_max_delay]
+
+    max_corr_index = np.argmax(xcorr)
+
+    lag, corr = lags[max_corr_index], xcorr[max_corr_index]
+    return lag, corr
 
 
 def main():
   config_realtime_process([0, 1, 2, 3], 5)
 
   pm = messaging.PubMaster(['liveActuatorDelay', 'alertDebug'])
-  sm = messaging.SubMaster(['carControl', 'carState', 'controlsState'], poll='controlsState')
+  sm = messaging.SubMaster(['livePose', 'liveCalibration', 'carControl', 'carState', 'controlsState'], poll='controlsState')
 
   params = Params()
   CP = messaging.log_from_bytes(params.get("CarParams", block=True), car.CarParams)
-  estimator = LagEstimator(CP, DT_CTRL, DT_CTRL * 25, MAX_LAG_HIST_LEN_SEC, MOVING_CORR_WINDOW)
+  estimator = LagEstimator(CP, DT_CTRL, MOVING_CORR_WINDOW, MIN_OKAY_WINDOW)
 
   while True:
     sm.update()
     if sm.all_checks():
-      for which in sm.updated.keys():
+      for which in sm.services:
         if sm.updated[which]:
           t = sm.logMonoTime[which] * 1e-9
           estimator.handle_log(t, which, sm[which])

selfdrive/test/process_replay/process_replay.py

@@ -571,7 +571,7 @@ CONFIGS = [
   ),
   ProcessConfig(
     proc_name="lagd",
-    pubs=["carState", "carControl", "controlsState"],
+    pubs=["carState", "carControl", "controlsState", "livePose", "liveCalibration"],
     subs=["liveActuatorDelay"],
     ignore=["logMonoTime"],
     init_callback=get_car_params_callback,