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

Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>

tests/test_fuse_trajectory.py

@@ -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

tests/test_umeyama.py

@@ -0,0 +1,41 @@
+import numpy as np
+import pytest
+
+def test_umeyama_identity():
+    from fuse.fuse_trajectory import umeyama
+    src = np.random.default_rng(0).standard_normal((10, 3))
+    scale, R, t = umeyama(src, src)
+    assert abs(scale - 1.0) < 1e-5
+    assert np.allclose(R, np.eye(3), atol=1e-5)
+    assert np.allclose(t, np.zeros(3), atol=1e-5)
+
+def test_umeyama_known_transform():
+    from fuse.fuse_trajectory import umeyama
+    rng = np.random.default_rng(42)
+    src = rng.standard_normal((20, 3))
+    true_scale = 2.5
+    true_R = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]], dtype=float)
+    true_t = np.array([1.0, 2.0, 3.0])
+    dst = true_scale * (src @ true_R.T) + true_t
+    scale, R, t = umeyama(src, dst)
+    assert abs(scale - true_scale) < 1e-4
+    assert np.allclose(R, true_R, atol=1e-4)
+    assert np.allclose(t, true_t, atol=1e-4)
+
+def test_umeyama_weighted():
+    from fuse.fuse_trajectory import umeyama
+    rng = np.random.default_rng(0)
+    src = rng.standard_normal((15, 3))
+    true_scale, true_t = 1.5, np.array([0.5, -0.5, 1.0])
+    dst = true_scale * src + true_t
+    weights = np.ones(15)
+    weights[0] = 0.0  # outlier with zero weight
+    scale, R, t = umeyama(src, dst, weights=weights)
+    assert abs(scale - true_scale) < 1e-3
+    assert np.allclose(t, true_t, atol=1e-3)
+
+def test_umeyama_raises_on_few_points():
+    from fuse.fuse_trajectory import umeyama
+    src = np.random.default_rng(0).standard_normal((2, 3))
+    with pytest.raises(ValueError, match="at least 3"):
+        umeyama(src, src)