floppyrj45/cosma-nav
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: fuse_trajectory — Umeyama weighted alignment lingbot→world + graceful fallbacks

Browse Source 
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
Floppyrj45
2026-04-24 02:00:32 +02:00
parent ae107160c1
commit 1251efdd98
3 changed files with 252 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

28
tests/test_fuse_trajectory.py Normal file
View File

@@ -0,0 +1,28 @@
import tempfile, os
import numpy as np
import h5py
def test_fuse_creates_output_without_lingbot():
"""When lingbot_poses.npz doesn't exist, fuse creates HDF5 with sources only."""
from fuse.fuse_trajectory import fuse
with tempfile.TemporaryDirectory() as tmpdir:
out_h5 = os.path.join(tmpdir, "traj.h5")
# Create minimal sparse_fixes.h5
fixes_h5 = os.path.join(tmpdir, "fixes.h5")
with h5py.File(fixes_h5, "w") as f:
grp = f.create_group("usv_gps")
grp.create_dataset("t_ns", data=np.array([1000, 2000], dtype=np.int64))
grp.create_dataset("easting", data=np.array([100.0, 101.0]))
grp.create_dataset("northing",data=np.array([200.0, 201.0]))
grp.create_dataset("rtk_status", data=np.array([0, 0], dtype=np.int8))
grp.attrs["utm_zone"] = "31T"
grp2 = f.create_group("auv_mcap")
grp2.create_dataset("t_ns", data=np.array([1000, 2000], dtype=np.int64))
grp2.create_dataset("lat", data=np.array([0.0, 0.0]))
grp2.create_dataset("lon", data=np.array([0.0, 0.0]))
grp2.create_dataset("depth_m",data=np.array([5.0, 6.0]))
fuse(fixes_h5, "/nonexistent/lingbot.npz", out_h5)
assert os.path.exists(out_h5)
with h5py.File(out_h5, "r") as f:
assert "status" in f.attrs