-
Notifications
You must be signed in to change notification settings - Fork 0
/
Helena AprilTag Code
107 lines (88 loc) · 3.95 KB
/
Helena AprilTag Code
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
# APRILTAG CODE: gets the video stream, converts to black and white and detects for apriltags.
# Date: 1/30/23
# By: Helena S.
# Importing Libraries
import robotpy_apriltag
import cv2
import numpy as np
import argparse
import time
# live feed from camera
capture = cv2.VideoCapture(0)
# feeds an image to the computer to detect
#img = cv2.imread('/Users/helenasieh/Desktop/Vision/apriltag.png');
font = cv2.FONT_HERSHEY_SIMPLEX
#img = cv2.imread('/Users/helenasieh/Desktop/Vision/apriltagrobots_overlay.png')
# function that converts an image to grayscale
def convert(capture):
gray = cv2.cvtColor(capture, cv2.COLOR_RGB2GRAY)
# show the image in grayscale
return gray
print("* Detecting AprilTags...")
# creates a detector that runs a detector on the image
det = robotpy_apriltag.AprilTagDetector()
# Copying parameters from the calibration script result
fx, fy, cx, cy = (1395.1527042544735, 1391.6280089009383, 639.070620047402, 353.97356161241004)
# creates an ApriltagPoseEstimator to calculate the position, angle, distance, etc of apriltag
config = robotpy_apriltag.AprilTagPoseEstimator.Config(6, 1395.1527042544735, 1391.6280089009383, 639.070620047402, 353.97356161241004)
estimator = robotpy_apriltag.AprilTagPoseEstimator(config)
# adds a family of apriltags
tag = det.addFamily("tag16h5")
#detector = robotpy_apriltag.AprilTagDetection(tag)
# prints number of apriltags detected
#print("{} total AprilTags detected".format(len(detectionList)))
#print("Detection Details:", detectionList)
# returns a prebuilt list of python objects
keyPress = False
while not keyPress:
t_begin = time.time()
err, frame = capture.read()
# converts stream into grayscale
processed = convert(frame)
#cts = contours(processed)
#with_labels = draw(frame, cts)
t_end = time.time()
detectionList = det.detect(processed)
# for loop that detects the corners and position of the apriltags
for i in detectionList:
# decision_margin
# ignore anything lower than 35
print(i)
# returns a prebuilt list of python objects
detectionList = det.detect(processed)
# identifying the corners of the apriltag
corners = i.getCorners([0]*8)
# reshape corners into a 4 by 2 matrix
order = np.array(corners).reshape((4,2))
# prints the corner points
#print(order)
# shows the class type (numpy.ndarray)
print("Type:", type(order))
# shows data type (float64 in this case)
print("dtype: ", order.dtype)
# draw lines from the corner points
cv2.line(processed, order[0,:].astype(int), order[1,:].astype(int), (0,0,255), 3)
cv2.line(processed, order[1,:].astype(int), order[2,:].astype(int), (0,0,255), 3)
cv2.line(processed, order[2,:].astype(int), order[3,:].astype(int), (0,0,255), 3)
cv2.line(processed, order[3,:].astype(int), order[0,:].astype(int), (0,0,255), 3)
# converts float number to int to plot a circle on the corner
cv2.circle(processed, (tuple(order[0,:].astype(int))), 8, (255,100,0), 4) # left bottom
cv2.circle(processed, (tuple(order[1,:].astype(int))), 8, (255,100,0), 4) # right bottom
cv2.circle(processed, (tuple(order[2,:].astype(int))), 8, (255,100,0), 4) # right top
cv2.circle(processed, (tuple(order[3,:].astype(int))), 8, (255,100,0), 4) # left top
#cv2.putText(capture, "ID", tuple(order[1,:].astype(int)), font, 2, (255,200,150), 5, cv2.LINE_AA, False)
# AprilTagPoseEstimator
#pose = estimator.estimate(det)
# Print the estimated pose
print("Pose Estimator: ", estimator.estimate(i))
# shows the image with the circles and lines on
cv2.imshow( "Image:", processed)
'''
print("Frame {}: {:.3f} ms".format(fnum, (t_end - t_begin) * 1000.0))
fnum = fnum + 1
'''
# press any key to exit program
if cv2.pollKey() == 27:
keyPress = True
capture.release()
cv2.destroyAllWindows()