85e9a4d4b0
- 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)
197 lines
6.3 KiB
Python
197 lines
6.3 KiB
Python
"""
|
||
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})")
|