spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

ModelDataRaw struct part 3 (#22639) 

* 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
pull/22688/head
Greg Hogan 2021-10-21 14:59:07 -07:00 committed by GitHub
parent 2e0bc9d365
commit 755a0a63a2
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 177 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
selfdrive/common/modeldata.h
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
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
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,