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

2026-02-11: Pose Graph Edge Direction 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).

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

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

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

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.