AprilTags and PhotonPoseEstimator

Note

For more information on how to methods to get AprilTag data, look here.

PhotonLib includes a PhotonPoseEstimator class, which allows you to combine the pose data from all tags in view in order to get a field relative pose. For each camera, a separate instance of the PhotonPoseEstimator class should be created.

Creating an AprilTagFieldLayout

AprilTagFieldLayout is used to represent a layout of AprilTags within a space (field, shop at home, classroom, etc.). WPILib provides a JSON that describes the layout of AprilTags on the field which you can then use in the AprilTagFieldLayout constructor. You can also specify a custom layout.

The API documentation can be found in here: Java, C++, and Python.

JAVA

        public static final AprilTagFieldLayout kTagLayout =
                AprilTagFieldLayout.loadField(AprilTagFields.kDefaultField);

C++

inline const wpi::apriltag::AprilTagFieldLayout kTagLayout{
    wpi::apriltag::AprilTagFieldLayout::LoadField(

PYTHON

        self.cam = PhotonCamera("YOUR CAMERA NAME")

Defining the Robot to Camera Transform3d

Another necessary argument for creating a PhotonPoseEstimator is the Transform3d representing the robot-relative location and orientation of the camera. A Transform3d contains a Translation3d and a Rotation3d. The Translation3d is created in meters and the Rotation3d is created with radians. For more information on the coordinate system, please see the Coordinate Systems documentation.

JAVA

        public static final Transform3d kRobotToCam =
                new Transform3d(new Translation3d(0.5, 0.0, 0.5), new Rotation3d(0, 0, 0));

C++

inline const wpi::math::Transform3d kRobotToCam{
    wpi::math::Translation3d{0.5_m, 0.0_m, 0.5_m},
    wpi::math::Rotation3d{0_rad, -30_deg, 0_rad}};

PYTHON

kRobotToCam = wpimath.Transform3d(
    wpimath.Translation3d(0.5, 0.0, 0.5),
    wpimath.Rotation3d.fromDegrees(0.0, -30.0, 0.0),
)

Creating a PhotonPoseEstimator

The PhotonPoseEstimator has a constructor that takes an AprilTagFieldLayout (see above) and Transform3d.

JAVA

        photonEstimator = new PhotonPoseEstimator(kTagLayout, kRobotToCam);

C++

 private:
  photon::PhotonPoseEstimator photonEstimator{constants::Vision::kTagLayout,

PYTHON

        self.swerve = drivetrain.Drivetrain()
        self.cam = PhotonCamera("YOUR CAMERA NAME")
        self.camPoseEst = PhotonPoseEstimator(
            AprilTagFieldLayout.loadField(AprilTagField.kDefaultField),

Using a PhotonPoseEstimator

To use your PhotonPoseEstimator, you must create a PhotonCamera and feed the results into your PhotonPoseEstimator. To do this, you must first set the name of your camera in Photon Client. From there you can define the camera in code.

JAVA

        camera = new PhotonCamera(kCameraName);

C++

                                              constants::Vision::kRobotToCam};

PYTHON

        self.controller = wpilib.NiDsXboxController(0)

When taking in a result from a PhotonCamera, PhotonPoseEstimator offers nine possible “strategies” for calculating a pose from a pipeline result in the form of methods that you can call, following the pattern estimate<strategy name>Pose:

• Coprocessor MultiTag (estimateCoprocMultiTagPose)

  • Calculates a new robot position estimate by combining all visible tag corners. Recommended for all teams as it will be the most accurate.

  • Must configure the AprilTagFieldLayout properly in the UI, please see here for more information.

• Lowest Ambiguity (estimateLowestAmbiguityPose)

  • Choose the Pose with the lowest ambiguity.

• Closest to Camera Height (estimateClosestToCameraHeightPose)

  • Choose the Pose which is closest to the camera height.

• Closest to Reference Pose (estimateClosestToReferencePose)

  • Choose the Pose which is closest to the pose that is passed into the function.

• Average Best Targets (estimateAverageBestTargetsPose)

  • Choose the Pose which is the average of all the poses from each tag.

• roboRio MultiTag (estimateRioMultiTagPose)

  • A slower, older version of Coprocessor MultiTag, not recommended for use.

• PnP Distance Trig Solve (estimatePnpDistanceTrigSolvePose)

  • Use distance data from best visible tag to compute a Pose. This runs on the RoboRIO in order to access the robot’s yaw heading, and MUST have addHeadingData called every frame so heading data is up-to-date. Based on a reference implementation by FRC Team 6328 Mechanical Advantage.

• Constrained SolvePnP (estimateConstrainedSolvepnpPose)

  • Solve a constrained version of the Perspective-n-Point problem with the robot’s drivebase flat on the floor. This computation takes place on the RoboRIO, and should not take more than 2ms. This also requires addHeadingData to be called every frame so heading data is up to date.

Calling one of the estimate<strategy>Pose() methods on your PhotonPoseEstimator will return an Optional<EstimatedRobotPose>, which will be empty if there are no detected tags, not enough detected tags (for multi-tag strategies), missing data (typically heading data), or if the internal solvers failed (this is a rare scenario). EstimatedRobotPose includes a Pose3d of the latest estimated pose (using the selected strategy) along with a double of the timestamp when the robot pose was estimated. The recommended way to use the estimatePose methods is to

1. do estimation with one of MultiTag methods, check if the result is empty, then

2. fallback to single tag estimation using a method like estimateLowestAmbiguityPose.

JAVA

            visionEst = photonEstimator.estimateCoprocMultiTagPose(result);
            if (visionEst.isEmpty()) {
                visionEst = photonEstimator.estimateLowestAmbiguityPose(result);
            }

C++

      auto visionEst = photonEstimator.EstimateCoprocMultiTagPose(result);
      if (!visionEst) {
        visionEst = photonEstimator.EstimateLowestAmbiguityPose(result);
      }

PYTHON

    def robotPeriodic(self) -> None:
        for result in self.cam.getAllUnreadResults():
            camEstPose = self.camPoseEst.estimateCoprocMultiTagPose(result)

For Constrained SolvePnP, it’s recommended to do the previously mentioned steps, and then feed the pose (if it exists) into estimateConstrainedSolvepnpPose, and if the Constrained SolvePnP result is empty, simply feed the seed pose into your drivetrain pose estimator.

Once you have the Optional<EstimatedRobotPose>, you can check to see if there’s an actual pose inside, and act accordingly. You should be updating your drivetrain pose estimator with the result from the PhotonPoseEstimator every loop using addVisionMeasurement(). For Java and C++, the examples pass a method from the drivetrain to a Vision object, with the parameter being called estConsumer. Python calls the drivetrain directly.

JAVA

        vision = new Vision(drivetrain::addVisionMeasurement);

C++

  Vision vision{[=, this](wpi::math::Pose2d pose,
                          wpi::units::second_t timestamp,
                          Eigen::Matrix<double, 3, 1> stddevs) {
    drivetrain.AddVisionMeasurement(pose, timestamp, stddevs);

PYTHON

                camEstPose = self.camPoseEst.estimateLowestAmbiguityPose(result)

            self.swerve.addVisionPoseEstimate(

JAVA

        Optional<EstimatedRobotPose> visionEst = Optional.empty();
        for (var result : camera.getAllUnreadResults()) {
            visionEst = photonEstimator.estimateCoprocMultiTagPose(result);
            if (visionEst.isEmpty()) {
                visionEst = photonEstimator.estimateLowestAmbiguityPose(result);
            }
            updateEstimationStdDevs(visionEst, result.getTargets());

            if (Robot.isSimulation()) {
                visionEst.ifPresentOrElse(
                        est ->
                                getSimDebugField()
                                        .getObject("VisionEstimation")
                                        .setPose(est.estimatedPose.toPose2d()),
                        () -> {
                            getSimDebugField().getObject("VisionEstimation").setPoses();
                        });
            }

            visionEst.ifPresent(
                    est -> {
                        // Change our trust in the measurement based on the tags we can see
                        var estStdDevs = getEstimationStdDevs();

                        estConsumer.accept(est.estimatedPose.toPose2d(), est.timestampSeconds, estStdDevs);
                    });
        }

C++

    for (const auto& result : camera.GetAllUnreadResults()) {
      // cache result and update pose estimator
      auto visionEst = photonEstimator.EstimateCoprocMultiTagPose(result);
      if (!visionEst) {
        visionEst = photonEstimator.EstimateLowestAmbiguityPose(result);
      }
      m_latestResult = result;

      // In sim only, add our vision estimate to the sim debug field
      if (wpi::RobotBase::IsSimulation()) {
        if (visionEst) {
          GetSimDebugField()
              .GetObject("VisionEstimation")
              ->SetPose(visionEst->estimatedPose.ToPose2d());
        } else {
          GetSimDebugField().GetObject("VisionEstimation")->SetPoses({});
        }
      }

      if (visionEst) {
        estConsumer(visionEst->estimatedPose.ToPose2d(), visionEst->timestamp,
                    GetEstimationStdDevs(visionEst->estimatedPose.ToPose2d()));
      }
    }

PYTHON

    def robotPeriodic(self) -> None:
        for result in self.cam.getAllUnreadResults():
            camEstPose = self.camPoseEst.estimateCoprocMultiTagPose(result)

Complete Examples

The complete examples for the PhotonPoseEstimator can be found in the following locations:

• Java

• C++

• Python

Additional PhotonPoseEstimator Methods

For more information on the PhotonPoseEstimator class, please see the API documentation.

• Java Documentation

• C++ Documentation

• Python Documentation