新增独立RTK与IMU外参标定流程及质量验证
This commit is contained in:
@@ -0,0 +1,17 @@
|
||||
# RTK–IMU 标定产物说明
|
||||
|
||||
- `all_sessions/`:8 会话、5 s 平移节点、带 conditional rotation LOO 和 translation LOO 的当前完整基线。
|
||||
- `rotation_hpr_time/`:改用 GNHPR 自带测量时刻后的全量 rotation-only 对照。
|
||||
- `rotation_smoke/`:较早的 GGA 最近邻姿态时刻对照,不作为当前结果。
|
||||
- `batch_0808_full_smoke/`:0808 三会话完整诊断。
|
||||
- `batch_0815_rotation/`:0815 四会话 rotation-only 诊断。
|
||||
- `single_smoke/`:早期单会话性能/数值冒烟,不作为当前结果。
|
||||
|
||||
每个正式运行目录包含:
|
||||
|
||||
- `dataset_audit.json`:样本数、固定解比例、共同时间范围和 ENU 原点。
|
||||
- `rotation_result.json`:旋转、RPY、时间审计、GNHPR 候选、偏置、残差、协方差、逐会话指标和 LOO。
|
||||
- `translation_result.json`:杆臂、平移、齐次矩阵、协方差/秩、位置/速度残差、偏置和 LOO。
|
||||
- `summary.json`:供程序读取的最终状态和候选矩阵。
|
||||
|
||||
当前 `all_sessions/summary.json` 是 `diagnostic_not_accepted`。其中平移约 `[0.771, 0.569, -21.073] m` 明显不具机械真实性,禁止用于车辆配置。完整解释见 `docs/rtk_imu_calibration.md`。
|
||||
@@ -0,0 +1,157 @@
|
||||
{
|
||||
"session_count": 8,
|
||||
"sessions": [
|
||||
{
|
||||
"session_id": "priority_174005_174515",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 31000,
|
||||
"rtk_samples": 4780,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.854602510460251,
|
||||
"common_time_span_s": [
|
||||
16265.2325992,
|
||||
16575.0298107
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.465514786,
|
||||
114.092169888,
|
||||
29.4695
|
||||
],
|
||||
"imu_source": "31000 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "priority_174905_175450",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 34499,
|
||||
"rtk_samples": 5211,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8547303780464403,
|
||||
"common_time_span_s": [
|
||||
16805.1862481,
|
||||
17150.0907328
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4653424457,
|
||||
114.092237385,
|
||||
29.5366
|
||||
],
|
||||
"imu_source": "34499 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "priority_175910_180530",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 37998,
|
||||
"rtk_samples": 5786,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8567231247839613,
|
||||
"common_time_span_s": [
|
||||
17410.2121738,
|
||||
17790.0458228
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654151935,
|
||||
114.090796384,
|
||||
29.5317
|
||||
],
|
||||
"imu_source": "37998 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "slope_190548_190730",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 10199,
|
||||
"rtk_samples": 1578,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8757921419518377,
|
||||
"common_time_span_s": [
|
||||
36466.328875,
|
||||
36568.2301441
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4652183602,
|
||||
114.090839984,
|
||||
29.9891
|
||||
],
|
||||
"imu_source": "10199 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "circle_193412_193642",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 14989,
|
||||
"rtk_samples": 2162,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8288621646623496,
|
||||
"common_time_span_s": [
|
||||
38170.3385255,
|
||||
38317.8571971
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654799242,
|
||||
114.092155912,
|
||||
29.4779
|
||||
],
|
||||
"imu_source": "14989 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "loop_194223_195003",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 45801,
|
||||
"rtk_samples": 6741,
|
||||
"fixed_position_ratio": 0.9998516540572615,
|
||||
"fixed_attitude_ratio": 0.8377095386441181,
|
||||
"common_time_span_s": [
|
||||
38661.2831251,
|
||||
39121.1709553
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654856957,
|
||||
114.092151015,
|
||||
29.4651
|
||||
],
|
||||
"imu_source": "45801 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "accel_195608_195958",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 23002,
|
||||
"rtk_samples": 3456,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8457754629629629,
|
||||
"common_time_span_s": [
|
||||
39486.3574509,
|
||||
39716.3199405
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.465443014,
|
||||
114.092179168,
|
||||
29.5309
|
||||
],
|
||||
"imu_source": "23002 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "motion_sms_154023_154359",
|
||||
"batch_id": "0819",
|
||||
"imu_samples": 21556,
|
||||
"rtk_samples": 3294,
|
||||
"fixed_position_ratio": 0.49271402550091076,
|
||||
"fixed_attitude_ratio": 0.4344262295081967,
|
||||
"common_time_span_s": [
|
||||
25829.8725525,
|
||||
26027.4217529
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4651580863,
|
||||
114.090773052,
|
||||
36.5626
|
||||
],
|
||||
"imu_source": "21556 normalized samples",
|
||||
"rtk_source": "D:\\data\\0819\\dense5\\sessions_v2_device_affine\\motion_sms_154023_154359\\rtk.csv"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,130 @@
|
||||
{
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9996841792313951,
|
||||
0.02499811511405725,
|
||||
-0.002576050309343804
|
||||
],
|
||||
[
|
||||
-0.025002617750669198,
|
||||
0.9996858874629868,
|
||||
-0.0017307550243271697
|
||||
],
|
||||
[
|
||||
0.002531975526313299,
|
||||
0.0017946164171362589,
|
||||
0.9999951842143289
|
||||
]
|
||||
],
|
||||
"rpy_deg": [
|
||||
0.1028243313393031,
|
||||
-0.1450716664951083,
|
||||
-1.43269836393699
|
||||
],
|
||||
"gyro_bias_by_session_rad_s": {
|
||||
"priority_174005_174515": [
|
||||
-8.90944066727157e-05,
|
||||
7.078609864979291e-05,
|
||||
0.0001460390779870233
|
||||
],
|
||||
"priority_174905_175450": [
|
||||
-4.327374730750894e-05,
|
||||
-0.0001097548390022348,
|
||||
3.000549725593387e-05
|
||||
],
|
||||
"priority_175910_180530": [
|
||||
-7.036551812132934e-05,
|
||||
-0.0008054961048184173,
|
||||
1.3026193503057623e-05
|
||||
],
|
||||
"slope_190548_190730": [
|
||||
6.035222904568755e-05,
|
||||
-8.913884157613711e-06,
|
||||
0.00015470949872434692
|
||||
],
|
||||
"circle_193412_193642": [
|
||||
-3.696799268832588e-05,
|
||||
-0.00019200076224600124,
|
||||
-5.9567987995056394e-05
|
||||
],
|
||||
"loop_194223_195003": [
|
||||
-0.0001979655352189002,
|
||||
-0.00015937450336643818,
|
||||
-4.343089303706036e-05
|
||||
],
|
||||
"accel_195608_195958": [
|
||||
-7.841839817698704e-05,
|
||||
-2.604932294598783e-06,
|
||||
5.8586555655011475e-05
|
||||
],
|
||||
"motion_sms_154023_154359": [
|
||||
0.00011745666467620586,
|
||||
0.00021434926285955486,
|
||||
-4.5147354343760625e-05
|
||||
]
|
||||
},
|
||||
"time_offset": {
|
||||
"offset_s": 0.04000000000000031,
|
||||
"peak_correlation": 0.5788788671828708,
|
||||
"second_best_correlation": 0.5773967219219845,
|
||||
"evaluated_samples": 12394,
|
||||
"reliable": false
|
||||
},
|
||||
"applied_time_offset_s": 0.0,
|
||||
"convention": {
|
||||
"name": "north_cw__pitch_nose_up__roll_right_down",
|
||||
"heading_sign": -1.0,
|
||||
"pitch_sign": -1.0,
|
||||
"roll_sign": 1.0
|
||||
},
|
||||
"convention_scores_deg": {
|
||||
"north_cw__pitch_nose_up__roll_right_down": 1.631322241364994,
|
||||
"north_cw__pitch_opposite": 1.737582794831228,
|
||||
"heading_opposite__pitch_nose_up": 7.263259116648845,
|
||||
"heading_opposite__pitch_opposite": 17.716319437929055
|
||||
},
|
||||
"pair_count": 731,
|
||||
"residual_rms_deg": 1.6276839301413086,
|
||||
"residual_median_deg": 0.5695938849052221,
|
||||
"residual_p95_deg": 3.0903953005641345,
|
||||
"rotation_std_deg": [
|
||||
0.2340126126406699,
|
||||
0.20048574131667432,
|
||||
2.314692068045375
|
||||
],
|
||||
"information_singular_values": [
|
||||
82178.77940373585,
|
||||
60441.86300479194,
|
||||
612.6358640387364
|
||||
],
|
||||
"per_session_rms_deg": {
|
||||
"priority_174005_174515": 0.6134232617562615,
|
||||
"priority_174905_175450": 2.2485932625979106,
|
||||
"priority_175910_180530": 3.095861798757508,
|
||||
"slope_190548_190730": 2.388478834012087,
|
||||
"circle_193412_193642": 0.5629885032374518,
|
||||
"loop_194223_195003": 0.6250976606325254,
|
||||
"accel_195608_195958": 0.975856888271022,
|
||||
"motion_sms_154023_154359": 1.3698317592842066
|
||||
},
|
||||
"loo_delta_deg": {
|
||||
"priority_174005_174515": 0.7895996403426686,
|
||||
"priority_174905_175450": 0.9224102138653388,
|
||||
"priority_175910_180530": 5.144190932547379,
|
||||
"slope_190548_190730": 0.24769087330915346,
|
||||
"circle_193412_193642": 1.2637729378484346,
|
||||
"loop_194223_195003": 0.4844375238260164,
|
||||
"accel_195608_195958": 1.0976412402728102,
|
||||
"motion_sms_154023_154359": 0.6896079810115167
|
||||
},
|
||||
"ok": false,
|
||||
"notes": [
|
||||
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||
"GNHPR convention score gap=0.1063 deg",
|
||||
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
|
||||
"LOO is conditional: per-session gyro biases are held at their all-session estimates",
|
||||
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||
"rotation failed one or more strict acceptance gates"
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,65 @@
|
||||
{
|
||||
"status": "diagnostic_not_accepted",
|
||||
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||
"rtk_frame_definition": "",
|
||||
"rtk_reference_point": "",
|
||||
"interpretation_blockers": [
|
||||
"rotation quality gates failed",
|
||||
"translation quality gates failed or were not run",
|
||||
"RTK frame_definition is empty",
|
||||
"RTK reference_point is empty"
|
||||
],
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9996841792313951,
|
||||
0.02499811511405725,
|
||||
-0.002576050309343804
|
||||
],
|
||||
[
|
||||
-0.025002617750669198,
|
||||
0.9996858874629868,
|
||||
-0.0017307550243271697
|
||||
],
|
||||
[
|
||||
0.002531975526313299,
|
||||
0.0017946164171362589,
|
||||
0.9999951842143289
|
||||
]
|
||||
],
|
||||
"t_RTK_IMU_m": [
|
||||
0.7710937276592442,
|
||||
0.5693018309069646,
|
||||
-21.07303743539233
|
||||
],
|
||||
"T_RTK_IMU": [
|
||||
[
|
||||
0.9996841792313951,
|
||||
0.02499811511405725,
|
||||
-0.002576050309343804,
|
||||
0.7710937276592442
|
||||
],
|
||||
[
|
||||
-0.025002617750669198,
|
||||
0.9996858874629868,
|
||||
-0.0017307550243271697,
|
||||
0.5693018309069646
|
||||
],
|
||||
[
|
||||
0.002531975526313299,
|
||||
0.0017946164171362589,
|
||||
0.9999951842143289,
|
||||
-21.07303743539233
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"rotation_ok": false,
|
||||
"translation_ok": false,
|
||||
"rotation_result": "rotation_result.json",
|
||||
"translation_result": "translation_result.json",
|
||||
"dataset_audit": "dataset_audit.json"
|
||||
}
|
||||
@@ -0,0 +1,161 @@
|
||||
{
|
||||
"lever_IMU_to_RTK_in_IMU_m": [
|
||||
-0.7032597491310882,
|
||||
-0.5505808768918061,
|
||||
21.07590765040047
|
||||
],
|
||||
"t_RTK_IMU_m": [
|
||||
0.7710937276592442,
|
||||
0.5693018309069646,
|
||||
-21.07303743539233
|
||||
],
|
||||
"T_RTK_IMU": [
|
||||
[
|
||||
0.9996841792313951,
|
||||
0.02499811511405725,
|
||||
-0.002576050309343804,
|
||||
0.7710937276592442
|
||||
],
|
||||
[
|
||||
-0.025002617750669198,
|
||||
0.9996858874629868,
|
||||
-0.0017307550243271697,
|
||||
0.5693018309069646
|
||||
],
|
||||
[
|
||||
0.002531975526313299,
|
||||
0.0017946164171362589,
|
||||
0.9999951842143289,
|
||||
-21.07303743539233
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"translation_std_m": [
|
||||
0.367657527272358,
|
||||
0.36764001580136974,
|
||||
2.960858932902556
|
||||
],
|
||||
"lever_information_singular_values": [
|
||||
7.467287688797444,
|
||||
7.419771560016465,
|
||||
0.11404687553142893
|
||||
],
|
||||
"lever_precision_rank": 3,
|
||||
"position_residual_rms_xyz_m": [
|
||||
0.0026057774092802665,
|
||||
0.0026172203224947795,
|
||||
0.0008707288139887198
|
||||
],
|
||||
"velocity_residual_rms_xyz_m_s": [
|
||||
1.0344084464154262,
|
||||
1.0018456371907998,
|
||||
0.08189262443294992
|
||||
],
|
||||
"accel_bias_by_session_m_s2": {
|
||||
"priority_174005_174515": [
|
||||
-0.0033446448235936025,
|
||||
0.04850567116751809,
|
||||
0.014864908749227664
|
||||
],
|
||||
"priority_174905_175450": [
|
||||
0.051484854191344014,
|
||||
0.009750775340658668,
|
||||
0.016837766756117954
|
||||
],
|
||||
"priority_175910_180530": [
|
||||
-0.0012098409852818557,
|
||||
0.04428237322299069,
|
||||
0.015044444493724668
|
||||
],
|
||||
"slope_190548_190730": [
|
||||
0.007720083607057322,
|
||||
-0.07045791547639069,
|
||||
0.011096872492579776
|
||||
],
|
||||
"circle_193412_193642": [
|
||||
0.010695150095323893,
|
||||
0.07124837152622766,
|
||||
0.012181970165526993
|
||||
],
|
||||
"loop_194223_195003": [
|
||||
0.017798177348527063,
|
||||
0.026531175431114495,
|
||||
0.013517325106459492
|
||||
],
|
||||
"accel_195608_195958": [
|
||||
0.010843370014374661,
|
||||
0.0021847264023688797,
|
||||
0.012720554759472001
|
||||
],
|
||||
"motion_sms_154023_154359": [
|
||||
-0.01917068926914634,
|
||||
0.04878246850510192,
|
||||
0.011986246228615108
|
||||
]
|
||||
},
|
||||
"knot_count_by_session": {
|
||||
"priority_174005_174515": 62,
|
||||
"priority_174905_175450": 69,
|
||||
"priority_175910_180530": 76,
|
||||
"slope_190548_190730": 21,
|
||||
"circle_193412_193642": 30,
|
||||
"loop_194223_195003": 92,
|
||||
"accel_195608_195958": 46,
|
||||
"motion_sms_154023_154359": 20
|
||||
},
|
||||
"loo_delta_m": {
|
||||
"priority_174005_174515": [
|
||||
-0.08631101169626099,
|
||||
-0.16878866579372004,
|
||||
-0.1969063529933237
|
||||
],
|
||||
"priority_174905_175450": [
|
||||
0.052982832020427084,
|
||||
0.23694572858391594,
|
||||
0.5921699991558107
|
||||
],
|
||||
"priority_175910_180530": [
|
||||
-0.03728037456994515,
|
||||
-0.0587695607071852,
|
||||
0.36745157813128415
|
||||
],
|
||||
"slope_190548_190730": [
|
||||
0.06820599394639204,
|
||||
0.13646009238200618,
|
||||
0.8245922313333871
|
||||
],
|
||||
"circle_193412_193642": [
|
||||
-0.06745036261863124,
|
||||
-0.18645839932341524,
|
||||
-0.8300513550738842
|
||||
],
|
||||
"loop_194223_195003": [
|
||||
0.12045919921905746,
|
||||
0.06572884705689208,
|
||||
-1.774476169745249
|
||||
],
|
||||
"accel_195608_195958": [
|
||||
0.019243586803303625,
|
||||
-0.002985337002138544,
|
||||
0.3653882998138158
|
||||
],
|
||||
"motion_sms_154023_154359": [
|
||||
-0.0383153365081248,
|
||||
0.027686850248512473,
|
||||
0.507160468738487
|
||||
]
|
||||
},
|
||||
"ok": false,
|
||||
"notes": [
|
||||
"lever l is vector IMU-origin -> RTK-origin expressed in IMU",
|
||||
"transform translation uses t_RTK_IMU = -R_RTK_IMU @ l",
|
||||
"RTK position is never differentiated; position and velocity preintegration factors are solved jointly",
|
||||
"upstream rotation is not accepted, so translation is diagnostic only",
|
||||
"translation failed one or more strict acceptance gates"
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,62 @@
|
||||
{
|
||||
"session_count": 3,
|
||||
"sessions": [
|
||||
{
|
||||
"session_id": "priority_174005_174515",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 31000,
|
||||
"rtk_samples": 4780,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.854602510460251,
|
||||
"common_time_span_s": [
|
||||
16265.2325992,
|
||||
16575.0298107
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.465514786,
|
||||
114.092169888,
|
||||
29.4695
|
||||
],
|
||||
"imu_source": "31000 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "priority_174905_175450",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 34499,
|
||||
"rtk_samples": 5211,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8547303780464403,
|
||||
"common_time_span_s": [
|
||||
16805.1862481,
|
||||
17150.0907328
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4653424457,
|
||||
114.092237385,
|
||||
29.5366
|
||||
],
|
||||
"imu_source": "34499 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "priority_175910_180530",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 37998,
|
||||
"rtk_samples": 5786,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8567231247839613,
|
||||
"common_time_span_s": [
|
||||
17410.2121738,
|
||||
17790.0458228
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654151935,
|
||||
114.090796384,
|
||||
29.5317
|
||||
],
|
||||
"imu_source": "37998 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,89 @@
|
||||
{
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9996274297857513,
|
||||
0.0272885099045028,
|
||||
-0.0005821057674967719
|
||||
],
|
||||
[
|
||||
-0.027285914629035343,
|
||||
0.9996193516550798,
|
||||
0.0040780705652228135
|
||||
],
|
||||
[
|
||||
0.0006931686589101494,
|
||||
-0.004060667909321538,
|
||||
0.9999915152106744
|
||||
]
|
||||
],
|
||||
"rpy_deg": [
|
||||
-0.23265982849493025,
|
||||
-0.03971564182674239,
|
||||
-1.5635621825359411
|
||||
],
|
||||
"gyro_bias_by_session_rad_s": {
|
||||
"priority_174005_174515": [
|
||||
-9.355487703345326e-05,
|
||||
6.022278219673199e-05,
|
||||
0.0001599879605718389
|
||||
],
|
||||
"priority_174905_175450": [
|
||||
-6.763782096065313e-05,
|
||||
-0.00013271246982721386,
|
||||
2.7304435743171578e-05
|
||||
],
|
||||
"priority_175910_180530": [
|
||||
-4.9469524877665416e-05,
|
||||
-0.0005605818923478396,
|
||||
4.72341323992995e-06
|
||||
]
|
||||
},
|
||||
"time_offset": {
|
||||
"offset_s": 0.07500000000000034,
|
||||
"peak_correlation": 0.2075894836191556,
|
||||
"second_best_correlation": 0.2075830757517665,
|
||||
"evaluated_samples": 6286,
|
||||
"reliable": false
|
||||
},
|
||||
"convention": {
|
||||
"name": "north_cw__pitch_nose_up__roll_right_down",
|
||||
"heading_sign": -1.0,
|
||||
"pitch_sign": -1.0,
|
||||
"roll_sign": 1.0
|
||||
},
|
||||
"convention_scores_deg": {
|
||||
"north_cw__pitch_nose_up__roll_right_down": 2.124836124403544,
|
||||
"north_cw__pitch_opposite": 2.246244073697405,
|
||||
"heading_opposite__pitch_nose_up": 14.081722937345578,
|
||||
"heading_opposite__pitch_opposite": 14.187723789068325
|
||||
},
|
||||
"pair_count": 347,
|
||||
"residual_rms_deg": 2.121956852743709,
|
||||
"residual_median_deg": 0.6620410270629032,
|
||||
"residual_p95_deg": 4.932454679529714,
|
||||
"rotation_std_deg": [
|
||||
0.5378381823216046,
|
||||
0.43991569563643235,
|
||||
3.604673882923754
|
||||
],
|
||||
"information_singular_values": [
|
||||
17058.156967522238,
|
||||
11392.471369813427,
|
||||
252.6038959367189
|
||||
],
|
||||
"per_session_rms_deg": {
|
||||
"priority_174005_174515": 0.6074812737155098,
|
||||
"priority_174905_175450": 2.2498284381524494,
|
||||
"priority_175910_180530": 3.09642157433063
|
||||
},
|
||||
"loo_delta_deg": {},
|
||||
"ok": false,
|
||||
"notes": [
|
||||
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||
"GNHPR convention score gap=0.1214 deg",
|
||||
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
|
||||
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||
"rotation failed one or more strict acceptance gates"
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,57 @@
|
||||
{
|
||||
"status": "diagnostic_not_accepted",
|
||||
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9996274297857513,
|
||||
0.0272885099045028,
|
||||
-0.0005821057674967719
|
||||
],
|
||||
[
|
||||
-0.027285914629035343,
|
||||
0.9996193516550798,
|
||||
0.0040780705652228135
|
||||
],
|
||||
[
|
||||
0.0006931686589101494,
|
||||
-0.004060667909321538,
|
||||
0.9999915152106744
|
||||
]
|
||||
],
|
||||
"t_RTK_IMU_m": [
|
||||
1.017036626166409,
|
||||
0.7992624136748191,
|
||||
-22.250154010194304
|
||||
],
|
||||
"T_RTK_IMU": [
|
||||
[
|
||||
0.9996274297857513,
|
||||
0.0272885099045028,
|
||||
-0.0005821057674967719,
|
||||
1.017036626166409
|
||||
],
|
||||
[
|
||||
-0.027285914629035343,
|
||||
0.9996193516550798,
|
||||
0.0040780705652228135,
|
||||
0.7992624136748191
|
||||
],
|
||||
[
|
||||
0.0006931686589101494,
|
||||
-0.004060667909321538,
|
||||
0.9999915152106744,
|
||||
-22.250154010194304
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"rotation_ok": false,
|
||||
"translation_ok": false,
|
||||
"rotation_result": "rotation_result.json",
|
||||
"translation_result": "translation_result.json",
|
||||
"dataset_audit": "dataset_audit.json"
|
||||
}
|
||||
@@ -0,0 +1,90 @@
|
||||
{
|
||||
"lever_IMU_to_RTK_in_IMU_m": [
|
||||
-0.979425993211181,
|
||||
-0.9170620761729408,
|
||||
22.247297796687818
|
||||
],
|
||||
"t_RTK_IMU_m": [
|
||||
1.017036626166409,
|
||||
0.7992624136748191,
|
||||
-22.250154010194304
|
||||
],
|
||||
"T_RTK_IMU": [
|
||||
[
|
||||
0.9996274297857513,
|
||||
0.0272885099045028,
|
||||
-0.0005821057674967719,
|
||||
1.017036626166409
|
||||
],
|
||||
[
|
||||
-0.027285914629035343,
|
||||
0.9996193516550798,
|
||||
0.0040780705652228135,
|
||||
0.7992624136748191
|
||||
],
|
||||
[
|
||||
0.0006931686589101494,
|
||||
-0.004060667909321538,
|
||||
0.9999915152106744,
|
||||
-22.250154010194304
|
||||
],
|
||||
[
|
||||
0.0,
|
||||
0.0,
|
||||
0.0,
|
||||
1.0
|
||||
]
|
||||
],
|
||||
"translation_std_m": [
|
||||
0.6410402171684149,
|
||||
0.6447113787519164,
|
||||
3.8963000265795396
|
||||
],
|
||||
"lever_information_singular_values": [
|
||||
2.4406600443109308,
|
||||
2.412462748498746,
|
||||
0.06586096829714262
|
||||
],
|
||||
"lever_precision_rank": 3,
|
||||
"position_residual_rms_xyz_m": [
|
||||
0.0023101392989052683,
|
||||
0.002794889168410421,
|
||||
0.0005246719035281052
|
||||
],
|
||||
"velocity_residual_rms_xyz_m_s": [
|
||||
1.1059333756244145,
|
||||
1.368704336610928,
|
||||
0.11603076744633863
|
||||
],
|
||||
"accel_bias_by_session_m_s2": {
|
||||
"priority_174005_174515": [
|
||||
0.015165392523580496,
|
||||
0.10623623265620695,
|
||||
0.015221547657584966
|
||||
],
|
||||
"priority_174905_175450": [
|
||||
0.06725309871307823,
|
||||
0.06785033078439083,
|
||||
0.0171641139816057
|
||||
],
|
||||
"priority_175910_180530": [
|
||||
0.015302724485125336,
|
||||
0.10160587655627046,
|
||||
0.015281702682303734
|
||||
]
|
||||
},
|
||||
"knot_count_by_session": {
|
||||
"priority_174005_174515": 62,
|
||||
"priority_174905_175450": 69,
|
||||
"priority_175910_180530": 76
|
||||
},
|
||||
"loo_delta_m": {},
|
||||
"ok": false,
|
||||
"notes": [
|
||||
"lever l is vector IMU-origin -> RTK-origin expressed in IMU",
|
||||
"transform translation uses t_RTK_IMU = -R_RTK_IMU @ l",
|
||||
"RTK position is never differentiated; position and velocity preintegration factors are solved jointly",
|
||||
"upstream rotation is not accepted, so translation is diagnostic only",
|
||||
"translation failed one or more strict acceptance gates"
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,81 @@
|
||||
{
|
||||
"session_count": 4,
|
||||
"sessions": [
|
||||
{
|
||||
"session_id": "slope_190548_190730",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 10199,
|
||||
"rtk_samples": 1578,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8757921419518377,
|
||||
"common_time_span_s": [
|
||||
36466.328875,
|
||||
36568.2301441
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4652183602,
|
||||
114.090839984,
|
||||
29.9891
|
||||
],
|
||||
"imu_source": "10199 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "circle_193412_193642",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 14989,
|
||||
"rtk_samples": 2162,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8288621646623496,
|
||||
"common_time_span_s": [
|
||||
38170.3385255,
|
||||
38317.8571971
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654799242,
|
||||
114.092155912,
|
||||
29.4779
|
||||
],
|
||||
"imu_source": "14989 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "loop_194223_195003",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 45801,
|
||||
"rtk_samples": 6741,
|
||||
"fixed_position_ratio": 0.9998516540572615,
|
||||
"fixed_attitude_ratio": 0.8377095386441181,
|
||||
"common_time_span_s": [
|
||||
38661.2831251,
|
||||
39121.1709553
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654856957,
|
||||
114.092151015,
|
||||
29.4651
|
||||
],
|
||||
"imu_source": "45801 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "accel_195608_195958",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 23002,
|
||||
"rtk_samples": 3456,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8457754629629629,
|
||||
"common_time_span_s": [
|
||||
39486.3574509,
|
||||
39716.3199405
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.465443014,
|
||||
114.092179168,
|
||||
29.5309
|
||||
],
|
||||
"imu_source": "23002 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,96 @@
|
||||
{
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9999453769836999,
|
||||
0.010155336209751328,
|
||||
-0.002472285459453144
|
||||
],
|
||||
[
|
||||
-0.010163396322394882,
|
||||
0.9999430054296535,
|
||||
-0.003269750373547743
|
||||
],
|
||||
[
|
||||
0.002438939138240274,
|
||||
0.003294698586865735,
|
||||
0.9999915982332558
|
||||
]
|
||||
],
|
||||
"rpy_deg": [
|
||||
0.18877322677308359,
|
||||
-0.13974105765052158,
|
||||
-0.5823314723802753
|
||||
],
|
||||
"gyro_bias_by_session_rad_s": {
|
||||
"slope_190548_190730": [
|
||||
5.517563410757097e-05,
|
||||
-1.0668622945796592e-05,
|
||||
0.00015474647141927933
|
||||
],
|
||||
"circle_193412_193642": [
|
||||
-4.092656265416791e-05,
|
||||
-0.00017676217236757262,
|
||||
-5.914138553067106e-05
|
||||
],
|
||||
"loop_194223_195003": [
|
||||
-0.000196149370561509,
|
||||
-0.0001486662046316796,
|
||||
-3.6375768145656054e-05
|
||||
],
|
||||
"accel_195608_195958": [
|
||||
-7.483644204529091e-05,
|
||||
-4.17320520562614e-06,
|
||||
5.861303135056825e-05
|
||||
]
|
||||
},
|
||||
"time_offset": {
|
||||
"offset_s": 0.04000000000000031,
|
||||
"peak_correlation": 0.7578237026149359,
|
||||
"second_best_correlation": 0.757682618781891,
|
||||
"evaluated_samples": 5443,
|
||||
"reliable": false
|
||||
},
|
||||
"convention": {
|
||||
"name": "north_cw__pitch_nose_up__roll_right_down",
|
||||
"heading_sign": -1.0,
|
||||
"pitch_sign": -1.0,
|
||||
"roll_sign": 1.0
|
||||
},
|
||||
"convention_scores_deg": {
|
||||
"north_cw__pitch_nose_up__roll_right_down": 0.9457675569035681,
|
||||
"north_cw__pitch_opposite": 1.064327146928918,
|
||||
"heading_opposite__pitch_nose_up": 5.753424726280977,
|
||||
"heading_opposite__pitch_opposite": 13.362267974377751
|
||||
},
|
||||
"pair_count": 351,
|
||||
"residual_rms_deg": 0.9428350359826574,
|
||||
"residual_median_deg": 0.4993317818354644,
|
||||
"residual_p95_deg": 1.5023594730593923,
|
||||
"rotation_std_deg": [
|
||||
0.191917244945292,
|
||||
0.16245924204992213,
|
||||
2.5710977852526464
|
||||
],
|
||||
"information_singular_values": [
|
||||
125441.66767899474,
|
||||
90572.5161990362,
|
||||
496.5407001788204
|
||||
],
|
||||
"per_session_rms_deg": {
|
||||
"slope_190548_190730": 2.386329048920402,
|
||||
"circle_193412_193642": 0.560395991888806,
|
||||
"loop_194223_195003": 0.6239401903763562,
|
||||
"accel_195608_195958": 0.9797575195873669
|
||||
},
|
||||
"loo_delta_deg": {},
|
||||
"ok": false,
|
||||
"notes": [
|
||||
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||
"GNHPR convention score gap=0.1186 deg",
|
||||
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
|
||||
"LOO is conditional: per-session gyro biases are held at their all-session estimates",
|
||||
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||
"rotation failed one or more strict acceptance gates"
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
{
|
||||
"status": "diagnostic_not_accepted",
|
||||
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9999453769836999,
|
||||
0.010155336209751328,
|
||||
-0.002472285459453144
|
||||
],
|
||||
[
|
||||
-0.010163396322394882,
|
||||
0.9999430054296535,
|
||||
-0.003269750373547743
|
||||
],
|
||||
[
|
||||
0.002438939138240274,
|
||||
0.003294698586865735,
|
||||
0.9999915982332558
|
||||
]
|
||||
],
|
||||
"t_RTK_IMU_m": null,
|
||||
"T_RTK_IMU": null,
|
||||
"rotation_ok": false,
|
||||
"translation_ok": null,
|
||||
"rotation_result": "rotation_result.json",
|
||||
"translation_result": null,
|
||||
"dataset_audit": "dataset_audit.json"
|
||||
}
|
||||
@@ -0,0 +1,157 @@
|
||||
{
|
||||
"session_count": 8,
|
||||
"sessions": [
|
||||
{
|
||||
"session_id": "priority_174005_174515",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 31000,
|
||||
"rtk_samples": 4780,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.854602510460251,
|
||||
"common_time_span_s": [
|
||||
16265.2325992,
|
||||
16575.0298107
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.465514786,
|
||||
114.092169888,
|
||||
29.4695
|
||||
],
|
||||
"imu_source": "31000 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "priority_174905_175450",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 34499,
|
||||
"rtk_samples": 5211,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8547303780464403,
|
||||
"common_time_span_s": [
|
||||
16805.1862481,
|
||||
17150.0907328
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4653424457,
|
||||
114.092237385,
|
||||
29.5366
|
||||
],
|
||||
"imu_source": "34499 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "priority_175910_180530",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 37998,
|
||||
"rtk_samples": 5786,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8567231247839613,
|
||||
"common_time_span_s": [
|
||||
17410.2121738,
|
||||
17790.0458228
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654151935,
|
||||
114.090796384,
|
||||
29.5317
|
||||
],
|
||||
"imu_source": "37998 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "slope_190548_190730",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 10199,
|
||||
"rtk_samples": 1578,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8757921419518377,
|
||||
"common_time_span_s": [
|
||||
36466.328875,
|
||||
36568.2301441
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4652183602,
|
||||
114.090839984,
|
||||
29.9891
|
||||
],
|
||||
"imu_source": "10199 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "circle_193412_193642",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 14989,
|
||||
"rtk_samples": 2162,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8288621646623496,
|
||||
"common_time_span_s": [
|
||||
38170.3385255,
|
||||
38317.8571971
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654799242,
|
||||
114.092155912,
|
||||
29.4779
|
||||
],
|
||||
"imu_source": "14989 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "loop_194223_195003",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 45801,
|
||||
"rtk_samples": 6741,
|
||||
"fixed_position_ratio": 0.9998516540572615,
|
||||
"fixed_attitude_ratio": 0.8377095386441181,
|
||||
"common_time_span_s": [
|
||||
38661.2831251,
|
||||
39121.1709553
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654856957,
|
||||
114.092151015,
|
||||
29.4651
|
||||
],
|
||||
"imu_source": "45801 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "accel_195608_195958",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 23002,
|
||||
"rtk_samples": 3456,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8457754629629629,
|
||||
"common_time_span_s": [
|
||||
39486.3574509,
|
||||
39716.3199405
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.465443014,
|
||||
114.092179168,
|
||||
29.5309
|
||||
],
|
||||
"imu_source": "23002 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "motion_sms_154023_154359",
|
||||
"batch_id": "0819",
|
||||
"imu_samples": 21556,
|
||||
"rtk_samples": 3294,
|
||||
"fixed_position_ratio": 0.49271402550091076,
|
||||
"fixed_attitude_ratio": 0.4344262295081967,
|
||||
"common_time_span_s": [
|
||||
25829.8725525,
|
||||
26027.4217529
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4651580863,
|
||||
114.090773052,
|
||||
36.5626
|
||||
],
|
||||
"imu_source": "21556 normalized samples",
|
||||
"rtk_source": "D:\\data\\0819\\dense5\\sessions_v2_device_affine\\motion_sms_154023_154359\\rtk.csv"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,119 @@
|
||||
{
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9996841792313951,
|
||||
0.02499811511405725,
|
||||
-0.002576050309343804
|
||||
],
|
||||
[
|
||||
-0.025002617750669198,
|
||||
0.9996858874629868,
|
||||
-0.0017307550243271697
|
||||
],
|
||||
[
|
||||
0.002531975526313299,
|
||||
0.0017946164171362589,
|
||||
0.9999951842143289
|
||||
]
|
||||
],
|
||||
"rpy_deg": [
|
||||
0.1028243313393031,
|
||||
-0.1450716664951083,
|
||||
-1.43269836393699
|
||||
],
|
||||
"gyro_bias_by_session_rad_s": {
|
||||
"priority_174005_174515": [
|
||||
-8.90944066727157e-05,
|
||||
7.078609864979291e-05,
|
||||
0.0001460390779870233
|
||||
],
|
||||
"priority_174905_175450": [
|
||||
-4.327374730750894e-05,
|
||||
-0.0001097548390022348,
|
||||
3.000549725593387e-05
|
||||
],
|
||||
"priority_175910_180530": [
|
||||
-7.036551812132934e-05,
|
||||
-0.0008054961048184173,
|
||||
1.3026193503057623e-05
|
||||
],
|
||||
"slope_190548_190730": [
|
||||
6.035222904568755e-05,
|
||||
-8.913884157613711e-06,
|
||||
0.00015470949872434692
|
||||
],
|
||||
"circle_193412_193642": [
|
||||
-3.696799268832588e-05,
|
||||
-0.00019200076224600124,
|
||||
-5.9567987995056394e-05
|
||||
],
|
||||
"loop_194223_195003": [
|
||||
-0.0001979655352189002,
|
||||
-0.00015937450336643818,
|
||||
-4.343089303706036e-05
|
||||
],
|
||||
"accel_195608_195958": [
|
||||
-7.841839817698704e-05,
|
||||
-2.604932294598783e-06,
|
||||
5.8586555655011475e-05
|
||||
],
|
||||
"motion_sms_154023_154359": [
|
||||
0.00011745666467620586,
|
||||
0.00021434926285955486,
|
||||
-4.5147354343760625e-05
|
||||
]
|
||||
},
|
||||
"time_offset": {
|
||||
"offset_s": 0.04000000000000031,
|
||||
"peak_correlation": 0.5788788671828708,
|
||||
"second_best_correlation": 0.5773967219219845,
|
||||
"evaluated_samples": 12394,
|
||||
"reliable": false
|
||||
},
|
||||
"convention": {
|
||||
"name": "north_cw__pitch_nose_up__roll_right_down",
|
||||
"heading_sign": -1.0,
|
||||
"pitch_sign": -1.0,
|
||||
"roll_sign": 1.0
|
||||
},
|
||||
"convention_scores_deg": {
|
||||
"north_cw__pitch_nose_up__roll_right_down": 1.631322241364994,
|
||||
"north_cw__pitch_opposite": 1.737582794831228,
|
||||
"heading_opposite__pitch_nose_up": 7.263259116648845,
|
||||
"heading_opposite__pitch_opposite": 17.716319437929055
|
||||
},
|
||||
"pair_count": 731,
|
||||
"residual_rms_deg": 1.6276839301413086,
|
||||
"residual_median_deg": 0.5695938849052221,
|
||||
"residual_p95_deg": 3.0903953005641345,
|
||||
"rotation_std_deg": [
|
||||
0.2340126126406699,
|
||||
0.20048574131667432,
|
||||
2.314692068045375
|
||||
],
|
||||
"information_singular_values": [
|
||||
82178.77940373585,
|
||||
60441.86300479194,
|
||||
612.6358640387364
|
||||
],
|
||||
"per_session_rms_deg": {
|
||||
"priority_174005_174515": 0.6134232617562615,
|
||||
"priority_174905_175450": 2.2485932625979106,
|
||||
"priority_175910_180530": 3.095861798757508,
|
||||
"slope_190548_190730": 2.388478834012087,
|
||||
"circle_193412_193642": 0.5629885032374518,
|
||||
"loop_194223_195003": 0.6250976606325254,
|
||||
"accel_195608_195958": 0.975856888271022,
|
||||
"motion_sms_154023_154359": 1.3698317592842066
|
||||
},
|
||||
"loo_delta_deg": {},
|
||||
"ok": false,
|
||||
"notes": [
|
||||
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||
"GNHPR convention score gap=0.1063 deg",
|
||||
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
|
||||
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||
"rotation failed one or more strict acceptance gates"
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
{
|
||||
"status": "diagnostic_not_accepted",
|
||||
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9996841792313951,
|
||||
0.02499811511405725,
|
||||
-0.002576050309343804
|
||||
],
|
||||
[
|
||||
-0.025002617750669198,
|
||||
0.9996858874629868,
|
||||
-0.0017307550243271697
|
||||
],
|
||||
[
|
||||
0.002531975526313299,
|
||||
0.0017946164171362589,
|
||||
0.9999951842143289
|
||||
]
|
||||
],
|
||||
"t_RTK_IMU_m": null,
|
||||
"T_RTK_IMU": null,
|
||||
"rotation_ok": false,
|
||||
"translation_ok": null,
|
||||
"rotation_result": "rotation_result.json",
|
||||
"translation_result": null,
|
||||
"dataset_audit": "dataset_audit.json"
|
||||
}
|
||||
@@ -0,0 +1,157 @@
|
||||
{
|
||||
"session_count": 8,
|
||||
"sessions": [
|
||||
{
|
||||
"session_id": "priority_174005_174515",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 31000,
|
||||
"rtk_samples": 4780,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.854602510460251,
|
||||
"common_time_span_s": [
|
||||
16265.2325992,
|
||||
16575.0298107
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.465514786,
|
||||
114.092169888,
|
||||
29.4695
|
||||
],
|
||||
"imu_source": "31000 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174005_174515\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "priority_174905_175450",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 34499,
|
||||
"rtk_samples": 5211,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8547303780464403,
|
||||
"common_time_span_s": [
|
||||
16805.1862481,
|
||||
17150.0907328
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4653424457,
|
||||
114.092237385,
|
||||
29.5366
|
||||
],
|
||||
"imu_source": "34499 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_174905_175450\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "priority_175910_180530",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 37998,
|
||||
"rtk_samples": 5786,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8567231247839613,
|
||||
"common_time_span_s": [
|
||||
17410.2121738,
|
||||
17790.0458228
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654151935,
|
||||
114.090796384,
|
||||
29.5317
|
||||
],
|
||||
"imu_source": "37998 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "slope_190548_190730",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 10199,
|
||||
"rtk_samples": 1578,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8757921419518377,
|
||||
"common_time_span_s": [
|
||||
36466.328875,
|
||||
36568.2301441
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4652183602,
|
||||
114.090839984,
|
||||
29.9891
|
||||
],
|
||||
"imu_source": "10199 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\slope_190548_190730\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "circle_193412_193642",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 14989,
|
||||
"rtk_samples": 2162,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8288621646623496,
|
||||
"common_time_span_s": [
|
||||
38170.3385255,
|
||||
38317.8571971
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654799242,
|
||||
114.092155912,
|
||||
29.4779
|
||||
],
|
||||
"imu_source": "14989 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\circle_193412_193642\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "loop_194223_195003",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 45801,
|
||||
"rtk_samples": 6741,
|
||||
"fixed_position_ratio": 0.9998516540572615,
|
||||
"fixed_attitude_ratio": 0.8377095386441181,
|
||||
"common_time_span_s": [
|
||||
38661.2831251,
|
||||
39121.1709553
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654856957,
|
||||
114.092151015,
|
||||
29.4651
|
||||
],
|
||||
"imu_source": "45801 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\loop_194223_195003\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "accel_195608_195958",
|
||||
"batch_id": "0815",
|
||||
"imu_samples": 23002,
|
||||
"rtk_samples": 3456,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8457754629629629,
|
||||
"common_time_span_s": [
|
||||
39486.3574509,
|
||||
39716.3199405
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.465443014,
|
||||
114.092179168,
|
||||
29.5309
|
||||
],
|
||||
"imu_source": "23002 normalized samples",
|
||||
"rtk_source": "D:\\data\\0815\\sessions_v2_device_affine\\accel_195608_195958\\rtk.csv"
|
||||
},
|
||||
{
|
||||
"session_id": "motion_sms_154023_154359",
|
||||
"batch_id": "0819",
|
||||
"imu_samples": 21556,
|
||||
"rtk_samples": 3294,
|
||||
"fixed_position_ratio": 0.49271402550091076,
|
||||
"fixed_attitude_ratio": 0.4344262295081967,
|
||||
"common_time_span_s": [
|
||||
25829.8725525,
|
||||
26027.4217529
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4651580863,
|
||||
114.090773052,
|
||||
36.5626
|
||||
],
|
||||
"imu_source": "21556 normalized samples",
|
||||
"rtk_source": "D:\\data\\0819\\dense5\\sessions_v2_device_affine\\motion_sms_154023_154359\\rtk.csv"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,119 @@
|
||||
{
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
-0.999754381938718,
|
||||
0.021803011844914608,
|
||||
0.003975483470215314
|
||||
],
|
||||
[
|
||||
0.021793866945340735,
|
||||
0.9997597719617833,
|
||||
-0.0023293197487744975
|
||||
],
|
||||
[
|
||||
-0.0040253146336934244,
|
||||
-0.002242106467980424,
|
||||
-0.9999893848440022
|
||||
]
|
||||
],
|
||||
"rpy_deg": [
|
||||
-179.8715356137635,
|
||||
0.23063416255936714,
|
||||
178.75119441523574
|
||||
],
|
||||
"gyro_bias_by_session_rad_s": {
|
||||
"priority_174005_174515": [
|
||||
-0.00018079953472427648,
|
||||
-9.3706764772758e-05,
|
||||
-1.6471539455612585e-05
|
||||
],
|
||||
"priority_174905_175450": [
|
||||
-0.0001527801061740737,
|
||||
-0.0001164065722445753,
|
||||
-5.5002757975692905e-05
|
||||
],
|
||||
"priority_175910_180530": [
|
||||
0.00010732176614734042,
|
||||
-9.217694744503191e-05,
|
||||
0.00037171055222583177
|
||||
],
|
||||
"slope_190548_190730": [
|
||||
-0.0001400513048024026,
|
||||
-9.490085614046164e-05,
|
||||
-7.313488292136488e-05
|
||||
],
|
||||
"circle_193412_193642": [
|
||||
-5.9488404294769446e-05,
|
||||
-0.00029434501957892675,
|
||||
0.000102661594942897
|
||||
],
|
||||
"loop_194223_195003": [
|
||||
-0.0002980828626939094,
|
||||
9.137138703447405e-05,
|
||||
4.430930647242609e-05
|
||||
],
|
||||
"accel_195608_195958": [
|
||||
5.9388956272647935e-05,
|
||||
9.900621296014341e-06,
|
||||
8.065768586636302e-05
|
||||
],
|
||||
"motion_sms_154023_154359": [
|
||||
0.0016442968972331072,
|
||||
0.00012387305081956675,
|
||||
8.435528364463815e-08
|
||||
]
|
||||
},
|
||||
"time_offset": {
|
||||
"offset_s": 0.08500000000000035,
|
||||
"peak_correlation": 0.46191352508231565,
|
||||
"second_best_correlation": 0.460889710037625,
|
||||
"evaluated_samples": 12384,
|
||||
"reliable": false
|
||||
},
|
||||
"convention": {
|
||||
"name": "heading_opposite__pitch_nose_up",
|
||||
"heading_sign": 1.0,
|
||||
"pitch_sign": -1.0,
|
||||
"roll_sign": 1.0
|
||||
},
|
||||
"convention_scores_deg": {
|
||||
"north_cw__pitch_nose_up__roll_right_down": 1.6671304616155285,
|
||||
"north_cw__pitch_opposite": 1.7955511909491206,
|
||||
"heading_opposite__pitch_nose_up": 1.6669796415371239,
|
||||
"heading_opposite__pitch_opposite": 19.709790964275243
|
||||
},
|
||||
"pair_count": 6164,
|
||||
"residual_rms_deg": 1.6669796415371239,
|
||||
"residual_median_deg": 0.6097138083592458,
|
||||
"residual_p95_deg": 2.9952512236242987,
|
||||
"rotation_std_deg": [
|
||||
1.0204560247927847,
|
||||
0.06460681216547948,
|
||||
0.11838973994567728
|
||||
],
|
||||
"information_singular_values": [
|
||||
818353.8002481281,
|
||||
235774.70975965378,
|
||||
3151.7390576048515
|
||||
],
|
||||
"per_session_rms_deg": {
|
||||
"priority_174005_174515": 0.7068100152136895,
|
||||
"priority_174905_175450": 1.7590065687347056,
|
||||
"priority_175910_180530": 3.0674316757114104,
|
||||
"slope_190548_190730": 2.07580304513421,
|
||||
"circle_193412_193642": 0.6574485751543728,
|
||||
"loop_194223_195003": 0.6594363752510715,
|
||||
"accel_195608_195958": 0.7631418065216562,
|
||||
"motion_sms_154023_154359": 3.709386878989403
|
||||
},
|
||||
"loo_delta_deg": {},
|
||||
"ok": false,
|
||||
"notes": [
|
||||
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||
"GNHPR convention score gap=0.0002 deg",
|
||||
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||
"empirical best GNHPR convention differs from protocol expectation; manual verification required",
|
||||
"rotation failed one or more strict acceptance gates"
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
{
|
||||
"status": "diagnostic_not_accepted",
|
||||
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
-0.999754381938718,
|
||||
0.021803011844914608,
|
||||
0.003975483470215314
|
||||
],
|
||||
[
|
||||
0.021793866945340735,
|
||||
0.9997597719617833,
|
||||
-0.0023293197487744975
|
||||
],
|
||||
[
|
||||
-0.0040253146336934244,
|
||||
-0.002242106467980424,
|
||||
-0.9999893848440022
|
||||
]
|
||||
],
|
||||
"t_RTK_IMU_m": null,
|
||||
"T_RTK_IMU": null,
|
||||
"rotation_ok": false,
|
||||
"translation_ok": null,
|
||||
"rotation_result": "rotation_result.json",
|
||||
"translation_result": null,
|
||||
"dataset_audit": "dataset_audit.json"
|
||||
}
|
||||
@@ -0,0 +1,24 @@
|
||||
{
|
||||
"session_count": 1,
|
||||
"sessions": [
|
||||
{
|
||||
"session_id": "priority_175910_180530",
|
||||
"batch_id": "0808",
|
||||
"imu_samples": 37998,
|
||||
"rtk_samples": 5786,
|
||||
"fixed_position_ratio": 1.0,
|
||||
"fixed_attitude_ratio": 0.8567231247839613,
|
||||
"common_time_span_s": [
|
||||
17410.2121738,
|
||||
17790.0458228
|
||||
],
|
||||
"origin_geodetic": [
|
||||
30.4654151935,
|
||||
114.090796384,
|
||||
29.5317
|
||||
],
|
||||
"imu_source": "37998 normalized samples",
|
||||
"rtk_source": "D:\\data\\calibration_usable_20260808\\sessions_v2_device_affine\\priority_175910_180530\\rtk.csv"
|
||||
}
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,76 @@
|
||||
{
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9974493913373024,
|
||||
-0.07135460605626225,
|
||||
0.0017977528752911507
|
||||
],
|
||||
[
|
||||
0.0713431523484532,
|
||||
0.997435050761847,
|
||||
0.005785682733905998
|
||||
],
|
||||
[
|
||||
-0.0022059768426676615,
|
||||
-0.00564266836413856,
|
||||
0.9999816468115312
|
||||
]
|
||||
],
|
||||
"rpy_deg": [
|
||||
-0.32330358477785043,
|
||||
0.1263932653005637,
|
||||
4.0911470587568495
|
||||
],
|
||||
"gyro_bias_by_session_rad_s": {
|
||||
"priority_175910_180530": [
|
||||
-0.00010509442179307857,
|
||||
-0.0001426783269500731,
|
||||
3.086292346981605e-05
|
||||
]
|
||||
},
|
||||
"time_offset": {
|
||||
"offset_s": -0.0949999999999998,
|
||||
"peak_correlation": 0.1329249352233157,
|
||||
"second_best_correlation": 0.13247995527618098,
|
||||
"evaluated_samples": 2316,
|
||||
"reliable": false
|
||||
},
|
||||
"convention": {
|
||||
"name": "north_cw__pitch_nose_up__roll_right_down",
|
||||
"heading_sign": -1.0,
|
||||
"pitch_sign": -1.0,
|
||||
"roll_sign": 1.0
|
||||
},
|
||||
"convention_scores_deg": {
|
||||
"north_cw__pitch_nose_up__roll_right_down": 2.8406184024910064,
|
||||
"north_cw__pitch_opposite": 2.8942770966870084,
|
||||
"heading_opposite__pitch_nose_up": 3.8836333647028383,
|
||||
"heading_opposite__pitch_opposite": 8.244842702849073
|
||||
},
|
||||
"pair_count": 91,
|
||||
"residual_rms_deg": 2.8406184024910064,
|
||||
"residual_median_deg": 1.624215282655001,
|
||||
"residual_p95_deg": 6.087913500787023,
|
||||
"rotation_std_deg": [
|
||||
1.6029337786454214,
|
||||
1.0761604678920118,
|
||||
6.305203755276785
|
||||
],
|
||||
"information_singular_values": [
|
||||
2941.184567996004,
|
||||
1268.1548949190246,
|
||||
82.52753961323997
|
||||
],
|
||||
"per_session_rms_deg": {
|
||||
"priority_175910_180530": 2.8406184024910064
|
||||
},
|
||||
"loo_delta_deg": {},
|
||||
"ok": false,
|
||||
"notes": [
|
||||
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||
"residual time convention: t_IMU = t_RTK + +0.000000 s",
|
||||
"GNHPR convention score gap=0.0537 deg",
|
||||
"time-offset correlation was ambiguous; held residual offset at zero",
|
||||
"rotation failed one or more strict acceptance gates"
|
||||
]
|
||||
}
|
||||
@@ -0,0 +1,28 @@
|
||||
{
|
||||
"status": "diagnostic_not_accepted",
|
||||
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||
"R_RTK_IMU": [
|
||||
[
|
||||
0.9974493913373024,
|
||||
-0.07135460605626225,
|
||||
0.0017977528752911507
|
||||
],
|
||||
[
|
||||
0.0713431523484532,
|
||||
0.997435050761847,
|
||||
0.005785682733905998
|
||||
],
|
||||
[
|
||||
-0.0022059768426676615,
|
||||
-0.00564266836413856,
|
||||
0.9999816468115312
|
||||
]
|
||||
],
|
||||
"t_RTK_IMU_m": null,
|
||||
"T_RTK_IMU": null,
|
||||
"rotation_ok": false,
|
||||
"translation_ok": null,
|
||||
"rotation_result": "rotation_result.json",
|
||||
"translation_result": null,
|
||||
"dataset_audit": "dataset_audit.json"
|
||||
}
|
||||
@@ -0,0 +1,52 @@
|
||||
# RTK–IMU 候选数据清点(2026-08-20)
|
||||
|
||||
本目录记录 0808、0815、0819 三批 LiDAR/IMU 会话所对应的 G90 RTK 原始记录与当前导出状态。此处只做数据血缘和可用性评估,尚未求解 `T_RTK_IMU`。
|
||||
|
||||
## 时间与质量约定
|
||||
|
||||
- 原始 RTK 为 Wheeltec G90 V2 `.rscap`,包含 `$GNGGA/$GPGGA` 位置和 `$GNHPR` heading/pitch/roll。
|
||||
- 切窗使用 NMEA 报文自带的测量 UTC,再通过每个会话的 IMU device→host affine clock 映射到 IMU 设备时间。
|
||||
- 主机接收时间比 NMEA 测量时间晚约 3–4 s,且有波动;不能按接收时间直接切窗。
|
||||
- 当前项目门控按 GGA `fix_quality=4` 和 HPR `heading_quality∈{4,5}` 判断固定位置/有效航向。
|
||||
- `heading_valid` 未覆盖全部 GGA 行主要因为 GGA 与 HPR 频率不同、最近邻匹配阈值为 80 ms;不代表该会话航向整体失效。
|
||||
|
||||
## 数据映射与质量
|
||||
|
||||
详细机器可读清单见 `rtk_session_inventory.csv`。
|
||||
|
||||
| 会话 | RTK结论 | 适合的标定作用 |
|
||||
| --- | --- | --- |
|
||||
| `priority_174005_174515` | 100% GGA质量4;有效航向约309 s;yaw变化约485° | 多圈 yaw 与 XY 杠杆臂 |
|
||||
| `priority_174905_175450` | 100% GGA质量4;Pitch跨度约11.4°;XY约58×22 m | 强候选:yaw、pitch、XY/Z耦合解除 |
|
||||
| `priority_175910_180530` | 100% GGA质量4;Pitch跨度约13.6°;XY约144×59 m | 最强候选:长基线、pitch、平移 |
|
||||
| `slope_190548_190730` | 100% GGA质量4;Pitch跨度约7.4°;约102 s | 坡度/Pitch补充 |
|
||||
| `circle_193412_193642` | 100% GGA质量4;yaw变化约445° | 平面旋转与XY杠杆臂 |
|
||||
| `loop_194223_195003` | 仅1个异常GGA;yaw累计变化约1203°;约460 s | 最强 yaw/多圈转弯候选 |
|
||||
| `accel_195608_195958` | 100% GGA质量4;XY约46×23 m;约230 s | 加减速、速度和水平杠杆臂 |
|
||||
| `motion_sms_154023_154359` | 全窗仅约49%为质量4;可用连续子段约100 s | 仅用15:41:28.2–15:43:08.15固定解/有效航向段 |
|
||||
|
||||
## 导出状态
|
||||
|
||||
- 0808 当前 `sessions_v2_device_affine` 原先没有 RTK CSV,本次已从原始 G90 `.rscap` 按 NMEA 测量 UTC补导三个会话,未覆盖旧文件。
|
||||
- 0815 `sessions_v2_device_affine` 与 dense5 slope 已经采用同一测量时间导出规则,无需重导。
|
||||
- 0819 `sessions_v2_device_affine` 与 dense5 motion 已经采用同一规则;需要在求解器中按质量与时间连续段过滤,而不是重新解释为全窗固定解。
|
||||
- 0808 旧 `sessions_v1_host_aligned_00` RTK CSV 不含 `t_measurement_utc_s` 字段,只保留作历史对照;新求解应使用 `sessions_v2_device_affine`。
|
||||
|
||||
## 已发现的 LiDAR–RTK 资料边界
|
||||
|
||||
`D:\data\calibration_usable_20260808\rtk_lidar_station_report*` 保存的是静止站点候选:27个站点、29个候选段,并非包含 `T_RTK_lidar`、协方差和留一验证的正式手眼结果。本轮在 0808 数据目录的 JSON/YAML/Markdown/CSV/日志中没有找到 `T_RTK_lidar` 矩阵。若要通过链式关系得到 LiDAR–IMU,需要继续定位原手眼结果及其坐标约定:
|
||||
|
||||
```text
|
||||
T_IMU_lidar = inverse(T_RTK_IMU) @ T_RTK_lidar
|
||||
```
|
||||
|
||||
## 初步可行性判断
|
||||
|
||||
这些数据足以启动直接 RTK–IMU 标定,且比当前纯 LiDAR–IMU Phase-B 更有希望约束 XY:RTK 提供绝对位置,GNHPR 提供航向和 Pitch,多会话包含长基线、转弯、加减速与坡度。仍需注意:
|
||||
|
||||
1. GNHPR roll 的变化仅约0.006°–0.065°,不能指望它提供有效 roll 激励。
|
||||
2. 应先用 RTK heading/pitch 角速度与 IMU gyro 做残余时间偏置和坐标轴验证,再求旋转。
|
||||
3. 平移应使用 RTK绝对位置 + IMU预积分的联合状态模型,估计共享 `T_RTK_IMU`、每会话速度/bias;不应把RTK轨迹简单二次差分后直接最小二乘。
|
||||
4. `motion_sms` 必须仅使用其连续固定解子段。
|
||||
5. 跨0808/0815/0819时应使用每会话IMU bias,外参共享,并检查安装期间是否发生机械变动。
|
||||
|
||||
@@ -0,0 +1,9 @@
|
||||
session,batch,raw_rtk_rscap,current_rtk_csv,rows,fixed_gga_ratio,fixed_heading_valid_rows,valid_duration_s,xy_robust_span_x_m,xy_robust_span_y_m,altitude_robust_span_m,heading_unwrapped_span_deg,pitch_robust_span_deg,roll_robust_span_deg,notes
|
||||
priority_174005_174515,0808,D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap,D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_174005_174515\rtk.csv,4780,1.0000,4085,309.45,12.045,17.380,0.112,485.411,3.462,0.012,newly exported from NMEA measurement UTC
|
||||
priority_174905_175450,0808,D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap,D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_174905_175450\rtk.csv,5211,1.0000,4454,344.90,58.436,21.923,0.277,350.616,11.379,0.008,newly exported from NMEA measurement UTC
|
||||
priority_175910_180530,0808,D:\data\calibration_usable_20260808\rtk_rscap\wheeltec-g90_20260808-092827.574_e361e39d-c918-4673-be70-b699ca4394f7.rscap,D:\data\calibration_usable_20260808\sessions_v2_device_affine\priority_175910_180530\rtk.csv,5786,1.0000,4957,379.85,143.775,58.657,0.971,260.213,13.574,0.065,newly exported from NMEA measurement UTC
|
||||
slope_190548_190730,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-110519.251_e0edf32b-c82b-4a38-b5df-2c0e4cac364f.rscap,D:\data\0815\sessions_v2_device_affine\slope_190548_190730\rtk.csv,1578,1.0000,1382,101.90,11.354,18.567,0.714,155.356,7.399,0.026,root is named 0815 but raw measurement date is 2026-08-14 local
|
||||
circle_193412_193642,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-113355.356_b4b37794-d4d6-433e-87e9-037cad5517d1.rscap,D:\data\0815\sessions_v2_device_affine\circle_193412_193642\rtk.csv,2162,1.0000,1792,147.25,10.105,10.277,0.097,444.994,2.834,0.006,raw capture has truncated tail but target messages are checksum-valid
|
||||
loop_194223_195003,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-114200.914_36911c82-f8c0-451b-99e3-f5b663ec6115.rscap,D:\data\0815\sessions_v2_device_affine\loop_194223_195003\rtk.csv,6741,0.9999,5647,459.90,11.555,18.800,0.104,1202.601,2.727,0.009,one malformed/non-fixed GGA excluded
|
||||
accel_195608_195958,0815,D:\data\0815\raw_serial_capture_v2\wheeltec-g90_20260814-115547.472_8722f326-8314-4db1-9ec4-bce185c54a78.rscap,D:\data\0815\sessions_v2_device_affine\accel_195608_195958\rtk.csv,3456,1.0000,2923,229.90,45.542,22.610,0.130,188.236,3.248,0.010,measurement-time export already present
|
||||
motion_sms_154023_154359,0819,D:\data\0819\raw_serial_capture_v2\wheeltec-g90_20260819-074023.329_3d9da6eb-7ef8-4f7b-9262-9193328238b0.rscap,D:\data\0819\dense5\sessions_v2_device_affine\motion_sms_154023_154359\rtk.csv,3294,0.4924,1431,99.95,12.679,17.356,0.839,308.864,8.985,0.020,use only 15:41:28.200-15:43:08.150 fixed+valid sub-window
|
||||
|
@@ -0,0 +1,72 @@
|
||||
# RTK–IMU 独立标定链
|
||||
|
||||
本分支将 RTK–IMU 标定与原 LiDAR–IMU Phase-A/B/C 隔离。只复用 SO(3)/SE(3)、IMU 数据契约和预积分数学,不复用 LiDAR 命名的结果对象或门限。
|
||||
|
||||
## 坐标与外参约定
|
||||
|
||||
唯一输出约定为 `p_RTK = R_RTK_IMU p_IMU + t_RTK_IMU`,即 `T_RTK_IMU` 把 IMU 坐标变换到 RTK 传感器坐标。杆臂 `l_IMU_to_RTK_in_IMU` 是“IMU 原点指向 RTK 定位参考点”的向量,表达在 IMU 坐标中,因此 `t_RTK_IMU = -R_RTK_IMU @ l_IMU_to_RTK_in_IMU`。
|
||||
|
||||
世界坐标采用局部 ENU。GGA 的第一个有效固定解作为每会话 ENU 原点。GGA 高程在局部差分中使用;因为设备输出的高程基准尚未单独核验,Z 方向使用更大的噪声并执行独立门控。
|
||||
|
||||
当前 GNHPR 解释候选把 heading 看作从真北顺时针,pitch 看作机头上仰为正,并按 Z-Y-X 生成 `R_ENU_RTK`。代码同时检查 heading/pitch 的反号候选。固定的 RTK 轴置换不能由手眼残差自行识别,因此 `frame_definition` 和 `reference_point` 为空时,最终状态必须是 diagnostic。
|
||||
|
||||
## 数据与时间规则
|
||||
|
||||
- IMU CSV:`t,gx,gy,gz,ax,ay,az`,陀螺为 rad/s,加速度为 m/s²。
|
||||
- RTK CSV:位置使用 GGA,姿态使用 GNHPR;仅接受位置质量 4/5、姿态质量 4/5。
|
||||
- RTK 的 `t` 必须是 NMEA 测量 UTC 经 IMU device→host affine clock 反变换得到的 IMU 设备时间。禁止回退到延迟约 3–4 s 的主机接收时间。
|
||||
- 姿态使用 `hpr_measurement_utc_s` 恢复的 HPR 测量时间,不挂在最近邻 GGA 时刻。这一修正把 `motion_sms` 的旋转 RMS 从约 4.60° 降到约 1.37°。
|
||||
- 残余时间偏移通过 RTK/IMU 角速度模长相关扫描。相关峰不唯一时保持 0 s,并把候选峰只作为诊断值。
|
||||
|
||||
## 求解方法
|
||||
|
||||
R0 数据和时间审计:检查固定解比例、共同时间范围、HPR 有效性、ENU 原点和残余时间偏移。`motion_sms` 自动只使用固定位置且有效姿态的连续段。
|
||||
|
||||
R1 旋转:对 0.75 s、1.5 s、3.0 s 相对运动构造 `Delta_R_RTK R_RTK_IMU = R_RTK_IMU Delta_R_IMU`。IMU 侧采用陀螺预积分。先用无偏置闭式手眼解筛选四种 GNHPR 符号候选,只对最优候选联合优化共享旋转和每会话常值陀螺偏置。输出 RMS/median/P95、边缘协方差、信息奇异值、逐会话 RMS 和固定偏置 conditional LOO。
|
||||
|
||||
T1 杆臂和平移:不对 RTK 位置做二次差分。固定 R1 的旋转和陀螺偏置,以可配置节点构造 IMU 位置/速度预积分因子,联合求解跨会话共享杆臂、每节点速度和每会话常值加速度偏置。输出边缘协方差、信息奇异值/精度秩、位置/速度残差和 LOO。上游旋转未通过时,平移无条件标为 diagnostic。
|
||||
|
||||
## 代码结构
|
||||
|
||||
- `imu_lidar/geodesy.py`:WGS84 → ECEF → 局部 ENU。
|
||||
- `imu_lidar/rtk_io.py`:RTK CSV、质量掩码、GGA/HPR 独立测量时刻。
|
||||
- `imu_lidar/rtk_attitude.py`:GNHPR 候选约定与 SO(3) 插值。
|
||||
- `imu_lidar/rtk_imu_rotation.py`:时间审计、旋转手眼、偏置、协方差、LOO。
|
||||
- `imu_lidar/rtk_imu_translation.py`:轻量全 IMU 预积分、杆臂线性因子系统、协方差、LOO。
|
||||
- `imu_lidar/rtk_imu_replay.py`:清单加载、端到端运行、结构化结果发布。
|
||||
- `tools/run_rtk_imu_calibration.py`:命令行入口,支持会话/批次筛选和 rotation-only。
|
||||
- `tests/test_rtk_imu_calibration.py`:ENU、heading 方向、HPR 时刻、预积分等价性测试。
|
||||
|
||||
公共 IMU 预积分的端点插值已从每小段扫描完整数组,改为当前相邻样本内的等价局部插值;公共 IMU CSV 加载器也改为先校验表头、再按列加载。完整项目测试通过。
|
||||
|
||||
## 当前结果(8 会话,2026-08-21)
|
||||
|
||||
结果目录:`artifacts/rtk_imu_calibration_v1/all_sessions`。
|
||||
|
||||
```text
|
||||
RPY(R_RTK_IMU) = [0.102824, -0.145072, -1.432698] deg
|
||||
rotation RMS / median / P95 = 1.627683 / 0.569594 / 3.090395 deg
|
||||
rotation std = [0.234013, 0.200486, 2.314692] deg
|
||||
max conditional rotation LOO = 5.144191 deg(去掉 priority_175910_180530)
|
||||
|
||||
t_RTK_IMU = [0.771094, 0.569302, -21.073037] m
|
||||
translation std = [0.367658, 0.367640, 2.960859] m
|
||||
position RMS XYZ = [0.002606, 0.002617, 0.000871] m
|
||||
velocity RMS XYZ = [1.034408, 1.001846, 0.081893] m/s
|
||||
max translation LOO Z = 1.774476 m(去掉 loop_194223_195003)
|
||||
```
|
||||
|
||||
预期 GNHPR 符号候选胜出,预筛 RMS 1.6313°;pitch 反号为 1.7376°,heading 反号候选为 7.2633°/17.7163°。heading 方向基本可判定,但固定 RTK 轴定义仍未判定。
|
||||
|
||||
平移明显不具机械真实性。很小的位置残差来自节点速度自由度和鲁棒权重吸收,不能抵消约 1 m/s 的水平速度残差、米级标准差与巨大 LOO 变化。该候选禁止写入配置。
|
||||
|
||||
逐日期旋转:0808 为 `[-0.232660, -0.039716, -1.563562]°`、RMS 2.121957°;0815 为 `[0.188773, -0.139741, -0.582331]°`、RMS 0.942835°。两批 yaw 相差约 0.98°,但 yaw 自身标准差约 3.60°/2.57°,不能据此声称安装发生变化。
|
||||
|
||||
## 当前阻断项与下一步
|
||||
|
||||
1. RTK `frame_definition` 和 `reference_point` 为空。必须从协议、天线安装方向和原 LiDAR–RTK 手眼结果确认 heading 物理轴、pitch/roll 旋转轴及 GGA 相位中心。
|
||||
2. 残余时间候选约 +0.04 s,但峰宽且次峰接近,当前按门限固定为 0 s。需要独立高动态数据或同步边沿确认。
|
||||
3. `priority_175910_180530` 的 conditional LOO 牵引达到 5.14°、自身 RMS 3.10°,应检查其 HPR 突跳、IMU 丢帧、动态挠曲和安装状态。
|
||||
4. 坡道会话 RMS 2.39°,说明 pitch 数据或姿态轴/符号仍可能不完整。
|
||||
5. 平移水平速度因子 RMS 约 1 m/s、Z 杆臂漂到约 21 m。应先冻结平移,完成 RTK 帧和旋转验证,再检查加速度轴、重力符号、高程基准与噪声模型。
|
||||
6. 只有旋转、时间和 RTK 帧定义全部通过后,才重新运行平移并做严格的全 nuisance LOO/留出验证。
|
||||
@@ -0,0 +1,74 @@
|
||||
"""Small WGS84 geodesy helpers used by the RTK--IMU calibration path."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
WGS84_A_M = 6378137.0
|
||||
WGS84_F = 1.0 / 298.257223563
|
||||
WGS84_E2 = WGS84_F * (2.0 - WGS84_F)
|
||||
|
||||
|
||||
def geodetic_to_ecef(
|
||||
latitude_deg: np.ndarray,
|
||||
longitude_deg: np.ndarray,
|
||||
altitude_m: np.ndarray,
|
||||
) -> np.ndarray:
|
||||
"""Convert WGS84 latitude/longitude/ellipsoidal height to ECEF metres."""
|
||||
|
||||
latitude = np.deg2rad(np.asarray(latitude_deg, dtype=float))
|
||||
longitude = np.deg2rad(np.asarray(longitude_deg, dtype=float))
|
||||
altitude = np.asarray(altitude_m, dtype=float)
|
||||
latitude, longitude, altitude = np.broadcast_arrays(latitude, longitude, altitude)
|
||||
sin_lat = np.sin(latitude)
|
||||
cos_lat = np.cos(latitude)
|
||||
radius = WGS84_A_M / np.sqrt(1.0 - WGS84_E2 * sin_lat**2)
|
||||
x = (radius + altitude) * cos_lat * np.cos(longitude)
|
||||
y = (radius + altitude) * cos_lat * np.sin(longitude)
|
||||
z = (radius * (1.0 - WGS84_E2) + altitude) * sin_lat
|
||||
return np.stack([x, y, z], axis=-1)
|
||||
|
||||
|
||||
def geodetic_to_enu(
|
||||
latitude_deg: np.ndarray,
|
||||
longitude_deg: np.ndarray,
|
||||
altitude_m: np.ndarray,
|
||||
*,
|
||||
origin_latitude_deg: float | None = None,
|
||||
origin_longitude_deg: float | None = None,
|
||||
origin_altitude_m: float | None = None,
|
||||
) -> tuple[np.ndarray, tuple[float, float, float]]:
|
||||
"""Convert WGS84 samples to a local east/north/up frame.
|
||||
|
||||
When no origin is supplied, the first finite sample is used. The returned
|
||||
origin tuple is ``(latitude_deg, longitude_deg, altitude_m)``.
|
||||
"""
|
||||
|
||||
lat = np.asarray(latitude_deg, dtype=float).reshape(-1)
|
||||
lon = np.asarray(longitude_deg, dtype=float).reshape(-1)
|
||||
alt = np.asarray(altitude_m, dtype=float).reshape(-1)
|
||||
if not (lat.size == lon.size == alt.size):
|
||||
raise ValueError("latitude, longitude and altitude must have equal length")
|
||||
finite = np.isfinite(lat) & np.isfinite(lon) & np.isfinite(alt)
|
||||
if not np.any(finite):
|
||||
raise ValueError("no finite geodetic sample")
|
||||
first = int(np.flatnonzero(finite)[0])
|
||||
lat0 = float(lat[first] if origin_latitude_deg is None else origin_latitude_deg)
|
||||
lon0 = float(lon[first] if origin_longitude_deg is None else origin_longitude_deg)
|
||||
alt0 = float(alt[first] if origin_altitude_m is None else origin_altitude_m)
|
||||
|
||||
ecef = geodetic_to_ecef(lat, lon, alt)
|
||||
ecef0 = geodetic_to_ecef(np.array(lat0), np.array(lon0), np.array(alt0)).reshape(3)
|
||||
delta = ecef - ecef0
|
||||
phi = np.deg2rad(lat0)
|
||||
lam = np.deg2rad(lon0)
|
||||
rotation = np.array(
|
||||
[
|
||||
[-np.sin(lam), np.cos(lam), 0.0],
|
||||
[-np.sin(phi) * np.cos(lam), -np.sin(phi) * np.sin(lam), np.cos(phi)],
|
||||
[np.cos(phi) * np.cos(lam), np.cos(phi) * np.sin(lam), np.sin(phi)],
|
||||
],
|
||||
dtype=float,
|
||||
)
|
||||
return delta @ rotation.T, (lat0, lon0, alt0)
|
||||
+12
-10
@@ -15,6 +15,7 @@ Accepted inputs
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
@@ -36,16 +37,17 @@ def load_imu_samples(path: Path | str) -> ImuSeries:
|
||||
|
||||
|
||||
def _load_imu_csv(path: Path) -> ImuSeries:
|
||||
data = np.genfromtxt(path, delimiter=",", names=True, dtype=float)
|
||||
if data.ndim == 0:
|
||||
data = np.array([data])
|
||||
names = set(data.dtype.names or ())
|
||||
required = {"t", "gx", "gy", "gz", "ax", "ay", "az"}
|
||||
if not required.issubset(names):
|
||||
raise ValueError(f"IMU CSV must contain columns {sorted(required)}, got {sorted(names)}")
|
||||
t = np.asarray(data["t"], dtype=float).reshape(-1)
|
||||
gyro = np.column_stack([data["gx"], data["gy"], data["gz"]]).astype(float)
|
||||
acc = np.column_stack([data["ax"], data["ay"], data["az"]]).astype(float)
|
||||
required_order = ["t", "gx", "gy", "gz", "ax", "ay", "az"]
|
||||
with path.open("r", encoding="utf-8-sig", newline="") as handle:
|
||||
header = next(csv.reader(handle), [])
|
||||
names = set(header)
|
||||
if not set(required_order).issubset(names):
|
||||
raise ValueError(f"IMU CSV must contain columns {sorted(required_order)}, got {sorted(names)}")
|
||||
usecols = [header.index(name) for name in required_order]
|
||||
data = np.loadtxt(path, delimiter=",", skiprows=1, usecols=usecols, ndmin=2)
|
||||
t = np.asarray(data[:, 0], dtype=float).reshape(-1)
|
||||
gyro = np.asarray(data[:, 1:4], dtype=float)
|
||||
acc = np.asarray(data[:, 4:7], dtype=float)
|
||||
order = np.argsort(t)
|
||||
return ImuSeries(t_s=t[order], gyro_rad_s=gyro[order], acc_m_s2=acc[order])
|
||||
|
||||
|
||||
@@ -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)))
|
||||
|
||||
@@ -0,0 +1,73 @@
|
||||
"""GNHPR attitude conventions and SO(3) interpolation."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial.transform import Rotation, Slerp
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class GnhprConvention:
|
||||
"""Interpretation of GNHPR angles as ``R_ENU_RTK``.
|
||||
|
||||
Heading is normally clockwise from north. With ENU and an x-forward RTK
|
||||
frame this becomes yaw ``90 deg - heading``. Aircraft-positive pitch is
|
||||
nose-up, which is the negative mathematical Y rotation in an FLU frame.
|
||||
Alternative signs are retained for empirical protocol validation.
|
||||
"""
|
||||
|
||||
name: str
|
||||
heading_sign: float = -1.0
|
||||
pitch_sign: float = -1.0
|
||||
roll_sign: float = 1.0
|
||||
|
||||
|
||||
EXPECTED_GNHPR = GnhprConvention("north_cw__pitch_nose_up__roll_right_down")
|
||||
GNHPR_CANDIDATES = (
|
||||
EXPECTED_GNHPR,
|
||||
GnhprConvention("north_cw__pitch_opposite", -1.0, 1.0, 1.0),
|
||||
GnhprConvention("heading_opposite__pitch_nose_up", 1.0, -1.0, 1.0),
|
||||
GnhprConvention("heading_opposite__pitch_opposite", 1.0, 1.0, 1.0),
|
||||
)
|
||||
|
||||
|
||||
def gnhpr_to_rotation_enu_rtk(
|
||||
heading_deg: np.ndarray,
|
||||
pitch_deg: np.ndarray,
|
||||
roll_deg: np.ndarray,
|
||||
convention: GnhprConvention = EXPECTED_GNHPR,
|
||||
) -> np.ndarray:
|
||||
"""Build body-to-ENU matrices with an extrinsic Z-Y-X Euler sequence."""
|
||||
|
||||
heading = np.asarray(heading_deg, dtype=float).reshape(-1)
|
||||
pitch = np.asarray(pitch_deg, dtype=float).reshape(-1)
|
||||
roll = np.asarray(roll_deg, dtype=float).reshape(-1)
|
||||
if not (heading.size == pitch.size == roll.size):
|
||||
raise ValueError("heading, pitch and roll must have equal length")
|
||||
yaw_rad = np.deg2rad(90.0 + convention.heading_sign * heading)
|
||||
pitch_rad = np.deg2rad(convention.pitch_sign * pitch)
|
||||
roll_rad = np.deg2rad(convention.roll_sign * roll)
|
||||
angles = np.column_stack([yaw_rad, pitch_rad, roll_rad])
|
||||
return Rotation.from_euler('ZYX', angles).as_matrix()
|
||||
|
||||
|
||||
def interpolate_rotations(
|
||||
source_t_s: np.ndarray,
|
||||
rotations: np.ndarray,
|
||||
query_t_s: np.ndarray,
|
||||
) -> np.ndarray:
|
||||
"""Slerp a monotonic SO(3) series without extrapolation."""
|
||||
|
||||
source_t = np.asarray(source_t_s, dtype=float).reshape(-1)
|
||||
query_t = np.asarray(query_t_s, dtype=float).reshape(-1)
|
||||
matrices = np.asarray(rotations, dtype=float).reshape(-1, 3, 3)
|
||||
if source_t.size < 2 or matrices.shape[0] != source_t.size:
|
||||
raise ValueError("need at least two timestamped rotations")
|
||||
if np.any(np.diff(source_t) <= 0):
|
||||
unique_t, unique_indices = np.unique(source_t, return_index=True)
|
||||
source_t = unique_t
|
||||
matrices = matrices[unique_indices]
|
||||
if np.any(query_t < source_t[0]) or np.any(query_t > source_t[-1]):
|
||||
raise ValueError("rotation interpolation does not extrapolate")
|
||||
return Slerp(source_t, Rotation.from_matrix(matrices))(query_t).as_matrix()
|
||||
@@ -0,0 +1,172 @@
|
||||
"""End-to-end orchestration and JSON reporting for RTK--IMU calibration."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import csv
|
||||
import json
|
||||
from dataclasses import asdict, dataclass
|
||||
from pathlib import Path
|
||||
from typing import Any
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .imu_io import load_imu_samples
|
||||
from .rtk_imu_rotation import RotationCalibrationResult, RotationSession, solve_rtk_imu_rotation
|
||||
from .rtk_imu_translation import TranslationCalibrationResult, solve_rtk_imu_translation
|
||||
from .rtk_io import load_rtk_csv
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class InventoryEntry:
|
||||
session_id: str
|
||||
batch_id: str
|
||||
imu_csv: Path
|
||||
rtk_csv: Path
|
||||
|
||||
|
||||
def load_inventory(path: Path | str) -> list[InventoryEntry]:
|
||||
"""Load the project RTK inventory and derive each paired IMU path."""
|
||||
|
||||
source = Path(path)
|
||||
entries: list[InventoryEntry] = []
|
||||
with source.open("r", encoding="utf-8-sig", newline="") as handle:
|
||||
for row in csv.DictReader(handle):
|
||||
rtk_csv = Path(row["current_rtk_csv"])
|
||||
imu_csv = rtk_csv.with_name("imu.csv")
|
||||
entries.append(
|
||||
InventoryEntry(
|
||||
session_id=row["session"],
|
||||
batch_id=row["batch"],
|
||||
imu_csv=imu_csv,
|
||||
rtk_csv=rtk_csv,
|
||||
)
|
||||
)
|
||||
if not entries:
|
||||
raise ValueError(f"empty RTK inventory: {source}")
|
||||
return entries
|
||||
|
||||
|
||||
def load_sessions(entries: list[InventoryEntry] | tuple[InventoryEntry, ...]) -> list[RotationSession]:
|
||||
sessions = []
|
||||
for entry in entries:
|
||||
sessions.append(
|
||||
RotationSession(
|
||||
session_id=entry.session_id,
|
||||
batch_id=entry.batch_id,
|
||||
imu=load_imu_samples(entry.imu_csv),
|
||||
rtk=load_rtk_csv(entry.rtk_csv),
|
||||
)
|
||||
)
|
||||
return sessions
|
||||
|
||||
|
||||
def _jsonable(value: Any) -> Any:
|
||||
if isinstance(value, np.ndarray):
|
||||
return value.tolist()
|
||||
if isinstance(value, np.generic):
|
||||
return value.item()
|
||||
if isinstance(value, Path):
|
||||
return str(value)
|
||||
if hasattr(value, "__dataclass_fields__"):
|
||||
return {key: _jsonable(item) for key, item in asdict(value).items()}
|
||||
if isinstance(value, dict):
|
||||
return {str(key): _jsonable(item) for key, item in value.items()}
|
||||
if isinstance(value, (list, tuple)):
|
||||
return [_jsonable(item) for item in value]
|
||||
return value
|
||||
|
||||
|
||||
def dataset_audit(sessions: list[RotationSession]) -> dict[str, Any]:
|
||||
rows = []
|
||||
for session in sessions:
|
||||
rtk = session.rtk
|
||||
valid_position = rtk.position_valid
|
||||
valid_attitude = rtk.attitude_valid & valid_position
|
||||
rows.append(
|
||||
{
|
||||
"session_id": session.session_id,
|
||||
"batch_id": session.batch_id,
|
||||
"imu_samples": int(session.imu.t_s.size),
|
||||
"rtk_samples": int(rtk.t_s.size),
|
||||
"fixed_position_ratio": float(np.mean(valid_position)),
|
||||
"fixed_attitude_ratio": float(np.mean(valid_attitude)),
|
||||
"common_time_span_s": [
|
||||
float(max(session.imu.t_s[0], rtk.t_s[0])),
|
||||
float(min(session.imu.t_s[-1], rtk.t_s[-1])),
|
||||
],
|
||||
"origin_geodetic": list(rtk.origin_geodetic),
|
||||
"imu_source": str(session.imu.t_s.size) + " normalized samples",
|
||||
"rtk_source": str(rtk.source),
|
||||
}
|
||||
)
|
||||
return {"session_count": len(sessions), "sessions": rows}
|
||||
|
||||
|
||||
def run_calibration(
|
||||
sessions: list[RotationSession],
|
||||
output_directory: Path | str,
|
||||
*,
|
||||
rotation_only: bool = False,
|
||||
compute_loo: bool = True,
|
||||
knot_step_s: float = 2.0,
|
||||
rtk_frame_definition: str = '',
|
||||
rtk_reference_point: str = '',
|
||||
) -> tuple[RotationCalibrationResult, TranslationCalibrationResult | None]:
|
||||
"""Run calibration and publish human-readable JSON artifacts."""
|
||||
|
||||
output = Path(output_directory)
|
||||
output.mkdir(parents=True, exist_ok=True)
|
||||
rotation = solve_rtk_imu_rotation(sessions, compute_loo=compute_loo)
|
||||
translation = None
|
||||
if not rotation_only:
|
||||
translation = solve_rtk_imu_translation(
|
||||
sessions,
|
||||
rotation,
|
||||
knot_step_s=knot_step_s,
|
||||
compute_loo=compute_loo,
|
||||
)
|
||||
audit_payload = dataset_audit(sessions)
|
||||
rotation_payload = _jsonable(rotation)
|
||||
translation_payload = None if translation is None else _jsonable(translation)
|
||||
(output / "dataset_audit.json").write_text(
|
||||
json.dumps(audit_payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
|
||||
)
|
||||
(output / "rotation_result.json").write_text(
|
||||
json.dumps(rotation_payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
|
||||
)
|
||||
if translation_payload is not None:
|
||||
(output / "translation_result.json").write_text(
|
||||
json.dumps(translation_payload, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
|
||||
)
|
||||
interpretation_complete = bool(rtk_frame_definition.strip() and rtk_reference_point.strip())
|
||||
accepted = bool(
|
||||
rotation.ok and translation is not None and translation.ok and interpretation_complete
|
||||
)
|
||||
blockers = []
|
||||
if not rotation.ok:
|
||||
blockers.append('rotation quality gates failed')
|
||||
if translation is None or not translation.ok:
|
||||
blockers.append('translation quality gates failed or were not run')
|
||||
if not rtk_frame_definition.strip():
|
||||
blockers.append('RTK frame_definition is empty')
|
||||
if not rtk_reference_point.strip():
|
||||
blockers.append('RTK reference_point is empty')
|
||||
summary = {
|
||||
"status": "accepted" if accepted else "diagnostic_not_accepted",
|
||||
"transform_convention": "T_RTK_IMU maps IMU coordinates into the RTK sensor frame",
|
||||
"rtk_frame_definition": rtk_frame_definition,
|
||||
"rtk_reference_point": rtk_reference_point,
|
||||
"interpretation_blockers": blockers,
|
||||
"R_RTK_IMU": rotation.R_RTK_IMU.tolist(),
|
||||
"t_RTK_IMU_m": None if translation is None else translation.t_RTK_IMU_m.tolist(),
|
||||
"T_RTK_IMU": None if translation is None else translation.T_RTK_IMU.tolist(),
|
||||
"rotation_ok": rotation.ok,
|
||||
"translation_ok": None if translation is None else translation.ok,
|
||||
"rotation_result": "rotation_result.json",
|
||||
"translation_result": None if translation is None else "translation_result.json",
|
||||
"dataset_audit": "dataset_audit.json",
|
||||
}
|
||||
(output / "summary.json").write_text(
|
||||
json.dumps(summary, ensure_ascii=False, indent=2) + "\n", encoding="utf-8"
|
||||
)
|
||||
return rotation, translation
|
||||
@@ -0,0 +1,439 @@
|
||||
"""Rotation and residual time-offset calibration between G90 RTK and HI13 IMU."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
from scipy.optimize import least_squares
|
||||
from scipy.sparse import lil_matrix
|
||||
|
||||
from .contracts import ImuSeries, MotionPair
|
||||
from .geometry import orthonormalize_rotation, rpy_deg_xyz, so3_exp, so3_log
|
||||
from .imu_preintegration import apply_bias_jacobian_correction, preintegrate_gyro
|
||||
from .rotation_handeye import estimate_rotation_handeye_initial
|
||||
from .rtk_attitude import GNHPR_CANDIDATES, GnhprConvention, gnhpr_to_rotation_enu_rtk
|
||||
from .rtk_io import RtkSeries
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RotationSession:
|
||||
session_id: str
|
||||
batch_id: str
|
||||
imu: ImuSeries
|
||||
rtk: RtkSeries
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TimeOffsetAudit:
|
||||
offset_s: float
|
||||
peak_correlation: float
|
||||
second_best_correlation: float
|
||||
evaluated_samples: int
|
||||
reliable: bool
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RotationCalibrationResult:
|
||||
R_RTK_IMU: np.ndarray
|
||||
rpy_deg: np.ndarray
|
||||
gyro_bias_by_session_rad_s: dict[str, np.ndarray]
|
||||
time_offset: TimeOffsetAudit
|
||||
applied_time_offset_s: float
|
||||
convention: GnhprConvention
|
||||
convention_scores_deg: dict[str, float]
|
||||
pair_count: int
|
||||
residual_rms_deg: float
|
||||
residual_median_deg: float
|
||||
residual_p95_deg: float
|
||||
rotation_std_deg: np.ndarray
|
||||
information_singular_values: np.ndarray
|
||||
per_session_rms_deg: dict[str, float]
|
||||
loo_delta_deg: dict[str, float]
|
||||
ok: bool
|
||||
notes: tuple[str, ...]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _Pair:
|
||||
session_index: int
|
||||
session_id: str
|
||||
R_A: np.ndarray
|
||||
delta_R_zero_bias: np.ndarray
|
||||
J_bg: np.ndarray
|
||||
weight: float
|
||||
|
||||
|
||||
def _attitude_rows(rtk: RtkSeries, convention: GnhprConvention) -> tuple[np.ndarray, np.ndarray]:
|
||||
valid = rtk.attitude_valid & rtk.position_valid
|
||||
t = rtk.attitude_t_s[valid]
|
||||
angles = np.column_stack(
|
||||
[rtk.heading_deg[valid], rtk.pitch_deg[valid], rtk.roll_deg[valid]]
|
||||
)
|
||||
if t.size < 2:
|
||||
raise ValueError(f"not enough valid RTK attitude rows: {rtk.source}")
|
||||
# GGA is faster than HPR, so nearest-neighbour export repeats attitude rows.
|
||||
# Keep only changes and place them at the first associated GGA measurement.
|
||||
changed = np.ones(t.size, dtype=bool)
|
||||
changed[1:] = np.any(np.abs(np.diff(angles, axis=0)) > 1e-10, axis=1)
|
||||
t = t[changed]
|
||||
angles = angles[changed]
|
||||
order = np.argsort(t)
|
||||
t = t[order]
|
||||
angles = angles[order]
|
||||
unique_t, unique_indices = np.unique(t, return_index=True)
|
||||
rotations = gnhpr_to_rotation_enu_rtk(
|
||||
angles[unique_indices, 0],
|
||||
angles[unique_indices, 1],
|
||||
angles[unique_indices, 2],
|
||||
convention,
|
||||
)
|
||||
return unique_t, rotations
|
||||
|
||||
|
||||
def _rtk_angular_speed(t: np.ndarray, rotations: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||
dt = np.diff(t)
|
||||
valid = (dt >= 0.03) & (dt <= 0.5)
|
||||
midpoint = 0.5 * (t[:-1] + t[1:])
|
||||
speed = np.array(
|
||||
[np.linalg.norm(so3_log(rotations[i].T @ rotations[i + 1])) for i in range(t.size - 1)]
|
||||
) / np.maximum(dt, 1e-6)
|
||||
return midpoint[valid], speed[valid]
|
||||
|
||||
|
||||
def _correlation(a: np.ndarray, b: np.ndarray) -> float:
|
||||
a = np.asarray(a, dtype=float)
|
||||
b = np.asarray(b, dtype=float)
|
||||
if a.size < 20 or np.std(a) < 1e-5 or np.std(b) < 1e-5:
|
||||
return np.nan
|
||||
return float(np.corrcoef(a, b)[0, 1])
|
||||
|
||||
|
||||
def audit_time_offset(
|
||||
sessions: list[RotationSession] | tuple[RotationSession, ...],
|
||||
*,
|
||||
search_half_width_s: float = 0.30,
|
||||
step_s: float = 0.005,
|
||||
) -> TimeOffsetAudit:
|
||||
"""Estimate residual ``t_IMU - t_RTK`` from invariant angular-speed norms."""
|
||||
|
||||
offsets = np.arange(-search_half_width_s, search_half_width_s + 0.5 * step_s, step_s)
|
||||
session_series: list[tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]] = []
|
||||
total_samples = 0
|
||||
for session in sessions:
|
||||
t_rtk, rotations = _attitude_rows(session.rtk, GNHPR_CANDIDATES[0])
|
||||
midpoint, rtk_speed = _rtk_angular_speed(t_rtk, rotations)
|
||||
imu_speed = np.linalg.norm(session.imu.gyro_rad_s, axis=1)
|
||||
motion = rtk_speed > np.deg2rad(0.5)
|
||||
midpoint = midpoint[motion]
|
||||
rtk_speed = rtk_speed[motion]
|
||||
if midpoint.size >= 20:
|
||||
session_series.append((midpoint, rtk_speed, session.imu.t_s, imu_speed))
|
||||
total_samples += int(midpoint.size)
|
||||
if not session_series:
|
||||
return TimeOffsetAudit(0.0, np.nan, np.nan, 0, False)
|
||||
scores = []
|
||||
for offset in offsets:
|
||||
per_session = []
|
||||
for midpoint, rtk_speed, imu_t, imu_speed in session_series:
|
||||
query = midpoint + offset
|
||||
inside = (query >= imu_t[0]) & (query <= imu_t[-1])
|
||||
if np.count_nonzero(inside) < 20:
|
||||
continue
|
||||
interpolated = np.interp(query[inside], imu_t, imu_speed)
|
||||
value = _correlation(rtk_speed[inside], interpolated)
|
||||
if np.isfinite(value):
|
||||
per_session.append(value)
|
||||
scores.append(float(np.median(per_session)) if per_session else np.nan)
|
||||
values = np.asarray(scores, dtype=float)
|
||||
if not np.any(np.isfinite(values)):
|
||||
return TimeOffsetAudit(0.0, np.nan, np.nan, total_samples, False)
|
||||
best_index = int(np.nanargmax(values))
|
||||
exclusion = np.abs(offsets - offsets[best_index]) >= 0.03
|
||||
second = float(np.nanmax(values[exclusion])) if np.any(np.isfinite(values[exclusion])) else np.nan
|
||||
peak = float(values[best_index])
|
||||
reliable = bool(peak >= 0.35 and (not np.isfinite(second) or peak - second >= 0.015))
|
||||
return TimeOffsetAudit(float(offsets[best_index]), peak, second, total_samples, reliable)
|
||||
|
||||
|
||||
def _nearest_index(times: np.ndarray, target: float) -> int:
|
||||
index = int(np.searchsorted(times, target))
|
||||
candidates = [max(0, index - 1), min(times.size - 1, index)]
|
||||
return min(candidates, key=lambda item: abs(float(times[item]) - target))
|
||||
|
||||
|
||||
def _make_pairs(
|
||||
sessions: list[RotationSession],
|
||||
convention: GnhprConvention,
|
||||
time_offset_s: float,
|
||||
*,
|
||||
anchor_step_s: float = 5.0,
|
||||
intervals_s: tuple[float, ...] = (0.75, 1.5, 3.0),
|
||||
preintegration_cache: dict[tuple[str, float, float], object] | None = None,
|
||||
) -> list[_Pair]:
|
||||
pairs: list[_Pair] = []
|
||||
cache = {} if preintegration_cache is None else preintegration_cache
|
||||
for session_index, session in enumerate(sessions):
|
||||
t, rotations = _attitude_rows(session.rtk, convention)
|
||||
next_anchor = float(t[0])
|
||||
for i in range(t.size - 1):
|
||||
if t[i] + 1e-9 < next_anchor:
|
||||
continue
|
||||
next_anchor = float(t[i] + anchor_step_s)
|
||||
for duration in intervals_s:
|
||||
j = _nearest_index(t, float(t[i] + duration))
|
||||
if j <= i or abs(float(t[j] - t[i]) - duration) > 0.18:
|
||||
continue
|
||||
imu_t0 = float(t[i] + time_offset_s)
|
||||
imu_t1 = float(t[j] + time_offset_s)
|
||||
if imu_t0 < session.imu.t_s[0] or imu_t1 > session.imu.t_s[-1]:
|
||||
continue
|
||||
r_a = orthonormalize_rotation(rotations[i].T @ rotations[j])
|
||||
cache_key = (session.session_id, round(imu_t0, 6), round(imu_t1, 6))
|
||||
preint = cache.get(cache_key)
|
||||
if preint is None:
|
||||
preint = preintegrate_gyro(
|
||||
session.imu.t_s,
|
||||
session.imu.gyro_rad_s,
|
||||
imu_t0,
|
||||
imu_t1,
|
||||
)
|
||||
cache[cache_key] = preint
|
||||
angle_a = np.linalg.norm(so3_log(r_a))
|
||||
angle_b = np.linalg.norm(so3_log(preint.delta_R))
|
||||
if min(angle_a, angle_b) < np.deg2rad(0.8):
|
||||
continue
|
||||
weight = float(np.clip(min(angle_a, angle_b) / np.deg2rad(5.0), 0.2, 3.0))
|
||||
pairs.append(
|
||||
_Pair(
|
||||
session_index=session_index,
|
||||
session_id=session.session_id,
|
||||
R_A=r_a,
|
||||
delta_R_zero_bias=preint.delta_R,
|
||||
J_bg=preint.J_bg,
|
||||
weight=weight,
|
||||
)
|
||||
)
|
||||
return pairs
|
||||
|
||||
|
||||
def _solve_core(sessions: list[RotationSession], pairs: list[_Pair]) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
|
||||
if len(pairs) < 6:
|
||||
raise ValueError("need at least 6 excited RTK--IMU rotation pairs")
|
||||
generic = [
|
||||
MotionPair(
|
||||
session_id=pair.session_id,
|
||||
i=index,
|
||||
j=index + 1,
|
||||
t_i_s=0.0,
|
||||
t_j_s=1.0,
|
||||
R_A=pair.R_A,
|
||||
R_B=pair.delta_R_zero_bias,
|
||||
metadata={"weight": pair.weight},
|
||||
)
|
||||
for index, pair in enumerate(pairs)
|
||||
]
|
||||
r0 = estimate_rotation_handeye_initial(generic, min_rotation_deg=0.5)
|
||||
session_count = len(sessions)
|
||||
|
||||
def unpack(parameters: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||
return orthonormalize_rotation(so3_exp(parameters[:3])), parameters[3:].reshape(session_count, 3)
|
||||
|
||||
def residual(parameters: np.ndarray) -> np.ndarray:
|
||||
r_x, biases = unpack(parameters)
|
||||
rows = []
|
||||
for pair in pairs:
|
||||
corrected = apply_bias_jacobian_correction(
|
||||
pair.delta_R_zero_bias,
|
||||
pair.J_bg,
|
||||
biases[pair.session_index],
|
||||
)
|
||||
error = so3_log(r_x.T @ pair.R_A @ r_x @ corrected.T)
|
||||
rows.append(np.sqrt(pair.weight) * error)
|
||||
# HI13 bias is session-specific; this weak prior only removes degenerate
|
||||
# bias/extrinsic trades and is much looser than observed static bias.
|
||||
rows.append((biases / 0.03).reshape(-1))
|
||||
return np.concatenate(rows)
|
||||
|
||||
initial = np.concatenate([so3_log(r0), np.zeros(3 * session_count)])
|
||||
jacobian_pattern = lil_matrix((3 * len(pairs) + 3 * session_count, initial.size), dtype=int)
|
||||
for pair_index, pair in enumerate(pairs):
|
||||
row = 3 * pair_index
|
||||
jacobian_pattern[row : row + 3, 0:3] = 1
|
||||
bias_col = 3 + 3 * pair.session_index
|
||||
jacobian_pattern[row : row + 3, bias_col : bias_col + 3] = 1
|
||||
prior_row = 3 * len(pairs)
|
||||
jacobian_pattern[prior_row:, 3:] = 1
|
||||
opt = least_squares(
|
||||
residual,
|
||||
initial,
|
||||
loss="huber",
|
||||
f_scale=np.deg2rad(0.5),
|
||||
jac_sparsity=jacobian_pattern.tocsr(),
|
||||
tr_solver='lsmr',
|
||||
max_nfev=40,
|
||||
)
|
||||
r_x, biases = unpack(opt.x)
|
||||
errors = []
|
||||
for pair in pairs:
|
||||
corrected = apply_bias_jacobian_correction(
|
||||
pair.delta_R_zero_bias,
|
||||
pair.J_bg,
|
||||
biases[pair.session_index],
|
||||
)
|
||||
errors.append(np.degrees(np.linalg.norm(so3_log(r_x.T @ pair.R_A @ r_x @ corrected.T))))
|
||||
jacobian = opt.jac.toarray() if hasattr(opt.jac, 'toarray') else np.asarray(opt.jac, dtype=float)
|
||||
information = jacobian.T @ jacobian
|
||||
dof = max(residual(opt.x).size - opt.x.size, 1)
|
||||
variance = float(np.sum(residual(opt.x) ** 2) / dof)
|
||||
covariance = np.linalg.pinv(information, rcond=1e-10) * variance
|
||||
return r_x, biases, np.asarray(errors), covariance
|
||||
|
||||
|
||||
def solve_rtk_imu_rotation(
|
||||
sessions: list[RotationSession] | tuple[RotationSession, ...],
|
||||
*,
|
||||
compute_loo: bool = True,
|
||||
) -> RotationCalibrationResult:
|
||||
"""Solve shared ``R_RTK_IMU`` and per-session gyro biases."""
|
||||
|
||||
items = list(sessions)
|
||||
if not items:
|
||||
raise ValueError("at least one RTK--IMU session is required")
|
||||
time_audit = audit_time_offset(items)
|
||||
offset = time_audit.offset_s if time_audit.reliable else 0.0
|
||||
candidates: list[tuple[GnhprConvention, list[_Pair]]] = []
|
||||
scores: dict[str, float] = {}
|
||||
preintegration_cache: dict[tuple[str, float, float], object] = {}
|
||||
for convention in GNHPR_CANDIDATES:
|
||||
pairs = _make_pairs(items, convention, offset, preintegration_cache=preintegration_cache)
|
||||
if len(pairs) < 6:
|
||||
scores[convention.name] = 1e9
|
||||
continue
|
||||
generic = [
|
||||
MotionPair(
|
||||
session_id=pair.session_id,
|
||||
i=index,
|
||||
j=index + 1,
|
||||
t_i_s=0.0,
|
||||
t_j_s=1.0,
|
||||
R_A=pair.R_A,
|
||||
R_B=pair.delta_R_zero_bias,
|
||||
metadata={'weight': pair.weight},
|
||||
)
|
||||
for index, pair in enumerate(pairs)
|
||||
]
|
||||
initial_rotation = estimate_rotation_handeye_initial(generic, min_rotation_deg=0.5)
|
||||
preliminary_errors = np.asarray(
|
||||
[
|
||||
np.degrees(
|
||||
np.linalg.norm(
|
||||
so3_log(
|
||||
initial_rotation.T
|
||||
@ pair.R_A
|
||||
@ initial_rotation
|
||||
@ pair.delta_R_zero_bias.T
|
||||
)
|
||||
)
|
||||
)
|
||||
for pair in pairs
|
||||
]
|
||||
)
|
||||
scores[convention.name] = float(np.sqrt(np.mean(preliminary_errors**2)))
|
||||
candidates.append((convention, pairs))
|
||||
if not candidates:
|
||||
raise ValueError("no GNHPR convention produced enough rotation pairs")
|
||||
convention, pairs = min(candidates, key=lambda item: scores[item[0].name])
|
||||
rotation, biases, errors, covariance = _solve_core(items, pairs)
|
||||
per_session = {}
|
||||
for session in items:
|
||||
values = [error for pair, error in zip(pairs, errors) if pair.session_id == session.session_id]
|
||||
per_session[session.session_id] = (
|
||||
float(np.sqrt(np.mean(np.asarray(values) ** 2))) if values else np.nan
|
||||
)
|
||||
loo = {}
|
||||
if compute_loo and len(items) >= 3:
|
||||
for omitted in items:
|
||||
kept_pairs = [
|
||||
pair
|
||||
for pair in pairs
|
||||
if pair.session_id != omitted.session_id
|
||||
]
|
||||
if len(kept_pairs) < 6:
|
||||
loo[omitted.session_id] = np.nan
|
||||
continue
|
||||
def loo_residual(rotvec: np.ndarray) -> np.ndarray:
|
||||
candidate = orthonormalize_rotation(so3_exp(rotvec))
|
||||
rows = []
|
||||
for pair in kept_pairs:
|
||||
corrected = apply_bias_jacobian_correction(
|
||||
pair.delta_R_zero_bias,
|
||||
pair.J_bg,
|
||||
biases[pair.session_index],
|
||||
)
|
||||
rows.append(
|
||||
np.sqrt(pair.weight)
|
||||
* so3_log(candidate.T @ pair.R_A @ candidate @ corrected.T)
|
||||
)
|
||||
return np.concatenate(rows)
|
||||
|
||||
loo_opt = least_squares(
|
||||
loo_residual,
|
||||
so3_log(rotation),
|
||||
loss='huber',
|
||||
f_scale=np.deg2rad(0.5),
|
||||
max_nfev=30,
|
||||
)
|
||||
loo_rotation = orthonormalize_rotation(so3_exp(loo_opt.x))
|
||||
loo[omitted.session_id] = float(np.degrees(np.linalg.norm(so3_log(rotation.T @ loo_rotation))))
|
||||
rotation_cov = covariance[:3, :3]
|
||||
std_deg = np.degrees(np.sqrt(np.maximum(np.diag(rotation_cov), 0.0)))
|
||||
singular_values = np.linalg.svd(np.linalg.pinv(rotation_cov, rcond=1e-12), compute_uv=False)
|
||||
rms = float(np.sqrt(np.mean(errors**2)))
|
||||
median = float(np.median(errors))
|
||||
p95 = float(np.percentile(errors, 95.0))
|
||||
finite_loo = [value for value in loo.values() if np.isfinite(value)]
|
||||
sorted_scores = sorted(scores.values())
|
||||
convention_gap = sorted_scores[1] - sorted_scores[0] if len(sorted_scores) > 1 else np.inf
|
||||
ok = bool(
|
||||
len(pairs) >= 20
|
||||
and rms <= 1.0
|
||||
and p95 <= 2.0
|
||||
and float(np.max(std_deg)) <= 0.5
|
||||
and (not finite_loo or max(finite_loo) <= 1.0)
|
||||
and convention_gap >= 0.05
|
||||
)
|
||||
notes = [
|
||||
"transform convention: p_RTK = R_RTK_IMU p_IMU",
|
||||
f"residual time convention: t_IMU = t_RTK + {offset:+.6f} s",
|
||||
f"GNHPR convention score gap={convention_gap:.4f} deg",
|
||||
"GNHPR alternatives use zero-bias prescreen scores; only the winner is jointly refined",
|
||||
"LOO is conditional: per-session gyro biases are held at their all-session estimates",
|
||||
]
|
||||
if not time_audit.reliable:
|
||||
notes.append("time-offset correlation was ambiguous; held residual offset at zero")
|
||||
if convention is not GNHPR_CANDIDATES[0]:
|
||||
notes.append("empirical best GNHPR convention differs from protocol expectation; manual verification required")
|
||||
if not ok:
|
||||
notes.append("rotation failed one or more strict acceptance gates")
|
||||
return RotationCalibrationResult(
|
||||
R_RTK_IMU=rotation,
|
||||
rpy_deg=rpy_deg_xyz(rotation),
|
||||
gyro_bias_by_session_rad_s={
|
||||
session.session_id: biases[index].copy() for index, session in enumerate(items)
|
||||
},
|
||||
time_offset=time_audit,
|
||||
applied_time_offset_s=offset,
|
||||
convention=convention,
|
||||
convention_scores_deg=scores,
|
||||
pair_count=len(pairs),
|
||||
residual_rms_deg=rms,
|
||||
residual_median_deg=median,
|
||||
residual_p95_deg=p95,
|
||||
rotation_std_deg=std_deg,
|
||||
information_singular_values=singular_values,
|
||||
per_session_rms_deg=per_session,
|
||||
loo_delta_deg=loo,
|
||||
ok=ok,
|
||||
notes=tuple(notes),
|
||||
)
|
||||
@@ -0,0 +1,389 @@
|
||||
"""Lever-arm calibration from RTK positions and full IMU preintegration."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
|
||||
import numpy as np
|
||||
from scipy.sparse import coo_matrix, csr_matrix, eye
|
||||
from scipy.sparse.linalg import lsqr, splu
|
||||
from scipy.spatial.transform import Rotation, Slerp
|
||||
|
||||
from .geometry import make_transform, orthonormalize_rotation, so3_exp
|
||||
from .rtk_imu_rotation import RotationCalibrationResult, RotationSession, _attitude_rows
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class TranslationCalibrationResult:
|
||||
lever_IMU_to_RTK_in_IMU_m: np.ndarray
|
||||
t_RTK_IMU_m: np.ndarray
|
||||
T_RTK_IMU: np.ndarray
|
||||
translation_std_m: np.ndarray
|
||||
lever_information_singular_values: np.ndarray
|
||||
lever_precision_rank: int
|
||||
position_residual_rms_xyz_m: np.ndarray
|
||||
velocity_residual_rms_xyz_m_s: np.ndarray
|
||||
accel_bias_by_session_m_s2: dict[str, np.ndarray]
|
||||
knot_count_by_session: dict[str, int]
|
||||
loo_delta_m: dict[str, np.ndarray]
|
||||
ok: bool
|
||||
notes: tuple[str, ...]
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class _SessionFactors:
|
||||
session: RotationSession
|
||||
knot_t_s: np.ndarray
|
||||
position_enu_m: np.ndarray
|
||||
R_ENU_IMU: np.ndarray
|
||||
delta_p: tuple[np.ndarray, ...]
|
||||
delta_v: tuple[np.ndarray, ...]
|
||||
J_p_ba: tuple[np.ndarray, ...]
|
||||
J_v_ba: tuple[np.ndarray, ...]
|
||||
duration_s: np.ndarray
|
||||
|
||||
|
||||
def _preintegrate_translation_interval(
|
||||
times_s: np.ndarray,
|
||||
gyro_rad_s: np.ndarray,
|
||||
acc_m_s2: np.ndarray,
|
||||
t0: float,
|
||||
t1: float,
|
||||
gyro_bias_rad_s: np.ndarray,
|
||||
) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray, float]:
|
||||
"""Fast nominal ``delta_p/delta_v`` and accel-bias Jacobians.
|
||||
|
||||
Rotation covariance and gyro-bias Jacobians are deliberately omitted here:
|
||||
rotation and gyro bias have already been fixed by Phase R1, while the
|
||||
translation linear system only consumes the accelerometer-bias Jacobians.
|
||||
"""
|
||||
|
||||
left = max(int(np.searchsorted(times_s, t0, side='left') - 1), 0)
|
||||
right = min(int(np.searchsorted(times_s, t1, side='right')), times_s.size - 1)
|
||||
delta_r = np.eye(3)
|
||||
delta_v = np.zeros(3)
|
||||
delta_p = np.zeros(3)
|
||||
j_v_ba = np.zeros((3, 3))
|
||||
j_p_ba = np.zeros((3, 3))
|
||||
for index in range(left, right):
|
||||
sample_t0 = float(times_s[index])
|
||||
sample_t1 = float(times_s[index + 1])
|
||||
if sample_t1 <= t0 or sample_t0 >= t1:
|
||||
continue
|
||||
segment_t0 = max(sample_t0, t0)
|
||||
segment_t1 = min(sample_t1, t1)
|
||||
dt = segment_t1 - segment_t0
|
||||
if dt <= 0.0:
|
||||
continue
|
||||
sample_dt = max(sample_t1 - sample_t0, 1e-12)
|
||||
u0 = (segment_t0 - sample_t0) / sample_dt
|
||||
u1 = (segment_t1 - sample_t0) / sample_dt
|
||||
gyro0 = (1.0 - u0) * gyro_rad_s[index] + u0 * gyro_rad_s[index + 1]
|
||||
gyro1 = (1.0 - u1) * gyro_rad_s[index] + u1 * gyro_rad_s[index + 1]
|
||||
acc0 = (1.0 - u0) * acc_m_s2[index] + u0 * acc_m_s2[index + 1]
|
||||
acc1 = (1.0 - u1) * acc_m_s2[index] + u1 * acc_m_s2[index + 1]
|
||||
omega = 0.5 * (gyro0 + gyro1) - gyro_bias_rad_s
|
||||
acc = 0.5 * (acc0 + acc1)
|
||||
r_i = delta_r
|
||||
delta_p = delta_p + delta_v * dt + 0.5 * r_i @ acc * dt**2
|
||||
delta_v = delta_v + r_i @ acc * dt
|
||||
j_p_ba = j_p_ba + j_v_ba * dt - 0.5 * r_i * dt**2
|
||||
j_v_ba = j_v_ba - r_i * dt
|
||||
delta_r = orthonormalize_rotation(delta_r @ so3_exp(omega * dt))
|
||||
return delta_p, delta_v, j_p_ba, j_v_ba, float(max(t1 - t0, 0.0))
|
||||
|
||||
|
||||
def _make_session_factors(
|
||||
session: RotationSession,
|
||||
rotation: RotationCalibrationResult,
|
||||
*,
|
||||
knot_step_s: float,
|
||||
) -> _SessionFactors:
|
||||
t_attitude, r_enu_rtk = _attitude_rows(session.rtk, rotation.convention)
|
||||
position_valid = session.rtk.position_valid
|
||||
t_position = session.rtk.t_s[position_valid]
|
||||
position = session.rtk.position_enu_m[position_valid]
|
||||
time_offset_s = rotation.applied_time_offset_s
|
||||
start = max(float(t_attitude[0]), float(t_position[0]), float(session.imu.t_s[0] - time_offset_s))
|
||||
end = min(float(t_attitude[-1]), float(t_position[-1]), float(session.imu.t_s[-1] - time_offset_s))
|
||||
if end - start < 5.0:
|
||||
raise ValueError(f"{session.session_id}: less than 5 s common RTK/IMU support")
|
||||
knot_t = np.arange(start + 0.25, end - 0.25, knot_step_s)
|
||||
if knot_t.size < 4:
|
||||
raise ValueError(f"{session.session_id}: not enough translation knots")
|
||||
position_knots = np.column_stack(
|
||||
[np.interp(knot_t, t_position, position[:, axis]) for axis in range(3)]
|
||||
)
|
||||
r_enu_rtk_knots = Slerp(t_attitude, Rotation.from_matrix(r_enu_rtk))(knot_t).as_matrix()
|
||||
r_enu_imu = r_enu_rtk_knots @ rotation.R_RTK_IMU
|
||||
bg = rotation.gyro_bias_by_session_rad_s[session.session_id]
|
||||
delta_p: list[np.ndarray] = []
|
||||
delta_v: list[np.ndarray] = []
|
||||
j_p_ba: list[np.ndarray] = []
|
||||
j_v_ba: list[np.ndarray] = []
|
||||
durations = []
|
||||
for t0, t1 in zip(knot_t[:-1], knot_t[1:]):
|
||||
dp, dv, jp, jv, duration = _preintegrate_translation_interval(
|
||||
session.imu.t_s,
|
||||
session.imu.gyro_rad_s,
|
||||
session.imu.acc_m_s2,
|
||||
float(t0 + time_offset_s),
|
||||
float(t1 + time_offset_s),
|
||||
bg,
|
||||
)
|
||||
delta_p.append(dp)
|
||||
delta_v.append(dv)
|
||||
j_p_ba.append(jp)
|
||||
j_v_ba.append(jv)
|
||||
durations.append(duration)
|
||||
return _SessionFactors(
|
||||
session=session,
|
||||
knot_t_s=knot_t,
|
||||
position_enu_m=position_knots,
|
||||
R_ENU_IMU=r_enu_imu,
|
||||
delta_p=tuple(delta_p),
|
||||
delta_v=tuple(delta_v),
|
||||
J_p_ba=tuple(j_p_ba),
|
||||
J_v_ba=tuple(j_v_ba),
|
||||
duration_s=np.asarray(durations),
|
||||
)
|
||||
|
||||
|
||||
def _append_block(
|
||||
rows: list[int],
|
||||
cols: list[int],
|
||||
values: list[float],
|
||||
rhs: list[float],
|
||||
groups: list[int],
|
||||
matrix_blocks: list[tuple[int, np.ndarray]],
|
||||
vector: np.ndarray,
|
||||
sigma: np.ndarray,
|
||||
group: int,
|
||||
) -> None:
|
||||
row0 = len(rhs)
|
||||
for axis in range(3):
|
||||
rhs.append(float(vector[axis] / sigma[axis]))
|
||||
groups.append(group)
|
||||
for col0, block in matrix_blocks:
|
||||
for local_col in range(block.shape[1]):
|
||||
value = float(block[axis, local_col] / sigma[axis])
|
||||
if value != 0.0:
|
||||
rows.append(row0 + axis)
|
||||
cols.append(col0 + local_col)
|
||||
values.append(value)
|
||||
|
||||
|
||||
def _build_system(
|
||||
factors: list[_SessionFactors],
|
||||
*,
|
||||
position_sigma_xyz_m: np.ndarray,
|
||||
velocity_sigma_xyz_m_s: np.ndarray,
|
||||
) -> tuple[csr_matrix, np.ndarray, np.ndarray, dict[str, tuple[int, int]], list[tuple[str, int, str]]]:
|
||||
# x = [shared lever(3), per-session ba(3), per-knot velocities(3*K)]
|
||||
offsets: dict[str, tuple[int, int]] = {}
|
||||
variable_count = 3
|
||||
for item in factors:
|
||||
ba_offset = variable_count
|
||||
velocity_offset = ba_offset + 3
|
||||
offsets[item.session.session_id] = (ba_offset, velocity_offset)
|
||||
variable_count = velocity_offset + 3 * item.knot_t_s.size
|
||||
rows: list[int] = []
|
||||
cols: list[int] = []
|
||||
values: list[float] = []
|
||||
rhs: list[float] = []
|
||||
groups: list[int] = []
|
||||
factor_labels: list[tuple[str, int, str]] = []
|
||||
gravity = np.array([0.0, 0.0, -9.80665])
|
||||
group = 0
|
||||
for item in factors:
|
||||
ba_offset, velocity_offset = offsets[item.session.session_id]
|
||||
for index, dt in enumerate(item.duration_s):
|
||||
r_i = item.R_ENU_IMU[index]
|
||||
r_j = item.R_ENU_IMU[index + 1]
|
||||
dp_rtk = item.position_enu_m[index + 1] - item.position_enu_m[index]
|
||||
constant_p = dp_rtk - 0.5 * gravity * dt**2 - r_i @ item.delta_p[index]
|
||||
_append_block(
|
||||
rows,
|
||||
cols,
|
||||
values,
|
||||
rhs,
|
||||
groups,
|
||||
[
|
||||
(0, r_i - r_j),
|
||||
(ba_offset, -r_i @ item.J_p_ba[index]),
|
||||
(velocity_offset + 3 * index, -dt * np.eye(3)),
|
||||
],
|
||||
-constant_p,
|
||||
position_sigma_xyz_m,
|
||||
group,
|
||||
)
|
||||
factor_labels.append((item.session.session_id, group, "position"))
|
||||
group += 1
|
||||
constant_v = -gravity * dt - r_i @ item.delta_v[index]
|
||||
_append_block(
|
||||
rows,
|
||||
cols,
|
||||
values,
|
||||
rhs,
|
||||
groups,
|
||||
[
|
||||
(ba_offset, -r_i @ item.J_v_ba[index]),
|
||||
(velocity_offset + 3 * index, -np.eye(3)),
|
||||
(velocity_offset + 3 * (index + 1), np.eye(3)),
|
||||
],
|
||||
-constant_v,
|
||||
velocity_sigma_xyz_m_s,
|
||||
group,
|
||||
)
|
||||
factor_labels.append((item.session.session_id, group, "velocity"))
|
||||
group += 1
|
||||
# Loose physical bias prior. It prevents an unobservable constant
|
||||
# acceleration from masquerading as gravity while remaining data-led.
|
||||
_append_block(
|
||||
rows,
|
||||
cols,
|
||||
values,
|
||||
rhs,
|
||||
groups,
|
||||
[(ba_offset, np.eye(3))],
|
||||
np.zeros(3),
|
||||
np.full(3, 0.5),
|
||||
group,
|
||||
)
|
||||
factor_labels.append((item.session.session_id, group, "bias_prior"))
|
||||
group += 1
|
||||
matrix = coo_matrix((values, (rows, cols)), shape=(len(rhs), variable_count)).tocsr()
|
||||
return matrix, np.asarray(rhs), np.asarray(groups), offsets, factor_labels
|
||||
|
||||
|
||||
def _irls(matrix: csr_matrix, rhs: np.ndarray, groups: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
|
||||
row_weights = np.ones(rhs.size)
|
||||
solution = np.zeros(matrix.shape[1])
|
||||
for _ in range(5):
|
||||
weighted = matrix.multiply(row_weights[:, None])
|
||||
solution = lsqr(weighted, rhs * row_weights, atol=1e-10, btol=1e-10, iter_lim=3000)[0]
|
||||
residual = matrix @ solution - rhs
|
||||
new_weights = np.ones_like(row_weights)
|
||||
for group in np.unique(groups):
|
||||
selection = groups == group
|
||||
norm = float(np.linalg.norm(residual[selection]))
|
||||
if norm > 3.0:
|
||||
new_weights[selection] = np.sqrt(3.0 / norm)
|
||||
if np.max(np.abs(new_weights - row_weights)) < 1e-3:
|
||||
row_weights = new_weights
|
||||
break
|
||||
row_weights = new_weights
|
||||
return solution, row_weights
|
||||
|
||||
|
||||
def _solve_factors(
|
||||
factors: list[_SessionFactors],
|
||||
position_sigma: np.ndarray,
|
||||
velocity_sigma: np.ndarray,
|
||||
) -> tuple[np.ndarray, np.ndarray, csr_matrix, np.ndarray, dict[str, tuple[int, int]], np.ndarray, np.ndarray]:
|
||||
matrix, rhs, groups, offsets, labels = _build_system(
|
||||
factors,
|
||||
position_sigma_xyz_m=position_sigma,
|
||||
velocity_sigma_xyz_m_s=velocity_sigma,
|
||||
)
|
||||
solution, row_weights = _irls(matrix, rhs, groups)
|
||||
weighted = matrix.multiply(row_weights[:, None]).tocsr()
|
||||
residual = matrix @ solution - rhs
|
||||
data_groups = {group for _, group, kind in labels if kind != "bias_prior"}
|
||||
data_rows = np.isin(groups, list(data_groups))
|
||||
variance = float(np.sum((residual[data_rows] * row_weights[data_rows]) ** 2) / max(np.count_nonzero(data_rows) - solution.size, 1))
|
||||
information = (weighted.T @ weighted).tocsc() + eye(weighted.shape[1], format="csc") * 1e-10
|
||||
h_ll = information[:3, :3].toarray()
|
||||
h_ln = information[:3, 3:]
|
||||
h_nn = information[3:, 3:]
|
||||
nuisance_solve = splu(h_nn).solve(h_ln.T.toarray())
|
||||
schur = h_ll - h_ln.toarray() @ nuisance_solve
|
||||
covariance_lever = np.linalg.pinv(schur, rcond=1e-10) * variance
|
||||
return solution, covariance_lever, matrix, rhs, offsets, groups, residual
|
||||
|
||||
|
||||
def solve_rtk_imu_translation(
|
||||
sessions: list[RotationSession] | tuple[RotationSession, ...],
|
||||
rotation: RotationCalibrationResult,
|
||||
*,
|
||||
knot_step_s: float = 2.0,
|
||||
compute_loo: bool = True,
|
||||
) -> TranslationCalibrationResult:
|
||||
"""Estimate the shared IMU-to-RTK lever arm and return ``T_RTK_IMU``."""
|
||||
|
||||
items = list(sessions)
|
||||
factors = [_make_session_factors(session, rotation, knot_step_s=knot_step_s) for session in items]
|
||||
position_sigma = np.array([0.025, 0.025, 0.060])
|
||||
velocity_sigma = np.array([0.08, 0.08, 0.12])
|
||||
solution, covariance_l, matrix, rhs, offsets, groups, residual = _solve_factors(
|
||||
factors, position_sigma, velocity_sigma
|
||||
)
|
||||
lever = solution[:3]
|
||||
t_rtk_imu = -rotation.R_RTK_IMU @ lever
|
||||
covariance_t = rotation.R_RTK_IMU @ covariance_l @ rotation.R_RTK_IMU.T
|
||||
std_t = np.sqrt(np.maximum(np.diag(covariance_t), 0.0))
|
||||
schur_information = np.linalg.pinv(covariance_l, rcond=1e-12)
|
||||
singular_values = np.linalg.svd(schur_information, compute_uv=False)
|
||||
threshold = max(float(singular_values[0]) * 1e-4, 1e-9)
|
||||
rank = int(np.count_nonzero(singular_values > threshold))
|
||||
|
||||
# Recover physical residuals: system rows are grouped in XYZ triples and
|
||||
# alternate position/velocity, followed by one bias prior per session.
|
||||
position_errors: list[np.ndarray] = []
|
||||
velocity_errors: list[np.ndarray] = []
|
||||
cursor = 0
|
||||
for item in factors:
|
||||
for _ in range(item.knot_t_s.size - 1):
|
||||
position_errors.append(residual[cursor : cursor + 3] * position_sigma)
|
||||
cursor += 3
|
||||
velocity_errors.append(residual[cursor : cursor + 3] * velocity_sigma)
|
||||
cursor += 3
|
||||
cursor += 3
|
||||
pos_rms = np.sqrt(np.mean(np.asarray(position_errors) ** 2, axis=0))
|
||||
vel_rms = np.sqrt(np.mean(np.asarray(velocity_errors) ** 2, axis=0))
|
||||
biases = {
|
||||
item.session.session_id: solution[offsets[item.session.session_id][0] : offsets[item.session.session_id][0] + 3].copy()
|
||||
for item in factors
|
||||
}
|
||||
loo: dict[str, np.ndarray] = {}
|
||||
if compute_loo and len(factors) >= 3:
|
||||
for omitted in factors:
|
||||
kept = [item for item in factors if item.session.session_id != omitted.session.session_id]
|
||||
loo_solution, *_ = _solve_factors(kept, position_sigma, velocity_sigma)
|
||||
loo[omitted.session.session_id] = (-rotation.R_RTK_IMU @ loo_solution[:3]) - t_rtk_imu
|
||||
max_loo_xy = max((float(np.linalg.norm(value[:2])) for value in loo.values()), default=0.0)
|
||||
max_loo_z = max((abs(float(value[2])) for value in loo.values()), default=0.0)
|
||||
ok = bool(
|
||||
rank == 3
|
||||
and float(np.max(std_t[:2])) <= 0.05
|
||||
and float(std_t[2]) <= 0.10
|
||||
and float(np.max(pos_rms[:2])) <= 0.10
|
||||
and float(pos_rms[2]) <= 0.20
|
||||
and max_loo_xy <= 0.10
|
||||
and max_loo_z <= 0.20
|
||||
)
|
||||
notes = [
|
||||
"lever l is vector IMU-origin -> RTK-origin expressed in IMU",
|
||||
"transform translation uses t_RTK_IMU = -R_RTK_IMU @ l",
|
||||
"RTK position is never differentiated; position and velocity preintegration factors are solved jointly",
|
||||
]
|
||||
if not rotation.ok:
|
||||
notes.append("upstream rotation is not accepted, so translation is diagnostic only")
|
||||
ok = False
|
||||
if not ok:
|
||||
notes.append("translation failed one or more strict acceptance gates")
|
||||
return TranslationCalibrationResult(
|
||||
lever_IMU_to_RTK_in_IMU_m=lever,
|
||||
t_RTK_IMU_m=t_rtk_imu,
|
||||
T_RTK_IMU=make_transform(t_rtk_imu, rotation.R_RTK_IMU),
|
||||
translation_std_m=std_t,
|
||||
lever_information_singular_values=singular_values,
|
||||
lever_precision_rank=rank,
|
||||
position_residual_rms_xyz_m=pos_rms,
|
||||
velocity_residual_rms_xyz_m_s=vel_rms,
|
||||
accel_bias_by_session_m_s2=biases,
|
||||
knot_count_by_session={item.session.session_id: int(item.knot_t_s.size) for item in factors},
|
||||
loo_delta_m=loo,
|
||||
ok=ok,
|
||||
notes=tuple(notes),
|
||||
)
|
||||
@@ -0,0 +1,116 @@
|
||||
"""RTK CSV loading for the independent RTK--IMU calibration path."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import dataclass
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from .geodesy import geodetic_to_enu
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RtkSeries:
|
||||
"""Normalized RTK observations on the IMU device clock."""
|
||||
|
||||
t_s: np.ndarray
|
||||
attitude_t_s: np.ndarray
|
||||
position_enu_m: np.ndarray
|
||||
heading_deg: np.ndarray
|
||||
pitch_deg: np.ndarray
|
||||
roll_deg: np.ndarray
|
||||
fix_quality: np.ndarray
|
||||
heading_quality: np.ndarray
|
||||
hdop: np.ndarray
|
||||
origin_geodetic: tuple[float, float, float]
|
||||
source: Path
|
||||
|
||||
@property
|
||||
def attitude_valid(self) -> np.ndarray:
|
||||
return (
|
||||
np.isfinite(self.heading_deg)
|
||||
& np.isfinite(self.pitch_deg)
|
||||
& np.isfinite(self.roll_deg)
|
||||
& np.isin(self.heading_quality, (4.0, 5.0))
|
||||
)
|
||||
|
||||
@property
|
||||
def position_valid(self) -> np.ndarray:
|
||||
return np.all(np.isfinite(self.position_enu_m), axis=1) & np.isin(
|
||||
self.fix_quality, (4.0, 5.0)
|
||||
)
|
||||
|
||||
|
||||
def _column(data: np.ndarray, name: str, *, default: float = np.nan) -> np.ndarray:
|
||||
names = set(data.dtype.names or ())
|
||||
if name not in names:
|
||||
return np.full(data.shape[0], default, dtype=float)
|
||||
return np.asarray(data[name], dtype=float).reshape(-1)
|
||||
|
||||
|
||||
def load_rtk_csv(path: Path | str) -> RtkSeries:
|
||||
"""Load an exported G90 RTK CSV and convert its positions to local ENU.
|
||||
|
||||
The required ``t`` column must already be NMEA measurement UTC mapped onto
|
||||
the IMU device clock. Host receive time is deliberately never accepted as
|
||||
a fallback because it is delayed by several seconds in the recorded data.
|
||||
"""
|
||||
|
||||
source = Path(path)
|
||||
if not source.is_file():
|
||||
raise FileNotFoundError(source)
|
||||
data = np.genfromtxt(source, delimiter=",", names=True, dtype=float, encoding="utf-8")
|
||||
if data.ndim == 0:
|
||||
data = np.array([data], dtype=data.dtype)
|
||||
names = set(data.dtype.names or ())
|
||||
required = {"t", "lat_deg", "lon_deg", "altitude_m", "fix_quality"}
|
||||
if not required.issubset(names):
|
||||
raise ValueError(f"RTK CSV must contain {sorted(required)}, got {sorted(names)}")
|
||||
t_s = _column(data, "t")
|
||||
measurement_utc = _column(data, "t_measurement_utc_s")
|
||||
hpr_measurement_utc = _column(data, "hpr_measurement_utc_s")
|
||||
attitude_t = t_s.copy()
|
||||
has_hpr_time = np.isfinite(measurement_utc) & np.isfinite(hpr_measurement_utc)
|
||||
attitude_t[has_hpr_time] += hpr_measurement_utc[has_hpr_time] - measurement_utc[has_hpr_time]
|
||||
order = np.argsort(t_s)
|
||||
position, origin = geodetic_to_enu(
|
||||
_column(data, "lat_deg")[order],
|
||||
_column(data, "lon_deg")[order],
|
||||
_column(data, "altitude_m")[order],
|
||||
)
|
||||
return RtkSeries(
|
||||
t_s=t_s[order],
|
||||
attitude_t_s=attitude_t[order],
|
||||
position_enu_m=position,
|
||||
heading_deg=_column(data, "heading_deg")[order],
|
||||
pitch_deg=_column(data, "pitch_deg")[order],
|
||||
roll_deg=_column(data, "roll_deg")[order],
|
||||
fix_quality=_column(data, "fix_quality", default=0.0)[order],
|
||||
heading_quality=_column(data, "heading_quality", default=0.0)[order],
|
||||
hdop=_column(data, "hdop")[order],
|
||||
origin_geodetic=origin,
|
||||
source=source,
|
||||
)
|
||||
|
||||
|
||||
def longest_valid_interval(t_s: np.ndarray, valid: np.ndarray, *, max_gap_s: float = 0.2) -> tuple[float, float]:
|
||||
"""Return the longest contiguous valid time interval."""
|
||||
|
||||
times = np.asarray(t_s, dtype=float).reshape(-1)
|
||||
mask = np.asarray(valid, dtype=bool).reshape(-1)
|
||||
indices = np.flatnonzero(mask)
|
||||
if indices.size == 0:
|
||||
raise ValueError("no valid RTK samples")
|
||||
best_start = best_end = int(indices[0])
|
||||
start = previous = int(indices[0])
|
||||
for index in indices[1:]:
|
||||
index = int(index)
|
||||
if index != previous + 1 or times[index] - times[previous] > max_gap_s:
|
||||
if times[previous] - times[start] > times[best_end] - times[best_start]:
|
||||
best_start, best_end = start, previous
|
||||
start = index
|
||||
previous = index
|
||||
if times[previous] - times[start] > times[best_end] - times[best_start]:
|
||||
best_start, best_end = start, previous
|
||||
return float(times[best_start]), float(times[best_end])
|
||||
@@ -0,0 +1,35 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from datetime import datetime, timezone
|
||||
|
||||
import numpy as np
|
||||
|
||||
from tools.export_g90_rtk_to_sessions import nmea_utc_to_unix_s
|
||||
from tools.time_alignment import fit_affine_clock
|
||||
|
||||
|
||||
def test_fit_affine_clock_large_epoch_and_receive_spike() -> None:
|
||||
device = np.linspace(10_000.0, 10_300.0, 601)
|
||||
host = 1_786_000_000.0 + 1.00002 * (device - device[0])
|
||||
host[250] += 0.25
|
||||
|
||||
model = fit_affine_clock(device, host)
|
||||
|
||||
assert abs(model.scale - 1.00002) < 1e-7
|
||||
assert abs(model.map(device[400]) - host[400]) < 1e-4
|
||||
assert model.inlier_count < model.sample_count
|
||||
assert abs(model.inverse(model.map(device[123])) - device[123]) < 1e-7
|
||||
|
||||
|
||||
def test_nmea_utc_uses_measurement_time_not_receive_time() -> None:
|
||||
receive = datetime(2026, 8, 14, 11, 34, 12, tzinfo=timezone.utc).timestamp()
|
||||
measurement = nmea_utc_to_unix_s("113408.85", receive)
|
||||
|
||||
assert abs((receive - measurement) - 3.15) < 1e-6
|
||||
|
||||
|
||||
def test_nmea_utc_resolves_midnight_rollover() -> None:
|
||||
receive = datetime(2026, 8, 15, 0, 0, 1, tzinfo=timezone.utc).timestamp()
|
||||
measurement = nmea_utc_to_unix_s("235959.50", receive)
|
||||
|
||||
assert abs((receive - measurement) - 1.5) < 1e-6
|
||||
@@ -0,0 +1,67 @@
|
||||
from __future__ import annotations
|
||||
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
from imu_lidar.geodesy import geodetic_to_enu
|
||||
from imu_lidar.geometry import so3_exp
|
||||
from imu_lidar.imu_preintegration import preintegrate_gyro, preintegrate_imu
|
||||
from imu_lidar.rtk_attitude import gnhpr_to_rotation_enu_rtk
|
||||
from imu_lidar.rtk_imu_translation import _preintegrate_translation_interval
|
||||
from imu_lidar.rtk_io import load_rtk_csv
|
||||
|
||||
|
||||
def test_geodetic_to_enu_has_expected_axis_and_scale() -> None:
|
||||
enu, origin = geodetic_to_enu(
|
||||
np.array([0.0, 0.0, 1e-5]),
|
||||
np.array([0.0, 1e-5, 0.0]),
|
||||
np.array([10.0, 10.0, 10.0]),
|
||||
)
|
||||
assert origin == (0.0, 0.0, 10.0)
|
||||
assert np.allclose(enu[0], 0.0, atol=1e-8)
|
||||
assert np.allclose(enu[1], [1.1131949, 0.0, 0.0], atol=2e-4)
|
||||
assert np.allclose(enu[2], [0.0, 1.1057428, 0.0], atol=2e-4)
|
||||
|
||||
|
||||
def test_gnhpr_heading_maps_north_clockwise_into_enu() -> None:
|
||||
rotations = gnhpr_to_rotation_enu_rtk(
|
||||
np.array([0.0, 90.0]),
|
||||
np.zeros(2),
|
||||
np.zeros(2),
|
||||
)
|
||||
assert np.allclose(rotations[0][:, 0], [0.0, 1.0, 0.0], atol=1e-12)
|
||||
assert np.allclose(rotations[1][:, 0], [1.0, 0.0, 0.0], atol=1e-12)
|
||||
|
||||
|
||||
def test_rtk_loader_uses_hpr_measurement_time_for_attitude(tmp_path: Path) -> None:
|
||||
path = tmp_path / "rtk.csv"
|
||||
path.write_text(
|
||||
"t,t_measurement_utc_s,hpr_measurement_utc_s,lat_deg,lon_deg,altitude_m,"
|
||||
"fix_quality,heading_deg,pitch_deg,roll_deg,heading_quality,hdop\n"
|
||||
"10.0,1000.0,1000.05,30.0,114.0,20.0,4,12.0,1.0,0.0,4,0.6\n"
|
||||
"10.1,1000.1,1000.10,30.0,114.0,20.0,4,13.0,1.0,0.0,4,0.6\n",
|
||||
encoding="utf-8",
|
||||
)
|
||||
loaded = load_rtk_csv(path)
|
||||
assert np.allclose(loaded.t_s, [10.0, 10.1])
|
||||
assert np.allclose(loaded.attitude_t_s, [10.05, 10.1])
|
||||
assert np.all(loaded.attitude_valid)
|
||||
|
||||
|
||||
def test_fast_local_preintegration_matches_reference() -> None:
|
||||
times = np.linspace(0.0, 1.0, 101)
|
||||
gyro = np.tile(np.array([0.03, -0.02, 0.15]), (times.size, 1))
|
||||
acc = np.tile(np.array([0.4, -0.2, 9.7]), (times.size, 1))
|
||||
reference_rotation = preintegrate_gyro(times, gyro, 0.13, 0.87)
|
||||
assert np.allclose(reference_rotation.delta_R, so3_exp(gyro[0] * 0.74), atol=1e-10)
|
||||
|
||||
reference = preintegrate_imu(times, gyro, acc, 0.13, 0.87)
|
||||
dp, dv, jp, jv, duration = _preintegrate_translation_interval(
|
||||
times, gyro, acc, 0.13, 0.87, np.zeros(3)
|
||||
)
|
||||
assert np.isclose(duration, 0.74)
|
||||
assert np.allclose(dp, reference.delta_p, atol=1e-10)
|
||||
assert np.allclose(dv, reference.delta_v, atol=1e-10)
|
||||
assert np.allclose(jp, reference.J_ba[6:9], atol=1e-10)
|
||||
assert np.allclose(jv, reference.J_ba[3:6], atol=1e-10)
|
||||
@@ -0,0 +1,308 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Cut G90 captures into per-window RTK CSV files.
|
||||
|
||||
NMEA GGA/GNHPR UTC is the measurement time. Host receive UTC is retained only
|
||||
for diagnostics. The measurement UTC is mapped onto the IMU device clock with
|
||||
one robust affine clock model per window.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import json
|
||||
import math
|
||||
import sys
|
||||
from datetime import datetime, timedelta, timezone
|
||||
from pathlib import Path
|
||||
|
||||
import numpy as np
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from tools.h32_dlog.timeutil import utc_dotnet_ticks_to_unix_s
|
||||
from tools.rscap_v2.capture_format_v2 import file_summary, read_capture
|
||||
from tools.rscap_v2.g90_rtk import RtkSentence, iter_g90_sentences
|
||||
from tools.time_alignment import AffineClockModel, fit_affine_clock
|
||||
|
||||
LOCAL_TZ = timezone(timedelta(hours=8))
|
||||
HPR_MATCH_S = 0.08
|
||||
CSV_FIELDS = [
|
||||
"t",
|
||||
"t_measurement_utc_s",
|
||||
"t_host_utc_s",
|
||||
"receive_delay_s",
|
||||
"t_local",
|
||||
"receive_utc_ticks",
|
||||
"lat_deg",
|
||||
"lon_deg",
|
||||
"altitude_m",
|
||||
"fix_quality",
|
||||
"satellites",
|
||||
"hdop",
|
||||
"heading_deg",
|
||||
"pitch_deg",
|
||||
"roll_deg",
|
||||
"heading_quality",
|
||||
"heading_valid",
|
||||
"gga_utc",
|
||||
"hpr_utc",
|
||||
"hpr_measurement_utc_s",
|
||||
"checksum_valid",
|
||||
]
|
||||
|
||||
|
||||
def _fmt(value) -> str:
|
||||
if value is None:
|
||||
return ""
|
||||
if isinstance(value, bool):
|
||||
return "1" if value else "0"
|
||||
if isinstance(value, float):
|
||||
if math.isnan(value):
|
||||
return ""
|
||||
return f"{value:.12g}"
|
||||
return str(value)
|
||||
|
||||
|
||||
def load_imu_clock_model(
|
||||
imu_csv: Path,
|
||||
) -> tuple[float, float, float, float, AffineClockModel]:
|
||||
"""Return device/host spans and a robust ``IMU device -> host UTC`` model."""
|
||||
|
||||
t_device: list[float] = []
|
||||
t_host: list[float] = []
|
||||
with imu_csv.open("r", encoding="utf-8", newline="") as handle:
|
||||
reader = csv.DictReader(handle)
|
||||
for row in reader:
|
||||
t_device.append(float(row["t"]))
|
||||
t_host.append(float(row["t_host_utc_s"]))
|
||||
if not t_host:
|
||||
raise ValueError(f"empty IMU csv: {imu_csv}")
|
||||
model = fit_affine_clock(np.asarray(t_device), np.asarray(t_host))
|
||||
return min(t_device), max(t_device), min(t_host), max(t_host), model
|
||||
|
||||
|
||||
def sentence_host_s(row: RtkSentence) -> float:
|
||||
return utc_dotnet_ticks_to_unix_s(row.receive_utc_ticks)
|
||||
|
||||
|
||||
def nmea_utc_to_unix_s(value: str | None, receive_host_s: float) -> float:
|
||||
"""Resolve NMEA ``hhmmss.s`` to the UTC day nearest host receive time."""
|
||||
|
||||
if value is None or not str(value).strip():
|
||||
raise ValueError("missing NMEA UTC time")
|
||||
packed = float(value)
|
||||
hour = int(packed // 10000)
|
||||
minute = int((packed - hour * 10000) // 100)
|
||||
second = packed - hour * 10000 - minute * 100
|
||||
if not (0 <= hour < 24 and 0 <= minute < 60 and 0.0 <= second < 60.0):
|
||||
raise ValueError(f"invalid NMEA UTC time: {value!r}")
|
||||
receive = datetime.fromtimestamp(receive_host_s, tz=timezone.utc)
|
||||
midnight = datetime(
|
||||
receive.year,
|
||||
receive.month,
|
||||
receive.day,
|
||||
tzinfo=timezone.utc,
|
||||
).timestamp()
|
||||
same_day = midnight + hour * 3600 + minute * 60 + second
|
||||
return min(
|
||||
(same_day - 86400.0, same_day, same_day + 86400.0),
|
||||
key=lambda candidate: abs(candidate - receive_host_s),
|
||||
)
|
||||
|
||||
|
||||
def sentence_measurement_utc_s(row: RtkSentence) -> float:
|
||||
return nmea_utc_to_unix_s(
|
||||
row.fields.get("position_time_utc"),
|
||||
sentence_host_s(row),
|
||||
)
|
||||
|
||||
|
||||
def nearest_hpr(gga: RtkSentence, hpr_rows: list[RtkSentence]) -> RtkSentence | None:
|
||||
if not hpr_rows:
|
||||
return None
|
||||
lo, hi = 0, len(hpr_rows) - 1
|
||||
target = sentence_measurement_utc_s(gga)
|
||||
while lo < hi:
|
||||
mid = (lo + hi) // 2
|
||||
if sentence_measurement_utc_s(hpr_rows[mid]) < target:
|
||||
lo = mid + 1
|
||||
else:
|
||||
hi = mid
|
||||
best = hpr_rows[lo]
|
||||
if lo > 0 and abs(sentence_measurement_utc_s(hpr_rows[lo - 1]) - target) < abs(
|
||||
sentence_measurement_utc_s(best) - target
|
||||
):
|
||||
best = hpr_rows[lo - 1]
|
||||
if abs(sentence_measurement_utc_s(best) - target) > HPR_MATCH_S:
|
||||
return None
|
||||
return best
|
||||
|
||||
|
||||
def merged_row(
|
||||
gga: RtkSentence,
|
||||
hpr: RtkSentence | None,
|
||||
imu_to_host: AffineClockModel,
|
||||
) -> dict:
|
||||
t_host = sentence_host_s(gga)
|
||||
t_measurement = sentence_measurement_utc_s(gga)
|
||||
t_hpr = None if hpr is None else sentence_measurement_utc_s(hpr)
|
||||
local = datetime.fromtimestamp(t_measurement, tz=timezone.utc).astimezone(LOCAL_TZ)
|
||||
fields = gga.fields
|
||||
hpr_fields = hpr.fields if hpr is not None else {}
|
||||
checksum = gga.checksum_valid and (hpr is None or hpr.checksum_valid)
|
||||
return {
|
||||
"t": imu_to_host.inverse(t_measurement),
|
||||
"t_measurement_utc_s": t_measurement,
|
||||
"t_host_utc_s": t_host,
|
||||
"receive_delay_s": t_host - t_measurement,
|
||||
"t_local": local.strftime("%Y-%m-%dT%H:%M:%S.%f")[:-3],
|
||||
"receive_utc_ticks": gga.receive_utc_ticks,
|
||||
"lat_deg": fields.get("lat_deg"),
|
||||
"lon_deg": fields.get("lon_deg"),
|
||||
"altitude_m": fields.get("altitude_m"),
|
||||
"fix_quality": fields.get("fix_quality"),
|
||||
"satellites": fields.get("satellites"),
|
||||
"hdop": fields.get("hdop"),
|
||||
"heading_deg": hpr_fields.get("heading_deg"),
|
||||
"pitch_deg": hpr_fields.get("pitch_deg"),
|
||||
"roll_deg": hpr_fields.get("roll_deg"),
|
||||
"heading_quality": hpr_fields.get("heading_quality"),
|
||||
"heading_valid": hpr_fields.get("heading_valid"),
|
||||
"gga_utc": fields.get("position_time_utc"),
|
||||
"hpr_utc": hpr_fields.get("position_time_utc"),
|
||||
"hpr_measurement_utc_s": t_hpr,
|
||||
"checksum_valid": checksum,
|
||||
}
|
||||
|
||||
|
||||
def write_rtk_csv(path: Path, rows: list[dict]) -> None:
|
||||
path.parent.mkdir(parents=True, exist_ok=True)
|
||||
with path.open("w", newline="", encoding="utf-8") as handle:
|
||||
writer = csv.DictWriter(handle, fieldnames=CSV_FIELDS)
|
||||
writer.writeheader()
|
||||
for row in rows:
|
||||
writer.writerow({key: _fmt(row[key]) for key in CSV_FIELDS})
|
||||
|
||||
|
||||
def discover_windows(sessions_root: Path) -> list[Path]:
|
||||
windows = sorted(
|
||||
path
|
||||
for path in sessions_root.iterdir()
|
||||
if path.is_dir() and (path / "imu.csv").is_file()
|
||||
)
|
||||
if not windows:
|
||||
raise SystemExit(f"no session dirs with imu.csv under {sessions_root}")
|
||||
return windows
|
||||
|
||||
|
||||
def find_default_rscap(sessions_root: Path) -> Path:
|
||||
parents = [sessions_root, sessions_root.parent]
|
||||
matches: list[Path] = []
|
||||
for folder in parents:
|
||||
matches.extend(sorted(folder.glob("wheeltec-g90*.rscap")))
|
||||
matches.extend(sorted(folder.glob("*g90*.rscap")))
|
||||
if not matches:
|
||||
raise SystemExit(f"no G90 .rscap next to {sessions_root}")
|
||||
return matches[0]
|
||||
|
||||
|
||||
def _delay_summary(rows: list[dict]) -> dict[str, float] | None:
|
||||
if not rows:
|
||||
return None
|
||||
values = np.asarray([row["receive_delay_s"] for row in rows], dtype=np.float64)
|
||||
return {
|
||||
"median_s": float(np.median(values)),
|
||||
"p05_s": float(np.percentile(values, 5.0)),
|
||||
"p95_s": float(np.percentile(values, 95.0)),
|
||||
"std_s": float(np.std(values)),
|
||||
}
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument("--sessions-root", type=Path, required=True)
|
||||
parser.add_argument("--rtk-rscap", type=Path, action="append")
|
||||
parser.add_argument("--overwrite", action="store_true")
|
||||
args = parser.parse_args(argv)
|
||||
sessions_root = args.sessions_root.resolve()
|
||||
rscaps = [path.resolve() for path in (args.rtk_rscap or [find_default_rscap(sessions_root)])]
|
||||
for rscap in rscaps:
|
||||
if not rscap.is_file():
|
||||
raise SystemExit(f"missing RTK capture: {rscap}")
|
||||
|
||||
sentences: list[RtkSentence] = []
|
||||
captures_meta = []
|
||||
for rscap in rscaps:
|
||||
print(f"reading {rscap}", flush=True)
|
||||
capture = read_capture(rscap)
|
||||
print(f"chunks={len(capture.chunks)}", flush=True)
|
||||
sentences.extend(iter_g90_sentences(capture))
|
||||
captures_meta.append(file_summary(capture))
|
||||
sentences.sort(key=lambda row: row.receive_utc_ticks)
|
||||
gga_all = [row for row in sentences if row.sentence_type == "GGA"]
|
||||
hpr_all = [row for row in sentences if row.sentence_type == "GNHPR"]
|
||||
hpr_all.sort(key=sentence_measurement_utc_s)
|
||||
print(
|
||||
f"parsed sentences={len(sentences)} GGA={len(gga_all)} GNHPR={len(hpr_all)}",
|
||||
flush=True,
|
||||
)
|
||||
|
||||
summaries = {
|
||||
"rtk_rscap": [str(path) for path in rscaps],
|
||||
"captures": captures_meta,
|
||||
"parsed_sentences": len(sentences),
|
||||
"gga": len(gga_all),
|
||||
"gnhpr": len(hpr_all),
|
||||
"time_source": "NMEA measurement UTC mapped through IMU device->host affine clock",
|
||||
"windows": [],
|
||||
}
|
||||
for window in discover_windows(sessions_root):
|
||||
out_csv = window / "rtk.csv"
|
||||
if out_csv.exists() and not args.overwrite:
|
||||
raise SystemExit(f"{out_csv} exists; pass --overwrite")
|
||||
dev_min, dev_max, host_min, host_max, imu_to_host = load_imu_clock_model(
|
||||
window / "imu.csv"
|
||||
)
|
||||
gga = [
|
||||
row
|
||||
for row in gga_all
|
||||
if dev_min <= imu_to_host.inverse(sentence_measurement_utc_s(row)) <= dev_max
|
||||
]
|
||||
hpr = [
|
||||
row
|
||||
for row in hpr_all
|
||||
if dev_min - HPR_MATCH_S
|
||||
<= imu_to_host.inverse(sentence_measurement_utc_s(row))
|
||||
<= dev_max + HPR_MATCH_S
|
||||
]
|
||||
merged = [merged_row(row, nearest_hpr(row, hpr), imu_to_host) for row in gga]
|
||||
write_rtk_csv(out_csv, merged)
|
||||
brief = {
|
||||
"window": window.name,
|
||||
"imu_host_span_s": [host_min, host_max],
|
||||
"imu_device_span_s": [dev_min, dev_max],
|
||||
"imu_device_to_host_clock": imu_to_host.to_dict(),
|
||||
"rtk_receive_delay": _delay_summary(merged),
|
||||
"gga": len(gga),
|
||||
"gnhpr_in_window": len(hpr),
|
||||
"rows_written": len(merged),
|
||||
"heading_matched": sum(1 for row in merged if row["heading_deg"] is not None),
|
||||
"fix_quality_4_or_5": sum(
|
||||
1 for row in merged if row["fix_quality"] in {4, 5}
|
||||
),
|
||||
"out": str(out_csv),
|
||||
}
|
||||
summaries["windows"].append(brief)
|
||||
print(json.dumps(brief, ensure_ascii=False), flush=True)
|
||||
|
||||
manifest = sessions_root / "rtk_export_summary.json"
|
||||
manifest.write_text(json.dumps(summaries, ensure_ascii=False, indent=2) + "\n", encoding="utf-8")
|
||||
print(f"summary: {manifest}")
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,145 @@
|
||||
"""Decode Wheeltec G90 NMEA (GGA / GNHPR) from a V2 .rscap capture."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import bisect
|
||||
import math
|
||||
from dataclasses import dataclass
|
||||
|
||||
from .capture_format_v2 import CaptureFile, RawChunk, iter_contiguous_segments
|
||||
|
||||
|
||||
def nmea_checksum_valid(line: str) -> bool:
|
||||
star = line.rfind("*")
|
||||
if star < 0:
|
||||
return False
|
||||
try:
|
||||
expected = int(line[star + 1 : star + 3], 16)
|
||||
except ValueError:
|
||||
return False
|
||||
value = 0
|
||||
for char in line[1:star]:
|
||||
value ^= ord(char)
|
||||
return value == expected
|
||||
|
||||
|
||||
def _safe_float(value: str):
|
||||
try:
|
||||
return float(value)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
|
||||
|
||||
def _safe_int(value: str):
|
||||
try:
|
||||
return int(value)
|
||||
except (TypeError, ValueError):
|
||||
return None
|
||||
|
||||
|
||||
def parse_nmea_latlon(value: str, hemisphere: str):
|
||||
raw = _safe_float(value)
|
||||
if raw is None:
|
||||
return None
|
||||
degrees = math.floor(raw / 100.0)
|
||||
result = degrees + (raw - degrees * 100.0) / 60.0
|
||||
if hemisphere.upper() in ("S", "W"):
|
||||
result = -result
|
||||
return result
|
||||
|
||||
|
||||
def parse_gga(line: str) -> dict:
|
||||
fields = line[: line.rfind("*")].split(",")
|
||||
if len(fields) < 10:
|
||||
raise ValueError("GGA has too few fields")
|
||||
return {
|
||||
"type": "GGA",
|
||||
"position_time_utc": fields[1],
|
||||
"lat_deg": parse_nmea_latlon(fields[2], fields[3]),
|
||||
"lon_deg": parse_nmea_latlon(fields[4], fields[5]),
|
||||
"fix_quality": _safe_int(fields[6]),
|
||||
"satellites": _safe_int(fields[7]),
|
||||
"hdop": _safe_float(fields[8]),
|
||||
"altitude_m": _safe_float(fields[9]),
|
||||
}
|
||||
|
||||
|
||||
def parse_gnhpr(line: str) -> dict:
|
||||
fields = line[: line.rfind("*")].split(",")
|
||||
if len(fields) < 7:
|
||||
raise ValueError("GNHPR has too few fields")
|
||||
quality = _safe_int(fields[5])
|
||||
return {
|
||||
"type": "GNHPR",
|
||||
"position_time_utc": fields[1],
|
||||
"heading_deg": _safe_float(fields[2]),
|
||||
"pitch_deg": _safe_float(fields[3]),
|
||||
"roll_deg": _safe_float(fields[4]),
|
||||
"heading_quality": quality,
|
||||
"satellites": _safe_int(fields[6]),
|
||||
"heading_valid": quality in {4, 5},
|
||||
}
|
||||
|
||||
|
||||
def _chunk_starts(chunks: list[RawChunk]) -> list[int]:
|
||||
starts = []
|
||||
cursor = 0
|
||||
for chunk in chunks:
|
||||
starts.append(cursor)
|
||||
cursor += len(chunk.raw)
|
||||
return starts
|
||||
|
||||
|
||||
def _host_ticks_for_span(chunks: list[RawChunk], starts: list[int], end: int) -> int:
|
||||
end_index = max(0, min(len(chunks) - 1, bisect.bisect_left(starts, end) - 1))
|
||||
return chunks[end_index].receive_utc_ticks
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class RtkSentence:
|
||||
sentence_type: str
|
||||
receive_utc_ticks: int
|
||||
checksum_valid: bool
|
||||
fields: dict
|
||||
raw_line: str
|
||||
|
||||
|
||||
def iter_g90_sentences(capture: CaptureFile) -> list[RtkSentence]:
|
||||
"""Parse GGA/GNHPR lines; host time comes from the containing serial chunk."""
|
||||
|
||||
rows: list[RtkSentence] = []
|
||||
for _segment_id, chunks in iter_contiguous_segments(capture.chunks):
|
||||
stream = b"".join(chunk.raw for chunk in chunks)
|
||||
starts = _chunk_starts(chunks)
|
||||
cursor = 0
|
||||
while cursor < len(stream):
|
||||
newline = stream.find(b"\n", cursor)
|
||||
if newline < 0:
|
||||
break
|
||||
end = newline + 1
|
||||
raw_line = stream[cursor:end].rstrip(b"\r\n")
|
||||
cursor = end
|
||||
if not raw_line:
|
||||
continue
|
||||
line = raw_line.decode("ascii", "replace")
|
||||
if not (line.startswith("$GNGGA") or line.startswith("$GPGGA") or line.startswith("$GNHPR")):
|
||||
continue
|
||||
ticks = _host_ticks_for_span(chunks, starts, end)
|
||||
try:
|
||||
if line.startswith("$GNGGA") or line.startswith("$GPGGA"):
|
||||
fields = parse_gga(line)
|
||||
else:
|
||||
fields = parse_gnhpr(line)
|
||||
except ValueError:
|
||||
continue
|
||||
rows.append(
|
||||
RtkSentence(
|
||||
sentence_type=str(fields["type"]),
|
||||
receive_utc_ticks=int(ticks),
|
||||
checksum_valid=nmea_checksum_valid(line),
|
||||
fields=fields,
|
||||
raw_line=line,
|
||||
)
|
||||
)
|
||||
rows.sort(key=lambda row: row.receive_utc_ticks)
|
||||
return rows
|
||||
@@ -0,0 +1,93 @@
|
||||
#!/usr/bin/env python3
|
||||
"""Run the independent RTK--IMU calibration against the project inventory."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import sys
|
||||
from pathlib import Path
|
||||
|
||||
ROOT = Path(__file__).resolve().parents[1]
|
||||
if str(ROOT) not in sys.path:
|
||||
sys.path.insert(0, str(ROOT))
|
||||
|
||||
from imu_lidar.rtk_imu_replay import load_inventory, load_sessions, run_calibration
|
||||
|
||||
|
||||
def main(argv: list[str] | None = None) -> int:
|
||||
parser = argparse.ArgumentParser(description=__doc__)
|
||||
parser.add_argument(
|
||||
"--inventory",
|
||||
type=Path,
|
||||
default=ROOT / "artifacts" / "rtk_imu_inventory_v1" / "rtk_session_inventory.csv",
|
||||
)
|
||||
parser.add_argument(
|
||||
"--output-dir",
|
||||
type=Path,
|
||||
default=ROOT / "artifacts" / "rtk_imu_calibration_v1",
|
||||
)
|
||||
parser.add_argument("--session", action="append", help="session id to include; repeatable")
|
||||
parser.add_argument("--batch", action="append", help="batch id to include; repeatable")
|
||||
parser.add_argument("--rotation-only", action="store_true")
|
||||
parser.add_argument("--no-loo", action="store_true")
|
||||
parser.add_argument("--knot-step-s", type=float, default=2.0)
|
||||
parser.add_argument("--per-batch", action="store_true")
|
||||
parser.add_argument("--rtk-frame-definition", default='')
|
||||
parser.add_argument("--rtk-reference-point", default='')
|
||||
args = parser.parse_args(argv)
|
||||
|
||||
entries = load_inventory(args.inventory)
|
||||
if args.session:
|
||||
selected = set(args.session)
|
||||
entries = [entry for entry in entries if entry.session_id in selected]
|
||||
if args.batch:
|
||||
selected_batches = set(args.batch)
|
||||
entries = [entry for entry in entries if entry.batch_id in selected_batches]
|
||||
if not entries:
|
||||
raise SystemExit("no inventory rows match the requested selection")
|
||||
sessions = load_sessions(entries)
|
||||
rotation, translation = run_calibration(
|
||||
sessions,
|
||||
args.output_dir,
|
||||
rotation_only=args.rotation_only,
|
||||
compute_loo=not args.no_loo,
|
||||
knot_step_s=args.knot_step_s,
|
||||
rtk_frame_definition=args.rtk_frame_definition,
|
||||
rtk_reference_point=args.rtk_reference_point,
|
||||
)
|
||||
print(
|
||||
json.dumps(
|
||||
{
|
||||
"output": str(args.output_dir.resolve()),
|
||||
"sessions": [session.session_id for session in sessions],
|
||||
"rotation_ok": rotation.ok,
|
||||
"rotation_rpy_deg": rotation.rpy_deg.tolist(),
|
||||
"rotation_rms_deg": rotation.residual_rms_deg,
|
||||
"translation_ok": None if translation is None else translation.ok,
|
||||
"translation_m": None if translation is None else translation.t_RTK_IMU_m.tolist(),
|
||||
},
|
||||
ensure_ascii=False,
|
||||
indent=2,
|
||||
)
|
||||
)
|
||||
|
||||
if args.per_batch:
|
||||
for batch in sorted({entry.batch_id for entry in entries}):
|
||||
batch_entries = [entry for entry in entries if entry.batch_id == batch]
|
||||
if len(batch_entries) < 2:
|
||||
continue
|
||||
run_calibration(
|
||||
load_sessions(batch_entries),
|
||||
args.output_dir / "per_batch" / batch,
|
||||
rotation_only=args.rotation_only,
|
||||
compute_loo=False,
|
||||
knot_step_s=args.knot_step_s,
|
||||
rtk_frame_definition=args.rtk_frame_definition,
|
||||
rtk_reference_point=args.rtk_reference_point,
|
||||
)
|
||||
return 0
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
raise SystemExit(main())
|
||||
@@ -0,0 +1,97 @@
|
||||
"""Small, dependency-light helpers for mapping independent sensor clocks."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from dataclasses import asdict, dataclass
|
||||
|
||||
import numpy as np
|
||||
|
||||
|
||||
@dataclass(frozen=True)
|
||||
class AffineClockModel:
|
||||
"""Numerically stable affine map ``y = y_ref + scale * (x - x_ref)``."""
|
||||
|
||||
x_ref: float
|
||||
y_ref: float
|
||||
scale: float
|
||||
sample_count: int
|
||||
inlier_count: int
|
||||
residual_std_s: float
|
||||
residual_p95_s: float
|
||||
|
||||
def map(self, value: float | np.ndarray) -> float | np.ndarray:
|
||||
array = np.asarray(value, dtype=np.float64)
|
||||
mapped = self.y_ref + self.scale * (array - self.x_ref)
|
||||
return float(mapped) if array.ndim == 0 else mapped
|
||||
|
||||
def inverse(self, value: float | np.ndarray) -> float | np.ndarray:
|
||||
if abs(self.scale) < 1e-12:
|
||||
raise ValueError("clock model scale is zero")
|
||||
array = np.asarray(value, dtype=np.float64)
|
||||
mapped = self.x_ref + (array - self.y_ref) / self.scale
|
||||
return float(mapped) if array.ndim == 0 else mapped
|
||||
|
||||
def to_dict(self) -> dict[str, float | int]:
|
||||
return asdict(self)
|
||||
|
||||
|
||||
def fit_affine_clock(
|
||||
x: np.ndarray,
|
||||
y: np.ndarray,
|
||||
*,
|
||||
max_iterations: int = 4,
|
||||
min_residual_gate_s: float = 5e-4,
|
||||
) -> AffineClockModel:
|
||||
"""Robustly fit an affine clock map while rejecting receive-time spikes.
|
||||
|
||||
``x`` and ``y`` may have large, unrelated epochs. Centering around their
|
||||
medians avoids losing precision when host UTC is around 1e9 seconds.
|
||||
"""
|
||||
|
||||
x_values = np.asarray(x, dtype=np.float64).reshape(-1)
|
||||
y_values = np.asarray(y, dtype=np.float64).reshape(-1)
|
||||
finite = np.isfinite(x_values) & np.isfinite(y_values)
|
||||
x_values = x_values[finite]
|
||||
y_values = y_values[finite]
|
||||
if x_values.size < 2:
|
||||
raise ValueError("need at least two finite clock samples")
|
||||
|
||||
x_ref = float(np.median(x_values))
|
||||
y_ref = float(np.median(y_values))
|
||||
dx = x_values - x_ref
|
||||
dy = y_values - y_ref
|
||||
inliers = np.ones(x_values.size, dtype=bool)
|
||||
scale = 1.0
|
||||
offset = 0.0
|
||||
|
||||
for _ in range(max_iterations):
|
||||
local_x = dx[inliers]
|
||||
local_y = dy[inliers]
|
||||
denom = float(local_x @ local_x)
|
||||
if denom < 1e-18:
|
||||
raise ValueError("clock samples do not span enough time")
|
||||
scale = float(local_x @ local_y / denom)
|
||||
offset = float(np.median(local_y - scale * local_x))
|
||||
residual = dy - (offset + scale * dx)
|
||||
center = float(np.median(residual[inliers]))
|
||||
mad = float(np.median(np.abs(residual[inliers] - center)))
|
||||
sigma = 1.4826 * mad
|
||||
gate = max(float(min_residual_gate_s), 6.0 * sigma)
|
||||
updated = np.abs(residual - center) <= gate
|
||||
if np.count_nonzero(updated) < 2 or np.array_equal(updated, inliers):
|
||||
break
|
||||
inliers = updated
|
||||
|
||||
# Fold the small centered intercept into y_ref so map/inverse stay simple.
|
||||
y_ref += offset
|
||||
residual = y_values - (y_ref + scale * (x_values - x_ref))
|
||||
residual_inliers = residual[inliers]
|
||||
return AffineClockModel(
|
||||
x_ref=x_ref,
|
||||
y_ref=y_ref,
|
||||
scale=scale,
|
||||
sample_count=int(x_values.size),
|
||||
inlier_count=int(np.count_nonzero(inliers)),
|
||||
residual_std_s=float(np.std(residual_inliers)),
|
||||
residual_p95_s=float(np.percentile(np.abs(residual_inliers), 95.0)),
|
||||
)
|
||||
Reference in New Issue
Block a user