zed-playground/workspaces/py_workspace/.sisyphus/notepads/full-icp-pipeline/learnings.md

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:

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.