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
4225
data/out/cloud.ply
Normal file
4225
data/out/cloud.ply
Normal file
File diff suppressed because it is too large
Load Diff
2110
data/out/map_2d.csv
Normal file
2110
data/out/map_2d.csv
Normal file
File diff suppressed because it is too large
Load Diff
31
data/out/trajectory.csv
Normal file
31
data/out/trajectory.csv
Normal file
@@ -0,0 +1,31 @@
|
|||||||
|
t,x_dr,y_dr,x_corr,y_corr
|
||||||
|
4.0000,0.0052,1.2770,0.0052,1.2770
|
||||||
|
8.0000,0.0099,2.5682,0.0186,2.5028
|
||||||
|
12.0000,0.0150,3.7379,0.0032,3.5944
|
||||||
|
16.0000,-0.0363,3.7869,-0.0636,3.5496
|
||||||
|
20.0000,-1.2863,3.7897,-1.2616,3.5461
|
||||||
|
24.0000,-2.5763,3.7952,-2.4599,3.5471
|
||||||
|
28.0000,-2.7656,3.8377,-2.6040,3.5255
|
||||||
|
32.0000,-2.7679,4.8392,-2.5176,4.4853
|
||||||
|
36.0000,-2.7505,6.1210,-2.5054,5.5785
|
||||||
|
40.0000,-2.7465,7.0774,-2.4440,6.4784
|
||||||
|
44.0000,-2.8020,7.1050,-2.3972,6.2123
|
||||||
|
48.0000,-2.8469,7.0197,-2.4795,6.0262
|
||||||
|
52.0000,-2.8601,5.7395,-2.5417,4.6874
|
||||||
|
56.0000,-2.8680,4.4624,-2.5370,3.4324
|
||||||
|
60.0000,-2.8839,3.1747,-2.3394,2.7384
|
||||||
|
64.0000,-2.8918,1.8854,-2.4141,1.5799
|
||||||
|
68.0000,-2.8906,1.1199,-2.4494,0.8736
|
||||||
|
72.0000,-2.4417,1.0912,-2.0733,1.0443
|
||||||
|
76.0000,-1.1468,1.0653,-0.8350,1.0251
|
||||||
|
80.0000,0.1292,1.0489,0.3113,0.9984
|
||||||
|
84.0000,1.4097,1.0307,1.5811,0.9940
|
||||||
|
88.0000,1.4998,1.0806,1.6091,1.0336
|
||||||
|
92.0000,1.5174,2.1988,1.5662,2.1307
|
||||||
|
96.0000,1.5338,3.4865,1.6061,3.3106
|
||||||
|
100.0000,1.5112,3.8156,1.5762,3.6191
|
||||||
|
104.0000,0.6588,3.8443,0.8076,3.6364
|
||||||
|
108.0000,-0.1266,3.8464,0.0730,3.6349
|
||||||
|
112.0000,-0.1646,3.4383,0.1422,3.2418
|
||||||
|
116.0000,-0.1861,2.1536,0.1467,1.9940
|
||||||
|
120.0000,-0.2090,0.8700,0.1515,0.7458
|
||||||
|
2439
data/sim/run_L.jsonl
Normal file
2439
data/sim/run_L.jsonl
Normal file
File diff suppressed because it is too large
Load Diff
2440
data/sim/run_L_truth.csv
Normal file
2440
data/sim/run_L_truth.csv
Normal file
File diff suppressed because it is too large
Load Diff
BIN
docs/validation.png
Normal file
BIN
docs/validation.png
Normal file
Binary file not shown.
|
After Width: | Height: | Size: 126 KiB |
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