feat: USBL heading rotation + poids RTK pour fixes acoustiques

extract/parse_usv_gps.py

@@ -29,7 +29,8 @@ def parse_nav_log(source: Union[str, IO]) -> list[dict]:
             val  = row.get("value", "").strip()
             if ts not in buf:
                 buf[ts] = {"ts_str": ts, "easting": None, "northing": None,
-                           "utm_num": None, "utm_let": None, "rtk_status": 0}
+                           "utm_num": None, "utm_let": None, "rtk_status": 0,
+                           "heading_deg": None, "fix_type_raw": None}
             if data == "Easting":
                 buf[ts]["easting"] = float(val)
             elif data == "Northing":
@@ -38,17 +39,44 @@ def parse_nav_log(source: Union[str, IO]) -> list[dict]:
                 buf[ts]["utm_num"] = str(int(float(val)))
             elif data == "UTM_letter":
                 buf[ts]["utm_let"] = val
+            elif data.lower() in ("heading", "hdg", "cog", "course", "yaw",
+                                   "yaw_deg", "heading_deg", "course_deg"):
+                try:
+                    buf[ts]["heading_deg"] = float(val)
+                except ValueError:
+                    pass
+            elif data.lower() in ("fix_type", "gps_fix_type", "rtk_type",
+                                   "fix_status", "gps_status"):
+                try:
+                    buf[ts]["fix_type_raw"] = int(float(val))
+                except ValueError:
+                    pass
         rows = []
         for r in buf.values():
             if r["easting"] is None or r["northing"] is None:
                 continue
             zone = (r["utm_num"] or "") + (r["utm_let"] or "")
+            # Map ArduPilot fix_type (0-6) → our 3-level RTK status
+            # 0=NoGPS,1=NoFix,2=2D,3=3D,4=DGPS → 0 (3D/none)
+            # 5=RTK_Float → 1, 6=RTK_Fixed → 2
+            raw_fix = r.get("fix_type_raw")
+            if raw_fix is not None:
+                if raw_fix >= 6:
+                    rtk = 2
+                elif raw_fix == 5:
+                    rtk = 1
+                else:
+                    rtk = 0
+            else:
+                rtk = r["rtk_status"]  # default 0
+
             rows.append({
-                "t_ns":       _ts_to_ns(r["ts_str"]),
-                "easting":    r["easting"],
-                "northing":   r["northing"],
-                "utm_zone":   zone,
-                "rtk_status": r["rtk_status"],
+                "t_ns":        _ts_to_ns(r["ts_str"]),
+                "easting":     r["easting"],
+                "northing":    r["northing"],
+                "utm_zone":    zone,
+                "rtk_status":  rtk,
+                "heading_deg": r.get("heading_deg"),  # float or None
             })
         return sorted(rows, key=lambda r: r["t_ns"])
     finally:
@@ -61,14 +89,20 @@ def write_usv_gps_group(h5_path: str, rows: list[dict]) -> None:
     e   = np.array([r["easting"]    for r in rows], dtype=np.float64)
     n   = np.array([r["northing"]   for r in rows], dtype=np.float64)
     rtk = np.array([r["rtk_status"] for r in rows], dtype=np.int8)
+    hdg = np.array(
+        [r["heading_deg"] if r["heading_deg"] is not None else np.nan
+         for r in rows],
+        dtype=np.float64,
+    )
     with h5py.File(h5_path, "a") as f:
         if "usv_gps" in f:
             del f["usv_gps"]
         grp = f.create_group("usv_gps")
-        grp.create_dataset("t_ns",       data=t,   compression="gzip")
-        grp.create_dataset("easting",    data=e,   compression="gzip")
-        grp.create_dataset("northing",   data=n,   compression="gzip")
-        grp.create_dataset("rtk_status", data=rtk, compression="gzip")
+        grp.create_dataset("t_ns",        data=t,   compression="gzip")
+        grp.create_dataset("easting",     data=e,   compression="gzip")
+        grp.create_dataset("northing",    data=n,   compression="gzip")
+        grp.create_dataset("rtk_status",  data=rtk, compression="gzip")
+        grp.create_dataset("heading_deg", data=hdg, compression="gzip")
         grp.attrs["utm_zone"] = rows[0]["utm_zone"] if rows else ""
 
 

fuse/fuse_trajectory.py

