ui: use current calibration to center vanishing point

selfdrive/ui/qt/onroad.cc

@@ -208,6 +208,12 @@ void NvgWindow::updateState(const UIState &s) {
     setProperty("engageable", cs.getEngageable() || cs.getEnabled());
     setProperty("dmActive", sm["driverMonitoringState"].getDriverMonitoringState().getIsActiveMode());
   }
+
+  if (s.scene.calibration_valid) {
+    CameraViewWidget::updateCalibration(s.scene.view_from_calib);
+  } else {
+    CameraViewWidget::updateCalibration(DEFAULT_CALIBRATION);
+  }
 }
 
 void NvgWindow::drawHud(QPainter &p) {
@@ -399,23 +405,20 @@ void NvgWindow::initializeGL() {
   setBackgroundColor(bg_colors[STATUS_DISENGAGED]);
 }
 
-void NvgWindow::updateFrameMat(int w, int h) {
-  CameraViewWidget::updateFrameMat(w, h);
-
+void NvgWindow::updateFrameMat() {
+  CameraViewWidget::updateFrameMat();
   UIState *s = uiState();
+  int w = width(), h = height();
+
   s->fb_w = w;
   s->fb_h = h;
-  auto intrinsic_matrix = s->wide_camera ? ecam_intrinsic_matrix : fcam_intrinsic_matrix;
-  float zoom = ZOOM / intrinsic_matrix.v[0];
-  if (s->wide_camera) {
-    zoom *= 0.5;
-  }
+
   // 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, h / 2 + y_offset)
+  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]);
 }

selfdrive/ui/qt/onroad.h

@@ -70,7 +70,7 @@ class NvgWindow : public CameraViewWidget {
   void paintGL() override;
   void initializeGL() override;
   void showEvent(QShowEvent *event) override;
-  void updateFrameMat(int w, int h) override;
+  void updateFrameMat() override;
   void drawLaneLines(QPainter &painter, const UIState *s);
   void drawLead(QPainter &painter, const cereal::ModelDataV2::LeadDataV3::Reader &lead_data, const QPointF &vd);
   void drawHud(QPainter &p);

selfdrive/ui/qt/widgets/cameraview.cc

@@ -6,6 +6,8 @@
 #include <GLES3/gl3.h>
 #endif
 
+#include <cmath>
+
 #include <QOpenGLBuffer>
 #include <QOffscreenSurface>
 
@@ -59,13 +61,6 @@ const char frame_fragment_shader[] =
   "}\n";
 #endif
 
-const mat4 device_transform = {{
-  1.0,  0.0, 0.0, 0.0,
-  0.0,  1.0, 0.0, 0.0,
-  0.0,  0.0, 1.0, 0.0,
-  0.0,  0.0, 0.0, 1.0,
-}};
-
 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;
@@ -185,35 +180,55 @@ void CameraViewWidget::hideEvent(QHideEvent *event) {
   }
 }
 
-void CameraViewWidget::updateFrameMat(int w, int h) {
+void CameraViewWidget::updateFrameMat() {
+  int w = width(), h = height();
+
   if (zoomed_view) {
     if (stream_type == VISION_STREAM_DRIVER) {
-      frame_mat = matmul(device_transform, get_driver_view_transform(w, h, stream_width, stream_height));
+      frame_mat = get_driver_view_transform(w, h, stream_width, stream_height);
     } else {
-      auto intrinsic_matrix = stream_type == VISION_STREAM_WIDE_ROAD ? ecam_intrinsic_matrix : fcam_intrinsic_matrix;
-      float zoom = ZOOM / intrinsic_matrix.v[0];
-      if (stream_type == VISION_STREAM_WIDE_ROAD) {
-        zoom *= 0.5;
-      }
+      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
+      // TODO: use proper perspective transform?
+      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] / width();
       float zy = zoom * 2 * intrinsic_matrix.v[5] / height();
-
       const mat4 frame_transform = {{
-        zx, 0.0, 0.0, 0.0,
-        0.0, zy, 0.0, -y_offset / height() * 2,
+        zx, 0.0, 0.0, -x_offset / width() * 2,
+        0.0, zy, 0.0, y_offset / height() * 2,
         0.0, 0.0, 1.0, 0.0,
         0.0, 0.0, 0.0, 1.0,
       }};
-      frame_mat = matmul(device_transform, frame_transform);
+      frame_mat = frame_transform;
     }
   } else if (stream_width > 0 && stream_height > 0) {
     // fit frame to widget size
     float widget_aspect_ratio = (float)width() / height();
     float frame_aspect_ratio = (float)stream_width  / stream_height;
-    frame_mat = matmul(device_transform, get_fit_view_transform(widget_aspect_ratio, frame_aspect_ratio));
+    frame_mat = get_fit_view_transform(widget_aspect_ratio, frame_aspect_ratio);
   }
 }
 
