-
Notifications
You must be signed in to change notification settings - Fork 4
/
aruco_thread.py
201 lines (156 loc) · 6.22 KB
/
aruco_thread.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
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
import numpy as np
import cv2
import cv2.aruco as aruco
import glob
import math
import threading
import csv
import pandas as pd
#cap = cv2.VideoCapture(1)
class VideoCaptureAsync:
def __init__(self, src=0, width=640, height=480):
self.src = src
self.cap = cv2.VideoCapture(self.src)
self.cap.set(cv2.CAP_PROP_FRAME_WIDTH, width)
self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, height)
self.grabbed, self.frame = self.cap.read()
self.started = False
self.read_lock = threading.Lock()
def set(self, var1, var2):
self.cap.set(var1, var2)
def start(self):
if self.started:
print('[!] Asynchroneous video capturing has already been started.')
return None
self.started = True
self.thread = threading.Thread(target=self.update, args=())
self.thread.start()
return self
def update(self):
while self.started:
grabbed, frame = self.cap.read()
with self.read_lock:
self.grabbed = grabbed
self.frame = frame
def read(self):
with self.read_lock:
frame = self.frame.copy()
grabbed = self.grabbed
return grabbed, frame
def stop(self):
self.started = False
self.thread.join()
def __exit__(self, exec_type, exc_value, traceback):
self.cap.release()
cap = VideoCaptureAsync(1)
####---------------------- CALIBRATION ---------------------------
# termination criteria for the iterative algorithm
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
# checkerboard of size (7 x 6) is used
objp = np.zeros((6*9,3), np.float32)
objp[:,:2] = np.mgrid[0:9,0:6].T.reshape(-1,2)
objp *= 3.5
# arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
# iterating through all calibration images
# in the folder
images = glob.glob('./calib_images/*.png')
for fname in images:
img = cv2.imread(fname)
gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
# find the chess board (calibration pattern) corners
ret, corners = cv2.findChessboardCorners(gray, (9,6),None)
# if calibration pattern is found, add object points,
# image points (after refining them)
if ret == True:
objpoints.append(objp)
# Refine the corners of the detected corners
corners2 = cv2.cornerSubPix(gray,corners,(5,5),(-1,-1),criteria)
imgpoints.append(corners2)
# Draw and display the corners
img = cv2.drawChessboardCorners(img, (9,6), corners2,ret)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)
print("\nRMS:", ret)
print("camera matrix:\n", mtx)
print("distortion coefficients: ", dist.ravel())
def detect_angle(x1,y1,x2,y2):
try:
val = (y2-y1)/(x2-x1)
val = math.atan(abs(val))
val = ((int)(val* 180/3.1416))%360
if(x2-x1 ==0):
val = 90
val = 360 - val
if(x2<x1 and y2>y1):
val = (180 - val)%360
elif(x2<x1 and y1>y2):
val = (180 + val)%360
elif(x2>x1 and y2>y1):
pass
elif(x2>x1 and y2<y1):
val = (360 - val)%360
return val
except:
pass
###------------------ ARUCO TRACKER ---------------------------
cap.start()
while (True):
ret, frame = cap.read()
# operations on the frame
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# set dictionary size depending on the aruco marker selected
aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_50)
# detector parameters can be set here (List of detection parameters[3])
parameters = aruco.DetectorParameters_create()
parameters.adaptiveThreshConstant = 10
# lists of ids and the corners belonging to each id
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
# frame = cv2.circle(frame,(300,400),10,(0,0,200),-1)
# font for displaying text (below)
font = cv2.FONT_HERSHEY_SIMPLEX
# check if the ids list is not empty
# if no check is added the code will crash
if np.all(ids != None):
# estimate pose of each marker and return the values
# rvet and tvec-different from camera coefficients
rvec, tvec ,_ = aruco.estimatePoseSingleMarkers(corners, 0.05, mtx, dist)
(rvec-tvec).any() # get rid of that nasty numpy value array error
# for i in range(0, ids.size):
# # draw axis for the aruco markers
# aruco.drawAxis(frame, mtx, dist, rvec[i], tvec[i], 0.1)
# draw a square around the markers
aruco.drawDetectedMarkers(frame, corners)
# code to show ids of the marker found
strg = ''
for i in range(0, ids.size):
strg += str(ids[i][0])+', '
x = np.round(((corners[i-1][0][0][0] + corners[i-1][0][1][0] + corners[i-1][0][2][0] + corners[i-1][0][3][0]) / 4),0)
y = np.round(((corners[i-1][0][0][1] + corners[i-1][0][1][1] + corners[i-1][0][2][1] + corners[i-1][0][3][1]) / 4),0)
# x1 = np.round((0.5*abs(corners[i-1][0][2][0]+corners[i-1][0][0][0])),0)
# y1 = np.round((0.5*abs(corners[i-1][0][2][1]+corners[i-1][0][0][1])),0)
x2 = np.round((0.5*abs(corners[i-1][0][1][0]+corners[i-1][0][0][0])),0)
y2 = np.round((0.5*abs(corners[i-1][0][1][1]+corners[i-1][0][0][1])),0)
cv2.putText(frame, "ID:" + str(ids[i-1][0]), (int(x-30),int(y-65)), font, 0.8, (255,0,0),2,cv2.LINE_AA)
cv2.putText(frame, "," + str(y), (int(x),int(y-40)), font, 0.8, (0,0,255),2,cv2.LINE_AA)
cv2.putText(frame, str(x), (int(x-80),int(y-40)), font, 0.8, (0,0,255),2,cv2.LINE_AA)
# rotM = np.zeros(shape=(3,3))
# dst, jacobian = cv2.Rodrigues(rvec[i-1], rotM, jacobian = 0)
# ypr = cv2.RQDecomp3x3(rotM)
# yaw = np.round((ypr[0][0]),0)
# cv2.putText(frame, "deg: " + str(angle), (0,250), font, 1, (0,255,0),2,cv2.LINE_AA)
val = (str(ids[i-1][0]), str(x),str(y), str(x2), str(y2))
df = pd.DataFrame(data = val).T
df.to_csv('bot_' +str(ids[i-1][0])+ '.csv', mode='a', header=False)
else:
# code to show 'No Ids' when no markers are found
cv2.putText(frame, "No Ids", (0,64), font, 1, (0,255,0),2,cv2.LINE_AA)
# display the resulting frame
cv2.imshow('frame',frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# When everything done, release the capture
cap.release()
cap.stop()
cv2.destroyAllWindows()