sadmen
/
dragonpilot
mirror of https://github.com/dragonpilot-community/dragonpilot.git
1
0
Fork 0
Code Issues 67 Packages Projects Releases Wiki Activity

model output struct for metadata (#23139)

Browse Source 
* model output struct for metadata

* better max element search

* ModelDataRawDesireProb.to_array()

* eliminate some copies

* not worth messing with softmax now

* remove unused includes

* more cleanup

* no longer pointers

* better variable name

* fix recurrent state

* improve variable name

* fix OUTPUT_SIZE and NET_OUTPUT_SIZE
pull/214/head
Greg Hogan 4 years ago committed by GitHub
parent 8068df0b2a
commit 40d2e4ec90
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 189 additions and 181 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. 6
      selfdrive/modeld/modeld.cc
  2. 127
      selfdrive/modeld/models/driving.cc
  3. 237
      selfdrive/modeld/models/driving.h

6
selfdrive/modeld/modeld.cc
Unescape View File

@ -104,7 +104,7 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) {
}
double mt1 = millis_since_boot();
ModelDataRaw model_buf = model_eval_frame(&model, buf->buf_cl, buf->width, buf->height,
ModelOutput *model_output = model_eval_frame(&model, buf->buf_cl, buf->width, buf->height,
model_transform, vec_desire);
double mt2 = millis_since_boot();
float model_execution_time = (mt2 - mt1) / 1000.0;
@ -119,9 +119,9 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) {
float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, model_buf, extra.timestamp_eof, model_execution_time,
model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, *model_output, extra.timestamp_eof, model_execution_time,
kj::ArrayPtr<const float>(model.output.data(), model.output.size()));
posenet_publish(pm, extra.frame_id, vipc_dropped_frames, model_buf, extra.timestamp_eof);
posenet_publish(pm, extra.frame_id, vipc_dropped_frames, *model_output, extra.timestamp_eof);
//printf("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f\n", mt2 - mt1, mt1 - last, extra.frame_id, frame_id, frame_drop_ratio);
last = mt1;

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

