From d25ab3e3663f1e8a7b08348692d6555fac84e73b Mon Sep 17 00:00:00 2001 From: HaraldSchafer Date: Fri, 21 Oct 2022 01:29:58 -0700 Subject: [PATCH] Wide transform model (#26165) * d8501d20-bb59-4193-aa82-82b2737dedd6/449 f78009d1-2184-4136-9fbc-1c5de55d460d/700 * Change model outs * Add cereal * Publish transform * d8501d20-bb59-4193-aa82-82b2737dedd6/449 f78009d1-2184-4136-9fbc-1c5de55d460d/700 * Bump cereal to master * Bump cereal to master * Update model ref --- cereal | 2 +- selfdrive/modeld/models/driving.cc | 5 +++ selfdrive/modeld/models/driving.h | 41 ++++--------------- selfdrive/modeld/models/supercombo.onnx | 4 +- .../process_replay/model_replay_ref_commit | 2 +- 5 files changed, 17 insertions(+), 37 deletions(-) diff --git a/cereal b/cereal index 1e3dd70a39..38133307b2 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit 1e3dd70a391bc1bbe437d3eea8be30947f929a75 +Subproject commit 38133307b2e6036e76684b39878e79212e545e06 diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index 8b70589036..cc4a83de62 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -363,14 +363,19 @@ void posenet_publish(PubMaster &pm, uint32_t vipc_frame_id, uint32_t vipc_droppe MessageBuilder msg; const auto &v_mean = net_outputs.pose.velocity_mean; const auto &r_mean = net_outputs.pose.rotation_mean; + const auto &t_mean = net_outputs.wide_from_device_euler.mean; const auto &v_std = net_outputs.pose.velocity_std; const auto &r_std = net_outputs.pose.rotation_std; + const auto &t_std = net_outputs.wide_from_device_euler.std; auto posenetd = msg.initEvent(valid && (vipc_dropped_frames < 1)).initCameraOdometry(); posenetd.setTrans({v_mean.x, v_mean.y, v_mean.z}); posenetd.setRot({r_mean.x, r_mean.y, r_mean.z}); + posenetd.setWideFromDeviceEuler({t_mean.x, t_mean.y, t_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.setWideFromDeviceEulerStd({exp(t_std.x), exp(t_std.y), exp(t_std.z)}); + posenetd.setTimestampEof(timestamp_eof); posenetd.setFrameId(vipc_frame_id); diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 8bb84d0245..64218467df 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -28,7 +28,6 @@ constexpr int BLINKER_LEN = 6; constexpr int META_STRIDE = 7; constexpr int PLAN_MHP_N = 5; -constexpr int STOP_LINE_MHP_N = 3; constexpr int LEAD_MHP_N = 2; constexpr int LEAD_TRAJ_LEN = 6; @@ -151,36 +150,6 @@ struct ModelOutputLeads { }; static_assert(sizeof(ModelOutputLeads) == (sizeof(ModelOutputLeadPrediction)*LEAD_MHP_N) + (sizeof(float)*LEAD_MHP_SELECTION)); -struct ModelOutputStopLineElement { - ModelOutputXYZ position; - ModelOutputXYZ rotation; - float speed; - float time; -}; -static_assert(sizeof(ModelOutputStopLineElement) == (sizeof(ModelOutputXYZ)*2 + sizeof(float)*2)); - -struct ModelOutputStopLinePrediction { - ModelOutputStopLineElement mean; - ModelOutputStopLineElement std; - float prob; -}; -static_assert(sizeof(ModelOutputStopLinePrediction) == (sizeof(ModelOutputStopLineElement)*2 + sizeof(float))); - -struct ModelOutputStopLines { - std::array prediction; - float prob; - - constexpr const ModelOutputStopLinePrediction &get_best_prediction(int t_idx) const { - int max_idx = 0; - for (int i = 1; i < prediction.size(); i++) { - if (prediction[i].prob > prediction[max_idx].prob) { - max_idx = i; - } - } - return prediction[max_idx]; - } -}; -static_assert(sizeof(ModelOutputStopLines) == (sizeof(ModelOutputStopLinePrediction)*STOP_LINE_MHP_N) + sizeof(float)); struct ModelOutputPose { ModelOutputXYZ velocity_mean; @@ -190,6 +159,12 @@ struct ModelOutputPose { }; static_assert(sizeof(ModelOutputPose) == sizeof(ModelOutputXYZ)*4); +struct ModelOutputWideFromDeviceEuler { + ModelOutputXYZ mean; + ModelOutputXYZ std; +}; +static_assert(sizeof(ModelOutputWideFromDeviceEuler) == sizeof(ModelOutputXYZ)*2); + struct ModelOutputDisengageProb { float gas_disengage; float brake_disengage; @@ -245,9 +220,9 @@ struct ModelOutput { const ModelOutputLaneLines lane_lines; const ModelOutputRoadEdges road_edges; const ModelOutputLeads leads; - const ModelOutputStopLines stop_lines; const ModelOutputMeta meta; const ModelOutputPose pose; + const ModelOutputWideFromDeviceEuler wide_from_device_euler; }; constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float); @@ -257,7 +232,7 @@ constexpr int OUTPUT_SIZE = sizeof(ModelOutput) / sizeof(float); #else constexpr int TEMPORAL_SIZE = 0; #endif -constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + FEATURE_LEN; +constexpr int NET_OUTPUT_SIZE = OUTPUT_SIZE + FEATURE_LEN + 2; // TODO: convert remaining arrays to std::array and update model runners struct ModelState { diff --git a/selfdrive/modeld/models/supercombo.onnx b/selfdrive/modeld/models/supercombo.onnx index 59b8883d2a..21602ea70a 100644 --- a/selfdrive/modeld/models/supercombo.onnx +++ b/selfdrive/modeld/models/supercombo.onnx @@ -1,3 +1,3 @@ version https://git-lfs.github.com/spec/v1 -oid sha256:022a830c39267f378f45204682060c93e3aa304bbd8cfa6b2dfe4fa8f419102d -size 56972617 +oid sha256:9c119aa811a929b3a442dbe8e462a9d88e8bfb9f5809a4e737687fb7380d7e05 +size 45922882 diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index 5bb045ec29..b5fd11fee8 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -865885fc49b2766326568e5cc7ec06be8a3f6fad +46cb9c522e942d8b25deac90ffdee89efff60ce9