diff --git a/viz/server.py b/viz/server.py new file mode 100644 index 0000000..2560a31 --- /dev/null +++ b/viz/server.py @@ -0,0 +1,142 @@ +# 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") + +# 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_auv = f["auv_mcap/t_ns"][:].tolist() + # AUV lat/lon zeros — plot depth over time as Z only + n_pts = len(dep) + out["auv_depth"] = { + "x": list(range(n_pts)), # index as proxy for time + "y": [0.0] * n_pts, + "z": (-dep).tolist(), # positive = deeper + "t": t_auv, + } + + if "usbl_fixes" in f: + north = f["usbl_fixes/north_m"][:] + east = f["usbl_fixes/east_m"][:] + depth = f["usbl_fixes/depth_m"][:] + valid = ~(np.isnan(north) | (north == 0) & (east == 0)) + if valid.any(): + out["usbl"] = { + "x": east[valid].tolist(), + "y": north[valid].tolist(), + "z": (-depth[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) + if n_pts > 200_000: + vox = float((pcd.get_max_bound() - pcd.get_min_bound()).max()) * (200_000 / n_pts) ** (1/3) + pcd = pcd.voxel_down_sample(float(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 '' + +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) + +if __name__ == "__main__": + _main() diff --git a/viz/static/js/scene.js b/viz/static/js/scene.js new file mode 100644 index 0000000..1019cec --- /dev/null +++ b/viz/static/js/scene.js @@ -0,0 +1,170 @@ +import * as THREE from 'three'; +import { OrbitControls } from 'three/addons/controls/OrbitControls.js'; + +// ── Renderer ────────────────────────────────────────────────────────────────── +const renderer = new THREE.WebGLRenderer({ antialias: true }); +renderer.setPixelRatio(Math.min(devicePixelRatio, 2)); +renderer.setSize(innerWidth, innerHeight); +document.body.appendChild(renderer.domElement); + +// ── Scene ───────────────────────────────────────────────────────────────────── +const scene = new THREE.Scene(); +scene.background = new THREE.Color(0x06060f); +scene.fog = new THREE.Fog(0x06060f, 500, 2000); +scene.add(new THREE.GridHelper(1000, 100, 0x111133, 0x0a0a22)); +scene.add(new THREE.AxesHelper(5)); + +// ── Camera + Controls ───────────────────────────────────────────────────────── +const camera = new THREE.PerspectiveCamera(60, innerWidth / innerHeight, 0.1, 5000); +camera.position.set(0, 150, 250); +const controls = new OrbitControls(camera, renderer.domElement); +controls.enableDamping = true; +controls.dampingFactor = 0.08; +controls.minDistance = 1; +controls.maxDistance = 3000; + +// ── Layer registry ──────────────────────────────────────────────────────────── +const layers = {}; + +function addLayer(name, obj) { + layers[name] = layers[name] || []; + layers[name].push(obj); + scene.add(obj); +} + +// ── Geometry helpers ────────────────────────────────────────────────────────── +function makeLine(xArr, yArr, zArr, color) { + const pts = xArr.map((x, i) => new THREE.Vector3(x, zArr[i], -yArr[i])); + const geo = new THREE.BufferGeometry().setFromPoints(pts); + return new THREE.Line(geo, new THREE.LineBasicMaterial({ color })); +} + +function makePoints(xArr, yArr, zArr, colorFn) { + const pos = new Float32Array(xArr.length * 3); + const col = new Float32Array(xArr.length * 3); + for (let i = 0; i < xArr.length; i++) { + pos[i*3] = xArr[i]; + pos[i*3+1] = zArr[i]; + pos[i*3+2] = -yArr[i]; + const [r,g,b] = colorFn(i); + col[i*3]=r; col[i*3+1]=g; col[i*3+2]=b; + } + const geo = new THREE.BufferGeometry(); + geo.setAttribute('position', new THREE.BufferAttribute(pos, 3)); + geo.setAttribute('color', new THREE.BufferAttribute(col, 3)); + return new THREE.Points(geo, new THREE.PointsMaterial({ size: 4, vertexColors: true, sizeAttenuation: true })); +} + +function makeFrustum(T_flat) { + // Wireframe camera pyramid: apex at origin, 1m×0.75m rect at z=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, + ]); + const geo = new THREE.BufferGeometry(); + geo.setAttribute('position', new THREE.BufferAttribute(verts, 3)); + const mesh = new THREE.LineSegments(geo, + new THREE.LineBasicMaterial({ color: 0xffd700, transparent: true, opacity: 0.5 })); + const m = new THREE.Matrix4(); + // T_flat is row-major 4x4 from Python (need to transpose for Three.js column-major) + m.set( + T_flat[0][0],T_flat[0][1],T_flat[0][2],T_flat[0][3], + T_flat[1][0],T_flat[1][1],T_flat[1][2],T_flat[1][3], + T_flat[2][0],T_flat[2][1],T_flat[2][2],T_flat[2][3], + T_flat[3][0],T_flat[3][1],T_flat[3][2],T_flat[3][3], + ); + mesh.applyMatrix4(m); + return mesh; +} + +// ── RTK color mapping ───────────────────────────────────────────────────────── +const RTK_COLORS = [[0.8,0.3,0.1],[0.9,0.7,0.1],[0.1,0.9,0.2]]; + +// ── Load data ───────────────────────────────────────────────────────────────── +fetch('/api/trajectory') + .then(r => r.json()) + .then(d => { + document.getElementById('status').textContent = + `traj: ${d.traj_status || 'n/a'}`; + + if (d.usv_gps) { + const { x, y, z, rtk } = d.usv_gps; + addLayer('usv_gps', makeLine(x, y, z, 0x3a9fff)); + addLayer('usv_gps', makePoints(x, y, z, i => RTK_COLORS[Math.min(rtk[i], 2)])); + } + + if (d.auv_depth) { + const { x, y, z } = d.auv_depth; + addLayer('auv_depth', makeLine(x, y, z, 0x44ffaa)); + } + + if (d.usbl) { + const { x, y, z } = d.usbl; + addLayer('usbl', makePoints(x, y, z, () => [1, 0.42, 0.21])); + } + + const trajData = d.fused || d.lingbot_local; + const trajLayer = d.fused ? 'fused' : 'lingbot_local'; + if (trajData) { + const { x, y, z, T_4x4 } = trajData; + addLayer(trajLayer, makeLine(x, y, z, d.fused ? 0xffffff : 0xffd700)); + + if (T_4x4 && T_4x4.length > 0) { + const step = Math.max(1, Math.floor(T_4x4.length / 100)); // max 100 frustums + for (let i = 0; i < T_4x4.length; i += step) { + addLayer('frustums', makeFrustum(T_4x4[i])); + } + } + + // Center camera + if (x.length > 0) { + controls.target.set(x[0], z[0], -y[0]); + camera.position.set(x[0], z[0] + 50, -y[0] + 100); + controls.update(); + } + } + + if (d.ply) { + const { x, y, z } = d.ply; + addLayer('ply', makePoints(x, y, z, (i) => { + const t = Math.min(1, Math.max(0, (-z[i]) / 20)); + return [0.2+0.3*t, 0.35+0.2*t, 0.5-0.2*t]; + })); + } + + // If only USV GPS, center on that + if (!trajData && d.usv_gps) { + const { x, y, z } = d.usv_gps; + const cx = x.reduce((a,b)=>a+b,0)/x.length; + const cy = y.reduce((a,b)=>a+b,0)/y.length; + controls.target.set(cx, 0, -cy); + camera.position.set(cx, 50, -cy + 80); + controls.update(); + } + }) + .catch(err => { + document.getElementById('status').textContent = `Error: ${err}`; + }); + +// ── Layer toggles ───────────────────────────────────────────────────────────── +document.querySelectorAll('[data-layer]').forEach(cb => { + cb.addEventListener('change', e => { + const name = e.target.dataset.layer; + (layers[name] || []).forEach(obj => { obj.visible = e.target.checked; }); + }); +}); + +// ── Resize ──────────────────────────────────────────────────────────────────── +window.addEventListener('resize', () => { + camera.aspect = innerWidth / innerHeight; + camera.updateProjectionMatrix(); + renderer.setSize(innerWidth, innerHeight); +}); + +// ── Render loop ─────────────────────────────────────────────────────────────── +(function animate() { + requestAnimationFrame(animate); + controls.update(); + renderer.render(scene, camera); +})(); diff --git a/viz/static/trajectory.html b/viz/static/trajectory.html new file mode 100644 index 0000000..6a2ed75 --- /dev/null +++ b/viz/static/trajectory.html @@ -0,0 +1,62 @@ + + + + + COSMA NAV — Trajectory Viewer + + + +
+

COSMA NAV

+ + + + + + + +
+
Loading…
+
Orbit: drag · Zoom: scroll · Pan: right-drag
+ + + + +