DM: more precision running on DSP + e2e outputs (commaai#23900)

* update cereal

* run but not use

* log distraction type

* regression scaling

* clean up naming

* add calib buf

* add to header

* fake model

* no calib model

* adjust threshs

* 018a305f

* fix bn

* tweak1

* tweak2

* 0ff2/666

* tweak3

* t4

* t5

* fix out of bound

* skip when replaying old segments

* update ref

* fix onnxmodel

* get calib

* update model replay refs

* up ref

Author: ZwX1616

cereal

@@ -1,2 +1,2 @@
-4e19be90-bd5b-485d-b79a-2462f7f1b49e
-08f7ec37b78228cd1cb750b6ddb9c6ba1769e911
+0ff292a8-3a9c-47e7-9134-2c3b0f69a1fe
+d7c2883ee58d7b757e588bdf13ec45891caf397c

models/dmonitoring_model.current

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:895ee32e2a1c77496e015270db475eef65034b25331f2859bac0ccf702f64298
-size 3294407
+oid sha256:36cdea3c4b03f91cb243e4fa51f52e350e4906afcc5014345190e0c1d2bfaf25
+size 3792414

models/dmonitoring_model.onnx

@@ -1,3 +1,3 @@
 version https://git-lfs.github.com/spec/v1
-oid sha256:6e4ac870984d11cd8e86cda4a63e3321fde837bacf4a055a27b7c8ba34facfe2
-size 916079
+oid sha256:41901abff0e16e6c404627234cca68001fbd21f6961709a729a84a8d5e3cc56d
+size 1146001

models/dmonitoring_model_q.dlc

@@ -13,15 +13,25 @@ ExitHandler do_exit;
 
 void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) {
   PubMaster pm({"driverState"});
+  SubMaster sm({"liveCalibration"});
+  float calib[CALIB_LEN] = {0};
   double last = 0;
 
   while (!do_exit) {
     VisionIpcBufExtra extra = {};
     VisionBuf *buf = vipc_client.recv(&extra);
     if (buf == nullptr) continue;
 
+    sm.update(0);
+    if (sm.updated("liveCalibration")) {
+      auto calib_msg = sm["liveCalibration"].getLiveCalibration().getRpyCalib();
+      for (int i = 0; i < CALIB_LEN; i++) {
+        calib[i] = calib_msg[i];
+      }
+    }
+
     double t1 = millis_since_boot();
-    DMonitoringResult res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height);
+    DMonitoringResult res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height, calib);
     double t2 = millis_since_boot();
 
     // send dm packet

selfdrive/modeld/dmonitoringmodeld.cc

