upload the pose test function

Code/Control/Laptop_Code/ESP32_AT/imageTread_AT.py

@@ -84,6 +84,77 @@ 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
+
+def get_AT_6DOF_info_posR(url):
+    tid,tx,ty,tz= 0,100000,100000,0
+    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)
+    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)
+    # set camera parameters
+    fx = 800
+    fy = 600
+    cx = 400
+    cy = 300
+    #AT_size = 0.16
+    AT_size = 0.74
+    results = detector.detect(gray_image, estimate_tag_pose=True, camera_params=[fx, fy, cx, cy], tag_size=AT_size)
+    debug_print = 0
+    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 # 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
+        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
+
+
+        tagFamily = r.tag_family.decode("utf-8")
+
+
+        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)
+
+        if debug_print == 1:
+            print("tx,ty,tz:{},{},{}".format(tx, ty, tz))
+            print("cX,cY:{},{}".format(cX, cY))
+            print("[INFO] tag id: {}".format(tid))
+
+    # show the output image after AprilTag detection
+    cv2.imshow("Image", frame)
+
     return tid,tx,ty,tz,rx,ry,rz,pose_r