Return named type from PhotonPoseEstimator (#734)

Adds PhotonPoseEstimator class, and deprecates RobotPoseEstimator

photon-lib/src/main/java/org/photonvision/EstimatedRobotPose.java

@@ -0,0 +1,47 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.photonvision;
+
+import edu.wpi.first.math.geometry.Pose3d;
+
+/** An estimated pose based on pipeline result */
+public class EstimatedRobotPose {
+    /** The estimated pose */
+    public final Pose3d estimatedPose;
+
+    /** The estimated time the frame used to derive the robot pose was taken */
+    public final double timestampSeconds;
+
+    /**
+     * Constructs an EstimatedRobotPose
+     *
+     * @param estimatedPose estimated pose
+     * @param timestampSeconds timestamp of the estimate
+     */
+    public EstimatedRobotPose(Pose3d estimatedPose, double timestampSeconds) {
+        this.estimatedPose = estimatedPose;
+        this.timestampSeconds = timestampSeconds;
+    }
+}

photon-lib/src/main/java/org/photonvision/PhotonPoseEstimator.java

@@ -0,0 +1,492 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in all
+ * copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+package org.photonvision;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.wpilibj.DriverStation;
+import java.util.ArrayList;
+import java.util.HashSet;
+import java.util.List;
+import java.util.Optional;
+import java.util.Set;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+/**
+ * The PhotonPoseEstimator class filters or combines readings from all the fiducials visible at a
+ * given timestamp on the field to produce a single robot in field pose, using the strategy set
+ * below. Example usage can be found in our apriltagExample example project.
+ */
+public class PhotonPoseEstimator {
+    /** Position estimation strategies that can be used by the {@link PhotonPoseEstimator} class. */
+    public enum PoseStrategy {
+        /** Choose the Pose with the lowest ambiguity. */
+        LOWEST_AMBIGUITY,
+
+        /** Choose the Pose which is closest to the camera height. */
+        CLOSEST_TO_CAMERA_HEIGHT,
+
+        /** Choose the Pose which is closest to a set Reference position. */
+        CLOSEST_TO_REFERENCE_POSE,
+
+        /** Choose the Pose which is closest to the last pose calculated */
+        CLOSEST_TO_LAST_POSE,
+
+        /** Choose the Pose with the lowest ambiguity. */
+        AVERAGE_BEST_TARGETS
+    }
+
+    private AprilTagFieldLayout fieldTags;
+    private PoseStrategy strategy;
+    private final PhotonCamera camera;
+    private final Transform3d robotToCamera;
+
+    private Pose3d lastPose;
+    private Pose3d referencePose;
+    private final Set<Integer> reportedErrors = new HashSet<>();
+
+    /**
+     * Create a new PhotonPoseEstimator.
+     *
+     * @param fieldTags 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 camera PhotonCameras and
+     * @param robotToCamera Transform3d from the center of the robot to the camera mount positions
+     *     (ie, robot ➔ camera).
+     */
+    public PhotonPoseEstimator(
+            AprilTagFieldLayout fieldTags,
+            PoseStrategy strategy,
+            PhotonCamera camera,
+            Transform3d robotToCamera) {
+        this.fieldTags = fieldTags;
+        this.strategy = strategy;
+        this.camera = camera;
+        this.robotToCamera = robotToCamera;
+    }
+
+    /**
+     * Get the AprilTagFieldLayout being used by the PositionEstimator.
+     *
+     * @return the AprilTagFieldLayout
+     */
+    public AprilTagFieldLayout getFieldTags() {
+        return fieldTags;
+    }
+
+    /**
+     * Set the AprilTagFieldLayout being used by the PositionEstimator.
+     *
+     * @param fieldTags the AprilTagFieldLayout
+     */
+    public void setFieldTags(AprilTagFieldLayout fieldTags) {
+        this.fieldTags = fieldTags;
+    }
+
+    /**
+     * Get the Position Estimation Strategy being used by the Position Estimator.
+     *
+     * @return the strategy
+     */
+    public PoseStrategy getStrategy() {
+        return strategy;
+    }
+
+    /**
+     * Set the Position Estimation Strategy used by the Position Estimator.
+     *
+     * @param strategy the strategy to set
+     */
+    public void setStrategy(PoseStrategy strategy) {
+        this.strategy = strategy;
+    }
+
+    /**
+     * Return the reference position that is being used by the estimator.
+     *
+     * @return the referencePose
+     */
+    public Pose3d getReferencePose() {
+        return referencePose;
+    }
+
+    /**
+     * Update the stored reference pose for use when using the <b>CLOSEST_TO_REFERENCE_POSE</b>
+     * strategy.
+     *
+     * @param referencePose the referencePose to set
+     */
+    public void setReferencePose(Pose3d referencePose) {
+        this.referencePose = referencePose;
+    }
+
+    /**
+     * Update the stored reference pose for use when using the <b>CLOSEST_TO_REFERENCE_POSE</b>
+     * strategy.
+     *
+     * @param referencePose the referencePose to set
+     */
+    public void setReferencePose(Pose2d referencePose) {
+        this.referencePose = new Pose3d(referencePose);
+    }
+
+    /**
+     * Update the stored last pose. Useful for setting the initial estimate when using the
+     * <b>CLOSEST_TO_LAST_POSE</b> strategy.
+     *
+     * @param lastPose the lastPose to set
+     */
+    public void setLastPose(Pose3d lastPose) {
+        this.lastPose = lastPose;
+    }
+
+    /**
+     * Update the stored last pose. Useful for setting the initial estimate when using the
+     * <b>CLOSEST_TO_LAST_POSE</b> strategy.
+     *
+     * @param lastPose the lastPose to set
+     */
+    public void setLastPose(Pose2d lastPose) {
+        this.lastPose = new Pose3d(lastPose);
+    }
+
+    /**
+     * Poll data from the configured cameras and update the estimated position of the robot. Returns
+     * empty if there are no cameras set or no targets were found from the cameras.
+     *
+     * @return an EstimatedRobotPose with an estimated pose, and information about the camera(s) and
+     *     pipeline results used to create the estimate
+     */
+    public Optional<EstimatedRobotPose> update() {
+        if (camera == null) {
+            DriverStation.reportError("[PhotonPoseEstimator] Missing camera!", false);
+            return Optional.empty();
+        }
+
+        PhotonPipelineResult cameraResult = camera.getLatestResult();
+        if (!cameraResult.hasTargets()) {
+            return Optional.empty();
+        }
+
+        Optional<EstimatedRobotPose> estimatedPose;
+        switch (strategy) {
+            case LOWEST_AMBIGUITY:
+                estimatedPose = lowestAmbiguityStrategy(cameraResult);
+                break;
+            case CLOSEST_TO_CAMERA_HEIGHT:
+                estimatedPose = closestToCameraHeightStrategy(cameraResult);
+                break;
+            case CLOSEST_TO_LAST_POSE:
+                setReferencePose(lastPose);
+            case CLOSEST_TO_REFERENCE_POSE:
+                estimatedPose = closestToReferencePoseStrategy(cameraResult, referencePose);
+                break;
+            case AVERAGE_BEST_TARGETS:
+                estimatedPose = averageBestTargetsStrategy(cameraResult);
+                break;
+            default:
+                DriverStation.reportError(
+                        "[PhotonPoseEstimator] Unknown Position Estimation Strategy!", false);
+                return Optional.empty();
+        }
+
+        if (estimatedPose.isEmpty()) {
+            lastPose = null;
+        }
+
+        return estimatedPose;
+    }
+
+    /**
+     * Return the estimated position of the robot with the lowest position ambiguity from a List of
+     * pipeline results.
+     *
+     * @param result pipeline result
+     * @return the estimated position of the robot in the FCS and the estimated timestamp of this
+     *     estimation.
+     */
+    private Optional<EstimatedRobotPose> lowestAmbiguityStrategy(PhotonPipelineResult result) {
+        PhotonTrackedTarget lowestAmbiguityTarget = null;
+
+        double lowestAmbiguityScore = 10;
+
+        for (PhotonTrackedTarget target : result.targets) {
+            double targetPoseAmbiguity = target.getPoseAmbiguity();
+            // Make sure the target is a Fiducial target.
+            if (targetPoseAmbiguity != -1 && targetPoseAmbiguity < lowestAmbiguityScore) {
+                lowestAmbiguityScore = targetPoseAmbiguity;
+                lowestAmbiguityTarget = target;
+            }
+        }
+
+        // Although there are confirmed to be targets, none of them may be fiducial
+        // targets.
+        if (lowestAmbiguityTarget == null) return Optional.empty();
+
+        int targetFiducialId = lowestAmbiguityTarget.getFiducialId();
+
+        Optional<Pose3d> targetPosition = fieldTags.getTagPose(targetFiducialId);
+
+        if (targetPosition.isEmpty()) {
+            reportFiducialPoseError(targetFiducialId);
+            return Optional.empty();
+        }
+
+        return Optional.of(
+                new EstimatedRobotPose(
+                        targetPosition
+                                .get()
+                                .transformBy(lowestAmbiguityTarget.getBestCameraToTarget().inverse())
+                                .transformBy(robotToCamera.inverse()),
+                        result.getTimestampSeconds()));
+    }
+
+    /**
+     * Return the estimated position of the robot using the target with the lowest delta height
+     * difference between the estimated and actual height of the camera.
+     *
+     * @param result pipeline result
+     * @return the estimated position of the robot in the FCS and the estimated timestamp of this
+     *     estimation.
+     */
+    private Optional<EstimatedRobotPose> closestToCameraHeightStrategy(PhotonPipelineResult result) {
+        double smallestHeightDifference = 10e9;
+        EstimatedRobotPose closestHeightTarget = null;
+
+        for (PhotonTrackedTarget target : result.targets) {
+            int targetFiducialId = target.getFiducialId();
+
+            // Don't report errors for non-fiducial targets. This could also be resolved by
+            // adding -1 to
+            // the initial HashSet.
+            if (targetFiducialId == -1) continue;
+
+            Optional<Pose3d> targetPosition = fieldTags.getTagPose(target.getFiducialId());
+
+            if (targetPosition.isEmpty()) {
+                reportFiducialPoseError(target.getFiducialId());
+                continue;
+            }
+
+            double alternateTransformDelta =
+                    Math.abs(
+                            robotToCamera.getZ()
+                                    - targetPosition
+                                            .get()
+                                            .transformBy(target.getAlternateCameraToTarget().inverse())
+                                            .getZ());
+            double bestTransformDelta =
+                    Math.abs(
+                            robotToCamera.getZ()
+                                    - targetPosition
+                                            .get()
+                                            .transformBy(target.getBestCameraToTarget().inverse())
+                                            .getZ());
+
+            if (alternateTransformDelta < smallestHeightDifference) {
+                smallestHeightDifference = alternateTransformDelta;
+                closestHeightTarget =
+                        new EstimatedRobotPose(
+                                targetPosition
+                                        .get()
+                                        .transformBy(target.getAlternateCameraToTarget().inverse())
+                                        .transformBy(robotToCamera.inverse()),
+                                result.getTimestampSeconds());
+            }
+
+            if (bestTransformDelta < smallestHeightDifference) {
+                smallestHeightDifference = bestTransformDelta;
+                closestHeightTarget =
+                        new EstimatedRobotPose(
+                                targetPosition
+                                        .get()
+                                        .transformBy(target.getBestCameraToTarget().inverse())
+                                        .transformBy(robotToCamera.inverse()),
+                                result.getTimestampSeconds());
+            }
+        }
+
+        // Need to null check here in case none of the provided targets are fiducial.
+        return Optional.ofNullable(closestHeightTarget);
+    }
+
+    /**
+     * Return the estimated position of the robot using the target with the lowest delta in the vector
+     * magnitude between it and the reference pose.
+     *
+     * @param result pipeline result
+     * @param referencePose reference pose to check vector magnitude difference against.
+     * @return the estimated position of the robot in the FCS and the estimated timestamp of this
+     *     estimation.
+     */
+    private Optional<EstimatedRobotPose> closestToReferencePoseStrategy(
+            PhotonPipelineResult result, Pose3d referencePose) {
+        if (referencePose == null) {
+            DriverStation.reportError(
+                    "[PhotonPoseEstimator] Tried to use reference pose strategy without setting the reference!",
+                    false);
+            return Optional.empty();
+        }
+
+        double smallestPoseDelta = 10e9;
+        EstimatedRobotPose lowestDeltaPose = null;
+
+        for (PhotonTrackedTarget target : result.targets) {
+            int targetFiducialId = target.getFiducialId();
+
+            // Don't report errors for non-fiducial targets. This could also be resolved by
+            // adding -1 to
+            // the initial HashSet.
+            if (targetFiducialId == -1) continue;
+
+            Optional<Pose3d> targetPosition = fieldTags.getTagPose(target.getFiducialId());
+
+            if (targetPosition.isEmpty()) {
+                reportFiducialPoseError(targetFiducialId);
+                continue;
+            }
+
+            Pose3d altTransformPosition =
+                    targetPosition
+                            .get()
+                            .transformBy(target.getAlternateCameraToTarget().inverse())
+                            .transformBy(robotToCamera.inverse());
+            Pose3d bestTransformPosition =
+                    targetPosition
+                            .get()
+                            .transformBy(target.getBestCameraToTarget().inverse())
+                            .transformBy(robotToCamera.inverse());
+
+            double altDifference = Math.abs(calculateDifference(referencePose, altTransformPosition));
+            double bestDifference = Math.abs(calculateDifference(referencePose, bestTransformPosition));
+
+            if (altDifference < smallestPoseDelta) {
+                smallestPoseDelta = altDifference;
+                lowestDeltaPose =
+                        new EstimatedRobotPose(altTransformPosition, result.getTimestampSeconds());
+            }
+            if (bestDifference < smallestPoseDelta) {
+                smallestPoseDelta = bestDifference;
+                lowestDeltaPose =
+                        new EstimatedRobotPose(bestTransformPosition, result.getTimestampSeconds());
+            }
+        }
+        return Optional.ofNullable(lowestDeltaPose);
+    }
+
+    /**
+     * Return the average of the best target poses using ambiguity as weight.
+     *
+     * @param result pipeline result
+     * @return the estimated position of the robot in the FCS and the estimated timestamp of this
+     *     estimation.
+     */
+    private Optional<EstimatedRobotPose> averageBestTargetsStrategy(PhotonPipelineResult result) {
+        List<Pair<PhotonTrackedTarget, Pose3d>> estimatedRobotPoses = new ArrayList<>();
+        double totalAmbiguity = 0;
+
+        for (PhotonTrackedTarget target : result.targets) {
+            int targetFiducialId = target.getFiducialId();
+
+            // Don't report errors for non-fiducial targets. This could also be resolved by
+            // adding -1 to
+            // the initial HashSet.
+            if (targetFiducialId == -1) continue;
+
+            Optional<Pose3d> targetPosition = fieldTags.getTagPose(target.getFiducialId());
+
+            if (targetPosition.isEmpty()) {
+                reportFiducialPoseError(targetFiducialId);
+                continue;
+            }
+
+            double targetPoseAmbiguity = target.getPoseAmbiguity();
+
+            // Pose ambiguity is 0, use that pose
+            if (targetPoseAmbiguity == 0) {
+                return Optional.of(
+                        new EstimatedRobotPose(
+                                targetPosition
+                                        .get()
+                                        .transformBy(target.getBestCameraToTarget().inverse())
+                                        .transformBy(robotToCamera.inverse()),
+                                result.getTimestampSeconds()));
+            }
+
+            totalAmbiguity += 1.0 / target.getPoseAmbiguity();
+
+            estimatedRobotPoses.add(
+                    new Pair<>(
+                            target,
+                            targetPosition
+                                    .get()
+                                    .transformBy(target.getBestCameraToTarget().inverse())
+                                    .transformBy(robotToCamera.inverse())));
+        }
+
+        // Take the average
+
+        Translation3d transform = new Translation3d();
+        Rotation3d rotation = new Rotation3d();
+
+        if (estimatedRobotPoses.isEmpty()) return Optional.empty();
+
+        for (Pair<PhotonTrackedTarget, Pose3d> pair : estimatedRobotPoses) {
+            // Total ambiguity is non-zero confirmed because if it was zero, that pose was
+            // returned.
+            double weight = (1.0 / pair.getFirst().getPoseAmbiguity()) / totalAmbiguity;
+            Pose3d estimatedPose = pair.getSecond();
+            transform = transform.plus(estimatedPose.getTranslation().times(weight));
+            rotation = rotation.plus(estimatedPose.getRotation().times(weight));
+        }
+
+        return Optional.of(
+                new EstimatedRobotPose(new Pose3d(transform, rotation), result.getTimestampSeconds()));
+    }
+
+    /**
+     * Difference is defined as the vector magnitude between the two poses
+     *
+     * @return The absolute "difference" (>=0) between two Pose3ds.
+     */
+    private double calculateDifference(Pose3d x, Pose3d y) {
+        return x.getTranslation().getDistance(y.getTranslation());
+    }
+
+    private void reportFiducialPoseError(int fiducialId) {
+        if (!reportedErrors.contains(fiducialId)) {
+            DriverStation.reportError(
+                    "[PhotonPoseEstimator] Tried to get pose of unknown April Tag: " + fiducialId, false);
+            reportedErrors.add(fiducialId);
+        }
+    }
+}

photon-lib/src/main/java/org/photonvision/RobotPoseEstimator.java

@@ -40,6 +40,8 @@ import java.util.Set;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 
+/** @deprecated Use {@link PhotonPoseEstimator} */
+@Deprecated
 public class RobotPoseEstimator {
     /**
      *

photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp

@@ -0,0 +1,270 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include "photonlib/PhotonPoseEstimator.h"
+
+#include <iostream>
+#include <limits>
+#include <map>
+#include <span>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <frc/Errors.h>
+#include <frc/geometry/Pose3d.h>
+#include <frc/geometry/Rotation3d.h>
+#include <frc/geometry/Transform3d.h>
+#include <units/time.h>
+
+#include "photonlib/PhotonCamera.h"
+#include "photonlib/PhotonPipelineResult.h"
+#include "photonlib/PhotonTrackedTarget.h"
+
+namespace photonlib {
+PhotonPoseEstimator::PhotonPoseEstimator(frc::AprilTagFieldLayout tags,
+                                         PoseStrategy strat, PhotonCamera&& cam,
+                                         frc::Transform3d robotToCamera)
+    : aprilTags(tags),
+      strategy(strat),
+      camera(std::move(cam)),
+      m_robotToCamera(robotToCamera),
+      lastPose(frc::Pose3d()),
+      referencePose(frc::Pose3d()) {}
+
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update() {
+  auto result = camera.GetLatestResult();
+
+  if (!result.HasTargets()) {
+    return std::nullopt;
+  }
+
+  std::optional<EstimatedRobotPose> ret = std::nullopt;
+
+  switch (strategy) {
+    case LOWEST_AMBIGUITY:
+      ret = LowestAmbiguityStrategy(result);
+      break;
+    case CLOSEST_TO_CAMERA_HEIGHT:
+      ret = ClosestToCameraHeightStrategy(result);
+      break;
+    case CLOSEST_TO_REFERENCE_POSE:
+      ret = ClosestToReferencePoseStrategy(result);
+      break;
+    case CLOSEST_TO_LAST_POSE:
+      SetReferencePose(lastPose);
+      ret = ClosestToReferencePoseStrategy(result);
+      break;
+    case AVERAGE_BEST_TARGETS:
+      ret = AverageBestTargetsStrategy(result);
+      break;
+    default:
+      FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
+                      "");
+      return std::nullopt;
+  }
+
+  if (!ret) {
+    // TODO
+  }
+
+  return ret;
+}
+
+std::optional<EstimatedRobotPose> PhotonPoseEstimator::LowestAmbiguityStrategy(
+    PhotonPipelineResult result) {
+  int lowestAJ = -1;
+  double lowestAmbiguityScore = std::numeric_limits<double>::infinity();
+  auto targets = result.GetTargets();
+  for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
+    if (targets[j].GetPoseAmbiguity() < lowestAmbiguityScore) {
+      lowestAJ = j;
+      lowestAmbiguityScore = targets[j].GetPoseAmbiguity();
+    }
+  }
+
+  if (lowestAJ == -1) {
+    return std::nullopt;
+  }
+
+  PhotonTrackedTarget bestTarget = targets[lowestAJ];
+
+  std::optional<frc::Pose3d> fiducialPose =
+      aprilTags.GetTagPose(bestTarget.GetFiducialId());
+  if (!fiducialPose) {
+    FRC_ReportError(frc::warn::Warning,
+                    "Tried to get pose of unknown April Tag: {}",
+                    bestTarget.GetFiducialId());
+    return std::nullopt;
+  }
+
+  return EstimatedRobotPose{
+      fiducialPose.value()
+          .TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
+          .TransformBy(m_robotToCamera.Inverse()),
+      result.GetTimestamp()};
+}
+
+std::optional<EstimatedRobotPose>
+PhotonPoseEstimator::ClosestToCameraHeightStrategy(
+    PhotonPipelineResult result) {
+  units::meter_t smallestHeightDifference =
+      units::meter_t(std::numeric_limits<double>::infinity());
+
+  std::optional<EstimatedRobotPose> pose = std::nullopt;
+
+  for (auto& target : result.GetTargets()) {
+    std::optional<frc::Pose3d> fiducialPose =
+        aprilTags.GetTagPose(target.GetFiducialId());
+    if (!fiducialPose) {
+      FRC_ReportError(frc::warn::Warning,
+                      "Tried to get pose of unknown April Tag: {}",
+                      target.GetFiducialId());
+      continue;
+    }
+    frc::Pose3d targetPose = fiducialPose.value();
+
+    units::meter_t alternativeDifference = units::math::abs(
+        m_robotToCamera.Z() -
+        targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
+            .Z());
+
+    units::meter_t bestDifference = units::math::abs(
+        m_robotToCamera.Z() -
+        targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z());
+
+    if (alternativeDifference < smallestHeightDifference) {
+      smallestHeightDifference = alternativeDifference;
+      pose = EstimatedRobotPose{
+          targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
+              .TransformBy(m_robotToCamera.Inverse()),
+          result.GetTimestamp()};
+    }
+    if (bestDifference < smallestHeightDifference) {
+      smallestHeightDifference = bestDifference;
+      pose = EstimatedRobotPose{
+          targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
+              .TransformBy(m_robotToCamera.Inverse()),
+          result.GetTimestamp()};
+    }
+  }
+
+  return pose;
+}
+
+std::optional<EstimatedRobotPose>
+PhotonPoseEstimator::ClosestToReferencePoseStrategy(
+    PhotonPipelineResult result) {
+  units::meter_t smallestDifference =
+      units::meter_t(std::numeric_limits<double>::infinity());
+  units::second_t stateTimestamp = units::second_t(0);
+  frc::Pose3d pose = lastPose;
+
+  auto targets = result.GetTargets();
+  for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
+    PhotonTrackedTarget target = targets[j];
+    std::optional<frc::Pose3d> fiducialPose =
+        aprilTags.GetTagPose(target.GetFiducialId());
+    if (!fiducialPose) {
+      FRC_ReportError(frc::warn::Warning,
+                      "Tried to get pose of unknown April Tag: {}",
+                      target.GetFiducialId());
+      continue;
+    }
+    frc::Pose3d targetPose = fiducialPose.value();
+
+    const auto altPose =
+        targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
+            .TransformBy(m_robotToCamera.Inverse());
+    const auto bestPose =
+        targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
+            .TransformBy(m_robotToCamera.Inverse());
+
+    units::meter_t alternativeDifference = units::math::abs(
+        referencePose.Translation().Distance(altPose.Translation()));
+    units::meter_t bestDifference = units::math::abs(
+        referencePose.Translation().Distance(bestPose.Translation()));
+    if (alternativeDifference < smallestDifference) {
+      smallestDifference = alternativeDifference;
+      pose = altPose;
+      stateTimestamp = result.GetTimestamp();
+    }
+
+    if (bestDifference < smallestDifference) {
+      smallestDifference = bestDifference;
+      pose = bestPose;
+      stateTimestamp = result.GetTimestamp();
+    }
+  }
+
+  return EstimatedRobotPose{pose, stateTimestamp};
+}
+
+std::optional<EstimatedRobotPose>
+PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
+  std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
+      tempPoses;
+  double totalAmbiguity = 0;
+
+  auto targets = result.GetTargets();
+  for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
+    PhotonTrackedTarget target = targets[j];
+    std::optional<frc::Pose3d> fiducialPose =
+        aprilTags.GetTagPose(target.GetFiducialId());
+    if (!fiducialPose) {
+      FRC_ReportError(frc::warn::Warning,
+                      "Tried to get pose of unknown April Tag: {}",
+                      target.GetFiducialId());
+      continue;
+    }
+
+    frc::Pose3d targetPose = fiducialPose.value();
+    // Ambiguity = 0, use that pose
+    if (target.GetPoseAmbiguity() == 0) {
+      return EstimatedRobotPose{
+          targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
+              .TransformBy(m_robotToCamera.Inverse()),
+          result.GetLatency()};
+    }
+    totalAmbiguity += 1. / target.GetPoseAmbiguity();
+
+    tempPoses.push_back(std::make_pair(
+        targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
+        std::make_pair(target.GetPoseAmbiguity(), result.GetTimestamp())));
+  }
+
+  frc::Translation3d transform = frc::Translation3d();
+  frc::Rotation3d rotation = frc::Rotation3d();
+
+  for (std::pair<frc::Pose3d, std::pair<double, units::second_t>>& pair :
+       tempPoses) {
+    double weight = (1. / pair.second.first) / totalAmbiguity;
+    transform = transform + pair.first.Translation() * weight;
+    rotation = rotation + pair.first.Rotation() * weight;
+  }
+
+  return EstimatedRobotPose{frc::Pose3d(transform, rotation),
+                            result.GetTimestamp()};
+}
+}  // namespace photonlib

photon-lib/src/main/native/cpp/photonlib/PhotonTrackedTarget.cpp

@@ -38,7 +38,8 @@ PhotonTrackedTarget::PhotonTrackedTarget(
     double yaw, double pitch, double area, double skew, int id,
     const frc::Transform3d& pose, const frc::Transform3d& alternatePose,
     double ambiguity,
-    const wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners)
+    const wpi::SmallVector<std::pair<double, double>, 4> minAreaRectCorners,
+    const std::vector<std::pair<double, double>> detectedCorners)
     : yaw(yaw),
       pitch(pitch),
       area(area),
@@ -47,7 +48,8 @@ PhotonTrackedTarget::PhotonTrackedTarget(
       bestCameraToTarget(pose),
       altCameraToTarget(alternatePose),
       poseAmbiguity(ambiguity),
-      minAreaRectCorners(minAreaRectCorners) {}
+      minAreaRectCorners(minAreaRectCorners),
+      detectedCorners(detectedCorners) {}
 
 bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {
   return other.yaw == yaw && other.pitch == pitch && other.area == area &&

photon-lib/src/main/native/cpp/photonlib/RobotPoseEstimator.cpp

@@ -54,11 +54,12 @@ RobotPoseEstimator::RobotPoseEstimator(
       lastPose(frc::Pose3d()),
       referencePose(frc::Pose3d()) {}
 
-std::pair<frc::Pose3d, units::millisecond_t> RobotPoseEstimator::Update() {
+std::pair<frc::Pose3d, units::second_t> RobotPoseEstimator::Update() {
   if (cameras.empty()) {
-    return std::make_pair(lastPose, units::millisecond_t(0));
+    return std::make_pair(lastPose, units::second_t(0));
   }
-  std::pair<frc::Pose3d, units::millisecond_t> pair;
+
+  std::pair<frc::Pose3d, units::second_t> pair;
   switch (strategy) {
     case LOWEST_AMBIGUITY:
       pair = LowestAmbiguityStrategy();
@@ -73,7 +74,7 @@ std::pair<frc::Pose3d, units::millisecond_t> RobotPoseEstimator::Update() {
       lastPose = pair.first;
       return pair;
     case CLOSEST_TO_LAST_POSE:
-      referencePose = lastPose;
+      SetReferencePose(lastPose);
       pair = ClosestToReferencePoseStrategy();
       lastPose = pair.first;
       return pair;
@@ -85,10 +86,11 @@ std::pair<frc::Pose3d, units::millisecond_t> RobotPoseEstimator::Update() {
       FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
                       "");
   }
-  return std::make_pair(lastPose, units::millisecond_t(0));
+
+  return std::make_pair(lastPose, units::second_t(0));
 }
 
-std::pair<frc::Pose3d, units::millisecond_t>
+std::pair<frc::Pose3d, units::second_t>
 RobotPoseEstimator::LowestAmbiguityStrategy() {
   int lowestAI = -1;
   int lowestAJ = -1;
@@ -107,7 +109,7 @@ RobotPoseEstimator::LowestAmbiguityStrategy() {
   }
 
   if (lowestAI == -1 || lowestAJ == -1) {
-    return std::make_pair(lastPose, units::millisecond_t(0));
+    return std::make_pair(lastPose, units::second_t(0));
   }
 
   PhotonTrackedTarget bestTarget =
@@ -119,20 +121,21 @@ RobotPoseEstimator::LowestAmbiguityStrategy() {
     FRC_ReportError(frc::warn::Warning,
                     "Tried to get pose of unknown April Tag: {}",
                     bestTarget.GetFiducialId());
-    return std::make_pair(lastPose, units::millisecond_t(0));
+    return std::make_pair(lastPose, units::second_t(0));
   }
 
   return std::make_pair(
       fiducialPose.value()
           .TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
           .TransformBy(cameras[lowestAI].second.Inverse()),
-      cameras[lowestAI].first->GetLatestResult().GetLatency() / 1000.);
+      cameras[lowestAI].first->GetLatestResult().GetTimestamp());
 }
-std::pair<frc::Pose3d, units::millisecond_t>
+
+std::pair<frc::Pose3d, units::second_t>
 RobotPoseEstimator::ClosestToCameraHeightStrategy() {
   units::meter_t smallestHeightDifference =
       units::meter_t(std::numeric_limits<double>::infinity());
-  units::millisecond_t milli = units::millisecond_t(0);
+  units::second_t stateTimestamp = units::second_t(0);
   frc::Pose3d pose = lastPose;
 
   for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
@@ -161,22 +164,23 @@ RobotPoseEstimator::ClosestToCameraHeightStrategy() {
         smallestHeightDifference = alternativeDifference;
         pose = targetPose.TransformBy(
             target.GetAlternateCameraToTarget().Inverse());
-        milli = p.first->GetLatestResult().GetLatency() / 1000.;
+        stateTimestamp = p.first->GetLatestResult().GetTimestamp();
       }
       if (bestDifference < smallestHeightDifference) {
         smallestHeightDifference = bestDifference;
         pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
-        milli = p.first->GetLatestResult().GetLatency() / 1000.;
+        stateTimestamp = p.first->GetLatestResult().GetTimestamp();
       }
     }
   }
-  return std::make_pair(pose, milli);
+  return std::make_pair(pose, stateTimestamp);
 }
-std::pair<frc::Pose3d, units::millisecond_t>
+
+std::pair<frc::Pose3d, units::second_t>
 RobotPoseEstimator::ClosestToReferencePoseStrategy() {
   units::meter_t smallestDifference =
       units::meter_t(std::numeric_limits<double>::infinity());
-  units::millisecond_t milli = units::millisecond_t(0);
+  units::second_t stateTimestamp = units::second_t(0);
   frc::Pose3d pose = lastPose;
 
   for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
@@ -207,29 +211,32 @@ RobotPoseEstimator::ClosestToReferencePoseStrategy() {
         smallestDifference = alternativeDifference;
         pose = targetPose.TransformBy(
             target.GetAlternateCameraToTarget().Inverse());
-        milli = p.first->GetLatestResult().GetLatency() / 1000.;
+        stateTimestamp = p.first->GetLatestResult().GetTimestamp();
       }
 
       if (bestDifference < smallestDifference) {
         smallestDifference = bestDifference;
         pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
-        milli = p.first->GetLatestResult().GetLatency() / 1000.;
+        stateTimestamp = p.first->GetLatestResult().GetTimestamp();
       }
     }
   }
-  return std::make_pair(pose, milli);
+
+  return std::make_pair(pose, stateTimestamp);
 }
 
-std::pair<frc::Pose3d, units::millisecond_t>
+std::pair<frc::Pose3d, units::second_t>
 RobotPoseEstimator::AverageBestTargetsStrategy() {
-  std::vector<std::pair<frc::Pose3d, std::pair<double, units::millisecond_t>>>
+  std::vector<std::pair<frc::Pose3d, std::pair<double, units::second_t>>>
       tempPoses;
   double totalAmbiguity = 0;
+  units::second_t timstampSum = units::second_t(0);
 
   for (RobotPoseEstimator::size_type i = 0; i < cameras.size(); ++i) {
     std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d> p = cameras[i];
     std::span<const PhotonTrackedTarget> targets =
         p.first->GetLatestResult().GetTargets();
+    timstampSum += p.first->GetLatestResult().GetTimestamp();
     for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
       PhotonTrackedTarget target = targets[j];
       std::optional<frc::Pose3d> fiducialPose =
@@ -255,20 +262,21 @@ RobotPoseEstimator::AverageBestTargetsStrategy() {
       tempPoses.push_back(std::make_pair(
           targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
           std::make_pair(target.GetPoseAmbiguity(),
-                         p.first->GetLatestResult().GetLatency() / 1000.)));
+                         p.first->GetLatestResult().GetTimestamp())));
     }
   }
+
   frc::Translation3d transform = frc::Translation3d();
   frc::Rotation3d rotation = frc::Rotation3d();
-  units::millisecond_t latency = units::millisecond_t(0);
 
-  for (std::pair<frc::Pose3d, std::pair<double, units::millisecond_t>>& pair :
+  for (std::pair<frc::Pose3d, std::pair<double, units::second_t>>& pair :
        tempPoses) {
     double weight = (1. / pair.second.first) / totalAmbiguity;
     transform = transform + pair.first.Translation() * weight;
     rotation = rotation + pair.first.Rotation() * weight;
-    latency += pair.second.second * weight;
   }
-  return std::make_pair(frc::Pose3d(transform, rotation), latency);
+
+  return std::make_pair(frc::Pose3d(transform, rotation),
+                        timstampSum / cameras.size());
 }
 }  // namespace photonlib

photon-lib/src/main/native/include/photonlib/PhotonCamera.h

@@ -68,6 +68,8 @@ class PhotonCamera {
    */
   explicit PhotonCamera(const std::string_view cameraName);
 
+  PhotonCamera(PhotonCamera&&) = default;
+
   virtual ~PhotonCamera() = default;
 
   /**

photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h

@@ -0,0 +1,196 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#pragma once
+
+#include <map>
+#include <memory>
+#include <utility>
+#include <vector>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/geometry/Pose3d.h>
+#include <frc/geometry/Transform3d.h>
+
+#include "photonlib/PhotonCamera.h"
+
+namespace photonlib {
+enum PoseStrategy : int {
+  LOWEST_AMBIGUITY,
+  CLOSEST_TO_CAMERA_HEIGHT,
+  CLOSEST_TO_REFERENCE_POSE,
+  CLOSEST_TO_LAST_POSE,
+  AVERAGE_BEST_TARGETS
+};
+
+struct EstimatedRobotPose {
+  /** The estimated pose */
+  frc::Pose3d estimatedPose;
+  /** The estimated time the frame used to derive the robot pose was taken, in
+   * the same timebase as the RoboRIO FPGA Timestamp */
+  units::second_t timestamp;
+
+  EstimatedRobotPose(frc::Pose3d pose_, units::second_t time_)
+      : estimatedPose(pose_), timestamp(time_) {}
+};
+
+/**
+ * The PhotonPoseEstimator class filters or combines readings from all the
+ * fiducials visible at a given timestamp on the field to produce a single robot
+ * in field pose, using the strategy set below. Example usage can be found in
+ * our apriltagExample example project.
+ */
+class PhotonPoseEstimator {
+ public:
+  using map_value_type =
+      std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>;
+  using size_type = std::vector<map_value_type>::size_type;
+
+  /**
+   * Create a new PhotonPoseEstimator.
+   *
+   * <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>();
+   * <p> map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is
+   * at (1.0,2.0,3.0) </code> }
+   *
+   * @param aprilTags A 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 camera PhotonCameras and
+   * @param robotToCamera Transform3d from the center of the robot to the camera
+   * mount positions (ie, robot ➔ camera).
+   */
+  explicit PhotonPoseEstimator(frc::AprilTagFieldLayout aprilTags,
+                               PoseStrategy strategy, PhotonCamera&& camera,
+                               frc::Transform3d robotToCamera);
+
+  /**
+   * Get the AprilTagFieldLayout being used by the PositionEstimator.
+   *
+   * @return the AprilTagFieldLayout
+   */
+  frc::AprilTagFieldLayout GetFieldLayout() const { return aprilTags; }
+
+  /**
+   * Get the Position Estimation Strategy being used by the Position Estimator.
+   *
+   * @return the strategy
+   */
+  PoseStrategy GetPoseStrategy() const { return strategy; }
+
+  /**
+   * Set the Position Estimation Strategy used by the Position Estimator.
+   *
+   * @param strategy the strategy to set
+   */
+  inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
+
+  /**
+   * Return the reference position that is being used by the estimator.
+   *
+   * @return the referencePose
+   */
+  frc::Pose3d GetReferencePose() const { return referencePose; }
+
+  /**
+   * Update the stored reference pose for use when using the
+   * CLOSEST_TO_REFERENCE_POSE strategy.
+   *
+   * @param referencePose the referencePose to set
+   */
+  inline void SetReferencePose(frc::Pose3d referencePose) {
+    this->referencePose = referencePose;
+  }
+
+  /**
+   * Update the stored last pose. Useful for setting the initial estimate when
+   * using the CLOSEST_TO_LAST_POSE strategy.
+   *
+   * @param lastPose the lastPose to set
+   */
+  inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
+
+  /**
+   * Update the pose estimator. Internally grabs a new PhotonPipelineResult from
+   * the camera and process it.
+   */
+  std::optional<EstimatedRobotPose> Update();
+
+  inline PhotonCamera& GetCamera() { return camera; }
+
+ private:
+  frc::AprilTagFieldLayout aprilTags;
+  PoseStrategy strategy;
+
+  PhotonCamera camera;
+  frc::Transform3d m_robotToCamera;
+
+  frc::Pose3d lastPose;
+  frc::Pose3d referencePose;
+
+  /**
+   * Return the estimated position of the robot with the lowest position
+   * ambiguity from a List of pipeline results.
+   *
+   * @return the estimated position of the robot in the FCS and the estimated
+   * timestamp of this estimation.
+   */
+  std::optional<EstimatedRobotPose> LowestAmbiguityStrategy(
+      PhotonPipelineResult result);
+
+  /**
+   * Return the estimated position of the robot using the target with the lowest
+   * delta height difference between the estimated and actual height of the
+   * camera.
+   *
+   * @return the estimated position of the robot in the FCS and the estimated
+   * timestamp of this estimation.
+   */
+  std::optional<EstimatedRobotPose> ClosestToCameraHeightStrategy(
+      PhotonPipelineResult result);
+
+  /**
+   * Return the estimated position of the robot using the target with the lowest
+   * delta in the vector magnitude between it and the reference pose.
+   *
+   * @param referencePose reference pose to check vector magnitude difference
+   * against.
+   * @return the estimated position of the robot in the FCS and the estimated
+   * timestamp of this estimation.
+   */
+  std::optional<EstimatedRobotPose> ClosestToReferencePoseStrategy(
+      PhotonPipelineResult result);
+
+  /**
+   * Return the average of the best target poses using ambiguity as weight.
+
+   * @return the estimated position of the robot in the FCS and the estimated
+   timestamp of this
+   *     estimation.
+   */
+  std::optional<EstimatedRobotPose> AverageBestTargetsStrategy(
+      PhotonPipelineResult result);
+};
+
+}  // namespace photonlib

