Apriltag码坐标系方向可视化

通过下面的脚本可以可视化tag_id为11的Apriltag码的坐标系个坐标轴的朝向：

import numpy as np
import apriltag
import cv2

img = cv2.imread('color_1.png')
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
at_detector = apriltag.Detector(apriltag.DetectorOptions(families='tag16h5'))
tags = at_detector.detect(gray)
print("tags: {}".format(tags))

fx, fy, cx, cy = 593.0, 588.0, 311.0, 243.0 #相机内参
cam_params = [fx, fy, cx, cy]
K = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
for tag in tags:
    if tag.tag_id==11 :
        for i in range(4):
            cv2.circle(img, tuple(tag.corners[i].astype(int)), 4, (2, 180, 200), 2) #标记apriltag码角点
        cv2.circle(img, tuple(tag.center.astype(int)), 4, (2, 180, 200), 4) #标记apriltag码中心点
        M, e1, e2 = at_detector.detection_pose(tag, cam_params)
        P = M[:3,:4] #相机投影矩阵
        P = np.matmul(K,P)
        x = np.matmul(P,np.array([[1],[0],[0],[1]]))
        x = x / x[2]
        y = np.matmul(P,np.array([[0],[1],[0],[1]]))
        y = y / y[2]
        z = np.matmul(P,np.array([[0],[0],[1],[1]]))
        z = z / z[2]
        cv2.line(img, tuple(tag.center.astype(int)), tuple(x[:2].reshape(-1).astype(int)), (0,0,255), 2) #x轴红色
        cv2.line(img, tuple(tag.center.astype(int)), tuple(y[:2].reshape(-1).astype(int)), (0,255,0), 2) #y轴绿色
        cv2.line(img, tuple(tag.center.astype(int)), tuple(z[:2].reshape(-1).astype(int)), (255,0,0), 2) #z轴蓝色
        cv2.imwrite("mark.png",img)