29 lines
1.6 KiB
Markdown
29 lines
1.6 KiB
Markdown
|
|
## 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.
|