From 33ddd8eb444ca4767d141e46e3b7e0abed6815da Mon Sep 17 00:00:00 2001
From: Shane Smiskol <shane@smiskol.com>
Date: Wed, 24 May 2023 17:28:33 -0700
Subject: [PATCH] ui: smooth wide cam transition (#28277)

* horribly messy

* bit simpler

* is this right?

* simpler and should work?

* used to be int/frames, but added easing so treat as float

* end slow (start fast)

* little faster

* clean up

* not needed

* try ease in/ease out and fix

* remove debugging print

* fix

* revert to previous curve

* based on speed

* not right

* fix

* fix

* this kinda works

* Revert zoom by speed

Revert "this kinda works"

This reverts commit 48aa30b945148b8eb79fbe33eb58e3fc3a6a7009.

Revert "fix"

This reverts commit 4ff2d33486231727d7cd68d366342c2273e3a315.

Revert "fix"

This reverts commit 15b22f8e8284eb017000856abb05b5e8973a6c0e.

Revert "not right"

This reverts commit 378b9965e14250c57ed39e1976de60d89054c2c8.

Revert "based on speed"

This reverts commit 1f7bfa5d73a2dee3740096da64eda24b33288b51.

Revert "Revert "not right""

This reverts commit 1beeb402534a755208d19771eb4a2afdc69b8739.

* better curve

* revert

* use constants and fixes

* up here too

* feels more intuitive to make zoom_transition=1 be zoomed in

* rm line

* fix

* cmt

* better handling

* better name

* zoom if ANY other stream is requested

* Update selfdrive/ui/qt/widgets/cameraview.cc
old-commit-hash: 26064196d0362d7719ce7b1ee835eb2193a3dc57
---
 common/modeldata.h                    |  3 +++
 selfdrive/ui/qt/widgets/cameraview.cc | 24 ++++++++++++++++++++----
 selfdrive/ui/qt/widgets/cameraview.h  |  6 ++++++
 3 files changed, 29 insertions(+), 4 deletions(-)

diff --git a/common/modeldata.h b/common/modeldata.h
index a00d3d49d3..fdb5dd078d 100644
--- a/common/modeldata.h
+++ b/common/modeldata.h
@@ -34,6 +34,9 @@ const mat3 ecam_intrinsic_matrix = (mat3){{567.0, 0.0, 1928.0 / 2,
                                            0.0, 567.0, 1208.0 / 2,
                                            0.0, 0.0, 1.0}};
 
+// zoom multiplier between cameras
+const float ecam_to_fcam_zoom = fcam_intrinsic_matrix.v[0] / ecam_intrinsic_matrix.v[0];
+
 static inline mat3 get_model_yuv_transform() {
   float db_s = 1.0;
   const mat3 transform = (mat3){{
diff --git a/selfdrive/ui/qt/widgets/cameraview.cc b/selfdrive/ui/qt/widgets/cameraview.cc
index 016129c348..d4f7e294dd 100644
--- a/selfdrive/ui/qt/widgets/cameraview.cc
+++ b/selfdrive/ui/qt/widgets/cameraview.cc
@@ -201,6 +201,12 @@ void CameraWidget::updateFrameMat() {
   int w = glWidth(), h = glHeight();
 
   if (zoomed_view) {
+    if (active_stream_type != VISION_STREAM_WIDE_ROAD) {
+      // Always ready to switch if wide is not active, start at full zoom
+      zoom_transition = 1;
+      ready_to_switch_stream = true;
+    }
+
     if (active_stream_type == VISION_STREAM_DRIVER) {
       frame_mat = get_driver_view_transform(w, h, stream_width, stream_height);
     } else {
@@ -209,11 +215,21 @@ void CameraWidget::updateFrameMat() {
       // for narrow come and a little lower for wide cam.
       // TODO: use proper perspective transform?
       if (active_stream_type == VISION_STREAM_WIDE_ROAD) {
+        // If narrow road camera is requested, start zooming in.
+        // Mark ready to switch once we're fully zoomed in
+        if (requested_stream_type != VISION_STREAM_WIDE_ROAD) {
+          zoom_transition += zoom_transition * 0.2 + 0.01;
+        } else {
+          zoom_transition -= (1.0 - zoom_transition) * 0.2 + 0.01;
+        }
+        zoom_transition = std::clamp(zoom_transition, 0.0f, 1.0f);
+        ready_to_switch_stream = fabs(zoom_transition - 1) < 1e-3;
+
         intrinsic_matrix = ecam_intrinsic_matrix;
-        zoom = 2.0;
+        zoom = util::map_val(zoom_transition, 0.0f, 1.0f, ecam_zoom, ecam_to_fcam_zoom * fcam_zoom);
       } else {
         intrinsic_matrix = fcam_intrinsic_matrix;
-        zoom = 1.1;
+        zoom = fcam_zoom;
       }
       const vec3 inf = {{1000., 0., 0.}};
       const vec3 Ep = matvecmul3(calibration, inf);
@@ -372,13 +388,13 @@ void CameraWidget::vipcThread() {
   VisionIpcBufExtra meta_main = {0};
 
   while (!QThread::currentThread()->isInterruptionRequested()) {
-    if (!vipc_client || cur_stream != requested_stream_type) {
+    if (!vipc_client || ((cur_stream != requested_stream_type) && ready_to_switch_stream)) {
       clearFrames();
       qDebug().nospace() << "connecting to stream " << requested_stream_type << ", was connected to " << cur_stream;
       cur_stream = requested_stream_type;
       vipc_client.reset(new VisionIpcClient(stream_name, cur_stream, false));
+      active_stream_type = cur_stream;
     }
-    active_stream_type = cur_stream;
 
     if (!vipc_client->connected) {
       clearFrames();
diff --git a/selfdrive/ui/qt/widgets/cameraview.h b/selfdrive/ui/qt/widgets/cameraview.h
index 8a140e5290..931e85e842 100644
--- a/selfdrive/ui/qt/widgets/cameraview.h
+++ b/selfdrive/ui/qt/widgets/cameraview.h
@@ -73,6 +73,8 @@ protected:
   int stream_width = 0;
   int stream_height = 0;
   int stream_stride = 0;
+  bool ready_to_switch_stream = true;  // stream transition may be delayed by a zoom animation
+  float zoom_transition = 0;
   std::atomic<VisionStreamType> active_stream_type;
   std::atomic<VisionStreamType> requested_stream_type;
   std::set<VisionStreamType> available_streams;
@@ -85,6 +87,10 @@ protected:
   mat3 calibration = DEFAULT_CALIBRATION;
   mat3 intrinsic_matrix = fcam_intrinsic_matrix;
 
+  // Calibration constants
+  const float ecam_zoom = 2.0;
+  const float fcam_zoom = 1.1;
+
   std::recursive_mutex frame_lock;
   std::deque<std::pair<uint32_t, VisionBuf*>> frames;
   uint32_t draw_frame_id = 0;