feat: Three.js trajectory viewer — Flask :5051 + trajectoires + frustums + PLY décimé
Co-Authored-By: Claude Sonnet 4.6 <noreply@anthropic.com>
This commit is contained in:
Floppyrj45
142
viz/server.py
Normal file
142
viz/server.py
Normal file
@@ -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 '<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)
|
||||
|
||||
if __name__ == "__main__":
|
||||
_main()
|
||||
170
viz/static/js/scene.js
Normal file
170
viz/static/js/scene.js
Normal file
@@ -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);
|
||||
})();
|
||||
62
viz/static/trajectory.html
Normal file
62
viz/static/trajectory.html
Normal file
@@ -0,0 +1,62 @@
|
||||
<!DOCTYPE html>
|
||||
<html lang="fr">
|
||||
<head>
|
||||
<meta charset="UTF-8">
|
||||
<title>COSMA NAV — Trajectory Viewer</title>
|
||||
<style>
|
||||
* { margin: 0; padding: 0; box-sizing: border-box; }
|
||||
body { background: #06060f; color: #ccc; font-family: 'Courier New', monospace; overflow: hidden; }
|
||||
canvas { display: block; }
|
||||
#legend {
|
||||
position: fixed; top: 16px; left: 16px;
|
||||
background: rgba(4,4,16,0.85); padding: 14px 18px;
|
||||
border-radius: 8px; border: 1px solid #223;
|
||||
font-size: 12px; min-width: 180px; backdrop-filter: blur(4px);
|
||||
}
|
||||
#legend h3 { color: #8af; margin-bottom: 10px; font-size: 13px; letter-spacing: 1px; }
|
||||
#legend label { display: flex; align-items: center; gap: 8px; cursor: pointer; margin: 5px 0; user-select: none; }
|
||||
.swatch { width: 10px; height: 10px; border-radius: 2px; flex-shrink: 0; }
|
||||
#info {
|
||||
position: fixed; bottom: 16px; left: 16px;
|
||||
background: rgba(4,4,16,0.85); padding: 8px 14px;
|
||||
border-radius: 6px; font-size: 11px; color: #89a;
|
||||
backdrop-filter: blur(4px); border: 1px solid #223;
|
||||
}
|
||||
#status {
|
||||
position: fixed; top: 16px; right: 16px;
|
||||
background: rgba(4,4,16,0.85); padding: 8px 14px;
|
||||
border-radius: 6px; font-size: 11px; color: #67d;
|
||||
backdrop-filter: blur(4px); border: 1px solid #223;
|
||||
}
|
||||
</style>
|
||||
</head>
|
||||
<body>
|
||||
<div id="legend">
|
||||
<h3>COSMA NAV</h3>
|
||||
<label><input type="checkbox" checked data-layer="usv_gps">
|
||||
<span class="swatch" style="background:#3a9fff"></span>USV GPS</label>
|
||||
<label><input type="checkbox" checked data-layer="auv_depth">
|
||||
<span class="swatch" style="background:#44ffaa"></span>AUV depth</label>
|
||||
<label><input type="checkbox" checked data-layer="usbl">
|
||||
<span class="swatch" style="background:#ff6b35"></span>USBL fixes</label>
|
||||
<label><input type="checkbox" checked data-layer="fused">
|
||||
<span class="swatch" style="background:#ffffff"></span>Fused world</label>
|
||||
<label><input type="checkbox" checked data-layer="lingbot_local">
|
||||
<span class="swatch" style="background:#ffd700"></span>lingbot local</label>
|
||||
<label><input type="checkbox" checked data-layer="frustums">
|
||||
<span class="swatch" style="background:#ffd70066"></span>Frustums</label>
|
||||
<label><input type="checkbox" checked data-layer="ply">
|
||||
<span class="swatch" style="background:#556"></span>Point cloud</label>
|
||||
</div>
|
||||
<div id="status">Loading…</div>
|
||||
<div id="info">Orbit: drag · Zoom: scroll · Pan: right-drag</div>
|
||||
|
||||
<script type="importmap">
|
||||
{"imports":{
|
||||
"three":"https://cdn.jsdelivr.net/npm/three@0.160/build/three.module.js",
|
||||
"three/addons/":"https://cdn.jsdelivr.net/npm/three@0.160/examples/jsm/"
|
||||
}}
|
||||
</script>
|
||||
<script type="module" src="/static/js/scene.js"></script>
|
||||
</body>
|
||||
</html>
|
||||
Reference in New Issue
Block a user