68 lines
2.6 KiB
Python
68 lines
2.6 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 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)
|