支持主机桥接后固定δt与旋转先验,并落盘运动对供可视化直读。

Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
lichun.qu
2026-08-11 10:57:11 +08:00
co-authored by Cursor
parent c2f99b94a2
commit 03fcee7e32
14 changed files with 674 additions and 53 deletions
+16 -1
View File
@@ -190,6 +190,7 @@ def refine_time_offset_signed(
gyro_bias_rad_s: np.ndarray | None = None,
search_s: float = 0.08,
sample_hz: float = 50.0,
max_shift_s: float | None = 0.05,
) -> TimeOffsetResult:
"""Refine ``δt`` with signed 3-axis rates using a known ``R_IMU_lidar``.
@@ -293,9 +294,23 @@ def refine_time_offset_signed(
f"corr={best_corr:.3f}, mag_corr={mag_at_best:.3f} (coarse_mag={mag_at_coarse:.3f}), "
f"search=±{half:.3f}s"
)
shift = abs(best_delta - float(delta_t_s))
if max_shift_s is not None and shift > float(max_shift_s):
notes.append(
f"signed refine rejected: |Δδt|={shift:.4f}s exceeds max_shift={float(max_shift_s):.4f}s; "
"keeping previous delta_t"
)
return TimeOffsetResult(
delta_t_s=float(delta_t_s),
correlation_peak=mag_at_coarse if mag_at_coarse > 0 else best_corr,
search_s=search_s,
notes=tuple(notes),
ok=True,
)
# Require a meaningful MSE drop so tiny downhill noise cannot walk δt across iterations.
improved = (
np.isfinite(best_cost)
and best_cost < coarse_cost * 0.999
and best_cost < coarse_cost * 0.98
# Do not sacrifice the more reliable magnitude alignment for a noisy signed MSE gain.
and mag_at_best + 1e-4 >= mag_at_coarse
)