diff --git a/FRC_Fiducial_Tracking/April_PNP_Live.py b/FRC_Fiducial_Tracking/April_PNP_Live.py index 4aab4cc..e83a6b2 100644 --- a/FRC_Fiducial_Tracking/April_PNP_Live.py +++ b/FRC_Fiducial_Tracking/April_PNP_Live.py @@ -12,6 +12,7 @@ from networktables import NetworkTables import argparse from TagObj import TagObj from PiVid import PiVid +from Pose_Estimation import PoseEstimation RAD2DEG = 180*pi @@ -19,6 +20,7 @@ RAD2DEG = 180*pi parser = argparse.ArgumentParser(description="Select display") parser.add_argument("--display", action='store_true', help="enable a display of the camera") parser.add_argument("--high_res", action='store_true', help="enable resolution 1088x720 vs 640x480") +parser.add_argument("--pose_estimation", action='store_true', help="estimate pose based on detected tags") parser.add_argument("--ip_add", type=str, required=True) args = parser.parse_args() @@ -28,8 +30,8 @@ args = parser.parse_args() FOCAL_LEN_PIXELS = 621.5827338 # camera matrix from Calibrate_Camera.py. camera_matrix = np.array([[FOCAL_LEN_PIXELS, 0., 308.94165115], - [0., FOCAL_LEN_PIXELS, 221.9470321], - [0., 0.,1.]]) + [0., FOCAL_LEN_PIXELS, 221.9470321], + [0., 0.,1.]]) # from Camera_Calibration.py dist = np.array([ 2.32929183e-01, -1.35534844e+00, -1.51912733e-03, -2.17960810e-03, 2.25537289e+00]) @@ -57,10 +59,8 @@ vision_table = NetworkTables.getTable("Fiducial") FPS = 0 -TARGET_ID = 1 - # 2023 Field Apriltag Coordinates index = tag id -# format = [x, y, z, z-rotation] in inches +# format = [id, x, y, z, z-rotation] in inches tag_coords = [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]] @@ -122,7 +122,6 @@ while True: # points of the tag to be tracked tag_points = np.array([[det.center[0], det.center[1]], [det.corners[0][0], det.corners[0][1]], [det.corners[1][0], det.corners[1][1]], [det.corners[2][0], det.corners[2][1]], [det.corners[3][0], det.corners[3][1]]], dtype=np.float32) - ret,rvecs, tvecs = cv2.solvePnP(objp, tag_points, camera_matrix, dist, flags=0) # making translation and rotation vectors into a format good for networktables diff --git a/FRC_Fiducial_Tracking/Pose_Estimation.py b/FRC_Fiducial_Tracking/Pose_Estimation.py index 871a3c0..13d73fe 100644 --- a/FRC_Fiducial_Tracking/Pose_Estimation.py +++ b/FRC_Fiducial_Tracking/Pose_Estimation.py @@ -17,9 +17,11 @@ class PoseEstimation: coord_array = [] 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] + tagTvec = cam_pose_to_robot_tvec(self.data_array[i], self.robo_space_pose) + + tvecXCoord = math.sqrt((tagTvec[2]**2) + (tagTvec[0]**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((tagTvec[2]**2) + (tagTvec[1]**2)) * math.sin(90-self.data_array[i].rvec_x - self.robot_space_pose[3]) + tvecZCoord = math.sqrt((tagTvec[2]**2) + (tagTvec[0]**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) @@ -31,20 +33,45 @@ 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 + tag0Tvec = cam_pose_to_robot_tvec(self.data_array[0], self.robo_space_pose) + tag1Tvec = cam_pose_to_robot_tvec(self.data_array[1], self.robo_space_pose) + + tag0Coords = [self.tag_coords[self.data_array[0].tag_id][0], self.tag_coords[self.data_array[0].tag_id][2], self.tag_coords[self.data_array[0].tag_id][3]] + tag1Coords = [self.tag_coords[self.data_array[1].tag_id][0], self.tag_coords[self.data_array[1].tag_id][2], self.tag_coords[self.data_array[1].tag_id][3]] + + tag_line_dist = distance(tag0Coords[0], tag0Coords[1], tag1Coords[0], tag1Coords[1]) + tag0_cam_dist = math.hypot(tag0Tvec[0], tag0Tvec[2]) + tag1_cam_dist = math.hypot(tag1Tvec[0], tag1Tvec[2]) + + cam_angle = math.toDegrees(math.atan(tag0Tvec[0]/tag0Tvec[2])-math.atan(tag1Tvec[0]/tag1Tvec[2])) + tag0_angle = math.acos(((tag0_cam_dist**2) + (tag_line_dist**2) - (tag1_cam_dist**2)) / (2 * tag0_cam_dist * tag_line_dist)) + tag1_angle = 180-(cam_angle+tag0_angle) + + tvecZCoord = math.hypot(tag0Tvec[0], tag0Tvec[1]) 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] +# convert from camera detection translation vectors to robot-relative translation vectors +def cam_pose_to_robot_tvec(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] + + return [robot_tvec_x, robot_tvec_y, robot_tvec_z] def distance(d1x, d1y, d2x, d2y): return math.sqrt((d2x - d1x)**2 + (d2y - d1y)**2) +def slope_angle(x1, x2, y1, y2): + + if(x2-x1 != 0): + return math.toDegrees(math.atan2((y2-y1) / (x2-x1))) + else: + return 90.0 + +# math for converting one individual translation vector into robot-relative translation vector 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) + opposite_out = math.toDegrees(math.sin(tilt_theta - math.atan(opposite/adjacent)) * math.hypot(opposite, adjacent)) + adjacent_out = math.toDegrees(math.cos(tilt_theta - math.atan(opposite/adjacent)) * math.hypot(opposite, adjacent)) return [opposite_out, adjacent_out] diff --git a/FRC_Fiducial_Tracking/__pycache__/PiVid.cpython-39.pyc b/FRC_Fiducial_Tracking/__pycache__/PiVid.cpython-39.pyc new file mode 100644 index 0000000..87202cc Binary files /dev/null and b/FRC_Fiducial_Tracking/__pycache__/PiVid.cpython-39.pyc differ diff --git a/FRC_Fiducial_Tracking/__pycache__/TagObj.cpython-39.pyc b/FRC_Fiducial_Tracking/__pycache__/TagObj.cpython-39.pyc new file mode 100644 index 0000000..b844b2b Binary files /dev/null and b/FRC_Fiducial_Tracking/__pycache__/TagObj.cpython-39.pyc differ