photon-lib/src/main/native/include/photonlib/PhotonTrackedTarget.h

@@ -59,7 +59,8 @@ class PhotonTrackedTarget {
       double yaw, double pitch, double area, double skew, int fiducialID,
       const frc::Transform3d& pose, const frc::Transform3d& alternatePose,
       double ambiguity,
-      const wpi::SmallVector<std::pair<double, double>, 4> corners);
+      const wpi::SmallVector<std::pair<double, double>, 4> corners,
+      const std::vector<std::pair<double, double>> detectedCorners);
 
   /**
    * Returns the target yaw (positive-left).

photon-lib/src/main/native/include/photonlib/RobotPoseEstimator.h

@@ -48,7 +48,10 @@ enum PoseStrategy : int {
 };
 
 /**
- * A managing class to determine how an estimated pose should be chosen.
+ * The RobotPoseEstimator class filters or combines readings from all the
+ * fiducials visible at a given timestamp on the field to produce a single robot
+ * in field pose, using the strategy set below. Example usage can be found in
+ * our apriltagExample example project.
  */
 class RobotPoseEstimator {
  public:
@@ -59,44 +62,81 @@ class RobotPoseEstimator {
   /**
    * Create a new RobotPoseEstimator.
    *
-   * @param aprilTags A WPILib {@link AprilTagFieldLayout} linking AprilTag IDs
-   * to Pose3ds with respect to the FIRST field.
+   * <p>Example: {@code <code> <p> Map<Integer, Pose3d> map = new HashMap<>();
+   * <p> map.put(1, new Pose3d(1.0, 2.0, 3.0, new Rotation3d())); // Tag ID 1 is
+   * at (1.0,2.0,3.0) </code> }
+   *
+   * @param aprilTags A 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).
+   * Transform3ds from the center of the robot to the cameras.
    */
   explicit RobotPoseEstimator(
       std::shared_ptr<frc::AprilTagFieldLayout> aprilTags,
       PoseStrategy strategy, std::vector<map_value_type> cameras);
 
   /**
-   * Update the estimated pose using the selected strategy.
+   * Get the AprilTagFieldLayout being used by the PositionEstimator.
    *
-   * @return The updated estimated pose and the latency in milliseconds.
+   * @return the AprilTagFieldLayout
    */
-  std::pair<frc::Pose3d, units::millisecond_t> Update();
-
-  inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
-
-  inline void SetReferencePose(frc::Pose3d referencePose) {
-    this->referencePose = referencePose;
+  std::shared_ptr<frc::AprilTagFieldLayout> getFieldLayout() const {
+    return aprilTags;
   }
 
-  inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
-
+  /**
+   * Set the cameras to be used by the PoseEstimator.
+   *
+   * @param cameras cameras to set.
+   */
   inline void SetCameras(
       const std::vector<std::pair<std::shared_ptr<PhotonCamera>,
                                   frc::Transform3d>>& cameras) {
     this->cameras = cameras;
   }
 
+  /**
+   * Get the Position Estimation Strategy being used by the Position Estimator.
+   *
+   * @return the strategy
+   */
   PoseStrategy GetPoseStrategy() const { return strategy; }
 
-  frc::Pose3d GetLastPose() const { return lastPose; }
+  /**
+   * Set the Position Estimation Strategy used by the Position Estimator.
+   *
+   * @param strategy the strategy to set
+   */
+  inline void SetPoseStrategy(PoseStrategy strat) { strategy = strat; }
 
+  /**
+   * Return the reference position that is being used by the estimator.
+   *
+   * @return the referencePose
+   */
   frc::Pose3d GetReferencePose() const { return referencePose; }
 
+  /**
+   * Update the stored reference pose for use when using the
+   * CLOSEST_TO_REFERENCE_POSE strategy.
+   *
+   * @param referencePose the referencePose to set
+   */
+  inline void SetReferencePose(frc::Pose3d referencePose) {
+    this->referencePose = referencePose;
+  }
+
+  /**
+   * Update the stored last pose. Useful for setting the initial estimate when
+   * using the CLOSEST_TO_LAST_POSE strategy.
+   *
+   * @param lastPose the lastPose to set
+   */
+  inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
+
+  std::pair<frc::Pose3d, units::second_t> Update();
+
  private:
   std::shared_ptr<frc::AprilTagFieldLayout> aprilTags;
   PoseStrategy strategy;
@@ -104,13 +144,44 @@ class RobotPoseEstimator {
   frc::Pose3d lastPose;
   frc::Pose3d referencePose;
 
-  std::pair<frc::Pose3d, units::millisecond_t> LowestAmbiguityStrategy();
+  /**
+   * Return the estimated position of the robot with the lowest position
+   * ambiguity from a List of pipeline results.
+   *
+   * @return the estimated position of the robot in the FCS and the estimated
+   * timestamp of this estimation.
+   */
+  std::pair<frc::Pose3d, units::second_t> LowestAmbiguityStrategy();
 
-  std::pair<frc::Pose3d, units::millisecond_t> ClosestToCameraHeightStrategy();
+  /**
+   * Return the estimated position of the robot using the target with the lowest
+   * delta height difference between the estimated and actual height of the
+   * camera.
+   *
+   * @return the estimated position of the robot in the FCS and the estimated
+   * timestamp of this estimation.
+   */
+  std::pair<frc::Pose3d, units::second_t> ClosestToCameraHeightStrategy();
 
-  std::pair<frc::Pose3d, units::millisecond_t> ClosestToReferencePoseStrategy();
+  /**
+   * Return the estimated position of the robot using the target with the lowest
+   * delta in the vector magnitude between it and the reference pose.
+   *
+   * @param referencePose reference pose to check vector magnitude difference
+   * against.
+   * @return the estimated position of the robot in the FCS and the estimated
+   * timestamp of this estimation.
+   */
+  std::pair<frc::Pose3d, units::second_t> ClosestToReferencePoseStrategy();
 
-  std::pair<frc::Pose3d, units::millisecond_t> AverageBestTargetsStrategy();
+  /**
+   * Return the average of the best target poses using ambiguity as weight.
+
+   * @return the estimated position of the robot in the FCS and the estimated
+   timestamp of this
+   *     estimation.
+   */
+  std::pair<frc::Pose3d, units::second_t> AverageBestTargetsStrategy();
 };
 
 }  // namespace photonlib

photon-lib/src/main/native/include/photonlib/SimVisionSystem.h

@@ -193,6 +193,7 @@ class SimVisionSystem {
                                 camToTargetTransform,
                                 // TODO ambiguity
                                 0.0,
+                                {{0, 0}, {0, 0}, {0, 0}, {0, 0}},
                                 {{0, 0}, {0, 0}, {0, 0}, {0, 0}}});
       }
 

