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 2 (#22554)

Browse Source 
* 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
old-commit-hash: a7f36c9daf
commatwo_master
Greg Hogan 4 years ago committed by GitHub
parent 618e4c64e1
commit 0e89533b19
3 changed files with 133 additions and 39 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. 18
      selfdrive/common/modeldata.h
  2. 102
      selfdrive/modeld/models/driving.cc
  3. 52
      selfdrive/modeld/models/driving.h

18
selfdrive/common/modeldata.h
Unescape 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
Unescape 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);

52
selfdrive/modeld/models/driving.h
Unescape 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 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 {
float trans_arr[3];
float rot_arr[3];
float trans_std_arr[3];
float rot_std_arr[3];
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;

Write Preview
Loading…
Cancel
Save