retropilot
/
openpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

pyTorch DM (#19760) 

* new model files

* no more rot

* no rot no diag switch

* should be correct and cache aware

* 405d7aeb

* parse new outputs

* change uncertain policy accordingly

* 1bdbd9ed

* adjust

* 0.5 is fine

* 3d4f9cab

* no face prob no loss

* clean up

* clean up test

* update refs

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
albatross
ZwX1616 2021-01-28 14:02:58 -08:00 committed by GitHub
parent 75d0802804
commit 8605ba8ab9
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 48 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
models/dmonitoring_model.current
View File

@ -1 +1,2 @@
e96f9be6-5741-42ea-bdcd-0be6515b4230
3d4f9cab-ba54-4871-9449-37f440329ca1
2d400e7514d044cd6cfc1fbafb756e04bb161d0a

BIN
models/dmonitoring_model.keras (Stored with Git LFS)
View File

Binary file not shown.

BIN
models/dmonitoring_model.onnx (Stored with Git LFS) 100644
View File

Binary file not shown.

BIN
models/dmonitoring_model_q.dlc (Stored with Git LFS)
View File

Binary file not shown.

39
selfdrive/modeld/models/dmonitoring.cc
View File

@ -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());
}

7
selfdrive/modeld/models/dmonitoring.h
View File

@ -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<uint8_t> resized_buf;
std::vector<uint8_t> resized_buf_rot;
std::vector<uint8_t> cropped_buf;
std::vector<uint8_t> premirror_cropped_buf;
std::vector<float> net_input_buf;

23
selfdrive/monitoring/driver_monitor.py
View File

@ -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

24
selfdrive/monitoring/test_monitoring.py
View File

@ -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()

2
selfdrive/test/process_replay/ref_commit
View File

@ -1 +1 @@
c3f3c37d75f133c2ce0f94fc87ffb5305986bc07
f69a7c09518ad0ee7a7ca88cd4a8974145f24a6d