photon-lib/src/test/java/org/photonvision/PhotonPoseEstimatorTest.java

@@ -45,12 +45,12 @@ import java.util.List;
 import java.util.Optional;
 import org.junit.jupiter.api.BeforeAll;
 import org.junit.jupiter.api.Test;
-import org.photonvision.RobotPoseEstimator.PoseStrategy;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 import org.photonvision.targeting.TargetCorner;
 
-class RobotPoseEstimatorTest {
+class PhotonPoseEstimatorTest {
     static AprilTagFieldLayout aprilTags;
 
     @BeforeAll
@@ -62,24 +62,22 @@ class RobotPoseEstimatorTest {
 
         try {
             CombinedRuntimeLoader.loadLibraries(
-                    RobotPoseEstimatorTest.class, "wpiutiljni", "ntcorejni", "wpinetjni", "wpiHaljni");
+                    PhotonPoseEstimatorTest.class, "wpiutiljni", "ntcorejni", "wpinetjni", "wpiHaljni");
         } catch (IOException e) {
             // TODO Auto-generated catch block
             e.printStackTrace();
         }
 
-        List<AprilTag> atList = new ArrayList<AprilTag>(2);
-        atList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d())));
-        atList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d())));
-        var fl = Units.feetToMeters(54.0);
-        var fw = Units.feetToMeters(27.0);
-        aprilTags = new AprilTagFieldLayout(atList, fl, fw);
+        List<AprilTag> tagList = new ArrayList<AprilTag>(2);
+        tagList.add(new AprilTag(0, new Pose3d(3, 3, 3, new Rotation3d())));
+        tagList.add(new AprilTag(1, new Pose3d(5, 5, 5, new Rotation3d())));
+        double fieldLength = Units.feetToMeters(54.0);
+        double fieldWidth = Units.feetToMeters(27.0);
+        aprilTags = new AprilTagFieldLayout(tagList, fieldLength, fieldWidth);
     }
 
     @Test
     void testLowestAmbiguityStrategy() {
-        ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
-
         PhotonCameraInjector cameraOne = new PhotonCameraInjector();
         cameraOne.result =
                 new PhotonPipelineResult(
@@ -122,12 +120,7 @@ class RobotPoseEstimatorTest {
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
-                                                new TargetCorner(7, 8)))));
-        PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
-        cameraTwo.result =
-                new PhotonPipelineResult(
-                        4,
-                        List.of(
+                                                new TargetCorner(7, 8))),
                                 new PhotonTrackedTarget(
                                         9.0,
                                         -2.0,
@@ -147,17 +140,16 @@ class RobotPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
+        cameraOne.result.setTimestampSeconds(11);
 
-        cameras.add(Pair.of(cameraOne, new Transform3d()));
-        cameras.add(Pair.of(cameraTwo, new Transform3d()));
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameraOne, new Transform3d());
 
