bigmodel (commaai#23684)

* Added wide cam vipc client and bigmodel transform logic

* Added wide_frame to ModelState, should still work normally

* Refactored image input into addImage method, should still work normally

* Updated thneed/compile.cc

* Bigmodel, untested: 44f83118-b375-4d4c-ae12-2017124f0cf4/200

* Have to initialize extra buffer in SNPEModel

* Default paramater value in the wrong place I think

* Move USE_EXTRA to SConscript

* New model: 6c34d59a-acc3-4877-84bd-904c10745ba6/250

* move use extra check to runtime, not on C2

* this is always true

* more C2 checks

* log if frames are out of sync

* more logging on no frame

* store in pointer

* print sof

* add sync logic

* log based on sof difference as well

* keep both models

* less assumptions

* define above thneed

* typo

* simplify

* no need for second client is main is already wide

* more comments update

* no optional reference

* more logging to debug lags

* add to release files

* both defines

* New model: 6831a77f-2574-4bfb-8077-79b0972a2771/950

* Path offset no longer relevant

* Remove duplicate execute

* Moved bigmodel back to big_supercombo.dlc

* add wide vipc stream

* Tici must be tici

* Needs state too

* add wide cam support to model replay

* handle syncing better

* ugh, c2

* print that

* handle ecam lag

* skip first one

* so close

* update refs

Co-authored-by: mitchellgoffpc <mitchellgoffpc@gmail.com>
Co-authored-by: Harald Schafer <harald.the.engineer@gmail.com>
Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
Co-authored-by: Comma Device <device@comma.ai>

Author: 5 people

models/big_supercombo.dlc

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ba3fe3e61853cc1434e3e220f40c8e9d1f1b9bab8458196ba3bea6a10b82c6ed
+size 72718099

models/big_supercombo.onnx

@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bda57c1a66944f5a633ecd739a24d62702c717a234f2fdcc499dfa1d61c3c19e
+size 73147489

release/files_common

@@ -58,6 +58,7 @@ common/transformations/transformations.pyx
 common/api/__init__.py
 
 models/supercombo.dlc
+models/big_supercombo.dlc
 models/dmonitoring_model_q.dlc
 
 release/*

selfdrive/controls/lib/lane_planner.py

@@ -9,17 +9,16 @@
 
 TRAJECTORY_SIZE = 33
 # camera offset is meters from center car to camera
-# model path is in the frame of EON's camera. TICI is 0.1 m away,
-# however the average measured path difference is 0.04 m
+# model path is in the frame of the camera. Empirically 
+# the model knows the difference between TICI and EON
+# so a path offset is not needed
+PATH_OFFSET = 0.00
 if EON:
   CAMERA_OFFSET = -0.06
-  PATH_OFFSET = 0.0
 elif TICI:
   CAMERA_OFFSET = 0.04
-  PATH_OFFSET = 0.04
 else:
   CAMERA_OFFSET = 0.0
-  PATH_OFFSET = 0.0
 
 
 class LanePlanner:

selfdrive/modeld/SConscript

@@ -31,6 +31,9 @@ thneed_src = [
 
 use_thneed = not GetOption('no_thneed')
 
+use_extra = True  # arch == "larch64"
+lenv['CXXFLAGS'].append('-DUSE_EXTRA=true' if use_extra else '-DUSE_EXTRA=false')
+
 if arch == "aarch64" or arch == "larch64":
   libs += ['gsl', 'CB']
   libs += ['gnustl_shared'] if arch == "aarch64" else ['pthread', 'dl']
@@ -65,7 +68,7 @@ common_model = lenv.Object(common_src)
 
 # build thneed model
 if use_thneed and arch in ("aarch64", "larch64"):
-  fn = "../../models/supercombo"
+  fn = "../../models/big_supercombo" if use_extra else "../../models/supercombo"
   compiler = lenv.Program('thneed/compile', ["thneed/compile.cc"]+common_model, LIBS=libs)
   cmd = f"cd {Dir('.').abspath} && {compiler[0].abspath} {fn}.dlc {fn}.thneed --binary"
 

selfdrive/modeld/modeld.cc

@@ -1,6 +1,7 @@
 #include <cstdio>
 #include <cstdlib>
 #include <mutex>
+#include <cmath>
 
 #include <eigen3/Eigen/Dense>
 
@@ -15,42 +16,42 @@
 
 ExitHandler do_exit;
 
-mat3 update_calibration(cereal::LiveCalibrationData::Reader live_calib, bool wide_camera) {
+mat3 update_calibration(Eigen::Matrix<float, 3, 4> &extrinsics, bool wide_camera, bool bigmodel_frame) {
   /*
      import numpy as np
      from common.transformations.model import medmodel_frame_from_road_frame
      medmodel_frame_from_ground = medmodel_frame_from_road_frame[:, (0, 1, 3)]
      ground_from_medmodel_frame = np.linalg.inv(medmodel_frame_from_ground)
   */
-  static const auto ground_from_medmodel_frame = (Eigen::Matrix<float, 3, 3>() << 
-    0.00000000e+00, 0.00000000e+00, 1.00000000e+00,
+  static const auto ground_from_medmodel_frame = (Eigen::Matrix<float, 3, 3>() <<
+     0.00000000e+00, 0.00000000e+00, 1.00000000e+00,
     -1.09890110e-03, 0.00000000e+00, 2.81318681e-01,
     -1.84808520e-20, 9.00738606e-04, -4.28751576e-02).finished();
 
+  static const auto ground_from_sbigmodel_frame = (Eigen::Matrix<float, 3, 3>() <<
+     0.00000000e+00,  7.31372216e-19,  1.00000000e+00,
+    -2.19780220e-03,  4.11497335e-19,  5.62637363e-01,
+    -5.46146580e-20,  1.80147721e-03, -2.73464241e-01).finished();
+
   static const auto cam_intrinsics = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>(wide_camera ? ecam_intrinsic_matrix.v : fcam_intrinsic_matrix.v);
   static const mat3 yuv_transform = get_model_yuv_transform();
 
-  auto extrinsic_matrix = live_calib.getExtrinsicMatrix();
-  Eigen::Matrix<float, 3, 4> extrinsic_matrix_eigen;
-  for (int i = 0; i < 4*3; i++) {
-    extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i];
-  }
-
-  auto camera_frame_from_road_frame = cam_intrinsics * extrinsic_matrix_eigen;
+  auto ground_from_model_frame = bigmodel_frame ? ground_from_sbigmodel_frame : ground_from_medmodel_frame;
+  auto camera_frame_from_road_frame = cam_intrinsics * extrinsics;
   Eigen::Matrix<float, 3, 3> camera_frame_from_ground;
   camera_frame_from_ground.col(0) = camera_frame_from_road_frame.col(0);
   camera_frame_from_ground.col(1) = camera_frame_from_road_frame.col(1);
   camera_frame_from_ground.col(2) = camera_frame_from_road_frame.col(3);
 
-  auto warp_matrix = camera_frame_from_ground * ground_from_medmodel_frame;
+  auto warp_matrix = camera_frame_from_ground * ground_from_model_frame;
   mat3 transform = {};
   for (int i=0; i<3*3; i++) {
     transform.v[i] = warp_matrix(i / 3, i % 3);
   }
   return matmul3(yuv_transform, transform);
 }
 
