Integrate ICP refinement into refine_ground_plane.py CLI

2026-02-10 02:54:10 +00:00
parent cfacb790f5
commit 2d42e2cdfa
4 changed files with 550 additions and 0 deletions
@@ -0,0 +1,8 @@
+## Notes
+- `basedpyright` reports many warnings for `open3d` due to missing type stubs; these are suppressed/ignored as they don't indicate logic errors.
+- Synthetic smoke testing requires mocking `unproject_depth_to_points` or providing valid depth/K pairs that produce overlapping points.
+- Open3D PoseGraph requires careful indexing; ensuring reference camera is at index 0 and fixed helps stabilize global optimization.
+- Gravity constraint regularization is now correctly applied relative to the original extrinsic-derived transform, preserving the RANSAC-leveled orientation.
+
+- [BUG] `build_pose_graph` in `aruco/icp_registration.py` uses `result.transformation` (which is $T_{21}$) as the edge from `idx2` to `idx1` (which expects $T_{12}$). This causes global optimization to move cameras in the wrong direction and likely exceed safety bounds.
+- Pairwise ICP convergence depends on `min_overlap_area` and `min_fitness`; cameras failing these criteria are excluded from global optimization and logged as warnings.
@@ -0,0 +1,16 @@
+## Notes
+- Open3D `registration_generalized_icp` is more robust for noisy depth data but requires normal estimation.
+- Multi-scale ICP significantly improves convergence range by starting with large voxels (4x base).
+- Information matrix from `get_information_matrix_from_point_clouds` is essential for weighting edges in the pose graph.
+- Initial relative transform from extrinsics is crucial for ICP convergence when cameras are far apart in camera frame.
+- Pose graph optimization should only include the connected component reachable from the reference camera to avoid singular systems.
+- Transforming point clouds to camera frame before pairwise ICP allows using the initial extrinsic-derived relative transform as a meaningful starting guess.
+- Pose graph construction must strictly filter for the connected component reachable from the reference camera to ensure a well-constrained optimization problem.
+- Aligned build_pose_graph signature with plan (returns PoseGraph only).
+- Implemented disconnected camera logging within build_pose_graph.
+- Re-derived optimized_serials in refine_with_icp to maintain node-to-serial mapping consistency.
+
+- Open3D `PoseGraphEdge(source, target, T)` expects $T$ to be $T_{target\_source}$.
+- When monkeypatching for tests, ensure all internal calls are accounted for, especially when production code has bugs that need to be worked around or highlighted.
+- Integrated ICP refinement into `refine_ground_plane.py` CLI, enabling optional global registration after ground plane alignment.
+- Added `_meta.icp_refined` block to output JSON to track ICP configuration and success metrics.