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

some math for converting camera tvecs to robot relative tvecs

Browse Source
This commit is contained in:
Tylr-J42 2023-10-07 01:22:45 -04:00
parent 54ba4e65d2
commit 68262d5edd
2 changed files with 22 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
.vscode/settings.json vendored Normal file
View File

@ -0,0 +1,5 @@
{
"python.analysis.extraPaths": [
"./FRC_Fiducial_Tracking"
]
}

18
FRC_Fiducial_Tracking/Pose_Estimation.py
View File

@ -13,6 +13,7 @@ class PoseEstimation:
self.visible_tags = len(data_array)
def getCoords(self):
coord_array = []
if(len(self.visible_tags)>1):
for i in self.visible_tags:
@ -29,6 +30,21 @@ 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
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]
def distance(d1x, d1y, d2x, d2y):
return math.sqrt((d2x - d1x)**2 + (d2y - d1y)**2)
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)
return [opposite_out, adjacent_out]