diff --git a/py_workspace/.sisyphus/notepads/full-icp-pipeline/learnings.md b/py_workspace/.sisyphus/notepads/full-icp-pipeline/learnings.md index 2e219c4..e1342d0 100644 --- a/py_workspace/.sisyphus/notepads/full-icp-pipeline/learnings.md +++ b/py_workspace/.sisyphus/notepads/full-icp-pipeline/learnings.md @@ -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. diff --git a/py_workspace/aruco/icp_registration.py b/py_workspace/aruco/icp_registration.py index 5cdd857..3583f3c 100644 --- a/py_workspace/aruco/icp_registration.py +++ b/py_workspace/aruco/icp_registration.py @@ -33,6 +33,14 @@ class ICPConfig: max_correspondence_distance_factor: float = 2.5 max_rotation_deg: float = 10.0 # Safety bound on ICP delta max_translation_m: float = 0.3 # Safety bound on ICP delta + max_pair_rotation_deg: float = 5.0 # Plausibility gate for pairwise updates + max_pair_translation_m: float = 0.5 # Plausibility gate for pairwise updates + max_final_rotation_deg: float = ( + 15.0 # Stricter post-opt gate for final camera update + ) + max_final_translation_m: float = ( + 1.0 # Stricter post-opt gate for final camera update + ) region: str = "floor" # "floor", "hybrid", or "full" robust_kernel: str = "none" # "none" or "tukey" robust_kernel_k: float = 0.1 @@ -474,11 +482,13 @@ def build_pose_graph( idx1 = serial_to_idx[s1] idx2 = serial_to_idx[s2] - # Edge from idx2 to idx1 (transformation maps 1 to 2) - # Open3D PoseGraphEdge(source, target, T) means P_source = T * P_target - # Here P_2 = T_21 * P_1, so source=2, target=1 + # Edge from idx1 to idx2 + # Open3D PoseGraphEdge(s, t, T) enforces T = Pose(t)^-1 * Pose(s) + # i.e., T is the pose of s in t's frame (T_t_s). + # result.transformation is T_c2_c1 (aligns pcd1 to pcd2), which is Pose of c1 in c2. + # So s=c1 (idx1), t=c2 (idx2). edge = o3d.pipelines.registration.PoseGraphEdge( - idx2, idx1, result.transformation, result.information_matrix, uncertain=True + idx1, idx2, result.transformation, result.information_matrix, uncertain=True ) pose_graph.edges.append(edge) @@ -523,10 +533,16 @@ def refine_with_icp( metrics.reference_camera = serials[0] - # 1. Extract near-floor bands + # 1. Extract scene points camera_pcds: Dict[str, o3d.geometry.PointCloud] = {} camera_points: Dict[str, PointsNC] = {} + # Auto-set overlap mode based on region + if config.region == "floor": + config.overlap_mode = "xz" + elif config.region in ["hybrid", "full"]: + config.overlap_mode = "3d" + for serial in serials: if serial not in floor_planes or serial not in extrinsics: continue @@ -540,17 +556,29 @@ def refine_with_icp( # floor_y = -plane.d (distance to origin along normal) floor_y = -plane.d - band_points = extract_near_floor_band( - points_world, floor_y, config.band_height, plane.normal + + scene_points = extract_scene_points( + points_world, + floor_y, + plane.normal, + mode=config.region, + band_height=config.band_height, ) - if len(band_points) < 100: + if len(scene_points) < 100: continue pcd = o3d.geometry.PointCloud() - pcd.points = o3d.utility.Vector3dVector(band_points) + pcd.points = o3d.utility.Vector3dVector(scene_points) + + # Apply SOR preprocessing + pcd = preprocess_point_cloud(pcd, config.voxel_size) + + if len(pcd.points) < 100: + continue + camera_pcds[serial] = pcd - camera_points[serial] = band_points + camera_points[serial] = np.asarray(pcd.points) # 2. Pairwise ICP valid_serials = sorted(list(camera_pcds.keys())) @@ -560,12 +588,20 @@ def refine_with_icp( for j in range(i + 1, len(valid_serials)): s2 = valid_serials[j] - area = compute_overlap_xz( - camera_points[s1], camera_points[s2], config.overlap_margin - ) + # Dispatch overlap check + if config.overlap_mode == "3d": + area = compute_overlap_3d( + camera_points[s1], camera_points[s2], config.overlap_margin + ) + else: + area = compute_overlap_xz( + camera_points[s1], camera_points[s2], config.overlap_margin + ) + if area < config.min_overlap_area: + unit = "m^3" if config.overlap_mode == "3d" else "m^2" logger.debug( - f"Skipping pair ({s1}, {s2}) due to insufficient overlap: {area:.2f} m^2 < {config.min_overlap_area:.2f} m^2" + f"Skipping pair ({s1}, {s2}) due to insufficient overlap: {area:.2f} {unit} < {config.min_overlap_area:.2f} {unit}" ) continue @@ -577,22 +613,24 @@ def refine_with_icp( T_w2 = extrinsics[s2] init_T = invert_transform(T_w2) @ T_w1 - # pairwise_icp aligns source_pcd to target_pcd. - # We pass camera-frame points to pairwise_icp to use init_T meaningfully. - pcd1_cam = o3d.geometry.PointCloud() - pcd1_cam.points = o3d.utility.Vector3dVector( + # Prepare point clouds for ICP + # Always transform back to camera frame to refine relative transform directly + # This avoids world-frame identity traps and ensures consistent behavior across regions + pcd1 = o3d.geometry.PointCloud() + pcd1.points = o3d.utility.Vector3dVector( (np.asarray(camera_pcds[s1].points) - T_w1[:3, 3]) @ T_w1[:3, :3] ) - pcd2_cam = o3d.geometry.PointCloud() - pcd2_cam.points = o3d.utility.Vector3dVector( + pcd2 = o3d.geometry.PointCloud() + pcd2.points = o3d.utility.Vector3dVector( (np.asarray(camera_pcds[s2].points) - T_w2[:3, 3]) @ T_w2[:3, :3] ) + current_init = init_T if config.global_init: # Downsample for global registration voxel_size = config.voxel_size - source_down = pcd1_cam.voxel_down_sample(voxel_size) - target_down = pcd2_cam.voxel_down_sample(voxel_size) + source_down = pcd1.voxel_down_sample(voxel_size) + target_down = pcd2.voxel_down_sample(voxel_size) source_fpfh = compute_fpfh_features(source_down, voxel_size) target_fpfh = compute_fpfh_features(target_down, voxel_size) @@ -604,7 +642,10 @@ def refine_with_icp( if global_result.fitness > 0.1: # Validate against safety bounds relative to extrinsic init T_global = global_result.transformation - T_diff = T_global @ invert_transform(init_T) + + # Compare T_global against current_init + T_diff = T_global @ invert_transform(current_init) + rot_diff = Rotation.from_matrix(T_diff[:3, :3]).as_euler( "xyz", degrees=True ) @@ -618,7 +659,7 @@ def refine_with_icp( logger.info( f"Global registration accepted for ({s1}, {s2}): fitness={global_result.fitness:.3f}" ) - init_T = T_global + current_init = T_global else: logger.warning( f"Global registration rejected for ({s1}, {s2}): exceeds bounds (rot={rot_mag:.1f}, trans={trans_mag:.3f})" @@ -628,11 +669,24 @@ def refine_with_icp( f"Global registration failed for ({s1}, {s2}): low fitness {global_result.fitness:.3f}" ) - result = pairwise_icp(pcd1_cam, pcd2_cam, config, init_T) + result = pairwise_icp(pcd1, pcd2, config, current_init) # Apply gravity constraint to the result relative to original transform + # T_original is the initial T_21 (from extrinsics) + T_original_21 = invert_transform(T_w2) @ T_w1 + result.transformation = apply_gravity_constraint( - result.transformation, init_T, config.gravity_penalty_weight + result.transformation, T_original_21, config.gravity_penalty_weight + ) + + # Debug logging for transform deltas + T_delta = result.transformation @ invert_transform(init_T) + rot_delta = Rotation.from_matrix(T_delta[:3, :3]).as_euler( + "xyz", degrees=True + ) + trans_delta = np.linalg.norm(T_delta[:3, 3]) + logger.debug( + f"Pair ({s1}, {s2}) delta: rot={np.linalg.norm(rot_delta):.2f}deg, trans={trans_delta:.3f}m" ) metrics.per_pair_results[(s1, s2)] = result @@ -640,8 +694,20 @@ def refine_with_icp( f"Pair ({s1}, {s2}) ICP result: fitness={result.fitness:.3f}, rmse={result.inlier_rmse:.4f}, converged={result.converged}" ) if result.converged: - metrics.num_pairs_converged += 1 - pair_results[(s1, s2)] = result + # Pair-level plausibility gate + rot_mag = np.linalg.norm(rot_delta) + if ( + rot_mag > config.max_pair_rotation_deg + or trans_delta > config.max_pair_translation_m + ): + logger.warning( + f"Pair ({s1}, {s2}) converged but rejected by pair-level gate: " + f"rot={rot_mag:.2f}deg (max={config.max_pair_rotation_deg}), " + f"trans={trans_delta:.3f}m (max={config.max_pair_translation_m})" + ) + else: + metrics.num_pairs_converged += 1 + pair_results[(s1, s2)] = result if not pair_results: metrics.message = "No converged ICP pairs" @@ -676,6 +742,8 @@ def refine_with_icp( list(connected - {metrics.reference_camera}) ) + logger.info(f"Optimized connected component: {optimized_serials}") + metrics.num_disconnected = len(valid_serials) - len(optimized_serials) metrics.num_cameras_optimized = 0 @@ -689,9 +757,32 @@ def refine_with_icp( rot_mag = np.linalg.norm(rot_delta) trans_mag = np.linalg.norm(T_delta[:3, 3]) - if rot_mag > config.max_rotation_deg or trans_mag > config.max_translation_m: + logger.debug( + f"Camera {serial} optimization delta: rot={rot_mag:.2f}deg, trans={trans_mag:.3f}m" + ) + + # Validate delta against both existing and final safety bounds + if rot_mag > config.max_rotation_deg: logger.warning( - f"Camera {serial} ICP correction exceeds bounds: rot={rot_mag:.2f} deg, trans={trans_mag:.3f} m. Rejecting." + f"Camera {serial} ICP correction exceeds max_rotation_deg: {rot_mag:.2f}deg > {config.max_rotation_deg:.2f}deg. Rejecting." + ) + continue + + if rot_mag > config.max_final_rotation_deg: + logger.warning( + f"Camera {serial} ICP correction exceeds max_final_rotation_deg: {rot_mag:.2f}deg > {config.max_final_rotation_deg:.2f}deg. Rejecting." + ) + continue + + if trans_mag > config.max_translation_m: + logger.warning( + f"Camera {serial} ICP correction exceeds max_translation_m: {trans_mag:.3f}m > {config.max_translation_m:.3f}m. Rejecting." + ) + continue + + if trans_mag > config.max_final_translation_m: + logger.warning( + f"Camera {serial} ICP correction exceeds max_final_translation_m: {trans_mag:.3f}m > {config.max_final_translation_m:.3f}m. Rejecting." ) continue