@@ -83,10 +83,14 @@ def fuse(fixes_h5: str, poses_npz: str, out_h5: str,
         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([])
-        usbl_d = f["usbl_fixes/depth_m"][:] if "usbl_fixes" in f else np.array([])
-        usbl_t = f["usbl_fixes/t_ns"][:]    if "usbl_fixes" in f else np.array([], dtype=np.int64)
+        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([])
+        usbl_d       = f["usbl_fixes/depth_m"][:]   if "usbl_fixes" in f else np.array([])
+        usbl_t       = f["usbl_fixes/t_ns"][:]      if "usbl_fixes" in f else np.array([], dtype=np.int64)
+        usbl_bearing = f["usbl_fixes/bearing_deg"][:] if "usbl_fixes" in f else np.array([])
+        usbl_range_m = f["usbl_fixes/range_m"][:]     if "usbl_fixes" in f else np.array([])
+        usv_rtk      = f["usv_gps/rtk_status"][:]
+        usv_hdg_raw  = f["usv_gps/heading_deg"][:] if "usv_gps/heading_deg" in f else np.full(len(usv_t), np.nan)
 
     # 2. Check for lingbot poses
     poses_path = Path(poses_npz)
@@ -105,9 +109,10 @@ def fuse(fixes_h5: str, poses_npz: str, out_h5: str,
     ling_xyz = poses_34[:, :3, 3]     # camera positions in local frame
 
     # 4. Build absolute AUV reference points
-    has_usbl_fixes = (len(usbl_n) > 0 and
-                      not np.all(usbl_n == 0) and
-                      not np.all(usbl_e == 0))
+    usbl_weights = np.array([])  # populated only if USBL path used
+    has_usbl_fixes = (len(usbl_bearing) > 0
+                      and not np.all(np.isnan(usbl_bearing))
+                      and not np.all(np.isnan(usbl_range_m)))
 
     has_auv_gps = not np.all(auv_lat == 0.0)
 
@@ -128,12 +133,32 @@ def fuse(fixes_h5: str, poses_npz: str, out_h5: str,
         auv_xyz_world = np.column_stack([auv_x, auv_y, -auv_dep])
         ref_t = auv_t
     else:
-        # USBL relative offsets + USV GPS → AUV absolute
-        usv_e_i = np.interp(usbl_t.astype(float), usv_t.astype(float), usv_e)
-        usv_n_i = np.interp(usbl_t.astype(float), usv_t.astype(float), usv_n)
-        auv_xyz_world = np.column_stack([usv_e_i + usbl_e, usv_n_i + usbl_n, -usbl_d])
+        # USBL bearing+range → world AUV position using USV heading
+        usbl_t_f  = usbl_t.astype(float)
+        usv_t_f   = usv_t.astype(float)
+        usv_e_i   = np.interp(usbl_t_f, usv_t_f, usv_e)
+        usv_n_i   = np.interp(usbl_t_f, usv_t_f, usv_n)
+        usv_rtk_i = np.interp(usbl_t_f, usv_t_f, usv_rtk.astype(float))
+
+        has_heading = not np.all(np.isnan(usv_hdg_raw))
+        if has_heading:
+            usv_hdg_i = np.interp(usbl_t_f, usv_t_f, usv_hdg_raw)
+        else:
+            print("WARNING: no USV heading in HDF5 — using bearing as world-frame offset (inaccurate)")
+            usv_hdg_i = np.zeros(len(usbl_t))
+
+        # Horizontal range (remove depth component)
+        range_h = np.sqrt(np.maximum(0.0, usbl_range_m ** 2 - usbl_d ** 2))
+        angle_rad = np.deg2rad(usv_hdg_i + usbl_bearing)
+        auv_e_abs = usv_e_i + range_h * np.sin(angle_rad)
+        auv_n_abs = usv_n_i + range_h * np.cos(angle_rad)
+        auv_xyz_world = np.column_stack([auv_e_abs, auv_n_abs, -usbl_d])
         ref_t = usbl_t
 
+        # RTK quality weights for USBL correspondences
+        rtk_weight_map = np.array([0.05, 0.3, 1.0])  # index 0=3D, 1=float, 2=fix
+        usbl_weights = rtk_weight_map[np.clip(np.round(usv_rtk_i).astype(int), 0, 2)]
+
     # 6. Match lingbot timestamps → world reference points
     src_pts, dst_pts, weights = [], [], []
     for i, pt in enumerate(pose_t):
@@ -142,7 +167,11 @@ def fuse(fixes_h5: str, poses_npz: str, out_h5: str,
             continue
         src_pts.append(ling_xyz[i])
         dst_pts.append(auv_xyz_world[idx])
-        weights.append(1.0)
+        # Weight: USBL path uses RTK-based weight; GPS path uses 1.0
+        if has_usbl_fixes and not has_auv_gps:
+            weights.append(float(usbl_weights[idx]))
+        else:
+            weights.append(1.0)
 
     if len(src_pts) < 3:
         print(f"WARNING: Only {len(src_pts)} correspondences (need ≥3). Saving local only.")