diff --git a/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py b/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py
index f17cf50b98ea65da0647eafb4182d8a98671f471..842cabf91aeab45e154f042bcf2c449ec1680afd 100644
--- a/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py
+++ b/Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py
@@ -3,7 +3,9 @@ from urllib.request import urlopen, Request
import numpy as np
import time
import math
+from math import sin,atan2,cos,pi,atan,sqrt
# import apriltag
+import sys
from pupil_apriltags import Detector
def nothing(x):
pass
@@ -18,6 +20,7 @@ def get_AT_6DOF_info(url):
rx = [0.0,0.0,0.0]
ry = [0.0, 0.0, 0.0]
rz = [0.0, 0.0, 0.0]
+ pose_r = np.array([[0,0,0],[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)
@@ -50,6 +53,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
+ pose_r = r.pose_R
tid = r.tag_id
# draw the bounding box of the AprilTag detection
@@ -80,17 +84,90 @@ def get_AT_6DOF_info(url):
# show the output image after AprilTag detection
cv2.imshow("Image", frame)
- return tid,tx,ty,tz,rx,ry,rz
+ return tid,tx,ty,tz,rx,ry,rz,pose_r
+
+
+def rotationM2Euler(rx,ry,rz) :
+ R_00 = rx[0]
+ R_01 = rx[1]
+ R_02 = rx[2]
+ R_10 = ry[0]
+ R_11 = ry[1]
+ R_12 = ry[2]
+ R_20 = rz[0]
+ R_21 = rz[1]
+ R_22 = rz[2]
+
+ sy = math.sqrt(R_00 * R_00 + R_10 * R_10)
+ singular = sy < 1e-6
+ if not singular:
+ x = math.atan2(R_21, R_22)
+ y = math.atan2(-R_20, sy)
+ z = math.atan2(R_10, R_00)
+ else:
+ x = math.atan2(-R_12, R_11)
+ y = math.atan2(-R_20, sy)
+ z = 0
+
+ print("phi =", np.rad2deg(x))
+ print("theta =", np.rad2deg(y))
+ print("psi =", np.rad2deg(z))
+
+ """
+ tol = sys.float_info.epsilon * 10
+
+ if abs(R_00) < tol and abs(R_10) < tol:
+ eul1 = 0
+ eul2 = atan2(-R_20, R_00)
+ eul3 = atan2(-R_12, R_11)
+ else:
+ eul1 = atan2(R_10, R_00)
+ sp = sin(eul1)
+ cp = cos(eul1)
+ eul2 = atan2(-R_20, cp * R_00 + sp * R_10)
+ eul3 = atan2(sp * R_02 - cp * R_12, cp * R_11 - sp * R_01)
+
+ print("phi =", np.rad2deg(eul1))
+ print("theta =", np.rad2deg(eul2))
+ print("psi =", np.rad2deg(eul3))
+ """
+
+
+def rotationMatrixToEulerAngles(R) :
+ assert(isRotationMatrix(R))
+ sy = math.sqrt(R[0,0] * R[0,0] + R[1,0] * R[1,0])
+ singular = sy < 1e-6
+ if not singular :
+ x = math.atan2(R[2,1] , R[2,2])
+ y = math.atan2(-R[2,0], sy)
+ z = math.atan2(R[1,0], R[0,0])
+ else :
+ x = math.atan2(-R[1,2], R[1,1])
+ y = math.atan2(-R[2,0], sy)
+ z = 0
+ print("R_phi =", np.rad2deg(x))
+ print("R_theta =", np.rad2deg(y))
+ print("R_psi =", np.rad2deg(z))
+
+def R2EA(pose_r):
+ r11 = pose_r[0][0]
+ r12 = pose_r[0][1]
+ r13 = pose_r[0][2]
+ r21 = pose_r[1][0]
+ r22 = pose_r[1][1]
+ r23 = pose_r[1][2]
+ r31 = pose_r[2][0]
+ r32 = pose_r[2][1]
+ r33 = pose_r[2][2]
+
+ AprilTagPitch = round(np.degrees(atan(-r31 / sqrt((r32 * r32) + (r33 * r33)))), 3)
+ AprilTagRoll = round(np.degrees(atan(-r32 / r33)), 3)
+ ApriTagYaw = round(np.degrees(atan(r21 / r11)), 3)
+
+ print("pitch =", AprilTagPitch)
+ print("roll =", AprilTagRoll)
+ print("yaw =", ApriTagYaw )
-
-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
@@ -101,8 +178,7 @@ if __name__ == "__main__":
url = 'http://10.0.0.3/cam-hi.jpg' #03
url = 'http://10.0.0.5/cam-hi.jpg' #01
url = 'http://10.0.0.6/cam-hi.jpg' #02
- url = 'http://10.0.0.7/cam-hi.jpg' #06
-
+ url = 'http://10.0.0.9/cam-hi.jpg' # 6
# cv2.namedWindow("live transmission", cv2.WINDOW_AUTOSIZE)
@@ -115,94 +191,30 @@ if __name__ == "__main__":
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)
- imgnp=np.array(bytearray(img_resp.read()),dtype=np.uint8)
- frame=cv2.imdecode(imgnp,-1)
-
- gray_image = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
- #print(len(gray_image.shape))
- h,w,_ = frame.shape
-
- # 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])
- and tag_size (in meters) should be supplied.
- The detect method returns a list of Detection objects each having
- the following attributes
- (note that the ones with an asterisks are computed only if estimate_tag_pose=True):
- """
-
- """
- So fx and fy are the focal lengths expressed in pixels.
- Cx and Cy describe the coordinates of the so called principal
- point that should be in the center of the image.
- It is e.g. not in the center of the image if you cropped the image,
- what you should never do when calibrating.
-
- 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
- cy = 300
- results = detector.detect(gray_image,estimate_tag_pose=True, camera_params=[fx, fy, cx, cy], tag_size=0.16)
-
- # loop over the AprilTag detection results
- for r in results:
- # extract the bounding box (x, y)-coordinates for the AprilTag
- # and convert each of the (x, y)-coordinate pairs to integers
- (ptA, ptB, ptC, ptD) = r.corners
- 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]))
- tx,ty,tz = r.pose_t
- print("tx,ty,tz:{},{},{}".format(tx,ty,tz))
- # draw the bounding box of the AprilTag detection
- cv2.line(frame, ptA, ptB, (0, 255, 0), 5)
- cv2.line(frame, ptB, ptC, (0, 255, 0), 5)
- cv2.line(frame, ptC, ptD, (0, 255, 0), 5)
- cv2.line(frame, ptD, ptA, (0, 255, 0), 5)
- # draw the center (x, y)-coordinates of the AprilTag
- (cX, cY) = (int(r.center[0]), int(r.center[1]))
- cv2.circle(frame, (cX, cY), 5, (0, 0, 255), -1)
- # draw the tag family on the image
- print("cX,cY:{},{}".format(cX,cY))
-
-
- 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), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
- print("[INFO] tag id: {}".format(tid))
-
- # show the output image after AprilTag detection
- cv2.imshow("Image", frame)
- """
- tid,tx,ty,tz,rx,ry,rz = get_AT_6DOF_info(url)
+ tid,tx,ty,tz,rx,ry,rz,R = 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])
+
+ print("rotation matrix:{}".format(R))
+ R2EA(R)
+
+ # print("R00,R10,R20:{},{},{}".format(rx[0], ry[0], rz[0]))
+ # rotationM2Euler(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))
+ """
+ # rotationMatrixToEulerAngles(R)
key=cv2.waitKey(5)
if key==ord('q'):