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

Test model refactor (#2720)

Browse Source 
* template funcion fill_meta

* add function get_plan_max_idx

* add function get_mdn_max_idx

* remove temp builder variables

* get_mdn_max_idx->get_lead_data

* get_pan_max_idx->get_plan_data

* fill_lane_line

* combine fill_lane_line&fill_path into one function

* fill_lead_vw

* using unique_ptr

* prefer using constexpr

* more constexpr,remove duplicate defined TRAJECTORY_SIZE

* remove suffix _arr from variable name

* misc

* remove extern c

* refactor model_publish

* remove unused paramaters

* traffic_convention to c style array

* c style array:prev_desire&pulse_desire

* fix error&make easy for review

* const mat3 &transform

* move cl_command_queue into ModelState

* use maco LEAD_MHP_SELECTION

* move constexpr from .h to .cc

* remove #define MODEL_NAME

* modeldata.h: contexpr

* remove param temporal from model_init

* helper function get_best_data

* fix probs

* int

Co-authored-by: deanlee <deanlee3@gmail.com>
pull/2702/head
HaraldSchafer 5 years ago committed by GitHub
parent bbe868d312
commit 630c7309a5
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 160 additions and 278 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 16
      selfdrive/common/modeldata.h
  2. 16
      selfdrive/modeld/modeld.cc
  3. 2
      selfdrive/modeld/models/commonmodel.cc
  4. 17
      selfdrive/modeld/models/commonmodel.h
  5. 320
      selfdrive/modeld/models/driving.cc
  6. 67
      selfdrive/modeld/models/driving.h

16
selfdrive/common/modeldata.h
Unescape View File

@ -1,10 +1,10 @@
#pragma once #pragma once
#define MODEL_PATH_DISTANCE 192 constexpr int MODEL_PATH_DISTANCE = 192;
#define TRAJECTORY_SIZE 33 constexpr int TRAJECTORY_SIZE = 33;
#define MIN_DRAW_DISTANCE 10.0 constexpr float MIN_DRAW_DISTANCE = 10.0;
#define MAX_DRAW_DISTANCE 100.0 constexpr float MAX_DRAW_DISTANCE = 100.0;
#define POLYFIT_DEGREE 4 constexpr int POLYFIT_DEGREE = 4;
#define SPEED_PERCENTILES 10 constexpr int SPEED_PERCENTILES = 10;
#define DESIRE_PRED_SIZE 32 constexpr int DESIRE_PRED_SIZE = 32;
#define OTHER_META_SIZE 4 constexpr int OTHER_META_SIZE = 4;

16
selfdrive/modeld/modeld.cc
Unescape View File

@ -117,11 +117,10 @@ int main(int argc, char **argv) {
// cl init // cl init
cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT); cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err)); cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
cl_command_queue q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
// init the models // init the models
ModelState model; ModelState model;
model_init(&model, device_id, context, true); model_init(&model, device_id, context);
LOGW("models loaded, modeld starting"); LOGW("models loaded, modeld starting");
// loop // loop
@ -185,8 +184,8 @@ int main(int argc, char **argv) {
visionbuf_sync(&yuv_ion, VISIONBUF_SYNC_TO_DEVICE); visionbuf_sync(&yuv_ion, VISIONBUF_SYNC_TO_DEVICE);
ModelDataRaw model_buf = ModelDataRaw model_buf =
model_eval_frame(&model, q, yuv_ion.buf_cl, buf_info.width, buf_info.height, model_eval_frame(&model, yuv_ion.buf_cl, buf_info.width, buf_info.height,
model_transform, NULL, vec_desire); model_transform, vec_desire);
mt2 = millis_since_boot(); mt2 = millis_since_boot();
float model_execution_time = (mt2 - mt1) / 1000.0; float model_execution_time = (mt2 - mt1) / 1000.0;
@ -196,10 +195,9 @@ int main(int argc, char **argv) {
if (run_count < 10) frames_dropped = 0; // let frame drops warm up if (run_count < 10) frames_dropped = 0; // let frame drops warm up
float frame_drop_ratio = frames_dropped / (1 + frames_dropped); float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
const float* raw_pred_ptr = send_raw_pred ? (const float *)model.output : nullptr; const float *raw_pred_ptr = send_raw_pred ? &model.output[0] : nullptr;
model_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, raw_pred_ptr, extra.timestamp_eof, model_execution_time); model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, model_buf, raw_pred_ptr, extra.timestamp_eof, model_execution_time);
model_publish_v2(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, raw_pred_ptr, extra.timestamp_eof, model_execution_time); posenet_publish(pm, extra.frame_id, vipc_dropped_frames, model_buf, extra.timestamp_eof);
posenet_publish(pm, extra.frame_id, frame_id, vipc_dropped_frames, frame_drop_ratio, model_buf, extra.timestamp_eof);
LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_ratio); LOGD("model process: %.2fms, from last %.2fms, vipc_frame_id %zu, frame_id, %zu, frame_drop %.3f", mt2-mt1, mt1-last, extra.frame_id, frame_id, frame_drop_ratio);
last = mt1; last = mt1;
@ -217,8 +215,6 @@ int main(int argc, char **argv) {
err = pthread_join(live_thread_handle, NULL); err = pthread_join(live_thread_handle, NULL);
assert(err == 0); assert(err == 0);
CL_CHECK(clReleaseContext(context)); CL_CHECK(clReleaseContext(context));
CL_CHECK(clReleaseCommandQueue(q));
pthread_mutex_destroy(&transform_lock); pthread_mutex_destroy(&transform_lock);
return 0; return 0;
} }

