-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy patharuco_lib.py
150 lines (137 loc) · 6.02 KB
/
aruco_lib.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
# -*- coding: utf-8 -*-
"""
**************************************************************************
* E-Yantra Robotics Competition
* ================================
* This software is intended to check version compatiability of open source software
* Theme: ANT BOT
* MODULE: Task1A
* Filename: Task1A.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)
*
**************************************************************************
"""
import numpy as np
import cv2
import cv2.aruco as aruco
import sys
import math
'''
functions in this file:
* angle_calculate(pt1,pt2, trigger = 0) - function to return angle between two points
* detect_Aruco(img) - returns the detected aruco list dictionary with id: corners
* mark_Aruco(img, aruco_list) - function to mark the centre and display the id
* calculate_Robot_State(img,aruco_list) - gives the state of the bot (centre(x), centre(y), angle)
'''
def angle_calculate(pt1,pt2, trigger = 0): # function which returns angle between two points in the range of 0-359
angle_list_1 = list(range(359,0,-1))
#angle_list_1 = angle_list_1[90:] + angle_list_1[:90]
angle_list_2 = list(range(359,0,-1))
angle_list_2 = angle_list_2[-90:] + angle_list_2[:-90]
x=pt2[0]-pt1[0] # unpacking tuple
y=pt2[1]-pt1[1]
angle=int(math.degrees(math.atan2(y,x))) #takes 2 points nad give angle with respect to horizontal axis in range(-180,180)
if trigger == 0:
angle = angle_list_2[angle]
else:
angle = angle_list_1[angle]
return int(angle)
def detect_Aruco(img , dictionary): #returns the detected aruco list dictionary with id: corners
global aruco_list
aruco_list = {}
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.getPredefinedDictionary(dictionary)# detecting the aruco for specified dictionary as mentioned in the main code
#print(aruco_dict)
parameters = aruco.DetectorParameters_create() #refer opencv page for clarification
#lists of ids and the corners beloning to each id
#print(parameters)
corners, ids, _ = aruco.detectMarkers(gray, aruco_dict, parameters = parameters)
#corners is the list of corners(numpy array) of the detected markers. For each marker, its four corners are returned in their original order (which is clockwise starting with top left). So, the first corner is the top left corner, followed by the top right, bottom right and bottom left.
# print len(corners), corners, ids
#print(corners)
#print(len(corners))
gray = aruco.drawDetectedMarkers(gray, corners,ids)
# cv2.imshow('frame',gray)
#print (type(corners[0]))
if len(corners): #returns no of arucos
#print (len(corners))
#print (len(ids))
for k in range(len(corners)):
temp_1 = corners[k]
temp_1 = temp_1[0]
temp_2 = ids[k]
temp_2 = temp_2[0]
aruco_list[temp_2] = temp_1
return aruco_list
def mark_Aruco(img, aruco_list): #function to mark the centre and display the id
key_list = aruco_list.keys()
font = cv2.FONT_HERSHEY_SIMPLEX
for key in key_list:
dict_entry = aruco_list[key] #dict_entry is a numpy array with shape (4,2)
centre = dict_entry[0] + dict_entry[1] + dict_entry[2] + dict_entry[3]#so being numpy array, addition is not list addition
centre[:] = [int(x / 4) for x in centre] #finding the centre
#print centre
orient_centre = centre + [0.0,5.0]
#print orient_centre
centre = tuple(centre)
orient_centre = tuple((dict_entry[0]+dict_entry[1])/2)
#print centre
#print orient_centre
cv2.circle(img,centre,1,(0,0,255),8)
cv2.circle(img,tuple(dict_entry[0]),1,(0,0,255),8)
cv2.circle(img,tuple(dict_entry[1]),1,(0,255,0),8)
cv2.circle(img,tuple(dict_entry[2]),1,(255,0,0),8)
cv2.circle(img,orient_centre,1,(0,0,255),8)
cv2.line(img,centre,orient_centre,(255,0,0),4) #marking the centre of aruco
cv2.putText(img, str(key), (int(centre[0] + 20), int(centre[1])), font, 1, (0,0,255), 2, cv2.LINE_AA) # displaying the idno
return str(key)
def calculate_Robot_State(img,aruco_list): #gives the state of the bot (centre(x), centre(y), angle)
robot_state = {}
key_list = aruco_list.keys()
font = cv2.FONT_HERSHEY_SIMPLEX
for key in key_list:
dict_entry = aruco_list[key]
pt1 , pt2 = tuple(dict_entry[0]) , tuple(dict_entry[1])
centre = dict_entry[0] + dict_entry[1] + dict_entry[2] + dict_entry[3]
centre[:] = [int(x / 4) for x in centre]
centre = tuple(centre)
#print centre
angle = angle_calculate(pt1, pt2)
cv2.putText(img, str(angle), (int(centre[0] - 80), int(centre[1])), font, 1, (0,0,255), 2, cv2.LINE_AA)
robot_state[key] = (int(centre[0]), int(centre[1]), angle)#HOWEVER IF YOU ARE SCALING IMAGE AND ALL...THEN BETTER INVERT X AND Y...COZ THEN ONLY THE RATIO BECOMES SAME
#print (robot_state)
return robot_state
'''
det_aruco_list = {}
while(True):
# Capture frame-by-frame
ret, frame = cap.read()
det_aruco_list=detect_Aruco(frame)
img = mark_Aruco(frame,det_aruco_list)
robot_state=calculate_Robot_State(img,det_aruco_list)
print robot_state
cv2.imshow('image',img)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# When everything done, release the capture
cap.release()
cv2.destroyAllWindows()
'''