lichun.qu/calibration
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

新增独立RTK与IMU外参标定流程及质量验证

Browse Source
This commit is contained in:
lichun.qu
2026-08-21 10:04:41 +08:00
parent 1233f8aafd
commit c2da6dd192
38 changed files with 3937 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
imu_lidar/imu_preintegration.py
+15 -7
View File
@@ -158,9 +158,14 @@ def preintegrate_gyro(
if dt <= 0:
continue
# Exact endpoint gyro via linear interpolation inside the sample interval.
g_a = _interp_gyro(times_s, gyro_rad_s, seg0)
g_b = _interp_gyro(times_s, gyro_rad_s, seg1)
# Exact local endpoint interpolation. ``seg0`` and ``seg1`` are inside
# this adjacent sample interval, so scanning the full series with
# np.interp here would turn pair construction into quadratic work.
sample_dt = max(t_b - t_a, 1e-12)
u0 = (seg0 - t_a) / sample_dt
u1 = (seg1 - t_a) / sample_dt
g_a = (1.0 - u0) * gyro_rad_s[index] + u0 * gyro_rad_s[index + 1]
g_b = (1.0 - u1) * gyro_rad_s[index] + u1 * gyro_rad_s[index + 1]
omega = 0.5 * (g_a + g_b) - bias
gyro_norms.append(float(np.linalg.norm(omega)))
@@ -279,10 +284,13 @@ def preintegrate_imu(
if dt <= 0:
continue
g_a = _interp_vec(times_s, gyro_rad_s, seg0)
g_b = _interp_vec(times_s, gyro_rad_s, seg1)
a_a = _interp_vec(times_s, acc_m_s2, seg0)
a_b = _interp_vec(times_s, acc_m_s2, seg1)
sample_dt = max(t_b - t_a, 1e-12)
u0 = (seg0 - t_a) / sample_dt
u1 = (seg1 - t_a) / sample_dt
g_a = (1.0 - u0) * gyro_rad_s[index] + u0 * gyro_rad_s[index + 1]
g_b = (1.0 - u1) * gyro_rad_s[index] + u1 * gyro_rad_s[index + 1]
a_a = (1.0 - u0) * acc_m_s2[index] + u0 * acc_m_s2[index + 1]
a_b = (1.0 - u1) * acc_m_s2[index] + u1 * acc_m_s2[index + 1]
omega = 0.5 * (g_a + g_b) - bg
acc = 0.5 * (a_a + a_b) - ba
gyro_norms.append(float(np.linalg.norm(omega)))