diff --git a/.gitignore b/.gitignore index 3a728c7..b1fe8fc 100644 --- a/.gitignore +++ b/.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/ diff --git a/docs/source/calibration.rst b/docs/source/calibration.rst index b59bae1..f44d67e 100644 --- a/docs/source/calibration.rst +++ b/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) -- 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 -~~~~~~~~~~~~~~~~~~~~~~ +Étape 1 — Capture des paires (sur le Pi, en SSH) +~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ .. 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... - Calibration stéréo... - RMS reprojection error: 0.42 px ← bon si < 0.8 px - Sauvegarde: config/stereo_calib.yaml + [INFO] Chargement 30 paires depuis datasets/calib_raw + [INFO] Gauche: 28/30 images valides + [INFO] Droite: 27/30 images valides + [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 - D1: [k1, k2, p1, p2, k3] # distorsion gauche - K2: [[fx, 0, cx], [0, fy, cy], [0, 0, 1]] # caméra droite - 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 + %YAML:1.0 + File.version: "1.0" + Camera.type: "PinHole" -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 = ( - cv2.CALIB_FIX_INTRINSIC # si calibration individuelle faite avant - # ou cv2.CALIB_RATIONAL_MODEL # pour modèle 8 coefs - ) - criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 1e-5) + Stereo.T_c1_c2: !!opencv-matrix # transfo cam_droite → cam_gauche + rows: 4 + cols: 4 + dt: f + 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 < 0.8 px (idéalement < 0.5 px) +1. Erreur RMS stéréo **< 0.5 px** (gate strict, issue #1) 2. Lignes épipolaires horizontales sur image rectifiée -3. Translation T ≈ [−0.11, 0, 0] m (baseline sur axe X) -4. Épipole à l'infini (caméras parallèles après rectif) - -.. code-block:: bash - - # Visualisation rectification - python src/calibration/stereo_calibrate.py --show-rectified + (``python3 src/calibration/stereo_calibrate.py --show-rectified``) +3. Translation ``T ≈ [-0.110, 0, 0]`` m (baseline sur axe X) +4. Épipole à l'infini après rectification (caméras parallèles) Références ---------- diff --git a/src/calibration/stereo_calibrate.py b/src/calibration/stereo_calibrate.py index 3a88db6..50276e2 100644 --- a/src/calibration/stereo_calibrate.py +++ b/src/calibration/stereo_calibrate.py @@ -1,36 +1,49 @@ #!/usr/bin/env python3 """ -Calibration stéréo à partir de paires d'images de damier. -Damier: 9x6 cases intérieures, case 25mm. -Sortie: config/stereo_calib.yaml +Calibration stéréo OpenCV → format ORB-SLAM3. + +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 -CHESSBOARD_SIZE = (9, 6) # coins intérieurs (colonnes, lignes) -SQUARE_SIZE_MM = 25.0 # taille d'une case en mm +# --- Damier --------------------------------------------------------------- +CHESSBOARD_SIZE = (9, 6) # coins intérieurs (cols, rows) +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") -CONFIG_DIR = os.path.join(SCRIPT_DIR, "..", "..", "config") -OUTPUT_YAML = os.path.join(CONFIG_DIR, "stereo_calib.yaml") +REPO_ROOT = os.path.abspath(os.path.join(SCRIPT_DIR, "..", "..")) +DEFAULT_IMAGES_DIR = os.path.join(REPO_ROOT, "datasets", "calib_raw") +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(): - """Charge les paires d'images gauche/droite.""" - lefts = sorted(glob.glob(os.path.join(CALIB_IMAGES_DIR, "left_*.png"))) - rights = sorted(glob.glob(os.path.join(CALIB_IMAGES_DIR, "right_*.png"))) - - if len(lefts) == 0 or len(rights) == 0: - print(f"[ERREUR] Aucune image dans {CALIB_IMAGES_DIR}", file=sys.stderr) +def load_pairs(images_dir: str): + lefts = sorted(glob.glob(os.path.join(images_dir, "left_*.png"))) + rights = sorted(glob.glob(os.path.join(images_dir, "right_*.png"))) + if not lefts or not rights: + print(f"[ERREUR] Aucune image dans {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 - img_points = [] # 2D - valid_indices = [] + obj_points, img_points, 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) - - ret, corners = cv2.findChessboardCorners( - gray, CHESSBOARD_SIZE, - cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE - ) - - if ret: - corners_refined = cv2.cornerSubPix( - gray, corners, (11, 11), (-1, -1), SUBPIX_CRITERIA - ) + img_size = gray.shape[::-1] + found, corners = cv2.findChessboardCorners(gray, CHESSBOARD_SIZE, flags) + if found: + corners = 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 - - # Garder seulement les paires où les 2 caméras détectent le damier - valid_pairs = set(idx_l) & set(idx_r) - idx_map_l = {v: i for i, v in enumerate(idx_l)} - idx_map_r = {v: i for i, v in enumerate(idx_r)} - - obj_pts = [] - pts_left = [] - 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]]) - + # Pairs où damier détecté des deux côtés + valid_pairs = sorted(set(idx_l) & set(idx_r)) + map_l = {v: i for i, v in enumerate(idx_l)} + map_r = {v: i for i, v in enumerate(idx_r)} + obj_pts, pts_left, pts_right = [], [], [] + for k in valid_pairs: + obj_pts.append(obj_l[map_l[k]]) + pts_left.append(pts_l[map_l[k]]) + pts_right.append(pts_r[map_r[k]]) 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 caméra gauche...") - _, K1, D1, _, _ = cv2.calibrateCamera(obj_pts, pts_left, img_size, None, None) + print("[INFO] Calibration intrinsèque caméra GAUCHE…") + rms_l, 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...") - _, K2, D2, _, _ = cv2.calibrateCamera(obj_pts, pts_right, img_size, None, None) + print("[INFO] Calibration intrinsèque caméra DROITE…") + 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...") - flags = cv2.CALIB_FIX_INTRINSIC + print("[INFO] Calibration stéréo…") rms, K1, D1, K2, D2, R, T, E, F = cv2.stereoCalibrate( obj_pts, pts_left, pts_right, - K1, D1, K2, D2, - img_size, + K1, D1, K2, D2, 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"]), - np.array(calib["R1"]), np.array(calib["P1"]), - sz, cv2.CV_16SC2 - ) + calib["K1"], calib["D1"], calib["R1"], calib["P1"], sz, cv2.CV_16SC2) map1_r, map2_r = cv2.initUndistortRectifyMap( - np.array(calib["K2"]), np.array(calib["D2"]), - np.array(calib["R2"]), np.array(calib["P2"]), - sz, cv2.CV_16SC2 - ) + calib["K2"], calib["D2"], calib["R2"], 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, 360)), - cv2.resize(rect_r, (640, 360)) - ]) - for y in range(0, 360, 30): + combined = cv2.hconcat([cv2.resize(rect_l, (640, 480)), + cv2.resize(rect_r, (640, 480))]) + for y in range(0, 480, 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 parse_args(): + 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") + p.add_argument("--no-gate", action="store_true", + help="ne pas exit-code != 0 si RMS > 0.5 px") + return p.parse_args() + + def main(): - parser = argparse.ArgumentParser(description="Calibration stéréo OpenCV") - parser.add_argument("--show-rectified", action="store_true", - help="Afficher une paire rectifiée après calibration") - args = parser.parse_args() - - lefts, rights = load_pairs() + 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: - # Charger le yaml pour show - with open(OUTPUT_YAML) as f: - calib_loaded = yaml.safe_load(f) - show_rectified(calib_loaded, lefts[0], rights[0]) + write_orbslam3_yaml(calib, args.yaml) + + gate_passed = calib["rms_stereo"] < RMS_GATE_PX + write_report(calib, args.report, args.images, args.yaml, gate_passed) + + 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__": diff --git a/src/calibration/stereo_capture.py b/src/calibration/stereo_capture.py index 7ba0003..ad28eed 100644 --- a/src/calibration/stereo_capture.py +++ b/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 -OUTPUT_DIR = os.path.join(os.path.dirname(__file__), "calib_images") -os.makedirs(OUTPUT_DIR, exist_ok=True) +import cv2 -# Index des caméras -CAM_LEFT = 0 -CAM_RIGHT = 1 +# Index V4L2 par défaut (issue #1) : video0 = gauche, video2 = droite +DEFAULT_LEFT = 0 +DEFAULT_RIGHT = 2 -# Résolution cible -WIDTH = 1280 -HEIGHT = 720 +# Résolution / fps cible (HD-3000 YUYV) +WIDTH = 640 +HEIGHT = 480 +FPS = 30 -def open_camera(index): - """Ouvre une caméra et configure la résolution.""" - cap = cv2.VideoCapture(index, cv2.CAP_DSHOW) # CAP_DSHOW sur Windows +def _fourcc(code: str) -> int: + return cv2.VideoWriter_fourcc(*code) + + +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(): - cap_left = open_camera(CAM_LEFT) - cap_right = open_camera(CAM_RIGHT) +def warmup(cap_l, cap_r, n: int = 10) -> None: + """Vide quelques frames pour laisser l'AE/AGC se stabiliser.""" + 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}") - print("[INFO] ESPACE = capturer paire | Q = quitter") + 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: + f_l, f_r = grab_pair(cap_l, cap_r) + if f_l is None: + print("[WARN] Frame manquante, retry…") + time.sleep(0.05) + continue - while True: - ret_l, frame_l = cap_left.read() - ret_r, frame_r = cap_right.read() + disp_l = cv2.resize(f_l, (640, 480)) + disp_r = cv2.resize(f_r, (640, 480)) + combined = cv2.hconcat([disp_l, disp_r]) + cv2.putText(combined, f"Paires: {paire}/{args.count}", (10, 30), + cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) + cv2.imshow("Stereo Capture — ESPACE=capture Q=quit", combined) - 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) - display_l = cv2.resize(frame_l, (640, 360)) - display_r = cv2.resize(frame_r, (640, 360)) - combined = cv2.hconcat([display_l, display_r]) - 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 == ord('q') or key == 27: - break - - if key == ord(' '): - # Sauvegarde la paire - fname_l = os.path.join(OUTPUT_DIR, f"left_{paire:04d}.png") - 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: - print("[INFO] 30 paires capturées. Calibration possible.") - - cap_left.release() - cap_right.release() - cv2.destroyAllWindows() - print(f"[INFO] {paire} paires sauvegardées dans {OUTPUT_DIR}") + key = cv2.waitKey(1) & 0xFF + if key in (ord('q'), 27): + break + if key == ord(' '): + 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} sauvegardée") + if paire >= args.count: + print(f"[INFO] {paire} paires capturées, fin.") + break + finally: + cap_l.release() + cap_r.release() + if not args.headless: + cv2.destroyAllWindows() + print(f"[INFO] {paire} paires sauvegardées dans {out_dir}") if __name__ == "__main__":