fix #1: calibration stéréo HD-3000 (YUYV/V4L2, ORB-SLAM3 YAML, gate RMS<0.5px)
- stereo_capture.py : backend V4L2 + fourcc YUYV 640×480@30 (HD-3000 ne supporte pas MJPG), indices /dev/video0 et /dev/video2, mode --headless pour SSH, warmup AE/AGC, grab+retrieve pour meilleur synchronisme, sortie datasets/calib_raw/ à la racine du repo. - stereo_calibrate.py : log RMS per-cam + RMS stéréo global, écrit config/stereo_params.yaml au format ORB-SLAM3 (Camera1/2.fx, Stereo.T_c1_c2, Stereo.b/bf, IMU.T_b_c1 placeholder), génère config/calibration_report.md avec baseline mesurée vs cible 110 mm. Gate exit-code 2 si RMS > 0.5 px (bypass --no-gate). - .gitignore : ajoute datasets/calib_raw/ (captures brutes volumineuses). - docs/source/calibration.rst : procédure SSH headless reproduite, format YAML ORB-SLAM3 documenté, gate RMS<0.5 px expliqué. Note : la calibration numérique (RMS, K/D/R/T, baseline mesurée) doit être exécutée sur le Pi avec les caméras connectées — code prêt, hardware run à faire manuellement. Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
This commit is contained in:
Silver Surfer
7
.gitignore
vendored
7
.gitignore
vendored
@@ -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/
|
||||
|
||||
@@ -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
|
||||
----------
|
||||
|
||||
@@ -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__":
|
||||
|
||||
@@ -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__":
|
||||
|
||||
Reference in New Issue
Block a user