更新雷达相位中心 CAD 先验,并放宽主机桥接后的弱相关峰 δt 门控。
Co-authored-by: Cursor <cursoragent@cursor.com>
This commit is contained in:
@@ -13,9 +13,10 @@ installation:
|
|||||||
installation_id: "20260808_priority_windows"
|
installation_id: "20260808_priority_windows"
|
||||||
installed_at: "2026-08-08"
|
installed_at: "2026-08-08"
|
||||||
notes: >
|
notes: >
|
||||||
HI13R4 + H32 DLogCapture. CAD mounts are origins vs rear axle center only
|
HI13R4 + H32 DLogCapture. CAD mounts are origins vs rear axle center
|
||||||
(translation). IMU axes confirmed on vehicle as HI13R4 manual §2.4 RFU
|
(translation only). LiDAR phase-center Z = CAD dZ + 63.5 mm.
|
||||||
(X right, Y forward, Z up). LiDAR Cartesian assumed body-aligned.
|
IMU axes: HI13R4 manual §2.4 RFU (X right, Y forward, Z up).
|
||||||
|
LiDAR Cartesian assumed body-aligned.
|
||||||
|
|
||||||
sensors:
|
sensors:
|
||||||
imu:
|
imu:
|
||||||
@@ -25,7 +26,8 @@ sensors:
|
|||||||
axes: "X right, Y forward, Z up (RFU)"
|
axes: "X right, Y forward, Z up (RFU)"
|
||||||
driver_axis_remapped: false
|
driver_axis_remapped: false
|
||||||
mount_in_body:
|
mount_in_body:
|
||||||
# CAD 原点相对后轮轴中心(body: X fwd, Y left, Z up),单位 m
|
# CAD 图二:后轮轴中心 → IMU,单位 m(mm/1000)
|
||||||
|
# dX=2574.126255, dY=36.5, dZ=892.5
|
||||||
translation_m: [2.574126255, 0.0365, 0.8925]
|
translation_m: [2.574126255, 0.0365, 0.8925]
|
||||||
# body <- imu : p_body = R_body_imu * p_imu
|
# body <- imu : p_body = R_body_imu * p_imu
|
||||||
# R_body_imu = [[0,1,0],[-1,0,0],[0,0,1]] (fwd=imu_y, left=-imu_x, up=imu_z)
|
# R_body_imu = [[0,1,0],[-1,0,0],[0,0,1]] (fwd=imu_y, left=-imu_x, up=imu_z)
|
||||||
@@ -40,11 +42,13 @@ sensors:
|
|||||||
axes: "X forward, Y left, Z up (Cartesian metres in NPZ points)"
|
axes: "X forward, Y left, Z up (Cartesian metres in NPZ points)"
|
||||||
driver_axis_remapped: false
|
driver_axis_remapped: false
|
||||||
mount_in_body:
|
mount_in_body:
|
||||||
translation_m: [2.522276859, 0.000020526, 1.637499879]
|
# CAD 图一:后轮轴中心 → 雷达安装点,再加相位中心 +63.5 mm(仅 Z)
|
||||||
|
# dX=2522.276859, dY=0.020526, dZ=1637.499879+63.5=1700.999879
|
||||||
|
translation_m: [2.522276859, 0.000020526, 1.700999879]
|
||||||
# 假设雷达系与车体 CAD 轴一致(导出 XYZ 已按此约定)
|
# 假设雷达系与车体 CAD 轴一致(导出 XYZ 已按此约定)
|
||||||
rotation_matrix_body_lidar: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
|
rotation_matrix_body_lidar: [[1.0, 0.0, 0.0], [0.0, 1.0, 0.0], [0.0, 0.0, 1.0]]
|
||||||
rotation_quaternion_xyzw: null
|
rotation_quaternion_xyzw: null
|
||||||
source: "CAD dX/dY/dZ vs rear axle; attitude assumed = body"
|
source: "CAD dX/dY/dZ + phase-center +63.5mm on Z; attitude assumed = body"
|
||||||
|
|
||||||
rtk:
|
rtk:
|
||||||
frame_definition: ""
|
frame_definition: ""
|
||||||
@@ -62,18 +66,18 @@ time:
|
|||||||
# t_IMU_lidar = R_IMU_body * t_body, R_IMU_lidar = R_IMU_body * R_body_lidar
|
# t_IMU_lidar = R_IMU_body * t_body, R_IMU_lidar = R_IMU_body * R_body_lidar
|
||||||
derived_T_IMU_lidar_prior:
|
derived_T_IMU_lidar_prior:
|
||||||
R_IMU_lidar: [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]
|
R_IMU_lidar: [[0.0, -1.0, 0.0], [1.0, 0.0, 0.0], [0.0, 0.0, 1.0]]
|
||||||
t_IMU_lidar_m: [0.036479474, -0.051849396, 0.744999879]
|
t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879]
|
||||||
t_lidar_from_imu_in_body_m: [-0.051849396, -0.036479474, 0.744999879]
|
t_lidar_from_imu_in_body_m: [-0.051849396, -0.036479474, 0.808499879]
|
||||||
notes: >
|
notes: >
|
||||||
Rotation prior is ~90 deg yaw between body/lidar (X-fwd) and IMU RFU (Y-fwd).
|
Rotation prior is ~90 deg yaw between body/lidar (X-fwd) and IMU RFU (Y-fwd).
|
||||||
Translation prior from CAD only; use for full_se3 / sanity, not as hard lock
|
Translation prior from CAD + LiDAR phase-center offset; use for full_se3 /
|
||||||
for rotation_only.
|
sanity, not as hard lock for rotation_only.
|
||||||
|
|
||||||
initialization:
|
initialization:
|
||||||
translation_prior:
|
translation_prior:
|
||||||
enabled: true
|
enabled: true
|
||||||
sigma_m: [0.05, 0.05, 0.05]
|
sigma_m: [0.05, 0.05, 0.05]
|
||||||
t_IMU_lidar_m: [0.036479474, -0.051849396, 0.744999879]
|
t_IMU_lidar_m: [0.036479474, -0.051849396, 0.808499879]
|
||||||
rotation_prior:
|
rotation_prior:
|
||||||
enabled: true
|
enabled: true
|
||||||
sigma_deg: 15.0
|
sigma_deg: 15.0
|
||||||
|
|||||||
@@ -137,8 +137,16 @@ def estimate_time_offset(
|
|||||||
f"LiDAR mean pair rotation {np.mean([rotation_angle_deg(r) for r in rotations]):.2f} deg"
|
f"LiDAR mean pair rotation {np.mean([rotation_angle_deg(r) for r in rotations]):.2f} deg"
|
||||||
)
|
)
|
||||||
notes.append(f"searched delta_t in ±{search_s:.3f}s by direct correlation")
|
notes.append(f"searched delta_t in ±{search_s:.3f}s by direct correlation")
|
||||||
ok = peak > 0.15
|
# Host-UTC-bridged sessions are already on one timeline; |ω| peak can stay
|
||||||
if not ok:
|
# weak even at the correct lag (ICP rate vs gyro scale). Accept near-zero δt.
|
||||||
|
near_zero = abs(float(delta)) <= min(0.05, 0.25 * float(search_s))
|
||||||
|
ok = peak > 0.15 or near_zero
|
||||||
|
if peak <= 0.15 and near_zero:
|
||||||
|
notes.append(
|
||||||
|
f"correlation peak weak ({peak:.3f}) but |delta_t|={abs(delta):.4f}s ~0; "
|
||||||
|
"accepting as already-aligned (e.g. host UTC bridge)"
|
||||||
|
)
|
||||||
|
elif not ok:
|
||||||
notes.append("correlation peak is weak; check overlapping motion and axis units")
|
notes.append("correlation peak is weak; check overlapping motion and axis units")
|
||||||
return TimeOffsetResult(
|
return TimeOffsetResult(
|
||||||
delta_t_s=delta,
|
delta_t_s=delta,
|
||||||
|
|||||||
Reference in New Issue
Block a user