2
selfdrive/modeld/models/commonmodel.cc
Unescape View File

@ -25,7 +25,7 @@ void frame_init(ModelFrame* frame, int width, int height,
float *frame_prepare(ModelFrame* frame, cl_command_queue q, float *frame_prepare(ModelFrame* frame, cl_command_queue q,
cl_mem yuv_cl, int width, int height, cl_mem yuv_cl, int width, int height,
mat3 transform) { const mat3 &transform) {
transform_queue(&frame->transform, q, transform_queue(&frame->transform, q,
yuv_cl, width, height, yuv_cl, width, height,
frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl, frame->transformed_y_cl, frame->transformed_u_cl, frame->transformed_v_cl,

17
selfdrive/modeld/models/commonmodel.h
Unescape View File

@ -1,6 +1,4 @@
#ifndef COMMONMODEL_H #pragma once
#define COMMONMODEL_H
#define CL_USE_DEPRECATED_OPENCL_1_2_APIS #define CL_USE_DEPRECATED_OPENCL_1_2_APIS
#ifdef __APPLE__ #ifdef __APPLE__
#include <OpenCL/cl.h> #include <OpenCL/cl.h>
@ -14,10 +12,6 @@
#include "transforms/transform.h" #include "transforms/transform.h"
#include "transforms/loadyuv.h" #include "transforms/loadyuv.h"
#ifdef __cplusplus
extern "C" {
#endif
const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL; const bool send_raw_pred = getenv("SEND_RAW_PRED") != NULL;
void softmax(const float* input, float* output, size_t len); void softmax(const float* input, float* output, size_t len);
@ -37,12 +31,5 @@ void frame_init(ModelFrame* frame, int width, int height,
cl_device_id device_id, cl_context context); cl_device_id device_id, cl_context context);
float *frame_prepare(ModelFrame* frame, cl_command_queue q, float *frame_prepare(ModelFrame* frame, cl_command_queue q,
cl_mem yuv_cl, int width, int height, cl_mem yuv_cl, int width, int height,
mat3 transform); const mat3 &transform);
void frame_free(ModelFrame* frame); void frame_free(ModelFrame* frame);
#ifdef __cplusplus
}
#endif
#endif

320
selfdrive/modeld/models/driving.cc
Unescape View File

@ -8,25 +8,42 @@
#include "common/timing.h" #include "common/timing.h"
#include "common/params.h" #include "common/params.h"
#include "driving.h" #include "driving.h"
#include "clutil.h"
#define MIN_VALID_LEN 10.0
#define TRAJECTORY_SIZE 33 constexpr int MODEL_WIDTH = 512;
#define TRAJECTORY_TIME 10.0 constexpr int MODEL_HEIGHT = 256;
#define TRAJECTORY_DISTANCE 192.0 constexpr int MODEL_FRAME_SIZE = MODEL_WIDTH * MODEL_HEIGHT * 3 / 2;
#define PLAN_IDX 0
#define LL_IDX PLAN_IDX + PLAN_MHP_N*(PLAN_MHP_GROUP_SIZE) constexpr int PLAN_MHP_N = 5;
#define LL_PROB_IDX LL_IDX + 4*2*2*33 constexpr int PLAN_MHP_COLUMNS = 30;
#define RE_IDX LL_PROB_IDX + 4 constexpr int PLAN_MHP_VALS = 30*33;
#define LEAD_IDX RE_IDX + 2*2*2*33 constexpr int PLAN_MHP_SELECTION = 1;
#define LEAD_PROB_IDX LEAD_IDX + LEAD_MHP_N*(LEAD_MHP_GROUP_SIZE) constexpr int PLAN_MHP_GROUP_SIZE = (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION);
#define DESIRE_STATE_IDX LEAD_PROB_IDX + 3
#define META_IDX DESIRE_STATE_IDX + DESIRE_LEN constexpr int LEAD_MHP_N = 5;
#define POSE_IDX META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE constexpr int LEAD_MHP_VALS = 4;
#define OUTPUT_SIZE POSE_IDX + POSE_SIZE constexpr int LEAD_MHP_SELECTION = 3;
constexpr int LEAD_MHP_GROUP_SIZE = (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION);
constexpr int POSE_SIZE = 12;
constexpr int MIN_VALID_LEN = 10;
constexpr int TRAJECTORY_TIME = 10;
constexpr float TRAJECTORY_DISTANCE = 192.0;
constexpr int PLAN_IDX = 0;
constexpr int LL_IDX = PLAN_IDX + PLAN_MHP_N*PLAN_MHP_GROUP_SIZE;
constexpr int LL_PROB_IDX = LL_IDX + 4*2*2*33;
constexpr int RE_IDX = LL_PROB_IDX + 4;
constexpr int LEAD_IDX = RE_IDX + 2*2*2*33;
constexpr int LEAD_PROB_IDX = LEAD_IDX + LEAD_MHP_N*(LEAD_MHP_GROUP_SIZE);
constexpr int DESIRE_STATE_IDX = LEAD_PROB_IDX + 3;
constexpr int META_IDX = DESIRE_STATE_IDX + DESIRE_LEN;
constexpr int POSE_IDX = META_IDX + OTHER_META_SIZE + DESIRE_PRED_SIZE;
constexpr int OUTPUT_SIZE = POSE_IDX + POSE_SIZE;
#ifdef TEMPORAL #ifdef TEMPORAL
#define TEMPORAL_SIZE 512 constexpr int TEMPORAL_SIZE = 512;
#else #else
#define TEMPORAL_SIZE 0 constexpr int TEMPORAL_SIZE = 0;
#endif #endif
// #define DUMP_YUV // #define DUMP_YUV
@ -35,36 +52,26 @@ Eigen::Matrix<float, MODEL_PATH_DISTANCE, POLYFIT_DEGREE - 1> vander;
float X_IDXS[TRAJECTORY_SIZE]; float X_IDXS[TRAJECTORY_SIZE];
float T_IDXS[TRAJECTORY_SIZE]; float T_IDXS[TRAJECTORY_SIZE];
void model_init(ModelState* s, cl_device_id device_id, cl_context context, int temporal) { void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context); frame_init(&s->frame, MODEL_WIDTH, MODEL_HEIGHT, device_id, context);
s->input_frames = (float*)calloc(MODEL_FRAME_SIZE * 2, sizeof(float)); s->input_frames = std::make_unique<float[]>(MODEL_FRAME_SIZE * 2);
const int output_size = OUTPUT_SIZE + TEMPORAL_SIZE;
s->output = (float*)calloc(output_size, sizeof(float));
s->m = new DefaultRunModel("../../models/supercombo.dlc", s->output, output_size, USE_GPU_RUNTIME); constexpr int output_size = OUTPUT_SIZE + TEMPORAL_SIZE;
s->output = std::make_unique<float[]>(output_size);
s->m = std::make_unique<DefaultRunModel>("../../models/supercombo.dlc", &s->output[0], output_size, USE_GPU_RUNTIME);
#ifdef TEMPORAL #ifdef TEMPORAL
assert(temporal);
s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE); s->m->addRecurrent(&s->output[OUTPUT_SIZE], TEMPORAL_SIZE);
#endif #endif
#ifdef DESIRE #ifdef DESIRE
s->prev_desire = std::make_unique<float[]>(DESIRE_LEN); s->m->addDesire(s->pulse_desire, DESIRE_LEN);
s->pulse_desire = std::make_unique<float[]>(DESIRE_LEN);
s->m->addDesire(s->pulse_desire.get(), DESIRE_LEN);
#endif #endif
#ifdef TRAFFIC_CONVENTION #ifdef TRAFFIC_CONVENTION
s->traffic_convention = std::make_unique<float[]>(TRAFFIC_CONVENTION_LEN); const int idx = Params().read_db_bool("IsRHD") ? 1 : 0;
s->m->addTrafficConvention(s->traffic_convention.get(), TRAFFIC_CONVENTION_LEN); s->traffic_convention[idx] = 1.0;
s->m->addTrafficConvention(s->traffic_convention, TRAFFIC_CONVENTION_LEN);
bool is_rhd = Params().read_db_bool("IsRHD");
if (is_rhd) {
s->traffic_convention[1] = 1.0;
} else {
s->traffic_convention[0] = 1.0;
}
#endif #endif
// Build Vandermonde matrix // Build Vandermonde matrix
@ -75,12 +82,12 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context, int t
vander(i, j) = pow(X_IDXS[i], POLYFIT_DEGREE-j-1); vander(i, j) = pow(X_IDXS[i], POLYFIT_DEGREE-j-1);
} }
} }
s->q = CL_CHECK_ERR(clCreateCommandQueue(context, device_id, 0, &err));
} }
ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int height,
cl_mem yuv_cl, int width, int height, const mat3 &transform, float *desire_in) {
mat3 transform, void* sock,
float *desire_in) {
#ifdef DESIRE #ifdef DESIRE
if (desire_in != NULL) { if (desire_in != NULL) {
for (int i = 1; i < DESIRE_LEN; i++) { for (int i = 1; i < DESIRE_LEN; i++) {
@ -98,10 +105,10 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
//for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n"); //for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n");
float *new_frame_buf = frame_prepare(&s->frame, q, yuv_cl, width, height, transform); float *new_frame_buf = frame_prepare(&s->frame, s->q, yuv_cl, width, height, transform);
memmove(&s->input_frames[0], &s->input_frames[MODEL_FRAME_SIZE], sizeof(float)*MODEL_FRAME_SIZE); memmove(&s->input_frames[0], &s->input_frames[MODEL_FRAME_SIZE], sizeof(float)*MODEL_FRAME_SIZE);
memmove(&s->input_frames[MODEL_FRAME_SIZE], new_frame_buf, sizeof(float)*MODEL_FRAME_SIZE); memmove(&s->input_frames[MODEL_FRAME_SIZE], new_frame_buf, sizeof(float)*MODEL_FRAME_SIZE);
s->m->execute(s->input_frames, MODEL_FRAME_SIZE*2); s->m->execute(&s->input_frames[0], MODEL_FRAME_SIZE*2);
#ifdef DUMP_YUV #ifdef DUMP_YUV
FILE *dump_yuv_file = fopen("/sdcard/dump.yuv", "wb"); FILE *dump_yuv_file = fopen("/sdcard/dump.yuv", "wb");
@ -110,7 +117,7 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
assert(1==2); assert(1==2);
#endif #endif
clEnqueueUnmapMemObject(q, s->frame.net_input, (void*)new_frame_buf, 0, NULL, NULL); clEnqueueUnmapMemObject(s->q, s->frame.net_input, (void*)new_frame_buf, 0, NULL, NULL);
// net outputs // net outputs
ModelDataRaw net_outputs; ModelDataRaw net_outputs;
@ -126,10 +133,8 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q,
} }
void model_free(ModelState* s) { void model_free(ModelState* s) {
free(s->output);
free(s->input_frames);
frame_free(&s->frame); frame_free(&s->frame);
delete s->m; CL_CHECK(clReleaseCommandQueue(s->q));
} }
void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) { void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) {
@ -157,48 +162,53 @@ void poly_fit(float *in_pts, float *in_stds, float *out, int valid_len) {
out[3] = y0; out[3] = y0;
} }
void fill_path(cereal::ModelData::PathData::Builder path, const float * data, float valid_len, int valid_len_idx) { static const float *get_best_data(const float *data, int size, int group_size, int offset) {
float points_arr[TRAJECTORY_SIZE]; int max_idx = 0;
float stds_arr[TRAJECTORY_SIZE]; for (int i = 1; i < size; i++) {
float poly_arr[POLYFIT_DEGREE]; if (data[(i + 1) * group_size + offset] >
float std; data[(max_idx + 1) * group_size + offset]) {
max_idx = i;
for (int i=0; i<TRAJECTORY_SIZE; i++) { }
// negative sign because mpc has left positive
points_arr[i] = -data[30*i + 16];
stds_arr[i] = exp(data[30*(33 + i) + 16]);
} }
std = stds_arr[0]; return &data[max_idx * group_size];
poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx); }
path.setPoly(poly_arr); static const float *get_plan_data(float *plan) {
path.setProb(1.0); return get_best_data(plan, PLAN_MHP_N, PLAN_MHP_GROUP_SIZE, -1);
path.setStd(std);
path.setValidLen(valid_len);
} }
void fill_lane_line(cereal::ModelData::PathData::Builder path, const float * data, int ll_idx, float valid_len, int valid_len_idx, float prob) { static const float *get_lead_data(const float *lead, int t_offset) {
float points_arr[TRAJECTORY_SIZE]; return get_best_data(lead, LEAD_MHP_N, LEAD_MHP_GROUP_SIZE, t_offset - LEAD_MHP_SELECTION);
float stds_arr[TRAJECTORY_SIZE]; }
float poly_arr[POLYFIT_DEGREE];
float std; void fill_path(cereal::ModelData::PathData::Builder path, const float *data, const float prob,
float valid_len, int valid_len_idx, int ll_idx) {
float points[TRAJECTORY_SIZE] = {};
float stds[TRAJECTORY_SIZE] = {};
float poly[POLYFIT_DEGREE] = {};
for (int i=0; i<TRAJECTORY_SIZE; i++) { for (int i=0; i<TRAJECTORY_SIZE; i++) {
// negative sign because mpc has left positive // negative sign because mpc has left positive
points_arr[i] = -data[2*33*ll_idx + 2*i]; if (ll_idx == 0) {
stds_arr[i] = exp(data[2*33*(4 + ll_idx) + 2*i]); points[i] = -data[30 * i + 16];
stds[i] = exp(data[30 * (33 + i) + 16]);
} else {
points[i] = -data[2 * 33 * ll_idx + 2 * i];
stds[i] = exp(data[2 * 33 * (4 + ll_idx) + 2 * i]);
}
} }
std = stds_arr[0]; const float std = stds[0];
poly_fit(points_arr, stds_arr, poly_arr, valid_len_idx); poly_fit(points, stds, poly, valid_len_idx);
path.setPoly(poly_arr); path.setPoly(poly);
path.setProb(prob); path.setProb(prob);
path.setStd(std); path.setStd(std);
path.setValidLen(valid_len); path.setValidLen(valid_len);
} }
void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float * data, float prob, float t) { void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float *lead_data, const float *prob, int t_offset, float t) {
lead.setProb(prob); const float *data = get_lead_data(lead_data, t_offset);
lead.setProb(sigmoid(prob[t_offset]));
lead.setT(t); lead.setT(t);
float xyva_arr[LEAD_MHP_VALS]; float xyva_arr[LEAD_MHP_VALS];
float xyva_stds_arr[LEAD_MHP_VALS]; float xyva_stds_arr[LEAD_MHP_VALS];
@ -210,8 +220,9 @@ void fill_lead_v2(cereal::ModelDataV2::LeadDataV2::Builder lead, const float * d
lead.setXyvaStd(xyva_stds_arr); lead.setXyvaStd(xyva_stds_arr);
} }
void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, float prob) { void fill_lead(cereal::ModelData::LeadData::Builder lead, const float *lead_data, const float *prob, int t_offset) {
lead.setProb(prob); const float *data = get_lead_data(lead_data, t_offset);
lead.setProb(sigmoid(prob[t_offset]));
lead.setDist(data[0]); lead.setDist(data[0]);
lead.setStd(exp(data[LEAD_MHP_VALS])); lead.setStd(exp(data[LEAD_MHP_VALS]));
// TODO make all msgs same format // TODO make all msgs same format
@ -223,7 +234,8 @@ void fill_lead(cereal::ModelData::LeadData::Builder lead, const float * data, fl
lead.setRelAStd(exp(data[LEAD_MHP_VALS + 3])); lead.setRelAStd(exp(data[LEAD_MHP_VALS + 3]));
} }
void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_data) { template <class MetaBuilder>
void fill_meta(MetaBuilder meta, const float *meta_data) {
float desire_state_softmax[DESIRE_LEN]; float desire_state_softmax[DESIRE_LEN];
float desire_pred_softmax[4*DESIRE_LEN]; float desire_pred_softmax[4*DESIRE_LEN];
softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN); softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN);
@ -239,22 +251,6 @@ void fill_meta(cereal::ModelData::MetaData::Builder meta, const float * meta_dat
meta.setDesirePrediction(desire_pred_softmax); meta.setDesirePrediction(desire_pred_softmax);
} }
void fill_meta_v2(cereal::ModelDataV2::MetaData::Builder meta, const float * meta_data) {
float desire_state_softmax[DESIRE_LEN] = {};
float desire_pred_softmax[4*DESIRE_LEN] = {};
softmax(&meta_data[0], desire_state_softmax, DESIRE_LEN);
for (int i=0; i<4; i++) {
softmax(&meta_data[DESIRE_LEN + OTHER_META_SIZE + i*DESIRE_LEN],
&desire_pred_softmax[i*DESIRE_LEN], DESIRE_LEN);
}
meta.setDesireState(desire_state_softmax);
meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN]));
meta.setGasDisengageProb(sigmoid(meta_data[DESIRE_LEN + 1]));
meta.setBrakeDisengageProb(sigmoid(meta_data[DESIRE_LEN + 2]));
meta.setSteerOverrideProb(sigmoid(meta_data[DESIRE_LEN + 3]));
meta.setDesirePrediction(desire_pred_softmax);
}
void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data, void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data,
int columns, int column_offset, float * plan_t_arr) { int columns, int column_offset, float * plan_t_arr) {
float x_arr[TRAJECTORY_SIZE] = {}; float x_arr[TRAJECTORY_SIZE] = {};
@ -292,34 +288,9 @@ void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float * data,
xyzt.setT(t_arr); xyzt.setT(t_arr);
} }
void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_outputs) {
void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
uint32_t vipc_dropped_frames, float frame_drop,
const ModelDataRaw &net_outputs, const float* raw_pred, uint64_t timestamp_eof,
float model_execution_time) {
// make msg
MessageBuilder msg;
auto framed = msg.initEvent().initModelV2();
uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
framed.setFrameId(vipc_frame_id);
framed.setFrameAge(frame_age);
framed.setFrameDropPerc(frame_drop * 100);
framed.setTimestampEof(timestamp_eof);
framed.setModelExecutionTime(model_execution_time);
if (send_raw_pred) {
framed.setRawPred(kj::arrayPtr((const uint8_t*)raw_pred, (OUTPUT_SIZE+TEMPORAL_SIZE)*sizeof(float)));
}
// plan // plan
int plan_mhp_max_idx = 0; const float *best_plan = get_plan_data(net_outputs.plan);
for (int i=1; i<PLAN_MHP_N; i++) {
if (net_outputs.plan[(i + 1)*(PLAN_MHP_GROUP_SIZE) - 1] >
net_outputs.plan[(plan_mhp_max_idx + 1)*(PLAN_MHP_GROUP_SIZE) - 1]) {
plan_mhp_max_idx = i;
}
}
float * best_plan = &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)];
float plan_t_arr[TRAJECTORY_SIZE]; float plan_t_arr[TRAJECTORY_SIZE];
for (int i=0; i<TRAJECTORY_SIZE; i++) { for (int i=0; i<TRAJECTORY_SIZE; i++) {
plan_t_arr[i] = best_plan[i*PLAN_MHP_COLUMNS + 15]; plan_t_arr[i] = best_plan[i*PLAN_MHP_COLUMNS + 15];
@ -352,59 +323,24 @@ void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
framed.setRoadEdgeStds(road_edge_stds_arr); framed.setRoadEdgeStds(road_edge_stds_arr);
// meta // meta
auto meta = framed.initMeta(); fill_meta(framed.initMeta(), net_outputs.meta);
fill_meta_v2(meta, net_outputs.meta);
// leads // leads
auto leads = framed.initLeads(LEAD_MHP_SELECTION); auto leads = framed.initLeads(LEAD_MHP_SELECTION);
float t_offsets[LEAD_MHP_SELECTION] = {0.0, 2.0, 4.0}; float t_offsets[LEAD_MHP_SELECTION] = {0.0, 2.0, 4.0};
for (int t_offset=0; t_offset<LEAD_MHP_SELECTION; t_offset++) { for (int t_offset=0; t_offset<LEAD_MHP_SELECTION; t_offset++) {
int mdn_max_idx = 0; fill_lead_v2(leads[t_offset], net_outputs.lead, net_outputs.lead_prob, t_offset, t_offsets[t_offset]);
for (int i=1; i<LEAD_MHP_N; i++) {
if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - LEAD_MHP_SELECTION] >
net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - LEAD_MHP_SELECTION]) {
mdn_max_idx = i;
}
}
fill_lead_v2(leads[t_offset], &net_outputs.lead[mdn_max_idx * (LEAD_MHP_GROUP_SIZE)],
sigmoid(net_outputs.lead_prob[t_offset]), t_offsets[t_offset]);
} }
pm.send("modelV2", msg);
} }
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, void fill_model(cereal::ModelData::Builder &framed, const ModelDataRaw &net_outputs) {
uint32_t vipc_dropped_frames, float frame_drop,
const ModelDataRaw &net_outputs, const float* raw_pred, uint64_t timestamp_eof,
float model_execution_time) {
uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
MessageBuilder msg;
auto framed = msg.initEvent().initModel();
framed.setFrameId(vipc_frame_id);
framed.setFrameAge(frame_age);
framed.setFrameDropPerc(frame_drop * 100);
framed.setTimestampEof(timestamp_eof);
framed.setModelExecutionTime(model_execution_time);
if (send_raw_pred) {
framed.setRawPred(kj::arrayPtr((const uint8_t*)raw_pred, (OUTPUT_SIZE+TEMPORAL_SIZE)*sizeof(float)));
}
// Find the distribution that corresponds to the most probable plan // Find the distribution that corresponds to the most probable plan
int plan_mhp_max_idx = 0; const float *best_plan = get_plan_data(net_outputs.plan);
for (int i=1; i<PLAN_MHP_N; i++) {
if (net_outputs.plan[(i + 1)*(PLAN_MHP_GROUP_SIZE) - 1] >
net_outputs.plan[(plan_mhp_max_idx + 1)*(PLAN_MHP_GROUP_SIZE) - 1]) {
plan_mhp_max_idx = i;
}
}
// x pos at 10s is a good valid_len // x pos at 10s is a good valid_len
float valid_len = 0; float valid_len = 0;
float valid_len_candidate;
for (int i=1; i<TRAJECTORY_SIZE; i++) { for (int i=1; i<TRAJECTORY_SIZE; i++) {
valid_len_candidate = net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE) + 30*i]; if (const float len = best_plan[30*i]; len >= valid_len){
if (valid_len_candidate >= valid_len){ valid_len = len;
valid_len = valid_len_candidate;
} }
} }
// clamp to 10 and MODEL_PATH_DISTANCE // clamp to 10 and MODEL_PATH_DISTANCE
@ -415,47 +351,39 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
valid_len_idx = i; valid_len_idx = i;
} }
} }
fill_path(framed.initPath(), best_plan, 1.0, valid_len, valid_len_idx, 0);
fill_path(framed.initLeftLane(), net_outputs.lane_lines, sigmoid(net_outputs.lane_lines_prob[1]), valid_len, valid_len_idx, 1);
fill_path(framed.initRightLane(), net_outputs.lane_lines, sigmoid(net_outputs.lane_lines_prob[2]), valid_len, valid_len_idx, 2);
auto lpath = framed.initPath(); fill_lead(framed.initLead(), net_outputs.lead, net_outputs.lead_prob, 0);
fill_path(lpath, &net_outputs.plan[plan_mhp_max_idx*(PLAN_MHP_GROUP_SIZE)], valid_len, valid_len_idx); fill_lead(framed.initLeadFuture(), net_outputs.lead, net_outputs.lead_prob, 1);
auto left_lane = framed.initLeftLane();
int ll_idx = 1;
fill_lane_line(left_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx,
sigmoid(net_outputs.lane_lines_prob[ll_idx]));
auto right_lane = framed.initRightLane();
ll_idx = 2;
fill_lane_line(right_lane, net_outputs.lane_lines, ll_idx, valid_len, valid_len_idx,
sigmoid(net_outputs.lane_lines_prob[ll_idx]));
// Find the distribution that corresponds to the current lead
int mdn_max_idx = 0;
int t_offset = 0;
for (int i=1; i<LEAD_MHP_N; i++) {
if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3] >
net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3]) {
mdn_max_idx = i;
}
}
fill_lead(framed.initLead(), &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], sigmoid(net_outputs.lead_prob[t_offset]));
// Find the distribution that corresponds to the lead in 2s
mdn_max_idx = 0;
t_offset = 1;
for (int i=1; i<LEAD_MHP_N; i++) {
if (net_outputs.lead[(i+1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3] >
net_outputs.lead[(mdn_max_idx + 1)*(LEAD_MHP_GROUP_SIZE) + t_offset - 3]) {
mdn_max_idx = i;
}
}
fill_lead(framed.initLeadFuture(), &net_outputs.lead[mdn_max_idx*(LEAD_MHP_GROUP_SIZE)], sigmoid(net_outputs.lead_prob[t_offset]));
fill_meta(framed.initMeta(), net_outputs.meta); fill_meta(framed.initMeta(), net_outputs.meta);
}
pm.send("model", msg); void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
const ModelDataRaw &net_outputs, const float *raw_pred, uint64_t timestamp_eof,
float model_execution_time) {
const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
auto do_publish = [&](auto init_model_func, const char *pub_name) {
MessageBuilder msg;
auto framed = (msg.initEvent().*(init_model_func))();
framed.setFrameId(vipc_frame_id);
framed.setFrameAge(frame_age);
framed.setFrameDropPerc(frame_drop * 100);
framed.setTimestampEof(timestamp_eof);
framed.setModelExecutionTime(model_execution_time);
if (send_raw_pred) {
framed.setRawPred(kj::arrayPtr((const uint8_t *)raw_pred, (OUTPUT_SIZE + TEMPORAL_SIZE) * sizeof(float)));
}
fill_model(framed, net_outputs);
pm.send(pub_name, msg);
};
do_publish(&cereal::Event::Builder::initModel, "model");
do_publish(&cereal::Event::Builder::initModelV2, "modelV2");
} }
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
uint32_t vipc_dropped_frames, float frame_drop,
const ModelDataRaw &net_outputs, uint64_t timestamp_eof) { const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {
float trans_arr[3]; float trans_arr[3];
float trans_std_arr[3]; float trans_std_arr[3];

67
selfdrive/modeld/models/driving.h
Unescape View File

@ -1,12 +1,10 @@
#ifndef MODEL_H #pragma once
#define MODEL_H
// gate this here // gate this here
#define TEMPORAL #define TEMPORAL
#define DESIRE #define DESIRE
#define TRAFFIC_CONVENTION #define TRAFFIC_CONVENTION
#include "common/mat.h" #include "common/mat.h"
#include "common/util.h" #include "common/util.h"
#include "common/modeldata.h" #include "common/modeldata.h"
@ -17,29 +15,9 @@
#include <memory> #include <memory>
#include "messaging.hpp" #include "messaging.hpp"
#define MODEL_NAME "supercombo_dlc" constexpr int DESIRE_LEN = 8;
constexpr int TRAFFIC_CONVENTION_LEN = 2;
#define MODEL_WIDTH 512 constexpr int MODEL_FREQ = 20;
#define MODEL_HEIGHT 256
#define MODEL_FRAME_SIZE MODEL_WIDTH * MODEL_HEIGHT * 3 / 2
#define DESIRE_LEN 8
#define TRAFFIC_CONVENTION_LEN 2
#define PLAN_MHP_N 5
#define PLAN_MHP_COLUMNS 30
#define PLAN_MHP_VALS 30*33
#define PLAN_MHP_SELECTION 1
#define PLAN_MHP_GROUP_SIZE (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION)
#define LEAD_MHP_N 5
#define LEAD_MHP_VALS 4
#define LEAD_MHP_SELECTION 3
#define LEAD_MHP_GROUP_SIZE (2*LEAD_MHP_VALS + LEAD_MHP_SELECTION)
#define POSE_SIZE 12
#define MODEL_FREQ 20
struct ModelDataRaw { struct ModelDataRaw {
float *plan; float *plan;
float *lane_lines; float *lane_lines;
@ -56,33 +34,26 @@ struct ModelDataRaw {
typedef struct ModelState { typedef struct ModelState {
ModelFrame frame; ModelFrame frame;
float *output; std::unique_ptr<float[]> output;
float *input_frames; std::unique_ptr<float[]> input_frames;
RunModel *m; std::unique_ptr<RunModel> m;
cl_command_queue q;
#ifdef DESIRE #ifdef DESIRE
std::unique_ptr<float[]> prev_desire; float prev_desire[DESIRE_LEN] = {};
std::unique_ptr<float[]> pulse_desire; float pulse_desire[DESIRE_LEN] = {};
#endif #endif
#ifdef TRAFFIC_CONVENTION #ifdef TRAFFIC_CONVENTION
std::unique_ptr<float[]> traffic_convention; float traffic_convention[TRAFFIC_CONVENTION_LEN] = {};
#endif #endif
} ModelState; } ModelState;
void model_init(ModelState* s, cl_device_id device_id, void model_init(ModelState* s, cl_device_id device_id, cl_context context);
cl_context context, int temporal); ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int height,
ModelDataRaw model_eval_frame(ModelState* s, cl_command_queue q, const mat3 &transform, float *desire_in);
cl_mem yuv_cl, int width, int height,
mat3 transform, void* sock, float *desire_in);
void model_free(ModelState* s); void model_free(ModelState* s);
void poly_fit(float *in_pts, float *in_stds, float *out); void poly_fit(float *in_pts, float *in_stds, float *out);
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, const ModelDataRaw &net_outputs, const float *raw_pred, uint64_t timestamp_eof,
uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, const float* raw_pred, float model_execution_time);
uint64_t timestamp_eof, float model_execution_time); void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
void model_publish_v2(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, const ModelDataRaw &net_outputs, uint64_t timestamp_eof);
uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data, const float* raw_pred,
uint64_t timestamp_eof, float model_execution_time);
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id,
uint32_t vipc_dropped_frames, float frame_drop, const ModelDataRaw &data,
uint64_t timestamp_eof);
#endif

Write Preview
Loading…
Cancel
Save