Adding more stabile depth

Author: MaticTonin

dynamic-calibration/utils/dynamic_calibration_interactive.py

@@ -1,3 +1,4 @@
+from collections import deque
 import numpy as np
 import cv2
 import time
@@ -13,7 +14,10 @@
     print_final_calibration_results,
 )
 
-
+# Rolling average for depth ROI means
+DEPTH_HISTORY_SIZE = 10
+depth_history = deque(maxlen=DEPTH_HISTORY_SIZE)
+last_mouse_pos = None  # reset history if mouse moves to new pixel
 mouse_coords = (-1, -1)
 
 
@@ -234,23 +238,31 @@ def on_mouse_disparity(event, x, y, flags, param):
                 cy = int(round(local_y * 2))
                 cx = int(round(local_x * 2))
 
-                half = 10  # 9x9 window → radius 4
+                half = 10  # ROI radius → window size = 2*half + 1
                 y0 = max(0, cy - half)
                 y1 = min(depth_frame.shape[0], cy + half + 1)
                 x0 = max(0, cx - half)
                 x1 = min(depth_frame.shape[1], cx + half + 1)
 
                 roi = depth_frame[y0:y1, x0:x1].astype(np.float32)
 
-                # If you want to ignore invalid zeros (common in depth):
+                # Rolling-mean over last 10 frames; clears if cursor center changes
+                cur_mouse_pos = (cx, cy)
+                if last_mouse_pos != cur_mouse_pos:
+                    depth_history.clear()
+                    last_mouse_pos = cur_mouse_pos
+
                 valid = roi > 0
                 if np.any(valid):
-                    depth_val = float(roi[valid].mean()) / 1000.0  # mm → m
+                    depth_mm = float(roi[valid].mean())   # in millimeters
+                    depth_history.append(depth_mm)
+                # else: keep previous history if current ROI is invalid
+
+                if len(depth_history) > 0:
+                    depth_val = float(np.mean(depth_history)) / 1000.0  # mm -> m
                 else:
                     depth_val = float("nan")
-                depth_val = (
-                    depth_frame[int(local_y * 2), int(local_x * 2)] / 1000.0
-                )  # mm -> meters
+
                 display_text_depth = f"Depth: {depth_val:.2f}m"
                 text_pos = (scaled_mouse_x + 10, scaled_mouse_y + 10)
                 disp_x0 = disp_x_start + (x0 // 2)

dynamic-calibration/utils/dynamic_controler.py

@@ -1,4 +1,5 @@
 from typing import Optional, Tuple
+from collections import deque
 import numpy as np
 import time
 
@@ -59,6 +60,11 @@ def __init__(self):
         self._roi_half = 10  # radius in depth pixels (=> (2h+1)^2 window)
         self._hud_on = True  # toggle with 'g'
 
+        self._avg_len = 10
+        self._roi_mean_hist = deque(maxlen=self._avg_len)
+        self._center_hist = deque(maxlen=self._avg_len)
+        self._hist_cursor_xy = None
+
         self.out_annotations = self.createOutput(
             possibleDatatypes=[
                 dai.Node.DatatypeHierarchy(dai.DatatypeEnum.ImgAnnotations, True)
@@ -135,17 +141,56 @@ def _draw_depth_hud(self, hud: dai.ImgAnnotation, preview_w: int, preview_h: int
         roi = self._last_depth[y0:y1, x0:x1]
 
         # stats (ignore zeros if depth)
+        if self._hist_cursor_xy != (cx, cy):
+            self._roi_mean_hist.clear()
+            self._center_hist.clear()
+            self._hist_cursor_xy = (cx, cy)
+
         if self._depth_is_mm:
             valid = roi > 0
-            center_val = float(self._last_depth[cy, cx]) / 1000.0  # mm → m
-            mean_val = (
-                float(roi[valid].mean() / 1000.0) if np.any(valid) else float("nan")
+
+            # center (mm)
+            center_mm = float(self._last_depth[cy, cx])
+            if center_mm > 0:
+                self._center_hist.append(center_mm)
+
+            # roi mean (mm)
+            if np.any(valid):
+                roi_mean_mm = float(roi[valid].mean())
+                self._roi_mean_hist.append(roi_mean_mm)
+
+            # aggregated (m)
+            if len(self._center_hist) > 0:
+                center_val = float(np.mean(self._center_hist)) / 1000.0
+            else:
+                center_val = float("nan")
+
+            if len(self._roi_mean_hist) > 0:
+                mean_val = float(np.mean(self._roi_mean_hist)) / 1000.0
+            else:
+                mean_val = float("nan")
+
+            val_text = (
+                f"Depth: {center_val:.2f} m, "
+                f"ROI mean: {mean_val:.2f} m "
+                f"(avg {len(self._roi_mean_hist)}/{self._avg_len})"
             )
-            val_text = f"Depth: {center_val:.2f} m, ROI mean: {mean_val:.2f} m"
         else:
-            center_val = float(self._last_depth[cy, cx])
-            mean_val = float(roi.mean())
-            val_text = f"Disp: {center_val:.1f}, ROI mean: {mean_val:.1f}"
+            # disparity path (no zero filtering)
+            center_disp = float(self._last_depth[cy, cx])
+            roi_mean_disp = float(roi.mean())
+
+            self._center_hist.append(center_disp)
+            self._roi_mean_hist.append(roi_mean_disp)
+
+            center_val = float(np.mean(self._center_hist))
+            mean_val = float(np.mean(self._roi_mean_hist))
+
+            val_text = (
+                f"Disp: {center_val:.1f}, "
+                f"ROI mean: {mean_val:.1f} "
+                f"(avg {len(self._roi_mean_hist)}/{self._avg_len})"
+            )
 
         # normalized positions relative to preview (assume 1:1 mapping depth→preview)
         u = (cx + 0.5) / W