diff --git a/extract/parse_usv_gps.py b/extract/parse_usv_gps.py index 1feabb5..2e62a6d 100644 --- a/extract/parse_usv_gps.py +++ b/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 "" diff --git a/fuse/fuse_trajectory.py b/fuse/fuse_trajectory.py index 2222c3c..3fb5022 100644 --- a/fuse/fuse_trajectory.py +++ b/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.")