calibrage auto
This commit is contained in:
@@ -45,11 +45,11 @@ class VideoCaptureInterface(abc.ABC):
|
||||
Les sous-classes doivent implémenter les méthodes abstraites
|
||||
pour gérer le matériel spécifique.
|
||||
"""
|
||||
|
||||
|
||||
# Cadence par défaut en images par seconde
|
||||
DEFAULT_FPS: float = 5.0
|
||||
|
||||
def __init__(self, fps: float = DEFAULT_FPS, use_tracking: bool = False, px_per_mm: float = 2.15, display=None):
|
||||
def __init__(self, fps: float = DEFAULT_FPS, use_tracking: bool = False, display=None, parent=None):
|
||||
"""
|
||||
Initialise l'interface de capture.
|
||||
|
||||
@@ -58,6 +58,7 @@ class VideoCaptureInterface(abc.ABC):
|
||||
self._fps: float = fps
|
||||
self.use_tracking = use_tracking
|
||||
self.display = display
|
||||
self.parent = parent
|
||||
self._interval: float = 1.0 / fps # Intervalle en secondes entre chaque capture
|
||||
self._running: bool = False # Indique si la capture est en cours
|
||||
self._thread: Optional[threading.Thread] = None
|
||||
@@ -73,16 +74,16 @@ class VideoCaptureInterface(abc.ABC):
|
||||
min_area_px = settings.TRACKER_MIN_AREA,
|
||||
)
|
||||
self._aligner = TubeAligner(
|
||||
px_per_mm = px_per_mm, # à calibrer selon la caméra
|
||||
grbl_threshold_px = 20, # au-delà → correction GRBL
|
||||
dead_zone_px = 5, # en-dessous → rien à faire
|
||||
display = display,
|
||||
)
|
||||
self._last_detection = None # résultat du dernier alignement
|
||||
self._last_detection = None # résultat du dernier alignement
|
||||
|
||||
# calibrage ou lecture réelle
|
||||
#
|
||||
def align_on_well_arrival(self, frame: bytes, cnc_controller) -> dict:
|
||||
|
||||
def align_on_well_arrival(self, frame: bytes, cnc_controller, tube_diameter: float = 16.0) -> dict:
|
||||
"""
|
||||
Appelé UNE FOIS à l'arrivée sur un nouveau puits.
|
||||
Détecte le tube, décide l'action, exécute la correction.
|
||||
@@ -93,7 +94,7 @@ class VideoCaptureInterface(abc.ABC):
|
||||
"""
|
||||
nparr = np.frombuffer(frame, np.uint8)
|
||||
img = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
|
||||
detection = self._aligner.detect_tube(img)
|
||||
detection = self._aligner.detect_tube(img, tube_diameter=tube_diameter)
|
||||
|
||||
# Stockage pour process_frame
|
||||
self._last_detection = detection
|
||||
@@ -108,7 +109,7 @@ class VideoCaptureInterface(abc.ABC):
|
||||
dy_mm = detection["offset_y_mm"]
|
||||
|
||||
msg = f"align_on_well_arrival: correction CNC move_relative(dx={dx_mm:.3f}, dy={dy_mm:.3f})"
|
||||
#cnc_controller.move_relative(dx=-dx_mm, dy=-dy_mm, feed=150)
|
||||
cnc_controller.move_relative(dx=dx_mm, dy=dy_mm, feed=150)
|
||||
|
||||
self._tracker.reset()
|
||||
self._last_detection["action"] = "none"
|
||||
@@ -118,7 +119,7 @@ class VideoCaptureInterface(abc.ABC):
|
||||
|
||||
logger.info(msg)
|
||||
self.display(state='detect_tube', msg=msg)
|
||||
return detection
|
||||
return detection
|
||||
|
||||
|
||||
def on_well_change(self):
|
||||
@@ -276,18 +277,14 @@ class VideoCaptureInterface(abc.ABC):
|
||||
|
||||
# Mode debug
|
||||
if self._aligner.debug:
|
||||
self._last_detection = detection = self._aligner.detect_tube(frame)
|
||||
annotated = detection.get('frame_annotated')
|
||||
self._last_detection = self._aligner.detect_tube(frame, self.parent.data.tube_diameter or 16.0)
|
||||
annotated = self._last_detection.get('frame_annotated')
|
||||
frame = annotated if annotated is not None else frame
|
||||
'''
|
||||
else:
|
||||
detection = self._last_detection or {}
|
||||
|
||||
# --- Crop logiciel si nécessaire ---
|
||||
if (detection.get("action") == "crop" and detection.get("detected") and not self._aligner.debug ):
|
||||
frame = self._aligner.crop_to_tube(frame, detection)
|
||||
'''
|
||||
|
||||
|
||||
#if (self._last_detection.get("action") == "crop" and self._last_detection.get("detected")):
|
||||
# frame = self._aligner.crop_to_tube(frame, self._last_detection)
|
||||
|
||||
# mode racking
|
||||
if self.use_tracking:
|
||||
ts = datetime.now(timezone.utc).timestamp()
|
||||
frame, metrics = self._tracker.process(frame, ts)
|
||||
|
||||
@@ -12,7 +12,7 @@ import logging
|
||||
import serial
|
||||
import time
|
||||
import threading
|
||||
from typing import Optional
|
||||
|
||||
|
||||
logging.basicConfig(level=logging.INFO)
|
||||
logger = logging.getLogger(__name__)
|
||||
@@ -194,7 +194,7 @@ class GRBLController:
|
||||
x, y = self.get_mpos() # Position actuelle
|
||||
self.move_to(x + dx, y + dy, feed=feed)
|
||||
|
||||
def move_relative_(self, dx=0, dy=0, feed=1000):
|
||||
def move_relative__(self, dx=0, dy=0, feed=1000):
|
||||
self.send("G91") # Mode relatif
|
||||
cmd = f"G0 X{dx} Y{dy} F{feed}"
|
||||
self.send(cmd)
|
||||
|
||||
@@ -46,8 +46,8 @@ class PiCamera2Capture(VideoCaptureInterface):
|
||||
camera_index: int = 0,
|
||||
use_video_config: bool = True,
|
||||
use_tracking: bool = False,
|
||||
px_per_mm: float = 2.1,
|
||||
display = None,
|
||||
parent = None
|
||||
|
||||
):
|
||||
"""
|
||||
@@ -59,7 +59,7 @@ class PiCamera2Capture(VideoCaptureInterface):
|
||||
:param use_video_config: True = VideoConfiguration (flux continu, basse latence)
|
||||
False = StillConfiguration (haute résolution, plus lent)
|
||||
"""
|
||||
super().__init__(fps=fps, use_tracking=use_tracking, px_per_mm=px_per_mm, display=display)
|
||||
super().__init__(fps=fps, use_tracking=use_tracking, display=display, parent=parent)
|
||||
self._width: int = width
|
||||
self._height: int = height
|
||||
self._jpeg_quality: int = jpeg_quality
|
||||
|
||||
@@ -37,6 +37,9 @@ class PiCamera2Capture(VideoCaptureInterface):
|
||||
jpeg_quality: int = 85,
|
||||
camera_index: int = 0,
|
||||
use_video_config: bool = True,
|
||||
use_tracking: bool = False,
|
||||
display = None,
|
||||
parent=None,
|
||||
):
|
||||
"""
|
||||
:param fps: Cadence cible en images par seconde
|
||||
@@ -47,7 +50,7 @@ class PiCamera2Capture(VideoCaptureInterface):
|
||||
:param use_video_config: True = configuration VideoConfiguration (flux continu)
|
||||
False = StillConfiguration (haute résolution, plus lent)
|
||||
"""
|
||||
super().__init__(fps=fps)
|
||||
super().__init__(fps=fps, use_tracking=use_tracking, display=display, parent=parent)
|
||||
self._width: int = width
|
||||
self._height: int = height
|
||||
self._jpeg_quality: int = jpeg_quality
|
||||
|
||||
@@ -1,9 +1,9 @@
|
||||
'''
|
||||
modules/tube_aligner.py
|
||||
Created on 17 avr. 2026
|
||||
|
||||
@author: denis
|
||||
'''
|
||||
# modules/tube_aligner.py
|
||||
|
||||
import cv2
|
||||
import logging
|
||||
@@ -19,13 +19,11 @@ class TubeAligner:
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
px_per_mm : float = 10.0,
|
||||
grbl_threshold_px : int = 20,
|
||||
dead_zone_px : int = 5,
|
||||
debug : bool = False, # ← activable depuis la vue
|
||||
display = None,
|
||||
debug : bool = False, # ← activable depuis la vue
|
||||
display = None, # display function
|
||||
):
|
||||
self.TUBE_DIAMETER_MM = 16.0
|
||||
self.grbl_threshold_px = grbl_threshold_px
|
||||
self.dead_zone_px = dead_zone_px
|
||||
self.debug = debug
|
||||
@@ -35,7 +33,8 @@ class TubeAligner:
|
||||
# Détection principale
|
||||
# ------------------------------------------------------------------ #
|
||||
|
||||
def detect_tube(self, frame: np.ndarray) -> dict:
|
||||
def detect_tube(self, frame: np.ndarray, tube_diameter: float = 16.0) -> dict:
|
||||
TUBE_DIAMETER_MM = tube_diameter
|
||||
h, w = frame.shape[:2]
|
||||
cx_img = w // 2
|
||||
cy_img = h // 2
|
||||
@@ -93,16 +92,14 @@ class TubeAligner:
|
||||
ty = int(np.mean(all_cy))
|
||||
tr = int(np.mean(all_r))
|
||||
if tr > 0:
|
||||
self.px_per_mm = (2 * tr) / 16.0
|
||||
self.px_per_mm = (2 * tr) / TUBE_DIAMETER_MM
|
||||
|
||||
offset_x_px = tx - cx_img
|
||||
offset_y_px = ty - cy_img
|
||||
#offset_x_mm = offset_x_px / self.px_per_mm
|
||||
#offset_y_mm = offset_y_px /self. px_per_mm
|
||||
|
||||
offset_x_mm = offset_y_px /self. px_per_mm # (X CNC = Y image)
|
||||
offset_y_mm = -offset_x_px / self.px_per_mm # (Y CNC = -X image)
|
||||
|
||||
offset_y_mm = -offset_x_px / self.px_per_mm # (Y CNC = -X image)
|
||||
dist_px = float(np.sqrt(offset_x_px**2 + offset_y_px**2))
|
||||
|
||||
if dist_px <= self.dead_zone_px:
|
||||
@@ -162,83 +159,7 @@ class TubeAligner:
|
||||
# Redimensionne à la taille originale pour ne pas changer le pipeline
|
||||
return cv2.resize(cropped, (w, h), interpolation=cv2.INTER_LINEAR)
|
||||
|
||||
|
||||
def _detect_center_stable(
|
||||
self,
|
||||
capture_func, # callable() → frame bytes
|
||||
n_samples: int = 5,
|
||||
delay_s: float = 0.3,
|
||||
) -> tuple[float, float] | None:
|
||||
"""
|
||||
Capture N frames et retourne le centre moyen du tube.
|
||||
Réduit l'erreur de détection d'un facteur √N.
|
||||
|
||||
:param capture_func: callable sans argument → bytes JPEG
|
||||
:param n_samples: nombre de captures à moyenner
|
||||
:param delay_s: pause entre chaque capture
|
||||
:return: (cx_mean, cy_mean) ou None si échec
|
||||
"""
|
||||
import time
|
||||
centers = []
|
||||
|
||||
for i in range(n_samples):
|
||||
if i > 0:
|
||||
time.sleep(delay_s)
|
||||
|
||||
frame_bytes = capture_func()
|
||||
nparr = np.frombuffer(frame_bytes, np.uint8)
|
||||
frame = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
|
||||
|
||||
if frame is None:
|
||||
continue
|
||||
|
||||
detection = self.detect_tube(frame)
|
||||
if detection["detected"]:
|
||||
centers.append((detection["tube_cx"], detection["tube_cy"]))
|
||||
logger.debug(
|
||||
"_detect_center_stable [%d/%d] : cx=%d cy=%d",
|
||||
i+1, n_samples,
|
||||
detection["tube_cx"], detection["tube_cy"],
|
||||
)
|
||||
else:
|
||||
logger.warning("_detect_center_stable [%d/%d] : tube non détecté", i+1, n_samples)
|
||||
|
||||
if len(centers) < 3:
|
||||
logger.error("_detect_center_stable : seulement %d détections valides", len(centers))
|
||||
return None
|
||||
|
||||
# Filtre les valeurs aberrantes (écart > 2 sigma)
|
||||
cx_arr = np.array([c[0] for c in centers], dtype=float)
|
||||
cy_arr = np.array([c[1] for c in centers], dtype=float)
|
||||
|
||||
cx_mean, cx_std = np.mean(cx_arr), np.std(cx_arr)
|
||||
cy_mean, cy_std = np.mean(cy_arr), np.std(cy_arr)
|
||||
|
||||
mask = (
|
||||
(np.abs(cx_arr - cx_mean) <= 2 * cx_std) &
|
||||
(np.abs(cy_arr - cy_mean) <= 2 * cy_std)
|
||||
)
|
||||
filtered = [(cx_arr[i], cy_arr[i]) for i in range(len(centers)) if mask[i]]
|
||||
|
||||
if not filtered:
|
||||
filtered = centers # fallback si tout est filtré
|
||||
|
||||
cx_final = float(np.mean([c[0] for c in filtered]))
|
||||
cy_final = float(np.mean([c[1] for c in filtered]))
|
||||
|
||||
logger.info(
|
||||
"_detect_center_stable : %d/%d valides cx=%.1f±%.1f cy=%.1f±%.1f",
|
||||
len(filtered), n_samples,
|
||||
cx_final, cx_std, cy_final, cy_std,
|
||||
)
|
||||
return cx_final, cy_final
|
||||
|
||||
|
||||
|
||||
def calib_reset(self):
|
||||
pass
|
||||
|
||||
# ------------------------------------------------------------------ #
|
||||
# ------------------------------------------------------------------ #
|
||||
# Dessin debug
|
||||
# ------------------------------------------------------------------ #
|
||||
|
||||
@@ -259,26 +180,18 @@ class TubeAligner:
|
||||
|
||||
# Cercle intérieur du tube
|
||||
cv2.circle(frame, (tx, ty), tr, color, 2, cv2.LINE_AA)
|
||||
|
||||
# Rayon de zone morte (dead zone) en vert clair
|
||||
cv2.circle(frame, (cx_img, cy_img), self.dead_zone_px,
|
||||
(0, 255, 100), 1, cv2.LINE_AA)
|
||||
|
||||
cv2.circle(frame, (cx_img, cy_img), self.dead_zone_px, (0, 255, 100), 1, cv2.LINE_AA)
|
||||
# Rayon seuil GRBL en rouge pointillé (simulé par cercle fin)
|
||||
cv2.circle(frame, (cx_img, cy_img), self.grbl_threshold_px,
|
||||
(0, 80, 255), 1, cv2.LINE_AA)
|
||||
|
||||
cv2.circle(frame, (cx_img, cy_img), self.grbl_threshold_px, (0, 80, 255), 1, cv2.LINE_AA)
|
||||
# Croix centre image
|
||||
cv2.drawMarker(frame, (cx_img, cy_img),
|
||||
(255, 255, 255), cv2.MARKER_CROSS, 24, 1, cv2.LINE_AA)
|
||||
|
||||
cv2.drawMarker(frame, (cx_img, cy_img), (255, 255, 255), cv2.MARKER_CROSS, 24, 1, cv2.LINE_AA)
|
||||
# Centre tube
|
||||
cv2.circle(frame, (tx, ty), 5, color, -1, cv2.LINE_AA)
|
||||
|
||||
# Vecteur offset centre image → centre tube
|
||||
if dist_px > self.dead_zone_px:
|
||||
cv2.arrowedLine(frame, (cx_img, cy_img), (tx, ty),
|
||||
color, 2, cv2.LINE_AA, tipLength=0.2)
|
||||
cv2.arrowedLine(frame, (cx_img, cy_img), (tx, ty), color, 2, cv2.LINE_AA, tipLength=0.2)
|
||||
|
||||
# Panneau info — fond semi-transparent
|
||||
overlay = frame.copy()
|
||||
@@ -298,10 +211,8 @@ class TubeAligner:
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.48, col, 1, cv2.LINE_AA)
|
||||
|
||||
# Légende zones
|
||||
cv2.putText(frame, "dead zone", (cx_img + self.dead_zone_px + 3, cy_img - 3),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.35, (0, 255, 100), 1)
|
||||
cv2.putText(frame, "GRBL threshold", (cx_img + self.grbl_threshold_px + 3, cy_img + 6),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.35, (0, 80, 255), 1)
|
||||
cv2.putText(frame, "dead zone", (cx_img + self.dead_zone_px + 3, cy_img - 3), cv2.FONT_HERSHEY_SIMPLEX, 0.35, (0, 255, 100), 1)
|
||||
cv2.putText(frame, "GRBL threshold", (cx_img + self.grbl_threshold_px + 3, cy_img + 6), cv2.FONT_HERSHEY_SIMPLEX, 0.35, (0, 80, 255), 1)
|
||||
return frame
|
||||
|
||||
def _draw_debug_no_detection(self, frame, cx_img, cy_img) -> np.ndarray:
|
||||
|
||||
@@ -1,230 +0,0 @@
|
||||
'''
|
||||
Created on 17 avr. 2026
|
||||
|
||||
@author: denis
|
||||
'''
|
||||
# modules/tube_aligner.py
|
||||
|
||||
import cv2
|
||||
import logging
|
||||
import numpy as np
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class TubeAligner:
|
||||
"""
|
||||
Détecte le cercle du tube à essai dans une frame (vue par dessous,
|
||||
éclairage par dessus → cercle clair sur fond sombre).
|
||||
Calcule le décalage entre le centre du tube et le centre de l'image.
|
||||
Décide d'une correction GRBL (grand écart) ou d'un recadrage (petit écart).
|
||||
"""
|
||||
|
||||
# Seuil en pixels : au-delà → correction GRBL, en-dessous → recadrage
|
||||
GRBL_THRESHOLD_PX = 20
|
||||
# Tolérance : en-dessous → pas de correction nécessaire
|
||||
DEAD_ZONE_PX = 5
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
px_per_mm: float = 10.0, # facteur d'échelle calibration (px/mm)
|
||||
grbl_threshold_px: int = 20,
|
||||
dead_zone_px: int = 5,
|
||||
debug: bool = False, # ← activable depuis la vue
|
||||
):
|
||||
self.px_per_mm = px_per_mm
|
||||
self.grbl_threshold_px = grbl_threshold_px
|
||||
self.dead_zone_px = dead_zone_px
|
||||
|
||||
def detect_tube(self, frame: np.ndarray) -> dict:
|
||||
"""
|
||||
Détecte le cercle du tube et calcule le décalage par rapport au centre image.
|
||||
|
||||
:param frame: Frame BGR (numpy array)
|
||||
:return: dict avec cercle détecté, décalage px et mm, action recommandée
|
||||
"""
|
||||
h, w = frame.shape[:2]
|
||||
cx_img = w // 2
|
||||
cy_img = h // 2
|
||||
|
||||
result = {
|
||||
"detected" : False,
|
||||
"tube_cx" : None,
|
||||
"tube_cy" : None,
|
||||
"tube_radius" : None,
|
||||
"offset_x_px" : None,
|
||||
"offset_y_px" : None,
|
||||
"offset_x_mm" : None,
|
||||
"offset_y_mm" : None,
|
||||
"action" : "none", # "none" | "crop" | "grbl"
|
||||
"grbl_gcode" : None,
|
||||
"frame_annotated": None,
|
||||
}
|
||||
|
||||
# Prétraitement : niveaux de gris + flou
|
||||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||||
#blurred = cv2.GaussianBlur(gray, (9, 9), 2)
|
||||
blurred = cv2.GaussianBlur(gray, (15, 15), 3)
|
||||
|
||||
# param1 : seuil Canny haut, param2 : seuil accumulation (plus bas = plus permissif)
|
||||
min_radius = int(min(w, h) * 0.26) # ~260px sur 1000px
|
||||
max_radius = int(min(w, h) * 0.36) # ~360px sur 1000px — bord intérieur du verre
|
||||
|
||||
circles = cv2.HoughCircles(
|
||||
blurred,
|
||||
cv2.HOUGH_GRADIENT,
|
||||
dp = 1.2,
|
||||
minDist = min(w, h) // 2, # un seul tube attendu
|
||||
param1 = 50,
|
||||
param2 = 30,
|
||||
minRadius = min_radius,
|
||||
maxRadius = max_radius,
|
||||
)
|
||||
|
||||
if circles is None:
|
||||
logger.warning("TubeAligner: aucun cercle détecté")
|
||||
result["frame_annotated"] = self._annotate(frame.copy(), cx_img, cy_img, None)
|
||||
return result
|
||||
|
||||
circles = np.round(circles[0, :]).astype(int)
|
||||
|
||||
# Prend le cercle le plus proche du centre image
|
||||
best = min(
|
||||
circles,
|
||||
key=lambda c: np.sqrt((c[0] - cx_img)**2 + (c[1] - cy_img)**2)
|
||||
)
|
||||
tx, ty, tr = int(best[0]), int(best[1]), int(best[2])
|
||||
|
||||
# Décalage : positif = tube à droite/bas du centre image
|
||||
offset_x_px = tx - cx_img
|
||||
offset_y_px = ty - cy_img
|
||||
offset_x_mm = offset_x_px / self.px_per_mm
|
||||
offset_y_mm = offset_y_px / self.px_per_mm
|
||||
|
||||
dist_px = np.sqrt(offset_x_px**2 + offset_y_px**2)
|
||||
|
||||
# Décision d'action
|
||||
if dist_px <= self.dead_zone_px:
|
||||
action = "none"
|
||||
grbl_gcode = None
|
||||
elif dist_px <= self.grbl_threshold_px:
|
||||
action = "crop"
|
||||
grbl_gcode = None
|
||||
else:
|
||||
action = "grbl"
|
||||
# G91 = coordonnées relatives, G0 = déplacement rapide
|
||||
# Inversion du signe : si tube est à droite (+X image),
|
||||
# la CNC doit reculer (-X GRBL) pour recentrer
|
||||
#cmd = f"G53 G1 X{x:.2f} Y{y:.2f} F{feed}"
|
||||
|
||||
grbl_gcode = (
|
||||
f"G91\n"
|
||||
f"G1 X{-offset_x_mm:.3f} Y{-offset_y_mm:.3f}\n"
|
||||
f"G90"
|
||||
)
|
||||
logger.info(
|
||||
"TubeAligner: décalage %.1fpx (%.2fmm, %.2fmm) → GRBL: %s",
|
||||
dist_px, offset_x_mm, offset_y_mm, grbl_gcode.replace('\n', ' | ')
|
||||
)
|
||||
|
||||
result.update({
|
||||
"detected" : True,
|
||||
"tube_cx" : tx,
|
||||
"tube_cy" : ty,
|
||||
"tube_radius" : tr,
|
||||
"offset_x_px" : offset_x_px,
|
||||
"offset_y_px" : offset_y_px,
|
||||
"offset_x_mm" : round(offset_x_mm, 3),
|
||||
"offset_y_mm" : round(offset_y_mm, 3),
|
||||
"action" : action,
|
||||
"grbl_gcode" : None,
|
||||
"frame_annotated": self._annotate(
|
||||
frame.copy(), cx_img, cy_img, (tx, ty, tr), offset_x_px, offset_y_px
|
||||
),
|
||||
})
|
||||
|
||||
return result
|
||||
|
||||
def crop_to_tube(self, frame: np.ndarray, detection: dict) -> np.ndarray:
|
||||
"""
|
||||
Recadrage logiciel : recentre l'image sur le tube détecté.
|
||||
Utilisé quand action == "crop".
|
||||
"""
|
||||
if not detection["detected"]:
|
||||
return frame
|
||||
|
||||
tx = detection["tube_cx"]
|
||||
ty = detection["tube_cy"]
|
||||
tr = detection["tube_radius"]
|
||||
h, w = frame.shape[:2]
|
||||
|
||||
# Fenêtre carrée autour du centre du tube
|
||||
half = tr
|
||||
x1 = max(tx - half, 0)
|
||||
y1 = max(ty - half, 0)
|
||||
x2 = min(tx + half, w)
|
||||
y2 = min(ty + half, h)
|
||||
|
||||
cropped = frame[y1:y2, x1:x2]
|
||||
|
||||
# Redimensionne à la taille originale pour ne pas changer le pipeline
|
||||
return cv2.resize(cropped, (w, h), interpolation=cv2.INTER_LINEAR)
|
||||
|
||||
def _annotate(
|
||||
self,
|
||||
frame: np.ndarray,
|
||||
cx_img: int,
|
||||
cy_img: int,
|
||||
circle: tuple | None,
|
||||
offset_x: int = 0,
|
||||
offset_y: int = 0,
|
||||
) -> np.ndarray:
|
||||
"""
|
||||
Dessine le cercle détecté, le centre image et le vecteur de décalage.
|
||||
"""
|
||||
# Croix centre image
|
||||
cv2.drawMarker(
|
||||
frame, (cx_img, cy_img),
|
||||
(0, 255, 0), cv2.MARKER_CROSS, 20, 1, cv2.LINE_AA
|
||||
)
|
||||
|
||||
if circle is None:
|
||||
cv2.putText(frame, "Tube non detecte", (10, 30),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 1, cv2.LINE_AA)
|
||||
return frame
|
||||
|
||||
tx, ty, tr = circle
|
||||
|
||||
# Cercle du tube en cyan
|
||||
cv2.circle(frame, (tx, ty), tr, (255, 255, 0), 2, cv2.LINE_AA)
|
||||
|
||||
# Centre du tube en rouge
|
||||
cv2.circle(frame, (tx, ty), 4, (0, 0, 255), -1, cv2.LINE_AA)
|
||||
|
||||
# Vecteur décalage (centre image → centre tube)
|
||||
if abs(offset_x) > 2 or abs(offset_y) > 2:
|
||||
cv2.arrowedLine(
|
||||
frame,
|
||||
(cx_img, cy_img),
|
||||
(tx, ty),
|
||||
(0, 100, 255), 2, cv2.LINE_AA, tipLength=0.2
|
||||
)
|
||||
|
||||
# Texte décalage
|
||||
dist = np.sqrt(offset_x**2 + offset_y**2)
|
||||
cv2.putText(
|
||||
frame,
|
||||
f"dx={offset_x:+d}px dy={offset_y:+d}px dist={dist:.1f}px",
|
||||
(10, 28),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.45, (255, 255, 255), 1, cv2.LINE_AA,
|
||||
)
|
||||
cv2.putText(
|
||||
frame,
|
||||
f"r={tr}px",
|
||||
(10, 48),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.45, (200, 200, 200), 1, cv2.LINE_AA,
|
||||
)
|
||||
|
||||
return frame
|
||||
|
||||
|
||||
@@ -1,283 +0,0 @@
|
||||
'''
|
||||
Created on 17 avr. 2026
|
||||
|
||||
@author: denis
|
||||
'''
|
||||
# modules/tube_aligner.py
|
||||
|
||||
import cv2
|
||||
import logging
|
||||
import numpy as np
|
||||
|
||||
logger = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class TubeAligner:
|
||||
|
||||
GRBL_THRESHOLD_PX = 20
|
||||
DEAD_ZONE_PX = 5
|
||||
|
||||
def __init__(
|
||||
self,
|
||||
px_per_mm : float = 10.0,
|
||||
grbl_threshold_px : int = 20,
|
||||
dead_zone_px : int = 5,
|
||||
debug : bool = False, # ← activable depuis la vue
|
||||
):
|
||||
self.px_per_mm = px_per_mm
|
||||
self.grbl_threshold_px = grbl_threshold_px
|
||||
self.dead_zone_px = dead_zone_px
|
||||
self.debug = debug
|
||||
|
||||
# Etat calibration
|
||||
self._calib_step = 0 # 0=idle 1=point A enregistré
|
||||
self._calib_pos_A_px = None # centre tube point A en px
|
||||
self._calib_mpos_A = None # position CNC point A en mm
|
||||
|
||||
# ------------------------------------------------------------------ #
|
||||
# Détection principale
|
||||
# ------------------------------------------------------------------ #
|
||||
|
||||
def detect_tube(self, frame: np.ndarray) -> dict:
|
||||
h, w = frame.shape[:2]
|
||||
cx_img = w // 2
|
||||
cy_img = h // 2
|
||||
|
||||
result = {
|
||||
"detected" : False,
|
||||
"tube_cx" : None,
|
||||
"tube_cy" : None,
|
||||
"tube_radius" : None,
|
||||
"offset_x_px" : 0,
|
||||
"offset_y_px" : 0,
|
||||
"offset_x_mm" : 0.0,
|
||||
"offset_y_mm" : 0.0,
|
||||
"action" : "none",
|
||||
"frame_annotated": None,
|
||||
}
|
||||
|
||||
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
|
||||
blurred = cv2.GaussianBlur(gray, (15, 15), 3)
|
||||
|
||||
circles = cv2.HoughCircles(
|
||||
blurred,
|
||||
cv2.HOUGH_GRADIENT,
|
||||
dp = 1.2,
|
||||
minDist = min(w, h) // 2,
|
||||
param1 = 50,
|
||||
param2 = 30,
|
||||
minRadius = int(min(w, h) * 0.26),
|
||||
maxRadius = int(min(w, h) * 0.36),
|
||||
)
|
||||
|
||||
frame_out = frame.copy()
|
||||
|
||||
if circles is None:
|
||||
logger.warning("TubeAligner: aucun cercle détecté")
|
||||
if self.debug:
|
||||
frame_out = self._draw_debug_no_detection(frame_out, cx_img, cy_img)
|
||||
result["frame_annotated"] = frame_out
|
||||
return result
|
||||
|
||||
circles = np.round(circles[0, :]).astype(int)
|
||||
best = min(circles, key=lambda c: np.sqrt((c[0]-cx_img)**2 + (c[1]-cy_img)**2))
|
||||
tx, ty, tr = int(best[0]), int(best[1]), int(best[2])
|
||||
|
||||
offset_x_px = tx - cx_img
|
||||
offset_y_px = ty - cy_img
|
||||
offset_x_mm = offset_x_px / self.px_per_mm
|
||||
offset_y_mm = offset_y_px / self.px_per_mm
|
||||
dist_px = np.sqrt(offset_x_px**2 + offset_y_px**2)
|
||||
|
||||
if dist_px <= self.dead_zone_px:
|
||||
action = "none"
|
||||
elif dist_px <= self.grbl_threshold_px:
|
||||
action = "crop"
|
||||
else:
|
||||
action = "grbl"
|
||||
|
||||
if self.debug:
|
||||
frame_out = self._draw_debug(
|
||||
frame_out, cx_img, cy_img,
|
||||
tx, ty, tr,
|
||||
offset_x_px, offset_y_px,
|
||||
offset_x_mm, offset_y_mm,
|
||||
dist_px, action,
|
||||
)
|
||||
|
||||
|
||||
result.update({
|
||||
"detected" : True,
|
||||
"tube_cx" : tx,
|
||||
"tube_cy" : ty,
|
||||
"tube_radius" : tr,
|
||||
"offset_x_px" : offset_x_px,
|
||||
"offset_y_px" : offset_y_px,
|
||||
"offset_x_mm" : round(offset_x_mm, 3),
|
||||
"offset_y_mm" : round(offset_y_mm, 3),
|
||||
"action" : action,
|
||||
"frame_annotated": frame_out,
|
||||
})
|
||||
return result
|
||||
|
||||
# ------------------------------------------------------------------ #
|
||||
# Calibration px/mm — 2 points
|
||||
# ------------------------------------------------------------------ #
|
||||
|
||||
def calib_record_point_A(self, detection: dict, mpos: tuple) -> bool:
|
||||
"""
|
||||
Enregistre le point A (position CNC + centre tube en px).
|
||||
Appeler quand la CNC est immobile sur le point A.
|
||||
|
||||
:param detection: résultat de detect_tube()
|
||||
:param mpos: (x_mm, y_mm) retourné par cnc.get_mpos()
|
||||
:return: True si enregistré
|
||||
"""
|
||||
if not detection["detected"]:
|
||||
logger.warning("calib_record_point_A: tube non détecté")
|
||||
return False
|
||||
|
||||
self._calib_pos_A_px = (detection["tube_cx"], detection["tube_cy"])
|
||||
self._calib_mpos_A = mpos
|
||||
self._calib_step = 1
|
||||
logger.info("Calibration point A : px=%s mpos=%s", self._calib_pos_A_px, mpos)
|
||||
return True
|
||||
|
||||
def calib_record_point_B(self, detection: dict, mpos: tuple) -> dict | None:
|
||||
"""
|
||||
Enregistre le point B et calcule px_per_mm.
|
||||
Appeler après déplacement CNC manuel d'une distance connue.
|
||||
|
||||
:param detection: résultat de detect_tube()
|
||||
:param mpos: (x_mm, y_mm) retourné par cnc.get_mpos()
|
||||
:return: dict résultat calibration ou None si échec
|
||||
"""
|
||||
if self._calib_step != 1:
|
||||
logger.warning("calib_record_point_B: point A non enregistré")
|
||||
return None
|
||||
|
||||
if not detection["detected"]:
|
||||
logger.warning("calib_record_point_B: tube non détecté")
|
||||
return None
|
||||
|
||||
pos_B_px = (detection["tube_cx"], detection["tube_cy"])
|
||||
mpos_B = mpos
|
||||
|
||||
# Déplacement en px
|
||||
dpx = np.sqrt(
|
||||
(pos_B_px[0] - self._calib_pos_A_px[0])**2 +
|
||||
(pos_B_px[1] - self._calib_pos_A_px[1])**2
|
||||
)
|
||||
# Déplacement en mm (distance euclidienne CNC)
|
||||
dmm = np.sqrt(
|
||||
(mpos_B[0] - self._calib_mpos_A[0])**2 +
|
||||
(mpos_B[1] - self._calib_mpos_A[1])**2
|
||||
)
|
||||
|
||||
if dmm < 0.1 or dpx < 2:
|
||||
logger.warning("Déplacement trop faible : dpx=%.1f dmm=%.3f", dpx, dmm)
|
||||
return None
|
||||
|
||||
px_per_mm_new = dpx / dmm
|
||||
self.px_per_mm = px_per_mm_new
|
||||
self._calib_step = 0
|
||||
|
||||
result = {
|
||||
"px_per_mm" : round(px_per_mm_new, 4),
|
||||
"mm_per_px" : round(dmm / dpx, 6),
|
||||
"delta_px" : round(dpx, 2),
|
||||
"delta_mm" : round(dmm, 3),
|
||||
"point_A_px" : self._calib_pos_A_px,
|
||||
"point_B_px" : pos_B_px,
|
||||
"mpos_A" : self._calib_mpos_A,
|
||||
"mpos_B" : mpos_B,
|
||||
}
|
||||
logger.info("Calibration OK : %.4f px/mm (%.6f mm/px)", px_per_mm_new, dmm/dpx)
|
||||
return result
|
||||
|
||||
def calib_reset(self):
|
||||
self._calib_step = 0
|
||||
self._calib_pos_A_px = None
|
||||
self._calib_mpos_A = None
|
||||
|
||||
# ------------------------------------------------------------------ #
|
||||
# Dessin debug
|
||||
# ------------------------------------------------------------------ #
|
||||
|
||||
def _draw_debug(
|
||||
self, frame, cx_img, cy_img,
|
||||
tx, ty, tr,
|
||||
offset_x_px, offset_y_px,
|
||||
offset_x_mm, offset_y_mm,
|
||||
dist_px, action,
|
||||
) -> np.ndarray:
|
||||
|
||||
# Couleur selon action
|
||||
color = {
|
||||
"none" : (0, 255, 0), # vert — centré
|
||||
"crop" : (0, 200, 255), # orange — recadrage
|
||||
"grbl" : (0, 0, 255), # rouge — correction CNC
|
||||
}.get(action, (200, 200, 200))
|
||||
|
||||
# Cercle intérieur du tube
|
||||
cv2.circle(frame, (tx, ty), tr, color, 2, cv2.LINE_AA)
|
||||
|
||||
# Rayon de zone morte (dead zone) en vert clair
|
||||
cv2.circle(frame, (cx_img, cy_img), self.dead_zone_px,
|
||||
(0, 255, 100), 1, cv2.LINE_AA)
|
||||
|
||||
# Rayon seuil GRBL en rouge pointillé (simulé par cercle fin)
|
||||
cv2.circle(frame, (cx_img, cy_img), self.grbl_threshold_px,
|
||||
(0, 80, 255), 1, cv2.LINE_AA)
|
||||
|
||||
# Croix centre image
|
||||
cv2.drawMarker(frame, (cx_img, cy_img),
|
||||
(255, 255, 255), cv2.MARKER_CROSS, 24, 1, cv2.LINE_AA)
|
||||
|
||||
# Centre tube
|
||||
cv2.circle(frame, (tx, ty), 5, color, -1, cv2.LINE_AA)
|
||||
|
||||
# Vecteur offset centre image → centre tube
|
||||
if dist_px > self.dead_zone_px:
|
||||
cv2.arrowedLine(frame, (cx_img, cy_img), (tx, ty),
|
||||
color, 2, cv2.LINE_AA, tipLength=0.2)
|
||||
|
||||
# Panneau info — fond semi-transparent
|
||||
overlay = frame.copy()
|
||||
cv2.rectangle(overlay, (8, 8), (400, 130), (0, 0, 0), -1)
|
||||
cv2.addWeighted(overlay, 0.45, frame, 0.55, 0, frame)
|
||||
|
||||
lines = [
|
||||
(f"Tube cx={tx} cy={ty} r={tr}px", (0, 255, 180)),
|
||||
(f"Offset dx={offset_x_px:+d}px dy={offset_y_px:+d}px", color),
|
||||
(f"Offset dx={offset_x_mm:+.3f}mm dy={offset_y_mm:+.3f}mm", color),
|
||||
(f"Dist={dist_px:.1f}px action={action.upper()}", color),
|
||||
(f"px/mm={self.px_per_mm:.4f}", (180, 180, 180)),
|
||||
]
|
||||
for i, (text, col) in enumerate(lines):
|
||||
cv2.putText(frame, text, (14, 30 + i * 20),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.48, col, 1, cv2.LINE_AA)
|
||||
|
||||
# Légende zones
|
||||
cv2.putText(frame, "dead zone", (cx_img + self.dead_zone_px + 3, cy_img - 3),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.35, (0, 255, 100), 1)
|
||||
cv2.putText(frame, "GRBL threshold", (cx_img + self.grbl_threshold_px + 3, cy_img - 3),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.35, (0, 80, 255), 1)
|
||||
|
||||
# Indicateur calibration en cours
|
||||
if self._calib_step == 1:
|
||||
cv2.putText(frame, "CALIB — En attente point B",
|
||||
(14, frame.shape[0] - 14),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.55, (0, 200, 255), 2, cv2.LINE_AA)
|
||||
|
||||
return frame
|
||||
|
||||
def _draw_debug_no_detection(self, frame, cx_img, cy_img) -> np.ndarray:
|
||||
cv2.drawMarker(frame, (cx_img, cy_img),
|
||||
(255, 255, 255), cv2.MARKER_CROSS, 24, 1, cv2.LINE_AA)
|
||||
cv2.putText(frame, "Tube non detecte", (14, 30),
|
||||
cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 0, 255), 2, cv2.LINE_AA)
|
||||
return frame
|
||||
|
||||
|
||||
|
||||
@@ -40,8 +40,8 @@ class VideoFileCapture(VideoCaptureInterface):
|
||||
height: Optional[int] = None,
|
||||
video_lists = [],
|
||||
use_tracking: bool = False,
|
||||
px_per_mm: float = 2.1,
|
||||
display = None,
|
||||
parent = None,
|
||||
):
|
||||
"""
|
||||
:param video_file: fichier video
|
||||
@@ -50,7 +50,7 @@ class VideoFileCapture(VideoCaptureInterface):
|
||||
:param width: Largeur souhaitée (None = valeur par défaut du pilote)
|
||||
:param height: Hauteur souhaitée (None = valeur par défaut du pilote)
|
||||
"""
|
||||
super().__init__(fps=fps, use_tracking=use_tracking, px_per_mm=px_per_mm, display=display)
|
||||
super().__init__(fps=fps, use_tracking=use_tracking, display=display, parent=parent)
|
||||
self._video_file: str = video_file
|
||||
self._jpeg_quality: int = jpeg_quality
|
||||
self._width: Optional[int] = width
|
||||
|
||||
@@ -39,8 +39,8 @@ class WebcamCapture(VideoCaptureInterface):
|
||||
width: Optional[int] = None,
|
||||
height: Optional[int] = None,
|
||||
use_tracking: bool = False,
|
||||
px_per_mm: float = 2.1,
|
||||
display = None,
|
||||
parent = None,
|
||||
):
|
||||
"""
|
||||
:param device_index: Index du périphérique V4L2 (0 = première webcam)
|
||||
@@ -49,7 +49,7 @@ class WebcamCapture(VideoCaptureInterface):
|
||||
:param width: Largeur souhaitée (None = valeur par défaut du pilote)
|
||||
:param height: Hauteur souhaitée (None = valeur par défaut du pilote)
|
||||
"""
|
||||
super().__init__(fps=fps, use_tracking=use_tracking, px_per_mm=px_per_mm, display=display)
|
||||
super().__init__(fps=fps, use_tracking=use_tracking, display=display, parent=parent)
|
||||
self._device_index: int = device_index
|
||||
self._jpeg_quality: int = jpeg_quality
|
||||
self._width: Optional[int] = width
|
||||
|
||||
Reference in New Issue
Block a user