feat: implement depth bias estimation and correction in ICP pipeline

py_workspace/.sisyphus/notepads/full-icp-pipeline/learnings.md

@@ -1,28 +1,58 @@
 
-## 2026-02-11: Pose Graph Edge Direction Fix
+
+## 2026-02-11: Pose Graph Edge Transform Fix
 
 ### Problem
-Pose graph optimization was producing implausibly large deltas for cameras that were already reasonably aligned.
-Investigation revealed that `o3d.pipelines.registration.PoseGraphEdge(source, target, T)` expects `T` to be the transformation from `source` to `target` (i.e., `P_target = T * P_source`? No, Open3D convention is `P_source = T * P_target`).
+Pose graph optimization was producing implausibly large deltas.
+Investigation revealed that `o3d.pipelines.registration.PoseGraphEdge(s, t, T)` enforces `T_w_t = T_w_s * T`.
+This means `T` is the transformation from `s` to `t` in the graph frame (usually world).
 
-Wait, let's clarify Open3D semantics:
-`PoseGraphEdge(s, t, T)` means `T` is the measurement of `s` in `t`'s frame.
-`Pose(s) = T * Pose(t)` (if poses are world-to-camera? No, usually camera-to-world).
+However, `pairwise_icp` returns `T_icp` such that `P_c2 = T_icp * P_c1`.
+This `T_icp` is the transformation from `c1` to `c2` in the camera frame (or rather, it maps points from c1 to c2).
 
-Let's stick to the verified behavior in `tests/test_icp_graph_direction.py`:
-- `T_c2_c1` aligns `pcd1` to `pcd2`.
-- `pcd2 = T_c2_c1 * pcd1`.
-- This means `T_c2_c1` is the pose of `c1` in `c2`'s frame.
-- If we use `PoseGraphEdge(idx1, idx2, T)`, where `idx1=c1`, `idx2=c2`, it works.
-- The previous code used `PoseGraphEdge(idx2, idx1, T)`, which implied `T` was the pose of `c2` in `c1`'s frame (inverted).
+We derived that if we use `Edge(idx1, idx2, T_edge)`, we need `T_edge` such that `T_w_c2 = T_w_c1 * T_edge`.
+Substituting `P_w = T_w_c * P_c` into `P_c2 = T_icp * P_c1`, we found:
+`T_w_c2^-1 * P_w = T_icp * (T_w_c1^-1 * P_w)`
+`T_w_c2^-1 = T_icp * T_w_c1^-1`
+`T_w_c2 = T_w_c1 * T_icp^-1`
+
+Thus, `T_edge` must be `T_icp^-1`.
 
 ### Fix
-Swapped the indices in `PoseGraphEdge` construction in `aruco/icp_registration.py`:
-- Old: `edge = o3d.pipelines.registration.PoseGraphEdge(idx2, idx1, result.transformation, ...)`
-- New: `edge = o3d.pipelines.registration.PoseGraphEdge(idx1, idx2, result.transformation, ...)`
+In `aruco/icp_registration.py`, we now invert the ICP result before creating the PoseGraphEdge:
+```python
+T_edge = np.linalg.inv(result.transformation)
+edge = o3d.pipelines.registration.PoseGraphEdge(
+    idx1, idx2, T_edge, result.information_matrix, uncertain=True
+)
+```
+We used `np.linalg.inv` explicitly to ensure correct matrix inversion.
 
 ### Verification
-- Created `tests/test_icp_graph_direction.py` which sets up a known identity scenario.
-- The test failed with the old code (target camera moved to wrong position).
-- The test passed with the fix (target camera remained at correct position).
-- Existing tests in `tests/test_icp_registration.py` passed.
+- Created `tests/test_icp_fix_verification.py` which sets up a scenario where `T_icp` is a translation `(1, 0, 0)` and `T_w_c2` is `(-1, 0, 0)` relative to `T_w_c1`.
+- The test confirms that with `T_edge = inv(T_icp)`, the optimization correctly maintains the relative pose.
+- Verified that existing tests in `tests/test_icp_registration.py` still pass.
+
+# Learnings from ICP Hardening
+
+## Technical Improvements
+1. **Explicit ICP Bounds**: Added `--icp-max-rotation-deg` and `--icp-max-translation-m` CLI flags. This decouples ICP safety checks from the initial ground plane alignment bounds, allowing for tighter or looser constraints as needed for the refinement step.
+2. **Meaningful Final Gating**: Fixed the final acceptance logic in `refine_with_icp`. Previously, cameras were counted as optimized even if they were rejected by the final safety gate. Now, `num_cameras_optimized` accurately reflects only those cameras that passed all checks and were updated.
+3. **Reference Camera Exclusion**: The reference camera (anchor) is no longer counted in `num_cameras_optimized`. This prevents misleading success metrics where only the reference camera "succeeded" (which is a no-op).
+4. **Deterministic Testing**: Updated tests to verify these behaviors, ensuring that rejected cameras are not applied and that the reference camera doesn't inflate the success count.
+
+## Verification
+- `tests/test_icp_registration.py` passes all 40 tests, covering new gating logic and reference camera exclusion.
+- `tests/test_refine_ground_cli.py` passes, confirming CLI flag integration.
+- Type checking raised warnings about missing stubs (open3d, scipy) and deprecated types, but no critical errors in the modified logic.
+
+## Future Considerations
+- The `open3d` and `scipy` type stubs are missing, leading to many `reportUnknownMemberType` warnings. Adding these stubs or suppression would clean up the type check output.
+- The `ICPConfig` object is becoming large; consider grouping related parameters (e.g., `safety_bounds`, `registration_params`) if it grows further.
+
+## 2026-02-11: ICP Depth Bias Diagnosis
+- **Finding**: Geometric overlap is high (~71%–80%), but cross-camera depth bias is the primary blocker for ICP convergence.
+- **Evidence**: Median absolute signed residuals between pairs reach up to 0.137m (13.7cm).
+- **Outlier**: Camera `44435674` is involved in the most biased pairs, suggesting a unit-specific depth scale or offset issue.
+- **Planarity**: Overlap regions are not degenerate ($\lambda_3/\sum \lambda_i \approx 0.136-0.170$), confirming the issue is depth accuracy, not scene geometry.
+- **Action**: Recommended a "Static Target Depth Sweep" to isolate absolute offsets per unit before further ICP refinement.