@ -16,8 +16,8 @@ constexpr float FCW_THRESHOLD_5MS2_HIGH = 0.15;
constexpr float FCW_THRESHOLD_5MS2_LOW = 0.05;
constexpr float FCW_THRESHOLD_3MS2 = 0.7;
float prev_brake_5ms2_probs[5] = {0,0,0,0,0};
float prev_brake_3ms2_probs[3] = {0,0,0};
std::array<float, 5> prev_brake_5ms2_probs = {0,0,0,0,0};
std::array<float, 3> prev_brake_3ms2_probs = {0,0,0};
// #define DUMP_YUV
@ -52,7 +52,7 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
#endif
}
ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int height,
ModelOutput* model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int height,
const mat3 &transform, float *desire_in) {
#ifdef DESIRE
if (desire_in != NULL) {
@ -69,35 +69,18 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int heigh
}
#endif
//for (int i = 0; i < NET_OUTPUT_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n");
// if getInputBuf is not NULL, net_input_buf will be
auto net_input_buf = s->frame->prepare(yuv_cl, width, height, transform, static_cast<cl_mem*>(s->m->getInputBuf()));
s->m->execute(net_input_buf, s->frame->buf_size);
// net outputs
ModelDataRaw net_outputs {
.plans = (ModelDataRawPlans*)&s->output[PLAN_IDX],
.lane_lines = (ModelDataRawLaneLines*)&s->output[LL_IDX],
.road_edges = (ModelDataRawRoadEdges*)&s->output[RE_IDX],
.leads = (ModelDataRawLeads*)&s->output[LEAD_IDX],
.meta = &s->output[DESIRE_STATE_IDX],
.pose = (ModelDataRawPose*)&s->output[POSE_IDX],
};
return net_outputs;
return (ModelOutput*)&s->output;
}
void model_free(ModelState* s) {
delete s->frame;
}
void fill_sigmoid(const float *input, float *output, int len, int stride) {
for (int i=0; i<len; i++) {
output[i] = sigmoid(input[i*stride]);
}
}
void fill_lead(cereal::ModelDataV2::LeadDataV3::Builder lead, const ModelDataRawLeads &leads, int t_idx, float prob_t) {
void fill_lead(cereal::ModelDataV2::LeadDataV3::Builder lead, const ModelOutputLeads &leads, int t_idx, float prob_t) {
std::array<float, LEAD_TRAJ_LEN> lead_t = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0};
const auto &best_prediction = leads.get_best_prediction(t_idx);
lead.setProb(sigmoid(leads.prob[t_idx]));
@ -125,56 +108,54 @@ void fill_lead(cereal::ModelDataV2::LeadDataV3::Builder lead, const ModelDataRaw
lead.setAStd(to_kj_array_ptr(lead_a_std));
}
void fill_meta(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);
void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const ModelOutputMeta &meta_data) {
std::array<float, DESIRE_LEN> desire_state_softmax;
softmax(meta_data.desire_state_prob.array.data(), desire_state_softmax.data(), DESIRE_LEN);
std::array<float, DESIRE_PRED_LEN * DESIRE_LEN> desire_pred_softmax;
for (int i=0; i<DESIRE_PRED_LEN; i++) {
softmax(meta_data.desire_pred_prob[i].array.data(), desire_pred_softmax.data() + (i * DESIRE_LEN), DESIRE_LEN);
}
std::array<float, DISENGAGE_LEN> lat_long_t = {2,4,6,8,10};
std::array<float, DISENGAGE_LEN> gas_disengage_sigmoid, brake_disengage_sigmoid, steer_override_sigmoid,
brake_3ms2_sigmoid, brake_4ms2_sigmoid, brake_5ms2_sigmoid;
for (int i=0; i<DISENGAGE_LEN; i++) {
gas_disengage_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].gas_disengage);
brake_disengage_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_disengage);
steer_override_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].steer_override);
brake_3ms2_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_3ms2);
brake_4ms2_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_4ms2);
brake_5ms2_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].brake_5ms2);
//gas_pressed_sigmoid[i] = sigmoid(meta_data.disengage_prob[i].gas_pressed);
}
float gas_disengage_sigmoid[NUM_META_INTERVALS];
float brake_disengage_sigmoid[NUM_META_INTERVALS];
float steer_override_sigmoid[NUM_META_INTERVALS];
float brake_3ms2_sigmoid[NUM_META_INTERVALS];
float brake_4ms2_sigmoid[NUM_META_INTERVALS];
float brake_5ms2_sigmoid[NUM_META_INTERVALS];
fill_sigmoid(&meta_data[DESIRE_LEN+1], gas_disengage_sigmoid, NUM_META_INTERVALS, META_STRIDE);
fill_sigmoid(&meta_data[DESIRE_LEN+2], brake_disengage_sigmoid, NUM_META_INTERVALS, META_STRIDE);
fill_sigmoid(&meta_data[DESIRE_LEN+3], steer_override_sigmoid, NUM_META_INTERVALS, META_STRIDE);
fill_sigmoid(&meta_data[DESIRE_LEN+4], brake_3ms2_sigmoid, NUM_META_INTERVALS, META_STRIDE);
fill_sigmoid(&meta_data[DESIRE_LEN+5], brake_4ms2_sigmoid, NUM_META_INTERVALS, META_STRIDE);
fill_sigmoid(&meta_data[DESIRE_LEN+6], brake_5ms2_sigmoid, NUM_META_INTERVALS, META_STRIDE);
//fill_sigmoid(&meta_data[DESIRE_LEN+7], GAS PRESSED, NUM_META_INTERVALS, META_STRIDE);
std::memmove(prev_brake_5ms2_probs, &prev_brake_5ms2_probs[1], 4*sizeof(float));
std::memmove(prev_brake_3ms2_probs, &prev_brake_3ms2_probs[1], 2*sizeof(float));
std::memmove(prev_brake_5ms2_probs.data(), &prev_brake_5ms2_probs[1], 4*sizeof(float));
std::memmove(prev_brake_3ms2_probs.data(), &prev_brake_3ms2_probs[1], 2*sizeof(float));
prev_brake_5ms2_probs[4] = brake_5ms2_sigmoid[0];
prev_brake_3ms2_probs[2] = brake_3ms2_sigmoid[0];
bool above_fcw_threshold = true;
for (int i=0; i<5; i++) {
for (int i=0; i<prev_brake_5ms2_probs.size(); i++) {
float threshold = i < 2 ? FCW_THRESHOLD_5MS2_LOW : FCW_THRESHOLD_5MS2_HIGH;
above_fcw_threshold = above_fcw_threshold && prev_brake_5ms2_probs[i] > threshold;
}
for (int i=0; i<3; i++) {
for (int i=0; i<prev_brake_3ms2_probs.size(); i++) {
above_fcw_threshold = above_fcw_threshold && prev_brake_3ms2_probs[i] > FCW_THRESHOLD_3MS2;
}
auto disengage = meta.initDisengagePredictions();
disengage.setT({2,4,6,8,10});
disengage.setGasDisengageProbs(gas_disengage_sigmoid);
disengage.setBrakeDisengageProbs(brake_disengage_sigmoid);
disengage.setSteerOverrideProbs(steer_override_sigmoid);
disengage.setBrake3MetersPerSecondSquaredProbs(brake_3ms2_sigmoid);
disengage.setBrake4MetersPerSecondSquaredProbs(brake_4ms2_sigmoid);
disengage.setBrake5MetersPerSecondSquaredProbs(brake_5ms2_sigmoid);
meta.setEngagedProb(sigmoid(meta_data[DESIRE_LEN]));
meta.setDesirePrediction(desire_pred_softmax);
meta.setDesireState(desire_state_softmax);
disengage.setT(to_kj_array_ptr(lat_long_t));
disengage.setGasDisengageProbs(to_kj_array_ptr(gas_disengage_sigmoid));
disengage.setBrakeDisengageProbs(to_kj_array_ptr(brake_disengage_sigmoid));
disengage.setSteerOverrideProbs(to_kj_array_ptr(steer_override_sigmoid));
disengage.setBrake3MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_3ms2_sigmoid));
disengage.setBrake4MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_4ms2_sigmoid));
disengage.setBrake5MetersPerSecondSquaredProbs(to_kj_array_ptr(brake_5ms2_sigmoid));
meta.setEngagedProb(sigmoid(meta_data.engaged_prob));
meta.setDesirePrediction(to_kj_array_ptr(desire_pred_softmax));
meta.setDesireState(to_kj_array_ptr(desire_state_softmax));
meta.setHardBrakePredicted(above_fcw_threshold);
}
@ -197,7 +178,7 @@ void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const std::array<flo
xyzt.setZStd(to_kj_array_ptr(z_std));
}
void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelDataRawPlanPrediction &plan) {
void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelOutputPlanPrediction &plan) {
std::array<float, TRAJECTORY_SIZE> pos_x, pos_y, pos_z;
std::array<float, TRAJECTORY_SIZE> pos_x_std, pos_y_std, pos_z_std;
std::array<float, TRAJECTORY_SIZE> vel_x, vel_y, vel_z;
@ -229,7 +210,7 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelDataRawPlanPredi
}
void fill_lane_lines(cereal::ModelDataV2::Builder &framed, const std::array<float, TRAJECTORY_SIZE> &plan_t,
const ModelDataRawLaneLines &lanes) {
const ModelOutputLaneLines &lanes) {
std::array<float, TRAJECTORY_SIZE> left_far_y, left_far_z;
std::array<float, TRAJECTORY_SIZE> left_near_y, left_near_z;
std::array<float, TRAJECTORY_SIZE> right_near_y, right_near_z;
@ -267,7 +248,7 @@ void fill_lane_lines(cereal::ModelDataV2::Builder &framed, const std::array<floa
}
void fill_road_edges(cereal::ModelDataV2::Builder &framed, const std::array<float, TRAJECTORY_SIZE> &plan_t,
const ModelDataRawRoadEdges &edges) {
const ModelOutputRoadEdges &edges) {
std::array<float, TRAJECTORY_SIZE> left_y, left_z;
std::array<float, TRAJECTORY_SIZE> right_y, right_z;
for (int j=0; j<TRAJECTORY_SIZE; j++) {
@ -287,8 +268,8 @@ void fill_road_edges(cereal::ModelDataV2::Builder &framed, const std::array<floa
});
}
void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_outputs) {
const auto &best_plan = net_outputs.plans->get_best_prediction();
void fill_model(cereal::ModelDataV2::Builder &framed, const ModelOutput &net_outputs) {
const auto &best_plan = net_outputs.plans.get_best_prediction();
std::array<float, TRAJECTORY_SIZE> plan_t;
std::fill_n(plan_t.data(), plan_t.size(), NAN);
plan_t[0] = 0.0;
@ -311,8 +292,8 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
}
fill_plan(framed, best_plan);
fill_lane_lines(framed, plan_t, *net_outputs.lane_lines);
fill_road_edges(framed, plan_t, *net_outputs.road_edges);
fill_lane_lines(framed, plan_t, net_outputs.lane_lines);
fill_road_edges(framed, plan_t, net_outputs.road_edges);
// meta
fill_meta(framed.initMeta(), net_outputs.meta);
@ -321,12 +302,12 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
auto leads = framed.initLeadsV3(LEAD_MHP_SELECTION);
std::array<float, LEAD_MHP_SELECTION> t_offsets = {0.0, 2.0, 4.0};
for (int i=0; i<LEAD_MHP_SELECTION; i++) {
fill_lead(leads[i], *net_outputs.leads, i, t_offsets[i]);
fill_lead(leads[i], net_outputs.leads, i, t_offsets[i]);
}
}
void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, float frame_drop,
const ModelDataRaw &net_outputs, uint64_t timestamp_eof,
const ModelOutput &net_outputs, uint64_t timestamp_eof,
float model_execution_time, kj::ArrayPtr<const float> raw_pred) {
const uint32_t frame_age = (frame_id > vipc_frame_id) ? (frame_id - vipc_frame_id) : 0;
MessageBuilder msg;
@ -344,12 +325,12 @@ void model_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t frame_id, flo
}
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
const ModelDataRaw &net_outputs, uint64_t timestamp_eof) {
const ModelOutput &net_outputs, uint64_t timestamp_eof) {
MessageBuilder msg;
auto v_mean = net_outputs.pose->velocity_mean;
auto r_mean = net_outputs.pose->rotation_mean;
auto v_std = net_outputs.pose->velocity_std;
auto r_std = net_outputs.pose->rotation_std;
auto v_mean = net_outputs.pose.velocity_mean;
auto r_mean = net_outputs.pose.rotation_mean;
auto v_std = net_outputs.pose.velocity_std;
auto r_std = net_outputs.pose.rotation_std;
auto posenetd = msg.initEvent(vipc_dropped_frames < 1).initCameraOdometry();
posenetd.setTrans({v_mean.x, v_mean.y, v_mean.z});

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

@ -16,78 +16,54 @@
#include "selfdrive/modeld/runners/run.h"
constexpr int DESIRE_LEN = 8;
constexpr int DESIRE_PRED_LEN = 4;
constexpr int TRAFFIC_CONVENTION_LEN = 2;
constexpr int MODEL_FREQ = 20;
constexpr int DESIRE_PRED_SIZE = 32;
constexpr int OTHER_META_SIZE = 48;
constexpr int NUM_META_INTERVALS = 5;
constexpr int DISENGAGE_LEN = 5;
constexpr int BLINKER_LEN = 6;
constexpr int META_STRIDE = 7;
constexpr int PLAN_MHP_N = 5;
constexpr int PLAN_MHP_VALS = 15*33;
constexpr int PLAN_MHP_SELECTION = 1;
constexpr int PLAN_MHP_GROUP_SIZE = (2*PLAN_MHP_VALS + PLAN_MHP_SELECTION);
constexpr int LEAD_MHP_N = 2;
constexpr int LEAD_TRAJ_LEN = 6;
constexpr int LEAD_PRED_DIM = 4;
constexpr int LEAD_MHP_VALS = LEAD_PRED_DIM*LEAD_TRAJ_LEN;
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 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 + 8;
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
constexpr int TEMPORAL_SIZE = 512;
#else
constexpr int TEMPORAL_SIZE = 0;
#endif
constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + TEMPORAL_SIZE;
struct ModelDataRawXYZ {
struct ModelOutputXYZ {
float x;
float y;
float z;
};
static_assert(sizeof(ModelDataRawXYZ) == sizeof(float)*3);
static_assert(sizeof(ModelOutputXYZ) == sizeof(float)*3);
struct ModelDataRawYZ {
struct ModelOutputYZ {
float y;
float z;
};
static_assert(sizeof(ModelDataRawYZ) == sizeof(float)*2);
static_assert(sizeof(ModelOutputYZ) == sizeof(float)*2);
struct ModelDataRawPlanElement {
ModelDataRawXYZ position;
ModelDataRawXYZ velocity;
ModelDataRawXYZ acceleration;
ModelDataRawXYZ rotation;
ModelDataRawXYZ rotation_rate;
struct ModelOutputPlanElement {
ModelOutputXYZ position;
ModelOutputXYZ velocity;
ModelOutputXYZ acceleration;
ModelOutputXYZ rotation;
ModelOutputXYZ rotation_rate;
};
static_assert(sizeof(ModelDataRawPlanElement) == sizeof(ModelDataRawXYZ)*5);
static_assert(sizeof(ModelOutputPlanElement) == sizeof(ModelOutputXYZ)*5);
struct ModelDataRawPlanPrediction {
std::array<ModelDataRawPlanElement, TRAJECTORY_SIZE> mean;
std::array<ModelDataRawPlanElement, TRAJECTORY_SIZE> std;
struct ModelOutputPlanPrediction {
std::array<ModelOutputPlanElement, TRAJECTORY_SIZE> mean;
std::array<ModelOutputPlanElement, TRAJECTORY_SIZE> std;
float prob;
};
static_assert(sizeof(ModelDataRawPlanPrediction) == (sizeof(ModelDataRawPlanElement)*TRAJECTORY_SIZE*2) + sizeof(float));
static_assert(sizeof(ModelOutputPlanPrediction) == (sizeof(ModelOutputPlanElement)*TRAJECTORY_SIZE*2) + sizeof(float));
struct ModelDataRawPlans {
std::array<ModelDataRawPlanPrediction, PLAN_MHP_N> prediction;
struct ModelOutputPlans {
std::array<ModelOutputPlanPrediction, PLAN_MHP_N> prediction;
constexpr const ModelDataRawPlanPrediction &get_best_prediction() const {
constexpr const ModelOutputPlanPrediction &get_best_prediction() const {
int max_idx = 0;
for (int i = 1; i < prediction.size(); i++) {
if (prediction[i].prob > prediction[max_idx].prob) {
@ -97,69 +73,69 @@ struct ModelDataRawPlans {
return prediction[max_idx];
}
};
static_assert(sizeof(ModelDataRawPlans) == sizeof(ModelDataRawPlanPrediction)*PLAN_MHP_N);
static_assert(sizeof(ModelOutputPlans) == sizeof(ModelOutputPlanPrediction)*PLAN_MHP_N);
struct ModelDataRawLinesXY {
std::array<ModelDataRawYZ, TRAJECTORY_SIZE> left_far;
std::array<ModelDataRawYZ, TRAJECTORY_SIZE> left_near;
std::array<ModelDataRawYZ, TRAJECTORY_SIZE> right_near;
std::array<ModelDataRawYZ, TRAJECTORY_SIZE> right_far;
struct ModelOutputLinesXY {
std::array<ModelOutputYZ, TRAJECTORY_SIZE> left_far;
std::array<ModelOutputYZ, TRAJECTORY_SIZE> left_near;
std::array<ModelOutputYZ, TRAJECTORY_SIZE> right_near;
std::array<ModelOutputYZ, TRAJECTORY_SIZE> right_far;
};
static_assert(sizeof(ModelDataRawLinesXY) == sizeof(ModelDataRawYZ)*TRAJECTORY_SIZE*4);
static_assert(sizeof(ModelOutputLinesXY) == sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*4);
struct ModelDataRawLineProbVal {
struct ModelOutputLineProbVal {
float val_deprecated;
float val;
};
static_assert(sizeof(ModelDataRawLineProbVal) == sizeof(float)*2);
static_assert(sizeof(ModelOutputLineProbVal) == sizeof(float)*2);
struct ModelDataRawLinesProb {
ModelDataRawLineProbVal left_far;
ModelDataRawLineProbVal left_near;
ModelDataRawLineProbVal right_near;
ModelDataRawLineProbVal right_far;
struct ModelOutputLinesProb {
ModelOutputLineProbVal left_far;
ModelOutputLineProbVal left_near;
ModelOutputLineProbVal right_near;
ModelOutputLineProbVal right_far;
};
static_assert(sizeof(ModelDataRawLinesProb) == sizeof(ModelDataRawLineProbVal)*4);
static_assert(sizeof(ModelOutputLinesProb) == sizeof(ModelOutputLineProbVal)*4);
struct ModelDataRawLaneLines {
ModelDataRawLinesXY mean;
ModelDataRawLinesXY std;
ModelDataRawLinesProb prob;
struct ModelOutputLaneLines {
ModelOutputLinesXY mean;
ModelOutputLinesXY std;
ModelOutputLinesProb prob;
};
static_assert(sizeof(ModelDataRawLaneLines) == (sizeof(ModelDataRawLinesXY)*2) + sizeof(ModelDataRawLinesProb));
static_assert(sizeof(ModelOutputLaneLines) == (sizeof(ModelOutputLinesXY)*2) + sizeof(ModelOutputLinesProb));
struct ModelDataRawEdgessXY {
std::array<ModelDataRawYZ, TRAJECTORY_SIZE> left;
std::array<ModelDataRawYZ, TRAJECTORY_SIZE> right;
struct ModelOutputEdgessXY {
std::array<ModelOutputYZ, TRAJECTORY_SIZE> left;
std::array<ModelOutputYZ, TRAJECTORY_SIZE> right;
};
static_assert(sizeof(ModelDataRawEdgessXY) == sizeof(ModelDataRawYZ)*TRAJECTORY_SIZE*2);
static_assert(sizeof(ModelOutputEdgessXY) == sizeof(ModelOutputYZ)*TRAJECTORY_SIZE*2);
struct ModelDataRawRoadEdges {
ModelDataRawEdgessXY mean;
ModelDataRawEdgessXY std;
struct ModelOutputRoadEdges {
ModelOutputEdgessXY mean;
ModelOutputEdgessXY std;
};
static_assert(sizeof(ModelDataRawRoadEdges) == (sizeof(ModelDataRawEdgessXY)*2));
static_assert(sizeof(ModelOutputRoadEdges) == (sizeof(ModelOutputEdgessXY)*2));
struct ModelDataRawLeadElement {
struct ModelOutputLeadElement {
float x;
float y;
float velocity;
float acceleration;
};
static_assert(sizeof(ModelDataRawLeadElement) == sizeof(float)*4);
static_assert(sizeof(ModelOutputLeadElement) == sizeof(float)*4);
struct ModelDataRawLeadPrediction {
std::array<ModelDataRawLeadElement, LEAD_TRAJ_LEN> mean;
std::array<ModelDataRawLeadElement, LEAD_TRAJ_LEN> std;
struct ModelOutputLeadPrediction {
std::array<ModelOutputLeadElement, LEAD_TRAJ_LEN> mean;
std::array<ModelOutputLeadElement, LEAD_TRAJ_LEN> std;
std::array<float, LEAD_MHP_SELECTION> prob;
};
static_assert(sizeof(ModelDataRawLeadPrediction) == (sizeof(ModelDataRawLeadElement)*LEAD_TRAJ_LEN*2) + (sizeof(float)*LEAD_MHP_SELECTION));
static_assert(sizeof(ModelOutputLeadPrediction) == (sizeof(ModelOutputLeadElement)*LEAD_TRAJ_LEN*2) + (sizeof(float)*LEAD_MHP_SELECTION));
struct ModelDataRawLeads {
std::array<ModelDataRawLeadPrediction, LEAD_MHP_N> prediction;
struct ModelOutputLeads {
std::array<ModelOutputLeadPrediction, LEAD_MHP_N> prediction;
std::array<float, LEAD_MHP_SELECTION> prob;
constexpr const ModelDataRawLeadPrediction &get_best_prediction(int t_idx) const {
constexpr const ModelOutputLeadPrediction &get_best_prediction(int t_idx) const {
int max_idx = 0;
for (int i = 1; i < prediction.size(); i++) {
if (prediction[i].prob[t_idx] > prediction[max_idx].prob[t_idx]) {
@ -169,26 +145,77 @@ struct ModelDataRawLeads {
return prediction[max_idx];
}
};
static_assert(sizeof(ModelDataRawLeads) == (sizeof(ModelDataRawLeadPrediction)*LEAD_MHP_N) + (sizeof(float)*LEAD_MHP_SELECTION));
struct ModelDataRawPose {
ModelDataRawXYZ velocity_mean;
ModelDataRawXYZ rotation_mean;
ModelDataRawXYZ velocity_std;
ModelDataRawXYZ rotation_std;
};
static_assert(sizeof(ModelDataRawPose) == sizeof(ModelDataRawXYZ)*4);
struct ModelDataRaw {
const ModelDataRawPlans *const plans;
const ModelDataRawLaneLines *const lane_lines;
const ModelDataRawRoadEdges *const road_edges;
const ModelDataRawLeads *const leads;
const float *const desire_state;
const float *const meta;
const float *const desire_pred;
const ModelDataRawPose *const pose;
};
static_assert(sizeof(ModelOutputLeads) == (sizeof(ModelOutputLeadPrediction)*LEAD_MHP_N) + (sizeof(float)*LEAD_MHP_SELECTION));
struct ModelOutputPose {
ModelOutputXYZ velocity_mean;
ModelOutputXYZ rotation_mean;
ModelOutputXYZ velocity_std;
ModelOutputXYZ rotation_std;
};
static_assert(sizeof(ModelOutputPose) == sizeof(ModelOutputXYZ)*4);
struct ModelOutputDisengageProb {
float gas_disengage;
float brake_disengage;
float steer_override;
float brake_3ms2;
float brake_4ms2;
float brake_5ms2;
float gas_pressed;
};
static_assert(sizeof(ModelOutputDisengageProb) == sizeof(float)*7);
struct ModelOutputBlinkerProb {
float left;
float right;
};
static_assert(sizeof(ModelOutputBlinkerProb) == sizeof(float)*2);
struct ModelOutputDesireProb {
union {
struct {
float none;
float turn_left;
float turn_right;
float lane_change_left;
float lane_change_right;
float keep_left;
float keep_right;
float null;
};
struct {
std::array<float, DESIRE_LEN> array;
};
};
};
static_assert(sizeof(ModelOutputDesireProb) == sizeof(float)*DESIRE_LEN);
struct ModelOutputMeta {
ModelOutputDesireProb desire_state_prob;
float engaged_prob;
std::array<ModelOutputDisengageProb, DISENGAGE_LEN> disengage_prob;
std::array<ModelOutputBlinkerProb, BLINKER_LEN> blinker_prob;
std::array<ModelOutputDesireProb, DESIRE_PRED_LEN> desire_pred_prob;
};
static_assert(sizeof(ModelOutputMeta) == sizeof(ModelOutputDesireProb) + sizeof(float) + (sizeof(ModelOutputDisengageProb)*DISENGAGE_LEN) + (sizeof(ModelOutputBlinkerProb)*BLINKER_LEN) + (sizeof(ModelOutputDesireProb)*DESIRE_PRED_LEN));
struct ModelOutput {
const ModelOutputPlans plans;
const ModelOutputLaneLines lane_lines;
const ModelOutputRoadEdges road_edges;
const ModelOutputLeads leads;
const ModelOutputMeta meta;
const ModelOutputPose pose;
};
constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float);
#ifdef TEMPORAL
constexpr int TEMPORAL_SIZE = 512;
#else
constexpr int TEMPORAL_SIZE = 0;
#endif
constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + TEMPORAL_SIZE;
// TODO: convert remaining arrays to std::array and update model runners
struct ModelState {
@ -205,12 +232,12 @@ struct ModelState {
};
void model_init(ModelState* s, cl_device_id device_id, cl_context context);
ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int height,
ModelOutput *model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int height,
const mat3 &transform, float *desire_in);
void model_free(ModelState* s);
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,
const ModelDataRaw &net_outputs, uint64_t timestamp_eof,
const ModelOutput &net_outputs, uint64_t timestamp_eof,
float model_execution_time, kj::ArrayPtr<const float> raw_pred);
void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames,
const ModelDataRaw &net_outputs, uint64_t timestamp_eof);
const ModelOutput &net_outputs, uint64_t timestamp_eof);

Write Preview
Loading…
Cancel
Save