-        RobotPoseEstimator estimator =
-                new RobotPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameras);
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.get().estimatedPose;
 
-        Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
-        Pose3d pose = estimatedPose.get().getFirst();
-
-        assertEquals(2, estimatedPose.get().getSecond());
+        assertEquals(11, estimatedPose.get().timestampSeconds);
         assertEquals(1, pose.getX(), .01);
         assertEquals(3, pose.getY(), .01);
         assertEquals(2, pose.getZ(), .01);
@@ -209,12 +201,7 @@ class RobotPoseEstimatorTest {
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
-                                                new TargetCorner(7, 8)))));
-        PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
-        cameraTwo.result =
-                new PhotonPipelineResult(
-                        4,
-                        List.of(
+                                                new TargetCorner(7, 8))),
                                 new PhotonTrackedTarget(
                                         9.0,
                                         -2.0,
@@ -235,16 +222,19 @@ class RobotPoseEstimatorTest {
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
 
-        cameras.add(Pair.of(cameraOne, new Transform3d(new Translation3d(0, 0, 4), new Rotation3d())));
-        cameras.add(Pair.of(cameraTwo, new Transform3d(new Translation3d(0, 0, 2), new Rotation3d())));
+        cameraOne.result.setTimestampSeconds(4);
 
-        RobotPoseEstimator estimator =
-                new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT, cameras);
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        aprilTags,
+                        PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT,
+                        cameraOne,
+                        new Transform3d(new Translation3d(0, 0, 4), new Rotation3d()));
 
