diff --git a/fuse/fuse_trajectory.py b/fuse/fuse_trajectory.py index ba1834c..839297d 100644 --- a/fuse/fuse_trajectory.py +++ b/fuse/fuse_trajectory.py @@ -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): diff --git a/viz/static/js/scene.js b/viz/static/js/scene.js index 1019cec..69bbe5f 100644 --- a/viz/static/js/scene.js +++ b/viz/static/js/scene.js @@ -56,11 +56,18 @@ function makePoints(xArr, yArr, zArr, colorFn) { } function makeFrustum(T_flat) { - // Wireframe camera pyramid: apex at origin, 1m×0.75m rect at z=1.5m + // Wireframe camera pyramid pointing downward (-Y): apex at origin, base 1m×0.75m at y=-1.5m const verts = new Float32Array([ - 0,0,0, 0.5,0.375,1.5, 0.5,0.375,1.5, -0.5,0.375,1.5, - -0.5,0.375,1.5,-0.5,-0.375,1.5, -0.5,-0.375,1.5,0.5,-0.375,1.5, - 0.5,-0.375,1.5,0,0,0, 0,0,0,0.5,0.375,1.5, + // 4 edges from apex to base corners + 0,0,0, 0.5,-1.5, 0.375, + 0,0,0, -0.5,-1.5, 0.375, + 0,0,0, 0.5,-1.5,-0.375, + 0,0,0, -0.5,-1.5,-0.375, + // 4 edges of base rectangle + 0.5,-1.5, 0.375, -0.5,-1.5, 0.375, + -0.5,-1.5, 0.375, -0.5,-1.5,-0.375, + -0.5,-1.5,-0.375, 0.5,-1.5,-0.375, + 0.5,-1.5,-0.375, 0.5,-1.5, 0.375, ]); const geo = new THREE.BufferGeometry(); geo.setAttribute('position', new THREE.BufferAttribute(verts, 3));