From aadab84836652ba8282618115e8f1269cfa6b6a7 Mon Sep 17 00:00:00 2001 From: Tylr-J42 Date: Sat, 23 Dec 2023 19:11:10 -0500 Subject: [PATCH] working on pose estimation --- .../Apriltag_Pose_Localization.py | 27 +++++----- FRC_Fiducial_Tracking/PNP_Pose_Estimation.py | 30 +++++++---- FRC_Fiducial_Tracking/WTFTEST.py | 51 ------------------ .../PNP_Pose_Estimation.cpython-39.pyc | Bin 2183 -> 2082 bytes 4 files changed, 36 insertions(+), 72 deletions(-) delete mode 100644 FRC_Fiducial_Tracking/WTFTEST.py diff --git a/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py b/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py index f869146..c936e2b 100644 --- a/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py +++ b/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py @@ -66,6 +66,8 @@ tag_coords = [[0, 0.0, 0.0, 0.0, 0.0], [1, 610.77, 42.19, 18.22, 180], [2, 610.7 [4, 636.96, 265.74, 27.38, 180], [5, 14.25, 265.74, 27.38, 0], [6, 40.45, 174.19, 18.22, 0], [7, 40.45, 108.19, 18.22, 0], [8, 40.45, 42.19, 18.22, 0]] +testing_tags = [[0, 0.0, 0.0, 0.0, 0.0], [1, 12.0, 0.0, 0.0, 0.0], [2, -12.0, 0.0, 0.0, 0.0]] + # x,y,z,rx,ry,rz robo_space_pose = [0, 0, 0, 0, 0, 0] @@ -76,23 +78,24 @@ def tag_corners(tag_coords): x = tag_coords[i][1] y = tag_coords[i][2] z = tag_coords[i][3] - z_rotation = tag_coords[i][4] + y_rotation = tag_coords[i][4] coordinates = [[], [], [] ,[], []] - x_offset = (b/2)*math.cos(math.radians(z_rotation)) - y_offset = (b/2)*math.sin(math.radians(z_rotation)) + x_offset = (b/2)*math.cos(math.radians(y_rotation)) + z_offset = (b/2)*math.sin(math.radians(y_rotation)) coordinates[0] = tag_coords[i][0] - coordinates[1] = [x-x_offset, y+y_offset, z+b/2] - coordinates[2] = [x+x_offset, y+y_offset, z+b/2] - coordinates[3] = [x+x_offset, y+y_offset, z-b/2] - coordinates[4] = [x-x_offset, y+y_offset, z-b/2] + coordinates[1] = [x-x_offset, y+b/2, z+z_offset] + coordinates[2] = [x+x_offset, y+b/2, z+z_offset] + coordinates[3] = [x+x_offset, y-b/2, z+z_offset] + coordinates[4] = [x-x_offset, y-b/2, z+z_offset] corners = corners + [coordinates] return corners -field_tag_coords = tag_corners(tag_coords) +#field_tag_coords = tag_corners(tag_coords) +testing_tag_coords = tag_corners(testing_tags) def getTagCoords(tag_id): return tag_coords[tag_id] @@ -125,11 +128,11 @@ def display_features(image, imgpts): # setting up apriltag detection. Make sure this is OUTSIDE the loop next time options = apriltag.DetectorOptions(families='tag36h11', border=1, nthreads=4, -quad_decimate=2.0, quad_blur=0.0, refine_edges=True, +quad_decimate=1.0, quad_blur=0.0, refine_edges=True, refine_decode=False, refine_pose=False, debug=False, quad_contours=True) detector = apriltag.Detector(options) -pose_estimator = PNPPose(field_tag_coords, robo_space_pose, camera_matrix, dist) +pose_estimator = PNPPose(testing_tag_coords, robo_space_pose, camera_matrix, dist) # main vision processing code time.sleep(0.1) @@ -149,8 +152,8 @@ while True: if det[4]>30: # points of the tag to be tracked image_corners = list(image_corners) - image_corners.append(list(det.corners)) - image_corners = np.array(image_corners[0]) + image_corners = image_corners+(list(det.corners)) + image_corners = np.array(image_corners) tags_detected.append(det.tag_id) # only show display if you use --display for argparse diff --git a/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py b/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py index c1494f4..5ae2562 100644 --- a/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py +++ b/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py @@ -19,27 +19,39 @@ class PNPPose: yaw = 180*atan2(-R[1][0], R[0][0])/pi rot_params= [roll,pitch,yaw] return rot_params - + + def cam_to_field(self, tvecs, euler_angles): + position = [0, 0, 0] + rotation = [0, 0, 0] + + total_dist = math.sqrt(tvecs[0]**2 + tvecs[1]**2) + rotation + + return position, rotation + def calculate_coords(self, image_corners, tags_detected): tag_corners = self.tag_corners - PNP_obj_input = np.array([]) + PNP_obj_input = [] if len(tags_detected)>0: for i in range(len(tags_detected)): - if(PNP_obj_input.size == 0): - PNP_obj_input = np.array([tag_corners[tags_detected[i]][1], tag_corners[tags_detected[i]][2], tag_corners[tags_detected[i]][3], tag_corners[tags_detected[i]][4]]) + if(len(PNP_obj_input) == 0): + PNP_obj_input = [tag_corners[tags_detected[i]][1], tag_corners[tags_detected[i]][2], tag_corners[tags_detected[i]][3], tag_corners[tags_detected[i]][4]] else: - PNP_obj_input = np.stack([PNP_obj_input, np.array([tag_corners[tags_detected[i]][1], tag_corners[tags_detected[i]][2], tag_corners[tags_detected[i]][3], tag_corners[tags_detected[i]][4]])]) + PNP_obj_input = PNP_obj_input + [tag_corners[tags_detected[i]][1], tag_corners[tags_detected[i]][2], tag_corners[tags_detected[i]][3], tag_corners[tags_detected[i]][4]] + PNP_obj_input = np.array(PNP_obj_input) #print("PNP_obj_input: ", PNP_obj_input, ", image_corners: ", image_corners, "tags_detected: ", tags_detected) ret, rvecs, tvecs = cv2.solvePnP(PNP_obj_input, image_corners, self.camera_matrix, self.dist, flags=0) - + for i in range(len(rvecs)): rvecs[i] = math.degrees(rvecs[i]) - #cameraMatx, rotMatx, transVect, S - euler_angles = self.rot_params_rv(rvecs) + total_dist = math.hypot(math.hypot(tvecs[0], tvecs[1]), tvecs[2]) - print("euler ZYX: ", euler_angles, "rvecs: ", rvecs,"tvecs: ", tvecs) + print("euler XYZ: ", euler_angles, "tvecs: ", tvecs, "total_dist: ", total_dist) + + world_angleXYZ = [-euler_angles[0], -euler_angles[1], -euler_angles[2]] + z_line_offset_coord = [] \ No newline at end of file diff --git a/FRC_Fiducial_Tracking/WTFTEST.py b/FRC_Fiducial_Tracking/WTFTEST.py deleted file mode 100644 index 5861637..0000000 --- a/FRC_Fiducial_Tracking/WTFTEST.py +++ /dev/null @@ -1,51 +0,0 @@ -import math -from PNP_Pose_Estimation import PNPPose -import numpy as np - -FOCAL_LEN_PIXELS = 621.5827338 -# camera matrix from Calibrate_Camera.py. -camera_matrix = np.array([[FOCAL_LEN_PIXELS, 0., 308.94165115], - [0., FOCAL_LEN_PIXELS, 221.9470321], - [0., 0.,1.]]) - -# from Camera_Calibration.py -dist = np.array([ 2.32929183e-01, -1.35534844e+00, -1.51912733e-03, -2.17960810e-03, 2.25537289e+00]) - -# 2023 Field Apriltag Coordinates index = tag id -# format = [id, x, y, z, z-rotation] in inches -tag_coords = [[0, 0.0, 0.0, 0.0, 0.0], [1, 610.77, 42.19, 18.22, 180], [2, 610.77, 108.19, 18.22, 180], [3, 610.77, 174.19, 18.22, 180], -[4, 636.96, 265.74, 27.38, 180], [5, 14.25, 265.74, 27.38, 0], [6, 40.45, 174.19, 18.22, 0], [7, 40.45, 108.19, 18.22, 0], -[8, 40.45, 42.19, 18.22, 0]] - -# x,y,z,rx,ry,rz -robo_space_pose = [0, 0, 0, 0, 0, 0] - -b = 6.5 - -def tag_corners(tag_coords): - corners = [] - - for i in range(len(tag_coords)): - x = tag_coords[i][1] - y = tag_coords[i][2] - z = tag_coords[i][3] - z_rotation = tag_coords[i][4] - - coordinates = [[], [], [] ,[], []] - - x_offset = (b/2)*math.cos(math.radians(z_rotation)) - y_offset = (b/2)*math.sin(math.radians(z_rotation)) - coordinates[0] = tag_coords[i][0] - coordinates[1] = [x-x_offset, y+y_offset, z+b/2] - coordinates[2] = [x+x_offset, y+y_offset, z+b/2] - coordinates[3] = [x+x_offset, y+y_offset, z-b/2] - coordinates[4] = [x-x_offset, y+y_offset, z-b/2] - - corners = corners + [coordinates] - return corners - -field_tag_coords = tag_corners(tag_coords) - -pose_estimator = PNPPose(field_tag_coords, robo_space_pose, camera_matrix, dist) - -pose_estimator.calculate_coords(np.array([[341.3, 157.0], [220.99, 152.41], [140.93 , 83.28], [284.02, 81.93]]), np.array([0])) \ No newline at end of file diff --git a/FRC_Fiducial_Tracking/__pycache__/PNP_Pose_Estimation.cpython-39.pyc b/FRC_Fiducial_Tracking/__pycache__/PNP_Pose_Estimation.cpython-39.pyc index 54c282c38e2542cdffdead9aeaf6a9cc34bfa20d..dd6417bf61d2ae28c63d3c50641814d270a3a140 100644 GIT binary patch delta 1078 zcmZWo%}x|S5bmDo`Q06|2#Q1k96+-dW7LBo#zgc?R=p@<#&xGz2Zx!ZrWb|uWDmp` zO*E2y0pRA@lNTRkp1_+=U}Dvb2BMwxSJm~`T|fP9@}w1XgTQCF-YmbB-w%VI{LA65 zr!xX2;1JjZ7Qz<>-q(z8@LNq11q2farPF7;>WKX4o+=EG;14UQde*S_ z;B5X>Q>tB+|8sn~W?PK1FNEUSIN%X~B|dZ5P)2<|;P=_HPlbgiQE3f;@ds67~8?^Mm(}VS{r6z7f#_;k(~oxMZSQMy(3>c+M2}* z6TQuLerR^o&<@7(P}`)fG0H4M#LgTWNKK-JuYzdR(O%@D$kYCTjSce1v_TY&D~iSq z);Eg=CfbY`HqT$H`$jE;*7&_WKYILRePwaqsXB#B^RyEyi|q-Zfi2BMrZ{)lKv^2; zptQOG-94BhottOLuI!|p(}}J}!HHxaq|9NK%&eDtgG`my0OC}&gL;&Mczb-sxiq3n z)&uNgn7!Bv<8&}oC67xJq=NIPgG&T+1Pf$3@j8Pfss{?+zdp4eCUGjmY;!Z0D(qz$ zME}lFtJh6>!=$TZt*+c-!QMvFK39b=XI<~;J#hR+v`b7GExYoWr z_kSMce$e94pQnokFv3Q#5KMsgMDULx-J*B0NKGN+0JKrxyQSR+p(mP+t}lxj;UH~^ zJ-;r+8RvmvT*-2bOTiu7Htr@2#*+(aEP4+f`3@i=L#pU8jakHohGHIFCL=81o24Vb z0$C*BWhlclwn=O%Q|?2A2|i}(V=a?o7F%P^UM5+h3}EbJOpQ`OdLW%(b<8@cBlGzG z!aX;3Jo1nfGn+*wC@eVzu=5kUarphy@b}jb>p;#Ri8B*>COI>}@JS?kPVAFZMN7;9 zFXj)V2a$zH3U!?l2!sjT`BC!$C zA#0#MW&|1@P>*h#uc){d!FUCU1G+(;y@@Slwk()rLSe|@vsubx)b4c{+RB`e$uVuP z@r2Xj%2X5zx$tB>>e+|ZmJ!>r6LTz}oN@jIkUu+|QjWrAp!i-RyT23}=9B~3+-TZ! zl0(V5Sprfst5Vf8* z+I|41p%yfJ(5u<^wI!okeLK}sa~Zbx{Blq}J!vCp9nWtF-tOIhe@?B&sRSV4EE(-W7Gz>t6C(tK?v^x%`$DS