fix: replaced apriltags with pupil_apriltags

Author: juelg

.github/workflows/cpp.yaml

@@ -22,13 +22,6 @@ jobs:
           key: ${{ runner.os }}-apt-${{ hashFiles('debian_deps.txt') }}
           restore-keys: |
             ${{ runner.os }}-apt-
-      - name: Cache build directory
-        uses: actions/cache@v4
-        with:
-          path: build
-          key: ${{ runner.os }}-build-cpp-${{ hashFiles('src/**', 'include/**', 'CMakeLists.txt', 'pyproject.toml') }}
-          restore-keys: |
-            ${{ runner.os }}-build-cpp-
       - name: Install dependencies
         run: sudo apt install $(cat debian_deps.txt)
       - name: Set up Python 3.11

extensions/rcs_panda/CMakeLists.txt

@@ -25,6 +25,7 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
 set(CMAKE_CXX_STANDARD 20)
 set(CMAKE_CXX_STANDARD_REQUIRED ON)
 set(CMAKE_CXX_EXTENSIONS OFF)
+set(CMAKE_POLICY_VERSION_MINIMUM 3.5)
 
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
 

extensions/rcs_realsense/pyproject.toml

@@ -9,7 +9,7 @@ description="RCS realsense module"
 dependencies = [
     "rcs>=0.5.0",
     "pyrealsense2~=2.55.1",
-    "apriltag==0.0.16",
+    "pupil_apriltags",
     "diskcache",
 ]
 maintainers = [

extensions/rcs_realsense/src/rcs_realsense/calibration.py

@@ -4,7 +4,9 @@
 from pathlib import Path
 from time import sleep
 
-import apriltag
+# CHANGE 1: New Import
+from pupil_apriltags import Detector  
+
 import cv2
 import diskcache as dc
 import numpy as np
@@ -40,7 +42,7 @@ def calibrate(
         input()
         tries = 3
         while len(samples) < 10 and tries > 0:
-            logger.info("not enought frames in recorded, waiting 2 seconds...")
+            logger.info("not enough frames in recorded, waiting 2 seconds...")
             tries = -1
             sleep(2)
         if tries == 0:
@@ -51,7 +53,7 @@ def calibrate(
         with lock:
             for sample in samples:
                 frames.append(sample.camera.color.data.copy())
-        print(frames)
+        # print(frames) # Removed print for cleaner logs, optional
 
         _, tag_to_cam = get_average_marker_pose(frames, intrinsics=intrinsics, calib_tag_id=9, show_live_window=False)
 
@@ -71,9 +73,9 @@ def get_average_marker_pose(
     calib_tag_id,
     show_live_window,
 ):
-    # create detector
-    options = apriltag.DetectorOptions(families="tag25h9")
-    detector = apriltag.Detector(options=options)
+    # CHANGE 2: Simplified Initialization
+    # No "DetectorOptions" object needed anymore.
+    detector = Detector(families="tag25h9")
 
     # make while loop with tqdm
     poses = []
@@ -97,6 +99,7 @@ def get_average_marker_pose(
         camera_matrix = intrinsics[:3, :3]
 
         if show_live_window:
+            # Note: pose[:3, :3] works because we construct the 4x4 matrix in get_marker_pose below
             cv2.drawFrameAxes(frame, camera_matrix, None, pose[:3, :3], pose[:3, 3], 0.1)  # type: ignore
             # show frame
             cv2.imshow("frame", frame)
@@ -120,7 +123,20 @@ def get_average_marker_pose(
 
 def get_marker_pose(calib_tag_id, detector, intrinsics, frame):
     gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
-    detections = detector.detect(gray)
+    
+    # CHANGE 3: Pose estimation happens INSIDE .detect()
+    # We must extract camera params first to pass them here
+    fx = intrinsics[0, 0]
+    fy = intrinsics[1, 1]
+    cx = intrinsics[0, 2]
+    cy = intrinsics[1, 2]
+
+    detections = detector.detect(
+        gray,
+        estimate_tag_pose=True,
+        camera_params=[fx, fy, cx, cy],
+        tag_size=0.1
+    )
 
     # count detections
     n_det = 0
@@ -138,20 +154,10 @@ def get_marker_pose(calib_tag_id, detector, intrinsics, frame):
     if marker_det is None:
         return None, None
 
-    fx = intrinsics[0, 0]
-    fy = intrinsics[1, 1]
-    cx = intrinsics[0, 2]
-    cy = intrinsics[1, 2]
-
-    pose, _, _ = detector.detection_pose(
-        marker_det,
-        camera_params=(
-            fx,
-            fy,
-            cx,
-            cy,
-        ),
-        tag_size=0.1,
-    )
+    # CHANGE 4: Construct 4x4 Matrix manually
+    # pupil-apriltags gives us R (3x3) and t (3x1). We must stack them.
+    pose = np.eye(4)
+    pose[:3, :3] = marker_det.pose_R
+    pose[:3, 3] = marker_det.pose_t.ravel() # ravel() flattens the (3,1) array to (3,)
 
     return marker_det, pose