-        Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
-        Pose3d pose = estimatedPose.get().getFirst();
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.get().estimatedPose;
 
-        assertEquals(2, estimatedPose.get().getSecond());
+        assertEquals(4, estimatedPose.get().timestampSeconds);
         assertEquals(4, pose.getX(), .01);
         assertEquals(4, pose.getY(), .01);
         assertEquals(0, pose.getZ(), .01);
@@ -252,8 +242,6 @@ class RobotPoseEstimatorTest {
 
     @Test
     void closestToReferencePoseStrategy() {
-        ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
-
         PhotonCameraInjector cameraOne = new PhotonCameraInjector();
         cameraOne.result =
                 new PhotonPipelineResult(
@@ -296,12 +284,7 @@ class RobotPoseEstimatorTest {
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
-                                                new TargetCorner(7, 8)))));
-        PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
-        cameraTwo.result =
-                new PhotonPipelineResult(
-                        4,
-                        List.of(
+                                                new TargetCorner(7, 8))),
                                 new PhotonTrackedTarget(
                                         9.0,
                                         -2.0,
@@ -321,18 +304,20 @@ class RobotPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
+        cameraOne.result.setTimestampSeconds(17);
 
-        cameras.add(Pair.of(cameraOne, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
-        cameras.add(Pair.of(cameraTwo, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
-
-        RobotPoseEstimator estimator =
-                new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, cameras);
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        aprilTags,
+                        PoseStrategy.CLOSEST_TO_REFERENCE_POSE,
+                        cameraOne,
+                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
         estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
 
-        Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
-        Pose3d pose = estimatedPose.get().getFirst();
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.get().estimatedPose;
 
-        assertEquals(4, estimatedPose.get().getSecond());
+        assertEquals(17, estimatedPose.get().timestampSeconds);
         assertEquals(1, pose.getX(), .01);
         assertEquals(1.1, pose.getY(), .01);
         assertEquals(.9, pose.getZ(), .01);
@@ -340,8 +325,6 @@ class RobotPoseEstimatorTest {
 
     @Test
     void closestToLastPose() {
-        ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
-
         PhotonCameraInjector cameraOne = new PhotonCameraInjector();
         cameraOne.result =
                 new PhotonPipelineResult(
@@ -384,12 +367,7 @@ class RobotPoseEstimatorTest {
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
-                                                new TargetCorner(7, 8)))));
-        PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
-        cameraTwo.result =
-                new PhotonPipelineResult(
-                        4,
-                        List.of(
+                                                new TargetCorner(7, 8))),
                                 new PhotonTrackedTarget(
                                         9.0,
                                         -2.0,
@@ -410,16 +388,17 @@ class RobotPoseEstimatorTest {
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
 
-        cameras.add(Pair.of(cameraOne, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
-        cameras.add(Pair.of(cameraTwo, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
-
-        RobotPoseEstimator estimator =
-                new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_LAST_POSE, cameras);
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        aprilTags,
+                        PoseStrategy.CLOSEST_TO_LAST_POSE,
+                        cameraOne,
+                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
 
         estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
 
-        Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
-        Pose3d pose = estimatedPose.get().getFirst();
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.get().estimatedPose;
 
         cameraOne.result =
                 new PhotonPipelineResult(
@@ -462,11 +441,7 @@ class RobotPoseEstimatorTest {
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
-                                                new TargetCorner(7, 8)))));
-        cameraTwo.result =
-                new PhotonPipelineResult(
-                        4,
-                        List.of(
+                                                new TargetCorner(7, 8))),
                                 new PhotonTrackedTarget(
                                         9.0,
                                         -2.0,
@@ -486,11 +461,12 @@ class RobotPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8)))));
+        cameraOne.result.setTimestampSeconds(7);
 
         estimatedPose = estimator.update();
-        pose = estimatedPose.get().getFirst();
+        pose = estimatedPose.get().estimatedPose;
 
-        assertEquals(2, estimatedPose.get().getSecond());
+        assertEquals(7, estimatedPose.get().timestampSeconds);
         assertEquals(.9, pose.getX(), .01);
         assertEquals(1.1, pose.getY(), .01);
         assertEquals(1, pose.getZ(), .01);
@@ -542,12 +518,7 @@ class RobotPoseEstimatorTest {
                                                 new TargetCorner(1, 2),
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
-                                                new TargetCorner(7, 8))))); // 2 2 2 ambig .3
-        PhotonCameraInjector cameraTwo = new PhotonCameraInjector();
-        cameraTwo.result =
-                new PhotonPipelineResult(
-                        4,
-                        List.of(
+                                                new TargetCorner(7, 8))), // 2 2 2 ambig .3
                                 new PhotonTrackedTarget(
                                         9.0,
                                         -2.0,
@@ -567,16 +538,19 @@ class RobotPoseEstimatorTest {
                                                 new TargetCorner(3, 4),
                                                 new TargetCorner(5, 6),
                                                 new TargetCorner(7, 8))))); // 3 3 3 ambig .4
