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