diff --git a/selfdrive/locationd/calibrationd.py b/selfdrive/locationd/calibrationd.py index 4e996bc3b9..1c68eb67bd 100755 --- a/selfdrive/locationd/calibrationd.py +++ b/selfdrive/locationd/calibrationd.py @@ -66,7 +66,6 @@ class Calibrator: # Read saved calibration params = Params() calibration_params = params.get("CalibrationParams") - self.wide_camera = params.get_bool('WideCameraOnly') rpy_init = RPY_INIT wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT valid_blocks = 0 @@ -166,10 +165,7 @@ class Calibrator: self.old_rpy_weight = min(0.0, self.old_rpy_weight - 1/SMOOTH_CYCLES) straight_and_fast = ((self.v_ego > MIN_SPEED_FILTER) and (trans[0] > MIN_SPEED_FILTER) and (abs(rot[2]) < MAX_YAW_RATE_FILTER)) - if self.wide_camera: - angle_std_threshold = 4*MAX_VEL_ANGLE_STD - else: - angle_std_threshold = MAX_VEL_ANGLE_STD + angle_std_threshold = MAX_VEL_ANGLE_STD certain_if_calib = ((np.arctan2(trans_std[1], trans[0]) < angle_std_threshold) or (self.valid_blocks < INPUTS_NEEDED)) if not (straight_and_fast and certain_if_calib): @@ -185,7 +181,6 @@ class Calibrator: new_wide_from_device_euler = np.array(wide_from_device_euler) else: new_wide_from_device_euler = WIDE_FROM_DEVICE_EULER_INIT - self.rpys[self.block_idx] = (self.idx*self.rpys[self.block_idx] + (BLOCK_SIZE - self.idx) * new_rpy) / float(BLOCK_SIZE) self.wide_from_device_eulers[self.block_idx] = (self.idx*self.wide_from_device_eulers[self.block_idx] + diff --git a/selfdrive/ui/qt/onroad.cc b/selfdrive/ui/qt/onroad.cc index 1f677fc92d..346cb3ab96 100644 --- a/selfdrive/ui/qt/onroad.cc +++ b/selfdrive/ui/qt/onroad.cc @@ -100,10 +100,6 @@ void OnroadWindow::offroadTransition(bool offroad) { #endif alerts->updateAlert({}, bg); - - // update stream type - bool wide_cam = Params().getBool("WideCameraOnly"); - nvg->setStreamType(wide_cam ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD); } void OnroadWindow::paintEvent(QPaintEvent *event) { @@ -232,8 +228,10 @@ void AnnotatedCameraWidget::updateState(const UIState &s) { setProperty("rightHandDM", sm["driverMonitoringState"].getDriverMonitoringState().getIsRHD()); } + setStreamType(s.scene.wide_cam ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD); if (s.scene.calibration_valid) { - CameraWidget::updateCalibration(s.scene.view_from_calib); + auto calib = s.scene.wide_cam ? s.scene.view_from_wide_calib : s.scene.view_from_calib; + CameraWidget::updateCalibration(calib); } else { CameraWidget::updateCalibration(DEFAULT_CALIBRATION); } diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc index 4f46497776..13225414d0 100644 --- a/selfdrive/ui/qt/widgets/cameraview.cc +++ b/selfdrive/ui/qt/widgets/cameraview.cc @@ -188,12 +188,17 @@ void CameraWidget::updateFrameMat() { if (stream_type == VISION_STREAM_DRIVER) { frame_mat = get_driver_view_transform(w, h, stream_width, stream_height); } else { - intrinsic_matrix = (stream_type == VISION_STREAM_WIDE_ROAD) ? ecam_intrinsic_matrix : fcam_intrinsic_matrix; - zoom = (stream_type == VISION_STREAM_WIDE_ROAD) ? 2.5 : 1.1; - // Project point at "infinity" to compute x and y offsets // to ensure this ends up in the middle of the screen + // for narrow come and a little lower for wide cam. // TODO: use proper perspective transform? + if (stream_type == VISION_STREAM_WIDE_ROAD) { + intrinsic_matrix = ecam_intrinsic_matrix; + zoom = 2.0; + } else { + intrinsic_matrix = fcam_intrinsic_matrix; + zoom = 1.1; + } const vec3 inf = {{1000., 0., 0.}}; const vec3 Ep = matvecmul3(calibration, inf); const vec3 Kep = matvecmul3(intrinsic_matrix, Ep); diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc index 1a6027bf7f..d1d72b4dad 100644 --- a/selfdrive/ui/ui.cc +++ b/selfdrive/ui/ui.cc @@ -23,8 +23,8 @@ static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, const QRectF clip_region{-margin, -margin, s->fb_w + 2 * margin, s->fb_h + 2 * margin}; const vec3 pt = (vec3){{in_x, in_y, in_z}}; - const vec3 Ep = matvecmul3(s->scene.view_from_calib, pt); - const vec3 KEp = matvecmul3(s->wide_camera ? ecam_intrinsic_matrix : fcam_intrinsic_matrix, Ep); + const vec3 Ep = matvecmul3(s->scene.wide_cam ? s->scene.view_from_wide_calib : s->scene.view_from_calib, pt); + const vec3 KEp = matvecmul3(s->scene.wide_cam ? ecam_intrinsic_matrix : fcam_intrinsic_matrix, Ep); // Project. QPointF point = s->car_space_transform.map(QPointF{KEp.v[0] / KEp.v[2], KEp.v[1] / KEp.v[2]}); @@ -121,17 +121,23 @@ static void update_state(UIState *s) { if (sm.updated("liveCalibration")) { auto rpy_list = sm["liveCalibration"].getLiveCalibration().getRpyCalib(); + auto wfde_list = sm["liveCalibration"].getLiveCalibration().getWideFromDeviceEuler(); Eigen::Vector3d rpy; - rpy << rpy_list[0], rpy_list[1], rpy_list[2]; + Eigen::Vector3d wfde; + if (rpy_list.size() == 3) rpy << rpy_list[0], rpy_list[1], rpy_list[2]; + if (wfde_list.size() == 3) wfde << wfde_list[0], wfde_list[1], wfde_list[2]; Eigen::Matrix3d device_from_calib = euler2rot(rpy); + Eigen::Matrix3d wide_from_device = euler2rot(wfde); Eigen::Matrix3d view_from_device; view_from_device << 0,1,0, 0,0,1, 1,0,0; Eigen::Matrix3d view_from_calib = view_from_device * device_from_calib; + Eigen::Matrix3d view_from_wide_calib = view_from_device * wide_from_device * device_from_calib ; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { scene.view_from_calib.v[i*3 + j] = view_from_calib(i,j); + scene.view_from_wide_calib.v[i*3 + j] = view_from_wide_calib(i,j); } } scene.calibration_valid = sm["liveCalibration"].getLiveCalibration().getCalStatus() == 1; @@ -167,6 +173,17 @@ static void update_state(UIState *s) { scene.light_sensor = std::max(100.0f - scale * sm["wideRoadCameraState"].getWideRoadCameraState().getExposureValPercent(), 0.0f); } scene.started = sm["deviceState"].getDeviceState().getStarted() && scene.ignition; + + if (sm.updated("carState")) { + float v_ego = sm["carState"].getCarState().getVEgo(); + // TODO: support replays without ecam by using fcam + // Wide or narrow cam dependent on speed + if ((v_ego < 10) || s->wide_cam_only) { + scene.wide_cam = true; + } else if (v_ego > 15) { + scene.wide_cam = false; + } + } } void ui_update_params(UIState *s) { @@ -197,7 +214,7 @@ void UIState::updateStatus() { if (scene.started) { status = STATUS_DISENGAGED; scene.started_frame = sm->frame; - wide_camera = Params().getBool("WideCameraOnly"); + wide_cam_only = Params().getBool("WideCameraOnly"); } started_prev = scene.started; emit offroadTransition(!scene.started); @@ -219,7 +236,7 @@ UIState::UIState(QObject *parent) : QObject(parent) { }); Params params; - wide_camera = params.getBool("WideCameraOnly"); + wide_cam_only = params.getBool("WideCameraOnly"); prime_type = std::atoi(params.get("PrimeType").c_str()); language = QString::fromStdString(params.get("LanguageSetting")); diff --git a/selfdrive/ui/ui.h b/selfdrive/ui/ui.h index f60c26b59a..5a51dda8f8 100644 --- a/selfdrive/ui/ui.h +++ b/selfdrive/ui/ui.h @@ -87,7 +87,9 @@ const QColor bg_colors [] = { typedef struct UIScene { bool calibration_valid = false; + bool wide_cam = true; mat3 view_from_calib = DEFAULT_CALIBRATION; + mat3 view_from_wide_calib = DEFAULT_CALIBRATION; cereal::PandaState::PandaType pandaType; // modelV2 @@ -130,7 +132,7 @@ public: QString language; QTransform car_space_transform; - bool wide_camera; + bool wide_cam_only; signals: void uiUpdate(const UIState &s);