diff --git a/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py b/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py
index fd39118bde4a5c31f7d1d3f9c4117676820b4c0a..524fd94888b053838840213a986b61316ea54d4c 100644
--- a/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py
+++ b/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py
@@ -2,7 +2,7 @@ import cv2
from urllib.request import urlopen, Request
import numpy as np
import time
-
+import math
# import apriltag
from pupil_apriltags import Detector
def nothing(x):
@@ -11,16 +11,24 @@ def nothing(x):
# detector = apriltag.Detector()
-detector = Detector()
+
def get_AT_6DOF_info(url):
+ tid,tx,ty,tz= 0,0,0,0
+ rx = [0.0,0.0,0.0]
+ ry = [0.0, 0.0, 0.0]
+ rz = [0.0, 0.0, 0.0]
+
header = {
"User-Agent": "Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/92.0.4515.159 Safari/537.36."}
req = Request(url, headers=header)
img_resp = urlopen(req, timeout=60)
imgnp = np.array(bytearray(img_resp.read()), dtype=np.uint8)
frame = cv2.imdecode(imgnp, -1)
+
+ # ret,frame = cap.read()
gray_image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
+
h, w, _ = frame.shape
# put a dot in center of the frame
cv2.circle(frame, (w // 2, h // 2), 7, (255, 0, 0), -1)
@@ -41,6 +49,7 @@ def get_AT_6DOF_info(url):
ptA = (int(ptA[0]), int(ptA[1]))
tx, ty, tz = r.pose_t # in meters
rx,ry,rz = r.pose_R
+ tid = r.tag_id
# draw the bounding box of the AprilTag detection
cv2.line(frame, ptA, ptB, (0, 255, 0), 5)
@@ -55,7 +64,8 @@ def get_AT_6DOF_info(url):
tagFamily = r.tag_family.decode("utf-8")
- tid = r.tag_id
+
+
cv2.putText(frame, tagFamily, (ptA[0], ptA[1] - 15), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
cv2.putText(frame, "tx: {:.2f} ty: {:.2f} tz:{:.2f}".format(tx[0], ty[0], tz[0]), (ptA[0], ptA[1] + 30),
@@ -71,20 +81,35 @@ def get_AT_6DOF_info(url):
return tid,tx,ty,tz,rx,ry,rz
+
+def rotationMatrixToEulerAngles(rx,ry,rz) :
+
+ sy = math.sqrt(rx[0] * rx[0] + ry[0] * ry[0])
+ roll = math.atan2(rz[1] , rz[2])
+ pitch = math.atan2(-rz[0], sy)
+ yaw = math.atan2(ry[0], rx[0])
+
+ return np.array([roll, pitch, yaw])
if __name__ == "__main__":
#change the IP address below according to the
#IP shown in the Serial monitor of Arduino code
# url='http://192.168.4.1/cam-hi.jpg'
# url='http://192.168.1.107/cam-hi.jpg'
url='http://192.168.4.1/cam-hi.jpg'
- # url = 'http://192.168.1.118/cam-hi.jpg'
+ url = 'http://192.168.1.118/cam-hi.jpg'
# cv2.namedWindow("live transmission", cv2.WINDOW_AUTOSIZE)
# cv2.namedWindow("live transmission", cv2.WINDOW_NORMAL)
+ detector = Detector()
+ tid,tx,ty,tz,rx,ry,rz = 0,0,0,0,0,0,0
+ test_webcam = 0
+ if test_webcam == 1:
+ cap = cv2.VideoCapture(0)
while True:
+ """
header = {"User-Agent": "Mozilla/5.0 (Windows NT 10.0; Win64; x64) AppleWebKit/537.36 (KHTML, like Gecko) Chrome/92.0.4515.159 Safari/537.36."}
req = Request(url, headers=header)
img_resp = urlopen(req, timeout=60)
@@ -97,7 +122,7 @@ if __name__ == "__main__":
# put a dot in center of the frame
cv2.circle(frame, (w//2, h//2), 7, (255, 0, 0), -1)
-
+ """
"""
If you also want to extract the tag pose, estimate_tag_pose should be set to True
and camera_params ([fx, fy, cx, cy])
@@ -117,6 +142,7 @@ if __name__ == "__main__":
fx, fy, cx, cy are given in Pixels in Computer Vision ( and openCV)
but e.g. in Photogrammetry you often use mm
"""
+ """
fx = 800
fy = 600
cx = 400
@@ -156,11 +182,24 @@ if __name__ == "__main__":
# show the output image after AprilTag detection
cv2.imshow("Image", frame)
+ """
+ tid,tx,ty,tz,rx,ry,rz = get_AT_6DOF_info(url)
+ print("testing new function")
+ print("-----------------------")
+ print("tid:{}".format(tid))
+ print("tx,ty,tz:{},{},{}".format(tx,ty,tz))
+ print("rx,ry,rz:{},{},{}".format(rx,ry,rz))
+
+ # R = np.array([rx,ry,rz])
+ roll,pitch, yaw = rotationMatrixToEulerAngles(rx,ry,rz)
+ roll = roll * 180 /math.pi
+ pitch = pitch * 180 / math.pi
+ yaw = yaw * 180 / math.pi
+ print("roll,pitch,yaw:{},{},{}".format(roll, pitch, yaw))
key=cv2.waitKey(5)
if key==ord('q'):
break
-
#cv2.destroyAllWindows()