+        cameraOne.result.setTimestampSeconds(20);
 
-        cameras.add(Pair.of(cameraOne, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
-        cameras.add(Pair.of(cameraTwo, new Transform3d(new Translation3d(0, 0, 0), new Rotation3d())));
+        PhotonPoseEstimator estimator =
+                new PhotonPoseEstimator(
+                        aprilTags,
+                        PoseStrategy.AVERAGE_BEST_TARGETS,
+                        cameraOne,
+                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()));
 
-        RobotPoseEstimator estimator =
-                new RobotPoseEstimator(aprilTags, PoseStrategy.AVERAGE_BEST_TARGETS, cameras);
+        Optional<EstimatedRobotPose> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.get().estimatedPose;
 
-        Optional<Pair<Pose3d, Double>> estimatedPose = estimator.update();
-        Pose3d pose = estimatedPose.get().getFirst();
-        assertEquals(2.6885245901639347, estimatedPose.get().getSecond(), .01);
+        assertEquals(20, estimatedPose.get().timestampSeconds, .01);
         assertEquals(2.15, pose.getX(), .01);
         assertEquals(2.15, pose.getY(), .01);
         assertEquals(2.15, pose.getZ(), .01);

photon-lib/src/test/native/cpp/PacketTest.cpp

@@ -42,6 +42,7 @@ TEST(PacketTest, PhotonTrackedTarget) {
       frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
                        frc::Rotation3d(1_rad, 2_rad, 3_rad)),
       -1,
+      {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}},
       {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}};
 
   photonlib::Packet p;
@@ -79,6 +80,7 @@ TEST(PacketTest, PhotonPipelineResult) {
           frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
                            frc::Rotation3d(1_rad, 2_rad, 3_rad)),
           -1,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}},
           {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
       photonlib::PhotonTrackedTarget{
           3.0,
@@ -91,6 +93,7 @@ TEST(PacketTest, PhotonPipelineResult) {
           frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
                            frc::Rotation3d(1_rad, 2_rad, 3_rad)),
           -1,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}},
           {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
            std::pair{7, 8}}}};
 

photon-lib/src/test/native/cpp/PhotonPoseEstimatorTest.cpp

