-
Notifications
You must be signed in to change notification settings - Fork 0
/
aruco error.py
111 lines (104 loc) · 4.66 KB
/
aruco error.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
import cv2
import imutils
import sys
import os
import numpy as np
import argparse
import time
from imutils.video import VideoStream
# vs = VideoStream(src=0).start()
#
# time.sleep(2.0)
cap = cv2.VideoCapture(0)
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
}
# arucoDict = cv2.aruco.Dictionary_get(ARUCO_DICT[args['type']])
# arucoParams = cv2.aruco.DetectorParameters_create()
arucoParams = cv2.aruco.DetectorParameters_create()
arucoDict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_4X4_100)
CACHED_PTS = None
CACHED_IDS = None
Line_Pts = None
measure = None
while True:
Dist = []
_, image = cap.read()
# image = vs.read()
image = imutils.resize(image, width=800)
corners, ids, rejected= cv2.aruco.detectMarkers( image, arucoDict, parameters=arucoParams)
if len(corners) <= 0:
if CACHED_PTS is not None:
corners = CACHED_PTS
if len(corners) > 0:
CACHED_PTS = corners
if ids is not None:
ids = ids.flatten()
CACHED_IDS = ids
else:
if CACHED_IDS is not None:
ids = CACHED_IDS
if len(corners) < 2:
if len(CACHED_PTS) >= 2:
corners = CACHED_PTS
for (markerCorner, markerId) in zip(corners, ids):
#print("[INFO] Marker detected")
# if(markerId==20) or (markerId==34):
corners_abcd = markerCorner.reshape((4, 2))
(topLeft, topRight, bottomRight, bottomLeft) = corners_abcd
topRightPoint = (int(topRight[0]), int(topRight[1]))
topLeftPoint = (int(topLeft[0]), int(topLeft[1]))
bottomRightPoint = (int(bottomRight[0]), int(bottomRight[1]))
bottomLeftPoint = (int(bottomLeft[0]), int(bottomLeft[1]))
cv2.line(image, topLeftPoint, topRightPoint, (0, 255, 0), 2)
cv2.line(image, topRightPoint, bottomRightPoint, (0, 255, 0), 2)
cv2.line(image, bottomRightPoint, bottomLeftPoint, (0, 255, 0), 2)
cv2.line(image, bottomLeftPoint, topLeftPoint, (0, 255, 0), 2)
cX = int((topLeft[0] + bottomRight[0])//2)
cY = int((topLeft[1] + bottomRight[1])//2)
# cv2.putText(image, cX, cY, cv2.FONT_HERSHEY_COMPLEX, 1, (0, 0, 220))
#(cX,cY)=centroid_abcd
#centroid= (int(cX[0]),int(cY[1]))
#cv2.line(image,centroid,(topRightPoint+topLeftPoint)//2,(0,255,5),2)
measure = abs(3.5 / (topLeft[0] - cX))
cv2.circle(image, (cX, cY), 4, (255, 0, 0), -1)
cv2.putText(image, str(int(markerId)), (int(topLeft[0] - 10), int(topLeft[1] - 10)), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 0, 255))
Dist.append((cX, cY))
print("centroid at ", cX , cY )
# print(arucoDict)
if len(Dist) == 0:
if Line_Pts is not None:
Dist = Line_Pts
if len(Dist) == 2:
Line_Pts = Dist
if len(Dist) == 2:
cv2.line(image, Dist[0], Dist[1], (255, 0, 255), 2)
ed = ((Dist[0][0] - Dist[1][0]) ** 2 +
((Dist[0][1] - Dist[1][1]) ** 2)) ** (0.5)
cv2.putText(image, str(int(measure * (ed))) + "inch", (int(300), int(300)), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 0, 255))
cv2.putText(image, str(int(measure * (ed))) + "inch", (int(300), int(300)), cv2.FONT_HERSHEY_COMPLEX, 1, (0, 0, 255))
cv2.imshow("[INFO] marker detected", image)
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
break
cv2.destroyAllWindows()