# cosma-nav

Pipeline d'extraction, fusion et visualisation de trajectoires AUV/USV pour COSMA.

Fusionne MCAP ROS2 (AUV) + logs CSV USV (GPS/USBL Kogger) + poses visuelles lingbot-map
via alignement Umeyama → trajectoire géoréférencée → viewer Three.js.

## Installation

```bash
python -m venv .venv
source .venv/bin/activate  # Windows: .venv\Scripts\activate
pip install -r requirements.txt
```

## Utilisation

### 1. Inspecter les données (discovery)

```bash
python extract/inspect_topics.py  data/bags/      # topics MCAP
python extract/inspect_usv_csv.py data/SHIP/      # format CSV USV
```

### 2. Extraire les sources

```bash
python extract/extract_mcap.py   data/bags/  data/sparse_fixes.h5
python extract/parse_usv_gps.py  data/SHIP/  data/sparse_fixes.h5
python extract/decode_kogger.py  data/SHIP/  data/sparse_fixes.h5
```

### 3. Fusionner (requiert lingbot_poses.npz — voir Plan 2)

```bash
python fuse/fuse_trajectory.py \
  data/sparse_fixes.h5 \
  data/lingbot_poses.npz \
  data/trajectory_world.h5
```

### 4. Visualiser

```bash
python viz/server.py \
  --trajectory data/trajectory_world.h5 \
  --fixes data/sparse_fixes.h5 \
  --port 5051
# → http://localhost:5051
```

Avec PLY décimé (optionnel) :
```bash
python viz/server.py --ply data/scene.ply ...
```

## Structure HDF5

### sparse_fixes.h5
| Groupe | Datasets | Notes |
|--------|----------|-------|
| /usv_gps | t_ns, easting, northing, rtk_status | UTM, attribut utm_zone |
| /auv_mcap | t_ns, lat, lon, depth_m | lat/lon=0 si AUV sous l'eau |
| /usbl_fixes | t_ns, north_m, east_m, depth_m | Kogger binaire décodé |

### trajectory_world.h5
| Groupe | Contenu |
|--------|---------|
| /alignment | scale, R(3×3), t(3,), rmse_m |
| /poses_world | t_ns, x_m, y_m, z_m, T_4x4 |

## Architecture

```
extract/
  inspect_topics.py    # Liste topics MCAP
  inspect_usv_csv.py   # Inspecte CSV USV
  extract_mcap.py      # MCAP → HDF5 /auv_mcap
  parse_usv_gps.py     # navigation_log.csv → HDF5 /usv_gps
  decode_kogger.py     # USBL binaire Kogger → HDF5 /usbl_fixes
fuse/
  fuse_trajectory.py   # Umeyama alignment lingbot+fixes → trajectory_world.h5
viz/
  server.py            # Flask :5051
  static/trajectory.html + js/scene.js  # Three.js viewer
```

## Notes données La Ciotat (2026-04-08)

Les logs complets (MCAP + USBL avec fixes acoustiques) sont sur z620 (`~/logs/`).
Les bags locaux (`bags/20260407_171848_AUV009/`) sont de la mission 7 avril sans lock GPS AUV.
Les fixes USBL acoustiques nécessitent que le transponder réponde — vérifier le déploiement.

## Plan 2 — Patches cosma-qc

- `lingbot/demo.py --save_poses` → génère `lingbot_poses.npz`
- Stitcher guidé par `trajectory_world.h5` (remplace RANSAC)