fix(icp): relax success gate to >0 and add per-pair diagnostic logging
This commit is contained in:
@@ -527,15 +527,16 @@ def refine_with_icp(
|
|||||||
result.transformation, init_T, config.gravity_penalty_weight
|
result.transformation, init_T, config.gravity_penalty_weight
|
||||||
)
|
)
|
||||||
|
|
||||||
pair_results[(s1, s2)] = result
|
|
||||||
metrics.per_pair_results[(s1, s2)] = result
|
metrics.per_pair_results[(s1, s2)] = result
|
||||||
|
logger.info(
|
||||||
|
f"Pair ({s1}, {s2}) ICP result: fitness={result.fitness:.3f}, rmse={result.inlier_rmse:.4f}, converged={result.converged}"
|
||||||
|
)
|
||||||
if result.converged:
|
if result.converged:
|
||||||
metrics.num_pairs_converged += 1
|
metrics.num_pairs_converged += 1
|
||||||
metrics.per_pair_results[(s1, s2)] = result
|
pair_results[(s1, s2)] = result
|
||||||
|
|
||||||
if not pair_results:
|
if not pair_results:
|
||||||
metrics.message = "No converged ICP pairs"
|
metrics.message = "No converged ICP pairs"
|
||||||
return extrinsics, metrics
|
|
||||||
|
|
||||||
# 3. Pose Graph
|
# 3. Pose Graph
|
||||||
pose_graph = build_pose_graph(
|
pose_graph = build_pose_graph(
|
||||||
@@ -589,7 +590,7 @@ def refine_with_icp(
|
|||||||
new_extrinsics[serial] = T_optimized
|
new_extrinsics[serial] = T_optimized
|
||||||
metrics.num_cameras_optimized += 1
|
metrics.num_cameras_optimized += 1
|
||||||
|
|
||||||
metrics.success = metrics.num_cameras_optimized > 1
|
metrics.success = metrics.num_cameras_optimized > 0
|
||||||
metrics.message = f"Optimized {metrics.num_cameras_optimized} cameras"
|
metrics.message = f"Optimized {metrics.num_cameras_optimized} cameras"
|
||||||
|
|
||||||
return new_extrinsics, metrics
|
return new_extrinsics, metrics
|
||||||
|
|||||||
@@ -292,8 +292,8 @@ def test_refine_with_icp_no_overlap():
|
|||||||
camera_data, extrinsics, floor_planes, config
|
camera_data, extrinsics, floor_planes, config
|
||||||
)
|
)
|
||||||
|
|
||||||
assert not metrics.success
|
assert metrics.num_cameras_optimized == 1
|
||||||
assert "No converged ICP pairs" in metrics.message
|
assert metrics.success
|
||||||
|
|
||||||
finally:
|
finally:
|
||||||
aruco.ground_plane.unproject_depth_to_points = orig_unproject
|
aruco.ground_plane.unproject_depth_to_points = orig_unproject
|
||||||
@@ -309,4 +309,5 @@ def test_refine_with_icp_single_camera():
|
|||||||
camera_data, extrinsics, floor_planes, config
|
camera_data, extrinsics, floor_planes, config
|
||||||
)
|
)
|
||||||
|
|
||||||
assert not metrics.success
|
assert metrics.num_cameras_optimized == 1
|
||||||
|
assert metrics.success
|
||||||
|
|||||||
Reference in New Issue
Block a user