From daf90bcd9471f6b61200dc5444bea5cb47d07918 Mon Sep 17 00:00:00 2001 From: Tylr-J42 Date: Mon, 9 Oct 2023 02:35:12 -0400 Subject: [PATCH] math for solving the triangle in the dual tag trianglulation --- FRC_Fiducial_Tracking/April_PNP_Live.py | 11 ++-- FRC_Fiducial_Tracking/Pose_Estimation.py | 47 ++++++++++++++---- .../__pycache__/PiVid.cpython-39.pyc | Bin 0 -> 1427 bytes .../__pycache__/TagObj.cpython-39.pyc | Bin 0 -> 1572 bytes 4 files changed, 42 insertions(+), 16 deletions(-) create mode 100644 FRC_Fiducial_Tracking/__pycache__/PiVid.cpython-39.pyc create mode 100644 FRC_Fiducial_Tracking/__pycache__/TagObj.cpython-39.pyc diff --git a/FRC_Fiducial_Tracking/April_PNP_Live.py b/FRC_Fiducial_Tracking/April_PNP_Live.py index 4aab4cc..e83a6b2 100644 --- a/FRC_Fiducial_Tracking/April_PNP_Live.py +++ b/FRC_Fiducial_Tracking/April_PNP_Live.py @@ -12,6 +12,7 @@ from networktables import NetworkTables import argparse from TagObj import TagObj from PiVid import PiVid +from Pose_Estimation import PoseEstimation RAD2DEG = 180*pi @@ -19,6 +20,7 @@ RAD2DEG = 180*pi parser = argparse.ArgumentParser(description="Select display") parser.add_argument("--display", action='store_true', help="enable a display of the camera") parser.add_argument("--high_res", action='store_true', help="enable resolution 1088x720 vs 640x480") +parser.add_argument("--pose_estimation", action='store_true', help="estimate pose based on detected tags") parser.add_argument("--ip_add", type=str, required=True) args = parser.parse_args() @@ -28,8 +30,8 @@ args = parser.parse_args() 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.]]) + [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]) @@ -57,10 +59,8 @@ vision_table = NetworkTables.getTable("Fiducial") FPS = 0 -TARGET_ID = 1 - # 2023 Field Apriltag Coordinates index = tag id -# format = [x, y, z, z-rotation] in inches +# format = [id, x, y, z, z-rotation] in inches tag_coords = [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]] @@ -122,7 +122,6 @@ while True: # points of the tag to be tracked tag_points = np.array([[det.center[0], det.center[1]], [det.corners[0][0], det.corners[0][1]], [det.corners[1][0], det.corners[1][1]], [det.corners[2][0], det.corners[2][1]], [det.corners[3][0], det.corners[3][1]]], dtype=np.float32) - ret,rvecs, tvecs = cv2.solvePnP(objp, tag_points, camera_matrix, dist, flags=0) # making translation and rotation vectors into a format good for networktables diff --git a/FRC_Fiducial_Tracking/Pose_Estimation.py b/FRC_Fiducial_Tracking/Pose_Estimation.py index 871a3c0..13d73fe 100644 --- a/FRC_Fiducial_Tracking/Pose_Estimation.py +++ b/FRC_Fiducial_Tracking/Pose_Estimation.py @@ -17,9 +17,11 @@ class PoseEstimation: coord_array = [] if(len(self.visible_tags)>1): for i in self.visible_tags: - tvecXCoord = math.sqrt(self.data_array[i].tvec_z**2 + self.data_array[i].tvec_x**2) * math.cos(self.tag_coords[self.data_array[i].tag_id][4] - self.data_array[i].rvec_y - self.robot_space_pose[5]) - self.robo_space_pose[0] - tvecYCoord = math.sqrt(self.data_array[i].tvec_z**2 + self.data_array[i].tvec_y**2) * math.sin(90-self.data_array[i].rvec_x - self.robot_space_pose[3]) - tvecZCoord = math.sqrt(self.data_array[i].tvec_z**2 + self.data_array[i].tvec_x**2) * math.sin(self.tag_coords[self.data_array[i].tag_id][4] - self.data_array[i].rvec_y - self.robot_space_pose[5]) - self.robo_space_pose[2] + tagTvec = cam_pose_to_robot_tvec(self.data_array[i], self.robo_space_pose) + + tvecXCoord = math.sqrt((tagTvec[2]**2) + (tagTvec[0]**2)) * math.cos(self.tag_coords[self.data_array[i].tag_id][4] - self.data_array[i].rvec_y - self.robot_space_pose[5]) - self.robo_space_pose[0] + tvecYCoord = math.sqrt((tagTvec[2]**2) + (tagTvec[1]**2)) * math.sin(90-self.data_array[i].rvec_x - self.robot_space_pose[3]) + tvecZCoord = math.sqrt((tagTvec[2]**2) + (tagTvec[0]**2)) * math.sin(self.tag_coords[self.data_array[i].tag_id][4] - self.data_array[i].rvec_y - self.robot_space_pose[5]) - self.robo_space_pose[2] self.coordinates = [self.data_array[i].tag_id, tvecXCoord, tvecYCoord, tvecZCoord] coord_array.append(self.coordinates) @@ -31,20 +33,45 @@ class PoseEstimation: self.orientation = [self.data_array[i].tag_id, rvecPitch, rvecRoll, rvecYaw] coord_array.append(self.orientation) elif(len(self.visible_tags) == 2): - tvecXCoord + tag0Tvec = cam_pose_to_robot_tvec(self.data_array[0], self.robo_space_pose) + tag1Tvec = cam_pose_to_robot_tvec(self.data_array[1], self.robo_space_pose) + + tag0Coords = [self.tag_coords[self.data_array[0].tag_id][0], self.tag_coords[self.data_array[0].tag_id][2], self.tag_coords[self.data_array[0].tag_id][3]] + tag1Coords = [self.tag_coords[self.data_array[1].tag_id][0], self.tag_coords[self.data_array[1].tag_id][2], self.tag_coords[self.data_array[1].tag_id][3]] + + tag_line_dist = distance(tag0Coords[0], tag0Coords[1], tag1Coords[0], tag1Coords[1]) + tag0_cam_dist = math.hypot(tag0Tvec[0], tag0Tvec[2]) + tag1_cam_dist = math.hypot(tag1Tvec[0], tag1Tvec[2]) + + cam_angle = math.toDegrees(math.atan(tag0Tvec[0]/tag0Tvec[2])-math.atan(tag1Tvec[0]/tag1Tvec[2])) + tag0_angle = math.acos(((tag0_cam_dist**2) + (tag_line_dist**2) - (tag1_cam_dist**2)) / (2 * tag0_cam_dist * tag_line_dist)) + tag1_angle = 180-(cam_angle+tag0_angle) + + tvecZCoord = math.hypot(tag0Tvec[0], tag0Tvec[1]) return coord_array - def cam_pose_to_robot(self, tag, robot_space_pose): - robot_tvec_x = cam_rotation_comp(tag.tvec_x, tag.tvec_z, robot_space_pose[4])[0] - robot_tvec_y = cam_rotation_comp(tag.tvec_y, tag.tvec_z, robot_space_pose[3])[0] - robot_tvec_z = cam_rotation_comp(tag.tvec_y, tag.tvec_z, robot_space_pose[3])[1] +# convert from camera detection translation vectors to robot-relative translation vectors +def cam_pose_to_robot_tvec(tag, robot_space_pose): + robot_tvec_x = cam_rotation_comp(tag.tvec_x, tag.tvec_z, robot_space_pose[4])[0] + robot_tvec_y = cam_rotation_comp(tag.tvec_y, tag.tvec_z, robot_space_pose[3])[0] + robot_tvec_z = cam_rotation_comp(tag.tvec_y, tag.tvec_z, robot_space_pose[3])[1] + + return [robot_tvec_x, robot_tvec_y, robot_tvec_z] def distance(d1x, d1y, d2x, d2y): return math.sqrt((d2x - d1x)**2 + (d2y - d1y)**2) +def slope_angle(x1, x2, y1, y2): + + if(x2-x1 != 0): + return math.toDegrees(math.atan2((y2-y1) / (x2-x1))) + else: + return 90.0 + +# math for converting one individual translation vector into robot-relative translation vector def cam_rotation_comp(opposite, adjacent, tilt_theta): - opposite_out = math.sin(tilt_theta - math.atan(opposite/adjacent)) * math.hypot(opposite, adjacent) - adjacent_out = math.cos(tilt_theta - math.atan(opposite/adjacent)) * math.hypot(opposite, adjacent) + opposite_out = math.toDegrees(math.sin(tilt_theta - math.atan(opposite/adjacent)) * math.hypot(opposite, adjacent)) + adjacent_out = math.toDegrees(math.cos(tilt_theta - math.atan(opposite/adjacent)) * math.hypot(opposite, adjacent)) return [opposite_out, adjacent_out] diff --git a/FRC_Fiducial_Tracking/__pycache__/PiVid.cpython-39.pyc b/FRC_Fiducial_Tracking/__pycache__/PiVid.cpython-39.pyc new file mode 100644 index 0000000000000000000000000000000000000000..87202cc88d12e96e2691195846cdcee9d07b989d GIT binary patch literal 1427 zcmZux&1w`u5bmD8o!x9=jG%&g6-G$*B7z{IiA3+rd`3{d)bSQrR(9 z@m9H!O$wDkj&=`Jk;$A6UL775FEI6EkbEwFWJJq0F!dP3uv2!z1V7;??2rqXaNIWm@e>$4_pl$*>M#Ng z$+!&9eQ0}&(U*)O9$!Ags|YO6<*Z2k_xox$@ocnLt4U_krB)Z>V=0STG_^9D z$v_d%Hp(3!cAyGfSFI`QY1-dYPH757ZeuU|w2?KYRYkgzcY?_4sVS#z-D(?Y11Kjp zboXpeo4RQV3G0C_sy#dCYzUb6#?(zb`Qz#6pq>BLub+eU38GgH`5W7mA+>ISs# zMn_g?c?X0{Xu87q3ZE@>p1Dic+#;C&xJA_gr`Rj@Zv8VV0oA|+YT?ko2qgc4PM~T~ zR1@qX>RiU&TtcFtK1ik`?s@vxP-_VXN8*ilVS|EH|FuD-`v|-L*dX;d ziK+IcXbmCkK!`~#+lu6&5JzoRbvx=7Jx;k-gpayUlXzn40{Iy?8MW z+n#2{SyZb+5Hh$K*{G1Td1pE*Q8wS4s D?u#k! literal 0 HcmV?d00001 diff --git a/FRC_Fiducial_Tracking/__pycache__/TagObj.cpython-39.pyc b/FRC_Fiducial_Tracking/__pycache__/TagObj.cpython-39.pyc new file mode 100644 index 0000000000000000000000000000000000000000..b844b2b46b11c4f59aedd03edfc21eadb16cbf71 GIT binary patch literal 1572 zcmb7^OK;Oa5XX1@isLwK+LT9mNJu^9P;y5IRccj)go0FZpuVh(#|x_M;ljVVH>Q* z2K*Z5SIuLfy=5Ie!1cd|hG$1OlRlb|c^sIh0SE1xc{=D|pc-I;g=#_#Y*Y*C;Gou^ z0WPWyP1r)M!#1=~9oT_g)CRPXxtCdefAHr0Wndzr9OM)G7-#Yt&79AfU~^>eGxRum zn({RCbmi%Dy@(p<N`B0&8d2I5PhSB1_p zOBU-&ab4*wtEc1Ck58f`?U&1WX%NivfzOQefS3np7Azn*nJjE7-&5_SEF zjAG;-PR2=icp-!C>DjS3jbIu?ek}UZ4?aetK{qeZ)5%q43lWW?R0snzC6tbFi#tol zY`eI+yfUDtEC=~CRb4G~=IjzHvf#ZASNmRY*~DeF)7^wr50x(U^b@tIp- zRN#LYvW@p4g{X3m3~hgjVPmR3R_h%ROHBDRWvmLRwuy93+gL+7uR^jnk-U5BNnRCF z{a>Urs@{XuBo(VdqF>3{_e9ywAFn4>s$OFg$$PS%RH=H@hAW@RrSGe3H$feO6{ip*S2Omw!VC*I5jR^w<