Aruco Tag con Opencv (nuova versione)

Ho riscritto per la nuova versione di OpenCV il programma di estrazione dei dati degli Aruco Tag

Il programma e' completamente parametrizzato (con valori di default) ed ha un output in csv in cui sono inserite le coordinate 2D immagine, coordinate 3D nel sistema di riferimento della camera, angoli di roll,pitch ed yaw

 

from os import listdir
from os.path import isfile, join

import numpy as np
import argparse
import cv2
import sys
import math
import os

ARUCO_DICT = {
    "DICT_4X4_50": cv2.aruco.DICT_4X4_50,
    "DICT_4X4_100": cv2.aruco.DICT_4X4_100,
    "DICT_4X4_250": cv2.aruco.DICT_4X4_250,
    "DICT_4X4_1000": cv2.aruco.DICT_4X4_1000,
    "DICT_5X5_50": cv2.aruco.DICT_5X5_50,
    "DICT_5X5_100": cv2.aruco.DICT_5X5_100,
    "DICT_5X5_250": cv2.aruco.DICT_5X5_250,
    "DICT_5X5_1000": cv2.aruco.DICT_5X5_1000,
    "DICT_6X6_50": cv2.aruco.DICT_6X6_50,
    "DICT_6X6_100": cv2.aruco.DICT_6X6_100,
    "DICT_6X6_250": cv2.aruco.DICT_6X6_250,
    "DICT_6X6_1000": cv2.aruco.DICT_6X6_1000,
    "DICT_7X7_50": cv2.aruco.DICT_7X7_50,
    "DICT_7X7_100": cv2.aruco.DICT_7X7_100,
    "DICT_7X7_250": cv2.aruco.DICT_7X7_250,
    "DICT_7X7_1000": cv2.aruco.DICT_7X7_1000,
    "DICT_ARUCO_ORIGINAL": cv2.aruco.DICT_ARUCO_ORIGINAL,
    "DICT_APRILTAG_16h5": cv2.aruco.DICT_APRILTAG_16h5,
    "DICT_APRILTAG_25h9": cv2.aruco.DICT_APRILTAG_25h9,
    "DICT_APRILTAG_36h10": cv2.aruco.DICT_APRILTAG_36h10,
    "DICT_APRILTAG_36h11": cv2.aruco.DICT_APRILTAG_36h11
}

def isRotationMatrix(R):
    Rt = np.transpose(R)
    shouldBeIdentity = np.dot(Rt, R)
    I = np.identity(3, dtype=R.dtype)
    n = np.linalg.norm(I - shouldBeIdentity)
    return n < 1e-6

def rotationMatrixToEulerAngles(R):
    assert (isRotationMatrix(R))
    sy = math.sqrt(R[0, 0] * R[0, 0] + R[1, 0] * R[1, 0])
    singular = sy < 1e-6
    if not singular:
        x = math.atan2(R[2, 1], R[2, 2])
        y = math.atan2(-R[2, 0], sy)
        z = math.atan2(R[1, 0], R[0, 0])
    else:
        x = math.atan2(-R[1, 2], R[1, 1])
        y = math.atan2(-R[2, 0], sy)
        z = 0
    return np.array([x, y, z])


