238 lines
8.5 KiB
Python
238 lines
8.5 KiB
Python
# viz/server.py
|
|
import sys
|
|
import json
|
|
from pathlib import Path
|
|
import numpy as np
|
|
import h5py
|
|
from flask import Flask, jsonify, send_from_directory
|
|
|
|
app = Flask(__name__, static_folder="static", static_url_path="/nav/static")
|
|
|
|
# Configured at startup
|
|
_TRAJ_H5 = "data/trajectory_world.h5"
|
|
_FIXES_H5 = "data/sparse_fixes.h5"
|
|
_PLY_PATH = ""
|
|
|
|
def _load_data() -> dict:
|
|
out: dict = {}
|
|
|
|
# USV GPS track (always try first — most reliable)
|
|
try:
|
|
with h5py.File(_FIXES_H5, "r") as f:
|
|
if "usv_gps" in f:
|
|
e = f["usv_gps/easting"][:]
|
|
n = f["usv_gps/northing"][:]
|
|
t = f["usv_gps/t_ns"][:].tolist()
|
|
rtk = f["usv_gps/rtk_status"][:].tolist()
|
|
# Center on centroid for Three.js
|
|
ce, cn = float(e.mean()), float(n.mean())
|
|
out["origin"] = {"easting": ce, "northing": cn}
|
|
out["usv_gps"] = {
|
|
"x": (e - ce).tolist(), "y": (n - cn).tolist(),
|
|
"z": [0.0] * len(e), "t": t, "rtk": rtk,
|
|
}
|
|
else:
|
|
ce, cn = 0.0, 0.0
|
|
out["origin"] = {"easting": 0.0, "northing": 0.0}
|
|
|
|
if "auv_mcap" in f:
|
|
dep = f["auv_mcap/depth_m"][:]
|
|
t_ns_auv = f["auv_mcap/t_ns"][:]
|
|
alt = f["auv_mcap/altitude_m"][:] if "auv_mcap/altitude_m" in f \
|
|
else np.full(len(dep), np.nan)
|
|
step = max(1, len(dep) // 600)
|
|
t_s = ((t_ns_auv[::step].astype(float) - float(t_ns_auv[0])) / 1e9).tolist()
|
|
out["auv_profile"] = {
|
|
"t_s": t_s,
|
|
"depth_m": dep[::step].tolist(),
|
|
"altitude_m": alt[::step].tolist(),
|
|
}
|
|
|
|
if "usbl_fixes" in f and "usv_gps" in f:
|
|
bearing = f["usbl_fixes/bearing_deg"][:]
|
|
range_m = f["usbl_fixes/range_m"][:]
|
|
usbl_dep = f["usbl_fixes/depth_m"][:]
|
|
t_usbl = f["usbl_fixes/t_ns"][:]
|
|
usv_e_raw = f["usv_gps/easting"][:]
|
|
usv_n_raw = f["usv_gps/northing"][:]
|
|
usv_t_raw = f["usv_gps/t_ns"][:]
|
|
hdg_raw = f["usv_gps/heading_deg"][:] \
|
|
if "usv_gps/heading_deg" in f \
|
|
else np.full(len(usv_t_raw), np.nan)
|
|
|
|
valid = ~(np.isnan(bearing) | np.isnan(range_m) | np.isnan(usbl_dep))
|
|
if valid.any():
|
|
t_f = t_usbl[valid].astype(float)
|
|
t_usv = usv_t_raw.astype(float)
|
|
usv_ei = np.interp(t_f, t_usv, usv_e_raw)
|
|
usv_ni = np.interp(t_f, t_usv, usv_n_raw)
|
|
hdg_i = np.interp(t_f, t_usv, hdg_raw) \
|
|
if not np.all(np.isnan(hdg_raw)) \
|
|
else np.zeros(valid.sum())
|
|
range_h = np.sqrt(np.maximum(0.0, range_m[valid]**2 - usbl_dep[valid]**2))
|
|
angle = np.deg2rad(hdg_i + bearing[valid])
|
|
auv_e = usv_ei + range_h * np.sin(angle)
|
|
auv_n = usv_ni + range_h * np.cos(angle)
|
|
ce = out.get("origin", {}).get("easting", 0.0)
|
|
cn = out.get("origin", {}).get("northing", 0.0)
|
|
out["auv_usbl"] = {
|
|
"x": (auv_e - ce).tolist(),
|
|
"y": (auv_n - cn).tolist(),
|
|
"z": (-usbl_dep[valid]).tolist(),
|
|
}
|
|
except Exception as e:
|
|
out["error_fixes"] = str(e)
|
|
|
|
# Fused trajectory (lingbot-aligned)
|
|
try:
|
|
with h5py.File(_TRAJ_H5, "r") as f:
|
|
status = f.attrs.get("status", "unknown")
|
|
out["traj_status"] = status
|
|
if status == "aligned" and "poses_world" in f:
|
|
pw = f["poses_world"]
|
|
ce = out.get("origin", {}).get("easting", 0.0)
|
|
cn = out.get("origin", {}).get("northing", 0.0)
|
|
out["fused"] = {
|
|
"x": (pw["x_m"][:] - ce).tolist(),
|
|
"y": (pw["y_m"][:] - cn).tolist(),
|
|
"z": pw["z_m"][:].tolist(),
|
|
"t": pw["t_ns"][:].tolist(),
|
|
"T_4x4": pw["T_4x4"][:].tolist(),
|
|
}
|
|
elif "poses_world" in f:
|
|
# local only
|
|
pw = f["poses_world"]
|
|
out["lingbot_local"] = {
|
|
"x": pw["x_m"][:].tolist(),
|
|
"y": pw["y_m"][:].tolist(),
|
|
"z": pw["z_m"][:].tolist(),
|
|
"t": pw["t_ns"][:].tolist(),
|
|
"T_4x4": pw["T_4x4"][:].tolist() if "T_4x4" in pw else [],
|
|
}
|
|
except Exception as e:
|
|
out["error_traj"] = str(e)
|
|
|
|
# Decimated PLY
|
|
if _PLY_PATH and Path(_PLY_PATH).exists():
|
|
try:
|
|
import open3d as o3d
|
|
pcd = o3d.io.read_point_cloud(_PLY_PATH)
|
|
n_pts = len(pcd.points)
|
|
MAX_PTS = 300_000
|
|
if n_pts > MAX_PTS:
|
|
vol = float(np.prod(pcd.get_max_bound() - pcd.get_min_bound()))
|
|
vox = max((vol / MAX_PTS) ** (1/3), 0.02)
|
|
pcd = pcd.voxel_down_sample(vox)
|
|
pts = np.asarray(pcd.points)
|
|
ce = out.get("origin", {}).get("easting", 0.0)
|
|
cn = out.get("origin", {}).get("northing", 0.0)
|
|
out["ply"] = {
|
|
"x": (pts[:,0] - ce).tolist(),
|
|
"y": (pts[:,1] - cn).tolist(),
|
|
"z": pts[:,2].tolist(),
|
|
}
|
|
except Exception as e:
|
|
out["error_ply"] = str(e)
|
|
|
|
return out
|
|
|
|
@app.route("/api/trajectory")
|
|
def api_trajectory():
|
|
return jsonify(_load_data())
|
|
|
|
@app.route("/trajectory")
|
|
def viewer():
|
|
return send_from_directory("static", "trajectory.html")
|
|
|
|
@app.route("/")
|
|
def root():
|
|
return '<meta http-equiv="refresh" content="0;url=/trajectory">'
|
|
|
|
def _main():
|
|
global _TRAJ_H5, _FIXES_H5, _PLY_PATH
|
|
import argparse
|
|
p = argparse.ArgumentParser()
|
|
p.add_argument("--trajectory", default="data/trajectory_world.h5")
|
|
p.add_argument("--fixes", default="data/sparse_fixes.h5")
|
|
p.add_argument("--ply", default="")
|
|
p.add_argument("--port", type=int, default=5051)
|
|
args = p.parse_args()
|
|
_TRAJ_H5 = args.trajectory
|
|
_FIXES_H5 = args.fixes
|
|
_PLY_PATH = args.ply
|
|
app.run(host="0.0.0.0", port=args.port, debug=False)
|
|
|
|
# ── COSMA QC Platform additions ──────────────────────────
|
|
import os
|
|
from pathlib import Path
|
|
|
|
_DATA_DIR = Path(os.environ.get("COSMA_DATA_DIR", "/data/cosma"))
|
|
|
|
|
|
def _load_nav_data(job_id: int) -> dict:
|
|
out = {"job_id": job_id}
|
|
poses_path = _DATA_DIR / f"job_{job_id}_poses.npz"
|
|
ply_path = _DATA_DIR / f"job_{job_id}_decimated.ply"
|
|
if poses_path.exists():
|
|
d = np.load(str(poses_path))
|
|
poses = d["poses"] # (N, 3, 4)
|
|
xyz = poses[:, :3, 3]
|
|
out["track"] = {
|
|
"x": xyz[:, 0].tolist(),
|
|
"y": xyz[:, 1].tolist(),
|
|
"z": xyz[:, 2].tolist(),
|
|
}
|
|
out["n_poses"] = int(len(poses))
|
|
out["ply_ready"] = ply_path.exists()
|
|
out["ply_path"] = str(ply_path) if ply_path.exists() else None
|
|
return out
|
|
|
|
|
|
@app.route("/map")
|
|
def map_view():
|
|
from flask import render_template
|
|
return render_template("map.html")
|
|
|
|
|
|
@app.route("/nav")
|
|
def nav_view():
|
|
from flask import render_template
|
|
return render_template("nav.html")
|
|
|
|
|
|
@app.route("/api/jobs")
|
|
def api_jobs():
|
|
jobs = []
|
|
for p in sorted(_DATA_DIR.glob("job_*_decimated.ply")):
|
|
parts = p.stem.split("_")
|
|
jid = int(parts[1])
|
|
jobs.append({
|
|
"id": jid,
|
|
"ply": str(p),
|
|
"poses": str(_DATA_DIR / f"job_{jid}_poses.npz"),
|
|
})
|
|
return jsonify(jobs)
|
|
|
|
|
|
@app.route("/api/job/<int:job_id>/nav")
|
|
def api_job_nav(job_id: int):
|
|
return jsonify(_load_nav_data(job_id))
|
|
|
|
|
|
@app.route("/api/job/<int:job_id>/ply")
|
|
def api_job_ply(job_id: int):
|
|
ply_path = _DATA_DIR / f"job_{job_id}_decimated.ply"
|
|
if not ply_path.exists():
|
|
return jsonify({"error": "PLY non disponible"}), 404
|
|
try:
|
|
import open3d as o3d
|
|
pcd = o3d.io.read_point_cloud(str(ply_path))
|
|
pts = np.asarray(pcd.points)
|
|
return jsonify({"x": pts[:, 0].tolist(), "y": pts[:, 1].tolist(),
|
|
"z": pts[:, 2].tolist(), "n": int(len(pts))})
|
|
except ImportError:
|
|
return jsonify({"error": "open3d not installed"}), 503
|
|
|
|
if __name__ == "__main__":
|
|
_main()
|