支持主机桥接后固定δt与旋转先验,并落盘运动对供可视化直读。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -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
|
||||
)
|
||||
|
||||
Reference in New Issue
Block a user