From 0d58e2f55ea7800703902fa71838040a6839d73b Mon Sep 17 00:00:00 2001 From: Tylr-J42 Date: Wed, 22 Nov 2023 17:22:05 -0500 Subject: [PATCH] attempting to get euler angles --- .../Apriltag_Pose_Localization.py | 3 +- FRC_Fiducial_Tracking/PNP_Pose_Estimation.py | 35 +++++++++++++----- FRC_Fiducial_Tracking/PiVid.py | 1 + .../PNP_Pose_Estimation.cpython-39.pyc | Bin 1257 -> 1710 bytes .../__pycache__/PiVid.cpython-39.pyc | Bin 1427 -> 1452 bytes 5 files changed, 29 insertions(+), 10 deletions(-) diff --git a/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py b/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py index b4e5e59..f869146 100644 --- a/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py +++ b/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py @@ -83,6 +83,7 @@ def tag_corners(tag_coords): 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] @@ -140,7 +141,7 @@ while True: #detecting april tags tagFrame = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) - + output = detector.detect(tagFrame) for det in output: diff --git a/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py b/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py index 0433ec6..e5ca434 100644 --- a/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py +++ b/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py @@ -10,18 +10,35 @@ class PNPPose: self.camera_matrix = camera_matrix self.orientation = [0, 0, 0] self.robo_space_pose = robo_space_pose - + + def rot_params_rv(self, rvecs): + from math import pi,atan2,asin + R = cv2.Rodrigues(rvecs)[0] + roll = 180*atan2(-R[2][1], R[2][2])/pi + pitch = 180*asin(R[2][0])/pi + yaw = 180*atan2(-R[1][0], R[0][0])/pi + rot_params= [roll,pitch,yaw] + return rot_params + + def calculate_coords(self, image_corners, tags_detected): tag_corners = self.tag_corners PNP_obj_input = np.array([]) - 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]]) - 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]])]) + 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]]) + 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]])]) - 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) - print(tvecs,"poop") + #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]) + + euler_angles = self.rot_params_rv(rvecs) + + print("euler ZYX: ", euler_angles, "rvecs: ", rvecs) diff --git a/FRC_Fiducial_Tracking/PiVid.py b/FRC_Fiducial_Tracking/PiVid.py index 15f0a16..cc2d85f 100644 --- a/FRC_Fiducial_Tracking/PiVid.py +++ b/FRC_Fiducial_Tracking/PiVid.py @@ -12,6 +12,7 @@ class PiVid: self.camera = PiCamera() self.camera.resolution = camera_res self.camera.framerate = 60 + self.camera.rotation = 180 self.rawCapture = PiRGBArray(self.camera, size=camera_res) self.stream = self.camera.capture_continuous(self.rawCapture, format="bgr", use_video_port=True) 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 375a50631e22a6f29c0d1bd2ce7828e4a5c3fcc8..b7e5810e09b1406beba380bb488283d358ec90cb 100644 GIT binary patch delta 934 zcmZ8fOK;Oa5Zd9>85OIVbz^ofUVx^hgo%v?=+x>QCf2mZxO2r{K-oJSj{4_4AA>Fum zd%Nr#??_Af=2PNYUq3#eW2Qm#p$jD1BeeJ&olkuhk{(T2kEOiFQ$a~0rc%*M>eD(& zrOy>t(ie5&%Q`8M`5N~VRwIhtBB_BGF^^--gwErpt{r0n3w5)i7-zlyTWl#qnab+$ z)K)f436IFguSpt6uX6sb6AWC!fYxRIDg7Ro`fosH(8 z#Ks5tf~Q7mrdH9$oY11p%~U3(X&K)i6}qH)zE4hmDKf1n6Din9)i;&xTgsf^$M6a2 zV_)e5i%1<7(9QIvW&)i{b2&Z;y-qwT1*c&EtF7nHwpT}H@$%cN?gD6R&Y}Sd3*epQ zJPaaK#)~>ZF5>Pe$Yq>(?PIvEfmYlPhe0!Hg8sCi&wy(>VfsM_f*^)vy^{mzM#+`V z*=K&Q4z(hjXu(_Mg33M~5eC^0zP`4E$ygT>YEXw&*&<4|!*=-{T-BJvvQ4oc0VDozwaN delta 513 zcmXX?&5G1O5bjF4^S7JXpUZj}b?xff*C` zFlP@-?y|z31s}nKFVSx>ui(jQv7o-HuCDs}tNsyh!$B6u5kaFj7veX)9+WV={(P6d zCW-g+_Bp(C?d5+9G47(Dp{qX#Et!$d)o|k!A{{T6tDqaF|)%)*wofaDSS4hJ57W$)b1>-;XzejD&; z;{f3VMed9xppfl9aBOA;Zz;lhFCC|o`u%J&t(t{C9jy-@kEZji5R<&Vl9iAOYkN8? z)PxHyaxM7jC;`JhlvsO9o4Rh2U12{Uw_ U>5uXV4~A8Xd9#xH{mOgx7w$5J9RL6T diff --git a/FRC_Fiducial_Tracking/__pycache__/PiVid.cpython-39.pyc b/FRC_Fiducial_Tracking/__pycache__/PiVid.cpython-39.pyc index 87202cc88d12e96e2691195846cdcee9d07b989d..b9fe6c3a0cd5cd7ad05e413ec9cbc8e6ae55f8ca 100644 GIT binary patch delta 472 zcmZusyGjE=6rGuUWZcb0i9!rM(g+fae-PG6uu&8rg(Az6*}{-y2c{FWf`t;pg@m(I8 z2fh_tO2~DpZ}Ne8<7d2TggBH-G?62@ITu8|h*q5_sQ$q+RjUX!^CccnBCIA}3XtAG zg`_eOQp0G_Np%K`R`2xkVFvWwrT`mgy)&sb)Yr|YbKJ0M9ow0QCoW{E+BSRceuI)> zu4kz8+a2@l#__@J_a1^$