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
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})")
|
||||
Reference in New Issue
Block a user