+void CameraViewWidget::updateCalibration(const mat3 &calib) {
+  calibration = calib;
+  updateFrameMat();
+}
+
 void CameraViewWidget::paintGL() {
   glClearColor(bg.redF(), bg.greenF(), bg.blueF(), bg.alphaF());
   glClear(GL_STENCIL_BUFFER_BIT | GL_COLOR_BUFFER_BIT);
@@ -311,7 +326,7 @@ void CameraViewWidget::vipcConnected(VisionIpcClient *vipc_client) {
   assert(glGetError() == GL_NO_ERROR);
 #endif
 
-  updateFrameMat(width(), height());
+  updateFrameMat();
 }
 
 void CameraViewWidget::vipcFrameReceived(VisionBuf *buf, uint32_t frame_id) {

selfdrive/ui/qt/widgets/cameraview.h

@@ -42,11 +42,12 @@ class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions {
 protected:
   void paintGL() override;
   void initializeGL() override;
-  void resizeGL(int w, int h) override { updateFrameMat(w, h); }
+  void resizeGL(int w, int h) override { updateFrameMat(); }
   void showEvent(QShowEvent *event) override;
   void hideEvent(QHideEvent *event) override;
   void mouseReleaseEvent(QMouseEvent *event) override { emit clicked(); }
-  virtual void updateFrameMat(int w, int h);
+  virtual void updateFrameMat();
+  void updateCalibration(const mat3 &calib);
   void vipcThread();
 
   bool zoomed_view;
@@ -68,6 +69,13 @@ class CameraViewWidget : public QOpenGLWidget, protected QOpenGLFunctions {
   std::atomic<VisionStreamType> stream_type;
   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::deque<std::pair<uint32_t, VisionBuf*>> frames;
   uint32_t draw_frame_id = 0;
 

selfdrive/ui/ui.cc

@@ -140,6 +140,7 @@ static void update_state(UIState *s) {
         scene.view_from_calib.v[i*3 + j] = view_from_calib(i,j);
       }
     }
+    scene.calibration_valid = sm["liveCalibration"].getLiveCalibration().getCalStatus() == 1;
   }
   if (s->worldObjectsVisible()) {
     if (sm.updated("modelV2")) {

selfdrive/ui/ui.h

@@ -22,10 +22,7 @@ const int footer_h = 280;
 const int UI_FREQ = 20;   // Hz
 typedef cereal::CarControl::HUDControl::AudibleAlert AudibleAlert;
 
-// TODO: this is also hardcoded in common/transformations/camera.py
-// TODO: choose based on frame input size
-const float y_offset = 150.0;
-const float ZOOM = 2912.8;
+const mat3 DEFAULT_CALIBRATION = {{ 0.0, 1.0, 0.0, 0.0, 0.0, 1.0, 1.0, 0.0, 0.0 }};
 
 struct Alert {
   QString text1;
@@ -93,7 +90,8 @@ typedef struct {
 } line_vertices_data;
 
 typedef struct UIScene {
-  mat3 view_from_calib;
+  bool calibration_valid = false;
+  mat3 view_from_calib = DEFAULT_CALIBRATION;
   cereal::PandaState::PandaType pandaType;
 
   // modelV2