diff --git a/selfdrive/modeld/modeld.py b/selfdrive/modeld/modeld.py index 61edcfb006..1f30f52ed0 100755 --- a/selfdrive/modeld/modeld.py +++ b/selfdrive/modeld/modeld.py @@ -13,9 +13,12 @@ from openpilot.system.swaglog import cloudlog 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 @@ else: 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 diff --git a/selfdrive/modeld/models/driving.cc b/selfdrive/modeld/models/driving.cc index b4b15c2970..0a7f0c949d 100644 --- a/selfdrive/modeld/models/driving.cc +++ b/selfdrive/modeld/models/driving.cc @@ -1,59 +1,7 @@ #include "selfdrive/modeld/models/driving.h" -#include -#include - -#include #include -#include - -#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() << - 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() << - 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() << - 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(wide_camera ? ECAM_INTRINSIC_MATRIX.v : FCAM_INTRINSIC_MATRIX.v); - Eigen::Matrix device_from_calib = euler2rot(device_from_calib_euler_vec).cast (); - 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 lead_t = {0.0, 2.0, 4.0, 6.0, 8.0, 10.0}; diff --git a/selfdrive/modeld/models/driving.h b/selfdrive/modeld/models/driving.h index 39083dd6a9..f13cd155f1 100644 --- a/selfdrive/modeld/models/driving.h +++ b/selfdrive/modeld/models/driving.h @@ -4,7 +4,6 @@ #include #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 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); diff --git a/selfdrive/modeld/models/driving.pxd b/selfdrive/modeld/models/driving.pxd index 1b815267db..0de67208e6 100644 --- a/selfdrive/modeld/models/driving.pxd +++ b/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) diff --git a/selfdrive/modeld/models/driving_pyx.pyx b/selfdrive/modeld/models/driving_pyx.pyx index be96df1658..b98a8f3ff1 100644 --- a/selfdrive/modeld/models/driving_pyx.pyx +++ b/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 diff --git a/selfdrive/test/process_replay/model_replay_ref_commit b/selfdrive/test/process_replay/model_replay_ref_commit index e101afb227..e80315133f 100644 --- a/selfdrive/test/process_replay/model_replay_ref_commit +++ b/selfdrive/test/process_replay/model_replay_ref_commit @@ -1 +1 @@ -377ed362faa9410a8b81d50978d74ee60b8e6f6a +4f59f5945633f7a6e54e6f0fcd8ecf0baafa28cd