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

ModelDataRaw struct part 2 (#22554) 

* model pose more struct

* constexpr seems useful

* more concise

* fix order

* seems more readable

* plan struct

* attempt to fix compiling on device

* fix constexpr errors on device

* fix rotation rate in log

* maybe this fixes compiling constexpr on device

* exp() cannot be used in constexpr on c2

* only copy what we actually log

* simplify pivot()

* fix setting position std

* see if this works

* try lambda again

* giving up on the lambda to select member

* fix position std

* fix param order
pull/22627/head
Greg Hogan 2021-10-19 19:04:01 -07:00 committed by GitHub
parent d01c340f1f
commit a7f36c9daf
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
3 changed files with 134 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
selfdrive/common/modeldata.h
View File

@ -1,11 +1,23 @@
#pragma once
#include <array>
const int TRAJECTORY_SIZE = 33;
const int LAT_MPC_N = 16;
const int LON_MPC_N = 32;
const float MIN_DRAW_DISTANCE = 10.0;
const float MAX_DRAW_DISTANCE = 100.0;
const double T_IDXS[TRAJECTORY_SIZE] = {
template<typename T_SRC, typename T_DST, size_t size>
const std::array<T_DST, size> convert_array_to_type(const std::array<T_SRC, size> &src) {
std::array<T_DST, size> dst = {};
for (int i=0; i<size; i++) {
dst[i] = src[i];
}
return dst;
}
const std::array<double, TRAJECTORY_SIZE> T_IDXS = {
0. , 0.00976562, 0.0390625 , 0.08789062, 0.15625 ,
0.24414062, 0.3515625 , 0.47851562, 0.625 , 0.79101562,
0.9765625 , 1.18164062, 1.40625 , 1.65039062, 1.9140625 ,
@ -13,7 +25,9 @@ const double T_IDXS[TRAJECTORY_SIZE] = {
3.90625 , 4.30664062, 4.7265625 , 5.16601562, 5.625 ,
6.10351562, 6.6015625 , 7.11914062, 7.65625 , 8.21289062,
8.7890625 , 9.38476562, 10.};
const double X_IDXS[TRAJECTORY_SIZE] = {
const auto T_IDXS_FLOAT = convert_array_to_type<double, float, TRAJECTORY_SIZE>(T_IDXS);
const std::array<double, TRAJECTORY_SIZE> X_IDXS = {
0. , 0.1875, 0.75 , 1.6875, 3. , 4.6875,
6.75 , 9.1875, 12. , 15.1875, 18.75 , 22.6875,
27. , 31.6875, 36.75 , 42.1875, 48. , 54.1875,

102
selfdrive/modeld/models/driving.cc
View File

@ -17,8 +17,6 @@ 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_COLUMNS = 15;
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);
@ -57,6 +55,11 @@ float prev_brake_3ms2_probs[3] = {0,0,0};
// #define DUMP_YUV
template<class T, size_t size>
constexpr const kj::ArrayPtr<const T> to_kj_array_ptr(const std::array<T, size> &arr) {
return kj::ArrayPtr(arr.data(), arr.size());
}
void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
s->frame = new ModelFrame(device_id, context);
@ -111,7 +114,7 @@ ModelDataRaw model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int heigh
// net outputs
ModelDataRaw net_outputs;
net_outputs.plan = &s->output[PLAN_IDX];
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];
@ -137,10 +140,6 @@ static const float *get_best_data(const float *data, int size, int group_size, i
return &data[max_idx * group_size];
}
static const float *get_plan_data(float *plan) {
return get_best_data(plan, PLAN_MHP_N, PLAN_MHP_GROUP_SIZE, PLAN_MHP_GROUP_SIZE - 1);
}
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);
}
@ -239,6 +238,25 @@ void fill_meta(cereal::ModelDataV2::MetaData::Builder meta, const float *meta_da
meta.setHardBrakePredicted(above_fcw_threshold);
}
template<size_t size>
void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const std::array<float, size> &t,
const std::array<float, size> &x, const std::array<float, size> &y, const std::array<float, size> &z) {
xyzt.setT(to_kj_array_ptr(t));
xyzt.setX(to_kj_array_ptr(x));
xyzt.setY(to_kj_array_ptr(y));
xyzt.setZ(to_kj_array_ptr(z));
}
template<size_t size>
void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const std::array<float, size> &t,
const std::array<float, size> &x, const std::array<float, size> &y, const std::array<float, size> &z,
const std::array<float, size> &x_std, const std::array<float, size> &y_std, const std::array<float, size> &z_std) {
fill_xyzt(xyzt, t, x, y, z);
xyzt.setXStd(to_kj_array_ptr(x_std));
xyzt.setYStd(to_kj_array_ptr(y_std));
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] = {};
@ -275,14 +293,45 @@ void fill_xyzt(cereal::ModelDataV2::XYZTData::Builder xyzt, const float *data, i
}
}
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;
std::array<float, TRAJECTORY_SIZE> vel_x, vel_y, vel_z;
std::array<float, TRAJECTORY_SIZE> rot_x, rot_y, rot_z;
std::array<float, TRAJECTORY_SIZE> rot_rate_x, rot_rate_y, rot_rate_z;
for(int i=0; i<TRAJECTORY_SIZE; i++) {
pos_x[i] = plan.mean[i].position.x;
pos_y[i] = plan.mean[i].position.y;
pos_z[i] = plan.mean[i].position.z;
pos_x_std[i] = exp(plan.std[i].position.x);
pos_y_std[i] = exp(plan.std[i].position.y);
pos_z_std[i] = exp(plan.std[i].position.z);
vel_x[i] = plan.mean[i].velocity.x;
vel_y[i] = plan.mean[i].velocity.y;
vel_z[i] = plan.mean[i].velocity.z;
rot_x[i] = plan.mean[i].rotation.x;
rot_y[i] = plan.mean[i].rotation.y;
rot_z[i] = plan.mean[i].rotation.z;
rot_rate_x[i] = plan.mean[i].rotation_rate.x;
rot_rate_y[i] = plan.mean[i].rotation_rate.y;
rot_rate_z[i] = plan.mean[i].rotation_rate.z;
}
fill_xyzt(framed.initPosition(), T_IDXS_FLOAT, pos_x, pos_y, pos_z, pos_x_std, pos_y_std, pos_z_std);
fill_xyzt(framed.initVelocity(), T_IDXS_FLOAT, vel_x, vel_y, vel_z);
fill_xyzt(framed.initOrientation(), T_IDXS_FLOAT, rot_x, rot_y, rot_z);
fill_xyzt(framed.initOrientationRate(), T_IDXS_FLOAT, rot_rate_x, rot_rate_y, rot_rate_z);
}
void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_outputs) {
const float *best_plan = get_plan_data(net_outputs.plan);
auto best_plan = net_outputs.plan->best_plan();
float plan_t_arr[TRAJECTORY_SIZE];
std::fill_n(plan_t_arr, TRAJECTORY_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
for (int next_tid = tidx + 1; next_tid < TRAJECTORY_SIZE && best_plan[next_tid*PLAN_MHP_COLUMNS] < X_IDXS[xidx]; next_tid++) {
for (int next_tid = tidx + 1; next_tid < TRAJECTORY_SIZE && best_plan.mean[next_tid].position.x < X_IDXS[xidx]; next_tid++) {
tidx++;
}
if (tidx == TRAJECTORY_SIZE - 1) {
@ -292,16 +341,13 @@ void fill_model(cereal::ModelDataV2::Builder &framed, const ModelDataRaw &net_ou
}
// interpolate to find `t` for the current xidx
float current_x_val = best_plan[tidx * PLAN_MHP_COLUMNS];
float next_x_val = best_plan[(tidx+1) * PLAN_MHP_COLUMNS];
float current_x_val = best_plan.mean[tidx].position.x;
float next_x_val = best_plan.mean[tidx+1].position.x;
float p = (X_IDXS[xidx] - current_x_val) / (next_x_val - current_x_val);
plan_t_arr[xidx] = p * T_IDXS[tidx+1] + (1 - p) * T_IDXS[tidx];
}
fill_xyzt(framed.initPosition(), &best_plan[0], PLAN_MHP_COLUMNS, nullptr, true);
fill_xyzt(framed.initVelocity(), &best_plan[3], PLAN_MHP_COLUMNS);
fill_xyzt(framed.initOrientation(), &best_plan[9], PLAN_MHP_COLUMNS);
fill_xyzt(framed.initOrientationRate(), &best_plan[12], PLAN_MHP_COLUMNS);
fill_plan(framed, best_plan);
// lane lines
auto lane_lines = framed.initLaneLines(4);
@ -355,25 +401,17 @@ 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) {
float trans_arr[3];
float trans_std_arr[3];
float rot_arr[3];
float rot_std_arr[3];
for (int i =0; i < 3; i++) {
trans_arr[i] = net_outputs.pose->trans_arr[i];
trans_std_arr[i] = exp(net_outputs.pose->trans_std_arr[i]);
rot_arr[i] = net_outputs.pose->rot_arr[i];
rot_std_arr[i] = exp(net_outputs.pose->rot_std_arr[i]);
}
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 posenetd = msg.initEvent(vipc_dropped_frames < 1).initCameraOdometry();
posenetd.setTrans(trans_arr);
posenetd.setRot(rot_arr);
posenetd.setTransStd(trans_std_arr);
posenetd.setRotStd(rot_std_arr);
posenetd.setTrans({v_mean.x, v_mean.y, v_mean.z});
posenetd.setRot({r_mean.x, r_mean.y, r_mean.z});
posenetd.setTransStd({exp(v_std.x), exp(v_std.y), exp(v_std.z)});
posenetd.setRotStd({exp(r_std.x), exp(r_std.y), exp(r_std.z)});
posenetd.setTimestampEof(timestamp_eof);
posenetd.setFrameId(vipc_frame_id);

54
selfdrive/modeld/models/driving.h
View File

@ -5,6 +5,7 @@
#define DESIRE
#define TRAFFIC_CONVENTION
#include <array>
#include <memory>
#include "cereal/messaging/messaging.h"
@ -14,19 +15,60 @@
#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;
struct ModelDataRawPose {
float trans_arr[3];
float rot_arr[3];
float trans_std_arr[3];
float rot_std_arr[3];
struct ModelDataRawXYZ {
float x;
float y;
float z;
};
static_assert(sizeof(ModelDataRawXYZ) == sizeof(float)*3);
struct ModelDataRawPlanTimeStep {
ModelDataRawXYZ position;
ModelDataRawXYZ velocity;
ModelDataRawXYZ acceleration;
ModelDataRawXYZ rotation;
ModelDataRawXYZ rotation_rate;
};
static_assert(sizeof(ModelDataRawPlanTimeStep) == sizeof(ModelDataRawXYZ)*5);
struct ModelDataRawPlanPath {
std::array<ModelDataRawPlanTimeStep, TRAJECTORY_SIZE> mean;
std::array<ModelDataRawPlanTimeStep, TRAJECTORY_SIZE> std;
float prob;
};
static_assert(sizeof(ModelDataRawPlanPath) == (sizeof(ModelDataRawPlanTimeStep)*TRAJECTORY_SIZE*2) + sizeof(float));
struct ModelDataRawPlan {
std::array<ModelDataRawPlanPath, PLAN_MHP_N> path;
constexpr const ModelDataRawPlanPath &best_plan() const {
int max_idx = 0;
for (int i = 1; i < path.size(); i++) {
if (path[i].prob > path[max_idx].prob) {
max_idx = i;
}
}
return path[max_idx];
}
};
static_assert(sizeof(ModelDataRawPlan) == sizeof(ModelDataRawPlanPath)*PLAN_MHP_N);
struct ModelDataRawPose {
ModelDataRawXYZ velocity_mean;
ModelDataRawXYZ rotation_mean;
ModelDataRawXYZ velocity_std;
ModelDataRawXYZ rotation_std;
};
static_assert(sizeof(ModelDataRawPose) == sizeof(ModelDataRawXYZ)*4);
struct ModelDataRaw {
float *plan;
ModelDataRawPlan *plan;
float *lane_lines;
float *lane_lines_prob;
float *road_edges;