From 7bb13acc45c645c222bc22831008ad45f30e6a7d Mon Sep 17 00:00:00 2001 From: Greg Hogan Date: Mon, 25 Oct 2021 16:46:09 -0700 Subject: [PATCH] ModelDataRaw struct part 4 (#22673) * convert model data for leads to struct * make things more consistent * change names with path to plan * consistent use of plural --- selfdrive/modeld/models/driving.cc | 114 ++++++++++++----------------- selfdrive/modeld/models/driving.h | 60 +++++++++++---- 2 files changed, 91 insertions(+), 83 deletions(-) diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index a6877a1c4d..2be6e75fb6 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -77,11 +77,10 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int heigh // net outputs ModelDataRaw net_outputs { - .plan = (ModelDataRawPlans*)&s->output[PLAN_IDX], + .plans = (ModelDataRawPlans*)&s->output[PLAN_IDX], .lane_lines = (ModelDataRawLaneLines*)&s->output[LL_IDX], .road_edges = (ModelDataRawRoadEdges*)&s->output[RE_IDX], - .lead = &s->output[LEAD_IDX], - .lead_prob = &s->output[LEAD_PROB_IDX], + .leads = (ModelDataRawLeads*)&s->output[LEAD_IDX], .meta = &s->output[DESIRE_STATE_IDX], .pose = (ModelDataRawPose*)&s->output[POSE_IDX], }; @@ -92,59 +91,38 @@ void model_free(ModelState* s) { delete s->frame; } -static const float *get_best_data(const float *data, int size, int group_size, int weight_idx) { - int max_idx = 0; - for (int i = 1; i < size; i++) { - if (data[i * group_size + weight_idx] > - data[max_idx * group_size + weight_idx]) { - max_idx = i; - } - } - return &data[max_idx * group_size]; -} - -static const float *get_lead_data(const float *lead, int t_offset) { - return get_best_data(lead, LEAD_MHP_N, LEAD_MHP_GROUP_SIZE, LEAD_MHP_GROUP_SIZE - LEAD_MHP_SELECTION + t_offset); -} - void fill_sigmoid(const float *input, float *output, int len, int stride) { for (int i=0; i lead_t = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; + auto best_prediction = leads.get_best_prediction(t_idx); + lead.setProb(sigmoid(leads.prob[t_idx])); lead.setProbTime(prob_t); - float x_arr[LEAD_TRAJ_LEN]; - float y_arr[LEAD_TRAJ_LEN]; - float v_arr[LEAD_TRAJ_LEN]; - float a_arr[LEAD_TRAJ_LEN]; - float x_stds_arr[LEAD_TRAJ_LEN]; - float y_stds_arr[LEAD_TRAJ_LEN]; - float v_stds_arr[LEAD_TRAJ_LEN]; - float a_stds_arr[LEAD_TRAJ_LEN]; + std::array lead_x, lead_y, lead_v, lead_a; + std::array lead_x_std, lead_y_std, lead_v_std, lead_a_std; for (int i=0; i pos_x, pos_y, pos_z; std::array pos_x_std, pos_y_std, pos_z_std; std::array vel_x, vel_y, vel_z; @@ -250,7 +228,7 @@ void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelDataRawPlanPath fill_xyzt(framed.initOrientationRate(), T_IDXS_FLOAT, rot_rate_x, rot_rate_y, rot_rate_z); } -void fill_lane_lines(cereal::ModelDataV2::Builder &framed, const std::array &plan_t_arr, +void fill_lane_lines(cereal::ModelDataV2::Builder &framed, const std::array &plan_t, const ModelDataRawLaneLines &lanes) { std::array left_far_y, left_far_z; std::array left_near_y, left_near_z; @@ -268,10 +246,10 @@ void fill_lane_lines(cereal::ModelDataV2::Builder &framed, const std::array &plan_t_arr, +void fill_road_edges(cereal::ModelDataV2::Builder &framed, const std::array &plan_t, const ModelDataRawRoadEdges &edges) { std::array left_y, left_z; std::array right_y, right_z; @@ -300,8 +278,8 @@ void fill_road_edges(cereal::ModelDataV2::Builder &framed, const std::arrayget_best_plan(); - std::array plan_t_arr; - std::fill_n(plan_t_arr.data(), plan_t_arr.size(), NAN); - plan_t_arr[0] = 0.0; + auto best_plan = net_outputs.plans->get_best_prediction(); + std::array plan_t; + std::fill_n(plan_t.data(), plan_t.size(), NAN); + plan_t[0] = 0.0; for (int xidx=1, tidx=0; xidx t_offsets = {0.0, 2.0, 4.0}; + for (int i=0; i mean; - std::array std; +struct ModelDataRawPlanPrediction { + std::array mean; + std::array std; float prob; }; -static_assert(sizeof(ModelDataRawPlanPath) == (sizeof(ModelDataRawPlanTimeStep)*TRAJECTORY_SIZE*2) + sizeof(float)); +static_assert(sizeof(ModelDataRawPlanPrediction) == (sizeof(ModelDataRawPlanElement)*TRAJECTORY_SIZE*2) + sizeof(float)); struct ModelDataRawPlans { - std::array path; + std::array prediction; - constexpr const ModelDataRawPlanPath &get_best_plan() const { + constexpr const ModelDataRawPlanPrediction &get_best_prediction() const { int max_idx = 0; - for (int i = 1; i < path.size(); i++) { - if (path[i].prob > path[max_idx].prob) { + for (int i = 1; i < prediction.size(); i++) { + if (prediction[i].prob > prediction[max_idx].prob) { max_idx = i; } } - return path[max_idx]; + return prediction[max_idx]; } }; -static_assert(sizeof(ModelDataRawPlans) == sizeof(ModelDataRawPlanPath)*PLAN_MHP_N); +static_assert(sizeof(ModelDataRawPlans) == sizeof(ModelDataRawPlanPrediction)*PLAN_MHP_N); struct ModelDataRawLinesXY { std::array left_far; @@ -140,6 +140,37 @@ struct ModelDataRawRoadEdges { }; static_assert(sizeof(ModelDataRawRoadEdges) == (sizeof(ModelDataRawEdgessXY)*2)); +struct ModelDataRawLeadElement { + float x; + float y; + float velocity; + float acceleration; +}; +static_assert(sizeof(ModelDataRawLeadElement) == sizeof(float)*4); + +struct ModelDataRawLeadPrediction { + std::array mean; + std::array std; + std::array prob; +}; +static_assert(sizeof(ModelDataRawLeadPrediction) == (sizeof(ModelDataRawLeadElement)*LEAD_TRAJ_LEN*2) + (sizeof(float)*LEAD_MHP_SELECTION)); + +struct ModelDataRawLeads { + std::array prediction; + std::array prob; + + constexpr const ModelDataRawLeadPrediction &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]) { + max_idx = i; + } + } + 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; @@ -149,11 +180,10 @@ struct ModelDataRawPose { static_assert(sizeof(ModelDataRawPose) == sizeof(ModelDataRawXYZ)*4); struct ModelDataRaw { - const ModelDataRawPlans *const plan; + const ModelDataRawPlans *const plans; const ModelDataRawLaneLines *const lane_lines; const ModelDataRawRoadEdges *const road_edges; - const float *const lead; - const float *const lead_prob; + const ModelDataRawLeads *const leads; const float *const desire_state; const float *const meta; const float *const desire_pred;