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

ModelDataRaw struct part 3 (#22639)

Browse Source 
* lane lines and road edges

* roll back some changes to find what broke things

* rollback more changes to find issue

* fix order of lane line probs

* try outputs on stack again

* initialize output array
old-commit-hash: 755a0a63a2
commatwo_master
Greg Hogan 4 years ago committed by GitHub
parent b88eea00b5
commit cab0fbed7e
3 changed files with 177 additions and 129 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. 1
      selfdrive/common/modeldata.h
  2. 188
      selfdrive/modeld/models/driving.cc
  3. 117
      selfdrive/modeld/models/driving.h

1
selfdrive/common/modeldata.h
Unescape View File

@ -34,6 +34,7 @@ const std::array<double, TRAJECTORY_SIZE> X_IDXS = {
60.75 , 67.6875, 75. , 82.6875, 90.75 , 99.1875,
108. , 117.1875, 126.75 , 136.6875, 147. , 157.6875,
168.75 , 180.1875, 192.};
const auto X_IDXS_FLOAT = convert_array_to_type<double, float, TRAJECTORY_SIZE>(X_IDXS);
#ifdef __cplusplus

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

@ -12,40 +12,6 @@
#include "selfdrive/common/params.h"
#include "selfdrive/common/timing.h"
constexpr int DESIRE_PRED_SIZE = 32;
constexpr int OTHER_META_SIZE = 48;
constexpr int NUM_META_INTERVALS = 5;
constexpr int META_STRIDE = 7;
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 float FCW_THRESHOLD_5MS2_HIGH = 0.15;
constexpr float FCW_THRESHOLD_5MS2_LOW = 0.05;
constexpr float FCW_THRESHOLD_3MS2 = 0.7;
@ -63,15 +29,12 @@ constexpr const kj::ArrayPtr<const T> to_kj_array_ptr(const std::array<T, size>
void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
s->frame = new ModelFrame(device_id, context);
constexpr int output_size = OUTPUT_SIZE + TEMPORAL_SIZE;
s->output.resize(output_size);
#ifdef USE_THNEED
s->m = std::make_unique<ThneedModel>("../../models/supercombo.thneed", &s->output[0], output_size, USE_GPU_RUNTIME);
s->m = std::make_unique<ThneedModel>("../../models/supercombo.thneed", &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME);
#elif USE_ONNX_MODEL
s->m = std::make_unique<ONNXModel>("../../models/supercombo.onnx", &s->output[0], output_size, USE_GPU_RUNTIME);
s->m = std::make_unique<ONNXModel>("../../models/supercombo.onnx", &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME);
#else
s->m = std::make_unique<SNPEModel>("../../models/supercombo.dlc", &s->output[0], output_size, USE_GPU_RUNTIME);
s->m = std::make_unique<SNPEModel>("../../models/supercombo.dlc", &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME);
#endif
#ifdef TEMPORAL
@ -106,22 +69,22 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int heigh
}
#endif
//for (int i = 0; i < OUTPUT_SIZE + TEMPORAL_SIZE; i++) { printf("%f ", s->output[i]); } printf("\n");
//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;
net_outputs.plan = (ModelDataRawPlan*)&s->output[PLAN_IDX];
net_outputs.lane_lines = &s->output[LL_IDX];
net_outputs.lane_lines_prob = &s->output[LL_PROB_IDX];
net_outputs.road_edges = &s->output[RE_IDX];
net_outputs.lead = &s->output[LEAD_IDX];
net_outputs.lead_prob = &s->output[LEAD_PROB_IDX];
net_outputs.meta = &s->output[DESIRE_STATE_IDX];
net_outputs.pose = (ModelDataRawPose*)&s->output[POSE_IDX];
ModelDataRaw net_outputs {
.plan = (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],
.meta = &s->output[DESIRE_STATE_IDX],
.pose = (ModelDataRawPose*)&s->output[POSE_IDX],
};
return net_outputs;
}
@ -144,7 +107,6 @@ 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<len; i++) {
output[i] = sigmoid(input[i*stride]);
@ -257,42 +219,6 @@ void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const std::array<flo
xyzt.setZStd(to_kj_array_ptr(z_std));
}
void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float *data, int columns,
float *plan_t_arr = nullptr, bool fill_std = false) {
float x_arr[TRAJECTORY_SIZE] = {};
float y_arr[TRAJECTORY_SIZE] = {};
float z_arr[TRAJECTORY_SIZE] = {};
float x_std_arr[TRAJECTORY_SIZE];
float y_std_arr[TRAJECTORY_SIZE];
float z_std_arr[TRAJECTORY_SIZE];
float t_arr[TRAJECTORY_SIZE];
for (int i=0; i<TRAJECTORY_SIZE; i++) {
// plan_t_arr != nullptr means this data is X indexed not T indexed
if (plan_t_arr == nullptr) {
t_arr[i] = T_IDXS[i];
x_arr[i] = data[i*columns + 0];
x_std_arr[i] = exp(data[columns*(TRAJECTORY_SIZE + i) + 0]);
} else {
t_arr[i] = plan_t_arr[i];
x_arr[i] = X_IDXS[i];
x_std_arr[i] = NAN;
}
y_arr[i] = data[i*columns + 1];
y_std_arr[i] = exp(data[columns*(TRAJECTORY_SIZE + i) + 1]);
z_arr[i] = data[i*columns + 2];
z_std_arr[i] = exp(data[columns*(TRAJECTORY_SIZE + i) + 2]);
}
xyzt.setX(x_arr);
xyzt.setY(y_arr);
xyzt.setZ(z_arr);
xyzt.setT(t_arr);
if (fill_std) {
xyzt.setXStd(x_std_arr);
xyzt.setYStd(y_std_arr);
xyzt.setZStd(z_std_arr);
}
}
void fill_plan(cereal::ModelDataV2::Builder &framed, const ModelDataRawPlanPath &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;
@ -324,10 +250,69 @@ 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<float, TRAJECTORY_SIZE> &plan_t_arr,
const ModelDataRawLaneLines &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;
std::array<float, TRAJECTORY_SIZE> right_far_y, right_far_z;
for (int j=0; j<TRAJECTORY_SIZE; j++) {
left_far_y[j] = lanes.mean.left_far[j].y;
left_far_z[j] = lanes.mean.left_far[j].z;
left_near_y[j] = lanes.mean.left_near[j].y;
left_near_z[j] = lanes.mean.left_near[j].z;
right_near_y[j] = lanes.mean.right_near[j].y;
right_near_z[j] = lanes.mean.right_near[j].z;
right_far_y[j] = lanes.mean.right_far[j].y;
right_far_z[j] = lanes.mean.right_far[j].z;
}
auto lane_lines = framed.initLaneLines(4);
fill_xyzt(lane_lines[0], plan_t_arr, X_IDXS_FLOAT, left_far_y, left_far_z);
fill_xyzt(lane_lines[1], plan_t_arr, X_IDXS_FLOAT, left_near_y, left_near_z);
fill_xyzt(lane_lines[2], plan_t_arr, X_IDXS_FLOAT, right_near_y, right_near_z);
fill_xyzt(lane_lines[3], plan_t_arr, X_IDXS_FLOAT, right_far_y, right_far_z);
framed.setLaneLineStds({
exp(lanes.std.left_far[0].y),
exp(lanes.std.left_near[0].y),
exp(lanes.std.right_near[0].y),
exp(lanes.std.right_far[0].y),
});
framed.setLaneLineProbs({
sigmoid(lanes.prob.left_far.val),
sigmoid(lanes.prob.left_near.val),
sigmoid(lanes.prob.right_near.val),
sigmoid(lanes.prob.right_far.val),
});
}
void fill_road_edges(cereal::ModelDataV2::Builder &framed, const std::array<float, TRAJECTORY_SIZE> &plan_t_arr,
const ModelDataRawRoadEdges &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++) {
left_y[j] = edges.mean.left[j].y;
left_z[j] = edges.mean.left[j].z;
right_y[j] = edges.mean.right[j].y;
right_z[j] = edges.mean.right[j].z;
}
auto road_edges = framed.initRoadEdges(2);
fill_xyzt(road_edges[0], plan_t_arr, X_IDXS_FLOAT, left_y, left_z);
fill_xyzt(road_edges[1], plan_t_arr, X_IDXS_FLOAT, right_y, right_z);
framed.setRoadEdgeStds({
exp(edges.std.left[0].y),
exp(edges.std.right[0].y),
});
}
void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_outputs) {
auto best_plan = net_outputs.plan->best_plan();
float plan_t_arr[TRAJECTORY_SIZE];
std::fill_n(plan_t_arr, TRAJECTORY_SIZE, NAN);
auto best_plan = net_outputs.plan->get_best_plan();
std::array<float, TRAJECTORY_SIZE> plan_t_arr;
std::fill_n(plan_t_arr.data(), plan_t_arr.size(), NAN);
plan_t_arr[0] = 0.0;
for (int xidx=1, tidx=0; xidx<TRAJECTORY_SIZE; xidx++) {
// increment tidx until we find an element that's further away than the current xidx
@ -348,27 +333,8 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
}
fill_plan(framed, best_plan);
// lane lines
auto lane_lines = framed.initLaneLines(4);
float lane_line_probs_arr[4];
float lane_line_stds_arr[4];
for (int i = 0; i < 4; i++) {
fill_xyzt(lane_lines[i], &net_outputs.lane_lines[i*TRAJECTORY_SIZE*2-1], 2, plan_t_arr);
lane_line_probs_arr[i] = sigmoid(net_outputs.lane_lines_prob[i*2+1]);
lane_line_stds_arr[i] = exp(net_outputs.lane_lines[2*TRAJECTORY_SIZE*(4 + i)]);
}
framed.setLaneLineProbs(lane_line_probs_arr);
framed.setLaneLineStds(lane_line_stds_arr);
// road edges
auto road_edges = framed.initRoadEdges(2);
float road_edge_stds_arr[2];
for (int i = 0; i < 2; i++) {
fill_xyzt(road_edges[i], &net_outputs.road_edges[i*TRAJECTORY_SIZE*2-1], 2, plan_t_arr);
road_edge_stds_arr[i] = exp(net_outputs.road_edges[2*TRAJECTORY_SIZE*(2 + i)]);
}
framed.setRoadEdgeStds(road_edge_stds_arr);
fill_lane_lines(framed, plan_t_arr, *net_outputs.lane_lines);
fill_road_edges(framed, plan_t_arr, *net_outputs.road_edges);
// meta
fill_meta(framed.initMeta(), net_outputs.meta);

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

@ -15,12 +15,46 @@
#include "selfdrive/modeld/models/commonmodel.h"
#include "selfdrive/modeld/runners/run.h"
constexpr int PLAN_MHP_N = 5;
constexpr int DESIRE_LEN = 8;
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 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 {
float x;
float y;
@ -28,6 +62,12 @@ struct ModelDataRawXYZ {
};
static_assert(sizeof(ModelDataRawXYZ) == sizeof(float)*3);
struct ModelDataRawYZ {
float y;
float z;
};
static_assert(sizeof(ModelDataRawYZ) == sizeof(float)*2);
struct ModelDataRawPlanTimeStep {
ModelDataRawXYZ position;
ModelDataRawXYZ velocity;
@ -44,10 +84,10 @@ struct ModelDataRawPlanPath {
};
static_assert(sizeof(ModelDataRawPlanPath) == (sizeof(ModelDataRawPlanTimeStep)*TRAJECTORY_SIZE*2) + sizeof(float));
struct ModelDataRawPlan {
struct ModelDataRawPlans {
std::array<ModelDataRawPlanPath, PLAN_MHP_N> path;
constexpr const ModelDataRawPlanPath &best_plan() const {
constexpr const ModelDataRawPlanPath &get_best_plan() const {
int max_idx = 0;
for (int i = 1; i < path.size(); i++) {
if (path[i].prob > path[max_idx].prob) {
@ -57,7 +97,48 @@ struct ModelDataRawPlan {
return path[max_idx];
}
};
static_assert(sizeof(ModelDataRawPlan) == sizeof(ModelDataRawPlanPath)*PLAN_MHP_N);
static_assert(sizeof(ModelDataRawPlans) == sizeof(ModelDataRawPlanPath)*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;
};
static_assert(sizeof(ModelDataRawLinesXY) == sizeof(ModelDataRawYZ)*TRAJECTORY_SIZE*4);
struct ModelDataRawLineProbVal {
float val_deprecated;
float val;
};
static_assert(sizeof(ModelDataRawLineProbVal) == sizeof(float)*2);
struct ModelDataRawLinesProb {
ModelDataRawLineProbVal left_far;
ModelDataRawLineProbVal left_near;
ModelDataRawLineProbVal right_near;
ModelDataRawLineProbVal right_far;
};
static_assert(sizeof(ModelDataRawLinesProb) == sizeof(ModelDataRawLineProbVal)*4);
struct ModelDataRawLaneLines {
ModelDataRawLinesXY mean;
ModelDataRawLinesXY std;
ModelDataRawLinesProb prob;
};
static_assert(sizeof(ModelDataRawLaneLines) == (sizeof(ModelDataRawLinesXY)*2) + sizeof(ModelDataRawLinesProb));
struct ModelDataRawEdgessXY {
std::array<ModelDataRawYZ, TRAJECTORY_SIZE> left;
std::array<ModelDataRawYZ, TRAJECTORY_SIZE> right;
};
static_assert(sizeof(ModelDataRawEdgessXY) == sizeof(ModelDataRawYZ)*TRAJECTORY_SIZE*2);
struct ModelDataRawRoadEdges {
ModelDataRawEdgessXY mean;
ModelDataRawEdgessXY std;
};
static_assert(sizeof(ModelDataRawRoadEdges) == (sizeof(ModelDataRawEdgessXY)*2));
struct ModelDataRawPose {
ModelDataRawXYZ velocity_mean;
@ -68,21 +149,21 @@ struct ModelDataRawPose {
static_assert(sizeof(ModelDataRawPose) == sizeof(ModelDataRawXYZ)*4);
struct ModelDataRaw {
ModelDataRawPlan *plan;
float *lane_lines;
float *lane_lines_prob;
float *road_edges;
float *lead;
float *lead_prob;
float *desire_state;
float *meta;
float *desire_pred;
ModelDataRawPose *pose;
const ModelDataRawPlans *const plan;
const ModelDataRawLaneLines *const lane_lines;
const ModelDataRawRoadEdges *const road_edges;
const float *const lead;
const float *const lead_prob;
const float *const desire_state;
const float *const meta;
const float *const desire_pred;
const ModelDataRawPose *const pose;
};
typedef struct ModelState {
// TODO: convert remaining arrays to std::array and update model runners
struct ModelState {
ModelFrame *frame;
std::vector<float> output;
std::array<float, NET_OUTPUT_SIZE> output = {};
std::unique_ptr<RunModel> m;
#ifdef DESIRE
float prev_desire[DESIRE_LEN] = {};
@ -91,7 +172,7 @@ typedef struct ModelState {
#ifdef TRAFFIC_CONVENTION
float traffic_convention[TRAFFIC_CONVENTION_LEN] = {};
#endif
} 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,

Write Preview
Loading…
Cancel
Save