diff --git a/selfdrive/modeld/modeld.cc b/selfdrive/modeld/modeld.cc index 03b487bbbfdfa3..0facf3fa0b21bc 100644 --- a/selfdrive/modeld/modeld.cc +++ b/selfdrive/modeld/modeld.cc @@ -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 ground_from_medmodel_frame; - ground_from_medmodel_frame << + static const auto ground_from_medmodel_frame = (Eigen::Matrix() << 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 cam_intrinsics = Eigen::Matrix(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(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 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 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 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 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(model.output.data(), model.output.size()), valid); - posenet_publish(pm, extra.frame_id, vipc_dropped_frames, *model_output, extra.timestamp_eof, valid); + kj::ArrayPtr(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; }