feat: 2D Umeyama XY + pression Z + frustums camera vers le bas

fuse/fuse_trajectory.py

@@ -39,6 +39,34 @@ def umeyama(src: np.ndarray, dst: np.ndarray,
     return scale, R, t
 
 
+def umeyama_2d(src: np.ndarray, dst: np.ndarray,
+               weights: np.ndarray | None = None) -> tuple[float, np.ndarray, np.ndarray]:
+    """
+    2D Umeyama: XY alignment only.
+    src, dst : (N, 2) float64
+    Returns scale, R (2x2), t (2,)
+    """
+    N = len(src)
+    if N < 2:
+        raise ValueError(f"umeyama_2d requires at least 2 point pairs, got {N}")
+    if weights is None:
+        weights = np.ones(N, dtype=np.float64)
+    w = weights / weights.sum()
+    mu_s = (w[:, None] * src).sum(0)
+    mu_d = (w[:, None] * dst).sum(0)
+    src_c, dst_c = src - mu_s, dst - mu_d
+    cov = (dst_c * w[:, None]).T @ src_c
+    U, D, Vt = np.linalg.svd(cov)
+    S = np.eye(2)
+    if np.linalg.det(U) * np.linalg.det(Vt) < 0:
+        S[1, 1] = -1
+    R2 = U @ S @ Vt
+    var_s = (w * np.sum(src_c ** 2, axis=1)).sum()
+    scale = float(np.sum(D * np.diag(S)) / var_s) if var_s > 0 else 1.0
+    t2 = mu_d - scale * R2 @ mu_s
+    return scale, R2, t2
+
+
 def fuse(fixes_h5: str, poses_npz: str, out_h5: str,
          outlier_sigma: float = 2.0) -> None:
 
@@ -53,6 +81,7 @@ def fuse(fixes_h5: str, poses_npz: str, out_h5: str,
         auv_lon  = f["auv_mcap/lon"][:]
         auv_dep  = f["auv_mcap/depth_m"][:]
         auv_t    = f["auv_mcap/t_ns"][:]
+        auv_alt_m = f["auv_mcap/altitude_m"][:] if "auv_mcap/altitude_m" in f else None
 
         usbl_n = f["usbl_fixes/north_m"][:] if "usbl_fixes" in f else np.array([])
         usbl_e = f["usbl_fixes/east_m"][:]  if "usbl_fixes" in f else np.array([])
@@ -124,46 +153,99 @@ def fuse(fixes_h5: str, poses_npz: str, out_h5: str,
     dst = np.array(dst_pts)
     w   = np.array(weights)
 
-    # 7. Umeyama with outlier rejection
+    # 7. Choose alignment strategy: 2D when altitude data available, else 3D
-    scale, R, t = umeyama(src, dst, w)
+    has_altitude = (auv_alt_m is not None and len(auv_alt_m) > 0
-    residuals = np.linalg.norm((scale * (R @ src.T).T + t) - dst, axis=1)
+                    and not np.all(auv_alt_m == 0))
-    sigma = residuals.std()
-    mask = residuals < outlier_sigma * sigma if sigma > 0 else np.ones(len(src), dtype=bool)
-    print(f"Correspondences: {len(src)} total, {mask.sum()} after outlier rejection (σ={sigma:.3f}m)")
-    scale, R, t = umeyama(src[mask], dst[mask], w[mask])
 
-    # 8. Transform all lingbot poses to world frame
+    if has_altitude:
-    N = len(pose_t)
+        # 2D Umeyama (XY only) + pressure depth for Z
-    poses_world = np.zeros((N, 4, 4))
+        src_xy = src[:, :2]
-    poses_world[:, 3, 3] = 1.0
+        dst_xy = dst[:, :2]
-    for i in range(N):
+        scale_xy, R2, t2 = umeyama_2d(src_xy, dst_xy, w)
-        R_local = poses_34[i, :3, :3]
-        t_local = poses_34[i, :3, 3]
-        poses_world[i, :3, :3] = R @ R_local
-        poses_world[i, :3, 3]  = scale * R @ t_local + t
 
-    xyz_world = poses_world[:, :3, 3]
+        residuals_2d = np.linalg.norm(
+            (scale_xy * (R2 @ src_xy.T).T + t2) - dst_xy, axis=1)
+        sigma = residuals_2d.std()
+        mask = (residuals_2d < outlier_sigma * sigma) if sigma > 0 \
+               else np.ones(len(src_xy), dtype=bool)
+        print(f"2D Umeyama: {len(src_xy)} total, {mask.sum()} after outlier rejection "
+              f"(sigma_xy={sigma:.3f}m, scale_xy={scale_xy:.4f})")
+        scale_xy, R2, t2 = umeyama_2d(src_xy[mask], dst_xy[mask], w[mask])
+
+        # Expand R2 to 3x3 (identity for Z)
+        R3 = np.eye(3)
+        R3[:2, :2] = R2
+        t3 = np.array([t2[0], t2[1], 0.0])
+
+        # Transform lingbot poses; override Z from pressure sensor at each timestamp
+        N_poses = len(pose_t)
+        poses_world = np.zeros((N_poses, 4, 4))
+        poses_world[:, 3, 3] = 1.0
+
+        for i in range(N_poses):
+            R_local = poses_34[i, :3, :3]
+            t_local = poses_34[i, :3, 3]
+            poses_world[i, :3, :3] = R3 @ R_local
+            t_xy = scale_xy * R2 @ t_local[:2] + t2
+            # Z from pressure depth (interpolated at lingbot timestamp)
+            z_world = float(np.interp(
+                float(pose_t[i]), auv_t.astype(float), auv_dep.astype(float)))
+            poses_world[i, :3, 3] = [t_xy[0], t_xy[1], z_world]
+
+        xyz_world = poses_world[:, :3, 3]
+        rmse_m = float(residuals_2d[mask].mean())
+        scale_out = scale_xy
+        R_out = R3
+        t_out = t3
+        print(f"2D fusion OK: scale_xy={scale_xy:.4f} RMSE_xy={rmse_m:.3f}m -> {out_h5}")
+
+    else:
+        # 3D Umeyama fallback (original logic)
+        scale, R, t = umeyama(src, dst, w)
+        residuals = np.linalg.norm((scale * (R @ src.T).T + t) - dst, axis=1)
+        sigma = residuals.std()
+        mask = (residuals < outlier_sigma * sigma) if sigma > 0 \
+               else np.ones(len(src), dtype=bool)
+        print(f"3D Umeyama: {len(src)} total, {mask.sum()} after outlier rejection "
+              f"(sigma={sigma:.3f}m)")
+        scale, R, t = umeyama(src[mask], dst[mask], w[mask])
+
+        N_poses = len(pose_t)
+        poses_world = np.zeros((N_poses, 4, 4))
+        poses_world[:, 3, 3] = 1.0
+        for i in range(N_poses):
+            R_local = poses_34[i, :3, :3]
+            t_local = poses_34[i, :3, 3]
+            poses_world[i, :3, :3] = R @ R_local
+            poses_world[i, :3, 3]  = scale * R @ t_local + t
+
+        xyz_world = poses_world[:, :3, 3]
+        residuals_final = residuals
+        rmse_m = float(residuals[mask].mean())
+        scale_out = scale
+        R_out = R
+        t_out = t
+        print(f"3D fusion OK: scale={scale:.4f} RMSE={rmse_m:.3f}m -> {out_h5}")
 
     # 9. Write trajectory_world.h5
     with h5py.File(out_h5, "w") as f:
-        f.attrs["status"]   = "aligned"
+        f.attrs["status"]        = "aligned"
-        f.attrs["utm_zone"] = utm_zone
+        f.attrs["utm_zone"]      = utm_zone
+        f.attrs["fusion_mode"]   = "2d" if has_altitude else "3d"
 
         al = f.create_group("alignment")
-        al.attrs["scale"]             = scale
+        al.attrs["scale"]             = scale_out
-        al.attrs["rmse_m"]            = float(residuals[mask].mean())
+        al.attrs["rmse_m"]            = rmse_m
         al.attrs["n_correspondences"] = int(mask.sum())
-        al.create_dataset("R", data=R)
+        al.create_dataset("R", data=R_out)
-        al.create_dataset("t", data=t)
+        al.create_dataset("t", data=t_out)
 
         pw = f.create_group("poses_world")
-        pw.create_dataset("t_ns",  data=pose_t,         compression="gzip")
+        pw.create_dataset("t_ns",  data=pose_t,          compression="gzip")
         pw.create_dataset("x_m",   data=xyz_world[:, 0], compression="gzip")
         pw.create_dataset("y_m",   data=xyz_world[:, 1], compression="gzip")
         pw.create_dataset("z_m",   data=xyz_world[:, 2], compression="gzip")
-        pw.create_dataset("T_4x4", data=poses_world,    compression="gzip")
+        pw.create_dataset("T_4x4", data=poses_world,     compression="gzip")
-
-    print(f"Fusion OK: scale={scale:.4f} RMSE={residuals[mask].mean():.3f}m → {out_h5}")
 
 
 def _save_local_only(out_h5: str, pose_t, poses_34, ling_xyz, utm_zone):

viz/static/js/scene.js

@@ -56,11 +56,18 @@ function makePoints(xArr, yArr, zArr, colorFn) {
 }
 
 function makeFrustum(T_flat) {
-  // Wireframe camera pyramid: apex at origin, 1m×0.75m rect at z=1.5m
+  // Wireframe camera pyramid pointing downward (-Y): apex at origin, base 1m×0.75m at y=-1.5m
   const verts = new Float32Array([
-    0,0,0,  0.5,0.375,1.5,   0.5,0.375,1.5, -0.5,0.375,1.5,
+    // 4 edges from apex to base corners
-   -0.5,0.375,1.5,-0.5,-0.375,1.5,  -0.5,-0.375,1.5,0.5,-0.375,1.5,
+    0,0,0,   0.5,-1.5, 0.375,
-    0.5,-0.375,1.5,0,0,0,   0,0,0,0.5,0.375,1.5,
+    0,0,0,  -0.5,-1.5, 0.375,
+    0,0,0,   0.5,-1.5,-0.375,
+    0,0,0,  -0.5,-1.5,-0.375,
+    // 4 edges of base rectangle
+     0.5,-1.5, 0.375,  -0.5,-1.5, 0.375,
+    -0.5,-1.5, 0.375,  -0.5,-1.5,-0.375,
+    -0.5,-1.5,-0.375,   0.5,-1.5,-0.375,
+     0.5,-1.5,-0.375,   0.5,-1.5, 0.375,
   ]);
   const geo = new THREE.BufferGeometry();
   geo.setAttribute('position', new THREE.BufferAttribute(verts, 3));