refactor: things

2026-03-06 17:17:59 +08:00
parent 8c6087683f
commit 33ab1a5d9d
171 changed files with 293 additions and 29894 deletions
@@ -0,0 +1,58 @@
+
+
+## 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.