ui: refactor CameraView to serve as a generic vision stream display c…

…lass (commaai#33457)

refactor transform

selfdrive/ui/qt/offroad/driverview.cc

@@ -7,7 +7,7 @@
 
 const int FACE_IMG_SIZE = 130;
 
-DriverViewWindow::DriverViewWindow(QWidget* parent) : CameraWidget("camerad", VISION_STREAM_DRIVER, true, parent) {
+DriverViewWindow::DriverViewWindow(QWidget* parent) : CameraWidget("camerad", VISION_STREAM_DRIVER, parent) {
   face_img = loadPixmap("../assets/img_driver_face_static.png", {FACE_IMG_SIZE, FACE_IMG_SIZE});
   QObject::connect(this, &CameraWidget::clicked, this, &DriverViewWindow::done);
   QObject::connect(device(), &Device::interactiveTimeout, this, [this]() {
@@ -75,3 +75,15 @@ void DriverViewWindow::paintGL() {
   p.setOpacity(face_detected ? 1.0 : 0.2);
   p.drawPixmap(img_x, img_y, face_img);
 }
+
+mat4 DriverViewWindow::calcFrameMatrix() {
+  const float driver_view_ratio = 2.0;
+  const float yscale = stream_height * driver_view_ratio / stream_width;
+  const float xscale = yscale * glHeight() / glWidth() * stream_width / stream_height;
+  return mat4{{
+    xscale,  0.0, 0.0, 0.0,
+    0.0,  yscale, 0.0, 0.0,
+    0.0,  0.0, 1.0, 0.0,
+    0.0,  0.0, 0.0, 1.0,
+  }};
+}

selfdrive/ui/qt/offroad/driverview.h

@@ -12,6 +12,7 @@ class DriverViewWindow : public CameraWidget {
   void done();
 
 protected:
+  mat4 calcFrameMatrix() override;
   void showEvent(QShowEvent *event) override;
   void hideEvent(QHideEvent *event) override;
   void paintGL() override;

selfdrive/ui/qt/onroad/annotated_camera.cc

@@ -10,7 +10,8 @@
 #include "selfdrive/ui/qt/util.h"
 
 // Window that shows camera view and variety of info drawn on top
-AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget* parent) : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, true, parent) {
+AnnotatedCameraWidget::AnnotatedCameraWidget(VisionStreamType type, QWidget *parent)
+    : fps_filter(UI_FREQ, 3, 1. / UI_FREQ), CameraWidget("camerad", type, parent) {
   pm = std::make_unique<PubMaster>(std::vector<const char*>{"uiDebug"});
 
   main_layout = new QVBoxLayout(this);
@@ -125,22 +126,54 @@ void AnnotatedCameraWidget::initializeGL() {
   setBackgroundColor(bg_colors[STATUS_DISENGAGED]);
 }
 
-void AnnotatedCameraWidget::updateFrameMat() {
-  CameraWidget::updateFrameMat();
-  UIState *s = uiState();
+mat4 AnnotatedCameraWidget::calcFrameMatrix() {
+  // 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?
+
+  // Select intrinsic matrix and calibration based on camera type
+  auto *s = uiState();
+  bool wide_cam = active_stream_type == VISION_STREAM_WIDE_ROAD;
+  const auto &intrinsic_matrix = wide_cam ? ECAM_INTRINSIC_MATRIX : FCAM_INTRINSIC_MATRIX;
+  const auto &calibration = wide_cam ? s->scene.view_from_wide_calib : s->scene.view_from_calib;
+
+   // Compute the calibration transformation matrix
+  const auto calib_transform = intrinsic_matrix * calibration;
+
+  float zoom = wide_cam ? 2.0 : 1.1;
+  Eigen::Vector3f inf(1000., 0., 0.);
+  auto Kep = calib_transform * inf;
+
   int w = width(), h = height();
+  float center_x = intrinsic_matrix(0, 2);
+  float center_y = intrinsic_matrix(1, 2);
 
-  s->fb_w = w;
-  s->fb_h = h;
+  float max_x_offset = center_x * zoom - w / 2 - 5;
+  float max_y_offset = center_y * zoom - h / 2 - 5;
+  float x_offset = std::clamp<float>((Kep.x() / Kep.z() - center_x) * zoom, -max_x_offset, max_x_offset);
+  float y_offset = std::clamp<float>((Kep.y() / Kep.z() - center_y) * zoom, -max_y_offset, max_y_offset);
 
   // Apply transformation such that video pixel coordinates match video
   // 1) Put (0, 0) in the middle of the video
   // 2) Apply same scaling as video
   // 3) Put (0, 0) in top left corner of video
-  s->car_space_transform.reset();
-  s->car_space_transform.translate(w / 2 - x_offset, h / 2 - y_offset)
-      .scale(zoom, zoom)
-      .translate(-intrinsic_matrix.v[2], -intrinsic_matrix.v[5]);
+  Eigen::Matrix3f video_transform =(Eigen::Matrix3f() <<
+    zoom, 0.0f, (w / 2 - x_offset) - (center_x * zoom),
+    0.0f, zoom, (h / 2 - y_offset) - (center_y * zoom),
+    0.0f, 0.0f, 1.0f).finished();
+
+  s->car_space_transform = video_transform * calib_transform;
+  s->clip_region = rect().adjusted(-500, -500, 500, 500);
+
+  float zx = zoom * 2 * center_x / w;
+  float zy = zoom * 2 * center_y / h;
+  return mat4{{
+    zx, 0.0, 0.0, -x_offset / w * 2,
+    0.0, zy, 0.0, y_offset / h * 2,
+    0.0, 0.0, 1.0, 0.0,
+    0.0, 0.0, 0.0, 1.0,
+  }};
 }
 
 void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
@@ -271,18 +304,8 @@ void AnnotatedCameraWidget::paintGL() {
         wide_cam_requested = false;
       }
       wide_cam_requested = wide_cam_requested && sm["selfdriveState"].getSelfdriveState().getExperimentalMode();
-      // for replay of old routes, never go to widecam
-      wide_cam_requested = wide_cam_requested && s->scene.calibration_wide_valid;
     }
     CameraWidget::setStreamType(wide_cam_requested ? VISION_STREAM_WIDE_ROAD : VISION_STREAM_ROAD);
-
-    s->scene.wide_cam = CameraWidget::getStreamType() == VISION_STREAM_WIDE_ROAD;
-    if (s->scene.calibration_valid) {
-      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);
-    }
     CameraWidget::setFrameId(model.getFrameId());
     CameraWidget::paintGL();
   }

selfdrive/ui/qt/onroad/annotated_camera.h

@@ -36,7 +36,7 @@ class AnnotatedCameraWidget : public CameraWidget {
   void paintGL() override;
   void initializeGL() override;
   void showEvent(QShowEvent *event) override;
-  void updateFrameMat() override;
+  mat4 calcFrameMatrix() override;
   void drawLaneLines(QPainter &painter, const UIState *s);
   void drawLead(QPainter &painter, const cereal::RadarState::LeadData::Reader &lead_data, const QPointF &vd);
   void drawHud(QPainter &p);

selfdrive/ui/qt/onroad/onroad_home.cc

@@ -21,7 +21,7 @@ OnroadWindow::OnroadWindow(QWidget *parent) : QWidget(parent) {
   split->addWidget(nvg);
 
   if (getenv("DUAL_CAMERA_VIEW")) {
-    CameraWidget *arCam = new CameraWidget("camerad", VISION_STREAM_ROAD, true, this);
+    CameraWidget *arCam = new CameraWidget("camerad", VISION_STREAM_ROAD, this);
     split->insertWidget(0, arCam);
   }
 

selfdrive/ui/qt/widgets/cameraview.cc

@@ -7,13 +7,7 @@
 #endif
 
 #include <cmath>
-#include <set>
-#include <string>
-#include <utility>
-
 #include <QApplication>
-#include <QOpenGLBuffer>
-#include <QOffscreenSurface>
 
 namespace {
 
@@ -66,40 +60,10 @@ const char frame_fragment_shader[] =
   "}\n";
 #endif
 
-mat4 get_driver_view_transform(int screen_width, int screen_height, int stream_width, int stream_height) {
-  const float driver_view_ratio = 2.0;
-  const float yscale = stream_height * driver_view_ratio / stream_width;
-  const float xscale = yscale*screen_height/screen_width*stream_width/stream_height;
-  mat4 transform = (mat4){{
-    xscale,  0.0, 0.0, 0.0,
-    0.0,  yscale, 0.0, 0.0,
-    0.0,  0.0, 1.0, 0.0,
-    0.0,  0.0, 0.0, 1.0,
-  }};
-  return transform;
-}
-
-mat4 get_fit_view_transform(float widget_aspect_ratio, float frame_aspect_ratio) {
-  float zx = 1, zy = 1;
-  if (frame_aspect_ratio > widget_aspect_ratio) {
-    zy = widget_aspect_ratio / frame_aspect_ratio;
-  } else {
-    zx = frame_aspect_ratio / widget_aspect_ratio;
-  }
-
-  const mat4 frame_transform = {{
-    zx, 0.0, 0.0, 0.0,
-    0.0, zy, 0.0, 0.0,
-    0.0, 0.0, 1.0, 0.0,
-    0.0, 0.0, 0.0, 1.0,
-  }};
-  return frame_transform;
-}
-
 } // namespace
 
-CameraWidget::CameraWidget(std::string stream_name, VisionStreamType type, bool zoom, QWidget* parent) :
-                          stream_name(stream_name), active_stream_type(type), requested_stream_type(type), zoomed_view(zoom), QOpenGLWidget(parent) {
+CameraWidget::CameraWidget(std::string stream_name, VisionStreamType type, QWidget* parent) :
+                          stream_name(stream_name), active_stream_type(type), requested_stream_type(type), QOpenGLWidget(parent) {
   setAttribute(Qt::WA_OpaquePaintEvent);
   qRegisterMetaType<std::set<VisionStreamType>>("availableStreams");
   QObject::connect(this, &CameraWidget::vipcThreadConnected, this, &CameraWidget::vipcConnected, Qt::BlockingQueuedConnection);
@@ -214,59 +178,19 @@ void CameraWidget::availableStreamsUpdated(std::set<VisionStreamType> streams) {
   available_streams = streams;
 }
 
-void CameraWidget::updateFrameMat() {
-  int w = glWidth(), h = glHeight();
+mat4 CameraWidget::calcFrameMatrix() {
+  // Scale the frame to fit the widget while maintaining the aspect ratio.
+  float widget_aspect_ratio = (float)width() / height();
+  float frame_aspect_ratio = (float)stream_width / stream_height;
+  float zx = std::min(frame_aspect_ratio / widget_aspect_ratio, 1.0f);
+  float zy = std::min(widget_aspect_ratio / frame_aspect_ratio, 1.0f);
 
-  if (zoomed_view) {
-    if (active_stream_type == VISION_STREAM_DRIVER) {
-      if (stream_width > 0 && stream_height > 0) {
-        frame_mat = get_driver_view_transform(w, h, stream_width, stream_height);
-      }
-    } else {
-      // 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 (active_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);
-
-      float x_offset_ = (Kep.v[0] / Kep.v[2] - intrinsic_matrix.v[2]) * zoom;
-      float y_offset_ = (Kep.v[1] / Kep.v[2] - intrinsic_matrix.v[5]) * zoom;
-
-      float max_x_offset = intrinsic_matrix.v[2] * zoom - w / 2 - 5;
-      float max_y_offset = intrinsic_matrix.v[5] * zoom - h / 2 - 5;
-
-      x_offset = std::clamp(x_offset_, -max_x_offset, max_x_offset);
-      y_offset = std::clamp(y_offset_, -max_y_offset, max_y_offset);
-
-      float zx = zoom * 2 * intrinsic_matrix.v[2] / w;
-      float zy = zoom * 2 * intrinsic_matrix.v[5] / h;
-      const mat4 frame_transform = {{
-        zx, 0.0, 0.0, -x_offset / w * 2,
-        0.0, zy, 0.0, y_offset / h * 2,
-        0.0, 0.0, 1.0, 0.0,
-        0.0, 0.0, 0.0, 1.0,
-      }};
-      frame_mat = frame_transform;
-    }
-  } else if (stream_width > 0 && stream_height > 0) {
-    // fit frame to widget size
-    float widget_aspect_ratio = (float)w / h;
-    float frame_aspect_ratio = (float)stream_width  / stream_height;
-    frame_mat = get_fit_view_transform(widget_aspect_ratio, frame_aspect_ratio);
-  }
-}
-
-void CameraWidget::updateCalibration(const mat3 &calib) {
-  calibration = calib;
+  return mat4{{
+    zx, 0.0, 0.0, 0.0,
+    0.0, zy, 0.0, 0.0,
+    0.0, 0.0, 1.0, 0.0,
+    0.0, 0.0, 0.0, 1.0,
+  }};
 }
 
 void CameraWidget::paintGL() {
@@ -293,7 +217,7 @@ void CameraWidget::paintGL() {
   VisionBuf *frame = frames[frame_idx].second;
   assert(frame != nullptr);
 
-  updateFrameMat();
+  auto frame_mat = calcFrameMatrix();
 
   glViewport(0, 0, glWidth(), glHeight());
   glBindVertexArray(frame_vao);

selfdrive/ui/qt/widgets/cameraview.h

@@ -34,7 +34,7 @@ class CameraWidget : public QOpenGLWidget, protected QOpenGLFunctions {
 
 public:
   using QOpenGLWidget::QOpenGLWidget;
-  explicit CameraWidget(std::string stream_name, VisionStreamType stream_type, bool zoom, QWidget* parent = nullptr);
+  explicit CameraWidget(std::string stream_name, VisionStreamType stream_type, QWidget* parent = nullptr);
   ~CameraWidget();
   void setBackgroundColor(const QColor &color) { bg = color; }
   void setFrameId(int frame_id) { draw_frame_id = frame_id; }
@@ -51,21 +51,17 @@ class CameraWidget : public QOpenGLWidget, protected QOpenGLFunctions {
 protected:
   void paintGL() override;
   void initializeGL() override;
-  void resizeGL(int w, int h) override { updateFrameMat(); }
   void showEvent(QShowEvent *event) override;
   void mouseReleaseEvent(QMouseEvent *event) override { emit clicked(); }
-  virtual void updateFrameMat();
-  void updateCalibration(const mat3 &calib);
+  virtual mat4 calcFrameMatrix();
   void vipcThread();
   void clearFrames();
 
   int glWidth();
   int glHeight();
 
-  bool zoomed_view;
   GLuint frame_vao, frame_vbo, frame_ibo;
   GLuint textures[2];
-  mat4 frame_mat = {};
   std::unique_ptr<QOpenGLShaderProgram> program;
   QColor bg = QColor("#000000");
 
@@ -81,14 +77,6 @@ class CameraWidget : public QOpenGLWidget, protected QOpenGLFunctions {
   std::atomic<VisionStreamType> requested_stream_type;
   std::set<VisionStreamType> available_streams;
   QThread *vipc_thread = nullptr;
-
-  // Calibration
-  float x_offset = 0;
-  float y_offset = 0;
-  float zoom = 1.0;
-  mat3 calibration = DEFAULT_CALIBRATION;
-  mat3 intrinsic_matrix = FCAM_INTRINSIC_MATRIX;
-
   std::recursive_mutex frame_lock;
   std::deque<std::pair<uint32_t, VisionBuf*>> frames;
   uint32_t draw_frame_id = 0;