Move update_calibration to modeld.py (commaai#29688)

* Move update_calibration to modeld.py

* Updated ref commit

selfdrive/modeld/modeld.py

@@ -13,9 +13,12 @@
 from openpilot.common.params import Params
 from openpilot.common.filter_simple import FirstOrderFilter
 from openpilot.common.realtime import config_realtime_process
+from openpilot.common.transformations.orientation import rot_from_euler
+from openpilot.common.transformations.model import medmodel_frame_from_calib_frame, sbigmodel_frame_from_calib_frame
+from openpilot.common.transformations.camera import view_frame_from_device_frame, tici_fcam_intrinsics, tici_ecam_intrinsics
 from openpilot.selfdrive.modeld.models.commonmodel_pyx import ModelFrame, CLContext, Runtime
 from openpilot.selfdrive.modeld.models.driving_pyx import (
-  PublishState, create_model_msg, create_pose_msg, update_calibration,
+  PublishState, create_model_msg, create_pose_msg,
   FEATURE_LEN, HISTORY_BUFFER_LEN, DESIRE_LEN, TRAFFIC_CONVENTION_LEN, NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN,
   OUTPUT_SIZE, NET_OUTPUT_SIZE, MODEL_FREQ)
 
@@ -27,20 +30,16 @@
 
 MODEL_PATH = str(Path(__file__).parent / f"models/supercombo.{'thneed' if USE_THNEED else 'onnx'}")
 
-# NOTE: numpy matmuls don't seem to perfectly match eigen matmuls so the ref test fails, but we should switch to the np version after checking compare_runtime
-# from common.transformations.orientation import rot_from_euler
-# from common.transformations.model import medmodel_frame_from_calib_frame, sbigmodel_frame_from_calib_frame
-# from common.transformations.camera import view_frame_from_device_frame, tici_fcam_intrinsics, tici_ecam_intrinsics
-# calib_from_medmodel = np.linalg.inv(medmodel_frame_from_calib_frame[:, :3])
-# calib_from_sbigmodel = np.linalg.inv(sbigmodel_frame_from_calib_frame[:, :3])
-#
-# def update_calibration(device_from_calib_euler: np.ndarray, wide_camera: bool, bigmodel_frame: bool) -> np.ndarray:
-#   cam_intrinsics = tici_ecam_intrinsics if wide_camera else tici_fcam_intrinsics
-#   calib_from_model = calib_from_sbigmodel if bigmodel_frame else calib_from_medmodel
-#   device_from_calib = rot_from_euler(device_from_calib_euler)
-#   camera_from_calib = cam_intrinsics @ view_frame_from_device_frame @ device_from_calib
-#   warp_matrix: np.ndarray = camera_from_calib @ calib_from_model
-#   return warp_matrix
+calib_from_medmodel = np.linalg.inv(medmodel_frame_from_calib_frame[:, :3])
+calib_from_sbigmodel = np.linalg.inv(sbigmodel_frame_from_calib_frame[:, :3])
+
+def update_calibration(device_from_calib_euler: np.ndarray, wide_camera: bool, bigmodel_frame: bool) -> np.ndarray:
+  cam_intrinsics = tici_ecam_intrinsics if wide_camera else tici_fcam_intrinsics
+  calib_from_model = calib_from_sbigmodel if bigmodel_frame else calib_from_medmodel
+  device_from_calib = rot_from_euler(device_from_calib_euler)
+  camera_from_calib = cam_intrinsics @ view_frame_from_device_frame @ device_from_calib
+  warp_matrix: np.ndarray = camera_from_calib @ calib_from_model
+  return warp_matrix.astype(np.float32)
 
 class FrameMeta:
   frame_id: int = 0

selfdrive/modeld/models/driving.cc

@@ -1,59 +1,7 @@
 #include "selfdrive/modeld/models/driving.h"
 
-#include <fcntl.h>
-#include <unistd.h>
-
-#include <cassert>
 #include <cstring>
 
-#include <eigen3/Eigen/Dense>
-
-#include "common/clutil.h"
-#include "common/params.h"
-#include "common/timing.h"
-#include "common/swaglog.h"
-#include "common/transformations/orientation.hpp"
-
-
-mat3 update_calibration(float *device_from_calib_euler, bool wide_camera, bool bigmodel_frame) {
-  /*
-     import numpy as np
-     from common.transformations.model import medmodel_frame_from_calib_frame
-     medmodel_frame_from_calib_frame = medmodel_frame_from_calib_frame[:, :3]
-     calib_from_smedmodel_frame = np.linalg.inv(medmodel_frame_from_calib_frame)
-  */
-  static const auto calib_from_medmodel = (Eigen::Matrix<float, 3, 3>() <<
-     0.00000000e+00, 0.00000000e+00, 1.00000000e+00,
-     1.09890110e-03, 0.00000000e+00, -2.81318681e-01,
-    -2.25466395e-20, 1.09890110e-03, -5.23076923e-02).finished();
-
-  static const auto calib_from_sbigmodel = (Eigen::Matrix<float, 3, 3>() <<
-     0.00000000e+00,  7.31372216e-19,  1.00000000e+00,
-     2.19780220e-03,  4.11497335e-19, -5.62637363e-01,
-    -6.66298828e-20,  2.19780220e-03, -3.33626374e-01).finished();
-
-  static const auto view_from_device = (Eigen::Matrix<float, 3, 3>() <<
-     0.0,  1.0,  0.0,
-     0.0,  0.0,  1.0,
-     1.0,  0.0,  0.0).finished();
-
-  Eigen::Vector3d device_from_calib_euler_vec;
-  for (int i=0; i<3; i++) {
-    device_from_calib_euler_vec(i) = device_from_calib_euler[i];
-  }
-
-  const auto cam_intrinsics = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>(wide_camera ? ECAM_INTRINSIC_MATRIX.v : FCAM_INTRINSIC_MATRIX.v);
-  Eigen::Matrix<float, 3, 3, Eigen::RowMajor>  device_from_calib = euler2rot(device_from_calib_euler_vec).cast <float> ();
-  auto calib_from_model = bigmodel_frame ? calib_from_sbigmodel : calib_from_medmodel;
-  auto camera_from_calib = cam_intrinsics * view_from_device * device_from_calib;
-  auto warp_matrix = camera_from_calib * calib_from_model;
-
-  mat3 transform = {};
-  for (int i=0; i<3*3; i++) {
-    transform.v[i] = warp_matrix(i / 3, i % 3);
-  }
-  return transform;
-}
 
 void fill_lead(cereal::ModelDataV2::LeadDataV3::Builder lead, const ModelOutputLeads &leads, int t_idx, float prob_t) {
   std::array<float, LEAD_TRAJ_LEN> lead_t = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0};

selfdrive/modeld/models/driving.h

@@ -4,7 +4,6 @@
 #include <memory>
 
 #include "cereal/messaging/messaging.h"
-#include "common/mat.h"
 #include "common/modeldata.h"
 #include "common/util.h"
 #include "selfdrive/modeld/models/nav.h"
@@ -250,7 +249,6 @@ struct PublishState {
   std::array<float, 3> prev_brake_3ms2_probs = {};
 };
 
-mat3 update_calibration(float *device_from_calib_euler, bool wide_camera, bool bigmodel_frame);
 void fill_model_msg(MessageBuilder &msg, float *net_output_data, PublishState &ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
                     uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, const bool nav_enabled, const bool valid);
 void fill_pose_msg(MessageBuilder &msg, float *net_outputs, uint32_t vipc_frame_id, uint32_t vipc_dropped_frames, uint64_t timestamp_eof, const bool valid);

selfdrive/modeld/models/driving.pxd

@@ -23,6 +23,5 @@ cdef extern from "selfdrive/modeld/models/driving.h":
   cdef int MODEL_FREQ
   cdef struct PublishState: pass
 
-  mat3 update_calibration(float *, bool, bool)
   void fill_model_msg(MessageBuilder, float *, PublishState, uint32_t, uint32_t, uint32_t, float, uint64_t, uint64_t, float, bool, bool)
   void fill_pose_msg(MessageBuilder, float *, uint32_t, uint32_t, uint64_t, bool)

selfdrive/modeld/models/driving_pyx.pyx

@@ -13,7 +13,7 @@ from .driving cimport FEATURE_LEN as CPP_FEATURE_LEN, HISTORY_BUFFER_LEN as CPP_
                       NAV_FEATURE_LEN as CPP_NAV_FEATURE_LEN, NAV_INSTRUCTION_LEN as CPP_NAV_INSTRUCTION_LEN, \
                       OUTPUT_SIZE as CPP_OUTPUT_SIZE, NET_OUTPUT_SIZE as CPP_NET_OUTPUT_SIZE, MODEL_FREQ as CPP_MODEL_FREQ
 from .driving cimport MessageBuilder, PublishState as cppPublishState
-from .driving cimport fill_model_msg, fill_pose_msg, update_calibration as cpp_update_calibration
+from .driving cimport fill_model_msg, fill_pose_msg
 
 FEATURE_LEN = CPP_FEATURE_LEN
 HISTORY_BUFFER_LEN = CPP_HISTORY_BUFFER_LEN
@@ -29,13 +29,6 @@ MODEL_FREQ = CPP_MODEL_FREQ
 cdef class PublishState:
   cdef cppPublishState state
 
-def update_calibration(float[:] device_from_calib_euler, bool wide_camera, bool bigmodel_frame):
-  cdef mat3 result = cpp_update_calibration(&device_from_calib_euler[0], wide_camera, bigmodel_frame)
-  np_result = np.empty(9, dtype=np.float32)
-  cdef float[:] np_result_view = np_result
-  memcpy(&np_result_view[0], &result.v[0], 9*sizeof(float))
-  return np_result.reshape(3, 3)
-
 def create_model_msg(float[:] model_outputs, PublishState ps, uint32_t vipc_frame_id, uint32_t vipc_frame_id_extra, uint32_t frame_id, float frame_drop,
                      uint64_t timestamp_eof, uint64_t timestamp_llk, float model_execution_time, bool nav_enabled, bool valid):
   cdef MessageBuilder msg

selfdrive/test/process_replay/model_replay_ref_commit

@@ -1 +1 @@
-377ed362faa9410a8b81d50978d74ee60b8e6f6a
+4f59f5945633f7a6e54e6f0fcd8ecf0baafa28cd