mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-21 00:03:45 +08:00
Code Issues Packages Projects Releases Wiki Activity

Revert fullframe DM model (#24812)

Browse Source 
* Revert "fullframe DM: flip RHD yaw to use matching thresholds"

This reverts commit ce7daabc8847d18ba46e5d1879f5a6958d04ccc7.

* Revert "fullframe DM model (#24762)"

This reverts commit 817be81fb19004f4873881f6b29dcdfffbe7e3a8.

* revert cereal
old-commit-hash: c646eeee0ac54925db5afc51b95c5d869d6dba68
This commit is contained in:
ZwX1616
2022-06-10 16:16:46 -07:00
committed by GitHub
parent 77224f3332
commit c73d4e5fc6
25 changed files with 309 additions and 228 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/monitoring/dmonitoringd.py
+3 -4
View File
@@ -18,7 +18,7 @@ def dmonitoringd_thread(sm=None, pm=None):
pm = messaging.PubMaster(['driverMonitoringState'])
if sm is None:
sm = messaging.SubMaster(['driverStateV2', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverStateV2'])
sm = messaging.SubMaster(['driverState', 'liveCalibration', 'carState', 'controlsState', 'modelV2'], poll=['driverState'])
driver_status = DriverStatus(rhd=Params().get_bool("IsRHD"))
@@ -34,7 +34,7 @@ def dmonitoringd_thread(sm=None, pm=None):
while True:
sm.update()
if not sm.updated['driverStateV2']:
if not sm.updated['driverState']:
continue
# Get interaction
@@ -51,7 +51,7 @@ def dmonitoringd_thread(sm=None, pm=None):
# Get data from dmonitoringmodeld
events = Events()
driver_status.update_states(sm['driverStateV2'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled)
driver_status.update_states(sm['driverState'], sm['liveCalibration'].rpyCalib, sm['carState'].vEgo, sm['controlsState'].enabled)
# Block engaging after max number of distrations
if driver_status.terminal_alert_cnt >= driver_status.settings._MAX_TERMINAL_ALERTS or \
@@ -79,7 +79,6 @@ def dmonitoringd_thread(sm=None, pm=None):
"isLowStd": driver_status.pose.low_std,
"hiStdCount": driver_status.hi_stds,
"isActiveMode": driver_status.active_monitoring_mode,
"isRHD": driver_status.wheel_on_right,
}
pm.send('driverMonitoringState', dat)
selfdrive/monitoring/driver_monitor.py
+44 -46
View File
@@ -5,7 +5,6 @@ from common.numpy_fast import interp
from common.realtime import DT_DMON
from common.filter_simple import FirstOrderFilter
from common.stat_live import RunningStatFilter
from common.transformations.camera import tici_d_frame_size
EventName = car.CarEvent.EventName
@@ -27,29 +26,32 @@ class DRIVER_MONITOR_SETTINGS():
self._DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6.
self._FACE_THRESHOLD = 0.5
self._PARTIAL_FACE_THRESHOLD = 0.8
self._EYE_THRESHOLD = 0.65
self._SG_THRESHOLD = 0.925
self._BLINK_THRESHOLD = 0.861
self._BLINK_THRESHOLD = 0.8
self._BLINK_THRESHOLD_SLACK = 0.9
self._BLINK_THRESHOLD_STRICT = self._BLINK_THRESHOLD
self._EE_THRESH11 = 0.75
self._EE_THRESH12 = 3.25
self._EE_THRESH21 = 0.01
self._EE_THRESH22 = 0.35
self._POSE_PITCH_THRESHOLD = 0.3133
self._POSE_PITCH_THRESHOLD_SLACK = 0.3237
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.4020
self._POSE_YAW_THRESHOLD_SLACK = 0.5042
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.029 # initial value before offset is learned
self._YAW_NATURAL_OFFSET = 0.097 # initial value before offset is learned
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.3
self._POSESTD_THRESHOLD = 0.315
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
@@ -57,9 +59,6 @@ class DRIVER_MONITOR_SETTINGS():
self._POSE_OFFSET_MIN_COUNT = int(60 / self._DT_DMON) # valid data counts before calibration completes, 1min cumulative
self._POSE_OFFSET_MAX_COUNT = int(360 / self._DT_DMON) # stop deweighting new data after 6 min, aka "short term memory"
self._WHEELPOS_THRESHOLD = 0.5
self._WHEELPOS_FILTER_MIN_COUNT = int(5 / self._DT_DMON)
self._RECOVERY_FACTOR_MAX = 5. # relative to minus step change
self._RECOVERY_FACTOR_MIN = 1.25 # relative to minus step change
@@ -67,9 +66,9 @@ class DRIVER_MONITOR_SETTINGS():
self._MAX_TERMINAL_DURATION = int(30 / self._DT_DMON) # not allowed to engage after 30s of terminal alerts
# model output refers to center of undistorted+leveled image
EFL = 598.0 # focal length in K
W, H = tici_d_frame_size # corrected image has same size as raw
# model output refers to center of cropped image, so need to apply the x displacement offset
RESIZED_FOCAL = 320.0
H, W, FULL_W = 320, 160, 426
class DistractedType:
NOT_DISTRACTED = 0
@@ -77,22 +76,22 @@ class DistractedType:
DISTRACTED_BLINK = 2
DISTRACTED_E2E = 4
def face_orientation_from_net(angles_desc, pos_desc, rpy_calib):
def face_orientation_from_net(angles_desc, pos_desc, rpy_calib, is_rhd):
# the output of these angles are in device frame
# so from driver's perspective, pitch is up and yaw is right
pitch_net, yaw_net, roll_net = angles_desc
face_pixel_position = ((pos_desc[0]+0.5)*W, (pos_desc[1]+0.5)*H)
yaw_focal_angle = atan2(face_pixel_position[0] - W//2, EFL)
pitch_focal_angle = atan2(face_pixel_position[1] - H//2, EFL)
face_pixel_position = ((pos_desc[0] + .5)*W - W + FULL_W, (pos_desc[1]+.5)*H)
yaw_focal_angle = atan2(face_pixel_position[0] - FULL_W//2, RESIZED_FOCAL)
pitch_focal_angle = atan2(face_pixel_position[1] - H//2, RESIZED_FOCAL)
pitch = pitch_net + pitch_focal_angle
yaw = -yaw_net + yaw_focal_angle
# no calib for roll
pitch -= rpy_calib[1]
yaw -= rpy_calib[2]
yaw -= rpy_calib[2] * (1 - 2 * int(is_rhd)) # lhd -> -=, rhd -> +=
return roll_net, pitch, yaw
class DriverPose():
@@ -113,6 +112,7 @@ class DriverBlink():
def __init__(self):
self.left_blink = 0.
self.right_blink = 0.
self.cfactor = 1.
class DriverStatus():
def __init__(self, rhd=False, settings=DRIVER_MONITOR_SETTINGS()):
@@ -120,7 +120,7 @@ class DriverStatus():
self.settings = settings
# init driver status
# self.wheelpos_learner = RunningStatFilter()
self.is_rhd_region = rhd
self.pose = DriverPose(self.settings._POSE_OFFSET_MAX_COUNT)
self.pose_calibrated = False
self.blink = DriverBlink()
@@ -137,8 +137,8 @@ class DriverStatus():
self.distracted_types = []
self.driver_distracted = False
self.driver_distraction_filter = FirstOrderFilter(0., self.settings._DISTRACTED_FILTER_TS, self.settings._DT_DMON)
self.wheel_on_right = rhd
self.face_detected = False
self.face_partial = False
self.terminal_alert_cnt = 0
self.terminal_time = 0
self.step_change = 0.
@@ -197,7 +197,7 @@ class DriverStatus():
yaw_error > self.settings._POSE_YAW_THRESHOLD*self.pose.cfactor_yaw:
distracted_types.append(DistractedType.DISTRACTED_POSE)
if (self.blink.left_blink + self.blink.right_blink)*0.5 > self.settings._BLINK_THRESHOLD:
if (self.blink.left_blink + self.blink.right_blink)*0.5 > self.settings._BLINK_THRESHOLD*self.blink.cfactor:
distracted_types.append(DistractedType.DISTRACTED_BLINK)
if self.ee1_calibrated:
@@ -214,7 +214,13 @@ class DriverStatus():
return distracted_types
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],
@@ -225,36 +231,28 @@ class DriverStatus():
self.settings._POSE_YAW_THRESHOLD_STRICT]) / self.settings._POSE_YAW_THRESHOLD
def update_states(self, driver_state, cal_rpy, car_speed, op_engaged):
# rhd_pred = driver_state.wheelOnRightProb
# if car_speed > 0.01:
# self.wheelpos_learner.push_and_update(rhd_pred)
# if self.wheelpos_learner.filtered_stat.n > self.settings._WHEELPOS_FILTER_MIN_COUNT:
# self.wheel_on_right = self.wheelpos_learner.filtered_stat.M > self.settings._WHEELPOS_THRESHOLD
# else:
# self.wheel_on_right = rhd_pred > self.settings._WHEELPOS_THRESHOLD
driver_data = driver_state.rightDriverData if self.wheel_on_right else driver_state.leftDriverData
if not all(len(x) > 0 for x in (driver_data.faceOrientation, driver_data.facePosition,
driver_data.faceOrientationStd, driver_data.facePositionStd,
driver_data.readyProb, driver_data.notReadyProb)):
if not all(len(x) > 0 for x in (driver_state.faceOrientation, driver_state.facePosition,
driver_state.faceOrientationStd, driver_state.facePositionStd,
driver_state.readyProb, driver_state.notReadyProb)):
return
self.face_detected = driver_data.faceProb > self.settings._FACE_THRESHOLD
self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_data.faceOrientation, driver_data.facePosition, cal_rpy)
if self.wheel_on_right:
self.pose.yaw *= -1
self.pose.pitch_std = driver_data.faceOrientationStd[0]
self.pose.yaw_std = driver_data.faceOrientationStd[1]
self.face_partial = driver_state.partialFace > self.settings._PARTIAL_FACE_THRESHOLD
self.face_detected = driver_state.faceProb > self.settings._FACE_THRESHOLD or self.face_partial
self.pose.roll, self.pose.pitch, self.pose.yaw = face_orientation_from_net(driver_state.faceOrientation, driver_state.facePosition, cal_rpy, self.is_rhd_region)
self.pose.pitch_std = driver_state.faceOrientationStd[0]
self.pose.yaw_std = driver_state.faceOrientationStd[1]
# self.pose.roll_std = driver_state.faceOrientationStd[2]
model_std_max = max(self.pose.pitch_std, self.pose.yaw_std)
self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD
self.blink.left_blink = driver_data.leftBlinkProb * (driver_data.leftEyeProb > self.settings._EYE_THRESHOLD) * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
self.blink.right_blink = driver_data.rightBlinkProb * (driver_data.rightEyeProb > self.settings._EYE_THRESHOLD) * (driver_data.sunglassesProb < self.settings._SG_THRESHOLD)
self.eev1 = driver_data.notReadyProb[1]
self.eev2 = driver_data.readyProb[0]
self.pose.low_std = model_std_max < self.settings._POSESTD_THRESHOLD and not self.face_partial
self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb > self.settings._EYE_THRESHOLD) * (driver_state.sunglassesProb < self.settings._SG_THRESHOLD)
self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb > self.settings._EYE_THRESHOLD) * (driver_state.sunglassesProb < self.settings._SG_THRESHOLD)
self.eev1 = driver_state.notReadyProb[1]
self.eev2 = driver_state.readyProb[0]
self.distracted_types = self._get_distracted_types()
self.driver_distracted = (DistractedType.DISTRACTED_POSE in self.distracted_types or
DistractedType.DISTRACTED_BLINK in self.distracted_types) and \
driver_data.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
driver_state.faceProb > self.settings._FACE_THRESHOLD and self.pose.low_std
self.driver_distraction_filter.update(self.driver_distracted)
# update offseter
selfdrive/monitoring/test_monitoring.py
+12 -12
View File
@@ -17,19 +17,19 @@ INVISIBLE_SECONDS_TO_ORANGE = dm_settings._AWARENESS_TIME - dm_settings._AWARENE
INVISIBLE_SECONDS_TO_RED = dm_settings._AWARENESS_TIME + 1
def make_msg(face_detected, distracted=False, model_uncertain=False):
ds = log.DriverStateV2.new_message()
ds.leftDriverData.faceOrientation = [0., 0., 0.]
ds.leftDriverData.facePosition = [0., 0.]
ds.leftDriverData.faceProb = 1. * face_detected
ds.leftDriverData.leftEyeProb = 1.
ds.leftDriverData.rightEyeProb = 1.
ds.leftDriverData.leftBlinkProb = 1. * distracted
ds.leftDriverData.rightBlinkProb = 1. * distracted
ds.leftDriverData.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain]
ds.leftDriverData.facePositionStd = [1.*model_uncertain, 1.*model_uncertain]
ds = log.DriverState.new_message()
ds.faceOrientation = [0., 0., 0.]
ds.facePosition = [0., 0.]
ds.faceProb = 1. * face_detected
ds.leftEyeProb = 1.
ds.rightEyeProb = 1.
ds.leftBlinkProb = 1. * distracted
ds.rightBlinkProb = 1. * distracted
ds.faceOrientationStd = [1.*model_uncertain, 1.*model_uncertain, 1.*model_uncertain]
ds.facePositionStd = [1.*model_uncertain, 1.*model_uncertain]
# TODO: test both separately when e2e is used
ds.leftDriverData.readyProb = [0., 0., 0., 0.]
ds.leftDriverData.notReadyProb = [0., 0.]
ds.readyProb = [0., 0., 0., 0.]
ds.notReadyProb = [0., 0.]
return ds