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

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

Browse Source
This commit is contained in:
Floppyrj45
2026-04-24 11:59:35 +02:00
parent 7330b230b8
commit 4c01a9182a
2 changed files with 120 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

136
fuse/fuse_trajectory.py
View File

@@ -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
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"Correspondences: {len(src)} total, {mask.sum()} after outlier rejection (σ={sigma:.3f}m)")
scale, R, t = umeyama(src[mask], dst[mask], w[mask])
# 7. Choose alignment strategy: 2D when altitude data available, else 3D
has_altitude = (auv_alt_m is not None and len(auv_alt_m) > 0
and not np.all(auv_alt_m == 0))
# 8. Transform all lingbot poses to world frame
N = len(pose_t)
poses_world = np.zeros((N, 4, 4))
poses_world[:, 3, 3] = 1.0
for i in range(N):
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
if has_altitude:
# 2D Umeyama (XY only) + pressure depth for Z
src_xy = src[:, :2]
dst_xy = dst[:, :2]
scale_xy, R2, t2 = umeyama_2d(src_xy, dst_xy, w)
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["utm_zone"] = utm_zone
f.attrs["status"] = "aligned"
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["rmse_m"] = float(residuals[mask].mean())
al.attrs["scale"] = scale_out
al.attrs["rmse_m"] = rmse_m
al.attrs["n_correspondences"] = int(mask.sum())
al.create_dataset("R", data=R)
al.create_dataset("t", data=t)
al.create_dataset("R", data=R_out)
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")
print(f"Fusion OK: scale={scale:.4f} RMSE={residuals[mask].mean():.3f}m → {out_h5}")
pw.create_dataset("T_4x4", data=poses_world, compression="gzip")
def _save_local_only(out_h5: str, pose_t, poses_34, ling_xyz, utm_zone):