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

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_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.")