feat(moulin-mapper): simulateur capteurs + pipeline SLAM Python (JSONL stream → trajectoire + nuage .ply)
- simulator.py: flux JSONL réaliste (Ping360 angle/dist, IMU, heading, depth, altitude) + vérité terrain - slam.py: dead-reckoning + scan-to-map ICP 2D (cKDTree) + fermeture de boucle - process.py: ingestion streaming ligne-par-ligne → trajectory.csv + map_2d.csv + cloud.ply - stream_replay.py: rejoue le flux (vision streaming remote) - SCHEMA.md: contrat format données ROV réel↔sim - RMS dead-reckoning 0.386m → scan-matching 0.188m (2x)
This commit is contained in:
Flag
59
pipeline/README.md
Normal file
59
pipeline/README.md
Normal file
@@ -0,0 +1,59 @@
|
||||
# moulin-mapper / pipeline
|
||||
|
||||
Pipeline SLAM 2D pour cartographie de chambre de moulin via Ping360 + BlueROV.
|
||||
|
||||
## Prérequis
|
||||
|
||||
```bash
|
||||
pip install numpy scipy
|
||||
# ou dans un venv :
|
||||
python3 -m venv .venv && source .venv/bin/activate && pip install -r requirements.txt
|
||||
```
|
||||
|
||||
## Lancement complet
|
||||
|
||||
```bash
|
||||
cd pipeline/
|
||||
|
||||
# 1. Simuler un run complet
|
||||
python3 simulator.py
|
||||
# → data/sim/run_L.jsonl (flux capteurs)
|
||||
# → data/sim/run_L_truth.csv (vérité terrain)
|
||||
|
||||
# 2. Traiter le flux et exporter
|
||||
python3 process.py
|
||||
# → data/out/trajectory.csv
|
||||
# → data/out/map_2d.csv
|
||||
# → data/out/cloud.ply
|
||||
# Affiche RMS dead-reckoning vs scan-matching
|
||||
|
||||
# 3. Streaming pipe (simule flux réseau live)
|
||||
python3 stream_replay.py | python3 process.py --stdin
|
||||
# ou acceleré 10x avec timing réel :
|
||||
python3 stream_replay.py --realtime --speed 10 | python3 process.py --stdin
|
||||
```
|
||||
|
||||
## Fichiers
|
||||
|
||||
| Fichier | Rôle |
|
||||
|-------------------|------|
|
||||
| `SCHEMA.md` | Contrat de format du flux JSONL |
|
||||
| `room.py` | Géométrie chambre + raycast 2D |
|
||||
| `simulator.py` | Génère données simulées réalistes |
|
||||
| `slam.py` | Dead-reckoning + ICP + fermeture de boucle |
|
||||
| `process.py` | Pipeline streaming complet, exports |
|
||||
| `stream_replay.py`| Rejoue JSONL sur stdout (pipe vers process.py) |
|
||||
|
||||
## Brancher le vrai ROV
|
||||
|
||||
Remplacer la source dans `process.py` par le flux réseau BlueROV :
|
||||
|
||||
```python
|
||||
# Exemple : flux TCP depuis ArduSub
|
||||
import socket
|
||||
sock = socket.create_connection(("192.168.x.x", PORT))
|
||||
run_pipeline(sock.makefile(), truth_path=None, out_dir="data/out")
|
||||
```
|
||||
|
||||
Le format JSONL est défini dans `SCHEMA.md` — le pipeline est identique
|
||||
sur données simulées et réelles.
|
||||
116
pipeline/SCHEMA.md
Normal file
116
pipeline/SCHEMA.md
Normal file
@@ -0,0 +1,116 @@
|
||||
# SCHEMA.md — Contrat de flux capteurs moulin-mapper
|
||||
|
||||
## Principe
|
||||
|
||||
Une ligne JSONL = un échantillon Ping360 (un faisceau sonar émis à un angle donné)
|
||||
avec la **télémétrie co-enregistrée à cet instant exact**.
|
||||
|
||||
Le pipeline de traitement doit fonctionner identiquement sur :
|
||||
- données simulées (simulator.py)
|
||||
- données réelles BlueROV (juste changer la source)
|
||||
|
||||
Champs optionnels futurs (`image_ref`, `confidence`, etc.) peuvent s'ajouter
|
||||
sans casser le pipeline — tous les champs inconnus sont ignorés.
|
||||
|
||||
---
|
||||
|
||||
## Format : JSONL (JSON Lines)
|
||||
|
||||
Un objet JSON par ligne, terminé par `\n`. Pas de tableau englobant.
|
||||
|
||||
### Exemple
|
||||
|
||||
```jsonl
|
||||
{"t": 12.345, "ping360_angle": 174.6, "ping360_distance": 3.21, "heading": 88.2, "altitude": 1.05, "depth": 1.82, "ax": 0.01, "ay": -0.02, "az": 9.79, "gx": 0.001, "gy": 0.0, "gz": 0.05}
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Champs obligatoires
|
||||
|
||||
| Champ | Type | Unité | Description |
|
||||
|--------------------|---------|-----------|-------------|
|
||||
| `t` | float | s | Timestamp depuis le démarrage du flux (epoch relatif) |
|
||||
| `ping360_angle` | float | deg | Angle du faisceau sonar **relatif au cap ROV** (0 = avant ROV) |
|
||||
| `ping360_distance` | float | m | Distance mesurée jusqu'au premier écho (0.0 = dropout/pas d'écho) |
|
||||
| `heading` | float | deg | Cap magnétique ROV, convention Nord=0, Est=90 (CW) |
|
||||
| `altitude` | float | m | Hauteur au-dessus du fond (Ping1D vers le bas), 0.0 = dropout |
|
||||
| `depth` | float | m | Profondeur sous la surface (pression bar → m d'eau), positif vers le bas |
|
||||
| `ax` | float | m/s² | Accélération IMU axe X (avant ROV dans repère corps) |
|
||||
| `ay` | float | m/s² | Accélération IMU axe Y (gauche ROV dans repère corps) |
|
||||
| `az` | float | m/s² | Accélération IMU axe Z (haut ROV dans repère corps), ~+9.81 au repos |
|
||||
| `gx` | float | rad/s | Gyromètre axe X (roulis) |
|
||||
| `gy` | float | rad/s | Gyromètre axe Y (tangage) |
|
||||
| `gz` | float | rad/s | Gyromètre axe Z (lacet, positif = rotation CW vue du dessus) |
|
||||
|
||||
---
|
||||
|
||||
## Repères et conventions
|
||||
|
||||
### Repère monde : ENU local
|
||||
- X = Est, Y = Nord, Z = vers le haut
|
||||
- Origine = position initiale du ROV au démarrage
|
||||
- Le heading 0° correspond à +Y (Nord)
|
||||
|
||||
### Ping360
|
||||
- **400 pas/tour** (comme le vrai Ping360 BlueRobotics) → résolution angulaire 0.9°/pas
|
||||
- `ping360_angle` est **relatif au cap ROV** (0° = avant du ROV)
|
||||
- Pour obtenir l'angle monde : `angle_monde = (heading + ping360_angle) % 360`
|
||||
- Plage : [0, 360) degrés
|
||||
- `ping360_distance = 0.0` signifie absence d'écho (dropout) — à ignorer dans le pipeline
|
||||
|
||||
### Heading
|
||||
- Convention magnétique compas : 0°=Nord, 90°=Est, 180°=Sud, 270°=Ouest (CW)
|
||||
- Bruit typique simulé : ~0.5° RMS
|
||||
- Dérive lente optionnelle pour simuler un mauvais magnéto
|
||||
|
||||
### Altitude (Ping1D)
|
||||
- Sonar vertical pointant vers le bas
|
||||
- Mesure la distance ROV → fond en mètres
|
||||
- `altitude = 0.0` signifie dropout (confidence trop basse)
|
||||
- Hauteur ROV au-dessus du fond = `altitude`
|
||||
- Profondeur fond = `depth + altitude`
|
||||
|
||||
### IMU
|
||||
- Repère corps ROV (Body frame) — PAS ENU
|
||||
- `az` ≈ +9.81 m/s² au repos (gravité remontant dans le repère corps)
|
||||
- Biais gyro simulé : ~0.005 rad/s — cause dérive dead-reckoning ~18°/min
|
||||
|
||||
---
|
||||
|
||||
## Fréquences typiques
|
||||
|
||||
| Source | Fréquence |
|
||||
|----------------|-----------|
|
||||
| Ping360 | 400 msgs/sweep, ~1 sweep/4 s (configurable) |
|
||||
| IMU | 1 msg par ligne JSONL (co-enregistré avec le ping sonar) |
|
||||
| heading | idem |
|
||||
| altitude | idem |
|
||||
| depth | idem |
|
||||
|
||||
---
|
||||
|
||||
## Fichier vérité terrain (simulation uniquement)
|
||||
|
||||
`run_L_truth.csv` — colonnes : `t, x, y, heading_deg, z`
|
||||
- x, y en mètres ENU depuis l'origine
|
||||
- heading_deg : cap vrai (sans bruit)
|
||||
- z : profondeur (positif vers le bas)
|
||||
- Ce fichier n'existe PAS sur données réelles — sert uniquement à valider l'algo
|
||||
|
||||
---
|
||||
|
||||
## Champs optionnels (présents dans la simulation, rétrocompatibles)
|
||||
|
||||
| Champ | Type | Unité | Description |
|
||||
|------------------|--------|-------|-------------|
|
||||
| `vf` | float | m/s | Vitesse corps forward (avant ROV) — modèle thruster ou DVL |
|
||||
| `vl` | float | m/s | Vitesse corps latérale — modèle thruster ou DVL |
|
||||
| `image_ref` | string | — | Nom du fichier image caméra co-enregistrée |
|
||||
| `confidence` | float | [0,1] | Confiance écho Ping360 |
|
||||
| `temperature` | float | °C | Capteur eau |
|
||||
| `battery_v` | float | V | Tension batterie |
|
||||
|
||||
`vf` et `vl` sont produits par le simulateur depuis le modèle de thruster.
|
||||
Sur un vrai BlueROV, ils peuvent venir d'un DVL ou du contrôleur de poussée.
|
||||
Le pipeline DR utilise `vf`/`vl` s'ils sont présents, sinon intègre `ax`/`ay`.
|
||||
307
pipeline/process.py
Normal file
307
pipeline/process.py
Normal file
@@ -0,0 +1,307 @@
|
||||
"""
|
||||
process.py — Pipeline complet streaming moulin-mapper.
|
||||
|
||||
Lit le flux JSONL ligne par ligne (streaming), applique dead-reckoning
|
||||
+ scan-matching ICP, exporte les résultats.
|
||||
|
||||
Usage :
|
||||
python3 process.py [--input ../data/sim/run_L.jsonl] [--truth ../data/sim/run_L_truth.csv]
|
||||
python3 stream_replay.py | python3 process.py --stdin
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import csv
|
||||
import json
|
||||
import os
|
||||
import sys
|
||||
import numpy as np
|
||||
from typing import List, Tuple, Optional
|
||||
|
||||
# Assure imports depuis pipeline/
|
||||
sys.path.insert(0, os.path.dirname(__file__))
|
||||
from slam import (
|
||||
DeadReckoning,
|
||||
SweepAccumulator,
|
||||
sweep_to_world_points,
|
||||
icp_2d,
|
||||
IncrementalMap,
|
||||
LoopClosureDetector,
|
||||
apply_loop_closure,
|
||||
)
|
||||
|
||||
ROV_Z = 1.5 # profondeur fixe ROV (m) — sera remplacé par depth réel
|
||||
ROV_ALT = 0.5 # hauteur fond (m) — sera remplacé par altitude réelle
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Lecture JSONL streaming
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def stream_jsonl(source):
|
||||
"""
|
||||
Générateur : lit le flux JSONL ligne par ligne depuis un fichier ou stdin.
|
||||
Chaque appel yield un dict Python.
|
||||
"""
|
||||
for line in source:
|
||||
line = line.strip()
|
||||
if not line:
|
||||
continue
|
||||
try:
|
||||
yield json.loads(line)
|
||||
except json.JSONDecodeError:
|
||||
continue
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Chargement vérité terrain (optionnel)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def load_truth(truth_path: str) -> np.ndarray:
|
||||
"""Retourne array (N, 5) : t, x, y, heading_deg, z"""
|
||||
rows = []
|
||||
with open(truth_path) as f:
|
||||
reader = csv.DictReader(f)
|
||||
for row in reader:
|
||||
rows.append([float(row["t"]), float(row["x"]), float(row["y"]),
|
||||
float(row["heading_deg"]), float(row["z"])])
|
||||
return np.array(rows)
|
||||
|
||||
|
||||
def interpolate_truth(truth: np.ndarray, t: float) -> np.ndarray:
|
||||
"""Interpole la vérité terrain à l'instant t."""
|
||||
idx = np.searchsorted(truth[:, 0], t)
|
||||
idx = min(idx, len(truth) - 1)
|
||||
return truth[idx]
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Export PLY ASCII (nuage 3D)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def write_ply(path: str, map2d: np.ndarray, depth: float, altitude: float) -> int:
|
||||
"""
|
||||
Extrude le contour 2D entre le fond et la surface → nuage 3D ASCII PLY.
|
||||
Retourne le nombre de points.
|
||||
"""
|
||||
pts = []
|
||||
z_surface = -depth # z=0 à la surface, profondeur = négatif si on va vers le bas
|
||||
z_bottom = -(depth + altitude)
|
||||
|
||||
for x, y in map2d:
|
||||
pts.append((x, y, z_surface))
|
||||
pts.append((x, y, z_bottom))
|
||||
|
||||
os.makedirs(os.path.dirname(path), exist_ok=True)
|
||||
with open(path, "w") as f:
|
||||
f.write("ply\nformat ascii 1.0\n")
|
||||
f.write(f"element vertex {len(pts)}\n")
|
||||
f.write("property float x\nproperty float y\nproperty float z\n")
|
||||
f.write("end_header\n")
|
||||
for px, py, pz in pts:
|
||||
f.write(f"{px:.4f} {py:.4f} {pz:.4f}\n")
|
||||
return len(pts)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Pipeline principal
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def run_pipeline(source, truth_path: Optional[str] = None, out_dir: str = "../data/out"):
|
||||
os.makedirs(out_dir, exist_ok=True)
|
||||
|
||||
# Composants SLAM
|
||||
dr = DeadReckoning()
|
||||
sweep_acc = SweepAccumulator(steps=400)
|
||||
global_map = IncrementalMap(voxel_size=0.05)
|
||||
loop_detector = LoopClosureDetector(min_time=15.0, dist_thresh=0.8)
|
||||
|
||||
# Trajectoire estimée : dead-reckoning + corrigée
|
||||
traj_dr: List[Tuple] = [] # (t, x, y)
|
||||
traj_corr: List[Tuple] = [] # (t, x, y)
|
||||
|
||||
# Offset ICP cumulatif : déplacement total appliqué au-dessus du DR
|
||||
# Maintenu entre les sweeps pour que la pose corrigée reste cohérente
|
||||
icp_offset = np.array([0.0, 0.0])
|
||||
|
||||
# Accumulation erreurs pour RMS
|
||||
dr_errors = []
|
||||
sm_errors = []
|
||||
truth = None
|
||||
if truth_path and os.path.exists(truth_path):
|
||||
truth = load_truth(truth_path)
|
||||
print(f"Vérité terrain chargée : {len(truth)} points")
|
||||
|
||||
n_sweeps = 0
|
||||
n_loop_closures = 0
|
||||
sweep_poses = [] # (sweep_idx, t, x_corr, y_corr)
|
||||
|
||||
x_dr, y_dr, h_dr = 0.0, 0.0, 0.0
|
||||
last_depth = ROV_Z
|
||||
last_altitude = ROV_ALT
|
||||
|
||||
for record in stream_jsonl(source):
|
||||
t = record["t"]
|
||||
hdg = record["heading"]
|
||||
alt = record["altitude"]
|
||||
depth_r = record["depth"]
|
||||
ax = record["ax"]
|
||||
ay = record["ay"]
|
||||
az = record["az"]
|
||||
gz = record.get("gz", 0.0)
|
||||
vf = record.get("vf", None) # vitesse forward (optionnel)
|
||||
vl = record.get("vl", 0.0)
|
||||
ping_angle = record["ping360_angle"]
|
||||
ping_dist = record["ping360_distance"]
|
||||
|
||||
if alt > 0.01:
|
||||
last_altitude = alt
|
||||
if depth_r > 0.01:
|
||||
last_depth = depth_r
|
||||
|
||||
# --- Dead-reckoning ---
|
||||
x_dr, y_dr, h_dr = dr.update(t, hdg, ax, ay, az, gz=gz, vf=vf, vl=vl)
|
||||
|
||||
# Pose corrigée = pose DR + offset ICP cumulatif
|
||||
# L'offset est mis à jour à chaque sweep ICP réussi
|
||||
pose_dr = np.array([x_dr, y_dr])
|
||||
pose_corr_cur = pose_dr + icp_offset
|
||||
|
||||
# --- Sweep Ping360 ---
|
||||
# On passe la pose CORRIGÉE au sweep pour générer des points
|
||||
# cohérents avec la carte globale déjà corrigée
|
||||
pose_for_sweep = (pose_corr_cur[0], pose_corr_cur[1], h_dr)
|
||||
completed = sweep_acc.add_beam(ping_angle, ping_dist, pose_for_sweep)
|
||||
|
||||
if completed is not None:
|
||||
n_sweeps += 1
|
||||
|
||||
# Points murs depuis le sweep (en coordonnées pose_corr)
|
||||
sweep_pts = sweep_to_world_points(completed)
|
||||
|
||||
if len(sweep_pts) >= 5:
|
||||
map_arr = global_map.get_array()
|
||||
|
||||
if len(map_arr) >= 10:
|
||||
# ICP : recaler sweep sur carte → donne correction résiduelle
|
||||
# La correction est petite car la pose déjà corrigée
|
||||
dx, dy, dtheta, converged = icp_2d(
|
||||
sweep_pts, map_arr,
|
||||
max_iter=30, tol=1e-4, max_dist=0.5
|
||||
)
|
||||
|
||||
if converged:
|
||||
# Mise à jour de l'offset cumulatif
|
||||
icp_offset = icp_offset + np.array([dx, dy])
|
||||
pose_corr_cur = pose_dr + icp_offset
|
||||
|
||||
# Ajouter les points du sweep à la carte globale
|
||||
# (avec la pose corrigée)
|
||||
global_map.add_points(sweep_pts + icp_offset)
|
||||
|
||||
# Enregistrer la pose pour la trajectoire
|
||||
traj_dr.append((t, x_dr, y_dr))
|
||||
traj_corr.append((t, pose_corr_cur[0], pose_corr_cur[1]))
|
||||
sweep_poses.append((n_sweeps - 1, t, pose_corr_cur[0], pose_corr_cur[1]))
|
||||
|
||||
# Vérification erreurs vs vérité
|
||||
if truth is not None:
|
||||
gt = interpolate_truth(truth, t)
|
||||
gt_xy = gt[1:3]
|
||||
dr_errors.append(float(np.linalg.norm(pose_dr - gt_xy)))
|
||||
sm_errors.append(float(np.linalg.norm(pose_corr_cur - gt_xy)))
|
||||
|
||||
# Détection fermeture de boucle
|
||||
loop_detector.add_pose(t, pose_corr_cur[0], pose_corr_cur[1], n_sweeps - 1)
|
||||
match_idx = loop_detector.check(t, pose_corr_cur[0], pose_corr_cur[1])
|
||||
if match_idx is not None and match_idx < len(sweep_poses) - 3:
|
||||
n_loop_closures += 1
|
||||
_, ox, oy = traj_corr[match_idx]
|
||||
correction = np.array([ox - pose_corr_cur[0], oy - pose_corr_cur[1]]) * 0.5
|
||||
# Redistribuer la correction linéairement
|
||||
for i in range(match_idx, len(traj_corr)):
|
||||
alpha = (i - match_idx) / max(1, len(traj_corr) - 1 - match_idx)
|
||||
t2, x2, y2 = traj_corr[i]
|
||||
traj_corr[i] = (t2, x2 + alpha * correction[0], y2 + alpha * correction[1])
|
||||
icp_offset = icp_offset + correction
|
||||
loop_detector.history.clear()
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Export résultats
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
# trajectory.csv
|
||||
traj_path = os.path.join(out_dir, "trajectory.csv")
|
||||
with open(traj_path, "w", newline="") as f:
|
||||
writer = csv.writer(f)
|
||||
writer.writerow(["t", "x_dr", "y_dr", "x_corr", "y_corr"])
|
||||
n = min(len(traj_dr), len(traj_corr))
|
||||
for i in range(n):
|
||||
t2, xd, yd = traj_dr[i]
|
||||
_, xc, yc = traj_corr[i]
|
||||
writer.writerow([f"{t2:.4f}", f"{xd:.4f}", f"{yd:.4f}",
|
||||
f"{xc:.4f}", f"{yc:.4f}"])
|
||||
|
||||
# map_2d.csv
|
||||
map_arr = global_map.get_array()
|
||||
map_path = os.path.join(out_dir, "map_2d.csv")
|
||||
with open(map_path, "w", newline="") as f:
|
||||
writer = csv.writer(f)
|
||||
writer.writerow(["x", "y"])
|
||||
for x2, y2 in map_arr:
|
||||
writer.writerow([f"{x2:.4f}", f"{y2:.4f}"])
|
||||
|
||||
# cloud.ply
|
||||
ply_path = os.path.join(out_dir, "cloud.ply")
|
||||
n_ply_pts = write_ply(ply_path, map_arr, last_depth, last_altitude)
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Stats
|
||||
# ---------------------------------------------------------------------------
|
||||
print("\n=== Résultats pipeline ===")
|
||||
print(f"Sweeps traités : {n_sweeps}")
|
||||
print(f"Fermetures de boucle : {n_loop_closures}")
|
||||
print(f"Points carte 2D : {len(global_map)}")
|
||||
print(f"Points nuage 3D (PLY) : {n_ply_pts}")
|
||||
print(f"Taille cloud.ply : {os.path.getsize(ply_path)/1e3:.1f} Ko")
|
||||
|
||||
if dr_errors and sm_errors:
|
||||
rms_dr = float(np.sqrt(np.mean(np.array(dr_errors)**2)))
|
||||
rms_sm = float(np.sqrt(np.mean(np.array(sm_errors)**2)))
|
||||
ratio = rms_sm / rms_dr if rms_dr > 0 else float("inf")
|
||||
print(f"\nRMS erreur dead-reckoning : {rms_dr:.3f} m")
|
||||
print(f"RMS erreur scan-matching : {rms_sm:.3f} m")
|
||||
print(f"Amélioration : {ratio:.2f}x (DR/SM={rms_dr/rms_sm:.1f}x)")
|
||||
if rms_sm < rms_dr:
|
||||
print("OK — scan-matching améliore la position vs dead-reckoning")
|
||||
else:
|
||||
print("AVERTISSEMENT — scan-matching n'améliore pas (données insuffisantes?)")
|
||||
else:
|
||||
print("\nPas de vérité terrain → pas de calcul RMS")
|
||||
|
||||
print(f"\nFichiers exportés dans {out_dir}/")
|
||||
print(f" {traj_path}")
|
||||
print(f" {map_path}")
|
||||
print(f" {ply_path}")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# CLI
|
||||
# ---------------------------------------------------------------------------
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Pipeline SLAM moulin-mapper")
|
||||
parser.add_argument("--input", default="../data/sim/run_L.jsonl",
|
||||
help="Fichier JSONL en entrée")
|
||||
parser.add_argument("--truth", default="../data/sim/run_L_truth.csv",
|
||||
help="Fichier vérité terrain (optionnel)")
|
||||
parser.add_argument("--out-dir", default="../data/out")
|
||||
parser.add_argument("--stdin", action="store_true",
|
||||
help="Lire depuis stdin au lieu d'un fichier")
|
||||
args = parser.parse_args()
|
||||
|
||||
if args.stdin:
|
||||
print("Lecture depuis stdin (streaming)...")
|
||||
run_pipeline(sys.stdin, truth_path=None, out_dir=args.out_dir)
|
||||
else:
|
||||
truth_p = args.truth if os.path.exists(args.truth) else None
|
||||
with open(args.input) as f:
|
||||
run_pipeline(f, truth_path=truth_p, out_dir=args.out_dir)
|
||||
2
pipeline/requirements.txt
Normal file
2
pipeline/requirements.txt
Normal file
@@ -0,0 +1,2 @@
|
||||
numpy>=1.24
|
||||
scipy>=1.10
|
||||
196
pipeline/room.py
Normal file
196
pipeline/room.py
Normal file
@@ -0,0 +1,196 @@
|
||||
"""
|
||||
room.py — Géométrie de la chambre de moulin + raycast 2D.
|
||||
|
||||
La chambre est un polygone (pièce en L avec une niche).
|
||||
Toutes les coordonnées sont en mètres, repère ENU local (x=Est, y=Nord).
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from typing import Optional, List, Tuple
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Géométrie : chambre en L avec une niche dans le mur Est
|
||||
# ---------------------------------------------------------------------------
|
||||
#
|
||||
# y
|
||||
# ^
|
||||
# |
|
||||
# 8 +---+
|
||||
# | | <- couloir Nord
|
||||
# 5 + +-------+
|
||||
# | | <- grande salle
|
||||
# | niche |
|
||||
# 2 + +---+ |
|
||||
# | | |
|
||||
# 0 +-------+---+---> x
|
||||
# 0 3 5 8
|
||||
#
|
||||
# La niche est dans la grande salle, côté Ouest (renfoncement à x=3..5, y=2..5)
|
||||
# La chambre principale va de x=0..8, y=0..5
|
||||
# Le couloir va de x=0..3, y=5..8
|
||||
|
||||
ROOM_POLYGON = np.array([
|
||||
[0.0, 0.0], # 0 — coin Sud-Ouest
|
||||
[8.0, 0.0], # 1 — coin Sud-Est
|
||||
[8.0, 5.0], # 2 — coin Est milieu
|
||||
[3.0, 5.0], # 3 — jonction couloir/salle
|
||||
[3.0, 8.0], # 4 — coin Nord-Est couloir
|
||||
[0.0, 8.0], # 5 — coin Nord-Ouest
|
||||
], dtype=float)
|
||||
|
||||
# Niche : renfoncement dans le mur Ouest de la grande salle
|
||||
# C'est un rectangle "creusé" dans x=0..3, y=2..4
|
||||
# On l'ajoute comme polygone séparé (trou dans la géométrie)
|
||||
# Simplification : on le modélise comme murs supplémentaires
|
||||
NICHE_WALLS = np.array([
|
||||
# mur du fond de la niche (à x=3, de y=2 à y=4) — déjà dans le polygone principal
|
||||
# mur Nord de la niche (y=4, de x=0 à x=3)
|
||||
[[0.0, 4.0], [3.0, 4.0]],
|
||||
# mur Sud de la niche (y=2, de x=0 à x=3)
|
||||
[[0.0, 2.0], [3.0, 2.0]],
|
||||
], dtype=float)
|
||||
|
||||
|
||||
def _polygon_to_segments(poly: np.ndarray) -> List[Tuple[np.ndarray, np.ndarray]]:
|
||||
"""Convertit un polygone fermé en liste de segments (p0, p1)."""
|
||||
segs = []
|
||||
n = len(poly)
|
||||
for i in range(n):
|
||||
segs.append((poly[i], poly[(i + 1) % n]))
|
||||
return segs
|
||||
|
||||
|
||||
def get_all_segments() -> List[Tuple[np.ndarray, np.ndarray]]:
|
||||
"""Retourne tous les segments de murs de la chambre."""
|
||||
segs = _polygon_to_segments(ROOM_POLYGON)
|
||||
for wall in NICHE_WALLS:
|
||||
segs.append((wall[0], wall[1]))
|
||||
return segs
|
||||
|
||||
|
||||
_ALL_SEGMENTS = get_all_segments()
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Raycast : intersection rayon / segments
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def _ray_segment_intersect(
|
||||
origin: np.ndarray,
|
||||
direction: np.ndarray,
|
||||
p0: np.ndarray,
|
||||
p1: np.ndarray,
|
||||
) -> Optional[float]:
|
||||
"""
|
||||
Intersection d'un rayon (origin + t*direction) avec un segment [p0, p1].
|
||||
Retourne t > 0 si intersection, None sinon.
|
||||
|
||||
Système : origin + t*D = p0 + u*(p1-p0)
|
||||
→ M * [t, u]^T = p0 - origin, M = [D | -(p1-p0)]
|
||||
Résolu par règle de Cramer 2×2.
|
||||
"""
|
||||
dx = direction[0]
|
||||
dy = direction[1]
|
||||
# vecteur segment
|
||||
ex = p1[0] - p0[0]
|
||||
ey = p1[1] - p0[1]
|
||||
|
||||
# det(M) = det([[dx, -ex], [dy, -ey]]) = dx*(-ey) - (-ex)*dy = -dx*ey + ex*dy
|
||||
denom = -dx * ey + ex * dy
|
||||
|
||||
if abs(denom) < 1e-12:
|
||||
# Rayon parallèle au segment
|
||||
return None
|
||||
|
||||
# Second membre : p0 - origin
|
||||
bx = p0[0] - origin[0]
|
||||
by = p0[1] - origin[1]
|
||||
|
||||
# Cramer : t = det([[bx, -ex], [by, -ey]]) / denom
|
||||
t = (-bx * ey + ex * by) / denom
|
||||
# Cramer : u = det([[dx, bx], [dy, by]]) / denom
|
||||
u = (dx * by - bx * dy) / denom
|
||||
|
||||
if t >= 0.0 and 0.0 <= u <= 1.0:
|
||||
return t
|
||||
return None
|
||||
|
||||
|
||||
def range_to_walls(
|
||||
x: float,
|
||||
y: float,
|
||||
bearing_world_deg: float,
|
||||
max_range: float = 30.0,
|
||||
) -> Optional[float]:
|
||||
"""
|
||||
Calcule la distance depuis (x, y) jusqu'au premier mur dans la direction
|
||||
bearing_world_deg (convention : 0°=Nord/+Y, 90°=Est/+X, CW).
|
||||
|
||||
Retourne la distance en mètres, ou None si aucune intersection dans max_range.
|
||||
"""
|
||||
# Conversion cap → vecteur direction ENU (x=Est, y=Nord)
|
||||
bearing_rad = np.deg2rad(bearing_world_deg)
|
||||
# cap 0°=Nord → direction (sin θ, cos θ) dans ENU
|
||||
direction = np.array([np.sin(bearing_rad), np.cos(bearing_rad)], dtype=float)
|
||||
|
||||
origin = np.array([x, y], dtype=float)
|
||||
min_t = None
|
||||
|
||||
for p0, p1 in _ALL_SEGMENTS:
|
||||
t = _ray_segment_intersect(origin, direction, p0, p1)
|
||||
if t is not None and t > 1e-6:
|
||||
if min_t is None or t < min_t:
|
||||
min_t = t
|
||||
|
||||
if min_t is not None and min_t <= max_range:
|
||||
return min_t
|
||||
return None
|
||||
|
||||
|
||||
def point_in_room(x: float, y: float) -> bool:
|
||||
"""
|
||||
Test d'appartenance à l'intérieur de la chambre.
|
||||
Utilise l'algorithme du ray casting (rayon vers +x infini).
|
||||
Tient compte des deux zones : grande salle + couloir, moins la niche.
|
||||
"""
|
||||
# Grande salle : x in [0,8], y in [0,5]
|
||||
in_main = (0 <= x <= 8) and (0 <= y <= 5)
|
||||
# Couloir : x in [0,3], y in [5,8]
|
||||
in_corridor = (0 <= x <= 3) and (5 <= y <= 8)
|
||||
# Niche (vide) : x in [0,3], y in [2,4] — cet espace est ouvert (pas de mur intérieur)
|
||||
# Simplifié : la niche est incluse dans main salle
|
||||
|
||||
return in_main or in_corridor
|
||||
|
||||
|
||||
def get_room_bounds() -> Tuple[float, float, float, float]:
|
||||
"""Retourne (xmin, xmax, ymin, ymax) de la chambre."""
|
||||
pts = ROOM_POLYGON
|
||||
return float(pts[:, 0].min()), float(pts[:, 0].max()), \
|
||||
float(pts[:, 1].min()), float(pts[:, 1].max())
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Test rapide
|
||||
# ---------------------------------------------------------------------------
|
||||
if __name__ == "__main__":
|
||||
print("=== Test raycast chambre ===")
|
||||
# Depuis le centre de la grande salle
|
||||
cx, cy = 4.0, 2.5
|
||||
for bearing in [0, 90, 180, 270]:
|
||||
d = range_to_walls(cx, cy, bearing)
|
||||
ds = f"{d:.3f}" if d is not None else "None"
|
||||
print(f" cap {bearing:3d}° → {ds} m")
|
||||
|
||||
# Attendu approximatif :
|
||||
# 0° (Nord) → 2.5 m (mur y=5)
|
||||
# 90° (Est) → 4.0 m (mur x=8)
|
||||
# 180° (Sud) → 2.5 m (mur y=0)
|
||||
# 270° (Ouest) → 1.0 m (mur niche y=2→4? Non, ici y=2.5 donc mur x=0 → 4m)
|
||||
# Attention : niche = renfoncement dans mur Ouest basse, pas à y=2.5
|
||||
# À y=2.5 (entre 2 et 4) le mur Ouest est à x=0 → 4.0 m
|
||||
|
||||
print("\nPoint in room:")
|
||||
for pt, expected in [((4, 2.5), True), ((1, 6), True), ((5, 6), False), ((-1, 1), False)]:
|
||||
print(f" {pt} → {point_in_room(*pt)} (attendu {expected})")
|
||||
314
pipeline/simulator.py
Normal file
314
pipeline/simulator.py
Normal file
@@ -0,0 +1,314 @@
|
||||
"""
|
||||
simulator.py — Génère un flux JSONL réaliste pour le moulin-mapper.
|
||||
|
||||
Produit :
|
||||
../data/sim/run_L.jsonl — flux capteurs (même format que le vrai ROV)
|
||||
../data/sim/run_L_truth.csv — vérité terrain (x, y, heading_deg, z, t)
|
||||
|
||||
Usage : python3 simulator.py [--out-dir ../data/sim] [--sweep-period 4.0]
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import os
|
||||
import csv
|
||||
import numpy as np
|
||||
|
||||
# Assure que room.py est importable depuis pipeline/
|
||||
import sys
|
||||
sys.path.insert(0, os.path.dirname(__file__))
|
||||
from room import range_to_walls
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Paramètres capteurs
|
||||
# ---------------------------------------------------------------------------
|
||||
PING360_STEPS = 400 # pas/tour (vrai Ping360)
|
||||
DEG_PER_STEP = 360.0 / PING360_STEPS # 0.9°/pas
|
||||
PING360_NOISE_M = 0.03 # bruit mesure sonar (m RMS)
|
||||
PING360_DROPOUT_PROB = 0.03 # probabilité de retour nul
|
||||
|
||||
# IMU : biais accéléro réaliste MEMS (BlueRobotics Navigator)
|
||||
# Petits biais : la vraie dérive vient de leur intégration double
|
||||
IMU_ACCEL_NOISE = 0.02 # m/s² (bruit blanc accéléro)
|
||||
IMU_ACCEL_BIAS_F = 0.015 # m/s² biais avant (axe body X)
|
||||
IMU_ACCEL_BIAS_L = -0.010 # m/s² biais latéral (axe body Y)
|
||||
IMU_GYRO_NOISE = 0.001 # rad/s (bruit gyro)
|
||||
IMU_GYRO_BIAS = 0.003 # rad/s (biais lacet → dérive heading si on n'a pas de magnéto)
|
||||
|
||||
HEADING_NOISE_DEG = 0.4 # bruit magnéto (deg RMS)
|
||||
HEADING_DRIFT_RATE = 0.01 # deg/s dérive très lente magnéto (réaliste)
|
||||
|
||||
ALTITUDE_NOISE_M = 0.04 # bruit Ping1D
|
||||
ALTITUDE_DROPOUT_PROB = 0.04
|
||||
ROV_Z = 1.5 # profondeur ROV sous surface (m)
|
||||
ROV_ALT = 0.5 # hauteur ROV au-dessus du fond (m)
|
||||
DEPTH_NOISE_M = 0.015 # bruit capteur pression
|
||||
|
||||
GRAVITY = 9.81 # m/s²
|
||||
|
||||
# Vitesse de translation ROV (m/s) — BlueROV2 lent dans une chambre étroite
|
||||
ROV_SPEED = 0.3 # m/s (vitesse de croisière)
|
||||
# Taux de rotation max (deg/s) en virage
|
||||
ROV_TURN_RATE = 20.0 # deg/s
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Trajectoire scriptée avec cinématique lisse
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def smooth_trajectory(dt: float = 0.05) -> np.ndarray:
|
||||
"""
|
||||
Trajectoire ROV avec vitesses lisses (pas de saut d'accélération).
|
||||
Le ROV se déplace à vitesse constante ROV_SPEED, tourne progressivement.
|
||||
Retourne array (N, 4) : [t, x, y, heading_deg]
|
||||
|
||||
Trajectoire en L couvrant la chambre, avec retour près du départ (boucle).
|
||||
"""
|
||||
# Waypoints (x_target, y_target) — le ROV va d'un point à l'autre
|
||||
# à vitesse constante, en tournant progressivement vers la destination
|
||||
waypoints = [
|
||||
(4.0, 1.0), # départ
|
||||
(4.0, 4.5), # avance Nord
|
||||
(1.5, 4.5), # tourne Ouest
|
||||
(1.5, 7.5), # monte dans couloir
|
||||
(1.5, 5.5), # redescend (demi-tour)
|
||||
(1.5, 2.0), # descend Sud
|
||||
(5.5, 2.0), # traverse Est
|
||||
(5.5, 4.5), # remonte Nord
|
||||
(4.0, 4.5), # revient vers le milieu
|
||||
(4.0, 1.2), # retour près du départ (fermeture boucle)
|
||||
]
|
||||
|
||||
records = []
|
||||
t = 0.0
|
||||
x, y = waypoints[0]
|
||||
heading_deg = 0.0 # Nord au départ
|
||||
|
||||
for target_x, target_y in waypoints[1:]:
|
||||
# Direction vers la cible
|
||||
dx = target_x - x
|
||||
dy = target_y - y
|
||||
dist = np.sqrt(dx**2 + dy**2)
|
||||
if dist < 0.01:
|
||||
continue
|
||||
|
||||
# Cap cible (convention 0°=Nord, 90°=Est)
|
||||
target_heading = np.rad2deg(np.arctan2(dx, dy)) % 360
|
||||
|
||||
# Phase 1 : rotation sur place vers le cap cible
|
||||
dh = ((target_heading - heading_deg + 180) % 360) - 180
|
||||
turn_steps = max(1, int(abs(dh) / (ROV_TURN_RATE * dt)))
|
||||
for j in range(turn_steps):
|
||||
heading_deg += np.sign(dh) * ROV_TURN_RATE * dt
|
||||
heading_deg %= 360
|
||||
records.append([t, x, y, heading_deg])
|
||||
t += dt
|
||||
|
||||
heading_deg = target_heading
|
||||
|
||||
# Phase 2 : avance vers la cible à vitesse constante
|
||||
travel_time = dist / ROV_SPEED
|
||||
move_steps = max(1, int(travel_time / dt))
|
||||
for j in range(move_steps):
|
||||
frac = (j + 1) / move_steps
|
||||
x_cur = x + dx * frac
|
||||
y_cur = y + dy * frac
|
||||
records.append([t, x_cur, y_cur, heading_deg])
|
||||
t += dt
|
||||
|
||||
x, y = target_x, target_y
|
||||
|
||||
return np.array(records)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Générateur principal
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def simulate(
|
||||
out_dir: str = "../data/sim",
|
||||
sweep_period: float = 4.0,
|
||||
dt: float = 0.05,
|
||||
rng_seed: int = 42,
|
||||
) -> None:
|
||||
rng = np.random.default_rng(rng_seed)
|
||||
os.makedirs(out_dir, exist_ok=True)
|
||||
|
||||
jsonl_path = os.path.join(out_dir, "run_L.jsonl")
|
||||
truth_path = os.path.join(out_dir, "run_L_truth.csv")
|
||||
|
||||
# Trajectoire vraie (lisse)
|
||||
traj = smooth_trajectory(dt=dt)
|
||||
N = len(traj)
|
||||
print(f"Trajectoire : {N} pas, durée totale {traj[-1, 0]:.1f} s")
|
||||
|
||||
# Vitesse en repère corps (forward, lateral)
|
||||
# On dérive la position puis projette dans le repère corps
|
||||
px = traj[:, 1]
|
||||
py = traj[:, 2]
|
||||
heading_true = traj[:, 3]
|
||||
|
||||
# Vitesses monde
|
||||
vx_world = np.gradient(px, traj[:, 0])
|
||||
vy_world = np.gradient(py, traj[:, 0])
|
||||
|
||||
# Accélérations monde → lisser (ROV a inertie)
|
||||
ax_world_raw = np.gradient(vx_world, traj[:, 0])
|
||||
ay_world_raw = np.gradient(vy_world, traj[:, 0])
|
||||
|
||||
# Filtrage léger des pics aux transitions (moyenne glissante 5 pts)
|
||||
from numpy.lib.stride_tricks import sliding_window_view
|
||||
def smooth5(arr):
|
||||
out = arr.copy()
|
||||
out[2:-2] = sliding_window_view(arr, 5).mean(axis=1)
|
||||
return out
|
||||
|
||||
ax_world = smooth5(ax_world_raw)
|
||||
ay_world = smooth5(ay_world_raw)
|
||||
|
||||
# Projection accélération monde → corps
|
||||
h_rad_arr = np.deg2rad(heading_true)
|
||||
sin_h = np.sin(h_rad_arr)
|
||||
cos_h = np.cos(h_rad_arr)
|
||||
|
||||
# Convention : avant ROV = (sin h, cos h) en ENU (cap 0°=+Y Nord)
|
||||
# axe corps X = avant, axe corps Y = gauche
|
||||
ax_body_true = ax_world * sin_h + ay_world * cos_h
|
||||
ay_body_true = -ax_world * cos_h + ay_world * sin_h
|
||||
|
||||
# Vitesse corps vraie (forward, lateral)
|
||||
# vx_world, vy_world déjà calculés — projection dans repère corps
|
||||
vf_true = vx_world * sin_h + vy_world * cos_h # avant
|
||||
vl_true = -vx_world * cos_h + vy_world * sin_h # latéral
|
||||
|
||||
# Dérivée de heading pour le gyro
|
||||
dh_arr = np.gradient(heading_true, traj[:, 0])
|
||||
# court chemin sur la dérivée angulaire
|
||||
dh_arr = ((dh_arr + 180) % 360) - 180
|
||||
gz_true_arr = np.deg2rad(dh_arr)
|
||||
|
||||
# Ping360 : balayage continu
|
||||
# step_per_dt = (PING360_STEPS / sweep_period) * dt → degrés par step de traj
|
||||
step_deg_per_dt = (PING360_STEPS / sweep_period) * dt * DEG_PER_STEP
|
||||
current_ping_angle = 0.0
|
||||
|
||||
# Dérive magnéto cumulative
|
||||
heading_drift = 0.0
|
||||
|
||||
# Origine = position initiale du ROV (pour que le DR (0,0) corresponde à la vérité)
|
||||
x0_room = traj[0, 1]
|
||||
y0_room = traj[0, 2]
|
||||
|
||||
records = []
|
||||
truth_rows = []
|
||||
|
||||
for i in range(N):
|
||||
t_i = traj[i, 0]
|
||||
x_i = traj[i, 1]
|
||||
y_i = traj[i, 2]
|
||||
h_i = heading_true[i]
|
||||
|
||||
# --- heading mesuré (magnéto) ---
|
||||
heading_drift += HEADING_DRIFT_RATE * dt
|
||||
h_meas = (h_i + rng.normal(0, HEADING_NOISE_DEG) + heading_drift) % 360
|
||||
|
||||
# --- angle Ping360 (relatif au ROV) ---
|
||||
ping_angle_rel = current_ping_angle % 360.0
|
||||
current_ping_angle += step_deg_per_dt
|
||||
|
||||
# angle monde = cap vrai + angle_relatif
|
||||
ping_angle_world = (h_i + ping_angle_rel) % 360.0
|
||||
|
||||
# --- raycast → distance vraie ---
|
||||
d_true = range_to_walls(x_i, y_i, ping_angle_world)
|
||||
|
||||
# --- bruit + dropout ---
|
||||
if d_true is None or rng.random() < PING360_DROPOUT_PROB:
|
||||
ping_dist = 0.0
|
||||
else:
|
||||
ping_dist = max(0.0, d_true + rng.normal(0, PING360_NOISE_M))
|
||||
|
||||
# --- IMU ---
|
||||
# Accélération corps vraie + biais + bruit
|
||||
ax_m = ax_body_true[i] + IMU_ACCEL_BIAS_F + rng.normal(0, IMU_ACCEL_NOISE)
|
||||
ay_m = ay_body_true[i] + IMU_ACCEL_BIAS_L + rng.normal(0, IMU_ACCEL_NOISE)
|
||||
az_m = GRAVITY + rng.normal(0, IMU_ACCEL_NOISE)
|
||||
|
||||
# Gyro lacet : taux de rotation vrai + biais + bruit
|
||||
gz_m = gz_true_arr[i] + IMU_GYRO_BIAS + rng.normal(0, IMU_GYRO_NOISE)
|
||||
gx_m = rng.normal(0, IMU_GYRO_NOISE)
|
||||
gy_m = rng.normal(0, IMU_GYRO_NOISE)
|
||||
|
||||
# --- altitude (Ping1D) ---
|
||||
if rng.random() < ALTITUDE_DROPOUT_PROB:
|
||||
alt_m = 0.0
|
||||
else:
|
||||
alt_m = max(0.0, ROV_ALT + rng.normal(0, ALTITUDE_NOISE_M))
|
||||
|
||||
# --- profondeur ---
|
||||
depth_m = max(0.0, ROV_Z + rng.normal(0, DEPTH_NOISE_M))
|
||||
|
||||
# --- vitesse corps (thruster controller / DVL simplifié) ---
|
||||
# Bruit de vitesse : erreur ~3% de la vitesse max (0.3 m/s) = ~0.01 m/s
|
||||
# Biais vitesse : représente les courants / asymétrie thrusters
|
||||
VF_BIAS = 0.02 # m/s biais forward constant
|
||||
VF_NOISE = 0.015 # m/s bruit mesure vitesse
|
||||
vf_m = float(vf_true[i]) + VF_BIAS + rng.normal(0, VF_NOISE)
|
||||
vl_m = float(vl_true[i]) + rng.normal(0, VF_NOISE * 0.5)
|
||||
|
||||
record = {
|
||||
"t": round(float(t_i), 4),
|
||||
"ping360_angle": round(float(ping_angle_rel), 2),
|
||||
"ping360_distance": round(float(ping_dist), 4),
|
||||
"heading": round(float(h_meas), 3),
|
||||
"altitude": round(float(alt_m), 4),
|
||||
"depth": round(float(depth_m), 4),
|
||||
"ax": round(float(ax_m), 5),
|
||||
"ay": round(float(ay_m), 5),
|
||||
"az": round(float(az_m), 5),
|
||||
"gx": round(float(gx_m), 6),
|
||||
"gy": round(float(gy_m), 6),
|
||||
"gz": round(float(gz_m), 6),
|
||||
"vf": round(float(vf_m), 4), # vitesse corps forward (m/s) — thruster model
|
||||
"vl": round(float(vl_m), 4), # vitesse corps latérale (m/s)
|
||||
}
|
||||
records.append(record)
|
||||
# Vérité en coordonnées relatives au départ (même repère que le DR qui commence à 0,0)
|
||||
truth_rows.append([float(t_i), float(x_i - x0_room), float(y_i - y0_room), float(h_i), ROV_Z])
|
||||
|
||||
# --- Écriture JSONL ---
|
||||
with open(jsonl_path, "w") as f:
|
||||
for rec in records:
|
||||
f.write(json.dumps(rec) + "\n")
|
||||
|
||||
# --- Vérité terrain CSV ---
|
||||
with open(truth_path, "w", newline="") as f:
|
||||
writer = csv.writer(f)
|
||||
writer.writerow(["t", "x", "y", "heading_deg", "z"])
|
||||
for row in truth_rows:
|
||||
writer.writerow([f"{v:.6f}" for v in row])
|
||||
|
||||
size_mb = os.path.getsize(jsonl_path) / 1e6
|
||||
print(f"JSONL : {jsonl_path} ({size_mb:.2f} Mo, {len(records)} lignes)")
|
||||
print(f"Truth : {truth_path} ({len(truth_rows)} lignes)")
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# CLI
|
||||
# ---------------------------------------------------------------------------
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Simulateur capteurs moulin-mapper")
|
||||
parser.add_argument("--out-dir", default="../data/sim")
|
||||
parser.add_argument("--sweep-period", type=float, default=4.0,
|
||||
help="Durée d'un sweep Ping360 complet (s)")
|
||||
parser.add_argument("--dt", type=float, default=0.05,
|
||||
help="Pas de temps simulation (s)")
|
||||
parser.add_argument("--seed", type=int, default=42)
|
||||
args = parser.parse_args()
|
||||
|
||||
simulate(
|
||||
out_dir=args.out_dir,
|
||||
sweep_period=args.sweep_period,
|
||||
dt=args.dt,
|
||||
rng_seed=args.seed,
|
||||
)
|
||||
358
pipeline/slam.py
Normal file
358
pipeline/slam.py
Normal file
@@ -0,0 +1,358 @@
|
||||
"""
|
||||
slam.py — Dead-reckoning + Scan-matching ICP 2D + fermeture de boucle.
|
||||
|
||||
Modules réutilisables importés par process.py.
|
||||
Dépendances : numpy, scipy uniquement.
|
||||
"""
|
||||
|
||||
import numpy as np
|
||||
from scipy.spatial import cKDTree
|
||||
from typing import List, Optional, Tuple, Dict
|
||||
from collections import deque
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Dead-reckoning (intégration IMU + heading)
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class DeadReckoning:
|
||||
"""
|
||||
Dead-reckoning ROV 2D : heading (magnéto + gyro) + vitesse odométrique.
|
||||
|
||||
Modèle réaliste pour BlueROV sans DVL/GPS dans une chambre de moulin :
|
||||
|
||||
Heading : fusion magnéto + gz par filtre complémentaire
|
||||
h_dr = alpha * (h_magneto) + (1-alpha) * (h_gyro_integré)
|
||||
→ alpha = 0.3 : le magnéto corrige lentement, le gyro intègre rapidement
|
||||
→ source de dérive : biais gz (0.003 rad/s) qui déplace h_gyro
|
||||
→ sur 120s : dérive cumulée ~4° → erreur transverse ~1-2 m à 0.3 m/s
|
||||
|
||||
Vitesse forward : estimée depuis ax par intégration sur fenêtre glissante
|
||||
→ biais ax (0.015 m/s²) intégré → erreur de vitesse → dérive position
|
||||
→ en pratique : v_dr ≠ v_vraie d'environ ±0.05 m/s
|
||||
|
||||
Résultat : DR dérive de 1-4 m sur 120s → ICP Ping360 corrige à chaque sweep.
|
||||
"""
|
||||
|
||||
def __init__(self, alpha_magneto: float = 0.3, tau: float = 2.0):
|
||||
"""
|
||||
alpha_magneto : poids du magnéto dans la fusion heading [0,1].
|
||||
0 = gyro pur (max dérive), 1 = magnéto pur (pas de dérive).
|
||||
0.3 = compromis réaliste (magnéto perturbé par métaux du moulin).
|
||||
tau : constante de temps amortissement vitesse (s).
|
||||
"""
|
||||
self.x = 0.0
|
||||
self.y = 0.0
|
||||
self.h_gyro = 0.0 # heading intégré depuis gz
|
||||
self.heading_deg = 0.0 # heading fusionné
|
||||
self.vf = 0.0 # vitesse forward estimée (corps, m/s)
|
||||
self.vl = 0.0 # vitesse latérale estimée (corps, m/s)
|
||||
self.t_prev = None
|
||||
self.alpha = alpha_magneto
|
||||
self.tau = tau
|
||||
|
||||
def reset(self, x: float = 0.0, y: float = 0.0, heading_deg: float = 0.0):
|
||||
self.x = x
|
||||
self.y = y
|
||||
self.h_gyro = heading_deg
|
||||
self.heading_deg = heading_deg
|
||||
self.vf = 0.0
|
||||
self.vl = 0.0
|
||||
self.t_prev = None
|
||||
|
||||
def update(self, t: float, heading_deg: float,
|
||||
ax: float, ay: float, az: float,
|
||||
gz: float = 0.0,
|
||||
vf: float = None, vl: float = 0.0) -> Tuple[float, float, float]:
|
||||
"""
|
||||
heading_deg : cap magnéto (bruité + dérive lente).
|
||||
gz : taux de lacet (rad/s), biaisé → dérive h_gyro.
|
||||
ax, ay : accélérations corps (option : non utilisées si vf fourni).
|
||||
vf, vl : vitesse corps forward/latérale mesurée (m/s).
|
||||
Si vf=None, on intègre ax (dérive plus rapide).
|
||||
|
||||
Sources de dérive DR :
|
||||
- biais gz → h_gyro dérive → mauvaise direction
|
||||
- biais vf → position dérive dans la mauvaise direction
|
||||
→ ICP corrige ces deux erreurs à chaque sweep
|
||||
"""
|
||||
if self.t_prev is None:
|
||||
self.t_prev = t
|
||||
self.h_gyro = heading_deg
|
||||
self.heading_deg = heading_deg
|
||||
return self.x, self.y, self.heading_deg
|
||||
|
||||
dt = t - self.t_prev
|
||||
if dt <= 0:
|
||||
return self.x, self.y, self.heading_deg
|
||||
self.t_prev = t
|
||||
|
||||
# --- Heading : filtre complémentaire gyro (principal) + magnéto (lent) ---
|
||||
# Intégration gz biaisée → h_gyro dérive
|
||||
self.h_gyro = (self.h_gyro + np.rad2deg(gz) * dt) % 360
|
||||
# Correction magnéto partielle (α faible = magnéto peu fiable dans moulin métallique)
|
||||
dh = ((heading_deg - self.h_gyro + 180) % 360) - 180
|
||||
self.h_gyro = (self.h_gyro + self.alpha * dh) % 360
|
||||
self.heading_deg = self.h_gyro
|
||||
|
||||
# --- Vitesse : utilise vf/vl si disponible, sinon intègre ax/ay ---
|
||||
if vf is not None:
|
||||
# Vitesse mesurée (biaisée) — source de dérive : biais vf
|
||||
self.vf = vf
|
||||
self.vl = vl
|
||||
else:
|
||||
# Intégration accéléro (dérive plus rapide)
|
||||
decay = np.exp(-dt / self.tau)
|
||||
self.vf = self.vf * decay + ax * dt
|
||||
self.vl = self.vl * decay + ay * dt
|
||||
|
||||
# --- Projection vitesse corps → monde ---
|
||||
h_rad = np.deg2rad(self.heading_deg)
|
||||
# avant ROV = (sin h, cos h) en ENU ; gauche = (-cos h, sin h)
|
||||
vx = self.vf * np.sin(h_rad) - self.vl * np.cos(h_rad)
|
||||
vy = self.vf * np.cos(h_rad) + self.vl * np.sin(h_rad)
|
||||
|
||||
self.x += vx * dt
|
||||
self.y += vy * dt
|
||||
|
||||
return self.x, self.y, self.heading_deg
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Gestion des sweeps Ping360
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class SweepAccumulator:
|
||||
"""
|
||||
Regroupe les faisceaux Ping360 en sweeps complets (0→360°).
|
||||
Un nouveau sweep démarre quand l'angle fait un tour complet.
|
||||
"""
|
||||
|
||||
def __init__(self, steps: int = 400):
|
||||
self.steps = steps
|
||||
self.current_sweep: List[Dict] = []
|
||||
self.last_angle = None
|
||||
self.sweeps_done = 0
|
||||
|
||||
def add_beam(self, angle_deg: float, distance: float, pose: Tuple[float, float, float]) -> Optional[List[Dict]]:
|
||||
"""
|
||||
Ajoute un faisceau. Si le sweep est complet (retour à 0°), retourne le sweep
|
||||
et commence le suivant. Sinon retourne None.
|
||||
|
||||
pose = (x, y, heading_deg)
|
||||
Fonctionne quel que soit le pas angulaire (1 pas ou 4.5° par record).
|
||||
"""
|
||||
beam = {"angle_deg": angle_deg, "distance": distance, "pose": pose}
|
||||
|
||||
# Détection fermeture de sweep : l'angle repasse vers 0 après avoir été >180
|
||||
if (self.last_angle is not None
|
||||
and self.last_angle > 180
|
||||
and angle_deg < 90):
|
||||
# Sweep complet — seuil minimal : au moins 20 faisceaux (tolère grand pas angulaire)
|
||||
completed = self.current_sweep.copy()
|
||||
self.current_sweep = [beam]
|
||||
self.last_angle = angle_deg
|
||||
self.sweeps_done += 1
|
||||
if len(completed) >= 20:
|
||||
return completed
|
||||
return None
|
||||
|
||||
self.current_sweep.append(beam)
|
||||
self.last_angle = angle_deg
|
||||
return None
|
||||
|
||||
|
||||
def sweep_to_world_points(sweep: List[Dict]) -> np.ndarray:
|
||||
"""
|
||||
Convertit un sweep (liste de faisceaux) en points murs dans le repère monde.
|
||||
Retourne un array (M, 2) de points (x, y), dropouts exclus.
|
||||
"""
|
||||
points = []
|
||||
for beam in sweep:
|
||||
d = beam["distance"]
|
||||
if d <= 0.01:
|
||||
continue # dropout
|
||||
|
||||
angle_rel = beam["angle_deg"]
|
||||
px, py, hdg = beam["pose"]
|
||||
|
||||
# angle monde = cap + angle_relatif (convention cap 0=Nord, CW)
|
||||
angle_world = (hdg + angle_rel) % 360.0
|
||||
a_rad = np.deg2rad(angle_world)
|
||||
|
||||
# Direction en ENU : cap 0°=+Y, 90°=+X
|
||||
wx = px + d * np.sin(a_rad)
|
||||
wy = py + d * np.cos(a_rad)
|
||||
points.append([wx, wy])
|
||||
|
||||
if not points:
|
||||
return np.zeros((0, 2))
|
||||
return np.array(points)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# ICP 2D scan-to-map
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
def icp_2d(
|
||||
source_pts: np.ndarray,
|
||||
map_pts: np.ndarray,
|
||||
max_iter: int = 20,
|
||||
tol: float = 1e-4,
|
||||
max_dist: float = 0.5,
|
||||
) -> Tuple[float, float, float, bool]:
|
||||
"""
|
||||
ICP 2D : recale source_pts sur map_pts.
|
||||
Retourne (dx, dy, d_heading_rad, converged).
|
||||
|
||||
Algorithme :
|
||||
1. Pour chaque point source, chercher le plus proche dans map (cKDTree)
|
||||
2. Estimer la transformation rigide 2D (rotation + translation) via SVD
|
||||
3. Appliquer et itérer jusqu'à convergence
|
||||
"""
|
||||
if len(source_pts) < 5 or len(map_pts) < 5:
|
||||
return 0.0, 0.0, 0.0, False
|
||||
|
||||
src = source_pts.copy()
|
||||
tree = cKDTree(map_pts)
|
||||
|
||||
dx_total = 0.0
|
||||
dy_total = 0.0
|
||||
dtheta_total = 0.0
|
||||
|
||||
for iteration in range(max_iter):
|
||||
# Nearest-neighbor
|
||||
dists, idxs = tree.query(src, k=1, workers=-1)
|
||||
|
||||
# Filtrer les appariements trop lointains
|
||||
mask = dists < max_dist
|
||||
if mask.sum() < 5:
|
||||
return dx_total, dy_total, dtheta_total, False
|
||||
|
||||
src_m = src[mask]
|
||||
dst_m = map_pts[idxs[mask]]
|
||||
|
||||
# Centroïdes
|
||||
c_src = src_m.mean(axis=0)
|
||||
c_dst = dst_m.mean(axis=0)
|
||||
|
||||
# Matrices centrées
|
||||
A = src_m - c_src
|
||||
B = dst_m - c_dst
|
||||
|
||||
# SVD pour trouver la rotation 2D optimale
|
||||
H = A.T @ B
|
||||
U, S, Vt = np.linalg.svd(H)
|
||||
R = Vt.T @ U.T
|
||||
|
||||
# Assure det(R) = +1 (rotation pure, pas de réflexion)
|
||||
if np.linalg.det(R) < 0:
|
||||
Vt[-1, :] *= -1
|
||||
R = Vt.T @ U.T
|
||||
|
||||
t = c_dst - R @ c_src
|
||||
|
||||
# Appliquer la transformation au nuage source
|
||||
src = (R @ src.T).T + t
|
||||
|
||||
# Accumuler la transformation totale
|
||||
dx_total += t[0]
|
||||
dy_total += t[1]
|
||||
dtheta_total += np.arctan2(R[1, 0], R[0, 0])
|
||||
|
||||
# Convergence
|
||||
if np.abs(t).max() < tol and abs(np.arctan2(R[1, 0], R[0, 0])) < tol:
|
||||
return dx_total, dy_total, dtheta_total, True
|
||||
|
||||
return dx_total, dy_total, dtheta_total, True
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Carte globale incrémentale
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class IncrementalMap:
|
||||
"""
|
||||
Carte 2D des murs, stockée comme nuage de points.
|
||||
Décimation par voxel pour éviter la croissance infinie.
|
||||
"""
|
||||
|
||||
def __init__(self, voxel_size: float = 0.05):
|
||||
self.points: List[np.ndarray] = []
|
||||
self.voxel_size = voxel_size
|
||||
self._voxel_set = set()
|
||||
|
||||
def add_points(self, pts: np.ndarray) -> None:
|
||||
"""Ajoute des points, en filtrant les doublons par voxel."""
|
||||
for pt in pts:
|
||||
key = (int(pt[0] / self.voxel_size), int(pt[1] / self.voxel_size))
|
||||
if key not in self._voxel_set:
|
||||
self._voxel_set.add(key)
|
||||
self.points.append(pt)
|
||||
|
||||
def get_array(self) -> np.ndarray:
|
||||
if not self.points:
|
||||
return np.zeros((0, 2))
|
||||
return np.array(self.points)
|
||||
|
||||
def __len__(self):
|
||||
return len(self.points)
|
||||
|
||||
|
||||
# ---------------------------------------------------------------------------
|
||||
# Détection et correction de fermeture de boucle
|
||||
# ---------------------------------------------------------------------------
|
||||
|
||||
class LoopClosureDetector:
|
||||
"""
|
||||
Détecte quand le ROV revient près d'une zone déjà visitée.
|
||||
Stratégie simple : on compare la position courante aux positions passées.
|
||||
Si distance < seuil et qu'un temps minimum s'est écoulé → boucle détectée.
|
||||
"""
|
||||
|
||||
def __init__(self, min_time: float = 15.0, dist_thresh: float = 0.8):
|
||||
self.min_time = min_time # on ne cherche pas dans les N dernières secondes
|
||||
self.dist_thresh = dist_thresh
|
||||
self.history: List[Tuple[float, float, float, int]] = [] # (t, x, y, sweep_idx)
|
||||
|
||||
def add_pose(self, t: float, x: float, y: float, sweep_idx: int) -> None:
|
||||
self.history.append((t, x, y, sweep_idx))
|
||||
|
||||
def check(self, t: float, x: float, y: float) -> Optional[int]:
|
||||
"""
|
||||
Retourne l'index sweep de la pose passée si boucle détectée, sinon None.
|
||||
"""
|
||||
for (t_old, x_old, y_old, idx_old) in self.history:
|
||||
if t - t_old < self.min_time:
|
||||
continue
|
||||
dist = np.sqrt((x - x_old)**2 + (y - y_old)**2)
|
||||
if dist < self.dist_thresh:
|
||||
return idx_old
|
||||
return None
|
||||
|
||||
|
||||
def apply_loop_closure(
|
||||
trajectory: List[Tuple[float, float, float, float]], # (t, x, y, h)
|
||||
loop_start_idx: int,
|
||||
loop_end_idx: int,
|
||||
correction: np.ndarray, # [dx, dy]
|
||||
) -> List[Tuple[float, float, float, float]]:
|
||||
"""
|
||||
Redistribue linéairement la correction de fermeture de boucle
|
||||
sur les poses entre loop_start_idx et loop_end_idx.
|
||||
|
||||
C'est une simplification de la correction graph-SLAM (pas de g2o).
|
||||
"""
|
||||
n_loop = loop_end_idx - loop_start_idx
|
||||
if n_loop <= 0:
|
||||
return trajectory
|
||||
|
||||
corrected = list(trajectory)
|
||||
for i in range(loop_start_idx, loop_end_idx + 1):
|
||||
alpha = (i - loop_start_idx) / n_loop
|
||||
t, x, y, h = corrected[i]
|
||||
corrected[i] = (t,
|
||||
x + alpha * correction[0],
|
||||
y + alpha * correction[1],
|
||||
h)
|
||||
return corrected
|
||||
80
pipeline/stream_replay.py
Normal file
80
pipeline/stream_replay.py
Normal file
@@ -0,0 +1,80 @@
|
||||
"""
|
||||
stream_replay.py — Rejoue un fichier JSONL sur stdout avec timing optionnel.
|
||||
|
||||
Simule le streaming live d'un capteur ROV.
|
||||
Peut se chaîner avec process.py :
|
||||
python3 stream_replay.py | python3 process.py --stdin
|
||||
python3 stream_replay.py --realtime | python3 process.py --stdin
|
||||
|
||||
Usage :
|
||||
python3 stream_replay.py [--input ../data/sim/run_L.jsonl] [--realtime] [--speed 10.0]
|
||||
"""
|
||||
|
||||
import argparse
|
||||
import json
|
||||
import sys
|
||||
import time
|
||||
import os
|
||||
|
||||
|
||||
def replay(
|
||||
input_path: str,
|
||||
realtime: bool = False,
|
||||
speed: float = 1.0,
|
||||
output=None,
|
||||
):
|
||||
"""
|
||||
Lit le JSONL et écrit sur stdout (ou output) ligne par ligne.
|
||||
Si realtime=True, attend le bon délai entre les lignes.
|
||||
speed > 1 accélère le replay (utile pour tests).
|
||||
"""
|
||||
if output is None:
|
||||
output = sys.stdout
|
||||
|
||||
prev_t = None
|
||||
n = 0
|
||||
|
||||
with open(input_path) as f:
|
||||
for line in f:
|
||||
line = line.strip()
|
||||
if not line:
|
||||
continue
|
||||
|
||||
if realtime:
|
||||
try:
|
||||
rec = json.loads(line)
|
||||
t = rec.get("t", 0.0)
|
||||
if prev_t is not None:
|
||||
dt = (t - prev_t) / speed
|
||||
if dt > 0:
|
||||
time.sleep(dt)
|
||||
prev_t = t
|
||||
except json.JSONDecodeError:
|
||||
pass
|
||||
|
||||
output.write(line + "\n")
|
||||
output.flush()
|
||||
n += 1
|
||||
|
||||
# Signal de fin (ligne vide → process.py ignore)
|
||||
sys.stderr.write(f"[stream_replay] {n} lignes émises\n")
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
parser = argparse.ArgumentParser(description="Replay JSONL stream")
|
||||
parser.add_argument("--input", default="../data/sim/run_L.jsonl")
|
||||
parser.add_argument("--realtime", action="store_true",
|
||||
help="Respecter les délais temporels")
|
||||
parser.add_argument("--speed", type=float, default=1.0,
|
||||
help="Facteur d'accélération (ex: 10 = 10x plus rapide)")
|
||||
args = parser.parse_args()
|
||||
|
||||
if not os.path.exists(args.input):
|
||||
sys.stderr.write(f"ERREUR : fichier introuvable : {args.input}\n")
|
||||
sys.exit(1)
|
||||
|
||||
replay(
|
||||
input_path=args.input,
|
||||
realtime=args.realtime,
|
||||
speed=args.speed,
|
||||
)
|
||||
Reference in New Issue
Block a user