如何绘制一条指示Apriltag方向的线？

Question

我正在使用python和学生apriltag库来检测图片和视频中的apriltag。最终，我需要找到并能够与apriltag的方向工作。对于在图像或视频流中检测到的每个apriltag，提供apriltag的旋转矩阵和平移矩阵，并且我相信它们表示apriltag的方向。但是，为了证明这一点，我尝试从apriltag的中心到顶部(在其本机方向上)绘制一条线，但我在这样做时遇到了问题。我已经根据我在网上找到的一个函数提取了Euler角，并尝试从apriltag的中心开始绘制一条线，并从那里延伸到每个Euler角，但没有得到好的结果。我也尝试过计算一个距apriltag中心50像素(在x轴上)的“端点”，然后使用与apriltag相同的旋转矩阵绕原点旋转该点，但也没有得到好的结果。我可能做错了什么。我已经在下面包含了我的代码。请帮帮忙。

检测到的apriltag的图像，并尝试绘制一条方向线：https://i.stack.imgur.com/c7plp.png

import numpy
import numpy as np
import cv2
from pupil_apriltags import Detector
import mathutils
from google.colab.patches import cv2_imshow

numpy.set_printoptions(linewidth=500)

camera_matrix = numpy.array([[645.00185337, 0, 304.75823489], [0, 646.18621837, 253.75317465], [0, 0, 1]])
fx = camera_matrix[0][0]
fy = camera_matrix[1][1]
cx = camera_matrix[0][2]
cy = camera_matrix[1][2]
camera_intrinsics_vector = [fx, fy, cx, cy]

def rot_matrix_to_euler(R):
    y_rot = numpy.arcsin(R[2][0])
    x_rot = numpy.arccos(R[2][2]/numpy.cos(y_rot))
    z_rot = numpy.arccos(R[0][0]/numpy.cos(y_rot))
    y_rot_angle = y_rot *(180/numpy.pi)
    x_rot_angle = x_rot *(180/numpy.pi)
    z_rot_angle = z_rot *(180/numpy.pi)
    return (x_rot_angle,y_rot_angle,z_rot_angle)

image = cv2.imread("apriltag.png")

bw_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)

detector = Detector()
detections = detector.detect(bw_image, estimate_tag_pose=True, camera_params=camera_intrinsics_vector, tag_size=0.076)

for detection in detections:

    center = detection.center.astype(numpy.int32)
    cv2.circle(image, (center[0], center[1]), 3, (0, 255, 0), -1)

    corners = detection.corners.astype(numpy.int32)
    cv2.polylines(image, [corners], True, (0, 255, 0), thickness = 2)

    orientation = numpy.array([50, 0, 0])
    orientation = numpy.transpose(orientation)
    # endpoint = numpy.matmul(detection.pose_R, orientation).astype(numpy.int32) * 20
    orientation_line = numpy.matmul(detection.pose_R, orientation)

    euler_angles = rot_matrix_to_euler(detection.pose_R)

    # endpoint = numpy.array( [center[0] + int(orientation_line[0]), center[1] - int(orientation_line[1]) ] )
    endpoint = numpy.array( [center[0] + int(numpy.cos(euler_angles[0]) * 100), center[1] - int(numpy.sin(euler_angles[0]) * 100) ] )

    cv2.line(image, (center[0], center[1]), (endpoint[0], endpoint[1]), (0, 0, 255), 2)
    # cv2.line(image, (center[0], center[1]), (center[0] + endpoint[0], center[1] + endpoint[1]), (0, 0, 255), 2)

    # print(detection.pose_R.shape)
    # print(detection.pose_t)
    # print(orientation.shape)
    # print( numpy.matmul(detection.pose_R, orientation)  ) 

    # print(rot_matrix_to_euler(detection.pose_R))

cv2_imshow( image )```

Answer 1

我不知道我是否准确地理解了您的问题，但您可以使用以下代码开始。下面的函数在标签上绘制一个姿势和立方体，你可以进一步使用它来做你需要做的事情。

这些函数已经在Apriltag库中定义。可以编辑脚点数组以更改直线的尺寸。

希望它能有所帮助:D

def draw_pose(self,overlay, camera_params, tag_size, pose, z_sign=1):
    opoints = np.array([
        -2, -2, 0,
        2, -2, 0,
        2, 2, 0,
        2, -2, -4 * z_sign,
    ]).reshape(-1, 1, 3) * 0.5 * tag_size

    fx, fy, cx, cy = camera_params

    K = np.array([fx, 0, cx, 0, fy, cy, 0, 0, 1]).reshape(3, 3)

    rvec, _ = cv2.Rodrigues(pose[:3, :3])
    tvec = pose[:3, 3]

    dcoeffs = np.zeros(5)

    ipoints, _ = cv2.projectPoints(opoints, rvec, tvec, K, dcoeffs)

    ipoints = np.round(ipoints).astype(int)

    ipoints = [tuple(pt) for pt in ipoints.reshape(-1, 2)]

    cv2.line(overlay, ipoints[0], ipoints[1], (0,0,255), 2)
    cv2.line(overlay, ipoints[1], ipoints[2], (0,255,0), 2)
    cv2.line(overlay, ipoints[1], ipoints[3], (255,0,0), 2)
    font = cv2.FONT_HERSHEY_SIMPLEX
    cv2.putText(overlay, 'X', ipoints[0], font, 0.5, (0,0,255), 2, cv2.LINE_AA)
    cv2.putText(overlay, 'Y', ipoints[2], font, 0.5, (0,255,0), 2, cv2.LINE_AA)
    cv2.putText(overlay, 'Z', ipoints[3], font, 0.5, (255,0,0), 2, cv2.LINE_AA)

def _draw_cube(self,overlay, camera_params, tag_size, pose,centroid, z_sign=1):

    opoints = np.array([
        -10, -8, 0,
        10, -8, 0,
        10, 8, 0,
        -10, 8, 0,
        -10, -8, 2 * z_sign,
        10, -8, 2 * z_sign,
        10, 8, 2 * z_sign,
        -10, 8, 2 * z_sign,
    ]).reshape(-1, 1, 3) * 0.5 * tag_size

    edges = np.array([
        0, 1,
        1, 2,
        2, 3,
        3, 0,
        0, 4,
        1, 5,
        2, 6,
        3, 7,
        4, 5,
        5, 6,
        6, 7,
        7, 4
    ]).reshape(-1, 2)

    fx, fy, cx, cy = camera_params

    K = np.array([fx, 0, cx, 0, fy, cy, 0, 0, 1]).reshape(3, 3)

    rvec, _ = cv2.Rodrigues(pose[:3, :3])
    tvec = pose[:3, 3]

    dcoeffs = np.zeros(5)

    ipoints, _ = cv2.projectPoints(opoints, rvec, tvec, K, dcoeffs)

    ipoints = np.round(ipoints).astype(int)

    ipoints = [tuple(pt) for pt in ipoints.reshape(-1, 2)]

    for i, j in edges:
        cv2.line(overlay, ipoints[i], ipoints[j], (0, 255, 0), 1, 16)