@@ -70,7 +70,3 @@ void softmax(const float* input, float* output, size_t len) {
 float sigmoid(float input) {
   return 1 / (1 + expf(-input));
 }
-
-float softplus(float input) {
-  return log1p(expf(input));
-}

selfdrive/modeld/models/commonmodel.cc

@@ -19,7 +19,6 @@
 const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL;
 
 void softmax(const float* input, float* output, size_t len);
-float softplus(float input);
 float sigmoid(float input);
 
 class ModelFrame {

selfdrive/modeld/models/commonmodel.h

@@ -43,6 +43,8 @@ void dmonitoring_init(DMonitoringModelState* s) {
 #else
   s->m = new SNPEModel("../../models/dmonitoring_model_q.dlc", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME);
 #endif
+
+  s->m->addCalib(s->calib, CALIB_LEN);
 }
 
 static inline auto get_yuv_buf(std::vector<uint8_t> &buf, const int width, int height) {
@@ -65,7 +67,7 @@ void crop_yuv(uint8_t *raw, int width, int height, uint8_t *y, uint8_t *u, uint8
   }
 }
 
-DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height) {
+DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, float *calib) {
   Rect crop_rect;
   if (width == TICI_CAM_WIDTH) {
     const int cropped_height = tici_dm_crop::width / 1.33;
@@ -167,29 +169,38 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
 
   double t1 = millis_since_boot();
   s->m->addImage(net_input_buf, yuv_buf_len);
+  for (int i = 0; i < CALIB_LEN; i++) {
+    s->calib[i] = calib[i];
+  }
   s->m->execute();
   double t2 = millis_since_boot();
 
   DMonitoringResult ret = {0};
   for (int i = 0; i < 3; ++i) {
-    ret.face_orientation[i] = s->output[i];
-    ret.face_orientation_meta[i] = softplus(s->output[6 + i]);
+    ret.face_orientation[i] = s->output[i] * REG_SCALE;
+    ret.face_orientation_meta[i] = exp(s->output[6 + i]);
+  }
+  for (int i = 0; i < 2; ++i) {
+    ret.face_position[i] = s->output[3 + i] * REG_SCALE;
+    ret.face_position_meta[i] = exp(s->output[9 + i]);
+  }
+  for (int i = 0; i < 4; ++i) {
+    ret.ready_prob[i] = sigmoid(s->output[39 + i]);
   }
   for (int i = 0; i < 2; ++i) {
-    ret.face_position[i] = s->output[3 + i];
-    ret.face_position_meta[i] = softplus(s->output[9 + i]);
+    ret.not_ready_prob[i] = sigmoid(s->output[43 + i]);
   }
-  ret.face_prob = s->output[12];
-  ret.left_eye_prob = s->output[21];
-  ret.right_eye_prob = s->output[30];
-  ret.left_blink_prob = s->output[31];
-  ret.right_blink_prob = s->output[32];
-  ret.sg_prob = s->output[33];
-  ret.poor_vision = s->output[34];
-  ret.partial_face = s->output[35];
-  ret.distracted_pose = s->output[36];
-  ret.distracted_eyes = s->output[37];
-  ret.occluded_prob = s->output[38];
+  ret.face_prob = sigmoid(s->output[12]);
+  ret.left_eye_prob = sigmoid(s->output[21]);
+  ret.right_eye_prob = sigmoid(s->output[30]);
+  ret.left_blink_prob = sigmoid(s->output[31]);
+  ret.right_blink_prob = sigmoid(s->output[32]);
+  ret.sg_prob = sigmoid(s->output[33]);
+  ret.poor_vision = sigmoid(s->output[34]);
+  ret.partial_face = sigmoid(s->output[35]);
+  ret.distracted_pose = sigmoid(s->output[36]);
+  ret.distracted_eyes = sigmoid(s->output[37]);
+  ret.occluded_prob = sigmoid(s->output[38]);
   ret.dsp_execution_time = (t2 - t1) / 1000.;
   return ret;
 }
@@ -217,6 +228,8 @@ void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResu
   framed.setDistractedPose(res.distracted_pose);
   framed.setDistractedEyes(res.distracted_eyes);
   framed.setOccludedProb(res.occluded_prob);
+  framed.setReadyProb(res.ready_prob);
+  framed.setNotReadyProb(res.not_ready_prob);
   if (send_raw_pred) {
     framed.setRawPredictions(raw_pred.asBytes());
   }

selfdrive/modeld/models/dmonitoring.cc

@@ -7,7 +7,10 @@
 #include "selfdrive/modeld/models/commonmodel.h"
 #include "selfdrive/modeld/runners/run.h"
 
-#define OUTPUT_SIZE 39
+#define CALIB_LEN 3
+
+#define OUTPUT_SIZE 45
+#define REG_SCALE 0.25f
 
 typedef struct DMonitoringResult {
   float face_orientation[3];
@@ -25,6 +28,8 @@ typedef struct DMonitoringResult {
   float distracted_pose;
   float distracted_eyes;
   float occluded_prob;
+  float ready_prob[4];
+  float not_ready_prob[2];
   float dsp_execution_time;
 } DMonitoringResult;
 
@@ -36,11 +41,12 @@ typedef struct DMonitoringModelState {
   std::vector<uint8_t> cropped_buf;
   std::vector<uint8_t> premirror_cropped_buf;
   std::vector<float> net_input_buf;
+  float calib[CALIB_LEN];
   float tensor[UINT8_MAX + 1];
 } DMonitoringModelState;
 
 void dmonitoring_init(DMonitoringModelState* s);
-DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height);
+DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, float *calib);
 void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time, kj::ArrayPtr<const float> raw_pred);
 void dmonitoring_free(DMonitoringModelState* s);
 

selfdrive/modeld/models/dmonitoring.h

@@ -100,6 +100,11 @@ void ONNXModel::addTrafficConvention(float *state, int state_size) {
   traffic_convention_size = state_size;
 }
 
