-
Notifications
You must be signed in to change notification settings - Fork 0
/
detect.py
223 lines (184 loc) · 7.6 KB
/
detect.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
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
import numpy as np
import cv2
import cv2.aruco as aruco
import math
"""
**************************************************************************
* E-Yantra Robotics Competition
* ================================
* This software is intended to check version compatiability of open source software
* Theme: Thirsty Crow
* MODULE: Task1.1
* Filename: detect.py
* Version: 1.0.0
* Date: October 31, 2018
*
* Author: e-Yantra Project, Department of Computer Science
* and Engineering, Indian Institute of Technology Bombay.
*
* Software released under Creative Commons CC BY-NC-SA
*
* For legal information refer to:
* http://creativecommons.org/licenses/by-nc-sa/4.0/legalcode
*
*
* This software is made available on an “AS IS WHERE IS BASIS”.
* Licensee/end user indemnifies and will keep e-Yantra indemnified from
* any and all claim(s) that emanate from the use of the Software or
* breach of the terms of this agreement.
*
* e-Yantra - An MHRD project under National Mission on Education using
* ICT(NMEICT)
*
**************************************************************************
"""
####################### Define Utility Functions Here ##########################
"""
Function Name : getCameraMatrix()
Input: None
Output: camera_matrix, dist_coeff
Purpose: Loads the camera calibration file provided and returns the camera and
distortion matrix saved in the calibration file.
"""
def getCameraMatrix():
with np.load('System.npz') as X:
camera_matrix, dist_coeff, _, _ = [X[i] for i in ('mtx','dist','rvecs','tvecs')]
return camera_matrix, dist_coeff
"""
Function Name : sin()
Input: angle (in degrees)
Output: value of sine of angle specified
Purpose: Returns the sine of angle specified in degrees
"""
def sin(angle):
return math.sin(math.radians(angle))
"""
Function Name : cos()
Input: angle (in degrees)
Output: value of cosine of angle specified
Purpose: Returns the cosine of angle specified in degrees
"""
def cos(angle):
return math.cos(math.radians(angle))
################################################################################
"""
Function Name : detect_markers()
Input: img (numpy array), camera_matrix, dist_coeff
Output: aruco list in the form [(aruco_id_1, centre_1, rvec_1, tvec_1),(aruco_id_2,
centre_2, rvec_2, tvec_2), ()....]
Purpose: This function takes the image in form of a numpy array, camera_matrix and
distortion matrix as input and detects ArUco markers in the image. For each
ArUco marker detected in image, paramters such as ID, centre coord, rvec
and tvec are calculated and stored in a list in a prescribed format. The list
is returned as output for the function
"""
def detect_markers(img, camera_matrix, dist_coeff):
markerLength = 100
aruco_list = []
aruco_centre=[]
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_250)
parameters = aruco.DetectorParameters_create()
#lists of ids and the corners beloning to each id
corners, aruco_id, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
# np.array(aruco_id).tolist()
font = cv2.FONT_HERSHEY_SIMPLEX
if np.all(aruco_id != None):
rvec, tvec,_ = aruco.estimatePoseSingleMarkers(corners,markerLength,camera_matrix, dist_coeff)
aruco.drawDetectedMarkers(img, corners)
for i in range(len(corners)):
x = (corners[i][0][0][0] + corners[i][0][1][0] + corners[i][0][2][0] + corners[i][0][3][0]) / 4
y = (corners[i][0][0][1] + corners[i][0][1][1] + corners[i][0][2][1] + corners[i][0][3][1]) / 4
cv2.putText(img, "Id: " + str(aruco_id[i]), (int(x),int(y)), font, 1, (255,0,0),2,cv2.LINE_AA)
aruco_centre.extend([(int(x),int(y))])
np.array(aruco_id).tolist()
aruco_list=np.array(list(zip(aruco_id.astype(int),aruco_centre,rvec,tvec)))
print (aruco_list)
return aruco_list
######################## INSERT CODE HERE ########################
##################################################################
#return aruco_list
"""
Function Name : drawAxis()
Input: img (numpy array), aruco_list, aruco_id, camera_matrix, dist_coeff
Output: img (numpy array)
Purpose: This function takes the above specified outputs and draws 3 mutually
perpendicular axes on the specified aruco marker in the image and
returns the modified image.
"""
def drawAxis(img, aruco_list, aruco_id, camera_matrix, dist_coeff):
for x in aruco_list:
if aruco_id == x[0]:
rvec, tvec = x[2], x[3]
markerLength = 100
m = markerLength/2
pts = np.float32([[-m,m,0],[m,m,0],[-m,-m,0],[-m,m,m]])
pt_dict = {}
imgpts, _ = cv2.projectPoints(pts, rvec, tvec, camera_matrix, dist_coeff)
for i in range(len(pts)):
pt_dict[tuple(pts[i])] = tuple(imgpts[i].ravel())
src = pt_dict[tuple(pts[0])]; dst1 = pt_dict[tuple(pts[1])];
dst2 = pt_dict[tuple(pts[2])]; dst3 = pt_dict[tuple(pts[3])];
img = cv2.line(img, src, dst1, (0,255,0), 4)
img = cv2.line(img, src, dst2, (255,0,0), 4)
img = cv2.line(img, src, dst3, (0,0,255), 4)
return img
"""
Function Name : drawCube()
Input: img (numpy array), aruco_list, aruco_id, camera_matrix, dist_coeff
Output: img (numpy array)
Purpose: This function takes the above specified outputs and draws a cube
on the specified aruco marker in the image and returns the modified
image.
"""
def drawCube(img, ar_list, ar_id, camera_matrix, dist_coeff):
for x in ar_list:
if ar_id == x[0]:
rvec, tvec = x[2], x[3]
markerLength = 100
m = markerLength/2
pts = np.float32([[-m,m,0],[-m,-m, 0], [m,- m, 0], [m, m, 0],[-m,m,-m],[-m,-m,-m],[m,-m,-m],[m,m,-m]])
imgpts, _ = cv2.projectPoints(pts, rvec, tvec,camera_matrix,dist_coeff)
imgpts=np.int32(imgpts).reshape(-1,2)
img=cv2.drawContours(img,[imgpts[:4]],-1,(0,0,255),3)
for i,j in zip(range(4),range(4,8)):
img=cv2.line(img,tuple(imgpts[i]),tuple(imgpts[j]),(255,0,0),3)
img=cv2.drawContours(img,[imgpts[4:]],-1,(255,0,0),3)
cv2.imshow("img",img)
return img
######################## INSERT CODE HERE ########################
##################################################################
"""
Function Name : drawCylinder()
Input: img (numpy array), aruco_list, aruco_id, camera_matrix, dist_coeff
Output: img (numpy array)
Purpose: This function takes the above specified outputs and draws a cylinder
on the specified aruco marker in the image and returns the modified
image.
"""
def drawCylinder(img, ar_list, ar_id, camera_matrix, dist_coeff):
for x in ar_list:
if ar_id == x[0]:
rvec, tvec = x[2], x[3]
markerLength = 100
radius = markerLength/2; height = markerLength*1.5
######################## INSERT CODE HERE ########################
##################################################################
return img
"""
MAIN CODE
This main code reads images from the test cases folder and converts them into
numpy array format using cv2.imread. Then it draws axis, cubes or cylinders on
the ArUco markers detected in the images.
"""
if __name__=="__main__":
cam, dist = getCameraMatrix()
img = cv2.imread("..\\TestCases\\image_1.jpg")
aruco_list = detect_markers(img, cam, dist)
for i in aruco_list:
# img = drawAxis(img, aruco_list, i[0], cam, dist)
img = drawCube(img, aruco_list, i[0], cam, dist)
## img = drawCylinder(img, aruco_list, i[0], cam, dist)
cv2.imshow("img", img)
cv2.waitKey(0)
cv2.destroyAllWindows()