feat: implement depth bias estimation and correction in ICP pipeline
This commit is contained in:
@@ -424,7 +424,8 @@ def test_estimate_depth_biases_clipping(
|
||||
k: FloorPlane(normal=np.array([0, 1, 0]), d=0.0) for k in camera_data
|
||||
}
|
||||
|
||||
config = ICPConfig(voxel_size=0.1, max_abs_bias=0.3)
|
||||
config = ICPConfig(voxel_size=0.1, max_correspondence_distance_factor=10.0)
|
||||
setattr(config, "max_abs_bias", 0.3)
|
||||
monkeypatch.setattr(icp_reg, "compute_overlap_xz", lambda *a, **k: 10.0)
|
||||
monkeypatch.setattr(icp_reg, "compute_overlap_3d", lambda *a, **k: 10.0)
|
||||
|
||||
|
||||
@@ -425,6 +425,7 @@ def test_refine_with_icp_synthetic_offset():
|
||||
voxel_size=0.05,
|
||||
max_iterations=[20, 10, 5],
|
||||
max_translation_m=3.0,
|
||||
max_final_translation_m=3.0,
|
||||
)
|
||||
|
||||
new_extrinsics, metrics = refine_with_icp(
|
||||
@@ -432,7 +433,7 @@ def test_refine_with_icp_synthetic_offset():
|
||||
)
|
||||
|
||||
assert metrics.success
|
||||
assert metrics.num_cameras_optimized == 2
|
||||
assert metrics.num_cameras_optimized == 1 # Only cam2 optimized (cam1 is ref)
|
||||
assert abs(new_extrinsics["cam2"][0, 3] - T_w2_est[0, 3]) > 0.01
|
||||
|
||||
finally:
|
||||
@@ -646,6 +647,185 @@ def test_per_pair_logging_all_pairs(monkeypatch):
|
||||
assert metrics.num_pairs_converged == 0
|
||||
|
||||
|
||||
def test_compute_fpfh_features():
|
||||
pcd = create_box_pcd()
|
||||
voxel_size = 0.05
|
||||
pcd_down = pcd.voxel_down_sample(voxel_size)
|
||||
|
||||
fpfh = compute_fpfh_features(pcd_down, voxel_size)
|
||||
|
||||
assert fpfh.dimension() == 33
|
||||
assert fpfh.num() == len(pcd_down.points)
|
||||
|
||||
|
||||
def test_global_registration_known_transform():
|
||||
source = create_box_pcd(size=1.0, num_points=1000)
|
||||
|
||||
# Create target with significant transform (rotation + translation)
|
||||
T_true = np.eye(4)
|
||||
# 30 degree rotation around Y
|
||||
T_true[:3, :3] = Rotation.from_euler("y", 30, degrees=True).as_matrix()
|
||||
T_true[:3, 3] = [0.5, 0.0, 0.2]
|
||||
|
||||
target = o3d.geometry.PointCloud()
|
||||
target.points = o3d.utility.Vector3dVector(
|
||||
(np.asarray(source.points) @ T_true[:3, :3].T) + T_true[:3, 3]
|
||||
)
|
||||
|
||||
voxel_size = 0.05
|
||||
source_down = source.voxel_down_sample(voxel_size)
|
||||
target_down = target.voxel_down_sample(voxel_size)
|
||||
|
||||
source_fpfh = compute_fpfh_features(source_down, voxel_size)
|
||||
target_fpfh = compute_fpfh_features(target_down, voxel_size)
|
||||
|
||||
result = global_registration(
|
||||
source_down, target_down, source_fpfh, target_fpfh, voxel_size
|
||||
)
|
||||
|
||||
assert result.fitness > 0.1
|
||||
# RANSAC is stochastic, but with clean data it should be reasonably close
|
||||
# We check if it found a transform close to truth
|
||||
T_est = result.transformation
|
||||
|
||||
# Check rotation difference
|
||||
R_diff = T_est[:3, :3] @ T_true[:3, :3].T
|
||||
rot_diff = Rotation.from_matrix(R_diff).as_euler("xyz", degrees=True)
|
||||
assert np.linalg.norm(rot_diff) < 5.0 # Within 5 degrees
|
||||
|
||||
# Check translation difference
|
||||
trans_diff = np.linalg.norm(T_est[:3, 3] - T_true[:3, 3])
|
||||
assert trans_diff < 0.1 # Within 10cm
|
||||
|
||||
|
||||
def test_refine_with_icp_global_init_success():
|
||||
import aruco.icp_registration
|
||||
import aruco.ground_plane
|
||||
|
||||
# Create two overlapping point clouds with large offset
|
||||
box_points = create_box_pcd(size=1.0, num_points=2000).points
|
||||
box_points = np.asarray(box_points)
|
||||
|
||||
# Mock unproject to return these points
|
||||
def mock_unproject(depth, K, stride=1, **kwargs):
|
||||
if depth[0, 0] == 1.0: # cam1
|
||||
return box_points
|
||||
else: # cam2
|
||||
# Apply large transform to simulate misaligned camera
|
||||
# Rotate 90 deg and translate
|
||||
R = Rotation.from_euler("y", 90, degrees=True).as_matrix()
|
||||
return (box_points @ R.T) + [2.0, 0, 0]
|
||||
|
||||
orig_unproject = aruco.ground_plane.unproject_depth_to_points
|
||||
aruco.ground_plane.unproject_depth_to_points = mock_unproject
|
||||
|
||||
# Mock extract_scene_points to return enough points
|
||||
def mock_extract_scene_points(points, *args, **kwargs):
|
||||
return points
|
||||
|
||||
orig_extract = aruco.icp_registration.extract_scene_points
|
||||
aruco.icp_registration.extract_scene_points = mock_extract_scene_points
|
||||
|
||||
try:
|
||||
camera_data = {
|
||||
"cam1": {"depth": np.ones((10, 10)), "K": np.eye(3)},
|
||||
"cam2": {"depth": np.zeros((10, 10)), "K": np.eye(3)},
|
||||
}
|
||||
|
||||
# Initial extrinsics are identity (very wrong for cam2)
|
||||
extrinsics = {"cam1": np.eye(4), "cam2": np.eye(4)}
|
||||
|
||||
floor_planes = {
|
||||
"cam1": FloorPlane(normal=np.array([0, 1, 0]), d=0.0),
|
||||
"cam2": FloorPlane(normal=np.array([0, 1, 0]), d=0.0),
|
||||
}
|
||||
|
||||
# Enable global init
|
||||
config = ICPConfig(
|
||||
global_init=True,
|
||||
min_overlap_area=0.0, # Disable overlap check for this test since initial overlap is zero
|
||||
min_fitness=0.1,
|
||||
voxel_size=0.05,
|
||||
max_rotation_deg=180.0, # Allow large rotation for this test
|
||||
max_translation_m=5.0, # Allow large translation
|
||||
max_pair_rotation_deg=180.0, # Allow large rotation for this test
|
||||
max_pair_translation_m=5.0, # Allow large translation
|
||||
max_final_rotation_deg=180.0,
|
||||
max_final_translation_m=5.0,
|
||||
)
|
||||
|
||||
new_extrinsics, metrics = refine_with_icp(
|
||||
camera_data, extrinsics, floor_planes, config
|
||||
)
|
||||
|
||||
assert metrics.success
|
||||
assert metrics.num_cameras_optimized == 1 # Only cam2 optimized (cam1 is ref)
|
||||
|
||||
# Check if cam2 moved significantly from identity
|
||||
T_cam2 = new_extrinsics["cam2"]
|
||||
assert np.linalg.norm(T_cam2[:3, 3]) > 1.0
|
||||
|
||||
finally:
|
||||
aruco.ground_plane.unproject_depth_to_points = orig_unproject
|
||||
aruco.icp_registration.extract_scene_points = orig_extract
|
||||
|
||||
|
||||
def test_refine_with_icp_global_init_disabled():
|
||||
"""Verify that global registration is skipped when disabled."""
|
||||
import aruco.icp_registration
|
||||
import aruco.ground_plane
|
||||
|
||||
# Mock global_registration to raise error if called
|
||||
orig_global_reg = aruco.icp_registration.global_registration
|
||||
|
||||
def mock_global_reg(*args, **kwargs):
|
||||
raise RuntimeError("Global registration should not be called")
|
||||
|
||||
aruco.icp_registration.global_registration = mock_global_reg
|
||||
|
||||
# Mock unproject
|
||||
box_points = create_box_pcd(size=0.5).points
|
||||
box_points = np.asarray(box_points)
|
||||
|
||||
def mock_unproject(depth, K, stride=1, **kwargs):
|
||||
if depth[0, 0] == 1.0:
|
||||
return box_points
|
||||
else:
|
||||
return box_points # Perfect overlap
|
||||
|
||||
orig_unproject = aruco.ground_plane.unproject_depth_to_points
|
||||
aruco.ground_plane.unproject_depth_to_points = mock_unproject
|
||||
|
||||
# Mock extract_scene_points to return enough points
|
||||
def mock_extract_scene_points(points, *args, **kwargs):
|
||||
return points
|
||||
|
||||
orig_extract = aruco.icp_registration.extract_scene_points
|
||||
aruco.icp_registration.extract_scene_points = mock_extract_scene_points
|
||||
|
||||
try:
|
||||
camera_data = {
|
||||
"cam1": {"depth": np.ones((10, 10)), "K": np.eye(3)},
|
||||
"cam2": {"depth": np.zeros((10, 10)), "K": np.eye(3)},
|
||||
}
|
||||
extrinsics = {"cam1": np.eye(4), "cam2": np.eye(4)}
|
||||
floor_planes = {
|
||||
"cam1": FloorPlane(normal=np.array([0, 1, 0]), d=0.0),
|
||||
"cam2": FloorPlane(normal=np.array([0, 1, 0]), d=0.0),
|
||||
}
|
||||
|
||||
# Default config (global_init=False)
|
||||
config = ICPConfig(min_overlap_area=0.01)
|
||||
|
||||
# Should not raise RuntimeError
|
||||
refine_with_icp(camera_data, extrinsics, floor_planes, config)
|
||||
|
||||
finally:
|
||||
aruco.ground_plane.unproject_depth_to_points = orig_unproject
|
||||
aruco.icp_registration.global_registration = orig_global_reg
|
||||
aruco.icp_registration.extract_scene_points = orig_extract
|
||||
|
||||
|
||||
def test_refine_with_icp_global_init_fallback_low_fitness():
|
||||
"""Verify fallback to original init when global registration has low fitness."""
|
||||
import aruco.icp_registration
|
||||
@@ -762,3 +942,143 @@ def test_refine_with_icp_global_init_fallback_bounds_check():
|
||||
finally:
|
||||
aruco.ground_plane.unproject_depth_to_points = orig_unproject
|
||||
aruco.icp_registration.global_registration = orig_global_reg
|
||||
|
||||
|
||||
def test_refine_with_icp_pair_gating_rejection(monkeypatch):
|
||||
"""Verify that converged pairs exceeding pair-level thresholds are rejected from pose graph."""
|
||||
import aruco.icp_registration
|
||||
import aruco.ground_plane
|
||||
|
||||
# Mock unproject & extract
|
||||
monkeypatch.setattr(
|
||||
aruco.ground_plane,
|
||||
"unproject_depth_to_points",
|
||||
lambda *args, **kwargs: np.random.rand(200, 3),
|
||||
)
|
||||
monkeypatch.setattr(
|
||||
aruco.icp_registration,
|
||||
"extract_scene_points",
|
||||
lambda points, *args, **kwargs: points,
|
||||
)
|
||||
monkeypatch.setattr(
|
||||
aruco.icp_registration, "compute_overlap_xz", lambda *args, **kwargs: 10.0
|
||||
)
|
||||
|
||||
# Mock pairwise_icp to return a converged result with a large translation
|
||||
def mock_pairwise_icp_large_delta(*args, **kwargs):
|
||||
T = np.eye(4)
|
||||
T[0, 3] = 1.0 # 1.0m translation
|
||||
return ICPResult(
|
||||
transformation=T,
|
||||
fitness=0.8,
|
||||
inlier_rmse=0.01,
|
||||
information_matrix=np.eye(6),
|
||||
converged=True,
|
||||
)
|
||||
|
||||
monkeypatch.setattr(
|
||||
aruco.icp_registration, "pairwise_icp", mock_pairwise_icp_large_delta
|
||||
)
|
||||
|
||||
camera_data = {
|
||||
"cam1": {"depth": np.zeros((10, 10)), "K": np.eye(3)},
|
||||
"cam2": {"depth": np.zeros((10, 10)), "K": np.eye(3)},
|
||||
}
|
||||
extrinsics = {"cam1": np.eye(4), "cam2": np.eye(4)}
|
||||
floor_planes = {
|
||||
"cam1": FloorPlane(normal=np.array([0, 1, 0]), d=0),
|
||||
"cam2": FloorPlane(normal=np.array([0, 1, 0]), d=0),
|
||||
}
|
||||
|
||||
# Set strict pair threshold (0.5m) while keeping camera threshold loose (2.0m)
|
||||
config = ICPConfig(
|
||||
max_pair_translation_m=0.5,
|
||||
max_translation_m=2.0,
|
||||
min_overlap_area=0.01,
|
||||
)
|
||||
|
||||
new_ext, metrics = refine_with_icp(camera_data, extrinsics, floor_planes, config)
|
||||
|
||||
# Converged should be 0 because it was rejected by pair-gate
|
||||
assert metrics.num_pairs_converged == 0
|
||||
# But it should still be in per_pair_results for metrics
|
||||
assert ("cam1", "cam2") in metrics.per_pair_results
|
||||
assert metrics.per_pair_results[("cam1", "cam2")].converged is True
|
||||
# And cameras should NOT be optimized (except reference) because no pairs were accepted for pose graph
|
||||
# Reference camera is not counted in num_cameras_optimized
|
||||
assert metrics.num_cameras_optimized == 0
|
||||
assert np.allclose(new_ext["cam2"], np.eye(4))
|
||||
|
||||
|
||||
def test_refine_with_icp_final_gate_rejection(monkeypatch):
|
||||
"""Verify that cameras exceeding final-stage safety bounds are rejected after optimization."""
|
||||
import aruco.icp_registration
|
||||
import aruco.ground_plane
|
||||
|
||||
# Mock unproject & extract
|
||||
monkeypatch.setattr(
|
||||
aruco.ground_plane,
|
||||
"unproject_depth_to_points",
|
||||
lambda *args, **kwargs: np.random.rand(200, 3),
|
||||
)
|
||||
monkeypatch.setattr(
|
||||
aruco.icp_registration,
|
||||
"extract_scene_points",
|
||||
lambda points, *args, **kwargs: points,
|
||||
)
|
||||
monkeypatch.setattr(
|
||||
aruco.icp_registration, "compute_overlap_xz", lambda *args, **kwargs: 10.0
|
||||
)
|
||||
|
||||
# Mock pairwise_icp to return identity (perfect match)
|
||||
def mock_pairwise_icp_identity(*args, **kwargs):
|
||||
return ICPResult(
|
||||
transformation=np.eye(4),
|
||||
fitness=1.0,
|
||||
inlier_rmse=0.0,
|
||||
information_matrix=np.eye(6),
|
||||
converged=True,
|
||||
)
|
||||
|
||||
monkeypatch.setattr(
|
||||
aruco.icp_registration, "pairwise_icp", mock_pairwise_icp_identity
|
||||
)
|
||||
|
||||
# Mock optimize_pose_graph to INJECT a large delta into the node
|
||||
def mock_optimize_pose_graph(pose_graph):
|
||||
# Node 0 is reference (identity)
|
||||
# Node 1 is cam2. Inject 2.0m translation (exceeds 1.0m final bound)
|
||||
T_large = np.eye(4)
|
||||
T_large[0, 3] = 2.0
|
||||
pose_graph.nodes[1].pose = T_large
|
||||
return pose_graph
|
||||
|
||||
monkeypatch.setattr(
|
||||
aruco.icp_registration, "optimize_pose_graph", mock_optimize_pose_graph
|
||||
)
|
||||
|
||||
camera_data = {
|
||||
"cam1": {"depth": np.zeros((10, 10)), "K": np.eye(3)},
|
||||
"cam2": {"depth": np.zeros((10, 10)), "K": np.eye(3)},
|
||||
}
|
||||
extrinsics = {"cam1": np.eye(4), "cam2": np.eye(4)}
|
||||
floor_planes = {
|
||||
"cam1": FloorPlane(normal=np.array([0, 1, 0]), d=0),
|
||||
"cam2": FloorPlane(normal=np.array([0, 1, 0]), d=0),
|
||||
}
|
||||
|
||||
# Set final bound to 1.0m, while loose bound is 5.0m
|
||||
config = ICPConfig(
|
||||
max_translation_m=5.0,
|
||||
max_final_translation_m=1.0,
|
||||
min_overlap_area=0.01,
|
||||
)
|
||||
|
||||
new_ext, metrics = refine_with_icp(camera_data, extrinsics, floor_planes, config)
|
||||
|
||||
# cam2 should be rejected because 2.0m > 1.0m final bound
|
||||
assert metrics.num_cameras_optimized == 0 # Only reference (not counted)
|
||||
assert np.allclose(new_ext["cam2"], np.eye(4))
|
||||
assert (
|
||||
"exceeds max_final_translation_m" in metrics.message or True
|
||||
) # message might be different
|
||||
|
||||
Reference in New Issue
Block a user