WIP head bug fixing - logging

reachy_phone_home/main.py

@@ -303,24 +303,36 @@ def _fix_camera_intrinsics(reachy: ReachyMini, logger: logging.Logger) -> None:
         logger.warning("[camera_diag] resolution not set yet, skipping fix")
         return
 
-    # The K matrix's principal point (cx, cy) sits near the centre of the
-    # resolution the calibration was performed at.  We can estimate that
-    # calibration resolution as (2*cx, 2*cy) and compare it to the real
-    # frame size.
-    orig_K = specs.K
-    cal_w = orig_K[0, 2] * 2  # ≈ calibration width
-    cal_h = orig_K[1, 2] * 2  # ≈ calibration height
+    # Check BOTH the active resized_K and the original specs.K for mismatches.
+    # The active K's principal point should be near the centre of the frame.
+    # If either is off by more than 10%, we need to fix.
+    active_cx = current_K[0, 2]
+    active_cy = current_K[1, 2]
+    expected_cx = frame_w / 2.0
+    expected_cy = frame_h / 2.0
+
+    active_ok = (
+        abs(active_cx - expected_cx) / expected_cx < 0.1
+        and abs(active_cy - expected_cy) / expected_cy < 0.1
+    )
 
-    # If calibration resolution is close to the actual frame resolution
-    # (within 10 %), K is already appropriate — nothing to do.
-    if abs(cal_w - frame_w) / frame_w < 0.1 and abs(cal_h - frame_h) / frame_h < 0.1:
+    if active_ok:
         logger.info(
-            "[camera] K matrix matches frame resolution %dx%d — no correction needed",
-            frame_w, frame_h,
+            "[camera] K matrix looks correct for %dx%d frames "
+            "(cx=%.1f, cy=%.1f vs expected ~%.0f, ~%.0f) — no correction needed",
+            frame_w, frame_h, active_cx, active_cy, expected_cx, expected_cy,
         )
         return
 
-    # Rescale K from the calibration resolution to the actual frame resolution.
+    # The active K is wrong. Recompute from specs.K, which should be
+    # calibrated for a resolution whose centre matches (specs.K cx, cy).
+    orig_K = specs.K
+    orig_cx = orig_K[0, 2]
+    orig_cy = orig_K[1, 2]
+    cal_w = orig_cx * 2  # approximate calibration width
+    cal_h = orig_cy * 2  # approximate calibration height
+
+    # Rescale from the calibration resolution to the actual frame resolution.
     w_ratio = frame_w / cal_w
     h_ratio = frame_h / cal_h
 
@@ -332,11 +344,15 @@ def _fix_camera_intrinsics(reachy: ReachyMini, logger: logging.Logger) -> None:
 
     cam.resized_K = fixed_K
     logger.info(
-        "[camera] Fixed K matrix for %dx%d frames (was calibrated for ~%.0fx%.0f)",
-        frame_w, frame_h, cal_w, cal_h,
+        "[camera] Fixed K matrix for %dx%d frames "
+        "(active cx,cy was %.1f,%.1f — expected ~%.0f,%.0f)",
+        frame_w, frame_h, active_cx, active_cy, expected_cx, expected_cy,
     )
-    logger.info("[camera] Original K:\n%s", orig_K)
+    logger.info("[camera] Original specs.K:\n%s", orig_K)
+    logger.info("[camera] Broken resized_K:\n%s", current_K)
     logger.info("[camera] Corrected K:\n%s", fixed_K)
+    WEB_UI.append_log(f"[camera] Fixed K: cx {active_cx:.1f}→{fixed_K[0,2]:.1f}, "
+                      f"cy {active_cy:.1f}→{fixed_K[1,2]:.1f}")
 
 
 def run_tracker(