remove Qt transforms

commaai · Aug 27, 2024 · dd5f50c · dd5f50c
1 parent 48bceb2
commit dd5f50c
Show file tree

Hide file tree

Showing 3 changed files with 24 additions and 27 deletions.
diff --git a/selfdrive/ui/qt/onroad/annotated_camera.cc b/selfdrive/ui/qt/onroad/annotated_camera.cc
@@ -134,20 +134,21 @@ void AnnotatedCameraWidget::initializeGL() {
 
 void AnnotatedCameraWidget::updateFrameMat() {
   CameraWidget::updateFrameMat();
-  UIState *s = uiState();
-  int w = width(), h = height();
-
-  s->fb_w = w;
-  s->fb_h = h;
 
   // 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]);
+  mat3 video_transform = {{
+    zoom, 0.0f, (width() / 2 - x_offset) - (intrinsic_matrix.v[2] * zoom),
+    0.0f, zoom, (height() / 2 - y_offset) - (intrinsic_matrix.v[5] * zoom),
+    0.0f, 0.0f, 1.0f
+  }};
+
+  UIState *s = uiState();
+  auto calib_transform = matmul3(intrinsic_matrix, calibration);
+  s->car_space_transform = matmul3(video_transform, calib_transform);
+  s->clip_region = rect().adjusted(-500, -500, 500, 500);
 }
 
 void AnnotatedCameraWidget::drawLaneLines(QPainter &painter, const UIState *s) {

diff --git a/selfdrive/ui/ui.cc b/selfdrive/ui/ui.cc
@@ -3,6 +3,7 @@
 #include <algorithm>
 #include <cassert>
 #include <cmath>
+#include <limits>
 
 #include <QtConcurrent>
 
@@ -19,20 +20,18 @@
 // Projects a point in car to space to the corresponding point in full frame
 // image space.
 static bool calib_frame_to_full_frame(const UIState *s, float in_x, float in_y, float in_z, QPointF *out) {
-  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]});
-  if (clip_region.contains(point)) {
-    *out = point;
-    return true;
+  const vec3 pt = matvecmul3(s->car_space_transform, (vec3){{in_x, in_y, in_z}});
+  if (pt.v[2] < std::numeric_limits<float>::epsilon()) {
+    return false;
   }
-  return false;
+
+  QPointF screen_point(pt.v[0] / pt.v[2], pt.v[1] / pt.v[2]);
+  if (!s->clip_region.contains(screen_point)) {
+    return false;
+  }
+
+  *out = screen_point;
+  return true;
 }
 
 int get_path_length_idx(const cereal::XYZTData::Reader &line, const float path_height) {

diff --git a/selfdrive/ui/ui.h b/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"
@@ -117,16 +116,14 @@ class UIState : public QObject {
   inline PrimeType primeType() const { return prime_type; }
   inline bool hasPrime() const { return prime_type > PrimeType::PRIME_TYPE_NONE; }
 
-  int fb_w = 0, fb_h = 0;
-
   std::unique_ptr<SubMaster> sm;
-
   UIStatus status;
   UIScene scene = {};
 
   QString language;
 
-  QTransform car_space_transform;
+  mat3 car_space_transform;
+  QRectF clip_region;
 
 signals:
   void uiUpdate(const UIState &s);