Generate Apriltags Boards

generate_apriltags_boards.py

import cv2
import numpy as np
from PIL import Image

import file_methods as fm


def draw_apriltag(canvas, tag_id, pos, size):
    tag = cv2.resize(apriltags[tag_id], (size, size), interpolation=cv2.INTER_NEAREST)
    canvas[pos[1] : pos[1] + size, pos[0] : pos[0] + size, :] = tag


def draw_tag_row_full(
    canvas, start_tag, start_pos, marker_size, padding, margin, num_columns
):
    d = {}
    for idx, tag_id in enumerate(range(start_tag, start_tag + num_columns)):
        pos = (
            margin + padding + start_pos[0] + (marker_size + padding) * idx,
            margin + padding + start_pos[1],
        )
        extrinsics = np.zeros(6)
        extrinsics[3:5] = (np.array(pos) - margin + padding) / marker_size
        d[tag_id] = extrinsics.tolist()

        draw_apriltag(canvas, tag_id, pos, marker_size)
    return d


def draw_board(
    start_tag_id, num_rows=6, num_columns=6, marker_size=400, padding=200, margin=5
):
    height = margin * 2 + padding + (marker_size + padding) * num_rows
    width = margin * 2 + padding + (marker_size + padding) * num_columns

    print("height, width =", height, width)

    canvas = np.ones((height, width, 3)) * 255

    plmodel_dict = {}
    for row_id in range(num_rows):
        d = draw_tag_row_full(
            canvas,
            start_tag_id + num_columns * row_id,
            (0, 0 + (marker_size + padding) * row_id),
            marker_size,
            padding,
            margin,
            num_columns,
        )
        plmodel_dict.update(d)
    print(plmodel_dict)

    dict_representation = {
        "version": 1,
        "data": plmodel_dict,
        "recording_uuid": "00000",
    }
    fm.save_object(dict_representation, "/home/ch/recordings/perfect-190513.plmodel")

    canvas_PIL = Image.fromarray(canvas.astype("uint8"), "RGB")
    canvas_PIL.save("190513/apriltag_board-{}.jpg".format(start_tag_id), dpi=(254, 254))


if __name__ == "__main__":
    apriltag_path_template = "/cluster/users/Ching/apriltag/tag36h11/tag36_11_{:05}.png"
    apriltags = []
    for i in range(587):
        path = apriltag_path_template.format(i)
        tag = cv2.imread(path)[1:-1, 1:-1]
        apriltags.append(tag)

    draw_board(0)
    draw_board(100)
    draw_board(200)
    draw_board(300)
    draw_board(400)
    draw_board(500)