UI: fade into wide camera (#26203)

* UI: fade into wide camera

* handle routes with no wide calib

* more cleanup

Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>

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] +

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);
   }

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);

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"));
 

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);