diff --git a/models/dmonitoring_model.current b/models/dmonitoring_model.current index b0026f152a..7ba21c3a96 100644 --- a/models/dmonitoring_model.current +++ b/models/dmonitoring_model.current @@ -1 +1,2 @@ -e96f9be6-5741-42ea-bdcd-0be6515b4230 \ No newline at end of file +3d4f9cab-ba54-4871-9449-37f440329ca1 +2d400e7514d044cd6cfc1fbafb756e04bb161d0a \ No newline at end of file diff --git a/models/dmonitoring_model.keras b/models/dmonitoring_model.keras deleted file mode 100644 index c3d58f6fc7..0000000000 --- a/models/dmonitoring_model.keras +++ /dev/null @@ -1,3 +0,0 @@ -version https://git-lfs.github.com/spec/v1 -oid sha256:09aa11a17a5a8173e231071898c499f9ea632e6e64285586122828b1bbc70d41 -size 4165968 diff --git a/models/dmonitoring_model.onnx b/models/dmonitoring_model.onnx new file mode 100644 index 0000000000..6f5abbd71f --- /dev/null +++ b/models/dmonitoring_model.onnx @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:d70becdef7bd4e8406675312556f205e22f4d4be66ac84361664b295b2c01695 +size 4452109 diff --git a/models/dmonitoring_model_q.dlc b/models/dmonitoring_model_q.dlc index 558b359bfa..0fa5dd0296 100644 --- a/models/dmonitoring_model_q.dlc +++ b/models/dmonitoring_model_q.dlc @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:beecf140ddc5da96cbdae3b869ebb3f5453dcd8e61e09d7d079c91e006b6df98 -size 1134208 +oid sha256:709780fe17318c30d63cbf883952f556a5e4ba4fc5a9029425895847f0dcecc2 +size 1221118 diff --git a/selfdrive/modeld/models/dmonitoring.cc b/selfdrive/modeld/models/dmonitoring.cc index d08c2298f1..858cd79f83 100644 --- a/selfdrive/modeld/models/dmonitoring.cc +++ b/selfdrive/modeld/models/dmonitoring.cc @@ -95,7 +95,8 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ cropped_width, cropped_height); } - auto [resized_y_buf, resized_u_buf, resized_v_buf] = get_yuv_buf(s->resized_buf, resized_width, resized_height); + auto [resized_buf, resized_u_buf, resized_v_buf] = get_yuv_buf(s->resized_buf, resized_width, resized_height); + uint8_t *resized_y_buf = resized_buf; libyuv::FilterMode mode = libyuv::FilterModeEnum::kFilterBilinear; libyuv::I420Scale(cropped_y_buf, cropped_width, cropped_u_buf, cropped_width/2, @@ -107,36 +108,24 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ resized_width, resized_height, mode); - // prerotate to be cache aware - auto [resized_buf_rot, resized_u_buf_rot, resized_v_buf_rot] = get_yuv_buf(s->resized_buf_rot, resized_width, resized_height); - uint8_t *resized_y_buf_rot = resized_buf_rot; - libyuv::I420Rotate(resized_y_buf, resized_width, - resized_u_buf, resized_width/2, - resized_v_buf, resized_width/2, - resized_y_buf_rot, resized_height, - resized_u_buf_rot, resized_height/2, - resized_v_buf_rot, resized_height/2, - // negative height causes a vertical flip to match previous - resized_width, -resized_height, libyuv::kRotate90); - int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v -> y|y|y|y|u|v float *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len); // one shot conversion, O(n) anyway // yuvframe2tensor, normalize - for (int c = 0; c < MODEL_WIDTH/2; c++) { - for (int r = 0; r < MODEL_HEIGHT/2; r++) { + for (int r = 0; r < MODEL_HEIGHT/2; r++) { + for (int c = 0; c < MODEL_WIDTH/2; c++) { // Y_ul - net_input_buf[(c*MODEL_HEIGHT/2) + r + (0*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r) + (2*c)*resized_height]); + net_input_buf[(r*MODEL_WIDTH/2) + c + (0*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r)*resized_width + (2*c)]); // Y_dl - net_input_buf[(c*MODEL_HEIGHT/2) + r + (1*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r+1) + (2*c)*resized_height]); + net_input_buf[(r*MODEL_WIDTH/2) + c + (1*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r+1)*resized_width + (2*c)]); // Y_ur - net_input_buf[(c*MODEL_HEIGHT/2) + r + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r) + (2*c+1)*resized_height]); + net_input_buf[(r*MODEL_WIDTH/2) + c + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r)*resized_width + (2*c+1)]); // Y_dr - net_input_buf[(c*MODEL_HEIGHT/2) + r + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(2*r+1) + (2*c+1)*resized_height]); + net_input_buf[(r*MODEL_WIDTH/2) + c + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(2*r+1)*resized_width + (2*c+1)]); // U - net_input_buf[(c*MODEL_HEIGHT/2) + r + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(resized_width*resized_height) + r + (c*resized_height/2)]); + net_input_buf[(r*MODEL_WIDTH/2) + c + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + r*resized_width/2 + c]); // V - net_input_buf[(c*MODEL_HEIGHT/2) + r + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf_rot[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + r + (c*resized_height/2)]); + net_input_buf[(r*MODEL_WIDTH/2) + c + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = input_lambda(resized_buf[(resized_width*resized_height) + ((resized_width/2)*(resized_height/2)) + c + (r*resized_width/2)]); } } @@ -168,6 +157,10 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_ memcpy(&ret.left_blink_prob, &s->output[31], sizeof ret.right_eye_prob); memcpy(&ret.right_blink_prob, &s->output[32], sizeof ret.right_eye_prob); memcpy(&ret.sg_prob, &s->output[33], sizeof ret.sg_prob); + memcpy(&ret.poor_vision, &s->output[34], sizeof ret.poor_vision); + memcpy(&ret.partial_face, &s->output[35], sizeof ret.partial_face); + memcpy(&ret.distracted_pose, &s->output[36], sizeof ret.distracted_pose); + memcpy(&ret.distracted_eyes, &s->output[37], sizeof ret.distracted_eyes); ret.face_orientation_meta[0] = softplus(ret.face_orientation_meta[0]); ret.face_orientation_meta[1] = softplus(ret.face_orientation_meta[1]); ret.face_orientation_meta[2] = softplus(ret.face_orientation_meta[2]); @@ -195,6 +188,10 @@ void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResu framed.setLeftBlinkProb(res.left_blink_prob); framed.setRightBlinkProb(res.right_blink_prob); framed.setSgProb(res.sg_prob); + framed.setPoorVision(res.poor_vision); + framed.setPartialFace(res.partial_face); + framed.setDistractedPose(res.distracted_pose); + framed.setDistractedEyes(res.distracted_eyes); if (send_raw_pred) { framed.setRawPred(raw_pred.asBytes()); } diff --git a/selfdrive/modeld/models/dmonitoring.h b/selfdrive/modeld/models/dmonitoring.h index 82bcec9359..dd015d409f 100644 --- a/selfdrive/modeld/models/dmonitoring.h +++ b/selfdrive/modeld/models/dmonitoring.h @@ -5,7 +5,7 @@ #include "runners/run.h" #include "messaging.hpp" -#define OUTPUT_SIZE 34 +#define OUTPUT_SIZE 38 typedef struct DMonitoringResult { float face_orientation[3]; @@ -18,6 +18,10 @@ typedef struct DMonitoringResult { float left_blink_prob; float right_blink_prob; float sg_prob; + float poor_vision; + float partial_face; + float distracted_pose; + float distracted_eyes; float dsp_execution_time; } DMonitoringResult; @@ -26,7 +30,6 @@ typedef struct DMonitoringModelState { bool is_rhd; float output[OUTPUT_SIZE]; std::vector resized_buf; - std::vector resized_buf_rot; std::vector cropped_buf; std::vector premirror_cropped_buf; std::vector net_input_buf; diff --git a/selfdrive/monitoring/driver_monitor.py b/selfdrive/monitoring/driver_monitor.py index 56eac51109..e70bebd687 100644 --- a/selfdrive/monitoring/driver_monitor.py +++ b/selfdrive/monitoring/driver_monitor.py @@ -21,8 +21,9 @@ _DISTRACTED_TIME = 11. _DISTRACTED_PRE_TIME_TILL_TERMINAL = 8. _DISTRACTED_PROMPT_TIME_TILL_TERMINAL = 6. -_FACE_THRESHOLD = 0.6 -_EYE_THRESHOLD = 0.6 +_FACE_THRESHOLD = 0.5 +_PARTIAL_FACE_THRESHOLD = 0.5 +_EYE_THRESHOLD = 0.5 _SG_THRESHOLD = 0.5 _BLINK_THRESHOLD = 0.5 _BLINK_THRESHOLD_SLACK = 0.65 @@ -109,10 +110,12 @@ class DriverStatus(): self.driver_distracted = False self.driver_distraction_filter = FirstOrderFilter(0., _DISTRACTED_FILTER_TS, DT_DMON) self.face_detected = False + self.face_partial = False self.terminal_alert_cnt = 0 self.terminal_time = 0 self.step_change = 0. self.active_monitoring_mode = True + self.is_model_uncertain = False self.hi_stds = 0 self.hi_std_alert_enabled = True self.threshold_prompt = _DISTRACTED_PROMPT_TIME_TILL_TERMINAL / _DISTRACTED_TIME @@ -180,19 +183,19 @@ class DriverStatus(): driver_state.faceOrientationStd, driver_state.facePositionStd]): return + self.face_partial = driver_state.partialFace > _PARTIAL_FACE_THRESHOLD + self.face_detected = driver_state.faceProb > _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 < _POSESTD_THRESHOLD + self.pose.low_std = model_std_max < _POSESTD_THRESHOLD and not self.face_partial self.blink.left_blink = driver_state.leftBlinkProb * (driver_state.leftEyeProb > _EYE_THRESHOLD) * (driver_state.sgProb < _SG_THRESHOLD) self.blink.right_blink = driver_state.rightBlinkProb * (driver_state.rightEyeProb > _EYE_THRESHOLD) * (driver_state.sgProb < _SG_THRESHOLD) - self.face_detected = driver_state.faceProb > _FACE_THRESHOLD and \ - abs(driver_state.facePosition[0]) <= 0.4 and abs(driver_state.facePosition[1]) <= 0.45 - self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0 - # first order filters + self.driver_distracted = self._is_driver_distracted(self.pose, self.blink) > 0 and \ + driver_state.faceProb > _FACE_THRESHOLD and self.pose.low_std self.driver_distraction_filter.update(self.driver_distracted) # update offseter @@ -204,11 +207,9 @@ class DriverStatus(): self.pose_calibrated = self.pose.pitch_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT and \ self.pose.yaw_offseter.filtered_stat.n > _POSE_OFFSET_MIN_COUNT - is_model_uncertain = self.hi_stds * DT_DMON > _HI_STD_FALLBACK_TIME - self._set_timers(self.face_detected and not is_model_uncertain) + self.is_model_uncertain = self.hi_stds * DT_DMON > _HI_STD_FALLBACK_TIME + self._set_timers(self.face_detected and not self.is_model_uncertain) if self.face_detected and not self.pose.low_std: - if not is_model_uncertain: - self.step_change *= min(1.0, max(0.6, 1.6*(model_std_max-0.5)*(model_std_max-2))) self.hi_stds += 1 elif self.face_detected and self.pose.low_std: self.hi_stds = 0 diff --git a/selfdrive/monitoring/test_monitoring.py b/selfdrive/monitoring/test_monitoring.py index f914677587..7648d3fba1 100755 --- a/selfdrive/monitoring/test_monitoring.py +++ b/selfdrive/monitoring/test_monitoring.py @@ -9,7 +9,7 @@ from selfdrive.monitoring.driver_monitor import DriverStatus, \ _AWARENESS_TIME, _AWARENESS_PRE_TIME_TILL_TERMINAL, \ _AWARENESS_PROMPT_TIME_TILL_TERMINAL, _DISTRACTED_TIME, \ _DISTRACTED_PRE_TIME_TILL_TERMINAL, _DISTRACTED_PROMPT_TIME_TILL_TERMINAL, \ - _POSESTD_THRESHOLD, _HI_STD_TIMEOUT + _POSESTD_THRESHOLD, _HI_STD_TIMEOUT, _HI_STD_FALLBACK_TIME EventName = car.CarEvent.EventName @@ -187,31 +187,17 @@ class TestMonitoring(unittest.TestCase): self.assertEqual(events[int((_redlight_time-0.1)/DT_DMON)].names[0], EventName.preDriverDistracted) self.assertEqual(events[int((_redlight_time+0.5)/DT_DMON)].names[0], EventName.promptDriverDistracted) - # engaged, model is extremely uncertain. driver first attentive, then distracted - # - should pop a uncertain message first, then slowly into active green/orange, finally back to wheel touch but timer locked by orange - def test_one_indecisive_model(self): - ds_vector = [msg_ATTENTIVE_UNCERTAIN] * int(_UNCERTAIN_SECONDS_TO_GREEN/DT_DMON) + \ - [msg_ATTENTIVE] * int(_DISTRACTED_SECONDS_TO_ORANGE/DT_DMON) + \ - [msg_DISTRACTED_UNCERTAIN] * (int(_TEST_TIMESPAN/DT_DMON)-int((_DISTRACTED_SECONDS_TO_ORANGE+_UNCERTAIN_SECONDS_TO_GREEN)/DT_DMON)) - interaction_vector = always_false[:] - events = self._run_seq(ds_vector, interaction_vector, always_true, always_false)[0] - self.assertTrue(len(events[int(_UNCERTAIN_SECONDS_TO_GREEN*0.5/DT_DMON)]) == 0) - self.assertEqual(events[int((_HI_STD_TIMEOUT)/DT_DMON)].names[0], EventName.driverMonitorLowAcc) - self.assertTrue(len(events[int((_UNCERTAIN_SECONDS_TO_GREEN+_DISTRACTED_SECONDS_TO_ORANGE-0.5)/DT_DMON)]) == 0) - self.assertTrue(EventName.promptDriverDistracted in events[int((_TEST_TIMESPAN-5.)/DT_DMON)].names) - # engaged, model is somehow uncertain and driver is distracted - # - should slow down the alert countdown but it still gets there + # - should fall back to wheel touch after uncertain alert def test_somehow_indecisive_model(self): ds_vector = [msg_DISTRACTED_BUT_SOMEHOW_UNCERTAIN] * int(_TEST_TIMESPAN/DT_DMON) interaction_vector = always_false[:] events = self._run_seq(ds_vector, interaction_vector, always_true, always_false)[0] self.assertEqual(len(events[int(_UNCERTAIN_SECONDS_TO_GREEN*0.5/DT_DMON)]), 0) self.assertEqual(events[int((_HI_STD_TIMEOUT)/DT_DMON)].names[0], EventName.driverMonitorLowAcc) - self.assertTrue(EventName.preDriverDistracted in events[int((2*(_DISTRACTED_TIME-_DISTRACTED_PRE_TIME_TILL_TERMINAL))/DT_DMON)].names) - self.assertTrue(EventName.promptDriverDistracted in events[int((2*(_DISTRACTED_TIME-_DISTRACTED_PROMPT_TIME_TILL_TERMINAL))/DT_DMON)].names) - self.assertEqual(events[int((_DISTRACTED_TIME+1)/DT_DMON)].names[0], EventName.promptDriverDistracted) - self.assertEqual(events[int((_DISTRACTED_TIME*2.5)/DT_DMON)].names[0], EventName.promptDriverDistracted) # set_timer blocked + self.assertTrue(EventName.preDriverUnresponsive in events[int((_INVISIBLE_SECONDS_TO_ORANGE-1+_HI_STD_FALLBACK_TIME-0.1)/DT_DMON)].names) + self.assertTrue(EventName.promptDriverUnresponsive in events[int((_INVISIBLE_SECONDS_TO_ORANGE-1+_HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names) + self.assertTrue(EventName.driverUnresponsive in events[int((_INVISIBLE_SECONDS_TO_RED-1+_HI_STD_FALLBACK_TIME+0.1)/DT_DMON)].names) if __name__ == "__main__": unittest.main() diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 3e9569f3b2..5ebe739bef 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -c3f3c37d75f133c2ce0f94fc87ffb5305986bc07 \ No newline at end of file +f69a7c09518ad0ee7a7ca88cd4a8974145f24a6d \ No newline at end of file