## 2026-02-11: Pose Graph Edge Transform Fix

### Problem
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).

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).

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
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_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.