crosstyan/zed-playground
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(icp): correct pose graph edge direction

Browse Source
This commit is contained in:
crosstyan
2026-02-11 04:02:12 +00:00
parent e7a348e3ab
commit be3e454644
2 changed files with 147 additions and 48 deletions
Download Patch File Download Diff File Expand all files Collapse all files
py_workspace/.sisyphus/notepads/full-icp-pipeline/learnings.md
+26 -18
View File
@@ -1,20 +1,28 @@
## Task 5 Regression Fix
- Restored missing Task 6 tests (`test_compute_fpfh_features`, `test_global_registration_known_transform`, `test_refine_with_icp_global_init_*`) that were accidentally overwritten during Task 5 integration.
- Verified all 37 tests pass (33 from Task 5 + 4 restored from Task 6).
- Confirmed type safety remains clean.
## Task 7: CLI Flags
- Added `--icp-region`, `--icp-global-init`, `--icp-min-overlap`, `--icp-band-height`, `--icp-robust-kernel`, `--icp-robust-k` to `refine_ground_plane.py`.
- Verified flags appear in `--help`.
- Verified type safety with `basedpyright` (0 errors, only existing warnings).
- Flags are correctly passed to `ICPConfig` and recorded in output JSON metadata.
## 2026-02-11: Pose Graph Edge Direction Fix
## Task 9: Final Verification
- Added comprehensive tests for full-scene ICP pipeline:
- `test_success_gate_single_camera`: verified relaxed success gate (>0).
- `test_per_pair_logging_all_pairs`: verified all pairs logged regardless of convergence.
- Restored missing regression tests for floor/hybrid modes and SOR preprocessing.
- Updated README.md with new CLI flags and hybrid mode example.
- Verified full regression suite (129 tests passed).
- Confirmed `basedpyright` clean on modified files.
- Note: `test_per_pair_logging_all_pairs` required mocking `unproject_depth_to_points` to return enough points (200) to pass the `len(pcd.points) < 100` check in `refine_with_icp`.
### 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.