import cv2
import apriltag
import numpy as np

# AprilTag 相关设置
tag_size = 200  # 单个AprilTag的尺寸（像素）
family = 'tag36h11'  # AprilTag家族
grid_size = (5, 5)  # AprilGrid的大小，例如5x5

# 创建AprilTag生成器
tag_generator = apriltag.AprilTag(family)

# 创建一个空白画布，用于绘制AprilGrid
grid_image = np.ones((grid_size[0] * tag_size, grid_size[1] * tag_size), dtype=np.uint8) * 255

# 生成AprilGrid
for i in range(grid_size[0]):
    for j in range(grid_size[1]):
        # 生成单个AprilTag
        tag_id = i * grid_size[1] + j
        tag_image = tag_generator.generate(tag_id).astype(np.uint8)

        # 计算放置位置
        start_x = i * tag_size
        start_y = j * tag_size

        # 放置AprilTag到网格中
        grid_image[start_x:start_x + tag_size, start_y:start_y + tag_size] = tag_image

# 保存生成的AprilGrid图像
cv2.imwrite('aprilgrid.png', grid_image)

# 显示生成的AprilGrid图像
cv2.imshow('AprilGrid', grid_image)
cv2.waitKey(0)
cv2.destroyAllWindows()