mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-08-21 08:14:00 +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/modeld/models/dmonitoring.cc
+160 -76
View File
@@ -10,8 +10,8 @@
#include "selfdrive/modeld/models/dmonitoring.h"
constexpr int MODEL_WIDTH = 1440;
constexpr int MODEL_HEIGHT = 960;
constexpr int MODEL_WIDTH = 320;
constexpr int MODEL_HEIGHT = 640;
template <class T>
static inline T *get_buffer(std::vector<T> &buf, const size_t size) {
@@ -19,115 +19,199 @@ static inline T *get_buffer(std::vector<T> &buf, const size_t size) {
return buf.data();
}
static inline void init_yuv_buf(std::vector<uint8_t> &buf, const int width, int height) {
uint8_t *y = get_buffer(buf, width * height * 3 / 2);
uint8_t *u = y + width * height;
uint8_t *v = u + (width / 2) * (height / 2);
// needed on comma two to make the padded border black
// equivalent to RGB(0,0,0) in YUV space
memset(y, 16, width * height);
memset(u, 128, (width / 2) * (height / 2));
memset(v, 128, (width / 2) * (height / 2));
}
void dmonitoring_init(DMonitoringModelState* s) {
s->is_rhd = Params().getBool("IsRHD");
for (int x = 0; x < std::size(s->tensor); ++x) {
s->tensor[x] = (x - 128.f) * 0.0078125f;
}
init_yuv_buf(s->resized_buf, MODEL_WIDTH, MODEL_HEIGHT);
#ifdef USE_ONNX_MODEL
s->m = new ONNXModel("models/dmonitoring_model.onnx", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME, false, true);
s->m = new ONNXModel("models/dmonitoring_model.onnx", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME);
#else
s->m = new SNPEModel("models/dmonitoring_model_q.dlc", &s->output[0], OUTPUT_SIZE, USE_DSP_RUNTIME, false, true);
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);
}
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]);
static inline auto get_yuv_buf(std::vector<uint8_t> &buf, const int width, int height) {
uint8_t *y = get_buffer(buf, width * height * 3 / 2);
uint8_t *u = y + width * height;
uint8_t *v = u + (width /2) * (height / 2);
return std::make_tuple(y, u, v);
}
void fill_driver_data(cereal::DriverStateV2::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);
struct Rect {int x, y, w, h;};
void crop_nv12_to_yuv(uint8_t *raw, int stride, int uv_offset, uint8_t *y, uint8_t *u, uint8_t *v, const Rect &rect) {
uint8_t *raw_y = raw;
uint8_t *raw_uv = raw_y + uv_offset;
for (int r = 0; r < rect.h / 2; r++) {
memcpy(y + 2 * r * rect.w, raw_y + (2 * r + rect.y) * stride + rect.x, rect.w);
memcpy(y + (2 * r + 1) * rect.w, raw_y + (2 * r + rect.y + 1) * stride + rect.x, rect.w);
for (int h = 0; h < rect.w / 2; h++) {
u[r * rect.w/2 + h] = raw_uv[(r + (rect.y/2)) * stride + (rect.x/2 + h)*2];
v[r * rect.w/2 + h] = raw_uv[(r + (rect.y/2)) * stride + (rect.x/2 + h)*2 + 1];
}
}
}
DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib) {
int v_off = height - MODEL_HEIGHT;
int h_off = (width - MODEL_WIDTH) / 2;
int yuv_buf_len = MODEL_WIDTH * MODEL_HEIGHT;
uint8_t *raw_buf = (uint8_t *) stream_buf;
// vertical crop free
uint8_t *raw_y_start = raw_buf + stride * v_off;
uint8_t *net_input_buf = get_buffer(s->net_input_buf, yuv_buf_len);
// here makes a uint8 copy
for (int r = 0; r < MODEL_HEIGHT; ++r) {
memcpy(net_input_buf + r * MODEL_WIDTH, raw_y_start + r * stride + h_off, MODEL_WIDTH);
DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib) {
const int cropped_height = tici_dm_crop::width / 1.33;
Rect crop_rect = {width / 2 - tici_dm_crop::width / 2 + tici_dm_crop::x_offset,
height / 2 - cropped_height / 2 + tici_dm_crop::y_offset,
cropped_height / 2,
cropped_height};
if (!s->is_rhd) {
crop_rect.x += tici_dm_crop::width - crop_rect.w;
}
// printf("preprocess completed. %d \n", yuv_buf_len);
// FILE *dump_yuv_file = fopen("/tmp/rawdump.yuv", "wb");
// fwrite(net_input_buf, yuv_buf_len, sizeof(uint8_t), dump_yuv_file);
// fclose(dump_yuv_file);
int resized_width = MODEL_WIDTH;
int resized_height = MODEL_HEIGHT;
auto [cropped_y, cropped_u, cropped_v] = get_yuv_buf(s->cropped_buf, crop_rect.w, crop_rect.h);
if (!s->is_rhd) {
crop_nv12_to_yuv((uint8_t *)stream_buf, stride, uv_offset, cropped_y, cropped_u, cropped_v, crop_rect);
} else {
auto [mirror_y, mirror_u, mirror_v] = get_yuv_buf(s->premirror_cropped_buf, crop_rect.w, crop_rect.h);
crop_nv12_to_yuv((uint8_t *)stream_buf, stride, uv_offset, mirror_y, mirror_u, mirror_v, crop_rect);
libyuv::I420Mirror(mirror_y, crop_rect.w,
mirror_u, crop_rect.w / 2,
mirror_v, crop_rect.w / 2,
cropped_y, crop_rect.w,
cropped_u, crop_rect.w / 2,
cropped_v, crop_rect.w / 2,
crop_rect.w, crop_rect.h);
}
auto [resized_buf, resized_u, resized_v] = get_yuv_buf(s->resized_buf, resized_width, resized_height);
uint8_t *resized_y = resized_buf;
libyuv::FilterMode mode = libyuv::FilterModeEnum::kFilterBilinear;
libyuv::I420Scale(cropped_y, crop_rect.w,
cropped_u, crop_rect.w / 2,
cropped_v, crop_rect.w / 2,
crop_rect.w, crop_rect.h,
resized_y, resized_width,
resized_u, resized_width / 2,
resized_v, resized_width / 2,
resized_width, resized_height,
mode);
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 r = 0; r < MODEL_HEIGHT/2; r++) {
for (int c = 0; c < MODEL_WIDTH/2; c++) {
// Y_ul
net_input_buf[(r*MODEL_WIDTH/2) + c + (0*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_y[(2*r)*resized_width + 2*c]];
// Y_dl
net_input_buf[(r*MODEL_WIDTH/2) + c + (1*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_y[(2*r+1)*resized_width + 2*c]];
// Y_ur
net_input_buf[(r*MODEL_WIDTH/2) + c + (2*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_y[(2*r)*resized_width + 2*c+1]];
// Y_dr
net_input_buf[(r*MODEL_WIDTH/2) + c + (3*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_y[(2*r+1)*resized_width + 2*c+1]];
// U
net_input_buf[(r*MODEL_WIDTH/2) + c + (4*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_u[r*resized_width/2 + c]];
// V
net_input_buf[(r*MODEL_WIDTH/2) + c + (5*(MODEL_WIDTH/2)*(MODEL_HEIGHT/2))] = s->tensor[resized_v[r*resized_width/2 + c]];
}
}
//printf("preprocess completed. %d \n", yuv_buf_len);
//FILE *dump_yuv_file = fopen("/tmp/rawdump.yuv", "wb");
//fwrite(resized_buf, yuv_buf_len, sizeof(uint8_t), dump_yuv_file);
//fclose(dump_yuv_file);
// *** testing ***
// idat = np.frombuffer(open("/tmp/inputdump.yuv", "rb").read(), np.float32).reshape(6, 160, 320)
// imshow(cv2.cvtColor(tensor_to_frames(idat[None]/0.0078125+128)[0], cv2.COLOR_YUV2RGB_I420))
//FILE *dump_yuv_file2 = fopen("/tmp/inputdump.yuv", "wb");
//fwrite(net_input_buf, MODEL_HEIGHT*MODEL_WIDTH*3/2, sizeof(float), dump_yuv_file2);
//fclose(dump_yuv_file2);
double t1 = millis_since_boot();
s->m->addImage((float*)net_input_buf, yuv_buf_len / 4);
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();
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_prob = sigmoid(s->output[82]);
model_res.wheel_on_right_prob = sigmoid(s->output[83]);
model_res.dsp_execution_time = (t2 - t1) / 1000.;
return model_res;
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;
}
void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr<const float> raw_pred) {
void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringResult &res, float execution_time, kj::ArrayPtr<const float> raw_pred) {
// make msg
MessageBuilder msg;
auto framed = msg.initEvent().initDriverStateV2();
auto framed = msg.initEvent().initDriverState();
framed.setFrameId(frame_id);
framed.setModelExecutionTime(execution_time);
framed.setDspExecutionTime(model_res.dsp_execution_time);
framed.setPoorVisionProb(model_res.poor_vision_prob);
framed.setWheelOnRightProb(model_res.wheel_on_right_prob);
fill_driver_data(framed.initLeftDriverData(), model_res.driver_state_lhd);
fill_driver_data(framed.initRightDriverData(), model_res.driver_state_rhd);
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);
if (send_raw_pred) {
framed.setRawPredictions(raw_pred.asBytes());
}
pm.send("driverStateV2", msg);
pm.send("driverState", msg);
}
void dmonitoring_free(DMonitoringModelState* s) {
selfdrive/modeld/models/dmonitoring.h
+17 -16
View File
@@ -9,43 +9,44 @@
#define CALIB_LEN 3
#define OUTPUT_SIZE 84
#define OUTPUT_SIZE 45
#define REG_SCALE 0.25f
typedef struct DriverStateResult {
typedef struct DMonitoringResult {
float face_orientation[3];
float face_orientation_std[3];
float face_orientation_meta[3];
float face_position[2];
float face_position_std[2];
float face_position_meta[2];
float face_prob;
float left_eye_prob;
float right_eye_prob;
float left_blink_prob;
float right_blink_prob;
float sunglasses_prob;
float sg_prob;
float poor_vision;
float partial_face;
float distracted_pose;
float distracted_eyes;
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_prob;
float wheel_on_right_prob;
float dsp_execution_time;
} DMonitoringModelResult;
} DMonitoringResult;
typedef struct DMonitoringModelState {
RunModel *m;
bool is_rhd;
float output[OUTPUT_SIZE];
std::vector<uint8_t> net_input_buf;
std::vector<uint8_t> resized_buf;
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);
DMonitoringModelResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, float *calib);
void dmonitoring_publish(PubMaster &pm, uint32_t frame_id, const DMonitoringModelResult &model_res, float execution_time, kj::ArrayPtr<const float> raw_pred);
DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_buf, int width, int height, int stride, int uv_offset, 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_model.current
+2 -2
View File
@@ -1,2 +1,2 @@
5b02cff5-2b29-431d-b186-372e9c6fd0c7
bf33cc569076984626ac7e027f927aa593261fa7
a8236e30-5bee-4689-8ea0-fc102e2770e5
d508c79bae1c1c451f3af3e2bc231ce33678cb43
selfdrive/modeld/models/dmonitoring_model.onnx
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:aca1dd411b5f488bea605dc360656e631fc4301968a589ea072e2220c8092600
size 9157561
oid sha256:00731ebd06fcff7e5837607b91bc56cad3bed5d7ee89052c911c981e8f665308
size 3679940
selfdrive/modeld/models/dmonitoring_model_q.dlc
+2 -2
View File
@@ -1,3 +1,3 @@
version https://git-lfs.github.com/spec/v1
oid sha256:d146b1d1fd9d40d57d058e51d285f83676866e26d9e5aff9fa27623ce343b58a
size 2636941
oid sha256:667df5e925570a0f6a33dfb890e186a1f13f101885b46db47ec45305737dffb6
size 1145921