diff --git a/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java b/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java index 728668c94..4a83cb6d7 100644 --- a/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java +++ b/photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java @@ -73,14 +73,11 @@ public class RobotPoseEstimator { /** * Create a new RobotPoseEstimator. * - *

Example: {@code

Map map = new HashMap<>();

map.put(1, new - * Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is at (1.0,2.0,3.0) } - * - * @param aprilTags A AprilTagFieldLayout linking AprilTag IDs to Pose3ds with respect to the - * FIRST field. + * @param aprilTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs to Pose3ds with + * respect to the FIRST field. * @param strategy The strategy it should use to determine the best pose. * @param cameras An ArrayList of Pairs of PhotonCameras and their respective Transform3ds from - * the center of the robot to the cameras. + * the center of the robot to the camera mount positions (ie, robot -> camera). */ public RobotPoseEstimator( AprilTagFieldLayout aprilTags, @@ -96,7 +93,7 @@ public class RobotPoseEstimator { /** * Update the estimated pose using the selected strategy. * - * @return The updated estimated pose and the latency in milliseconds Estimated pose may be null + * @return The updated estimated pose and the latency in milliseconds. Estimated pose may be null * if no targets were seen */ public Optional> update() { diff --git a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java index e73d08430..e52c58f7f 100644 --- a/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java +++ b/photon-lib/src/main/java/org/photonvision/SimVisionSystem.java @@ -24,6 +24,8 @@ package org.photonvision; +import edu.wpi.first.apriltag.AprilTag; +import edu.wpi.first.apriltag.AprilTagFieldLayout; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation2d; @@ -49,7 +51,7 @@ public class SimVisionSystem { int cameraResWidth; int cameraResHeight; double minTargetArea; - Transform3d cameraToRobot; + Transform3d robotToCamera; Field2d dbgField; FieldObject2d dbgRobot; @@ -67,7 +69,8 @@ public class SimVisionSystem { * @param camDiagFOVDegrees Diagonal Field of View of the camera used. Align it with the * manufacturer specifications, and/or whatever is configured in the PhotonVision Setting * page. - * @param cameraToRobot Transform to move from the camera's mount position to the robot's position + * @param robotToCamera Transform to move from the center of the robot to the camera's mount + * position * @param maxLEDRangeMeters Maximum distance at which your camera can illuminate the target and * make it visible. Set to 9000 or more if your vision system does not rely on LED's. * @param cameraResWidth Width of your camera's image sensor in pixels @@ -79,12 +82,12 @@ public class SimVisionSystem { public SimVisionSystem( String camName, double camDiagFOVDegrees, - Transform3d cameraToRobot, + Transform3d robotToCamera, double maxLEDRangeMeters, int cameraResWidth, int cameraResHeight, double minTargetArea) { - this.cameraToRobot = cameraToRobot; + this.robotToCamera = robotToCamera; this.maxLEDRangeMeters = maxLEDRangeMeters; this.cameraResWidth = cameraResWidth; this.cameraResHeight = cameraResHeight; @@ -119,14 +122,31 @@ public class SimVisionSystem { ; } + /** + * Adds all apriltags from the provided {@link AprilTagFieldLayout} as sim vision targets. The + * poses added will preserve the tag layout's alliance origin at the time of calling this method. + * + * @param tagLayout The field tag layout to get Apriltag poses and IDs from + */ + public void addVisionTargets(AprilTagFieldLayout tagLayout) { + for (AprilTag tag : tagLayout.getTags()) { + addSimVisionTarget( + new SimVisionTarget( + tagLayout.getTagPose(tag.ID).get(), // preserve alliance rotation + Units.inchesToMeters(6), + Units.inchesToMeters(6), + tag.ID)); + } + } + /** * Adjust the camera position relative to the robot. Use this if your camera is on a gimbal or * turret or some other mobile platform. * - * @param newCameraToRobot New Transform from the robot to the camera + * @param newRobotToCamera New Transform from the robot to the camera */ - public void moveCamera(Transform3d newCameraToRobot) { - this.cameraToRobot = newCameraToRobot; + public void moveCamera(Transform3d newRobotToCamera) { + this.robotToCamera = newRobotToCamera; } /** @@ -150,7 +170,7 @@ public class SimVisionSystem { * PhotonVision parameters. */ public void processFrame(Pose3d robotPoseMeters) { - Pose3d cameraPose = robotPoseMeters.transformBy(cameraToRobot.inverse()); + Pose3d cameraPose = robotPoseMeters.transformBy(robotToCamera); dbgRobot.setPose(robotPoseMeters.toPose2d()); dbgCamera.setPose(cameraPose.toPose2d()); diff --git a/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h b/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h index a5493234e..4290e311b 100644 --- a/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h +++ b/photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h @@ -56,10 +56,25 @@ class RobotPoseEstimator { std::pair, frc::Transform3d>; using size_type = std::vector::size_type; + /** + * Create a new RobotPoseEstimator. + * + * @param aprilTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs + * to Pose3ds with respect to the FIRST field. + * @param strategy The strategy it should use to determine the best pose. + * @param cameras An ArrayList of Pairs of PhotonCameras and their respective + * Transform3ds from the center of the robot to the camera mount positions + * (ie, robot -> camera). + */ explicit RobotPoseEstimator( std::shared_ptr aprilTags, PoseStrategy strategy, std::vector cameras); + /** + * Update the estimated pose using the selected strategy. + * + * @return The updated estimated pose and the latency in milliseconds. + */ std::pair Update(); inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; } diff --git a/photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java b/photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java index 5201f3740..4493dbe54 100644 --- a/photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java +++ b/photon-lib/src/test/java/org/photonvision/SimVisionSystemTest.java @@ -182,8 +182,7 @@ class SimVisionSystemTest { new Pose3d(new Translation3d(15.98, 0, 2), new Rotation3d(0, 0, Math.PI)); var robotToCamera = new Transform3d(new Translation3d(0, 0, 1), new Rotation3d(0, Math.PI / 4, 0)); - var sysUnderTest = - new SimVisionSystem("Test", 80.0, robotToCamera.inverse(), 99999, 1234, 1234, 0); + var sysUnderTest = new SimVisionSystem("Test", 80.0, robotToCamera, 99999, 1234, 1234, 0); sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 3.0, 0.5, 1736)); var robotPose = new Pose2d(new Translation2d(14.98, 0), Rotation2d.fromDegrees(5)); @@ -247,14 +246,15 @@ class SimVisionSystemTest { @ParameterizedTest @ValueSource(doubles = {-10, -5, -0, -1, -2, 5, 7, 10.23, 20.21, -19.999}) public void testCameraPitch(double testPitch) { + testPitch = testPitch * -1; + final var targetPose = new Pose3d(new Translation3d(15.98, 0, 0), new Rotation3d(0, 0, 3 * Math.PI / 4)); final var robotPose = new Pose2d(new Translation2d(10, 0), new Rotation2d(0)); var sysUnderTest = new SimVisionSystem("Test", 120.0, new Transform3d(), 99999, 640, 480, 0); sysUnderTest.addSimVisionTarget(new SimVisionTarget(targetPose, 0.5, 0.5, 23)); - // Here, passing in a positive testPitch points the camera downward (since moveCamera takes the - // camera->robot transform) + // Transform is now robot -> camera sysUnderTest.moveCamera( new Transform3d( new Translation3d(), new Rotation3d(0, Units.degreesToRadians(testPitch), 0))); @@ -263,10 +263,11 @@ class SimVisionSystemTest { assertTrue(res.hasTargets()); var tgt = res.getBestTarget(); - // Since the camera is level with the target, a downward point will mean the target is in the - // upper half of the image - // which should produce positive pitch. - assertEquals(testPitch, tgt.getPitch(), 0.0001); + // Since the camera is level with the target, a positive-upward point will mean the target is in + // the + // lower half of the image + // which should produce negative pitch. + assertEquals(testPitch * -1, tgt.getPitch(), 0.0001); } private static Stream distCalCParamProvider() { @@ -309,7 +310,7 @@ class SimVisionSystemTest { new SimVisionSystem( "absurdlylongnamewhichshouldneveractuallyhappenbuteehwelltestitanywaysohowsyourdaygoingihopegoodhaveagreatrestofyourlife!", 160.0, - robotToCamera.inverse(), + robotToCamera, 99999, 640, 480,