Skip to content

AprilTag Recognition

AprilTag is an image-based identification system similar to QR codes but designed specifically for robotics vision and augmented reality (AR) applications. It was developed by the APRIL Laboratory at the University of Michigan to quickly and accurately detect and identify target objects through cameras.

This section will introduce how to recognize AprilTag content through visual recognition.

Running the Example

You can see three AprilTag codes on the white map included in the kit. Let's power on the OriginMan and then launch the following command in MoboXterm or VSCode:

ros2 run originman_apriltag_detect apriltag_detect

image-20220923124819442

Place OriginMan in front of the AprilTag code on the map. After running, you'll first see OriginMan lower its head to observe the AprilTag code in its field of view.

Code Implementation

The code is located at: originman_apriltag_detect/apriltag_detect.py

#!/usr/bin/env python3
# encoding:utf-8
import rclpy
from rclpy.node import Node
import cv2
from cv_bridge import CvBridge
from sensor_msgs.msg import Image
from std_msgs.msg import String
import apriltag
import numpy as np

class AprilTagDetectNode(Node):
    def __init__(self):
        super().__init__('image_publisher_node')
        self.bridge = CvBridge()
        # Create publisher for image_raw topic
        self.publisher_ = self.create_publisher(Image, 'image_raw', 10)
        # Create publisher for AprilTag recognition information
        self.apriltag_info_pub = self.create_publisher(String, 'apriltag_info', 10)
        # Open camera, 0 represents default camera
        self.cap = cv2.VideoCapture(0)
        if not self.cap.isOpened():
            self.get_logger().error("Cannot open camera")
        else:
            # Create timer to publish images periodically
            self.timer = self.create_timer(0.1, self.publish_image)
            # Flag to mark first image publication
            self.is_first_publish = True

        # Initialize AprilTag detector
        self.detector = apriltag.Detector(searchpath=apriltag._get_demo_searchpath())

    def publish_image(self):
        ret, frame = self.cap.read()
        if ret:
            try:
                # Perform AprilTag detection
                gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
                detections = self.detector.detect(gray, return_image=False)

                apriltag_info_msg = String()
                if len(detections) != 0:
                    tag_info_list = []
                    for detection in detections:
                        corners = np.int0(detection.corners)  # Get four corners
                        cv2.drawContours(frame, [np.array(corners, int)], -1, (0, 255, 255), 2)

                        tag_family = str(detection.tag_family, encoding='utf-8')  # Get tag_family
                        tag_id = int(detection.tag_id)  # Get tag_id

                        object_center_x, object_center_y = int(detection.center[0]), int(detection.center[1])  # Center point

                        object_angle = int(np.degrees(np.arctan2(corners[0][1] - corners[1][1], corners[0][0] - corners[1][0])))  # Calculate rotation angle

                        self.get_logger().info(f"Detected AprilTag - ID: {tag_id}, Family: {tag_family}")

                        # Draw label information on image
                        cv2.putText(frame, f"tag_id: {tag_id}", (10, frame.shape[0] - 30), cv2.FONT_HERSHEY_SIMPLEX, 0.65, [0, 255, 255], 2)
                        cv2.putText(frame, f"tag_family: {tag_family}", (10, frame.shape[0] - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.65, [0, 255, 255], 2)

                        tag_info_list.append(f"ID: {tag_id}, Family: {tag_family}")

                    apriltag_info_msg.data = "; ".join(tag_info_list)
                else:
                    cv2.putText(frame, "tag_id: None", (10, frame.shape[0] - 30), cv2.FONT_HERSHEY_SIMPLEX, 0.65, [0, 255, 255], 2)
                    cv2.putText(frame, "tag_family: None", (10, frame.shape[0] - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.65, [0, 255, 255], 2)
                    apriltag_info_msg.data = "No AprilTag detected"

                # Publish AprilTag recognition information
                self.apriltag_info_pub.publish(apriltag_info_msg)

                # Convert OpenCV image to ROS image message
                ros_image = self.bridge.cv2_to_imgmsg(frame, "bgr8")
                # Publish image message
                self.publisher_.publish(ros_image)
                if self.is_first_publish:
                    self.get_logger().info('Detecting AprilTag information')
                    self.is_first_publish = False
            except Exception as e:
                self.get_logger().error(f"Error converting image: {str(e)}")
        else:
            self.get_logger().error("Cannot read image frame")

    def __del__(self):
        # Release camera resources
        if self.cap.isOpened():
            self.cap.release()


def main(args=None):
    rclpy.init(args=args)
    image_publisher_node = AprilTagDetectNode()
    rclpy.spin(image_publisher_node)
    image_publisher_node.destroy_node()
    rclpy.shutdown()


if __name__ == '__main__':
    main()

Footer Image