'''
Sample Command:-
python detect_aruco_video.py --type DICT_5X5_100 --camera True
python detect_aruco_video.py --type DICT_5X5_100 --camera False --video test_video.mp4 -a 25 -k ./calibration_matrix.npy -d ./distortion_coefficients.npy
'''
from turtle import delay
import numpy as np
from utils import ARUCO_DICT, aruco_display
import argparse
import time
import cv2
import sys
import math
import time
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])
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
ap = argparse.ArgumentParser()
ap.add_argument("-i", "--camera", help="Set to True if using webcam")
ap.add_argument("-v", "--video", help="Path to the video file")
ap.add_argument("-t", "--type", type=str, default="DICT_ARUCO_ORIGINAL", help="Type of ArUCo tag to detect")
ap.add_argument("-k", "--K_Matrix", required=True, help="Path to calibration matrix (numpy file)")
ap.add_argument("-d", "--D_Coeff", required=True, help="Path to distortion coefficients (numpy file)")
ap.add_argument("-a", "--aruco_dim", required=True, help="ArUco tag dimension")
args = vars(ap.parse_args())
if args["camera"].lower() == "true":
video = cv2.VideoCapture(0)
time.sleep(2.0)
else:
if args["video"] is None:
print("[Error] Video file location is not provided")
sys.exit(1)
video = cv2.VideoCapture(args["video"])
if ARUCO_DICT.get(args["type"], None) is None:
print(f"ArUCo tag type '{args['type']}' is not supported")
sys.exit(0)
arucoDict = cv2.aruco.Dictionary_get(ARUCO_DICT[args["type"]])
calibration_matrix_path = args["K_Matrix"]
distortion_coefficients_path = args["D_Coeff"]
k = np.load(calibration_matrix_path)
d = np.load(distortion_coefficients_path)
arucoParams = cv2.aruco.DetectorParameters_create()
while True:
ret, frame = video.read()
time.sleep(0.05)
if ret is False:
break
h, w, _ = frame.shape
width=1000
height = int(width*(h/w))
frame = cv2.resize(frame, (width, height), interpolation=cv2.INTER_CUBIC)
corners, ids, rejected = cv2.aruco.detectMarkers(frame, arucoDict, parameters=arucoParams)
detected_markers = aruco_display(corners, ids, rejected, frame)
if len(corners) > 0:
#print(corners) #posizione degli angoli del marker
#print(len(ids))
if (len(ids) == 2):
for i in range(0, len(ids)):
rvec, tvec, markerPoints = cv2.aruco.estimatePoseSingleMarkers(corners[i], float(args["aruco_dim"]), k,d)
if (i == 0):
print("Vettore 1")
tvec0 = tvec
R_ct = np.matrix(cv2.Rodrigues(rvec)[0])
R_ct = R_ct.T
roll, pitch, yaw = rotationMatrixToEulerAngles(R_flip*R_ct)
A = ([math.cos(roll),math.cos(pitch),math.cos(yaw)])
print(tvec0)
print(A)
if (i==1):
print("Vettore 2")
tvec1 = tvec
#B = ([rvec[0][0][0],rvec[0][0][1],rvec[0][0][2]])
R_ct = np.matrix(cv2.Rodrigues(rvec)[0])
R_ct = R_ct.T
roll, pitch, yaw = rotationMatrixToEulerAngles(R_flip*R_ct)
B = ([math.cos(roll),math.cos(pitch),math.cos(yaw)])
print(tvec1)
print(B)
# primo metodo per il calcolo della distanza
tvec0_x = tvec0[0][0][0]
tvec0_y = tvec0[0][0][1]
tvec0_z = tvec0[0][0][2]
tvec1_x = tvec1[0][0][0]
tvec1_y = tvec1[0][0][1]
tvec1_z = tvec1[0][0][2]
dist1 = math.sqrt(pow((tvec0_x-tvec1_x),2)+pow((tvec0_y-tvec1_y),2)+pow((tvec0_z-tvec1_z),2))
distanza1= "Dist=%4.0f"%(dist1)
#secondo metodo per il calcolo della distanza
#
distanza= "Dist=%4.0f"%(np.linalg.norm(tvec1-tvec0))
cv2.putText(frame, distanza1,(50, 100),cv2.FONT_HERSHEY_SIMPLEX, 1,(0, 0, 255), 2, cv2.LINE_4)
dot_product = np.dot(A,B,out=None)
normA = (np.linalg.norm(A))
normB = (np.linalg.norm(B))
cos_angolo = dot_product/(normA*normB)
angolo_rad = np.arccos(cos_angolo)
angolo_deg = np.rad2deg(angolo_rad)
if (angolo_deg > 90):
angolo_deg = 180 - angolo_deg
ang = "Ang=%4.1f"%(angolo_deg)
cv2.putText(frame,ang ,(50, 150),cv2.FONT_HERSHEY_SIMPLEX, 1,(0, 0, 255), 2, cv2.LINE_4)
cv2.imshow("Image", detected_markers)
key = cv2.waitKey(1) & 0xFF
if key == ord("q"):
break
cv2.destroyAllWindows()
video.release()
