fix(icp): relax success gate to >0 and add per-pair diagnostic logging

This commit is contained in:
2026-02-10 15:21:33 +00:00
parent 89482187f1
commit 71b146bc72
2 changed files with 9 additions and 7 deletions
+5 -4
View File
@@ -527,15 +527,16 @@ def refine_with_icp(
result.transformation, init_T, config.gravity_penalty_weight
)
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:
metrics.num_pairs_converged += 1
metrics.per_pair_results[(s1, s2)] = result
pair_results[(s1, s2)] = result
if not pair_results:
metrics.message = "No converged ICP pairs"
return extrinsics, metrics
# 3. Pose Graph
pose_graph = build_pose_graph(
@@ -589,7 +590,7 @@ def refine_with_icp(
new_extrinsics[serial] = T_optimized
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"
return new_extrinsics, metrics