-
Notifications
You must be signed in to change notification settings - Fork 0
/
FindAprilTag.py
187 lines (144 loc) · 6.62 KB
/
FindAprilTag.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
#use findtape to find distance
from FindTape import*
#from apriltag import apriltag
#from pupil_apriltags import Detector
import robotpy_apriltag
import cv2
import numpy as np
from math import degrees
try:
from PrintPublisher import *
except ImportError:
from NetworkTablePublisher import *
# Marker size and object
marker_size = 5 + 15/16.0 # FRC targets might be a different size
# Data about the marker for solvePnP's SOLVEPNP_IPPE_SQUARE stuff
object_points = []
object_points.append( [float(-marker_size / 2),float(marker_size / 2), 0])
object_points.append( [float(marker_size / 2),float(marker_size / 2), 0])
object_points.append( [float(marker_size / 2),float(-marker_size / 2), 0])
object_points.append( [float(-marker_size / 2),float(-marker_size / 2), 0])
object_points = np.array(object_points)
options = apriltag.DetectorOptions(
families="tag16h5",
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.5,
)
detector = apriltag.Detector(options)
#detector = Detector(
# families="tag16h5",
# nthreads=1,
# quad_decimate=1.0,
# quad_sigma=0.0,
# refine_edges=1,
# decode_sharpening=0.5,
#)
#main function
def findAprilTagCorner(image, cameraFOV, CameraTiltAngle,MergeVisionPipeLineTableName):
apriltag_outer_corner(image, cameraFOV, CameraTiltAngle,MergeVisionPipeLineTableName)
#print("length: ",np.size(corners))
return image
def calc_distance(image, cameraFOV, corners, CameraTiltAngle):
size = image.shape
focal_length = size[1]
center = (size[1]/2, size[0]/2)
screenHeight, screenWidth, _ = image.shape
H_FOCAL_LENGTH, V_FOCAL_LENGTH = calculateFocalLengthsFromInput(cameraFOV, screenWidth, screenHeight)
#define camera matrix
camera_matrix = np.array(
[[H_FOCAL_LENGTH, 0, center[0]],
[0, V_FOCAL_LENGTH, center[1]],
[0, 0, 1]], dtype = "double"
)
#define camera distortion
dist_coeffs = np.array([[0.16171335604097975, -0.9962921370737408, -4.145368586842373e-05,
0.0015152030328047668, 1.230483016701437]])
pnpsuccess, rvec, tvec = cv2.solvePnP(object_points, corners, camera_matrix, dist_coeffs, flags = cv2.SOLVEPNP_IPPE_SQUARE)
s, rvec, tvec = findTvecRvec(image, corners, real_world_coordinates, H_FOCAL_LENGTH, V_FOCAL_LENGTH)
distance, angle1, angle2 = compute_output_values(rvec, tvec, CameraTiltAngle)
return distance, angle1
def apriltag_outer_corner(image, cameraFOV, CameraTiltAngle,MergeVisionPipeLineTableName):
#detector = Detector(
# families="tag16h5",
# nthreads=1,
# quad_decimate=1.0,
# quad_sigma=0.0,
# refine_edges=1,
# decode_sharpening=0.5,
#)
options = apriltag.DetectorOptions(
families="tag16h5",
nthreads=1,
quad_decimate=1.0,
quad_sigma=0.0,
refine_edges=1,
decode_sharpening=0.5,
)
detector = apriltag.Detector(options)
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
results = detector.detect(gray)
corners = np.empty
#print("before results")
# min_distance = -1
distance_to_tag_and_yaw = {}
for tag in results:
if (tag.hamming == 0):
# print("-------------------------------------")
# print(r)
# print("-------------------------------------")
(ptA, ptB, ptC, ptD) = tag.corners
# extract the bounding box (x, y)-coordinates for the AprilTag
# and convert each of the (x, y)-coordinate pairs to integers
ptB = (int(ptB[0]), int(ptB[1]))
ptC = (int(ptC[0]), int(ptC[1]))
ptD = (int(ptD[0]), int(ptD[1]))
ptA = (int(ptA[0]), int(ptA[1]))
# draw the bounding box of the AprilTag detection
cv2.line(image, ptA, ptB, (0, 255, 0), 2)
cv2.line(image, ptB, ptC, (0, 255, 0), 2)
cv2.line(image, ptC, ptD, (0, 255, 0), 2)
cv2.line(image, ptD, ptA, (0, 255, 0), 2)
cv2.line(image, ptD, ptA, (0, 255, 0), 2)
# draw the center (x, y)-coordinates of the AprilTag
(cX, cY) = (int(tag.center[0]), int(tag.center[1]))
cv2.circle(image, (cX, cY), 5, (0, 0, 255), -1)
# ERIK: Tag family should contain a string, "36h11" which is the family of the apriltag.
# FIRST says we should only have 36h11 tags so might be a good check to see if the target is good.
tagFamily = tag.tag_family.decode("utf-8")
# Put the text for the id of the tag
cv2.putText(image, f"id: {tag.tag_id}", (ptA[0], ptA[1] - 15),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
corners = np.array(tag.corners)
if np.size(corners) >= 4:
distance, yaw = calc_distance(image, cameraFOV, corners, CameraTiltAngle)
distance_to_tag_and_yaw[distance] = [tag, yaw]
# if min_distance == -1:
# min_distance = distance
# else:
# if distance < min_distance:
# min_distance = distance
cv2.putText(image, f"ft: {round(distance/12, 2)}", (ptA[0], ptA[1] + 15),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
cv2.putText(image, f"yaw: {round(yaw, 2)}", (ptA[0], ptA[1] + 40),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, (255, 255, 255), 2)
#cv2.putText(image, f"d: " {distance}, (ptA[0], ptA[1] + 15),
#cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
# Get the corners of the target in a numpy array for solvePnP
else:
print("hello world")
distance = -1
distance = min(distance_to_tag_and_yaw.keys())
yaw = distance_to_tag_and_yaw[distance][1]
tag_id = distance_to_tag_and_yaw[distance][0].tag_id
# convert distance from inches to feet
distance = round(distance/12, 2)
yaw = round(yaw, 2)
# publish values to network table
"""
publishNumber(MergeVisionPipeLineTableName, "DistanceToAprilTag", distance)
publishNumber(MergeVisionPipeLineTableName, "YawToAprilTag", yaw)
publishNumber(MergeVisionPipeLineTableName, "TagID", tag_id)
"""