Fix incorrect camera K matrix in desktop app

The bundled SDK v1.3.1 scales camera intrinsics incorrectly (cx=213 instead of 640),
causing large tracking errors. Added automatic detection and correction.

TODO: code refactor to remove some added complexity/tech debt and remove verbose logging from bug fixing in this latest commit

WIP head bug fixing - logging

reachy_phone_home/main.py

@@ -10,6 +10,7 @@ from pathlib import Path
 from typing import Callable, Optional
 
 import cv2
+import numpy as np
 from scipy.spatial.transform import Rotation as R
 from ultralytics import YOLO
 
@@ -48,6 +49,11 @@ class PhoneFollower:
         x1, y1, x2, y2 = _box_xyxy(box)
         cx = int((x1 + x2) / 2)
         cy = int(y1 + (y2 - y1) * y_ratio)
+        logger = logging.getLogger("yolo26l_phone_use_tracker")
+        conf = float(box.conf[0].item()) if hasattr(box, "conf") else None
+        logger.info("[phone_box] xyxy=(%d,%d,%d,%d) center=(%d,%d) conf=%.3f",
+                    x1, y1, x2, y2, cx, cy, conf if conf is not None else 0.0)
+        WEB_UI.append_log(f"[phone_box] xyxy=({x1},{y1},{x2},{y2}) center=({cx},{cy}) conf={conf}")
         self.update_xy(reachy, cx, cy)
 
     def update_xy(self, reachy: ReachyMini, cx: int, cy: int) -> None:
@@ -57,7 +63,55 @@ class PhoneFollower:
                 and abs(cy - self.last_y) < self.move_threshold_px
             ):
                 return
-        reachy.look_at_image(cx, cy, duration=self.head_duration, perform_movement=True)
+        logger = logging.getLogger("yolo26l_phone_use_tracker")
+        try:
+            cam = reachy.media_manager.camera
+            res = cam.resolution if cam else "N/A"
+            active_K = cam.resized_K if cam else "N/A"
+            logger.info("[look_at] pixel=(%d, %d) resolution=%s", cx, cy, res)
+            logger.info("[look_at] active K:\n%s", active_K)
+            WEB_UI.append_log(f"[look_at] pixel=({cx}, {cy}) res={res}")
+        except Exception:
+            logger.info("[look_at] pixel=(%d, %d) camera info unavailable", cx, cy)
+        try:
+            head_before = reachy.get_current_head_pose()
+            logger.info("[look_at] head BEFORE:\n%s", head_before)
+        except Exception:
+            head_before = None
+        # --- Reproduce look_at_image internals for diagnostics ---
+        try:
+            cam = reachy.media_manager.camera
+            K_active = cam.resized_K
+            D_active = cam.D
+            points = np.array([[[cx, cy]]], dtype=np.float32)
+            x_n, y_n = cv2.undistortPoints(points, K_active, D_active)[0, 0]
+            ray_cam = np.array([x_n, y_n, 1.0])
+            ray_cam /= np.linalg.norm(ray_cam)
+            T_world_head = reachy.get_current_head_pose()
+            T_world_cam = T_world_head @ reachy.T_head_cam
+            R_wc = T_world_cam[:3, :3]
+            t_wc = T_world_cam[:3, 3]
+            ray_world = R_wc @ ray_cam
+            P_world = t_wc + ray_world
+            logger.info("[look_at_detail] undistorted (x_n, y_n) = (%.4f, %.4f)", x_n, y_n)
+            logger.info("[look_at_detail] ray_cam     = %s", ray_cam)
+            logger.info("[look_at_detail] T_world_head:\n%s", T_world_head)
+            logger.info("[look_at_detail] T_world_cam :\n%s", T_world_cam)
+            logger.info("[look_at_detail] ray_world   = %s", ray_world)
+            logger.info("[look_at_detail] P_world     = %s", P_world)
+            WEB_UI.append_log(f"[look_at_detail] undist=({x_n:.4f},{y_n:.4f}) ray_cam={ray_cam} P_world={P_world}")
+        except Exception as e:
+            logger.warning("[look_at_detail] could not compute: %s", e)
+        # --- End internals ---
+        result_pose = reachy.look_at_image(cx, cy, duration=self.head_duration, perform_movement=True)
+        logger.info("[look_at] target pose returned:\n%s", result_pose)
+        WEB_UI.append_log(f"[look_at] target pose: {result_pose}")
+        try:
+            head_after = reachy.get_current_head_pose()
+            logger.info("[look_at] head AFTER:\n%s", head_after)
+            WEB_UI.append_log(f"[look_at] head after: {head_after}")
+        except Exception:
+            pass
         self.last_x = cx
         self.last_y = cy
 
@@ -195,6 +249,112 @@ class TrackerConfig:
     phone_home_pitch_deg: float = 25.0
 
 
