Skip to content

Commit 733def2

Browse files
author
c-pg-vega
committed
.gitignore added
1 parent e9330a7 commit 733def2

File tree

3 files changed

+325
-0
lines changed

3 files changed

+325
-0
lines changed

‎.gitignore

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
AprilTag/*
2+
AprilTag

‎support_files/receive_stream.py

Lines changed: 293 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,293 @@
1+
from flask import Flask, Response, render_template, request, jsonify
2+
import requests
3+
#from apriltag import Detector
4+
import apriltag
5+
import cv2
6+
import numpy as np
7+
from threading import Lock
8+
import socket
9+
import math
10+
11+
# Create the Flask app
12+
app2 = Flask(__name__)
13+
14+
# Create an AprilTag detector
15+
options = apriltag.DetectorOptions(families="tag36h11")
16+
detector = apriltag.Detector(options, searchpath=apriltag._get_dll_path())
17+
18+
# Lock for thread-safe access to shared variables
19+
tags_lock = Lock()
20+
21+
# Detected tags and tracking tag
22+
detected_tags = []
23+
tracking_tag = None
24+
25+
# Camera parameters
26+
focal_length_x = 547.7837
27+
focal_length_y = 547.8070
28+
principal_point_x = 303.9048
29+
principal_point_y = 243.7748
30+
31+
tag_height_meters = 0.065 # TODO: make sure to change for specific april tag
32+
@app2.route('/april_tag_click', methods=['POST'])
33+
def april_tag_click():
34+
global tracking_tag
35+
with tags_lock:
36+
data = request.get_json()
37+
if not data:
38+
return jsonify({'error': 'Invalid data'}), 400
39+
40+
x, y = data.get('x'), data.get('y')
41+
42+
for tag in detected_tags:
43+
if x >= tag['xMin'] and x <= tag['xMax'] and y >= tag['yMin'] and y <= tag['yMax']:
44+
tracking_tag = tag
45+
#
46+
return jsonify({'message': 'Switched to tag centering mode.'})
47+
48+
return jsonify({'message': 'Clicked outside of all tags'})
49+
50+
@app2.route('/')
51+
def index():
52+
return render_template('index.html')
53+
54+
@app2.route('/video')
55+
def video():
56+
return Response(get_frames(), mimetype='multipart/x-mixed-replace;boundary=frame')
57+
58+
def get_frames():
59+
url = 'http://192.168.1.3:5000/video_feed'
60+
response = requests.get(url, stream=True)
61+
62+
frame_buffer = b''
63+
64+
for chunk in response.iter_content(chunk_size=1024):
65+
frame_buffer += chunk
66+
frame_end = frame_buffer.find(b'\xff\xd9')
67+
while frame_end != -1:
68+
frame_start = frame_buffer.find(b'\xff\xd8')
69+
if frame_start != -1:
70+
frame = frame_buffer[frame_start:frame_end + 2]
71+
72+
processed_frame = process_frame(frame)
73+
74+
yield (b'--frame\r\n'
75+
b'Content-Type: image/jpeg\r\n\r\n' + processed_frame + b'\r\n')
76+
77+
frame_buffer = frame_buffer[frame_end + 2:]
78+
79+
frame_end = frame_buffer.find(b'\xff\xd9')
80+
def process_frame(frame):
81+
global detected_tags
82+
global tracking_tag
83+
84+
nparr = np.frombuffer(frame, np.uint8)
85+
image = cv2.imdecode(nparr, cv2.IMREAD_COLOR)
86+
87+
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
88+
# results = detector.detect(gray)
89+
90+
results, overlay = apriltag.detect_tags(gray,
91+
detector,
92+
camera_params=(focal_length_x, focal_length_y, principal_point_x, principal_point_y),
93+
tag_size=tag_height_meters,
94+
vizualization=3,
95+
verbose=3,
96+
annotation=True
97+
)
98+
99+
with tags_lock:
100+
if tracking_tag is None:
101+
detected_tags.clear()
102+
# for r in results:
103+
# # print("curr: ", r.tag_id)
104+
# corners = np.array(r.corners, dtype=np.int32)
105+
# cv2.polylines(image, [corners], True, (0, 255, 0), 2)
106+
# tag = {
107+
# 'xMin': int(corners[:, 0].min()),
108+
# 'yMin': int(corners[:, 1].min()),
109+
# 'xMax': int(corners[:, 0].max()),
110+
# 'yMax': int(corners[:, 1].max()),
111+
# 'id' : r.tag_id,
112+
113+
# }
114+
# detected_tags.append(tag)
115+
i = 0
116+
while (i < len(results)):
117+
(ptA, ptB, ptC, ptD) = results[i].corners
118+
corners = results[i].corners
119+
corners = np.array(results[i].corners, dtype=np.int32)
120+
cv2.polylines(image, [corners], True, (0, 255, 0), 2)
121+
ptB = (int(ptB[0]), int(ptB[1]))
122+
ptC = (int(ptC[0]), int(ptC[1]))
123+
ptD = (int(ptD[0]), int(ptD[1]))
124+
ptA = (int(ptA[0]), int(ptA[1]))
125+
126+
cX = int(results[i].center[0])
127+
cY = int(results[i].center[1])
128+
distance, angle = get_distance_and_angle(results[i + 1], results[i].center)
129+
130+
tag = {
131+
'xMin': int(corners[:, 0].min()),
132+
'yMin': int(corners[:, 1].min()),
133+
'xMax': int(corners[:, 0].max()),
134+
'yMax': int(corners[:, 1].max()),
135+
'id' : results[i].tag_id,
136+
'pose': results[i + 1],
137+
'center': results[i].center
138+
139+
}
140+
detected_tags.append(tag)
141+
i+=4
142+
else:
143+
# Handle tracking mode
144+
i = 0
145+
while (i < len(results)):
146+
corners = np.array(results[i].corners, dtype=np.int32)
147+
if tracking_tag['id'] == results[i].tag_id: # Assuming each tag has a unique 'id'
148+
cv2.polylines(image, [corners], True, (0, 0, 255), 2)
149+
# distance = calculate_distance(corners)
150+
distance, angle = get_distance_and_angle(results[i + 1], results[i].center)
151+
tracking_tag['corners'] = corners
152+
send_tag_data_to_remote_two(angle, distance, results[i + 1])
153+
if distance < 0.1:
154+
# tag_meet_distance = True
155+
send_message('DISTANCE MET')
156+
# Send head-on data
157+
if is_tag_head_on(angle):
158+
# tag_is_head_on = True
159+
send_message('HEAD ON MET')
160+
if distance < 0.1 and is_tag_head_on(angle):
161+
tracking_tag = None # Stop tracking
162+
print("DONE TRACKING TAG")
163+
send_tag_done_to_remote()
164+
165+
else:
166+
send_tag_data_to_remote(tracking_tag, distance, angle)
167+
break
168+
169+
ret, buffer = cv2.imencode('.jpg', image)
170+
return buffer.tobytes()
171+
172+
def calculate_distance(corners):
173+
y_min, y_max = corners[:, 1].min(), corners[:, 1].max()
174+
distance = -1 * (tag_height_meters * focal_length_y) / (y_min - y_max)
175+
return distance
176+
177+
def send_tag_data_to_remote_two(angle, distance, pose):
178+
print("sending info...")
179+
head_on = False
180+
if (is_tag_head_on(angle)):
181+
head_on = True
182+
data = {
183+
'angle': angle,
184+
'distance': distance,
185+
'headon': head_on,
186+
}
187+
remote_url = 'http://192.168.1.3:5003'
188+
try:
189+
# Send the data to the remote server
190+
requests.post(remote_url, json=data)
191+
192+
except requests.RequestException as e:
193+
print(f"Error sending tag data to remote: {e}")
194+
195+
def yaw_angle(R, center, distance):
196+
# Convert the rotation matrix to euler angles
197+
# beta = -np.arcsin(R[2,0]) # equivalent to y axis
198+
# gamma = np.arctan2(R[1,0]/np.cos(beta), R[0,0]/np.cos(beta)) # equivalent to z axis
199+
200+
#yaw = np.degrees(math.atan2(R[2, 1], R[1, 1]))
201+
# yaw = np.degrees(math.atan2(R[0, 2], R[2, 2]))
202+
# diff = center[0] - principal_point_x
203+
204+
# return (np.degrees(np.arctan2(diff, distance)))
205+
206+
# # scale = diff / distance
207+
# return np.degrees(np.arctan2(diff, distance))
208+
return center[0]
209+
210+
def get_distance_and_angle(pose_matrix, center):
211+
# # Extract translation vector (first 3 elements of the last column)
212+
# translation_vector = pose_matrix[:3, 3]
213+
# distance = np.linalg.norm(translation_vector)
214+
# rotation_matrix = pose_matrix[:3, :3]
215+
216+
# euler_angles_rad = np.arccos((np.trace(rotation_matrix) - 1) / 2)
217+
# angle_degrees = np.degrees(euler_angles_rad)
218+
219+
# return distance, angle_degrees
220+
221+
# Calculate the Yaw angle
222+
translation_vector = np.linalg.norm(pose_matrix[0:3, 3])
223+
distance = np.linalg.norm(translation_vector)
224+
angle = yaw_angle(pose_matrix, center, distance)
225+
226+
227+
# Calculate the distance
228+
229+
230+
231+
return distance, angle
232+
233+
234+
def is_tag_head_on(angle):
235+
236+
# Compute each corner's angle and check if they are within the tolerance
237+
tolerance = 10 # degrees
238+
expected_angle = 0 # degrees
239+
240+
if not (expected_angle - tolerance <= angle <= expected_angle + tolerance):
241+
return False
242+
243+
# If we reach here, all angles are within tolerance. The tag is considered head-on.
244+
return True
245+
246+
247+
def send_message(message):
248+
print("sending info...")
249+
data = {
250+
'status': message
251+
}
252+
remote_url = 'http://192.168.1.3:5003'
253+
try:
254+
requests.post(remote_url, json=data)
255+
except requests.RequestException as e:
256+
print(f"Error sending done message to remote: {e}")
257+
258+
259+
def send_tag_data_to_remote(tag, distance, angle):
260+
# You were passing corners and distance which would not be available here.
261+
# We need to fetch or calculate them within this function now.
262+
print("sending info...")
263+
# corners = tag['corners'] # Assuming the 'corners' are stored in the tag dictionary
264+
head_on = False
265+
if (is_tag_head_on(angle)):
266+
head_on = True
267+
268+
data = {
269+
'angle': angle,
270+
'distance': distance,
271+
'headon': head_on,
272+
}
273+
remote_url = 'http://192.168.1.3:5003'
274+
try:
275+
requests.post(remote_url, json=data)
276+
except requests.RequestException as e:
277+
print(f"Error sending tag data to remote: {e}")
278+
279+
def send_tag_done_to_remote():
280+
print("sending info...")
281+
data = {
282+
'status': 'DONE'
283+
}
284+
remote_url = 'http://192.168.1.3:5003'
285+
try:
286+
requests.post(remote_url, json=data)
287+
except requests.RequestException as e:
288+
print(f"Error sending done message to remote: {e}")
289+
290+
if __name__ == '__main__':
291+
hostname = socket.gethostname()
292+
IP = socket.gethostbyname(hostname)
293+
app2.run(host=IP, port=5001, threaded=True)

‎support_files/templates/index.html

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
<!DOCTYPE html>
2+
<html lang="en">
3+
<head>
4+
<meta charset="UTF-8">
5+
<meta name="viewport" content="width=device-width, initial-scale=1.0">
6+
<title>AprilTag Detection</title>
7+
</head>
8+
<body>
9+
<img src="{{ url_for('video') }}" id="video_feed" style="width: 640px; height: 480px;">
10+
<script type="text/javascript">
11+
document.addEventListener("DOMContentLoaded", function() {
12+
document.getElementById('video_feed').addEventListener('click', function(event) {
13+
var x = event.offsetX;
14+
var y = event.offsetY;
15+
var xhr = new XMLHttpRequest();
16+
xhr.open("POST", "/april_tag_click", true);
17+
xhr.setRequestHeader("Content-Type", "application/json;charset=UTF-8");
18+
19+
xhr.onreadystatechange = function() {
20+
if (xhr.readyState == XMLHttpRequest.DONE) {
21+
var response = JSON.parse(xhr.responseText);
22+
alert(response.message);
23+
}
24+
};
25+
xhr.send(JSON.stringify({ x: x, y: y }));
26+
});
27+
});
28+
</script>
29+
</body>
30+
</html>

0 commit comments

Comments
 (0)