Marqueurs ArUco et pose 6D

Les marqueurs ArUco sont des motifs carrés noir et blanc qui encodent un identifiant et permettent d’estimer une pose 6D complète (position et orientation) d’une seule image. C’est la méthode de détection la plus robuste de la journée — le détecteur « filet de sécurité » pour le projet final.

1. Qu’est-ce qu’un marqueur ArUco ?

Un marqueur ArUco est une grille binaire (ex. 4×4) entourée d’un bord noir. La grille encode un ID ; le bord et les coins permettent de retrouver la géométrie du marqueur dans l’image. Tous les marqueurs d’une même famille appartiennent à un dictionnaire (ici DICT_4X4_50 : 50 marqueurs de 4×4 bits).

Deux atouts pour la robotique :

la classe est gratuite : c’est l’ID du marqueur (pas de seuillage couleur fragile) ;
la pose est 6D : connaissant la taille réelle du marqueur et les intrinsèques de la caméra, on retrouve sa position et son orientation.

Dans bootcamp_vision, l’objet aruco_parcel est un colis portant un marqueur 4×4_50 de 0,05 m de côté (valeur documentée dans le README).

# Terminal 1
ros2 launch bootcamp_vision vision_world.launch.py
# Terminal 2 — spawn d'un colis avec le marqueur d'ID 1
ros2 launch bootcamp_vision spawn_object.launch.py object_type:=aruco aruco_id:=1

2. Détecter les marqueurs

Le module aruco fait partie de opencv-contrib-python (installé à l’étape Installation, voir aussi le socle commun).

import cv2 as cv

aruco_dict = cv.aruco.getPredefinedDictionary(cv.aruco.DICT_4X4_50)
params = cv.aruco.DetectorParameters()
detector = cv.aruco.ArucoDetector(aruco_dict, params)

img = cv.imread("capture.png")
coins, ids, rejets = detector.detectMarkers(img)

if ids is not None:
    cv.aruco.drawDetectedMarkers(img, coins, ids)   # dessine bords + ID
    print("Marqueurs détectés :", ids.flatten())
cv.imshow("ArUco", img)
cv.waitKey(0)

coins contient, pour chaque marqueur, les 4 coins (en pixels) ; ids les identifiants correspondants.

Pour fabriquer un marqueur (utile en réel) :

marqueur = cv.aruco.generateImageMarker(aruco_dict, id=1, sidePixels=200)
cv.imwrite("aruco_1.png", marqueur)

bootcamp_vision fournit déjà generate_aruco.py pour produire les textures du monde.

3. Estimer la pose 6D

La détection donne des coins 2D. Pour la pose 3D, on résout le problème PnP (Perspective-n-Point) : on associe les 4 coins 2D aux 4 coins 3D du marqueur (connus grâce à sa taille), et solvePnP en déduit la transformation caméra→marqueur (rvec = rotation, tvec = translation).

import numpy as np

TAILLE = 0.05   # côté du marqueur en mètres (README bootcamp_vision)

# Intrinsèques de la caméra (cf. /camera/camera_info)
K = np.array([[935.485, 0, 640.0],
              [0, 935.485, 360.0],
              [0,       0,     1.0]])
dist = np.zeros(5)   # pas de distorsion en simulation

# Coins 3D du marqueur dans son propre repère (centre à l'origine)
h = TAILLE / 2
objp = np.array([[-h,  h, 0], [h,  h, 0], [h, -h, 0], [-h, -h, 0]], dtype=np.float32)

for coin, mid in zip(coins, ids.flatten()):
    ok, rvec, tvec = cv.solvePnP(objp, coin[0], K, dist)
    if ok:
        cv.drawFrameAxes(img, K, dist, rvec, tvec, TAILLE)   # repère 3D sur l'image
        print(f"ID {mid} → position {tvec.flatten()}")

4. Publier sur `/detections`

On réutilise le squelette de nœud du socle commun. La position provient de tvec (translation caméra→marqueur) et — contrairement aux autres ateliers — l’orientation de rvec : ArUco remplit donc la pose 6D complète. La classe vient de l’ID :

from scipy.spatial.transform import Rotation

def on_image(self, msg):
    img = self.bridge.imgmsg_to_cv2(msg, "bgr8")
    coins, ids, _ = detector.detectMarkers(img)
    out = DetectionArray()
    out.header = msg.header
    if ids is not None:
        for coin, mid in zip(coins, ids.flatten()):
            ok, rvec, tvec = cv.solvePnP(objp, coin[0], K, dist)
            if not ok:
                continue
            tx, ty, tz = (float(c) for c in tvec.flatten())
            det = self.make_det(f"aruco_{int(mid)}", int(mid), 1.0,
                                coin[0][:, 0].mean(), coin[0][:, 1].mean(), 0.0, 0.0)
            # position : translation du marqueur (écrase la back-projection)
            det.pose.position.x, det.pose.position.y, det.pose.position.z = tx, ty, tz
            # orientation 6D : rvec (rotation vector) -> quaternion
            qx, qy, qz, qw = Rotation.from_rotvec(rvec.flatten()).as_quat()
            det.pose.orientation.x, det.pose.orientation.y = float(qx), float(qy)
            det.pose.orientation.z, det.pose.orientation.w = float(qz), float(qw)
            out.detections.append(det)
    self.pub.publish(out)

Testez :

ros2 run mon_projet_vision aruco_detector
ros2 topic echo /detections

Le class_name doit valoir aruco_1 (ou l’ID spawné), la pose.position la translation réelle du marqueur et la pose.orientation son orientation.

Pour aller plus loin

Spawnez plusieurs colis avec des IDs différents et publiez une Detection par marqueur.
Stabilisez la pose en moyennant sur plusieurs images (filtrage).
Envie d’IA plutôt que de marqueurs ? Tentez un autre atelier : YOLO ou Chiffres. Pour la vision classique sans marqueur : Formes.

Cours conçu et animé par Etienne Schmitz