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()