-void run_model(ModelState &model, VisionIpcClient &vipc_client, bool wide_camera) {
+void run_model(ModelState &model, VisionIpcClient &vipc_client_main, VisionIpcClient &vipc_client_extra, bool main_wide_camera, bool use_extra, bool use_extra_client) {
   // messaging
   PubMaster pm({"modelV2", "cameraOdometry"});
   SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration"});
@@ -62,20 +63,80 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client, bool wide_camera
   double last = 0;
   uint32_t run_count = 0;
 
-  mat3 model_transform = {};
+  mat3 model_transform_main = {};
+  mat3 model_transform_extra = {};
   bool live_calib_seen = false;
 
+  VisionBuf *buf_main = nullptr;
+  VisionBuf *buf_extra = nullptr;
+
+  VisionIpcBufExtra meta_main = {0};
+  VisionIpcBufExtra meta_extra = {0};
+
   while (!do_exit) {
-    VisionIpcBufExtra extra = {};
-    VisionBuf *buf = vipc_client.recv(&extra);
-    if (buf == nullptr) continue;
+    // TODO: change sync logic to use timestamp start of frame in case camerad skips a frame
+    // log frame id in model packet
+
+    // Keep receiving frames until we are at least 1 frame ahead of previous extra frame
+    while (meta_main.frame_id <= meta_extra.frame_id) {
+      buf_main = vipc_client_main.recv(&meta_main);
+      if (meta_main.frame_id <= meta_extra.frame_id) {
+        LOGE("main camera behind! main: %d (%.5f), extra: %d (%.5f)",
+          meta_main.frame_id, double(meta_main.timestamp_sof) / 1e9,
+          meta_extra.frame_id, double(meta_extra.timestamp_sof) / 1e9);
+      }
+
+      if (buf_main == nullptr)  break;
+    }
+
+    if (buf_main == nullptr) {
+      LOGE("vipc_client_main no frame");
+      continue;
+    }
+
+    if (use_extra_client) {
+      // Keep receiving extra frames until frame id matches main camera
+      do {
+        buf_extra = vipc_client_extra.recv(&meta_extra);
+
+        if (meta_main.frame_id > meta_extra.frame_id) {
+          LOGE("extra camera behind! main: %d (%.5f), extra: %d (%.5f)",
+            meta_main.frame_id, double(meta_main.timestamp_sof) / 1e9,
+            meta_extra.frame_id, double(meta_extra.timestamp_sof) / 1e9);
+        }
+      } while (buf_extra != nullptr && meta_main.frame_id > meta_extra.frame_id);
+
+      if (buf_extra == nullptr) {
+        LOGE("vipc_client_extra no frame");
+        continue;
+      }
+
+      if (meta_main.frame_id != meta_extra.frame_id || std::abs((int64_t)meta_main.timestamp_sof - (int64_t)meta_extra.timestamp_sof) > 10000000ULL) {
+        LOGE("frames out of sync! main: %d (%.5f), extra: %d (%.5f)",
+          meta_main.frame_id, double(meta_main.timestamp_sof) / 1e9,
+          meta_extra.frame_id, double(meta_extra.timestamp_sof) / 1e9);
+      }
+    } else {
+      // Use single camera
+      buf_extra = buf_main;
+      meta_extra = meta_main;
+    }
 
     // TODO: path planner timeout?
     sm.update(0);
     int desire = ((int)sm["lateralPlan"].getLateralPlan().getDesire());
     frame_id = sm["roadCameraState"].getRoadCameraState().getFrameId();
     if (sm.updated("liveCalibration")) {
-      model_transform = update_calibration(sm["liveCalibration"].getLiveCalibration(), wide_camera);
+      auto extrinsic_matrix = sm["liveCalibration"].getLiveCalibration().getExtrinsicMatrix();
+      Eigen::Matrix<float, 3, 4> extrinsic_matrix_eigen;
+      for (int i = 0; i < 4*3; i++) {
+        extrinsic_matrix_eigen(i / 4, i % 4) = extrinsic_matrix[i];
+      }
+
+      model_transform_main = update_calibration(extrinsic_matrix_eigen, main_wide_camera, false);
+      if (use_extra) {
+        model_transform_extra = update_calibration(extrinsic_matrix_eigen, Hardware::TICI(), true);
+      }
       live_calib_seen = true;
     }
 
@@ -85,13 +146,12 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client, bool wide_camera
     }
 
     double mt1 = millis_since_boot();
-    ModelOutput *model_output = model_eval_frame(&model, buf->buf_cl, buf->width, buf->height,
-                                              model_transform, vec_desire);
+    ModelOutput *model_output = model_eval_frame(&model, buf_main, buf_extra, model_transform_main, model_transform_extra, vec_desire);
     double mt2 = millis_since_boot();
     float model_execution_time = (mt2 - mt1) / 1000.0;
 
     // tracked dropped frames
-    uint32_t vipc_dropped_frames = extra.frame_id - last_vipc_frame_id - 1;
+    uint32_t vipc_dropped_frames = meta_main.frame_id - last_vipc_frame_id - 1;
     float frames_dropped = frame_dropped_filter.update((float)std::min(vipc_dropped_frames, 10U));
     if (run_count < 10) { // let frame drops warm up
       frame_dropped_filter.reset(0);
@@ -101,13 +161,13 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client, bool wide_camera
 
     float frame_drop_ratio = frames_dropped / (1 + frames_dropped);
 
-    model_publish(pm, extra.frame_id, frame_id, frame_drop_ratio, *model_output, extra.timestamp_eof, model_execution_time,
+    model_publish(pm, meta_main.frame_id, frame_id, frame_drop_ratio, *model_output, meta_main.timestamp_eof, model_execution_time,
                   kj::ArrayPtr<const float>(model.output.data(), model.output.size()), live_calib_seen);
-    posenet_publish(pm, extra.frame_id, vipc_dropped_frames, *model_output, extra.timestamp_eof, live_calib_seen);
+    posenet_publish(pm, meta_main.frame_id, vipc_dropped_frames, *model_output, meta_main.timestamp_eof, live_calib_seen);
 
     //printf("model process: %.2fms, from last %.2fms, vipc_frame_id %u, frame_id, %u, frame_drop %.3f\n", mt2 - mt1, mt1 - last, extra.frame_id, frame_id, frame_drop_ratio);
     last = mt1;
-    last_vipc_frame_id = extra.frame_id;
+    last_vipc_frame_id = meta_main.frame_id;
   }
 }
 
@@ -120,28 +180,42 @@ int main(int argc, char **argv) {
     assert(ret == 0);
   }
 
-  bool wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false;
+  bool main_wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false;
+  bool use_extra = USE_EXTRA;
+  bool use_extra_client = Hardware::TICI() && use_extra && !main_wide_camera;
 
   // cl init
   cl_device_id device_id = cl_get_device_id(CL_DEVICE_TYPE_DEFAULT);
   cl_context context = CL_CHECK_ERR(clCreateContext(NULL, 1, &device_id, NULL, NULL, &err));
 
   // init the models
   ModelState model;
-  model_init(&model, device_id, context);
+  model_init(&model, device_id, context, use_extra);
   LOGW("models loaded, modeld starting");
 
-  VisionIpcClient vipc_client = VisionIpcClient("camerad", wide_camera ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD, true, device_id, context);
-  while (!do_exit && !vipc_client.connect(false)) {
+  VisionIpcClient vipc_client_main = VisionIpcClient("camerad", main_wide_camera ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD, true, device_id, context);
+  VisionIpcClient vipc_client_extra = VisionIpcClient("camerad", VISION_STREAM_WIDE_ROAD, false, device_id, context);
+
+  while (!do_exit && !vipc_client_main.connect(false)) {
+    util::sleep_for(100);
+  }
+
+  while (!do_exit && use_extra_client && !vipc_client_extra.connect(false)) {
     util::sleep_for(100);
   }
 
   // run the models
   // vipc_client.connected is false only when do_exit is true
-  if (vipc_client.connected) {
-    const VisionBuf *b = &vipc_client.buffers[0];
-    LOGW("connected with buffer size: %d (%d x %d)", b->len, b->width, b->height);
-    run_model(model, vipc_client, wide_camera);
+  if (!do_exit) {
+    const VisionBuf *b = &vipc_client_main.buffers[0];
+    LOGW("connected main cam with buffer size: %d (%d x %d)", b->len, b->width, b->height);
+
+    if (use_extra_client) {
+      const VisionBuf *wb = &vipc_client_extra.buffers[0];
+      LOGW("connected extra cam with buffer size: %d (%d x %d)", wb->len, wb->width, wb->height);
+    }
+
+    run_model(model, vipc_client_main, vipc_client_extra, main_wide_camera, use_extra, use_extra_client);
   }
 
   model_free(&model);

selfdrive/modeld/models/dmonitoring.cc

@@ -166,7 +166,8 @@ DMonitoringResult dmonitoring_eval_frame(DMonitoringModelState* s, void* stream_
   //fclose(dump_yuv_file2);
 
   double t1 = millis_since_boot();
-  s->m->execute(net_input_buf, yuv_buf_len);
+  s->m->addImage(net_input_buf, yuv_buf_len);
+  s->m->execute();
   double t2 = millis_since_boot();
 
   DMonitoringResult ret = {0};

selfdrive/modeld/models/driving.cc

@@ -26,15 +26,19 @@ constexpr const kj::ArrayPtr<const T> to_kj_array_ptr(const std::array<T, size>
   return kj::ArrayPtr(arr.data(), arr.size());
 }
 
-void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
+void model_init(ModelState* s, cl_device_id device_id, cl_context context, bool use_extra) {
   s->frame = new ModelFrame(device_id, context);
+  s->wide_frame = new ModelFrame(device_id, context);
 
 #ifdef USE_THNEED
-  s->m = std::make_unique<ThneedModel>("../../models/supercombo.thneed", &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME);
+  s->m = std::make_unique<ThneedModel>(use_extra ? "../../models/big_supercombo.thneed" : "../../models/supercombo.thneed",
+   &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, use_extra);
 #elif USE_ONNX_MODEL
-  s->m = std::make_unique<ONNXModel>("../../models/supercombo.onnx", &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME);
+  s->m = std::make_unique<ONNXModel>(use_extra ? "../../models/big_supercombo.onnx" : "../../models/supercombo.onnx",
+   &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, use_extra);
 #else
-  s->m = std::make_unique<SNPEModel>("../../models/supercombo.dlc", &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME);
+  s->m = std::make_unique<SNPEModel>(use_extra ? "../../models/big_supercombo.dlc" : "../../models/supercombo.dlc",
+   &s->output[0], NET_OUTPUT_SIZE, USE_GPU_RUNTIME, use_extra);
 #endif
 
 #ifdef TEMPORAL
@@ -52,8 +56,8 @@ void model_init(ModelState* s, cl_device_id device_id, cl_context context) {
 #endif
 }
 
-ModelOutput* model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int height,
-                           const mat3 &transform, float *desire_in) {
+ModelOutput* model_eval_frame(ModelState* s, VisionBuf* buf, VisionBuf* wbuf,
+                              const mat3 &transform, const mat3 &transform_wide, float *desire_in) {
 #ifdef DESIRE
   if (desire_in != NULL) {
     for (int i = 1; i < DESIRE_LEN; i++) {
@@ -70,8 +74,14 @@ ModelOutput* model_eval_frame(ModelState* s, cl_mem yuv_cl, int width, int heigh
 #endif
 
   // 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);
+  auto net_input_buf = s->frame->prepare(buf->buf_cl, buf->width, buf->height, transform, static_cast<cl_mem*>(s->m->getInputBuf()));
+  s->m->addImage(net_input_buf, s->frame->buf_size);
+
+  if (wbuf != nullptr) {
+    auto net_extra_buf = s->wide_frame->prepare(wbuf->buf_cl, wbuf->width, wbuf->height, transform_wide, static_cast<cl_mem*>(s->m->getExtraBuf()));
+    s->m->addExtra(net_extra_buf, s->wide_frame->buf_size);
+  }
+  s->m->execute();
 
   return (ModelOutput*)&s->output;
 }