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, const std::initializer_list<const char *>>({"uiDebug"});
 
   main_layout = new QVBoxLayout(this);
@@ -132,10 +133,41 @@ void AnnotatedCameraWidget::initializeGL() {
   setBackgroundColor(bg_colors[STATUS_DISENGAGED]);
 }
 
-void AnnotatedCameraWidget::updateFrameMat() {
-  CameraWidget::updateFrameMat();
-  UIState *s = uiState();
-  int w = width(), h = height();
+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?
+  auto *s = uiState();
+  int w = width();
+  int h = height();
+  bool wide_cam = active_stream_type == VISION_STREAM_WIDE_ROAD;
+  const mat3 &intrinsic_matrix = wide_cam ? ECAM_INTRINSIC_MATRIX: FCAM_INTRINSIC_MATRIX;
+  const mat3 &calibration = wide_cam ? s->scene.view_from_wide_calib : s->scene.view_from_calib;
+  float zoom = wide_cam ? 2.0 : 1.1;
+
+  const vec3 inf = {{1000., 0., 0.}};
+  const vec3 Ep = matvecmul3(calibration, inf);
+  const vec3 Kep = matvecmul3(intrinsic_matrix, Ep);
+
+  float center_x = intrinsic_matrix.v[2];
+  float center_y = intrinsic_matrix.v[5];
+
+  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((Kep.v[0] / Kep.v[2] - center_x) * zoom, -max_x_offset, max_x_offset);
+  float y_offset = std::clamp((Kep.v[1] / Kep.v[2] - center_y) * zoom, -max_y_offset, max_y_offset);
+
+  float zx = zoom * 2 * center_x / w;
+  float zy = zoom * 2 * center_y / h;
+
+  mat4 frame_mat = {{
+    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,
+  }};
 
   s->fb_w = w;
   s->fb_h = h;
@@ -144,10 +176,17 @@ void AnnotatedCameraWidget::updateFrameMat() {
   // 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]);
+  mat3 video_transform = {
+    .v = {
+      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
+    }
+  };
+  auto calib_transform = matmul3(intrinsic_matrix, calibration);
+  s->car_space_transform = matmul3(video_transform, calib_transform);
+
+  return frame_mat;
 }
 
 void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {
@@ -323,14 +362,6 @@ void AnnotatedCameraWidget::paintGL() {
       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

@@ -39,7 +39,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

@@ -66,40 +66,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 +184,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 +223,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;

selfdrive/ui/ui.cc

@@ -22,12 +22,8 @@ static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y,
   const float margin = 500.0f;
   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.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]});
+  const vec3 pt = matvecmul3(s->car_space_transform, (vec3){{in_x, in_y, in_z}});
+  QPointF point(pt.v[0] / pt.v[2], pt.v[1] / pt.v[2]);
   if (clip_region.contains(point)) {
     *out = point;
     return true;

selfdrive/ui/ui.h

@@ -8,7 +8,6 @@
 #include <QColor>
 #include <QFuture>
 #include <QPolygonF>
-#include <QTransform>
 
 #include "cereal/messaging/messaging.h"
 #include "common/mat.h"
@@ -73,7 +72,6 @@ const QColor bg_colors [] = {
 typedef struct UIScene {
   bool calibration_valid = false;
   bool calibration_wide_valid  = false;
-  bool wide_cam = true;
   mat3 view_from_calib = DEFAULT_CALIBRATION;
   mat3 view_from_wide_calib = DEFAULT_CALIBRATION;
   cereal::PandaState::PandaType pandaType;
@@ -126,7 +124,7 @@ class UIState : public QObject {
 
   QString language;
 
-  QTransform car_space_transform;
+  mat3 car_space_transform = {};
 
 signals:
   void uiUpdate(const UIState &s);

selfdrive/ui/watch3.cc

@@ -19,14 +19,14 @@ int main(int argc, char *argv[]) {
   {
     QHBoxLayout *hlayout = new QHBoxLayout();
     layout->addLayout(hlayout);
-    hlayout->addWidget(new CameraWidget("camerad", VISION_STREAM_ROAD, false));
+    hlayout->addWidget(new CameraWidget("camerad", VISION_STREAM_ROAD));
   }
 
   {
     QHBoxLayout *hlayout = new QHBoxLayout();
     layout->addLayout(hlayout);
-    hlayout->addWidget(new CameraWidget("camerad", VISION_STREAM_DRIVER, false));
-    hlayout->addWidget(new CameraWidget("camerad", VISION_STREAM_WIDE_ROAD, false));
+    hlayout->addWidget(new CameraWidget("camerad", VISION_STREAM_DRIVER));
+    hlayout->addWidget(new CameraWidget("camerad", VISION_STREAM_WIDE_ROAD));
   }
 
   return a.exec();

tools/cabana/videowidget.cc

@@ -148,7 +148,7 @@ QWidget *VideoWidget::createCameraWidget() {
 
   QStackedLayout *stacked = new QStackedLayout();
   stacked->setStackingMode(QStackedLayout::StackAll);
-  stacked->addWidget(cam_widget = new StreamCameraView("camerad", VISION_STREAM_ROAD, false));
+  stacked->addWidget(cam_widget = new StreamCameraView("camerad", VISION_STREAM_ROAD));
   cam_widget->setMinimumHeight(MIN_VIDEO_HEIGHT);
   cam_widget->setSizePolicy(QSizePolicy::Preferred, QSizePolicy::MinimumExpanding);
   stacked->addWidget(alert_label = new InfoLabel(this));
@@ -420,8 +420,8 @@ void InfoLabel::paintEvent(QPaintEvent *event) {
   }
 }
 
-StreamCameraView::StreamCameraView(std::string stream_name, VisionStreamType stream_type, bool zoom, QWidget *parent)
-    : CameraWidget(stream_name, stream_type, zoom, parent) {
+StreamCameraView::StreamCameraView(std::string stream_name, VisionStreamType stream_type, QWidget *parent)
+    : CameraWidget(stream_name, stream_type, parent) {
   fade_animation = new QPropertyAnimation(this, "overlayOpacity");
   fade_animation->setDuration(500);
   fade_animation->setStartValue(0.2f);