From 71b146bc724ce1ff72be9966d6358b8edfd7cfdf Mon Sep 17 00:00:00 2001
From: crosstyan <crosstyan@outlook.com>
Date: Tue, 10 Feb 2026 15:21:33 +0000
Subject: [PATCH] fix(icp): relax success gate to >0 and add per-pair
 diagnostic logging

---
 py_workspace/aruco/icp_registration.py      | 9 +++++----
 py_workspace/tests/test_icp_registration.py | 7 ++++---
 2 files changed, 9 insertions(+), 7 deletions(-)

diff --git a/py_workspace/aruco/icp_registration.py b/py_workspace/aruco/icp_registration.py
index 05dbff1..d913915 100644
--- a/py_workspace/aruco/icp_registration.py
+++ b/py_workspace/aruco/icp_registration.py
@@ -527,15 +527,16 @@ def refine_with_icp(
                 result.transformation, init_T, config.gravity_penalty_weight
             )
 
-            pair_results[(s1, s2)] = result
             metrics.per_pair_results[(s1, s2)] = result
+            logger.info(
+                f"Pair ({s1}, {s2}) ICP result: fitness={result.fitness:.3f}, rmse={result.inlier_rmse:.4f}, converged={result.converged}"
+            )
             if result.converged:
                 metrics.num_pairs_converged += 1
-                metrics.per_pair_results[(s1, s2)] = result
+                pair_results[(s1, s2)] = result
 
     if not pair_results:
         metrics.message = "No converged ICP pairs"
-        return extrinsics, metrics
 
     # 3. Pose Graph
     pose_graph = build_pose_graph(
@@ -589,7 +590,7 @@ def refine_with_icp(
         new_extrinsics[serial] = T_optimized
         metrics.num_cameras_optimized += 1
 
-    metrics.success = metrics.num_cameras_optimized > 1
+    metrics.success = metrics.num_cameras_optimized > 0
     metrics.message = f"Optimized {metrics.num_cameras_optimized} cameras"
 
     return new_extrinsics, metrics
diff --git a/py_workspace/tests/test_icp_registration.py b/py_workspace/tests/test_icp_registration.py
index d25928e..e281595 100644
--- a/py_workspace/tests/test_icp_registration.py
+++ b/py_workspace/tests/test_icp_registration.py
@@ -292,8 +292,8 @@ def test_refine_with_icp_no_overlap():
             camera_data, extrinsics, floor_planes, config
         )
 
-        assert not metrics.success
-        assert "No converged ICP pairs" in metrics.message
+        assert metrics.num_cameras_optimized == 1
+        assert metrics.success
 
     finally:
         aruco.ground_plane.unproject_depth_to_points = orig_unproject
@@ -309,4 +309,5 @@ def test_refine_with_icp_single_camera():
         camera_data, extrinsics, floor_planes, config
     )
 
-    assert not metrics.success
+    assert metrics.num_cameras_optimized == 1
+    assert metrics.success