diff --git a/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py b/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py index 80b9d73..b4e5e59 100644 --- a/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py +++ b/FRC_Fiducial_Tracking/Apriltag_Pose_Localization.py @@ -159,7 +159,8 @@ while True: imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, camera_matrix, dist) image = display_features(image, imgpts) - pose_coords = pose_estimator.calculate_coords(image_corners, tags_detected) + if(len(tags_detected) > 0): + pose_coords = pose_estimator.calculate_coords(image_corners, tags_detected) #Showing image. use --display to show image if args.display: diff --git a/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py b/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py index 4c4666e..0433ec6 100644 --- a/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py +++ b/FRC_Fiducial_Tracking/PNP_Pose_Estimation.py @@ -21,7 +21,6 @@ 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]])]) - 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) diff --git a/FRC_Fiducial_Tracking/WTFTEST.py b/FRC_Fiducial_Tracking/WTFTEST.py index 588476c..5861637 100644 --- a/FRC_Fiducial_Tracking/WTFTEST.py +++ b/FRC_Fiducial_Tracking/WTFTEST.py @@ -13,7 +13,7 @@ dist = np.array([ 2.32929183e-01, -1.35534844e+00, -1.51912733e-03, -2.17960810e # 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]] 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 cfe240c..375a506 100644 Binary files a/FRC_Fiducial_Tracking/__pycache__/PNP_Pose_Estimation.cpython-39.pyc and b/FRC_Fiducial_Tracking/__pycache__/PNP_Pose_Estimation.cpython-39.pyc differ