floppyrj45/cosma-nav
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: USBL heading rotation + poids RTK pour fixes acoustiques

Browse Source
This commit is contained in:
Floppyrj45
2026-04-24 13:30:13 +02:00
parent 1d84258c48
commit 67b323eea8
2 changed files with 85 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files

54
extract/parse_usv_gps.py
View File

@@ -29,7 +29,8 @@ def parse_nav_log(source: Union[str, IO]) -> list[dict]:
val = row.get("value", "").strip() val = row.get("value", "").strip()
if ts not in buf: if ts not in buf:
buf[ts] = {"ts_str": ts, "easting": None, "northing": None, 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": if data == "Easting":
buf[ts]["easting"] = float(val) buf[ts]["easting"] = float(val)
elif data == "Northing": 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))) buf[ts]["utm_num"] = str(int(float(val)))
elif data == "UTM_letter": elif data == "UTM_letter":
buf[ts]["utm_let"] = val 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 = [] rows = []
for r in buf.values(): for r in buf.values():
if r["easting"] is None or r["northing"] is None: if r["easting"] is None or r["northing"] is None:
continue continue
zone = (r["utm_num"] or "") + (r["utm_let"] or "") 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({ rows.append({
"t_ns": _ts_to_ns(r["ts_str"]), "t_ns": _ts_to_ns(r["ts_str"]),
"easting": r["easting"], "easting": r["easting"],
"northing": r["northing"], "northing": r["northing"],
"utm_zone": zone, "utm_zone": zone,
"rtk_status": r["rtk_status"], "rtk_status": rtk,
"heading_deg": r.get("heading_deg"), # float or None
}) })
return sorted(rows, key=lambda r: r["t_ns"]) return sorted(rows, key=lambda r: r["t_ns"])
finally: 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) e = np.array([r["easting"] for r in rows], dtype=np.float64)
n = np.array([r["northing"] 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) 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: with h5py.File(h5_path, "a") as f:
if "usv_gps" in f: if "usv_gps" in f:
del f["usv_gps"] del f["usv_gps"]
grp = f.create_group("usv_gps") grp = f.create_group("usv_gps")
grp.create_dataset("t_ns", data=t, compression="gzip") grp.create_dataset("t_ns", data=t, compression="gzip")
grp.create_dataset("easting", data=e, compression="gzip") grp.create_dataset("easting", data=e, compression="gzip")
grp.create_dataset("northing", data=n, compression="gzip") grp.create_dataset("northing", data=n, compression="gzip")
grp.create_dataset("rtk_status", data=rtk, 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 "" grp.attrs["utm_zone"] = rows[0]["utm_zone"] if rows else ""

53
fuse/fuse_trajectory.py
View File

@@ -83,10 +83,14 @@ def fuse(fixes_h5: str, poses_npz: str, out_h5: str,
auv_t = f["auv_mcap/t_ns"][:] auv_t = f["auv_mcap/t_ns"][:]
auv_alt_m = f["auv_mcap/altitude_m"][:] if "auv_mcap/altitude_m" in f else None 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_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_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_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_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 # 2. Check for lingbot poses
poses_path = Path(poses_npz) 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 ling_xyz = poses_34[:, :3, 3] # camera positions in local frame
# 4. Build absolute AUV reference points # 4. Build absolute AUV reference points
has_usbl_fixes = (len(usbl_n) > 0 and usbl_weights = np.array([]) # populated only if USBL path used
not np.all(usbl_n == 0) and has_usbl_fixes = (len(usbl_bearing) > 0
not np.all(usbl_e == 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) 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]) auv_xyz_world = np.column_stack([auv_x, auv_y, -auv_dep])
ref_t = auv_t ref_t = auv_t
else: else:
# USBL relative offsets + USV GPS → AUV absolute # USBL bearing+range → world AUV position using USV heading
usv_e_i = np.interp(usbl_t.astype(float), usv_t.astype(float), usv_e) usbl_t_f = usbl_t.astype(float)
usv_n_i = np.interp(usbl_t.astype(float), usv_t.astype(float), usv_n) usv_t_f = usv_t.astype(float)
auv_xyz_world = np.column_stack([usv_e_i + usbl_e, usv_n_i + usbl_n, -usbl_d]) 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 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 # 6. Match lingbot timestamps → world reference points
src_pts, dst_pts, weights = [], [], [] src_pts, dst_pts, weights = [], [], []
for i, pt in enumerate(pose_t): for i, pt in enumerate(pose_t):
@@ -142,7 +167,11 @@ def fuse(fixes_h5: str, poses_npz: str, out_h5: str,
continue continue
src_pts.append(ling_xyz[i]) src_pts.append(ling_xyz[i])
dst_pts.append(auv_xyz_world[idx]) 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: if len(src_pts) < 3:
print(f"WARNING: Only {len(src_pts)} correspondences (need ≥3). Saving local only.") print(f"WARNING: Only {len(src_pts)} correspondences (need ≥3). Saving local only.")