Add marker distance input to use trilateration for distance estimatio…

…n WIP

foosball/__init__.py

@@ -7,7 +7,7 @@
 from const import CALIBRATION_MODE, CALIBRATION_IMG_PATH, CALIBRATION_VIDEO, CALIBRATION_SAMPLE_SIZE, ARUCO_BOARD, \
     FILE, CAMERA_ID, FRAMERATE, OUTPUT, CAPTURE, DISPLAY, BALL, XPAD, YPAD, SCALE, VERBOSE, HEADLESS, OFF, \
     MAX_PIPE_SIZE, INFO_VERBOSITY, GPU, AUDIO, WEBHOOK, BUFFER, BallPresets, CalibrationMode
-from foosball.arUcos.calibration import print_aruco_board, calibrate_camera
+from foosball.arUcos.camera_calibration import print_aruco_board, calibrate_camera
 from foosball.tracking.ai import AI
 
 logging.config.fileConfig("logging.ini")
@@ -64,6 +64,7 @@ def get_argparse():
     tracker.add_argument("-b", f"--{BUFFER}", type=int, default=16, help="max track buffer size")
 
     preprocess = ap.add_argument_group(title="Preprocessor", description="Options for the preprocessing step")
+    preprocess.add_argument("-dis", "--distance", type=float, default=125.0, help="Distance between ArucoMarker top left (1) and top right (2)")
     preprocess.add_argument("-xp", f"--{XPAD}", type=int, default=50,
                     help="Horizontal padding applied to ROI detected by aruco markers")
     preprocess.add_argument("-yp", f"--{YPAD}", type=int, default=20,

foosball/arUcos/calibration.py → foosball/arUcos/camera_calibration.py

@@ -27,7 +27,7 @@
 CALIBRATION_YAML = 'calibration.yaml'
 
 
-class Calibration:
+class CameraCalibration:
     camera_matrix: np.array = None
     dist_coefficients: np.array = None
     _image_markers: List[List[Aruco]] = []
@@ -109,7 +109,7 @@ def as_string(self):
         return f"camera_matrix={self.camera_matrix}\ndist_coeff={self.dist_coefficients}\n# marker_images={len(self._image_markers)}"
 
 
-def draw_markers(img: Frame, arucos: list[Aruco], calib: Calibration = None) -> Frame:
+def draw_markers(img: Frame, arucos: list[Aruco], calib: CameraCalibration = None) -> Frame:
     if len(arucos) > 0:
         corners = np.array([a.corners for a in arucos])
         ids = np.array([a.id for a in arucos])
@@ -124,24 +124,24 @@ def draw_markers(img: Frame, arucos: list[Aruco], calib: Calibration = None) ->
     return img
 
 
-def detect_markers(image, detector: aruco.ArucoDetector, calib: Calibration) -> list[Aruco]:
+def detect_markers(image, detector: aruco.ArucoDetector, calib: CameraCalibration, marker_length_cm: float = DEFAULT_MARKER_LENGTH_CM) -> list[Aruco]:
     corners, ids, rejected_img_points = detector.detectMarkers(image)
     ids = ensure_cpu(ids)
     # if rejected_img_points is not None:
     #     logger.debug(f"Marker detection rejected {len(rejected_img_points)}")
 
     if ids is not None:
         arucos = [Aruco(np.array(i[0]), c, None, None) for i, c in list(zip(ids, corners))]
-        return estimate_markers_poses(arucos, calib)
+        return estimate_markers_poses(arucos, calib, marker_length_cm)
     else:
         return []
 
 
-def estimate_markers_poses(arucos: List[Aruco], calib: Calibration):
+def estimate_markers_poses(arucos: List[Aruco], calib: CameraCalibration, marker_length_cm: float):
     corners = np.array([a.corners for a in arucos])
     # Estimate pose of each marker and return the values rotation_vector and translation_vector
     # (different from those of camera coefficients)
-    rotation_vectors, translation_vectors, marker_points = aruco.estimatePoseSingleMarkers(corners, 0.02,  # Assuming 1 pixel = 1 millimeter
+    rotation_vectors, translation_vectors, marker_points = aruco.estimatePoseSingleMarkers(corners, marker_length_cm,
                                                                                            calib.camera_matrix,
                                                                                            calib.dist_coefficients)
     if rotation_vectors is not None and translation_vectors is not None:
@@ -200,7 +200,7 @@ def calibrate_camera(camera_id=None, calibration_video_path=None, calibration_im
             img_list.append(img)
             # h, w, c = img.shape
         logger.debug(f'Calibrating {len(img_list)} images')
-        calib = Calibration(board)
+        calib = CameraCalibration(board)
         for idx, im in enumerate(tqdm(img_list)):
             logger.debug(f"Calibrating {idx} {fns[idx]}")
             img_gray = cv2.cvtColor(im, cv2.COLOR_RGB2GRAY)
@@ -214,7 +214,7 @@ def calibrate_camera(camera_id=None, calibration_video_path=None, calibration_im
             calib.store()
     # if camera_id given calibrate with live footage
     elif calibration_video_path is not None or camera_id is not None:
-        calib = Calibration(board).load()
+        calib = CameraCalibration(board).load()
         camera = cv2.VideoCapture(calibration_video_path if calibration_video_path is not None else camera_id)
         start = time.time()
         ret, img = camera.read()

foosball/tracking/__init__.py

@@ -30,14 +30,14 @@ def generate_frame_mask(width, height) -> Mask:
 
 class Tracking:
 
-    def __init__(self, stream, dims: FrameDimensions, goal_detector: GoalDetector, ball_detector: BallDetector, headless=False, maxPipeSize=128, calibrationMode=None, **kwargs):
+    def __init__(self, stream, dims: FrameDimensions, goal_detector: GoalDetector, ball_detector: BallDetector, headless=False, distance=125.0, maxPipeSize=128, calibrationMode=None, **kwargs):
         super().__init__()
         self.calibrationMode = calibrationMode
 
         width, height = dims.scaled
         mask = generate_frame_mask(width, height)
         gpu_flags = kwargs.get(GPU)
-        self.preprocessor = PreProcessor(dims, goal_detector, mask=mask, headless=headless, useGPU='preprocess' in gpu_flags,
+        self.preprocessor = PreProcessor(dims, distance, goal_detector, mask=mask, headless=headless, useGPU='preprocess' in gpu_flags,
                                          calibrationMode=calibrationMode, **kwargs)
         self.tracker = Tracker(ball_detector, useGPU='tracker' in gpu_flags, calibrationMode=calibrationMode, **kwargs)
         self.analyzer = Analyzer(**kwargs)

foosball/tracking/preprocess.py

@@ -8,10 +8,11 @@
 import numpy as np
 
 from const import CalibrationMode, OFF
-from ..arUcos import calibration, Calibration
-from ..arUcos.models import Aruco
-from ..detectors.color import GoalDetector, GoalColorConfig
-from ..models import Frame, PreprocessResult, Point, Rect, Blob, Goals, FrameDimensions, ScaleDirection, \
+from ..arUcos.camera_calibration import CameraCalibration, init_aruco_detector, detect_markers
+from ..models import Aruco, Point3D
+from ..detectors import GoalDetector
+from ..detectors.color import GoalColorConfig
+from ..models import Frame, Point, Rect, Blob, Goals, FrameDimensions, ScaleDirection, \
     InfoLog, Info, Verbosity
 from ..pipe.BaseProcess import BaseProcess, Msg
 from ..pipe.Pipe import clear
@@ -46,12 +47,13 @@ def pad_rect(rectangle: Rect, xpad: int, ypad: int) -> Rect:
 
 
 class PreProcessor(BaseProcess):
-    def __init__(self, dims: FrameDimensions, goal_detector: GoalDetector, headless=True, mask=None, used_markers=None,
+    def __init__(self, dims: FrameDimensions, distance: float, goal_detector: GoalDetector, headless=True, mask=None, used_markers=None,
                  redetect_markers_frames: int = 60, aruco_dictionary=cv2.aruco.DICT_4X4_1000,
                  aruco_params=cv2.aruco.DetectorParameters(), xpad: int = 50, ypad: int = 20,
                  goal_change_threshold: float = 0.10, useGPU: bool = False, calibrationMode=None, verbose=False, **kwargs):
         super().__init__(name="Preprocess")
         self.dims = dims
+        self.distance = distance
         self.goal_change_threshold = goal_change_threshold
         self.redetect_markers_frame_threshold = redetect_markers_frames
         if used_markers is None:
@@ -62,14 +64,14 @@ def __init__(self, dims: FrameDimensions, goal_detector: GoalDetector, headless=
         self.xpad = xpad
         self.ypad = ypad
         [self.proc, self.iproc] = generate_processor_switches(useGPU)
-        self.detector, _ = calibration.init_aruco_detector(aruco_dictionary, aruco_params)
+        self.detector, _ = init_aruco_detector(aruco_dictionary, aruco_params)
         self.markers = []
         self.position_estimation_inputs = None
         self.homography_matrix = None
         self.frames_since_last_marker_detection = 0
         self.goals = None
         self.calibrationMode = calibrationMode
-        self.camera_calibration = Calibration().load()
+        self.camera_calibration = CameraCalibration().load()
         self.verbose = verbose
         self.goals_calibration = self.calibrationMode == CalibrationMode.GOAL
         self.calibration_out = Queue() if self.goals_calibration else None
@@ -89,7 +91,7 @@ def close(self) -> None:
 
     def detect_markers(self, frame: Frame) -> list[Aruco]:
         img_gray = cv2.cvtColor(frame, cv2.COLOR_RGB2GRAY)
-        return calibration.detect_markers(img_gray, self.detector, self.camera_calibration)
+        return detect_markers(img_gray, self.detector, self.camera_calibration)
 
     def mask_frame(self, frame: Frame) -> Frame:
         """
@@ -161,7 +163,7 @@ def process(self, msg: Msg) -> Msg:
             "original": self.iproc(frame),
             "preprocessed": self.iproc(preprocessed),
             "arucos": self.markers,
-            "calibration": self.calibration,
+            "calibration": self.camera_calibration,
             "homography_matrix": self.homography_matrix,  # 3x3 matrix used to warp the image and project points
             "goals": self.goals,
             "info": info,
@@ -217,7 +219,7 @@ def four_point_transform(self, frame: Frame, markers: list[Aruco]) -> tuple[Fram
 
     def calc_3d_position_inputs(self, arucos: list[Aruco]) -> PositionEstimationInputs:
         aruco_marker_size_cm = 5.0  # TODO let this come from somewhere fixed or arguments
-        aruco_points = [corners2pt(a.corners) for a in arucos]
+        # aruco_points = [corners2pt(a.corners) for a in arucos]
         # Define the transformation matrices for the ArUco markers
         transformation_matrices = []
         marker_positions_3d = []
@@ -228,11 +230,18 @@ def calc_3d_position_inputs(self, arucos: list[Aruco]) -> PositionEstimationInpu
             T = arucos[i].translation_vector * scale_factor
             extrinsic_matrix = np.hstack((R, T.reshape(3, 1)))
             transformation_matrices.append(extrinsic_matrix)
-            homogeneous_marker = np.hstack((aruco_points[i], 1))
-            marker_position_3d = np.dot(np.linalg.inv(self.camera_calibration.camera_matrix), homogeneous_marker)
-            marker_positions_3d.append(marker_position_3d)
+            marker_positions_3d.append(self.project_2d_to_3d(corners2pt(arucos[i].corners)))
         return PositionEstimationInputs(transformation_matrices, marker_positions_3d)
 
+    def project_2d_to_3d(self, point_2d) -> Point3D:
+        # Assuming point_2d is a NumPy array [x, y]
+        x, y = point_2d
+        z = self.distance  # Real-world distance between marker 1 and 2 (top left, top right)
+        homogeneous_point = np.array([x, y, 1])
+        inverse_intrinsic = np.linalg.inv(self.camera_calibration.camera_matrix)
+        point_homogeneous = np.dot(inverse_intrinsic, homogeneous_point)
+        point_3d = z * point_homogeneous
+        return point_3d
 
 def corners2pt(corners) -> [int, int]:
     moments = cv2.moments(corners)

foosball/tracking/render.py

@@ -6,8 +6,8 @@
 
 from const import INFO_VERBOSITY
 from .preprocess import corners2pt, PositionEstimationInputs
-from ..arUcos.calibration import draw_markers
-from ..models import Goal, Score, FrameDimensions, Blob, InfoLog, Verbosity
+from ..arUcos.camera_calibration import draw_markers
+from ..models import Goal, Score, FrameDimensions, Blob, InfoLog, Verbosity, Frame, Aruco
 from ..pipe.BaseProcess import Msg, BaseProcess
 from ..utils import generate_processor_switches
 logger = logging.getLogger(__name__)
@@ -44,7 +44,7 @@ def r_info(frame, shape: tuple[int, int, int], info: InfoLog, text_scale=1.0, th
     for i in info:
         text = i.to_string()
         if w + x > width:
-            cv2.rectangle(frame, (x, y), (width, y - h), BLACK, -1) # fill
+            cv2.rectangle(frame, (x, y), (width, y - h), BLACK, -1)  # fill
             x = 0
             y = y - h
         [x0, _, w, h] = r_text(frame, text, x, y, text_color(text, i.value), text_scale=text_scale, thickness=thickness, background=BLACK, padding=(20, 20), ground_zero='bl')
@@ -112,9 +112,10 @@ def __init__(self, dims: FrameDimensions, headless=False, useGPU: bool = False,
         self.infoVerbosity = Verbosity(kwargs.get(INFO_VERBOSITY)) if kwargs.get(INFO_VERBOSITY) else None
         [self.proc, self.iproc] = generate_processor_switches(useGPU)
 
-    def r_distance(self, frame: Frame, arucos: list[Aruco], position_estimation_inputs: PositionEstimationInputs):
+    @staticmethod
+    def r_distance(frame: Frame, arucos: list[Aruco], position_estimation_inputs: PositionEstimationInputs):
         if arucos is not None:
-            (i1, a1), (i2,a2) = [(i, a) for i, a in enumerate(arucos) if a.id in [0, 3]]
+            (i1, a1), (i2, a2) = [(i, a) for i, a in enumerate(arucos) if a.id in [0, 3]]
             apt1 = np.array(corners2pt(a1.corners))
             apt2 = np.array(corners2pt(a2.corners))
             cv2.line(frame, apt1, apt2, ORANGE, 2, 1)
@@ -125,7 +126,6 @@ def r_distance(self, frame: Frame, arucos: list[Aruco], position_estimation_inpu
             pos = [int(x) for x in (apt1 + apt2) / 2]
             r_text(frame, f'{d:2f} cm', pos[0], pos[1], ORANGE, thickness=2)
 
-
     def process(self, msg: Msg) -> Msg:
         analyze_result = msg.kwargs['result']
         arucos = msg.kwargs.get('arucos')

foosball/tracking/tracker.py

@@ -4,13 +4,15 @@
 from queue import Empty
 
 import cv2
+import numpy as np
+
 from const import CalibrationMode
 from cv2.typing import Point3d
 
-from .preprocess import WarpMode, project_blob
-from ..arUcos import Calibration
+from .preprocess import WarpMode, project_blob, PositionEstimationInputs
+from ..arUcos.camera_calibration import CameraCalibration
 from ..detectors.color import BallColorDetector, BallColorConfig
-from ..models import TrackResult, Track, Info, Blob, Goals, InfoLog, Verbosity
+from ..models import TrackResult, Track, Info, Blob, Goals, InfoLog, Verbosity, Point, Point3D
 from ..pipe.BaseProcess import BaseProcess, Msg
 from ..pipe.Pipe import clear
 from ..utils import generate_processor_switches
@@ -32,7 +34,7 @@ def __init__(self, ball_detector: BallColorDetector, useGPU: bool = False, buffe
         self.off = off
         self.verbose = verbose
         self.calibrationMode = calibrationMode
-        self.camera_matrix = Calibration().load().camera_matrix
+        self.camera_matrix = CameraCalibration().load().camera_matrix
         self.last_timestamp = None
         [self.proc, self.iproc] = generate_processor_switches(useGPU)
         # define the lower_ball and upper_ball boundaries of the
@@ -110,7 +112,7 @@ def process(self, msg: Msg) -> Msg:
         ball = None
         goals = msg.kwargs['goals']
         ball_track = None
-        info = None
+        info = InfoLog(infos=[])
         speed = None
         try:
             if not self.off:
@@ -119,7 +121,7 @@ def process(self, msg: Msg) -> Msg:
                         self.ball_detector.config = self.bounds_in.get_nowait()
                     except Empty:
                         pass
-                f = self.proc(preprocess_result.preprocessed if preprocess_result.preprocessed is not None else preprocess_result.original)
+                f = self.proc(preprocessed if preprocessed is not None else original)
                 ball_detection_result = self.ball_detector.detect(f)
                 ball = ball_detection_result.ball
                 # if we have some markers detected in preprocess step, we can determine the 3d position of the ball
@@ -141,8 +143,7 @@ def process(self, msg: Msg) -> Msg:
                 self.update_3d_ball_track(ball3d)
                 ball_track = self.update_2d_ball_track(ball).copy()
             speed = self.calc_speed(timestamp)
-            info = preprocess_result.info
-            info.concat(self.get_info(ball_track))
+            info.concat(self.get_info(speed))
             self.last_timestamp = timestamp
         except Exception as e:
             logger.error(f"Error in track {e}")