@@ -0,0 +1,312 @@
+/*
+ * MIT License
+ *
+ * Copyright (c) 2022 PhotonVision
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+ * SOFTWARE.
+ */
+
+#include <map>
+#include <utility>
+#include <vector>
+
+#include <frc/apriltag/AprilTagFieldLayout.h>
+#include <frc/geometry/Pose3d.h>
+#include <frc/geometry/Rotation3d.h>
+#include <frc/geometry/Transform3d.h>
+#include <units/angle.h>
+#include <units/length.h>
+#include <wpi/SmallVector.h>
+
+#include "gtest/gtest.h"
+#include "photonlib/PhotonCamera.h"
+#include "photonlib/PhotonPipelineResult.h"
+#include "photonlib/PhotonPoseEstimator.h"
+#include "photonlib/PhotonTrackedTarget.h"
+
+static std::vector<frc::AprilTag> tags = {
+    {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
+                    frc::Rotation3d())},
+    {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
+                    frc::Rotation3d())}};
+
+static frc::AprilTagFieldLayout aprilTags{tags, 54_ft, 27_ft};
+
+static wpi::SmallVector<std::pair<double, double>, 4> corners{
+    std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
+static std::vector<std::pair<double, double>> detectedCorners{
+    std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
+
+TEST(PhotonPoseEstimatorTest, LowestAmbiguityStrategy) {
+  photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 0,
+          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
+                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
+                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
+          0.7, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1,
+          frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0,
+          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
+                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
+                           frc::Rotation3d(1_rad, 2_rad, 3_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {2_ms, targets};
+  cameraOne.testResult.SetTimestamp(units::second_t(11));
+
+  photonlib::PhotonPoseEstimator estimator(
+      aprilTags, photonlib::LOWEST_AMBIGUITY, std::move(cameraOne), {});
+  auto estimatedPose = estimator.Update();
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.value().timestamp),
+              .02);
+  EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
+}
+
+TEST(PhotonPoseEstimatorTest, ClosestToCameraHeightStrategy) {
+  std::vector<frc::AprilTag> tags = {
+      {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
+                      frc::Rotation3d())},
+      {1, frc::Pose3d(units::meter_t(5), units::meter_t(5), units::meter_t(5),
+                      frc::Rotation3d())},
+  };
+  auto aprilTags = frc::AprilTagFieldLayout(tags, 54_ft, 27_ft);
+
+  std::vector<std::pair<photonlib::PhotonCamera, frc::Transform3d>> cameras;
+
+  photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
+
+  // ID 0 at 3,3,3
+  // ID 1 at 5,5,5
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 1,
+          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.7, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1,
+          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0,
+          frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {2_ms, targets};
+  cameraOne.testResult.SetTimestamp(17_s);
+
+  photonlib::PhotonPoseEstimator estimator(
+      aprilTags, photonlib::CLOSEST_TO_CAMERA_HEIGHT, std::move(cameraOne),
+      {{0_m, 0_m, 4_m}, {}});
+  auto estimatedPose = estimator.Update();
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
+              .02);
+  EXPECT_NEAR(4, units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(4, units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(0, units::unit_cast<double>(pose.Z()), .01);
+}
+
+TEST(PhotonPoseEstimatorTest, ClosestToReferencePoseStrategy) {
+  photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 1,
+          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.7, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1,
+          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0,
+          frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {2_ms, targets};
+  cameraOne.testResult.SetTimestamp(units::second_t(17));
+
+  photonlib::PhotonPoseEstimator estimator(aprilTags,
+                                           photonlib::CLOSEST_TO_REFERENCE_POSE,
+                                           std::move(cameraOne), {});
+  estimator.SetReferencePose(
+      frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
+  auto estimatedPose = estimator.Update();
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.value().timestamp),
+              .01);
+  EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(.9, units::unit_cast<double>(pose.Z()), .01);
+}
+
+TEST(PhotonPoseEstimatorTest, ClosestToLastPose) {
+  photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 1,
+          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.7, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1,
+          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0,
+          frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {2_ms, targets};
+  cameraOne.testResult.SetTimestamp(units::second_t(17));
+
+  photonlib::PhotonPoseEstimator estimator(
+      aprilTags, photonlib::CLOSEST_TO_LAST_POSE, std::move(cameraOne), {});
+  estimator.SetLastPose(
+      frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
+  auto estimatedPose = estimator.Update();
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targetsThree{
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 1,
+          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.7, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 0,
+          frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0,
+          frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.4, corners, detectedCorners}};
+
+  estimator.GetCamera().testResult = {2_ms, targetsThree};
+  estimator.GetCamera().testResult.SetTimestamp(units::second_t(7));
+
+  estimatedPose = estimator.Update();
+  pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(7.0, units::unit_cast<double>(estimatedPose.value().timestamp),
+              .01);
+  EXPECT_NEAR(.9, units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(1, units::unit_cast<double>(pose.Z()), .01);
+}
+
+TEST(PhotonPoseEstimatorTest, AverageBestPoses) {
+  photonlib::PhotonCamera cameraOne = photonlib::PhotonCamera("test");
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 3> targets{
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.0, 4.0, 0,
+          frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.7, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          3.0, -4.0, 9.1, 6.7, 1,
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.3, corners, detectedCorners},
+      photonlib::PhotonTrackedTarget{
+          9.0, -2.0, 19.0, 3.0, 0,
+          frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
+                           frc::Rotation3d(0_rad, 0_rad, 0_rad)),
+          0.4, corners, detectedCorners}};
+
+  cameraOne.test = true;
+  cameraOne.testResult = {2_ms, targets};
+  cameraOne.testResult.SetTimestamp(units::second_t(15));
+
+  photonlib::PhotonPoseEstimator estimator(
+      aprilTags, photonlib::AVERAGE_BEST_TARGETS, std::move(cameraOne), {});
+  auto estimatedPose = estimator.Update();
+  frc::Pose3d pose = estimatedPose.value().estimatedPose;
+
+  EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.value().timestamp),
+              .01);
+  EXPECT_NEAR(2.15, units::unit_cast<double>(pose.X()), .01);
+  EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Y()), .01);
+  EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);
+}

photon-lib/src/test/native/cpp/RobotPoseEstimatorTest.cpp

@@ -40,6 +40,11 @@
 #include "photonlib/PhotonTrackedTarget.h"
 #include "photonlib/RobotPoseEstimator.h"
 
+static wpi::SmallVector<std::pair<double, double>, 4> corners{
+    std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
+static std::vector<std::pair<double, double>> detectedCorners{
+    std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}};
+
 TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
   std::vector<frc::AprilTag> tags = {
       {0, frc::Pose3d(units::meter_t(3), units::meter_t(3), units::meter_t(3),
@@ -61,51 +66,36 @@ TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
 
   wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
       photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.0,
-          4.0,
-          0,
+          3.0, -4.0, 9.0, 4.0, 0,
           frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
                            frc::Rotation3d(1_rad, 2_rad, 3_rad)),
           frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
                            frc::Rotation3d(1_rad, 2_rad, 3_rad)),
-          0.7,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+          0.7, corners, detectedCorners},
       photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.1,
-          6.7,
-          1,
+          3.0, -4.0, 9.1, 6.7, 1,
           frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           frc::Transform3d(frc::Translation3d(4_m, 2_m, 3_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.3, corners, detectedCorners}};
 
   cameraOne->test = true;
   cameraOne->testResult = {2_s, targets};
+  cameraOne->testResult.SetTimestamp(units::second_t(11));
 
   wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
       photonlib::PhotonTrackedTarget{
-          9.0,
-          -2.0,
-          19.0,
-          3.0,
-          0,
+          9.0, -2.0, 19.0, 3.0, 0,
           frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
                            frc::Rotation3d(1_rad, 2_rad, 3_rad)),
           frc::Transform3d(frc::Translation3d(1_m, 2_m, 3_m),
                            frc::Rotation3d(1_rad, 2_rad, 3_rad)),
-          0.4,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.4, corners, detectedCorners}};
 
   cameraTwo->test = true;
   cameraTwo->testResult = {4_s, targetsTwo};
+  cameraTwo->testResult.SetTimestamp(units::second_t(units::second_t(16)));
 
   cameras.push_back(std::make_pair(
       cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
@@ -119,7 +109,7 @@ TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
       estimator.Update();
   frc::Pose3d pose = estimatedPose.first;
 
-  EXPECT_NEAR(2, units::unit_cast<double>(estimatedPose.second), .01);
+  EXPECT_NEAR(11, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
   EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
   EXPECT_NEAR(3, units::unit_cast<double>(pose.Y()), .01);
   EXPECT_NEAR(2, units::unit_cast<double>(pose.Z()), .01);
@@ -146,51 +136,36 @@ TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) {
 
   wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
       photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.0,
-          4.0,
-          1,
+          3.0, -4.0, 9.0, 4.0, 1,
           frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.7,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+          0.7, corners, detectedCorners},
       photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.1,
-          6.7,
-          1,
+          3.0, -4.0, 9.1, 6.7, 1,
           frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.3, corners, detectedCorners}};
 
   cameraOne->test = true;
   cameraOne->testResult = {2_s, targets};
