feat: stitch.py --poses trajectory_world.h5 — T_init depuis poses monde, remplace RANSAC

tests/test_stitch_poses.py

@@ -0,0 +1,43 @@
+import sys, os, tempfile
+import numpy as np
+import h5py
+import pytest
+
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), ".."))
+
+def _make_test_h5(path, n_poses=100):
+    T = np.tile(np.eye(4), (n_poses, 1, 1)).astype(np.float64)
+    for i in range(n_poses):
+        T[i, 0, 3] = float(i) * 0.1
+    with h5py.File(path, "w") as f:
+        pw = f.create_group("poses_world")
+        pw.create_dataset("T_4x4", data=T)
+        pw.create_dataset("t_ns", data=np.arange(n_poses, dtype=np.int64) * int(1e8))
+        f.attrs["status"] = "aligned"
+
+def test_load_world_poses_returns_n_transforms():
+    from scripts.stitch import _load_world_poses
+    with tempfile.NamedTemporaryFile(suffix=".h5", delete=False) as tmp:
+        path = tmp.name
+    try:
+        _make_test_h5(path, n_poses=100)
+        transforms = _load_world_poses(path, 4)
+        assert len(transforms) == 4
+        assert np.allclose(transforms[0], np.eye(4), atol=1e-6), "T_0 must be identity"
+        assert not np.allclose(transforms[1], np.eye(4)), "T_1 must differ from identity"
+        for T in transforms:
+            assert T.shape == (4, 4)
+    finally:
+        os.unlink(path)
+
+def test_load_world_poses_single_ply():
+    from scripts.stitch import _load_world_poses
+    with tempfile.NamedTemporaryFile(suffix=".h5", delete=False) as tmp:
+        path = tmp.name
+    try:
+        _make_test_h5(path, n_poses=10)
+        transforms = _load_world_poses(path, 1)
+        assert len(transforms) == 1
+        assert np.allclose(transforms[0], np.eye(4), atol=1e-6)
+    finally:
+        os.unlink(path)