Tyler-J42/FRC-Apriltag-Pose-Detection
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

pose based on one tag Mega tag yet to come.

Browse Source
This commit is contained in:
Tylr-J42 2023-10-04 22:56:26 -04:00
parent 8dd209ff4d
commit 54ba4e65d2
2 changed files with 19 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
FRC_Fiducial_Tracking/April_PNP_Live.py
View File

@ -65,7 +65,7 @@ tag_coords = [0, [1, 610.77, 42.19, 18.22, 180], [2, 610.77, 108.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,rx
# x,y,z,rx,ry,rz
robo_space_pose = [0, 0, 0, 0, 0, 0]
def getTagCoords(tag_id):

27
FRC_Fiducial_Tracking/Pose_Estimation.py
View File

@ -6,20 +6,29 @@ class PoseEstimation:
def __init__(self, tag_coords, robo_space_pose):
self.tag_coords = tag_coords
self.robot_space_pose = robo_space_pose
self.orientation = [0, 0, 0]
self.coordinates = [0, 0, 0]
self.orientation = [0, 0, 0, 0]
self.coordinates = [0, 0, 0, 0]
def update(self, data_array):
self.visible_tags = len(data_array)
def getCoords(self):
coord_array = []
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]
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]
self.coordinates = [self.data_array[i].tag_id, tvecXCoord, tvecYCoord, tvecZCoord]
coord_array.append(self.coordinates)
rvecPitch = -self.data_array[i].rvec_z-self.robot_space_pose[3]
rvecRoll = -self.data_array[i].rvec_x-self.robot_space_pose[4]
rvecYaw = self.tag_coords[self.data_array[i].tag_id][4]-self.data_array[i].rvec_y-self.robot_space_pose[5]
self.orientation = [self.data_array[i].tag_id, rvecPitch, rvecRoll, rvecYaw]
coord_array.append(self.orientation)
self.coordinates = [self.data_array[i].tag_id, tvecXCoord, tvecYCoord, tvecZCoord]
coord_array.append(self.coordinates)
return coord_array