DM: more adaptive pose policy (#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>pull/23220/head
parent
2799ef5292
commit
af5a418fa6
|
@ -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()
|
||||
|
|
|
@ -32,15 +32,21 @@ class DRIVER_MONITOR_SETTINGS():
|
|||
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._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._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._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 @@ class DriverPose():
|
|||
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 @@ class DriverStatus():
|
|||
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,
|
||||
|
|
|
@ -1 +1 @@
|
|||
7f618b16bfa47352143d6edac30fba05f9fdfc28
|
||||
179e288c649e6fd7b51773e5942da2013bfbd211
|
Loading…
Reference in New Issue