+void ONNXModel::addCalib(float *state, int state_size) {
+  calib_input_buf = state;
+  calib_size = state_size;
+}
+
 void ONNXModel::addImage(float *image_buf, int buf_size) {
   image_input_buf = image_buf;
   image_buf_size = buf_size;
@@ -124,6 +129,9 @@ void ONNXModel::execute() {
   if (traffic_convention_input_buf != NULL) {
     pwrite(traffic_convention_input_buf, traffic_convention_size);
   }
+  if (calib_input_buf != NULL) {
+    pwrite(calib_input_buf, calib_size);
+  }
   if (rnn_input_buf != NULL) {
     pwrite(rnn_input_buf, rnn_state_size);
   }

selfdrive/modeld/runners/onnxmodel.cc

@@ -11,6 +11,7 @@ class ONNXModel : public RunModel {
   void addRecurrent(float *state, int state_size);
   void addDesire(float *state, int state_size);
   void addTrafficConvention(float *state, int state_size);
+  void addCalib(float *state, int state_size);
   void addImage(float *image_buf, int buf_size);
   void addExtra(float *image_buf, int buf_size);
   void execute();
@@ -26,6 +27,8 @@ class ONNXModel : public RunModel {
   int desire_state_size;
   float *traffic_convention_input_buf = NULL;
   int traffic_convention_size;
+  float *calib_input_buf = NULL;
+  int calib_size;
   float *image_input_buf = NULL;
   int image_buf_size;
   float *extra_input_buf = NULL;

selfdrive/modeld/runners/onnxmodel.h

@@ -5,6 +5,7 @@ class RunModel {
   virtual void addRecurrent(float *state, int state_size) {}
   virtual void addDesire(float *state, int state_size) {}
   virtual void addTrafficConvention(float *state, int state_size) {}
+  virtual void addCalib(float *state, int state_size) {}
   virtual void addImage(float *image_buf, int buf_size) {}
   virtual void addExtra(float *image_buf, int buf_size) {}
   virtual void execute() {}

selfdrive/modeld/runners/runmodel.h

@@ -141,6 +141,11 @@ void SNPEModel::addDesire(float *state, int state_size) {
   desireBuffer = this->addExtra(state, state_size, 1);
 }
 
+void SNPEModel::addCalib(float *state, int state_size) {
+  calib = state;
+  calibBuffer = this->addExtra(state, state_size, 1);
+}
+
 void SNPEModel::addImage(float *image_buf, int buf_size) {
   input = image_buf;
   input_size = buf_size;

selfdrive/modeld/runners/snpemodel.cc

@@ -25,6 +25,7 @@ class SNPEModel : public RunModel {
   SNPEModel(const char *path, float *loutput, size_t loutput_size, int runtime, bool luse_extra = false);
   void addRecurrent(float *state, int state_size);
   void addTrafficConvention(float *state, int state_size);
+  void addCalib(float *state, int state_size);
   void addDesire(float *state, int state_size);
   void addImage(float *image_buf, int buf_size);
   void addExtra(float *image_buf, int buf_size);
@@ -71,4 +72,6 @@ class SNPEModel : public RunModel {
   std::unique_ptr<zdl::DlSystem::IUserBuffer> trafficConventionBuffer;
   float *desire;
   std::unique_ptr<zdl::DlSystem::IUserBuffer> desireBuffer;
+  float *calib;
+  std::unique_ptr<zdl::DlSystem::IUserBuffer> calibBuffer;
 };

selfdrive/modeld/runners/snpemodel.h

@@ -51,22 +51,23 @@ def dmonitoringd_thread(sm=None, pm=None):
 
     # Get data from dmonitoringmodeld
     events = Events()
-    driver_status.get_pose(sm['driverState'], 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 \
        driver_status.terminal_time >= driver_status.settings._MAX_TERMINAL_DURATION:
       events.add(car.CarEvent.EventName.tooDistracted)
 
     # Update events from driver state
-    driver_status.update(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill)
+    driver_status.update_events(events, driver_engaged, sm['controlsState'].enabled, sm['carState'].standstill)
 
     # build driverMonitoringState packet
     dat = messaging.new_message('driverMonitoringState')
     dat.driverMonitoringState = {
       "events": events.to_msg(),
       "faceDetected": driver_status.face_detected,
       "isDistracted": driver_status.driver_distracted,
+      "distractedType": sum(driver_status.distracted_types),
       "awarenessStatus": driver_status.awareness,
       "posePitchOffset": driver_status.pose.pitch_offseter.filtered_stat.mean(),
       "posePitchValidCount": driver_status.pose.pitch_offseter.filtered_stat.n,