+  cameraOne->testResult.SetTimestamp(units::second_t(4));
 
   wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
       photonlib::PhotonTrackedTarget{
-          9.0,
-          -2.0,
-          19.0,
-          3.0,
-          0,
+          9.0, -2.0, 19.0, 3.0, 0,
           frc::Transform3d(frc::Translation3d(4_m, 4_m, 4_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           frc::Transform3d(frc::Translation3d(5_m, 5_m, 5_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.4,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.4, corners, detectedCorners}};
 
   cameraTwo->test = true;
   cameraTwo->testResult = {4_s, targetsTwo};
+  cameraOne->testResult.SetTimestamp(units::second_t(12));
 
   cameras.push_back(std::make_pair(
       cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 4_m),
@@ -204,7 +179,7 @@ TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) {
       estimator.Update();
   frc::Pose3d pose = estimatedPose.first;
 
-  EXPECT_NEAR(2, units::unit_cast<double>(estimatedPose.second), .01);
+  EXPECT_NEAR(12, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
   EXPECT_NEAR(4, units::unit_cast<double>(pose.X()), .01);
   EXPECT_NEAR(4, units::unit_cast<double>(pose.Y()), .01);
   EXPECT_NEAR(4, units::unit_cast<double>(pose.Z()), .01);
@@ -231,51 +206,36 @@ TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) {
 
   wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
       photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.0,
-          4.0,
-          1,
+          3.0, -4.0, 9.0, 4.0, 1,
           frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.7,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+          0.7, corners, detectedCorners},
       photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.1,
-          6.7,
-          1,
+          3.0, -4.0, 9.1, 6.7, 1,
           frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.3, corners, detectedCorners}};
 
   cameraOne->test = true;
   cameraOne->testResult = {2_s, targets};
+  cameraOne->testResult.SetTimestamp(units::second_t(4));
 
   wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
       photonlib::PhotonTrackedTarget{
-          9.0,
-          -2.0,
-          19.0,
-          3.0,
-          0,
+          9.0, -2.0, 19.0, 3.0, 0,
           frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.4,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.4, corners, detectedCorners}};
 
   cameraTwo->test = true;
   cameraTwo->testResult = {4_s, targetsTwo};
+  cameraTwo->testResult.SetTimestamp(units::second_t(17));
 
   cameras.push_back(std::make_pair(
       cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
@@ -291,7 +251,7 @@ TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) {
       estimator.Update();
   frc::Pose3d pose = estimatedPose.first;
 
-  EXPECT_NEAR(4, units::unit_cast<double>(estimatedPose.second), .01);
+  EXPECT_NEAR(17, units::unit_cast<double>(estimatedPose.second) / 1000.0, .01);
   EXPECT_NEAR(1, units::unit_cast<double>(pose.X()), .01);
   EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
   EXPECT_NEAR(.9, units::unit_cast<double>(pose.Z()), .01);
@@ -317,48 +277,31 @@ TEST(RobotPoseEstimatorTest, ClosestToLastPose) {
 
   wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
       photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.0,
-          4.0,
-          1,
+          3.0, -4.0, 9.0, 4.0, 1,
           frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.7,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+          0.7, corners, detectedCorners},
       photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.1,
-          6.7,
-          1,
+          3.0, -4.0, 9.1, 6.7, 1,
           frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.3, corners, detectedCorners}};
 
   cameraOne->test = true;
   cameraOne->testResult = {2_s, targets};
 
   wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
       photonlib::PhotonTrackedTarget{
-          9.0,
-          -2.0,
-          19.0,
-          3.0,
-          0,
+          9.0, -2.0, 19.0, 3.0, 0,
           frc::Transform3d(frc::Translation3d(2.2_m, 2.2_m, 2.2_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.4,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.4, corners, detectedCorners}};
 
   cameraTwo->test = true;
   cameraTwo->testResult = {4_s, targetsTwo};
@@ -379,49 +322,34 @@ TEST(RobotPoseEstimatorTest, ClosestToLastPose) {
 
   wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targetsThree{
       photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.0,
-          4.0,
-          1,
+          3.0, -4.0, 9.0, 4.0, 1,
           frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.7,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+          0.7, corners, detectedCorners},
       photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.1,
-          6.7,
-          0,
+          3.0, -4.0, 9.1, 6.7, 0,
           frc::Transform3d(frc::Translation3d(2.1_m, 1.9_m, 2_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.3, corners, detectedCorners}};
 
   cameraOne->testResult = {2_s, targetsThree};
+  cameraOne->testResult.SetTimestamp(units::second_t(7));
 
   wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsFour{
       photonlib::PhotonTrackedTarget{
-          9.0,
-          -2.0,
-          19.0,
-          3.0,
-          0,
+          9.0, -2.0, 19.0, 3.0, 0,
           frc::Transform3d(frc::Translation3d(2.4_m, 2.4_m, 2.2_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           frc::Transform3d(frc::Translation3d(2_m, 1_m, 2_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.4,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.4, corners, detectedCorners}};
 
   cameraTwo->testResult = {4_s, targetsFour};
+  cameraTwo->testResult.SetTimestamp(units::second_t(13));
 
   std::vector<
       std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
@@ -436,7 +364,8 @@ TEST(RobotPoseEstimatorTest, ClosestToLastPose) {
   estimatedPose = estimator.Update();
   pose = estimatedPose.first;
 
-  EXPECT_NEAR(2, units::unit_cast<double>(estimatedPose.second), .01);
+  EXPECT_NEAR(7.0, units::unit_cast<double>(estimatedPose.second) / 1000.0,
+              .01);
   EXPECT_NEAR(.9, units::unit_cast<double>(pose.X()), .01);
   EXPECT_NEAR(1.1, units::unit_cast<double>(pose.Y()), .01);
   EXPECT_NEAR(1, units::unit_cast<double>(pose.Z()), .01);
@@ -462,51 +391,36 @@ TEST(RobotPoseEstimatorTest, AverageBestPoses) {
 
   wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> targets{
       photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.0,
-          4.0,
-          0,
+          3.0, -4.0, 9.0, 4.0, 0,
           frc::Transform3d(frc::Translation3d(2_m, 2_m, 2_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           frc::Transform3d(frc::Translation3d(1_m, 1_m, 1_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.7,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+          0.7, corners, detectedCorners},
       photonlib::PhotonTrackedTarget{
-          3.0,
-          -4.0,
-          9.1,
-          6.7,
-          1,
+          3.0, -4.0, 9.1, 6.7, 1,
           frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           frc::Transform3d(frc::Translation3d(3_m, 3_m, 3_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.3,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.3, corners, detectedCorners}};
 
   cameraOne->test = true;
   cameraOne->testResult = {2_s, targets};
+  cameraOne->testResult.SetTimestamp(units::second_t(10));
 
   wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsTwo{
       photonlib::PhotonTrackedTarget{
-          9.0,
-          -2.0,
-          19.0,
-          3.0,
-          0,
+          9.0, -2.0, 19.0, 3.0, 0,
           frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
           frc::Transform3d(frc::Translation3d(2_m, 1.9_m, 2.1_m),
                            frc::Rotation3d(0_rad, 0_rad, 0_rad)),
-          0.4,
-          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
-           std::pair{7, 8}}}};
+          0.4, corners, detectedCorners}};
 
   cameraTwo->test = true;
   cameraTwo->testResult = {4_s, targetsTwo};
+  cameraTwo->testResult.SetTimestamp(units::second_t(20));
 
   cameras.push_back(std::make_pair(
       cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
@@ -520,8 +434,8 @@ TEST(RobotPoseEstimatorTest, AverageBestPoses) {
       estimator.Update();
   frc::Pose3d pose = estimatedPose.first;
 
-  EXPECT_NEAR(2.6885245901639347,
-              units::unit_cast<double>(estimatedPose.second), .01);
+  EXPECT_NEAR(15.0, units::unit_cast<double>(estimatedPose.second) / 1000.0,
+              .01);
   EXPECT_NEAR(2.15, units::unit_cast<double>(pose.X()), .01);
   EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Y()), .01);
   EXPECT_NEAR(2.15, units::unit_cast<double>(pose.Z()), .01);

photonlib-java-examples/apriltagExample/src/main/java/frc/robot/Drivetrain.java

@@ -24,7 +24,6 @@
 
 package frc.robot;
 
-import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.estimator.DifferentialDrivePoseEstimator;
@@ -49,6 +48,8 @@ import edu.wpi.first.wpilibj.simulation.EncoderSim;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.Constants.DriveTrainConstants;
+import java.util.Optional;
+import org.photonvision.EstimatedRobotPose;
 
 /** Represents a differential drive style drivetrain. */
 public class Drivetrain {
@@ -183,13 +184,14 @@ public class Drivetrain {
         // Also apply vision measurements. We use 0.3 seconds in the past as an example
         // -- on
         // a real robot, this must be calculated based either on latency or timestamps.
-        Pair<Pose2d, Double> result =
+        Optional<EstimatedRobotPose> result =
                 pcw.getEstimatedGlobalPose(m_poseEstimator.getEstimatedPosition());
-        var camPose = result.getFirst();
-        var camPoseObsTime = result.getSecond();
-        if (camPose != null) {
-            m_poseEstimator.addVisionMeasurement(camPose, camPoseObsTime);
-            m_fieldSim.getObject("Cam Est Pos").setPose(camPose);
+
+        if (result.isPresent()) {
+            EstimatedRobotPose camPose = result.get();
+            m_poseEstimator.addVisionMeasurement(
+                    camPose.estimatedPose.toPose2d(), camPose.timestampSeconds);
+            m_fieldSim.getObject("Cam Est Pos").setPose(camPose.estimatedPose.toPose2d());
         } else {
             // move it way off the screen to make it disappear
             m_fieldSim.getObject("Cam Est Pos").setPose(new Pose2d(-100, -100, new Rotation2d()));

photonlib-java-examples/apriltagExample/src/main/java/frc/robot/PhotonCameraWrapper.java

@@ -26,23 +26,21 @@ package frc.robot;
 
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
-import edu.wpi.first.math.Pair;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Transform3d;
-import edu.wpi.first.wpilibj.Timer;
 import frc.robot.Constants.FieldConstants;
 import frc.robot.Constants.VisionConstants;
 import java.util.ArrayList;
 import java.util.Optional;
+import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
-import org.photonvision.RobotPoseEstimator;
-import org.photonvision.RobotPoseEstimator.PoseStrategy;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 
 public class PhotonCameraWrapper {
     public PhotonCamera photonCamera;
-    public RobotPoseEstimator robotPoseEstimator;
+    public PhotonPoseEstimator photonPoseEstimator;
 
     public PhotonCameraWrapper() {
         // Set up a test arena of two apriltags at the center of each driver station set
@@ -73,14 +71,10 @@ public class PhotonCameraWrapper {
                                 .cameraName); // Change the name of your camera here to whatever it is in the
         // PhotonVision UI.
 
-        // ... Add other cameras here
-
-        // Assemble the list of cameras & mount locations
-        var camList = new ArrayList<Pair<PhotonCamera, Transform3d>>();
-        camList.add(new Pair<PhotonCamera, Transform3d>(photonCamera, VisionConstants.robotToCam));
-
-        robotPoseEstimator =
-                new RobotPoseEstimator(atfl, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, camList);
+        // Create pose estimator
+        photonPoseEstimator =
+                new PhotonPoseEstimator(
+                        atfl, PoseStrategy.CLOSEST_TO_REFERENCE_POSE, photonCamera, VisionConstants.robotToCam);
     }
 
     /**
@@ -88,16 +82,8 @@ public class PhotonCameraWrapper {
      * @return A pair of the fused camera observations to a single Pose2d on the field, and the time
      *     of the observation. Assumes a planar field and the robot is always firmly on the ground
      */
-    public Pair<Pose2d, Double> getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) {
-        robotPoseEstimator.setReferencePose(prevEstimatedRobotPose);
-
-        double currentTime = Timer.getFPGATimestamp();
-        Optional<Pair<Pose3d, Double>> result = robotPoseEstimator.update();
-        if (result.isPresent()) {
-            return new Pair<Pose2d, Double>(
-                    result.get().getFirst().toPose2d(), currentTime - result.get().getSecond());
-        } else {
-            return new Pair<Pose2d, Double>(null, 0.0);
-        }
+    public Optional<EstimatedRobotPose> getEstimatedGlobalPose(Pose2d prevEstimatedRobotPose) {
+        photonPoseEstimator.setReferencePose(prevEstimatedRobotPose);
+        return photonPoseEstimator.update();
     }
 }