+def _fix_camera_intrinsics(reachy: ReachyMini, logger: logging.Logger) -> None:
+    """Work around an SDK bug where the wireless camera's K matrix is not
+    rescaled to match the actual frame resolution.
+
+    ReachyMiniWirelessCamSpecs inherits its K from ReachyMiniLiteCamSpecs
+    (calibrated at 1920x1080) but overrides default_resolution to 1280x720.
+    Because CameraBase.set_resolution computes scaling ratios relative to
+    default_resolution, the ratio ends up being 1.0 and K is left unscaled.
+    This causes look_at_image to compute incorrect rays, producing large
+    unexpected head rotations.
+
+    This function detects the mismatch by comparing the resolution implied by
+    the K matrix's principal point (cx ≈ width/2) against the actual frame
+    resolution.  If they differ by more than 10 %, K is rescaled.
+    """
+    cam = getattr(reachy, "media_manager", None)
+    if cam is not None:
+        cam = getattr(cam, "camera", None)
+    if cam is None:
+        logger.warning("[camera_diag] No camera found on media_manager")
+        return
+
+    specs = getattr(cam, "camera_specs", None)
+    current_K = getattr(cam, "resized_K", None)
+
+    # --- Diagnostic dump ---
+    logger.info("[camera_diag] camera type        : %s", type(cam).__name__)
+    logger.info("[camera_diag] camera_specs type  : %s",
+                type(specs).__name__ if specs is not None else None)
+    logger.info("[camera_diag] camera_specs name  : %s",
+                getattr(specs, "name", None))
+    try:
+        logger.info("[camera_diag] resolution         : %s", cam.resolution)
+    except RuntimeError:
+        logger.info("[camera_diag] resolution         : NOT SET")
+    logger.info("[camera_diag] resized_K          :\n%s", current_K)
+    if specs is not None:
+        logger.info("[camera_diag] specs.K            :\n%s", specs.K)
+        logger.info("[camera_diag] specs.default_res  : %s", specs.default_resolution)
+    WEB_UI.append_log(f"[camera_diag] camera={type(cam).__name__} "
+                      f"specs={getattr(specs, 'name', None)} "
+                      f"resized_K={current_K}")
+    # --- End diagnostic dump ---
+
+    if specs is None or current_K is None:
+        logger.warning("[camera_diag] specs or resized_K is None, skipping fix")
+        return
+
+    try:
+        frame_w, frame_h = cam.resolution  # actual capture resolution
+    except RuntimeError:
+        logger.warning("[camera_diag] resolution not set yet, skipping fix")
+        return
+
+    # 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 active_ok:
+        logger.info(
+            "[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
+
+    # 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
+
+    fixed_K = orig_K.copy()
+    fixed_K[0, 0] *= w_ratio  # fx
+    fixed_K[1, 1] *= h_ratio  # fy
+    fixed_K[0, 2] *= w_ratio  # cx
+    fixed_K[1, 2] *= h_ratio  # cy
+
+    cam.resized_K = fixed_K
+    logger.info(
+        "[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 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(
     reachy: ReachyMini,
     config: TrackerConfig,
@@ -208,6 +368,21 @@ def run_tracker(
     logger.setLevel(logging.INFO)
     WEB_UI.set_download_callback(lambda w: _resolve_weights(w, logger))
 
+    _fix_camera_intrinsics(reachy, logger)
+
+    # --- SDK & transform diagnostics ---
+    try:
+        import reachy_mini as _rm
+        sdk_version = getattr(_rm, "__version__", "unknown")
+    except Exception:
+        sdk_version = "import_error"
+    logger.info("[sdk_diag] reachy_mini version   : %s", sdk_version)
+    logger.info("[sdk_diag] T_head_cam            :\n%s", reachy.T_head_cam)
+    logger.info("[sdk_diag] python executable      : %s", os.path.realpath(os.sys.executable))
+    WEB_UI.append_log(f"[sdk_diag] version={sdk_version} python={os.path.realpath(os.sys.executable)}")
+    WEB_UI.append_log(f"[sdk_diag] T_head_cam={reachy.T_head_cam}")
+    # --- End SDK diagnostics ---
+
     weights = _resolve_weights(config.weights, logger)
     model = YOLO(weights)
 
@@ -254,6 +429,7 @@ def run_tracker(
     last_curious = 0.0
     heartbeat_count = 0
     last_head_log = 0.0
+    first_look_down_logged = False
     mode = "tracking_phone"
     mode_start = time.time()
     quips_enabled = config.quips_enabled
@@ -364,6 +540,39 @@ def run_tracker(
 
             if mode == "looking_down":
                 if not look_down_active:
+                    if not first_look_down_logged:
+                        try:
+                            current_pose = reachy.get_current_head_pose()
+                            logger.info("[look_down] current head pose: %s", current_pose)
+                            WEB_UI.append_log(f"[look_down] current head pose: {current_pose}")
+                        except Exception:
+                            logger.info("[look_down] current head pose: unavailable")
+                            WEB_UI.append_log("[look_down] current head pose: unavailable")
+                        target_pose = create_head_pose(
+                            z=config.look_down_z_mm,
+                            pitch=config.look_down_pitch_deg,
+                            mm=True,
+                            degrees=True,
+                        )
+                        logger.info("[look_down] target head pose: %s", target_pose)
+                        WEB_UI.append_log(f"[look_down] target head pose: {target_pose}")
+                        if not config.no_head:
+                            try:
+                                reachy.goto_target(
+                                    head=create_head_pose(),
+                                    duration=min(0.6, config.neutral_duration),
+                                )
+                                time.sleep(0.5)
+                                try:
+                                    post_pose = reachy.get_current_head_pose()
+                                    logger.info("[look_down] post-neutral head pose: %s", post_pose)
+                                    WEB_UI.append_log(f"[look_down] post-neutral head pose: {post_pose}")
+                                except Exception:
+                                    logger.info("[look_down] post-neutral head pose: unavailable")
+                                    WEB_UI.append_log("[look_down] post-neutral head pose: unavailable")
+                            except Exception:
+                                pass
+                        first_look_down_logged = True
                     reachy.goto_target(
                         head=create_head_pose(
                             z=config.look_down_z_mm,