99 lines
3.7 KiB
Python
99 lines
3.7 KiB
Python
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 rtk_imu.rtk_attitude import gnhpr_to_baseline_enu, gnhpr_to_rotation_enu_rtk
|
|
from rtk_imu.rtk_imu_translation import _preintegrate_translation_interval
|
|
from rtk_imu.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_gnhpr_baseline_is_main_to_secondary_and_ignores_roll() -> None:
|
|
baseline = gnhpr_to_baseline_enu(
|
|
np.array([0.0, 90.0]),
|
|
np.array([30.0, 0.0]),
|
|
)
|
|
assert np.allclose(baseline[0], [0.0, np.sqrt(0.75), 0.5], atol=1e-12)
|
|
assert np.allclose(baseline[1], [1.0, 0.0, 0.0], atol=1e-12)
|
|
|
|
rotations = gnhpr_to_rotation_enu_rtk(
|
|
np.array([90.0, 90.0]),
|
|
np.zeros(2),
|
|
np.array([0.0, 45.0]),
|
|
)
|
|
assert np.allclose(rotations[0], rotations[1], 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_rtk_loader_accepts_only_checksum_valid_fixed_hpr(tmp_path: Path) -> None:
|
|
path = tmp_path / "rtk.csv"
|
|
path.write_text(
|
|
"t,lat_deg,lon_deg,altitude_m,fix_quality,heading_deg,pitch_deg,roll_deg,"
|
|
"heading_quality,checksum_valid\n"
|
|
"0.0,30.0,114.0,20.0,4,10.0,0.0,0.0,4,1\n"
|
|
"0.1,30.0,114.0,20.0,4,11.0,0.0,0.0,5,1\n"
|
|
"0.2,30.0,114.0,20.0,4,12.0,0.0,0.0,4,0\n",
|
|
encoding="utf-8",
|
|
)
|
|
loaded = load_rtk_csv(path)
|
|
assert loaded.attitude_valid.tolist() == [True, False, False]
|
|
assert loaded.attitude_float.tolist() == [False, True, False]
|
|
|
|
|
|
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)
|