DM: more adaptive pose policy (commaai#23184)

* rename and add dep

* proto thresholds

* tweak vk

* update natural offset and clip offsets

* 95th looks good

* no punish for being relaxed

* yaw laplace only

* some pay more attention

* update ref

Co-authored-by: Willem Melching <willem.melching@gmail.com>

selfdrive/monitoring/dmonitoringd.py

@@ -43,7 +43,7 @@ def dmonitoringd_thread(sm=None, pm=None):
       v_cruise_last = v_cruise
 
     if sm.updated['modelV2']:
-      driver_status.set_policy(sm['modelV2'])
+      driver_status.set_policy(sm['modelV2'], sm['carState'].vEgo)
 
     # Get data from dmonitoringmodeld
     events = Events()

selfdrive/monitoring/driver_monitor.py

@@ -32,15 +32,21 @@ def __init__(self, TICI=TICI, DT_DMON=DT_DMON):
     self._BLINK_THRESHOLD = 0.82 if TICI else 0.588
     self._BLINK_THRESHOLD_SLACK = 0.9 if TICI else 0.77
     self._BLINK_THRESHOLD_STRICT = self._BLINK_THRESHOLD
-    self._PITCH_WEIGHT = 1.35  # pitch matters a lot more
-    self._POSESTD_THRESHOLD = 0.38 if TICI else 0.3
 
-    self._METRIC_THRESHOLD = 0.48
-    self._METRIC_THRESHOLD_SLACK = 0.66
-    self._METRIC_THRESHOLD_STRICT = self._METRIC_THRESHOLD
-    self._PITCH_NATURAL_OFFSET = 0.02  # people don't seem to look straight when they drive relaxed, rather a bit up
-    self._YAW_NATURAL_OFFSET = 0.08  # people don't seem to look straight when they drive relaxed, rather a bit to the right (center of car)
+    self._POSE_PITCH_THRESHOLD = 0.3237
+    self._POSE_PITCH_THRESHOLD_SLACK = 0.3657
+    self._POSE_PITCH_THRESHOLD_STRICT = self._POSE_PITCH_THRESHOLD
+    self._POSE_YAW_THRESHOLD = 0.3109
+    self._POSE_YAW_THRESHOLD_SLACK = 0.4294
+    self._POSE_YAW_THRESHOLD_STRICT = self._POSE_YAW_THRESHOLD
+    self._PITCH_NATURAL_OFFSET = 0.057 # initial value before offset is learned
+    self._YAW_NATURAL_OFFSET = 0.11 # initial value before offset is learned
+    self._PITCH_MAX_OFFSET = 0.124
+    self._PITCH_MIN_OFFSET = -0.0881
+    self._YAW_MAX_OFFSET = 0.289
+    self._YAW_MIN_OFFSET = -0.0246
 
+    self._POSESTD_THRESHOLD = 0.38 if TICI else 0.3
     self._HI_STD_FALLBACK_TIME = int(10  / self._DT_DMON)  # fall back to wheel touch if model is uncertain for 10s
     self._DISTRACTED_FILTER_TS = 0.25  # 0.6Hz
 
@@ -93,7 +99,8 @@ def __init__(self, max_trackable):
     self.pitch_offseter = RunningStatFilter(max_trackable=max_trackable)
     self.yaw_offseter = RunningStatFilter(max_trackable=max_trackable)
     self.low_std = True
-    self.cfactor = 1.
+    self.cfactor_pitch = 1.
+    self.cfactor_yaw = 1.
 
 class DriverBlink():
   def __init__(self):
@@ -164,30 +171,38 @@ def _is_driver_distracted(self, pose, blink):
       pitch_error = pose.pitch - self.settings._PITCH_NATURAL_OFFSET
       yaw_error = pose.yaw - self.settings._YAW_NATURAL_OFFSET
     else:
-      pitch_error = pose.pitch - self.pose.pitch_offseter.filtered_stat.mean()
-      yaw_error = pose.yaw - self.pose.yaw_offseter.filtered_stat.mean()
+      pitch_error = pose.pitch - min(max(self.pose.pitch_offseter.filtered_stat.mean(),
+                                                       self.settings._PITCH_MIN_OFFSET), self.settings._PITCH_MAX_OFFSET)
+      yaw_error = pose.yaw - min(max(self.pose.yaw_offseter.filtered_stat.mean(),
+                                                    self.settings._YAW_MIN_OFFSET), self.settings._YAW_MAX_OFFSET)
 
     pitch_error = 0 if pitch_error > 0 else abs(pitch_error) # no positive pitch limit
     yaw_error = abs(yaw_error)
 
-    if pitch_error*self.settings._PITCH_WEIGHT > self.settings._METRIC_THRESHOLD*pose.cfactor or \
-       yaw_error > self.settings._METRIC_THRESHOLD*pose.cfactor:
+    if pitch_error > self.settings._POSE_PITCH_THRESHOLD*pose.cfactor_pitch or \
+       yaw_error > self.settings._POSE_YAW_THRESHOLD*pose.cfactor_yaw:
       return DistractedType.BAD_POSE
     elif (blink.left_blink + blink.right_blink)*0.5 > self.settings._BLINK_THRESHOLD*blink.cfactor:
       return DistractedType.BAD_BLINK
     else:
       return DistractedType.NOT_DISTRACTED
 
-  def set_policy(self, model_data):
-    ep = min(model_data.meta.engagedProb, 0.8) / 0.8
-    self.pose.cfactor = interp(ep, [0, 0.5, 1],
-                                           [self.settings._METRIC_THRESHOLD_STRICT,
-                                            self.settings. _METRIC_THRESHOLD,
-                                            self.settings._METRIC_THRESHOLD_SLACK]) / self.settings._METRIC_THRESHOLD
+  def set_policy(self, model_data, car_speed):
+    ep = min(model_data.meta.engagedProb, 0.8) / 0.8 # engaged prob
+    bp = model_data.meta.disengagePredictions.brakeDisengageProbs[0] # brake disengage prob in next 2s
+    # TODO: retune adaptive blink
     self.blink.cfactor = interp(ep, [0, 0.5, 1],
                                            [self.settings._BLINK_THRESHOLD_STRICT,
                                             self.settings._BLINK_THRESHOLD,
                                             self.settings._BLINK_THRESHOLD_SLACK]) / self.settings._BLINK_THRESHOLD
+    k1 = max(-0.00156*((car_speed-16)**2)+0.6, 0.2)
+    bp_normal = max(min(bp / k1, 0.5),0)
+    self.pose.cfactor_pitch = interp(bp_normal, [0, 0.5],
+                                           [self.settings._POSE_PITCH_THRESHOLD_SLACK,
+                                            self.settings._POSE_PITCH_THRESHOLD_STRICT]) / self.settings._POSE_PITCH_THRESHOLD
+    self.pose.cfactor_yaw = interp(bp_normal, [0, 0.5],
+                                           [self.settings._POSE_YAW_THRESHOLD_SLACK,
+                                            self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
 
   def get_pose(self, driver_state, cal_rpy, car_speed, op_engaged):
     if not all(len(x) > 0 for x in [driver_state.faceOrientation, driver_state.facePosition,

selfdrive/test/process_replay/ref_commit

@@ -1 +1 @@
-7f618b16bfa47352143d6edac30fba05f9fdfc28
+179e288c649e6fd7b51773e5942da2013bfbd211