diff --git a/artifacts/rtk_imu_calibration_v1/README.md b/artifacts/rtk_imu_calibration_v1/README.md new file mode 100644 index 0000000..f18542a --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/README.md @@ -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`。 diff --git a/artifacts/rtk_imu_calibration_v1/all_sessions/dataset_audit.json b/artifacts/rtk_imu_calibration_v1/all_sessions/dataset_audit.json new file mode 100644 index 0000000..ec72aad --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/all_sessions/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" + } + ] +} diff --git a/artifacts/rtk_imu_calibration_v1/all_sessions/rotation_result.json b/artifacts/rtk_imu_calibration_v1/all_sessions/rotation_result.json new file mode 100644 index 0000000..4a2e789 --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/all_sessions/rotation_result.json @@ -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" + ] +} diff --git a/artifacts/rtk_imu_calibration_v1/all_sessions/summary.json b/artifacts/rtk_imu_calibration_v1/all_sessions/summary.json new file mode 100644 index 0000000..7260f92 --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/all_sessions/summary.json @@ -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" +} diff --git a/artifacts/rtk_imu_calibration_v1/all_sessions/translation_result.json b/artifacts/rtk_imu_calibration_v1/all_sessions/translation_result.json new file mode 100644 index 0000000..3dd2205 --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/all_sessions/translation_result.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" + ] +} diff --git a/artifacts/rtk_imu_calibration_v1/batch_0808_full_smoke/dataset_audit.json b/artifacts/rtk_imu_calibration_v1/batch_0808_full_smoke/dataset_audit.json new file mode 100644 index 0000000..a78431f --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/batch_0808_full_smoke/dataset_audit.json @@ -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" + } + ] +} diff --git a/artifacts/rtk_imu_calibration_v1/batch_0808_full_smoke/rotation_result.json b/artifacts/rtk_imu_calibration_v1/batch_0808_full_smoke/rotation_result.json new file mode 100644 index 0000000..3c01f95 --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/batch_0808_full_smoke/rotation_result.json @@ -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" + ] +} diff --git a/artifacts/rtk_imu_calibration_v1/batch_0808_full_smoke/summary.json b/artifacts/rtk_imu_calibration_v1/batch_0808_full_smoke/summary.json new file mode 100644 index 0000000..158497c --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/batch_0808_full_smoke/summary.json @@ -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" +} diff --git a/artifacts/rtk_imu_calibration_v1/batch_0808_full_smoke/translation_result.json b/artifacts/rtk_imu_calibration_v1/batch_0808_full_smoke/translation_result.json new file mode 100644 index 0000000..1d42a52 --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/batch_0808_full_smoke/translation_result.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" + ] +} diff --git a/artifacts/rtk_imu_calibration_v1/batch_0815_rotation/dataset_audit.json b/artifacts/rtk_imu_calibration_v1/batch_0815_rotation/dataset_audit.json new file mode 100644 index 0000000..5460b27 --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/batch_0815_rotation/dataset_audit.json @@ -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" + } + ] +} diff --git a/artifacts/rtk_imu_calibration_v1/batch_0815_rotation/rotation_result.json b/artifacts/rtk_imu_calibration_v1/batch_0815_rotation/rotation_result.json new file mode 100644 index 0000000..db134cc --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/batch_0815_rotation/rotation_result.json @@ -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" + ] +} diff --git a/artifacts/rtk_imu_calibration_v1/batch_0815_rotation/summary.json b/artifacts/rtk_imu_calibration_v1/batch_0815_rotation/summary.json new file mode 100644 index 0000000..d341d55 --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/batch_0815_rotation/summary.json @@ -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" +} diff --git a/artifacts/rtk_imu_calibration_v1/rotation_hpr_time/dataset_audit.json b/artifacts/rtk_imu_calibration_v1/rotation_hpr_time/dataset_audit.json new file mode 100644 index 0000000..ec72aad --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/rotation_hpr_time/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" + } + ] +} diff --git a/artifacts/rtk_imu_calibration_v1/rotation_hpr_time/rotation_result.json b/artifacts/rtk_imu_calibration_v1/rotation_hpr_time/rotation_result.json new file mode 100644 index 0000000..f89d5f3 --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/rotation_hpr_time/rotation_result.json @@ -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" + ] +} diff --git a/artifacts/rtk_imu_calibration_v1/rotation_hpr_time/summary.json b/artifacts/rtk_imu_calibration_v1/rotation_hpr_time/summary.json new file mode 100644 index 0000000..61ce968 --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/rotation_hpr_time/summary.json @@ -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" +} diff --git a/artifacts/rtk_imu_calibration_v1/rotation_smoke/dataset_audit.json b/artifacts/rtk_imu_calibration_v1/rotation_smoke/dataset_audit.json new file mode 100644 index 0000000..ec72aad --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/rotation_smoke/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" + } + ] +} diff --git a/artifacts/rtk_imu_calibration_v1/rotation_smoke/rotation_result.json b/artifacts/rtk_imu_calibration_v1/rotation_smoke/rotation_result.json new file mode 100644 index 0000000..7dd6e56 --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/rotation_smoke/rotation_result.json @@ -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" + ] +} diff --git a/artifacts/rtk_imu_calibration_v1/rotation_smoke/summary.json b/artifacts/rtk_imu_calibration_v1/rotation_smoke/summary.json new file mode 100644 index 0000000..6b0d554 --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/rotation_smoke/summary.json @@ -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" +} diff --git a/artifacts/rtk_imu_calibration_v1/single_smoke/dataset_audit.json b/artifacts/rtk_imu_calibration_v1/single_smoke/dataset_audit.json new file mode 100644 index 0000000..05ba386 --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/single_smoke/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" + } + ] +} diff --git a/artifacts/rtk_imu_calibration_v1/single_smoke/rotation_result.json b/artifacts/rtk_imu_calibration_v1/single_smoke/rotation_result.json new file mode 100644 index 0000000..604aeef --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/single_smoke/rotation_result.json @@ -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" + ] +} diff --git a/artifacts/rtk_imu_calibration_v1/single_smoke/summary.json b/artifacts/rtk_imu_calibration_v1/single_smoke/summary.json new file mode 100644 index 0000000..d7669ea --- /dev/null +++ b/artifacts/rtk_imu_calibration_v1/single_smoke/summary.json @@ -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" +} diff --git a/artifacts/rtk_imu_inventory_v1/README.md b/artifacts/rtk_imu_inventory_v1/README.md new file mode 100644 index 0000000..bc56873 --- /dev/null +++ b/artifacts/rtk_imu_inventory_v1/README.md @@ -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，外参共享，并检查安装期间是否发生机械变动。 + diff --git a/artifacts/rtk_imu_inventory_v1/rtk_session_inventory.csv b/artifacts/rtk_imu_inventory_v1/rtk_session_inventory.csv new file mode 100644 index 0000000..b2aac56 --- /dev/null +++ b/artifacts/rtk_imu_inventory_v1/rtk_session_inventory.csv @@ -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 diff --git a/docs/rtk_imu_calibration.md b/docs/rtk_imu_calibration.md new file mode 100644 index 0000000..4ef0f68 --- /dev/null +++ b/docs/rtk_imu_calibration.md @@ -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/留出验证。 diff --git a/imu_lidar/geodesy.py b/imu_lidar/geodesy.py new file mode 100644 index 0000000..bd3d2e4 --- /dev/null +++ b/imu_lidar/geodesy.py @@ -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) diff --git a/imu_lidar/imu_io.py b/imu_lidar/imu_io.py index cd8b7d1..2cc5b46 100644 --- a/imu_lidar/imu_io.py +++ b/imu_lidar/imu_io.py @@ -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]) diff --git a/imu_lidar/imu_preintegration.py b/imu_lidar/imu_preintegration.py index d421722..419bed6 100644 --- a/imu_lidar/imu_preintegration.py +++ b/imu_lidar/imu_preintegration.py @@ -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))) diff --git a/imu_lidar/rtk_attitude.py b/imu_lidar/rtk_attitude.py new file mode 100644 index 0000000..ce299ef --- /dev/null +++ b/imu_lidar/rtk_attitude.py @@ -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() diff --git a/imu_lidar/rtk_imu_replay.py b/imu_lidar/rtk_imu_replay.py new file mode 100644 index 0000000..b08b254 --- /dev/null +++ b/imu_lidar/rtk_imu_replay.py @@ -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 diff --git a/imu_lidar/rtk_imu_rotation.py b/imu_lidar/rtk_imu_rotation.py new file mode 100644 index 0000000..76e1b96 --- /dev/null +++ b/imu_lidar/rtk_imu_rotation.py @@ -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), + ) diff --git a/imu_lidar/rtk_imu_translation.py b/imu_lidar/rtk_imu_translation.py new file mode 100644 index 0000000..2172ea1 --- /dev/null +++ b/imu_lidar/rtk_imu_translation.py @@ -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), + ) diff --git a/imu_lidar/rtk_io.py b/imu_lidar/rtk_io.py new file mode 100644 index 0000000..49341df --- /dev/null +++ b/imu_lidar/rtk_io.py @@ -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]) diff --git a/tests/test_device_time_alignment.py b/tests/test_device_time_alignment.py new file mode 100644 index 0000000..83070d8 --- /dev/null +++ b/tests/test_device_time_alignment.py @@ -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 diff --git a/tests/test_rtk_imu_calibration.py b/tests/test_rtk_imu_calibration.py new file mode 100644 index 0000000..71fbd41 --- /dev/null +++ b/tests/test_rtk_imu_calibration.py @@ -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) diff --git a/tools/export_g90_rtk_to_sessions.py b/tools/export_g90_rtk_to_sessions.py new file mode 100644 index 0000000..5989e69 --- /dev/null +++ b/tools/export_g90_rtk_to_sessions.py @@ -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()) diff --git a/tools/rscap_v2/g90_rtk.py b/tools/rscap_v2/g90_rtk.py new file mode 100644 index 0000000..115a2d6 --- /dev/null +++ b/tools/rscap_v2/g90_rtk.py @@ -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 diff --git a/tools/run_rtk_imu_calibration.py b/tools/run_rtk_imu_calibration.py new file mode 100644 index 0000000..d382013 --- /dev/null +++ b/tools/run_rtk_imu_calibration.py @@ -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()) diff --git a/tools/time_alignment.py b/tools/time_alignment.py new file mode 100644 index 0000000..20cb0b7 --- /dev/null +++ b/tools/time_alignment.py @@ -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)), + )