sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

refactor parsing

Browse Source
pull/24762/head
ZwX1616 3 years ago
parent 2fa73b7fdf
commit 1af6c77f81
3 changed files with 73 additions and 62 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 4
      selfdrive/modeld/dmonitoringmodeld.cc
  2. 106
      selfdrive/modeld/models/dmonitoring.cc
  3. 25
      selfdrive/modeld/models/dmonitoring.h

4
selfdrive/modeld/dmonitoringmodeld.cc
Unescape View File

@ -31,11 +31,11 @@ void run_model(DMonitoringModelState &model, VisionIpcClient &vipc_client) {
}
double t1 = millis_since_boot();
DMonitoringResult res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height, calib);
DMonitoringModelResult model_res = dmonitoring_eval_frame(&model, buf->addr, buf->width, buf->height, calib);
double t2 = millis_since_boot();
// send dm packet
dmonitoring_publish(pm, extra.frame_id, res, (t2 - t1) / 1000.0, model.output);
dmonitoring_publish(pm, extra.frame_id, model_res, (t2 - t1) / 1000.0, model.output);
//printf("dmonitoring process: %.2fms, from last %.2fms\n", t2 - t1, t1 - last);
last = t1;

106
selfdrive/modeld/models/dmonitoring.cc
Unescape View File

@ -31,7 +31,47 @@ void dmonitoring_init(DMonitoringModelState* s) {
s->m->addCalib(s->calib, CALIB_LEN);
}
DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, float *calib) {
void parse_driver_data(DriverStateResult &ds_res, const DMonitoringModelState* s, int out_idx_offset) {
for (int i = 0; i < 3; ++i) {
ds_res.face_orientation[i] = s->output[out_idx_offset+i] * REG_SCALE;
ds_res.face_orientation_std[i] = exp(s->output[out_idx_offset+6+i]);
}
for (int i = 0; i < 2; ++i) {
ds_res.face_position[i] = s->output[out_idx_offset+3+i] * REG_SCALE;
ds_res.face_position_std[i] = exp(s->output[out_idx_offset+9+i]);
}
for (int i = 0; i < 4; ++i) {
ds_res.ready_prob[i] = sigmoid(s->output[out_idx_offset+35+i]);
}
for (int i = 0; i < 2; ++i) {
ds_res.not_ready_prob[i] = sigmoid(s->output[out_idx_offset+39+i]);
}
ds_res.face_prob = sigmoid(s->output[out_idx_offset+12]);
ds_res.left_eye_prob = sigmoid(s->output[out_idx_offset+21]);
ds_res.right_eye_prob = sigmoid(s->output[out_idx_offset+30]);
ds_res.left_blink_prob = sigmoid(s->output[out_idx_offset+31]);
ds_res.right_blink_prob = sigmoid(s->output[out_idx_offset+32]);
ds_res.sunglasses_prob = sigmoid(s->output[out_idx_offset+33]);
ds_res.occluded_prob = sigmoid(s->output[out_idx_offset+34]);
}
void fill_driver_data(cereal::DriverState::DriverData::Builder ddata, const DriverStateResult &ds_res) {
ddata.setFaceOrientation(ds_res.face_orientation);
ddata.setFaceOrientationStd(ds_res.face_orientation_std);
ddata.setFacePosition(ds_res.face_position);
ddata.setFacePositionStd(ds_res.face_position_std);
ddata.setFaceProb(ds_res.face_prob);
ddata.setLeftEyeProb(ds_res.left_eye_prob);
ddata.setRightEyeProb(ds_res.right_eye_prob);
ddata.setLeftBlinkProb(ds_res.left_blink_prob);
ddata.setRightBlinkProb(ds_res.right_blink_prob);
ddata.setSunglassesProb(ds_res.sunglasses_prob);
ddata.setOccludedProb(ds_res.occluded_prob);
ddata.setReadyProb(ds_res.ready_prob);
ddata.setNotReadyProb(ds_res.not_ready_prob);
}
DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, float *calib) {
int v_off = height - MODEL_HEIGHT;
int h_off = (width - MODEL_WIDTH) / 2;
int yuv_buf_len = (MODEL_WIDTH/2) * (MODEL_HEIGHT/2) * 6; // Y|u|v, frame2tensor done in dsp
@ -72,61 +112,29 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
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] * 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.not_ready_prob[i] = sigmoid(s->output[43 + i]);
}
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;
DMonitoringModelResult model_res = {0};
parse_driver_data(model_res.driver_state_lhd, s, 0);
parse_driver_data(model_res.driver_state_rhd, s, 41);
model_res.poor_vision = sigmoid(s->output[82]);
model_res.wheel_on_right = sigmoid(s->output[83]);
model_res.dsp_execution_time = (t2 - t1) / 1000.;
return model_res;
}
void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time, kj::ArrayPtr<const float> raw_pred) {
void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr<const float> raw_pred) {
// make msg
MessageBuilder msg;
auto framed = msg.initEvent().initDriverState();
framed.setFrameId(frame_id);
framed.setModelExecutionTime(execution_time);
framed.setDspExecutionTime(res.dsp_execution_time);
framed.setFaceOrientation(res.face_orientation);
framed.setFaceOrientationStd(res.face_orientation_meta);
framed.setFacePosition(res.face_position);
framed.setFacePositionStd(res.face_position_meta);
framed.setFaceProb(res.face_prob);
framed.setLeftEyeProb(res.left_eye_prob);
framed.setRightEyeProb(res.right_eye_prob);
framed.setLeftBlinkProb(res.left_blink_prob);
framed.setRightBlinkProb(res.right_blink_prob);
framed.setSunglassesProb(res.sg_prob);
framed.setPoorVision(res.poor_vision);
framed.setPartialFace(res.partial_face);
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);
framed.setDspExecutionTime(model_res.dsp_execution_time);
framed.setPoorVision(model_res.poor_vision);
framed.setWheelOnRight(model_res.wheel_on_right);
fill_driver_data(framed.initDriverDataLH(), model_res.driver_state_lhd)
fill_driver_data(framed.initDriverDataRH(), model_res.driver_state_rhd)
if (send_raw_pred) {
framed.setRawPredictions(raw_pred.asBytes());
}

25
selfdrive/modeld/models/dmonitoring.h
Unescape View File

@ -12,26 +12,29 @@
#define OUTPUT_SIZE 84
#define REG_SCALE 0.25f
typedef struct DMonitoringResult {
typedef struct DriverStateResult {
float face_orientation[3];
float face_orientation_meta[3];
float face_orientation_std[3];
float face_position[2];
float face_position_meta[2];
float face_position_std[2];
float face_prob;
float left_eye_prob;
float right_eye_prob;
float left_blink_prob;
float right_blink_prob;
float sg_prob;
float poor_vision;
float partial_face;
float distracted_pose;
float distracted_eyes;
float sunglasses_prob;
float occluded_prob;
float ready_prob[4];
float not_ready_prob[2];
} DriverStateResult;
typedef struct DMonitoringModelResult {
DriverStateResult driver_state_lhd;
DriverStateResult driver_state_rhd;
float poor_vision;
float wheel_on_right;
float dsp_execution_time;
} DMonitoringResult;
} DMonitoringModelResult;
typedef struct DMonitoringModelState {
RunModel *m;
@ -43,7 +46,7 @@ typedef struct DMonitoringModelState {
} DMonitoringModelState;
void dmonitoring_init(DMonitoringModelState* s);
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);
DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, float *calib);
void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr<const float> raw_pred);
void dmonitoring_free(DMonitoringModelState* s);

Write Preview
Loading…
Cancel
Save