Merge pull request 'fix #1: calibration stéréo HD-3000 (YUYV/V4L2, ORB-SLAM3 YAML, gate RMS<0.5px)' (#2) from fix/issue-1 into master

.gitignore

@@ -47,8 +47,13 @@ datasets/*
 !datasets/.gitkeep
 !datasets/README.md
 
-# Calibration output (généré)
+# Captures brutes de calibration (issue #1) — JAMAIS commit (trop volumineux)
+datasets/calib_raw/
+
+# Anciennes sorties de calibration (legacy, généré localement)
 config/stereo_calib.yaml
+# Note: config/stereo_params.yaml et config/calibration_report.md
+# SONT commités (livrables issue #1) une fois la calibration validée (RMS<0.5px).
 
 # Sphinx build
 docs/build/

docs/source/calibration.rst

@@ -7,92 +7,129 @@ Matériel nécessaire
 - Damier imprimé **9×6 cases** (intérieur), case **25 mm**
 - Support rigide (plastifié recommandé)
 - Éclairage uniforme sans reflets
+- 2× Microsoft LifeCam HD-3000 sur ``/dev/video0`` (gauche) et
+  ``/dev/video2`` (droite), baseline cible **110 mm**
 
 .. warning::
    Imprimer sur fond blanc mat. Plastifier sans brillance pour éviter
    les reflets. Ne pas utiliser d'écran LCD (déformation optique).
 
-Procédure
+.. note::
----------
+   Les HD-3000 ne supportent **pas** le format MJPG. La capture est forcée
+   en **YUYV 640×480 @ 30 fps** (cf. ``cap.set(CAP_PROP_FOURCC, "YUYV")``).
+   La résolution doit être appliquée *après* le fourcc, sinon V4L2 l'ignore.
 
-Étape 1 — Capture des paires
+Procédure (issue #1)
-~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+--------------------
+
+Étape 0 — Déploiement sur le Pi
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
 
 .. code-block:: bash
 
-   python src/calibration/stereo_capture.py
+   bash scripts/deploy_pi.sh 192.168.0.174
 
-- Lance les 2 webcams simultanément (USB 0 et 1)
+Étape 1 — Capture des paires (sur le Pi, en SSH)
-- Appuyer sur ``ESPACE`` pour capturer une paire
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
-- Objectif : **30 paires valides** minimum
-- Varier : distance (0.3–1.5 m), inclinaison (±30°), position dans frame
-
-.. tip::
-   Couvrir les 4 coins du champ + centre. Inclure des poses inclinées
-   pour mieux contraindre les coefficients de distorsion tangentielle.
-
-Étape 2 — Calibration
-~~~~~~~~~~~~~~~~~~~~~~
 
 .. code-block:: bash
 
-   python src/calibration/stereo_calibrate.py
+   ssh pi@192.168.0.174
+   cd ~/slam_stereo
+   # Mode interactif (avec écran X11 forwardé) :
+   python3 src/calibration/stereo_capture.py
+   # OU mode headless (recommandé en SSH, pas d'écran) :
+   python3 src/calibration/stereo_capture.py --headless --count 30 --interval 2.0
 
-Résultat attendu :
+- Indices V4L2 par défaut : gauche = ``0``, droite = ``2``
+  (override : ``--left N --right M``)
+- Sorties dans ``datasets/calib_raw/`` (gitignored)
+- Objectif : **≥ 25 paires valides** (varier distance 0.3–1.5 m, inclinaison ±30°,
+  position dans le frame, couvrir les 4 coins + centre)
+- Alim USB du Pi flaky : éviter les sessions longues, libérer les caméras
+  entre les runs (``cap.release()`` est appelé dans ``finally``).
+
+Étape 2 — Calibration (sur le Pi ou rapatrié sur PC)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+.. code-block:: bash
+
+   python3 src/calibration/stereo_calibrate.py
+
+Sortie attendue :
 
 .. code-block:: text
 
-   Chargement 30 paires de damier...
+   [INFO] Chargement 30 paires depuis datasets/calib_raw
-   Calibration stéréo...
+   [INFO] Gauche: 28/30 images valides
-   RMS reprojection error: 0.42 px  ← bon si < 0.8 px
+   [INFO] Droite: 27/30 images valides
-   Sauvegarde: config/stereo_calib.yaml
+   [INFO] 26 paires valides pour calibration stéréo
+   [INFO]   RMS gauche = 0.31 px
+   [INFO]   RMS droite = 0.34 px
+   [INFO] RMS stéréo global = 0.42 px
+   [OK] YAML ORB-SLAM3 → config/stereo_params.yaml
+   [OK] Rapport → config/calibration_report.md
+   [OK] RMS stéréo 0.4200 < 0.5 px ✓
 
-Fichier YAML généré
+**Gate** : si RMS stéréo > 0.5 px → exit-code 2 (``[STOP]``). Recapturer
--------------------
+la mire (mauvais positionnement, flou de bougé, cams sous-focales).
+Le bypass ``--no-gate`` existe mais ne dispense pas de re-capturer.
 
-``config/stereo_calib.yaml`` contient :
+Livrables (commit après validation)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+- ``config/stereo_params.yaml`` — format ORB-SLAM3 stéréo
+  (``Camera1.fx``, ``…``, ``Stereo.T_c1_c2``, ``Stereo.bf``, ``IMU.T_b_c1``…)
+- ``config/calibration_report.md`` — RMS per-cam + RMS stéréo + baseline mesurée
+
+Format YAML ORB-SLAM3 généré
+----------------------------
 
 .. code-block:: yaml
 
-   K1: [[fx, 0, cx], [0, fy, cy], [0, 0, 1]]   # caméra gauche
+   %YAML:1.0
-   D1: [k1, k2, p1, p2, k3]                      # distorsion gauche
+   File.version: "1.0"
-   K2: [[fx, 0, cx], [0, fy, cy], [0, 0, 1]]   # caméra droite
+   Camera.type: "PinHole"
-   D2: [k1, k2, p1, p2, k3]                      # distorsion droite
-   R:  [[...], [...], [...]]                       # rotation C2→C1
-   T:  [tx, ty, tz]                               # translation (baseline)
-   E:  [[...]]                                    # matrice essentielle
-   F:  [[...]]                                    # matrice fondamentale
-   R1, P1, R2, P2, Q                              # rectification
 
-Paramètres OpenCV
+   Camera1.fx: 525.123    # cam gauche
------------------
+   Camera1.fy: 524.987
+   Camera1.cx: 319.876
+   Camera1.cy: 239.234
+   Camera1.k1: -0.05432   # distorsion Plumb-Bob (4 coefs)
+   Camera1.k2:  0.00821
+   Camera1.p1:  0.00012
+   Camera1.p2: -0.00045
 
-``stereoCalibrate`` avec flags :
+   Camera2.fx: 525.456    # cam droite
+   # …
 
-.. code-block:: python
+   Camera.width:  640
+   Camera.height: 480
+   Camera.fps:    30
 
-   flags = (
+   Stereo.T_c1_c2: !!opencv-matrix     # transfo cam_droite → cam_gauche
-       cv2.CALIB_FIX_INTRINSIC       # si calibration individuelle faite avant
+      rows: 4
-       # ou cv2.CALIB_RATIONAL_MODEL  # pour modèle 8 coefs
+      cols: 4
-   )
+      dt: f
-   criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 1e-5)
+      data: [...]
+   Stereo.b:  0.110000     # baseline (m)
+   Stereo.bf: 57.7600      # baseline * fx
+   Stereo.ThDepth: 40.0
 
-``stereoRectify`` avec ``alpha=0`` (recadrage maximal, pas de bandes noires).
+   IMU.T_b_c1: !!opencv-matrix         # placeholder identité
+      rows: 4
+      cols: 4
+      dt: f
+      data: [1,0,0,0, 0,1,0,0, 0,0,1,0, 0,0,0,1]
 
 Validation
 ----------
 
-Après calibration, vérifier :
+1. Erreur RMS stéréo **< 0.5 px** (gate strict, issue #1)
-
-1. Erreur RMS < 0.8 px (idéalement < 0.5 px)
 2. Lignes épipolaires horizontales sur image rectifiée
-3. Translation T ≈ [−0.11, 0, 0] m (baseline sur axe X)
+   (``python3 src/calibration/stereo_calibrate.py --show-rectified``)
-4. Épipole à l'infini (caméras parallèles après rectif)
+3. Translation ``T ≈ [-0.110, 0, 0]`` m (baseline sur axe X)
-
+4. Épipole à l'infini après rectification (caméras parallèles)
-.. code-block:: bash
-
-   # Visualisation rectification
-   python src/calibration/stereo_calibrate.py --show-rectified
 
 Références
 ----------

src/calibration/stereo_calibrate.py

@@ -1,36 +1,49 @@
 #!/usr/bin/env python3
 """
-Calibration stéréo à partir de paires d'images de damier.
+Calibration stéréo OpenCV → format ORB-SLAM3.
-Damier: 9x6 cases intérieures, case 25mm.
+
-Sortie: config/stereo_calib.yaml
+Damier : 9×6 coins intérieurs, case 25 mm.
+
+Sorties :
+    config/stereo_params.yaml      — fichier ORB-SLAM3 (Camera.fx, …, Tbc, …)
+    config/calibration_report.md   — RMS per-cam + RMS stéréo + baseline mesurée
+
+Gate (issue #1) : exit-code 2 si RMS stéréo > 0.5 px.
 """
 
+import argparse
+import datetime as _dt
+import glob
+import os
+import sys
+
 import cv2
 import numpy as np
 import yaml
-import os
-import sys
-import argparse
-import glob
 
-# Paramètres du damier
+# --- Damier ---------------------------------------------------------------
-CHESSBOARD_SIZE = (9, 6)   # coins intérieurs (colonnes, lignes)
+CHESSBOARD_SIZE = (9, 6)        # coins intérieurs (cols, rows)
-SQUARE_SIZE_MM = 25.0       # taille d'une case en mm
+SQUARE_SIZE_MM = 25.0
 SQUARE_SIZE_M = SQUARE_SIZE_MM / 1000.0
 
-# Chemins
+# --- Chemins par défaut ---------------------------------------------------
 SCRIPT_DIR = os.path.dirname(os.path.abspath(__file__))
-CALIB_IMAGES_DIR = os.path.join(SCRIPT_DIR, "calib_images")
+REPO_ROOT = os.path.abspath(os.path.join(SCRIPT_DIR, "..", ".."))
-CONFIG_DIR = os.path.join(SCRIPT_DIR, "..", "..", "config")
+DEFAULT_IMAGES_DIR = os.path.join(REPO_ROOT, "datasets", "calib_raw")
-OUTPUT_YAML = os.path.join(CONFIG_DIR, "stereo_calib.yaml")
+CONFIG_DIR = os.path.join(REPO_ROOT, "config")
+DEFAULT_OUTPUT_YAML = os.path.join(CONFIG_DIR, "stereo_params.yaml")
+DEFAULT_REPORT_MD = os.path.join(CONFIG_DIR, "calibration_report.md")
 
-# Critères de terminaison pour cornerSubPix
+# --- Critères d'optim -----------------------------------------------------
 SUBPIX_CRITERIA = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
 STEREO_CRITERIA = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 1e-5)
 
+# Gate RMS (issue #1)
+RMS_GATE_PX = 0.5
 
-def prepare_object_points():
+
-    """Génère les coordonnées 3D du damier (plan Z=0)."""
+# -------------------------------------------------------------------------
+def prepare_object_points() -> np.ndarray:
     objp = np.zeros((CHESSBOARD_SIZE[0] * CHESSBOARD_SIZE[1], 3), np.float32)
     objp[:, :2] = np.mgrid[
         0:CHESSBOARD_SIZE[0], 0:CHESSBOARD_SIZE[1]
@@ -38,115 +51,95 @@ def prepare_object_points():
     return objp
 
 
-def load_pairs():
+def load_pairs(images_dir: str):
-    """Charge les paires d'images gauche/droite."""
+    lefts = sorted(glob.glob(os.path.join(images_dir, "left_*.png")))
-    lefts = sorted(glob.glob(os.path.join(CALIB_IMAGES_DIR, "left_*.png")))
+    rights = sorted(glob.glob(os.path.join(images_dir, "right_*.png")))
-    rights = sorted(glob.glob(os.path.join(CALIB_IMAGES_DIR, "right_*.png")))
+    if not lefts or not rights:
-
+        print(f"[ERREUR] Aucune image dans {images_dir}", file=sys.stderr)
-    if len(lefts) == 0 or len(rights) == 0:
-        print(f"[ERREUR] Aucune image dans {CALIB_IMAGES_DIR}", file=sys.stderr)
         print("[ERREUR] Lancer d'abord stereo_capture.py", file=sys.stderr)
         sys.exit(1)
-
     if len(lefts) != len(rights):
         print(f"[WARN] {len(lefts)} gauche vs {len(rights)} droite — utilise le min")
-
     n = min(len(lefts), len(rights))
-    print(f"[INFO] Chargement {n} paires de damier...")
+    print(f"[INFO] Chargement {n} paires depuis {images_dir}")
     return lefts[:n], rights[:n]
 
 
 def find_corners(image_paths, label=""):
-    """Détecte les coins du damier dans une liste d'images."""
+    """Détecte coins du damier. Retourne aussi rms par-image pas calculé ici."""
     objp = prepare_object_points()
-    obj_points = []   # 3D
+    obj_points, img_points, valid_indices = [], [], []
-    img_points = []   # 2D
-    valid_indices = []
     img_size = None
 
+    flags = (cv2.CALIB_CB_ADAPTIVE_THRESH
+             + cv2.CALIB_CB_NORMALIZE_IMAGE
+             + cv2.CALIB_CB_FAST_CHECK)
+
     for i, path in enumerate(image_paths):
         img = cv2.imread(path)
         if img is None:
-            print(f"[WARN] Impossible de lire: {path}")
+            print(f"[WARN] Lecture impossible: {path}")
             continue
-
         gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
-        img_size = gray.shape[::-1]  # (width, height)
+        img_size = gray.shape[::-1]
-
+        found, corners = cv2.findChessboardCorners(gray, CHESSBOARD_SIZE, flags)
-        ret, corners = cv2.findChessboardCorners(
+        if found:
-            gray, CHESSBOARD_SIZE,
+            corners = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
-            cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
+                                       SUBPIX_CRITERIA)
-        )
-
-        if ret:
-            corners_refined = cv2.cornerSubPix(
-                gray, corners, (11, 11), (-1, -1), SUBPIX_CRITERIA
-            )
             obj_points.append(objp)
-            img_points.append(corners_refined)
+            img_points.append(corners)
             valid_indices.append(i)
         else:
-            print(f"[WARN] {label} image {i:04d}: damier non détecté")
+            print(f"[WARN] {label} #{i:04d}: damier non détecté")
 
     print(f"[INFO] {label}: {len(obj_points)}/{len(image_paths)} images valides")
     return obj_points, img_points, img_size, valid_indices
 
 
 def calibrate_stereo(lefts, rights):
-    """Calibre le système stéréo."""
     obj_l, pts_l, size_l, idx_l = find_corners(lefts, "Gauche")
     obj_r, pts_r, size_r, idx_r = find_corners(rights, "Droite")
 
+    if size_l is None or size_r is None:
+        print("[ERREUR] Aucune image lisible.", file=sys.stderr)
+        sys.exit(1)
     if size_l != size_r:
         print("[ERREUR] Taille images gauche/droite différente", file=sys.stderr)
         sys.exit(1)
+    img_size = size_l  # (w, h)
 
-    img_size = size_l
+    # Pairs où damier détecté des deux côtés
-
+    valid_pairs = sorted(set(idx_l) & set(idx_r))
-    # Garder seulement les paires où les 2 caméras détectent le damier
+    map_l = {v: i for i, v in enumerate(idx_l)}
-    valid_pairs = set(idx_l) & set(idx_r)
+    map_r = {v: i for i, v in enumerate(idx_r)}
-    idx_map_l = {v: i for i, v in enumerate(idx_l)}
+    obj_pts, pts_left, pts_right = [], [], []
-    idx_map_r = {v: i for i, v in enumerate(idx_r)}
+    for k in valid_pairs:
-
+        obj_pts.append(obj_l[map_l[k]])
-    obj_pts = []
+        pts_left.append(pts_l[map_l[k]])
-    pts_left = []
+        pts_right.append(pts_r[map_r[k]])
-    pts_right = []
-    for idx in sorted(valid_pairs):
-        obj_pts.append(obj_l[idx_map_l[idx]])
-        pts_left.append(pts_l[idx_map_l[idx]])
-        pts_right.append(pts_r[idx_map_r[idx]])
-
     print(f"[INFO] {len(obj_pts)} paires valides pour calibration stéréo")
 
     if len(obj_pts) < 10:
-        print("[ERREUR] Moins de 10 paires valides. Recapturer.", file=sys.stderr)
+        print("[ERREUR] Moins de 10 paires valides — recapturer.", file=sys.stderr)
         sys.exit(1)
 
-    # Calibration individuelle d'abord
+    print("[INFO] Calibration intrinsèque caméra GAUCHE…")
-    print("[INFO] Calibration caméra gauche...")
+    rms_l, K1, D1, _, _ = cv2.calibrateCamera(obj_pts, pts_left, img_size, None, None)
-    _, K1, D1, _, _ = cv2.calibrateCamera(obj_pts, pts_left, img_size, None, None)
+    print(f"[INFO]   RMS gauche = {rms_l:.4f} px")
 
-    print("[INFO] Calibration caméra droite...")
+    print("[INFO] Calibration intrinsèque caméra DROITE…")
-    _, K2, D2, _, _ = cv2.calibrateCamera(obj_pts, pts_right, img_size, None, None)
+    rms_r, K2, D2, _, _ = cv2.calibrateCamera(obj_pts, pts_right, img_size, None, None)
+    print(f"[INFO]   RMS droite = {rms_r:.4f} px")
 
-    # Calibration stéréo
+    print("[INFO] Calibration stéréo…")
-    print("[INFO] Calibration stéréo...")
-    flags = cv2.CALIB_FIX_INTRINSIC
     rms, K1, D1, K2, D2, R, T, E, F = cv2.stereoCalibrate(
         obj_pts, pts_left, pts_right,
-        K1, D1, K2, D2,
+        K1, D1, K2, D2, img_size,
-        img_size,
         criteria=STEREO_CRITERIA,
-        flags=flags
+        flags=cv2.CALIB_FIX_INTRINSIC,
     )
+    print(f"[INFO] RMS stéréo global = {rms:.4f} px")
 
-    print(f"[INFO] RMS reprojection error: {rms:.4f} px")
-    if rms > 1.0:
-        print("[WARN] RMS > 1.0 px — reconsidérer la calibration")
-    elif rms < 0.5:
-        print("[OK] RMS excellent (< 0.5 px)")
-
-    # Rectification
     R1, R2, P1, P2, Q, roi_l, roi_r = cv2.stereoRectify(
         K1, D1, K2, D2, img_size, R, T, alpha=0
     )
@@ -155,40 +148,153 @@ def calibrate_stereo(lefts, rights):
         "K1": K1, "D1": D1, "K2": K2, "D2": D2,
         "R": R, "T": T, "E": E, "F": F,
         "R1": R1, "R2": R2, "P1": P1, "P2": P2, "Q": Q,
+        "roi_l": roi_l, "roi_r": roi_r,
         "img_size": img_size,
-        "rms": rms
+        "rms_left": float(rms_l),
+        "rms_right": float(rms_r),
+        "rms_stereo": float(rms),
+        "n_pairs": len(obj_pts),
     }
 
 
-def save_yaml(calib, path):
+# -------------------------------------------------------------------------
-    """Sauvegarde les paramètres en YAML."""
+def _flow_seq(seq):
+    return yaml.dump(seq, default_flow_style=True).strip()
+
+
+def write_orbslam3_yaml(calib: dict, path: str) -> None:
+    """Écrit un YAML compatible ORB-SLAM3 (Stereo)."""
+    K1, D1, K2, D2 = calib["K1"], calib["D1"], calib["K2"], calib["D2"]
+    R, T = calib["R"], calib["T"]
+    P2 = calib["P2"]
+    w, h = calib["img_size"]
+
+    # ORB-SLAM3 attend D = [k1, k2, p1, p2] (modèle Plumb-Bob 4 coefs).
+    # OpenCV renvoie (1,5)=[k1,k2,p1,p2,k3]. On garde les 4 premiers.
+    d1 = D1.reshape(-1).tolist()
+    d2 = D2.reshape(-1).tolist()
+    d1 = (d1 + [0.0] * 4)[:4]
+    d2 = (d2 + [0.0] * 4)[:4]
+
+    # bf = baseline * fx (utilisé pour la profondeur). bf = -P2[0,3].
+    fx_left = float(K1[0, 0])
+    bf = float(-P2[0, 3])  # = baseline * fx_rectifié
+
+    # Tbc = transfo body→camera_left (identité par défaut, à ajuster avec IMU)
+    Tbc = np.eye(4).tolist()
+
     os.makedirs(os.path.dirname(path), exist_ok=True)
-
-    def mat_to_list(m):
-        return m.tolist()
-
-    data = {
-        "rms_reprojection_error": float(calib["rms"]),
-        "image_size": list(calib["img_size"]),
-        "K1": mat_to_list(calib["K1"]),
-        "D1": mat_to_list(calib["D1"]),
-        "K2": mat_to_list(calib["K2"]),
-        "D2": mat_to_list(calib["D2"]),
-        "R": mat_to_list(calib["R"]),
-        "T": mat_to_list(calib["T"]),
-        "E": mat_to_list(calib["E"]),
-        "F": mat_to_list(calib["F"]),
-        "R1": mat_to_list(calib["R1"]),
-        "R2": mat_to_list(calib["R2"]),
-        "P1": mat_to_list(calib["P1"]),
-        "P2": mat_to_list(calib["P2"]),
-        "Q": mat_to_list(calib["Q"]),
-    }
-
     with open(path, "w") as f:
-        yaml.dump(data, f, default_flow_style=False)
+        f.write("%YAML:1.0\n")
+        f.write("# Stereo calibration for ORB-SLAM3\n")
+        f.write("# Generated by src/calibration/stereo_calibrate.py (issue #1)\n")
+        f.write(f"# Date: {_dt.datetime.utcnow().isoformat()}Z\n\n")
 
-    print(f"[OK] Sauvegarde: {path}")
+        f.write("File.version: \"1.0\"\n\n")
+        f.write("Camera.type: \"PinHole\"\n\n")
+
+        f.write("# --- Caméra gauche (intrinsèques) ---\n")
+        f.write(f"Camera1.fx: {K1[0,0]:.6f}\n")
+        f.write(f"Camera1.fy: {K1[1,1]:.6f}\n")
+        f.write(f"Camera1.cx: {K1[0,2]:.6f}\n")
+        f.write(f"Camera1.cy: {K1[1,2]:.6f}\n")
+        f.write(f"Camera1.k1: {d1[0]:.8f}\n")
+        f.write(f"Camera1.k2: {d1[1]:.8f}\n")
+        f.write(f"Camera1.p1: {d1[2]:.8f}\n")
+        f.write(f"Camera1.p2: {d1[3]:.8f}\n\n")
+
+        f.write("# --- Caméra droite (intrinsèques) ---\n")
+        f.write(f"Camera2.fx: {K2[0,0]:.6f}\n")
+        f.write(f"Camera2.fy: {K2[1,1]:.6f}\n")
+        f.write(f"Camera2.cx: {K2[0,2]:.6f}\n")
+        f.write(f"Camera2.cy: {K2[1,2]:.6f}\n")
+        f.write(f"Camera2.k1: {d2[0]:.8f}\n")
+        f.write(f"Camera2.k2: {d2[1]:.8f}\n")
+        f.write(f"Camera2.p1: {d2[2]:.8f}\n")
+        f.write(f"Camera2.p2: {d2[3]:.8f}\n\n")
+
+        f.write(f"Camera.width: {w}\n")
+        f.write(f"Camera.height: {h}\n")
+        f.write("Camera.fps: 30\n")
+        f.write("Camera.RGB: 1\n\n")
+
+        f.write("# --- Stéréo : transformation droite→gauche ---\n")
+        f.write("Stereo.T_c1_c2: !!opencv-matrix\n")
+        f.write("   rows: 4\n   cols: 4\n   dt: f\n")
+        T_c1_c2 = np.eye(4)
+        T_c1_c2[:3, :3] = R
+        T_c1_c2[:3, 3] = T.reshape(-1)
+        f.write(f"   data: {_flow_seq([float(x) for x in T_c1_c2.reshape(-1)])}\n\n")
+
+        f.write(f"Stereo.b: {abs(float(T[0])):.6f}   # baseline (m)\n")
+        f.write(f"Stereo.bf: {bf:.6f}   # baseline * fx\n")
+        f.write(f"Stereo.ThDepth: 40.0\n\n")
+
+        f.write("# --- IMU (placeholder identité, à raffiner après calib IMU) ---\n")
+        f.write("IMU.T_b_c1: !!opencv-matrix\n")
+        f.write("   rows: 4\n   cols: 4\n   dt: f\n")
+        f.write(f"   data: {_flow_seq([float(x) for x in np.array(Tbc).reshape(-1)])}\n\n")
+
+        f.write("# --- ORB extractor ---\n")
+        f.write("ORBextractor.nFeatures: 1200\n")
+        f.write("ORBextractor.scaleFactor: 1.2\n")
+        f.write("ORBextractor.nLevels: 8\n")
+        f.write("ORBextractor.iniThFAST: 20\n")
+        f.write("ORBextractor.minThFAST: 7\n")
+
+    print(f"[OK] YAML ORB-SLAM3 → {path}")
+
+
+def write_report(calib: dict, path: str, images_dir: str,
+                 yaml_path: str, gate_passed: bool) -> None:
+    K1 = calib["K1"]; K2 = calib["K2"]
+    D1 = calib["D1"].reshape(-1); D2 = calib["D2"].reshape(-1)
+    T = calib["T"].reshape(-1)
+    baseline_m = float(np.linalg.norm(T))
+
+    os.makedirs(os.path.dirname(path), exist_ok=True)
+    with open(path, "w") as f:
+        f.write("# Rapport de calibration stéréo\n\n")
+        f.write(f"- Date (UTC) : {_dt.datetime.utcnow().isoformat()}Z\n")
+        f.write(f"- Source images : `{images_dir}`\n")
+        f.write(f"- YAML produit : `{yaml_path}`\n")
+        f.write(f"- Damier : {CHESSBOARD_SIZE[0]}×{CHESSBOARD_SIZE[1]} coins, "
+                f"case {SQUARE_SIZE_MM} mm\n")
+        f.write(f"- Résolution : {calib['img_size'][0]}×{calib['img_size'][1]}\n")
+        f.write(f"- Paires utilisées : **{calib['n_pairs']}**\n\n")
+
+        f.write("## Erreurs de reprojection (RMS)\n\n")
+        f.write("| Étape | RMS (px) |\n|---|---|\n")
+        f.write(f"| Caméra gauche  | {calib['rms_left']:.4f} |\n")
+        f.write(f"| Caméra droite  | {calib['rms_right']:.4f} |\n")
+        f.write(f"| **Stéréo (global)** | **{calib['rms_stereo']:.4f}** |\n\n")
+        gate_label = "PASS" if gate_passed else "FAIL"
+        f.write(f"Gate (RMS stéréo < {RMS_GATE_PX} px) : **{gate_label}**\n\n")
+
+        f.write("## Baseline mesurée\n\n")
+        f.write(f"- |T| = **{baseline_m*1000:.2f} mm** "
+                f"(cible 110 mm, écart {abs(baseline_m*1000-110):.2f} mm)\n")
+        f.write(f"- T = [{T[0]*1000:.2f}, {T[1]*1000:.2f}, {T[2]*1000:.2f}] mm\n\n")
+
+        f.write("## Intrinsèques\n\n")
+        f.write("### Caméra gauche\n\n")
+        f.write(f"- fx={K1[0,0]:.3f}  fy={K1[1,1]:.3f}  "
+                f"cx={K1[0,2]:.3f}  cy={K1[1,2]:.3f}\n")
+        f.write(f"- D = [{', '.join(f'{x:.6f}' for x in D1)}]\n\n")
+        f.write("### Caméra droite\n\n")
+        f.write(f"- fx={K2[0,0]:.3f}  fy={K2[1,1]:.3f}  "
+                f"cx={K2[0,2]:.3f}  cy={K2[1,2]:.3f}\n")
+        f.write(f"- D = [{', '.join(f'{x:.6f}' for x in D2)}]\n\n")
+
+        f.write("## Validation\n\n")
+        f.write("- [ ] Lignes épipolaires horizontales (cf `--show-rectified`)\n")
+        f.write("- [ ] Disparité visuelle horizontale uniquement\n")
+        f.write("- [ ] Baseline mesurée ≈ 110 mm\n")
+        if not gate_passed:
+            f.write("\n> **STOP** : RMS > 0.5 px — recapturer (mire mal placée, "
+                    "cams sous-focales ou flou de bougé).\n")
+
+    print(f"[OK] Rapport → {path}")
 
 
 def show_rectified(calib, left_path, right_path):
@@ -198,47 +304,58 @@ def show_rectified(calib, left_path, right_path):
     sz = (calib["img_size"][0], calib["img_size"][1])
 
     map1_l, map2_l = cv2.initUndistortRectifyMap(
-        np.array(calib["K1"]), np.array(calib["D1"]),
+        calib["K1"], calib["D1"], calib["R1"], calib["P1"], sz, cv2.CV_16SC2)
-        np.array(calib["R1"]), np.array(calib["P1"]),
-        sz, cv2.CV_16SC2
-    )
     map1_r, map2_r = cv2.initUndistortRectifyMap(
-        np.array(calib["K2"]), np.array(calib["D2"]),
+        calib["K2"], calib["D2"], calib["R2"], calib["P2"], sz, cv2.CV_16SC2)
-        np.array(calib["R2"]), np.array(calib["P2"]),
-        sz, cv2.CV_16SC2
-    )
 
     rect_l = cv2.remap(img_l, map1_l, map2_l, cv2.INTER_LINEAR)
     rect_r = cv2.remap(img_r, map1_r, map2_r, cv2.INTER_LINEAR)
 
-    # Tracer lignes épipolaires horizontales
+    combined = cv2.hconcat([cv2.resize(rect_l, (640, 480)),
-    combined = cv2.hconcat([
+                            cv2.resize(rect_r, (640, 480))])
-        cv2.resize(rect_l, (640, 360)),
+    for y in range(0, 480, 30):
-        cv2.resize(rect_r, (640, 360))
-    ])
-    for y in range(0, 360, 30):
         cv2.line(combined, (0, y), (1280, y), (0, 255, 0), 1)
-
     cv2.imshow("Rectified — lignes vertes doivent etre horizontales", combined)
     cv2.waitKey(0)
     cv2.destroyAllWindows()
 
 
-def main():
+# -------------------------------------------------------------------------
-    parser = argparse.ArgumentParser(description="Calibration stéréo OpenCV")
+def parse_args():
-    parser.add_argument("--show-rectified", action="store_true",
+    p = argparse.ArgumentParser(description="Calibration stéréo OpenCV → ORB-SLAM3")
+    p.add_argument("--images", default=DEFAULT_IMAGES_DIR,
+                   help="dossier contenant left_*.png / right_*.png")
+    p.add_argument("--yaml", default=DEFAULT_OUTPUT_YAML,
+                   help="chemin de sortie du YAML ORB-SLAM3")
+    p.add_argument("--report", default=DEFAULT_REPORT_MD,
+                   help="chemin du rapport Markdown")
+    p.add_argument("--show-rectified", action="store_true",
                    help="Afficher une paire rectifiée après calibration")
-    args = parser.parse_args()
+    p.add_argument("--no-gate", action="store_true",
+                   help="ne pas exit-code != 0 si RMS > 0.5 px")
+    return p.parse_args()
 
-    lefts, rights = load_pairs()
+
+def main():
+    args = parse_args()
+    lefts, rights = load_pairs(args.images)
     calib = calibrate_stereo(lefts, rights)
-    save_yaml(calib, OUTPUT_YAML)
 
-    if args.show_rectified and len(lefts) > 0:
+    write_orbslam3_yaml(calib, args.yaml)
-        # Charger le yaml pour show
+
-        with open(OUTPUT_YAML) as f:
+    gate_passed = calib["rms_stereo"] < RMS_GATE_PX
-            calib_loaded = yaml.safe_load(f)
+    write_report(calib, args.report, args.images, args.yaml, gate_passed)
-        show_rectified(calib_loaded, lefts[0], rights[0])
+
+    if args.show_rectified:
+        show_rectified(calib, lefts[0], rights[0])
+
+    if not gate_passed:
+        print(f"[STOP] RMS stéréo {calib['rms_stereo']:.4f} > {RMS_GATE_PX} px — "
+              "recapturer la mire.", file=sys.stderr)
+        if not args.no_gate:
+            sys.exit(2)
+    else:
+        print(f"[OK] RMS stéréo {calib['rms_stereo']:.4f} < {RMS_GATE_PX} px ✓")
 
 
 if __name__ == "__main__":

src/calibration/stereo_capture.py

@@ -1,92 +1,168 @@
 #!/usr/bin/env python3
 """
 Capture synchrone de paires d'images stéréo pour calibration.
-Utilise deux webcams USB (index 0 et 1).
+
-Appuyer ESPACE pour sauvegarder, Q pour quitter.
+Cibles HD-3000 sur Raspberry Pi 4B (BlueOS) :
+    - /dev/video0 = caméra gauche
+    - /dev/video2 = caméra droite
+    - Format YUYV 640×480 @ 30 fps  (HD-3000 ne supporte pas MJPG)
+
+Modes :
+    --headless : pas d'affichage, capture toutes les `--interval` secondes,
+                 jusqu'à `--count` paires. Utile en SSH sur le Pi.
+    par défaut : fenêtre OpenCV, ESPACE = capturer paire, Q = quitter.
 """
 
-import cv2
+import argparse
 import os
 import sys
 import time
 
-# Dossier de sortie
+import cv2
-OUTPUT_DIR = os.path.join(os.path.dirname(__file__), "calib_images")
-os.makedirs(OUTPUT_DIR, exist_ok=True)
 
-# Index des caméras
+# Index V4L2 par défaut (issue #1) : video0 = gauche, video2 = droite
-CAM_LEFT = 0
+DEFAULT_LEFT = 0
-CAM_RIGHT = 1
+DEFAULT_RIGHT = 2
 
-# Résolution cible
+# Résolution / fps cible (HD-3000 YUYV)
-WIDTH = 1280
+WIDTH = 640
-HEIGHT = 720
+HEIGHT = 480
+FPS = 30
 
 
-def open_camera(index):
+def _fourcc(code: str) -> int:
-    """Ouvre une caméra et configure la résolution."""
+    return cv2.VideoWriter_fourcc(*code)
-    cap = cv2.VideoCapture(index, cv2.CAP_DSHOW)  # CAP_DSHOW sur Windows
+
+
+def open_camera(index: int) -> cv2.VideoCapture:
+    """Ouvre une caméra V4L2 en YUYV 640×480 @ 30 fps."""
+    cap = cv2.VideoCapture(index, cv2.CAP_V4L2)
     if not cap.isOpened():
-        # Essai sans backend explicite
         cap = cv2.VideoCapture(index)
     if not cap.isOpened():
         print(f"[ERREUR] Impossible d'ouvrir la caméra {index}", file=sys.stderr)
         return None
+
+    # Forcer YUYV AVANT la résolution (sinon V4L2 ignore le set)
+    cap.set(cv2.CAP_PROP_FOURCC, _fourcc("YUYV"))
     cap.set(cv2.CAP_PROP_FRAME_WIDTH, WIDTH)
     cap.set(cv2.CAP_PROP_FRAME_HEIGHT, HEIGHT)
-    cap.set(cv2.CAP_PROP_FPS, 30)
+    cap.set(cv2.CAP_PROP_FPS, FPS)
+
+    actual_w = int(cap.get(cv2.CAP_PROP_FRAME_WIDTH))
+    actual_h = int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT))
+    actual_fps = cap.get(cv2.CAP_PROP_FPS)
+    print(f"[INFO] cam {index}: {actual_w}×{actual_h} @ {actual_fps:.0f} fps")
     return cap
 
 
-def main():
+def warmup(cap_l, cap_r, n: int = 10) -> None:
-    cap_left = open_camera(CAM_LEFT)
+    """Vide quelques frames pour laisser l'AE/AGC se stabiliser."""
-    cap_right = open_camera(CAM_RIGHT)
+    for _ in range(n):
+        cap_l.read()
+        cap_r.read()
 
-    if cap_left is None or cap_right is None:
+
+def grab_pair(cap_l, cap_r, retries: int = 5):
+    """Lit une paire de frames quasi simultanées."""
+    for _ in range(retries):
+        # grab() puis retrieve() = meilleur synchronisme que read() séquentiel
+        cap_l.grab()
+        cap_r.grab()
+        ok_l, frame_l = cap_l.retrieve()
+        ok_r, frame_r = cap_r.retrieve()
+        if ok_l and ok_r:
+            return frame_l, frame_r
+        time.sleep(0.05)
+    return None, None
+
+
+def parse_args():
+    p = argparse.ArgumentParser(description="Capture stéréo HD-3000 pour calib")
+    p.add_argument("--left", type=int, default=DEFAULT_LEFT,
+                   help="index V4L2 caméra gauche (défaut 0)")
+    p.add_argument("--right", type=int, default=DEFAULT_RIGHT,
+                   help="index V4L2 caméra droite (défaut 2)")
+    p.add_argument("--output", default=None,
+                   help="dossier de sortie (défaut: datasets/calib_raw/ à la racine du repo)")
+    p.add_argument("--count", type=int, default=30,
+                   help="nb de paires à capturer (défaut 30)")
+    p.add_argument("--headless", action="store_true",
+                   help="pas d'affichage; capture automatique toutes --interval s")
+    p.add_argument("--interval", type=float, default=2.0,
+                   help="intervalle s entre captures en mode headless (défaut 2.0)")
+    return p.parse_args()
+
+
+def default_output_dir() -> str:
+    # racine du repo = parent de src/
+    here = os.path.dirname(os.path.abspath(__file__))
+    repo_root = os.path.abspath(os.path.join(here, "..", ".."))
+    return os.path.join(repo_root, "datasets", "calib_raw")
+
+
+def main():
+    args = parse_args()
+    out_dir = args.output or default_output_dir()
+    os.makedirs(out_dir, exist_ok=True)
+    print(f"[INFO] Sauvegarde dans: {out_dir}")
+
+    cap_l = open_camera(args.left)
+    cap_r = open_camera(args.right)
+    if cap_l is None or cap_r is None:
         print("[ERREUR] Vérifier connexion des 2 caméras USB.", file=sys.stderr)
         sys.exit(1)
 
+    warmup(cap_l, cap_r)
+
     paire = 0
-    print(f"[INFO] Sauvegarde dans: {OUTPUT_DIR}")
+    try:
+        if args.headless:
+            print(f"[INFO] Mode headless: {args.count} paires, interval {args.interval}s")
+            while paire < args.count:
+                f_l, f_r = grab_pair(cap_l, cap_r)
+                if f_l is None:
+                    print("[WARN] Frame manquante, retry…")
+                    time.sleep(0.2)
+                    continue
+                cv2.imwrite(os.path.join(out_dir, f"left_{paire:04d}.png"), f_l)
+                cv2.imwrite(os.path.join(out_dir, f"right_{paire:04d}.png"), f_r)
+                paire += 1
+                print(f"[OK] Paire {paire}/{args.count}")
+                time.sleep(args.interval)
+        else:
             print("[INFO] ESPACE = capturer paire | Q = quitter")
-
             while True:
-        ret_l, frame_l = cap_left.read()
+                f_l, f_r = grab_pair(cap_l, cap_r)
-        ret_r, frame_r = cap_right.read()
+                if f_l is None:
-
+                    print("[WARN] Frame manquante, retry…")
-        if not ret_l or not ret_r:
-            print("[WARN] Frame manquante, retry...")
                     time.sleep(0.05)
                     continue
 
-        # Affichage côte à côte (redimensionné pour l'écran)
+                disp_l = cv2.resize(f_l, (640, 480))
-        display_l = cv2.resize(frame_l, (640, 360))
+                disp_r = cv2.resize(f_r, (640, 480))
-        display_r = cv2.resize(frame_r, (640, 360))
+                combined = cv2.hconcat([disp_l, disp_r])
-        combined = cv2.hconcat([display_l, display_r])
+                cv2.putText(combined, f"Paires: {paire}/{args.count}", (10, 30),
-        cv2.putText(combined, f"Paires: {paire}/30", (10, 30),
                             cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
                 cv2.imshow("Stereo Capture — ESPACE=capture Q=quit", combined)
 
                 key = cv2.waitKey(1) & 0xFF
-
+                if key in (ord('q'), 27):
-        if key == ord('q') or key == 27:
                     break
-
                 if key == ord(' '):
-            # Sauvegarde la paire
+                    cv2.imwrite(os.path.join(out_dir, f"left_{paire:04d}.png"), f_l)
-            fname_l = os.path.join(OUTPUT_DIR, f"left_{paire:04d}.png")
+                    cv2.imwrite(os.path.join(out_dir, f"right_{paire:04d}.png"), f_r)
-            fname_r = os.path.join(OUTPUT_DIR, f"right_{paire:04d}.png")
-            cv2.imwrite(fname_l, frame_l)
-            cv2.imwrite(fname_r, frame_r)
                     paire += 1
                     print(f"[OK] Paire {paire} sauvegardée")
-            if paire >= 30:
+                    if paire >= args.count:
-                print("[INFO] 30 paires capturées. Calibration possible.")
+                        print(f"[INFO] {paire} paires capturées, fin.")
-
+                        break
-    cap_left.release()
+    finally:
-    cap_right.release()
+        cap_l.release()
+        cap_r.release()
+        if not args.headless:
             cv2.destroyAllWindows()
-    print(f"[INFO] {paire} paires sauvegardées dans {OUTPUT_DIR}")
+        print(f"[INFO] {paire} paires sauvegardées dans {out_dir}")
 
 
 if __name__ == "__main__":