add rotation matrix and finalized the get_AT_6DOF_info

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()