def estrai_parametri(img):
    # inserire qui il ciclo per le img
    image = cv2.imread(img)
    image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)

    ArucoParams = cv2.aruco.DetectorParameters()
    ArucoParams.cornerRefinementMethod = 0

    R_flip = np.zeros((3, 3), dtype=np.float32)
    R_flip[0, 0] = 1.0
    R_flip[1, 1] = -1.0
    R_flip[2, 2] = -1.0

    Adict = cv2.aruco.getPredefinedDictionary(ARUCO_DICT[args["type"]])
    detector = cv2.aruco.ArucoDetector(Adict, ArucoParams)
    corners, ids, _ = cv2.aruco.detectMarkers(image, Adict, parameters=ArucoParams)

    if len(corners) > 0:
        x_sum = corners[0][0][0][0] + corners[0][0][1][0] + corners[0][0][2][0] + corners[0][0][3][0]
        y_sum = corners[0][0][0][1] + corners[0][0][1][1] + corners[0][0][2][1] + corners[0][0][3][1]
        x_centerPixel = x_sum / 4
        y_centerPixel = y_sum / 4

        # print(corners) #posizione degli angoli del marker
        for i in range(0, len(ids)):
            rvec, tvec, markerPoints = cv2.aruco.estimatePoseSingleMarkers(corners[i], float(args["aruco_dim"]), k, d)
            # distanza dalla camera
            dist = math.sqrt(
                (tvec[0][0][0] * tvec[0][0][0]) + (tvec[0][0][1] * tvec[0][0][1]) + (tvec[0][0][2] * tvec[0][0][2]))
            str_dist = "{:4.2f}".format(dist)

            R_ct = np.matrix(cv2.Rodrigues(rvec)[0])
            R_ct = R_ct.T
            roll_marker, pitch_marker, yaw_marker = rotationMatrixToEulerAngles(R_flip * R_ct)
            str_roll = "%4.2f" % (math.degrees(roll_marker))
            str_pitch = "%4.2f" % (math.degrees(pitch_marker))
            str_yaw = "%4.2f" % (math.degrees(yaw_marker))

            if (int(args["tag"]) > 0):
                if (int(ids[i]) == int(args["tag"])):
                    f.write(str(ids[i]) + ";" + str(x_centerPixel) + ";" + str(y_centerPixel) + ";" + str(
                        tvec[0][0][0]) + ";" + str(tvec[0][0][1]) + ";" + str(tvec[0][0][2]) + ";" + os.path.basename(img) + ";" + str_roll + ";" + str_pitch + ";" + str_yaw+"\n")
            else:
                f.write(str(ids[i]) + ";" + str(x_centerPixel) + ";" + str(y_centerPixel) + ";" + str(
                    tvec[0][0][0]) + ";" + str(tvec[0][0][1]) + ";" + str(tvec[0][0][2]) + ";" + os.path.basename(img) + ";" + str_roll + ";" + str_pitch + ";" + str_yaw+"\n")



ap = argparse.ArgumentParser()
#mettere rem sulla successiva per utilizzare il ciclo sulle img
#ap.add_argument("-i", "--image", required=True, help="path to input image containing ArUCo tag")
ap.add_argument("-t", "--type", type=str, default="DICT_4X4_250", help="type of ArUCo tag to detect")
ap.add_argument("-k", "--K_Matrix", type=str,default='./calibration_matrix.npy',help="Path to calibration matrix (numpy file)")
ap.add_argument("-d", "--D_Coeff",  type=str,default='./distortion_coefficients.npy',help="Path to distortion coefficients (numpy file)")
ap.add_argument("-a", "--aruco_dim", default=25,type=int, help="ArUco tag dimension")
ap.add_argument("-g", "--tag", default=0, type=str, help="Select only one Id")
ap.add_argument("-p", "--path", default="./",  help="Path folder immagini")


#la dimensione del tag e' quella dello spigolo esterno del quadrato nero esterno, non i singoli quadrati interni

args = vars(ap.parse_args())
if ARUCO_DICT.get(args["type"], None) is None:
    print(f"ArUCo tag type '{args['type']}' is not supported")
    sys.exit(0)


calibration_matrix_path = args["K_Matrix"]
distortion_coefficients_path = args["D_Coeff"]
k = np.load(calibration_matrix_path)
d = np.load(distortion_coefficients_path)



immagini = [f for f in listdir(args["path"]) if isfile(join(args["path"], f))]

with open('aruco'+str(args["tag"])+'.csv', 'w') as f:
    f.write("Id;Xpix;Ypix;X;Y;Z;Filename;Roll;Pitch;Roll\n")
    for i in immagini:
        print(args["path"]+i)
        estrai_parametri(args["path"]+i)
f.close()