modeld: update live calibration in main thread (#23235)
* update live calibration in main thread * static ground_from_medmodel_frame * more static * cleanup * no lambda * trigger cipull/23264/head
parent
cf466222f6
commit
ffc2f64f19
|
@ -14,64 +14,46 @@
|
|||
#include "selfdrive/modeld/models/driving.h"
|
||||
|
||||
ExitHandler do_exit;
|
||||
// globals
|
||||
bool live_calib_seen;
|
||||
mat3 cur_transform;
|
||||
std::mutex transform_lock;
|
||||
|
||||
void calibration_thread(bool wide_camera) {
|
||||
util::set_thread_name("calibration");
|
||||
util::set_realtime_priority(50);
|
||||
|
||||
SubMaster sm({"liveCalibration"});
|
||||
|
||||
mat3 update_calibration(cereal::LiveCalibrationData::Reader live_calib, bool wide_camera) {
|
||||
/*
|
||||
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)
|
||||
*/
|
||||
Eigen::Matrix<float, 3, 3> ground_from_medmodel_frame;
|
||||
ground_from_medmodel_frame <<
|
||||
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;
|
||||
-1.84808520e-20, 9.00738606e-04, -4.28751576e-02).finished();
|
||||
|
||||
Eigen::Matrix<float, 3, 3> cam_intrinsics = Eigen::Matrix<float, 3, 3, Eigen::RowMajor>(wide_camera ? ecam_intrinsic_matrix.v : fcam_intrinsic_matrix.v);
|
||||
const mat3 yuv_transform = get_model_yuv_transform();
|
||||
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();
|
||||
|
||||
while (!do_exit) {
|
||||
sm.update(100);
|
||||
if(sm.updated("liveCalibration")) {
|
||||
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];
|
||||
}
|
||||
|
||||
auto camera_frame_from_road_frame = cam_intrinsics * extrinsic_matrix_eigen;
|
||||
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;
|
||||
mat3 transform = {};
|
||||
for (int i=0; i<3*3; i++) {
|
||||
transform.v[i] = warp_matrix(i / 3, i % 3);
|
||||
}
|
||||
mat3 model_transform = matmul3(yuv_transform, transform);
|
||||
std::lock_guard lk(transform_lock);
|
||||
cur_transform = model_transform;
|
||||
live_calib_seen = true;
|
||||
}
|
||||
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;
|
||||
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;
|
||||
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) {
|
||||
void run_model(ModelState &model, VisionIpcClient &vipc_client, bool wide_camera) {
|
||||
// messaging
|
||||
PubMaster pm({"modelV2", "cameraOdometry"});
|
||||
SubMaster sm({"lateralPlan", "roadCameraState"});
|
||||
SubMaster sm({"lateralPlan", "roadCameraState", "liveCalibration"});
|
||||
|
||||
// setup filter to track dropped frames
|
||||
FirstOrderFilter frame_dropped_filter(0., 10., 1. / MODEL_FREQ);
|
||||
|
@ -80,20 +62,22 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) {
|
|||
double last = 0;
|
||||
uint32_t run_count = 0;
|
||||
|
||||
mat3 model_transform = {};
|
||||
bool live_calib_seen = false;
|
||||
|
||||
while (!do_exit) {
|
||||
VisionIpcBufExtra extra = {};
|
||||
VisionBuf *buf = vipc_client.recv(&extra);
|
||||
if (buf == nullptr) continue;
|
||||
|
||||
transform_lock.lock();
|
||||
mat3 model_transform = cur_transform;
|
||||
bool valid = live_calib_seen;
|
||||
transform_lock.unlock();
|
||||
|
||||
// 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);
|
||||
live_calib_seen = true;
|
||||
}
|
||||
|
||||
float vec_desire[DESIRE_LEN] = {0};
|
||||
if (desire >= 0 && desire < DESIRE_LEN) {
|
||||
|
@ -118,8 +102,8 @@ void run_model(ModelState &model, VisionIpcClient &vipc_client) {
|
|||
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,
|
||||
kj::ArrayPtr<const float>(model.output.data(), model.output.size()), valid);
|
||||
posenet_publish(pm, extra.frame_id, vipc_dropped_frames, *model_output, extra.timestamp_eof, valid);
|
||||
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);
|
||||
|
||||
//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;
|
||||
|
@ -138,9 +122,6 @@ int main(int argc, char **argv) {
|
|||
|
||||
bool wide_camera = Hardware::TICI() ? Params().getBool("EnableWideCamera") : false;
|
||||
|
||||
// start calibration thread
|
||||
std::thread thread = std::thread(calibration_thread, 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));
|
||||
|
@ -160,12 +141,10 @@ int main(int argc, char **argv) {
|
|||
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);
|
||||
run_model(model, vipc_client, wide_camera);
|
||||
}
|
||||
|
||||
model_free(&model);
|
||||
LOG("joining calibration thread");
|
||||
thread.join();
|
||||
CL_CHECK(clReleaseContext(context));
|
||||
return 0;
|
||||
}
|
||||
|
|
Loading…
Reference in New Issue