'''
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", required=True, 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('udp://127.0.0.1:5000',cv2.CAP_FFMPEG)
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.1)
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
for i in range(0, len(ids)):
rvec, tvec, markerPoints = cv2.aruco.estimatePoseSingleMarkers(corners[i], float(args["aruco_dim"]), k,d)
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]))
distanza = "Distanza=%4.0f"%(dist)
cv2.putText(frame, distanza,(50, 50),cv2.FONT_HERSHEY_SIMPLEX, 1,(0, 0, 255), 2, cv2.LINE_4)
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 = "Roll=%4.0f"%(math.degrees(roll_marker))
str_pitch = "Pitch=%4.0f"%(math.degrees(pitch_marker))
str_yaw= "Yaw=%4.0f"%(math.degrees(yaw_marker))
cv2.putText(frame, str_roll,(50, 100),cv2.FONT_HERSHEY_SIMPLEX, 1,(0, 0, 255), 2, cv2.LINE_4)
cv2.putText(frame, str_pitch,(50, 125),cv2.FONT_HERSHEY_SIMPLEX, 1,(0, 0, 255), 2, cv2.LINE_4)
cv2.putText(frame, str_yaw,(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()
