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

PNP for Pose almost works - errors

Browse Source
This commit is contained in:
Tylr-J42 2023-11-20 18:39:08 -05:00
parent 3c651154a7
commit ebcc528530
4 changed files with 5 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py
View File

@ -62,7 +62,7 @@ FPS = 0
# 2023 Field Apriltag Coordinates index = tag id
# format = [id, x, y, z, z-rotation] in inches
tag_coords = [[0, 69.0, 420.0, 69.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],
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]]

4
FRC_Fiducial_Tracking/PNP_Pose_Estimation.py
View File

@ -21,6 +21,8 @@ class PNPPose:
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)
image_corners = image_corners.astype("float")
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")

9
FRC_Fiducial_Tracking/WTFTEST.py
View File

@ -45,14 +45,7 @@ def tag_corners(tag_coords):
return corners
field_tag_coords = tag_corners(tag_coords)
robo_space_pose = [0, 0, 0, 0, 0, 0]
pose_estimator = PNPPose(field_tag_coords, robo_space_pose, camera_matrix, dist)
pose_estimator.calculate_coords( np.array([[ 65.75, 420., 72.25],
[ 72.25, 420., 72.25],
[ 72.25, 420., 65.75],
[ 65.75, 420., 65.75]]), np.array([[398.38015747, 154.20915222],
[260.04067993, 150.67947388],
[212.12528992 , 77.55628204],
[387.06124878, 77.81692505]]))
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]))

BIN
FRC_Fiducial_Tracking/__pycache__/PNP_Pose_Estimation.cpython-39.pyc
View File

Binary file not shown.