Revision history [back]

Let me answer my question. The tag 3D position w.r.t. camera is available in /tag_detections topic. Then I use image_geometry.PinholeCameraModel to project 3D point into 2D pixel location as shown here. My python piece of code for the mentioned repository is shown below.

from apriltag_ros.msg import AprilTagDetectionArray
from image_geometry import PinholeCameraModel
from sensor_msgs.msg import CameraInfo


class TagPixel:
    def __init__(self):
        rospy.Subscriber('tag_detections', AprilTagDetectionArray, self.TagPoseCallback)
        rospy.Subscriber('cgo3_camera/camera_info', CameraInfo, self.TagePixelCallback)
        self.pos_x = 0.0
        self.pos_y = 0.0
        self.pos_z = 0.0

    def TagPoseCallback(self, msg):
        valid = False
        if len(msg.detections) > 0:
            valid = True
        if valid:
            self.pos_x = -msg.detections[0].pose.pose.pose.position.y
            self.pos_y = -msg.detections[0].pose.pose.pose.position.z
            self.pos_z = msg.detections[0].pose.pose.pose.position.x

    def TagePixelCallback(self, msg):
        self.pcm = PinholeCameraModel()
        self.pcm.fromCameraInfo(msg)
        self.p_image = self.pcm.project3dToPixel((self.pos_x, self.pos_y, self.pos_z))
        rospy.loginfo("Pixel position is u = %s, v = %s", self.p_image[0], self.p_image[1])