mawei/dragonpilot
1
0
Fork 0
mirror of https://github.com/dragonpilot/dragonpilot.git synced 2026-06-24 23:42:05 +08:00
Code Issues Packages Projects Releases Wiki Activity

Move update_calibration to modeld.py (#29688)

Browse Source 
* Move update_calibration to modeld.py

* Updated ref commit
This commit is contained in:
Mitchell Goff
2023-08-30 14:12:52 -07:00
committed by GitHub
parent 78a4bff4c0
commit 1eb60516cb
6 changed files with 16 additions and 79 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/modeld/modeld.py
+14 -15
View File
@@ -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
selfdrive/modeld/models/driving.cc
-52
View File
@@ -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
-2
View File
@@ -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
-1
View File
@@ -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
+1 -8
View File
@@ -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
View File
@@ -1 +1 @@
377ed362faa9410a8b81d50978d74ee60b8e6f6a
4f59f5945633f7a6e54e6f0fcd8ecf0baafa28cd