Add RobotPoseEstimator (#571)

RobotPoseEstimator can pick the most likely pose for the robot given a number of possible poses, using a number of different strategies. Examples are still WIP.

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

@@ -0,0 +1,382 @@
+/*
+ * 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.Pair;
+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.Map;
+import org.photonvision.targeting.PhotonTrackedTarget;
+
+public class RobotPoseEstimator {
+    /**
+     *
+     *
+     * <ul>
+     *   <li><strong>LOWEST_AMBIGUITY</strong>: Choose the Pose with the lowest ambiguity
+     *   <li><strong>CLOSEST_TO_CAMERA_HEIGHT</strong>: Choose the Pose which is closest to the camera
+     *       height
+     *   <li><strong>CLOSEST_TO_REFERENCE_POSE</strong>: Choose the Pose which is closest to the pose
+     *       from setReferencePose()
+     *   <li><strong>CLOSEST_TO_LAST_POSE</strong>: Choose the Pose which is closest to the last pose
+     *       calculated
+     * </ul>
+     */
+    enum PoseStrategy {
+        LOWEST_AMBIGUITY,
+        CLOSEST_TO_CAMERA_HEIGHT,
+        CLOSEST_TO_REFERENCE_POSE,
+        CLOSEST_TO_LAST_POSE,
+        AVERAGE_BEST_TARGETS
+    }
+
+    private Map<Integer, Pose3d> aprilTags;
+    private PoseStrategy strategy;
+    private ArrayList<Pair<PhotonCamera, Transform3d>> cameras;
+    private Pose3d lastPose;
+
+    private Pose3d referencePose;
+    private HashSet<Integer> reportedErrors;
+
+    /**
+     * Create a new RobotPoseEstimator.
+     *
+     * <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 Map linking AprilTag IDs to Pose3ds with respect to the FIRST field.
+     * @param strategy The strategy it should use to determine the best pose.
+     * @param cameras An ArrayList of Pairs of PhotonCameras and their respective Transform3ds from
+     *     the center of the robot to the cameras.
+     */
+    public RobotPoseEstimator(
+            Map<Integer, Pose3d> aprilTags,
+            PoseStrategy strategy,
+            ArrayList<Pair<PhotonCamera, Transform3d>> cameras) {
+        this.aprilTags = aprilTags;
+        this.strategy = strategy;
+        this.cameras = cameras;
+        lastPose = new Pose3d();
+        reportedErrors = new HashSet<>();
+    }
+
+    /**
+     * Update the estimated pose using the selected strategy.
+     *
+     * @return The updated estimated pose and the latency in milliseconds
+     */
+    public Pair<Pose3d, Double> update() {
+        if (cameras.isEmpty()) {
+            DriverStation.reportError("[RobotPoseEstimator] Missing any camera!", false);
+            return Pair.of(lastPose, 0.);
+        }
+        Pair<Pose3d, Double> pair;
+        switch (strategy) {
+            case LOWEST_AMBIGUITY:
+                pair = lowestAmbiguityStrategy();
+                lastPose = pair.getFirst();
+                return pair;
+            case CLOSEST_TO_CAMERA_HEIGHT:
+                pair = closestToCameraHeightStrategy();
+                lastPose = pair.getFirst();
+                return pair;
+            case CLOSEST_TO_REFERENCE_POSE:
+                pair = closestToReferencePoseStrategy();
+                lastPose = pair.getFirst();
+                return pair;
+            case CLOSEST_TO_LAST_POSE:
+                referencePose = lastPose;
+                pair = closestToReferencePoseStrategy();
+                lastPose = pair.getFirst();
+                return pair;
+            case AVERAGE_BEST_TARGETS:
+                pair = averageBestTargetsStrategy();
+                lastPose = pair.getFirst();
+                return pair;
+            default:
+                DriverStation.reportError("[RobotPoseEstimator] Invalid pose strategy!", false);
+                return Pair.of(lastPose, 0.);
+        }
+    }
+
+    private Pair<Pose3d, Double> lowestAmbiguityStrategy() {
+        // Loop over each ambiguity of all the cameras
+        int lowestAI = -1;
+        int lowestAJ = -1;
+        double lowestAmbiguityScore = 10;
+        for (int i = 0; i < cameras.size(); i++) {
+            Pair<PhotonCamera, Transform3d> p = cameras.get(i);
+            List<PhotonTrackedTarget> targets = p.getFirst().getLatestResult().targets;
+            for (int j = 0; j < targets.size(); j++) {
+                if (targets.get(j).getPoseAmbiguity() < lowestAmbiguityScore) {
+                    lowestAI = i;
+                    lowestAJ = j;
+                    lowestAmbiguityScore = targets.get(j).getPoseAmbiguity();
+                }
+            }
+        }
+
+        // No targets, return the last pose
+        if (lowestAI == -1 || lowestAJ == -1) {
+            return Pair.of(lastPose, 0.);
+        }
+
+        // Pick the lowest and do the heavy calculations
+        PhotonTrackedTarget bestTarget =
+                cameras.get(lowestAI).getFirst().getLatestResult().targets.get(lowestAJ);
+
+        // If the map doesn't contain the ID fail
+        if (!aprilTags.containsKey(bestTarget.getFiducialId())) {
+            if (!reportedErrors.contains(bestTarget.getFiducialId())) {
+                DriverStation.reportError(
+                        "[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
+                                + bestTarget.getFiducialId(),
+                        false);
+                reportedErrors.add(bestTarget.getFiducialId());
+            }
+            return Pair.of(lastPose, 0.);
+        }
+
+        return Pair.of(
+                aprilTags
+                        .get(bestTarget.getFiducialId())
+                        .transformBy(bestTarget.getBestCameraToTarget().inverse())
+                        .transformBy(cameras.get(lowestAI).getSecond().inverse()),
+                cameras.get(lowestAI).getFirst().getLatestResult().getLatencyMillis());
+    }
+
+    private Pair<Pose3d, Double> closestToCameraHeightStrategy() {
+        double smallestHeightDifference = 10e9;
+        double mili = 0;
+        Pose3d pose = lastPose;
+
+        for (int i = 0; i < cameras.size(); i++) {
+            Pair<PhotonCamera, Transform3d> p = cameras.get(i);
+            List<PhotonTrackedTarget> targets = p.getFirst().getLatestResult().targets;
+            for (int j = 0; j < targets.size(); j++) {
+                PhotonTrackedTarget target = targets.get(j);
+                // If the map doesn't contain the ID fail
+                if (!aprilTags.containsKey(target.getFiducialId())) {
+                    if (!reportedErrors.contains(target.getFiducialId())) {
+                        DriverStation.reportWarning(
+                                "[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
+                                        + target.getFiducialId(),
+                                false);
+                        reportedErrors.add(target.getFiducialId());
+                    }
+                    continue;
+                }
+                Pose3d targetPose = aprilTags.get(target.getFiducialId());
+                double alternativeDifference =
+                        Math.abs(
+                                p.getSecond().getZ()
+                                        - targetPose.transformBy(target.getAlternateCameraToTarget().inverse()).getZ());
+                double bestDifference =
+                        Math.abs(
+                                p.getSecond().getZ()
+                                        - targetPose.transformBy(target.getBestCameraToTarget().inverse()).getZ());
+                if (alternativeDifference < smallestHeightDifference) {
+                    smallestHeightDifference = alternativeDifference;
+                    pose = targetPose.transformBy(target.getAlternateCameraToTarget().inverse());
+                    mili = p.getFirst().getLatestResult().getLatencyMillis();
+                }
+                if (bestDifference < smallestHeightDifference) {
+                    smallestHeightDifference = bestDifference;
+                    pose = targetPose.transformBy(target.getBestCameraToTarget().inverse());
+                    mili = p.getFirst().getLatestResult().getLatencyMillis();
+                }
+            }
+        }
+        return Pair.of(pose, mili);
+    }
+
+    private Pair<Pose3d, Double> closestToReferencePoseStrategy() {
+        if (referencePose == null) {
+            DriverStation.reportError(
+                    "[RobotPoseEstimator] Tried to use reference pose strategy without setting the reference!",
+                    false);
+            return Pair.of(lastPose, 0.);
+        }
+        double smallestDifference = 10e9;
+        double mili = 0;
+        Pose3d pose = lastPose;
+        for (int i = 0; i < cameras.size(); i++) {
+            Pair<PhotonCamera, Transform3d> p = cameras.get(i);
+            List<PhotonTrackedTarget> targets = p.getFirst().getLatestResult().targets;
+            for (int j = 0; j < targets.size(); j++) {
+                PhotonTrackedTarget target = targets.get(j);
+                // If the map doesn't contain the ID fail
+                if (!aprilTags.containsKey(target.getFiducialId())) {
+                    if (!reportedErrors.contains(target.getFiducialId())) {
+                        DriverStation.reportWarning(
+                                "[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
+                                        + target.getFiducialId(),
+                                false);
+                        reportedErrors.add(target.getFiducialId());
+                    }
+                    continue;
+                }
+                Pose3d targetPose = aprilTags.get(target.getFiducialId());
+                double alternativeDifference =
+                        Math.abs(
+                                calculateDifference(
+                                        referencePose,
+                                        targetPose.transformBy(target.getAlternateCameraToTarget().inverse())));
+                double bestDifference =
+                        Math.abs(
+                                calculateDifference(
+                                        referencePose,
+                                        targetPose.transformBy(target.getBestCameraToTarget().inverse())));
+                if (alternativeDifference < smallestDifference) {
+                    smallestDifference = alternativeDifference;
+                    pose = targetPose.transformBy(target.getAlternateCameraToTarget().inverse());
+                    mili = p.getFirst().getLatestResult().getLatencyMillis();
+                }
+                if (bestDifference < smallestDifference) {
+                    smallestDifference = bestDifference;
+                    pose = targetPose.transformBy(target.getBestCameraToTarget().inverse());
+                    mili = p.getFirst().getLatestResult().getLatencyMillis();
+                }
+            }
+        }
+        return Pair.of(pose, mili);
+    }
+
+    /** Return the average of the best target poses using ambiguity as weight */
+    private Pair<Pose3d, Double> averageBestTargetsStrategy() {
+        //                  Pair of Double, Double = Ambiguity, Mili
+        List<Pair<Pose3d, Pair<Double, Double>>> tempPoses = new ArrayList<>();
+        double totalAmbiguity = 0;
+        for (int i = 0; i < cameras.size(); i++) {
+            Pair<PhotonCamera, Transform3d> p = cameras.get(i);
+            List<PhotonTrackedTarget> targets = p.getFirst().getLatestResult().targets;
+            for (int j = 0; j < targets.size(); j++) {
+                PhotonTrackedTarget target = targets.get(j);
+                // If the map doesn't contain the ID fail
+                if (!aprilTags.containsKey(target.getFiducialId())) {
+                    if (!reportedErrors.contains(target.getFiducialId())) {
+                        DriverStation.reportWarning(
+                                "[RobotPoseEstimator] Tried to get pose of unknown April Tag: "
+                                        + target.getFiducialId(),
+                                false);
+                        reportedErrors.add(target.getFiducialId());
+                    }
+                    continue;
+                }
+                Pose3d targetPose = aprilTags.get(target.getFiducialId());
+                try {
+                    totalAmbiguity += 1. / target.getPoseAmbiguity();
+                } catch (ArithmeticException e) {
+                    // A total ambiguity of zero exists, using that pose instead!",
+                    return Pair.of(
+                            targetPose.transformBy(target.getBestCameraToTarget().inverse()),
+                            p.getFirst().getLatestResult().getLatencyMillis());
+                }
+                tempPoses.add(
+                        Pair.of(
+                                targetPose.transformBy(target.getBestCameraToTarget().inverse()),
+                                Pair.of(
+                                        target.getPoseAmbiguity(), p.getFirst().getLatestResult().getLatencyMillis())));
+            }
+        }
+
+        Translation3d transform = new Translation3d();
+        Rotation3d rotation = new Rotation3d();
+        double latency = 0;
+
+        for (Pair<Pose3d, Pair<Double, Double>> pair : tempPoses) {
+            try {
+                double weight = (1. / pair.getSecond().getFirst()) / totalAmbiguity;
+                transform = transform.plus(pair.getFirst().getTranslation().times(weight));
+                rotation = rotation.plus(pair.getFirst().getRotation().times(weight));
+                latency += pair.getSecond().getSecond() * weight; // NOTE: Average latency may not work well
+            } catch (ArithmeticException e) {
+                DriverStation.reportWarning(
+                        "[RobotPoseEstimator] A total ambiguity of zero exists, using that pose instead!",
+                        false);
+                return Pair.of(pair.getFirst(), pair.getSecond().getSecond());
+            }
+        }
+        return Pair.of(new Pose3d(transform, rotation), latency);
+    }
+
+    /**
+     * 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());
+    }
+
+    /** @param aprilTags the aprilTags to set */
+    public void setAprilTags(Map<Integer, Pose3d> aprilTags) {
+        this.aprilTags = aprilTags;
+    }
+
+    /** @return the aprilTags */
+    public Map<Integer, Pose3d> getAprilTags() {
+        return aprilTags;
+    }
+
+    /** @return the strategy */
+    public PoseStrategy getStrategy() {
+        return strategy;
+    }
+
+    /** @param strategy the strategy to set */
+    public void setStrategy(PoseStrategy strategy) {
+        this.strategy = strategy;
+    }
+
+    /** @return the referencePose */
+    public Pose3d getReferencePose() {
+        return referencePose;
+    }
+
+    /**
+     * Update the stored reference pose for use with CLOSEST_TO_REFERENCE_POSE
+     *
+     * @param referencePose the referencePose to set
+     */
+    public void setReferencePose(Pose3d referencePose) {
+        this.referencePose = referencePose;
+    }
+
+    /**
+     * UPdate the stored last pose. Useful for setting the initial estimate with CLOSEST_TO_LAST_POSE
+     *
+     * @param lastPose the lastPose to set
+     */
+    public void setLastPose(Pose3d lastPose) {
+        this.lastPose = lastPose;
+    }
+}

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

@@ -61,6 +61,7 @@ PhotonCamera::PhotonCamera(const std::string_view cameraName)
                    cameraName) {}
 
 PhotonPipelineResult PhotonCamera::GetLatestResult() {
+  if (test) return testResult;
   // Prints warning if not connected
   VerifyVersion();
 

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

@@ -34,7 +34,8 @@ namespace photonlib {
 
 PhotonTrackedTarget::PhotonTrackedTarget(
     double yaw, double pitch, double area, double skew, int id,
-    const frc::Transform3d& pose,
+    const frc::Transform3d& pose, const frc::Transform3d& alternatePose,
+    double ambiguity,
     const wpi::SmallVector<std::pair<double, double>, 4> corners)
     : yaw(yaw),
       pitch(pitch),
@@ -42,6 +43,8 @@ PhotonTrackedTarget::PhotonTrackedTarget(
       skew(skew),
       fiducialId(id),
       bestCameraToTarget(pose),
+      altCameraToTarget(alternatePose),
+      poseAmbiguity(ambiguity),
       corners(corners) {}
 
 bool PhotonTrackedTarget::operator==(const PhotonTrackedTarget& other) const {

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

@@ -0,0 +1,265 @@
+/*
+ * 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/RobotPoseEstimator.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 {
+RobotPoseEstimator::RobotPoseEstimator(
+    std::map<int, frc::Pose3d> tags, PoseStrategy strat,
+    std::vector<std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>>
+        cams)
+    : aprilTags(tags),
+      strategy(strat),
+      cameras(std::move(cams)),
+      lastPose(frc::Pose3d()),
+      referencePose(frc::Pose3d()) {}
+
+std::pair<frc::Pose3d, units::millisecond_t> RobotPoseEstimator::Update() {
+  if (cameras.empty()) {
+    return std::make_pair(lastPose, units::millisecond_t(0));
+  }
+  std::pair<frc::Pose3d, units::millisecond_t> pair;
+  switch (strategy) {
+    case LOWEST_AMBIGUITY:
+      pair = LowestAmbiguityStrategy();
+      lastPose = pair.first;
+      return pair;
+    case CLOSEST_TO_CAMERA_HEIGHT:
+      pair = ClosestToCameraHeightStrategy();
+      lastPose = pair.first;
+      return pair;
+    case CLOSEST_TO_REFERENCE_POSE:
+      pair = ClosestToReferencePoseStrategy();
+      lastPose = pair.first;
+      return pair;
+    case CLOSEST_TO_LAST_POSE:
+      referencePose = lastPose;
+      pair = ClosestToReferencePoseStrategy();
+      lastPose = pair.first;
+      return pair;
+    case AVERAGE_BEST_TARGETS:
+      pair = AverageBestTargetsStrategy();
+      lastPose = pair.first;
+      return pair;
+    default:
+      FRC_ReportError(frc::warn::Warning, "Invalid Pose Strategy selected!",
+                      "");
+  }
+  return std::make_pair(lastPose, units::millisecond_t(0));
+}
+
+std::pair<frc::Pose3d, units::millisecond_t>
+RobotPoseEstimator::LowestAmbiguityStrategy() {
+  int lowestAI = -1;
+  int lowestAJ = -1;
+  double lowestAmbiguityScore = std::numeric_limits<double>::infinity();
+  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();
+    for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
+      if (targets[j].GetPoseAmbiguity() < lowestAmbiguityScore) {
+        lowestAI = i;
+        lowestAJ = j;
+        lowestAmbiguityScore = targets[j].GetPoseAmbiguity();
+      }
+    }
+  }
+
+  if (lowestAI == -1 || lowestAJ == -1) {
+    return std::make_pair(lastPose, units::millisecond_t(0));
+  }
+
+  PhotonTrackedTarget bestTarget =
+      cameras[lowestAI].first->GetLatestResult().GetTargets()[lowestAJ];
+
+  if (aprilTags.count(bestTarget.GetFiducialId()) == 0) {
+    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(
+      aprilTags[bestTarget.GetFiducialId()]
+          .TransformBy(bestTarget.GetBestCameraToTarget().Inverse())
+          .TransformBy(cameras[lowestAI].second.Inverse()),
+      cameras[lowestAI].first->GetLatestResult().GetLatency() / 1000.);
+}
+std::pair<frc::Pose3d, units::millisecond_t>
+RobotPoseEstimator::ClosestToCameraHeightStrategy() {
+  units::meter_t smallestHeightDifference =
+      units::meter_t(std::numeric_limits<double>::infinity());
+  units::millisecond_t milli = units::millisecond_t(0);
+  frc::Pose3d pose = lastPose;
+
+  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();
+    for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
+      PhotonTrackedTarget target = targets[j];
+      if (aprilTags.count(target.GetFiducialId()) == 0) {
+        FRC_ReportError(frc::warn::Warning,
+                        "Tried to get pose of unknown April Tag: {}",
+                        target.GetFiducialId());
+        continue;
+      }
+      frc::Pose3d targetPose = aprilTags[target.GetFiducialId()];
+      units::meter_t alternativeDifference = units::math::abs(
+          p.second.Z() -
+          targetPose.TransformBy(target.GetAlternateCameraToTarget().Inverse())
+              .Z());
+      units::meter_t bestDifference = units::math::abs(
+          p.second.Z() -
+          targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()).Z());
+      if (alternativeDifference < smallestHeightDifference) {
+        smallestHeightDifference = alternativeDifference;
+        pose = targetPose.TransformBy(
+            target.GetAlternateCameraToTarget().Inverse());
+        milli = p.first->GetLatestResult().GetLatency() / 1000.;
+      }
+      if (bestDifference < smallestHeightDifference) {
+        smallestHeightDifference = bestDifference;
+        pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
+        milli = p.first->GetLatestResult().GetLatency() / 1000.;
+      }
+    }
+  }
+  return std::make_pair(pose, milli);
+}
+std::pair<frc::Pose3d, units::millisecond_t>
+RobotPoseEstimator::ClosestToReferencePoseStrategy() {
+  units::meter_t smallestDifference =
+      units::meter_t(std::numeric_limits<double>::infinity());
+  units::millisecond_t milli = units::millisecond_t(0);
+  frc::Pose3d pose = lastPose;
+
+  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();
+    for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
+      PhotonTrackedTarget target = targets[j];
+      if (aprilTags.count(target.GetFiducialId()) == 0) {
+        FRC_ReportError(frc::warn::Warning,
+                        "Tried to get pose of unknown April Tag: {}",
+                        target.GetFiducialId());
+        continue;
+      }
+      frc::Pose3d targetPose = aprilTags[target.GetFiducialId()];
+      units::meter_t alternativeDifference =
+          units::math::abs(referencePose.Translation().Distance(
+              targetPose
+                  .TransformBy(target.GetAlternateCameraToTarget().Inverse())
+                  .Translation()));
+      units::meter_t bestDifference =
+          units::math::abs(referencePose.Translation().Distance(
+              targetPose.TransformBy(target.GetBestCameraToTarget().Inverse())
+                  .Translation()));
+      if (alternativeDifference < smallestDifference) {
+        smallestDifference = alternativeDifference;
+        pose = targetPose.TransformBy(
+            target.GetAlternateCameraToTarget().Inverse());
+        milli = p.first->GetLatestResult().GetLatency() / 1000.;
+      }
+
+      if (bestDifference < smallestDifference) {
+        smallestDifference = bestDifference;
+        pose = targetPose.TransformBy(target.GetBestCameraToTarget().Inverse());
+        milli = p.first->GetLatestResult().GetLatency() / 1000.;
+      }
+    }
+  }
+  return std::make_pair(pose, milli);
+}
+
+std::pair<frc::Pose3d, units::millisecond_t>
+RobotPoseEstimator::AverageBestTargetsStrategy() {
+  std::vector<std::pair<frc::Pose3d, std::pair<double, units::millisecond_t>>>
+      tempPoses;
+  double totalAmbiguity = 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();
+    for (RobotPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
+      PhotonTrackedTarget target = targets[j];
+      if (aprilTags.count(target.GetFiducialId()) == 0) {
+        FRC_ReportError(frc::warn::Warning,
+                        "Tried to get pose of unknown April Tag: {}",
+                        target.GetFiducialId());
+        continue;
+      }
+
+      frc::Pose3d targetPose = aprilTags[target.GetFiducialId()];
+      if (target.GetPoseAmbiguity() == 0) {
+        FRC_ReportError(frc::warn::Warning,
+                        "Pose ambiguity of zero exists, using that instead!",
+                        "");
+        return std::make_pair(
+            targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
+            p.first->GetLatestResult().GetLatency() / 1000.);
+      }
+      totalAmbiguity += 1. / target.GetPoseAmbiguity();
+
+      tempPoses.push_back(std::make_pair(
+          targetPose.TransformBy(target.GetBestCameraToTarget().Inverse()),
+          std::make_pair(target.GetPoseAmbiguity(),
+                         p.first->GetLatestResult().GetLatency() / 1000.)));
+    }
+  }
+  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 :
+       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);
+}
+}  // namespace photonlib

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

@@ -67,11 +67,13 @@ class PhotonCamera {
    */
   explicit PhotonCamera(const std::string_view cameraName);
 
+  virtual ~PhotonCamera() = default;
+
   /**
    * Returns the latest pipeline result.
    * @return The latest pipeline result.
    */
-  PhotonPipelineResult GetLatestResult();
+  virtual PhotonPipelineResult GetLatestResult();
 
   /**
    * Toggles driver mode.
@@ -155,6 +157,10 @@ class PhotonCamera {
     PhotonCamera::VERSION_CHECK_ENABLED = enabled;
   }
 
+  // For use in tests
+  bool test = false;
+  PhotonPipelineResult testResult;
+
  protected:
   std::shared_ptr<nt::NetworkTable> mainTable;
   std::shared_ptr<nt::NetworkTable> rootTable;

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

@@ -52,11 +52,13 @@ class PhotonTrackedTarget {
    * @param area The area of the target.
    * @param skew The skew of the target.
    * @param pose The camera-relative pose of the target.
+   * @param alternatePose The alternate camera-relative pose of the target.
    * @Param corners The corners of the bounding rectangle.
    */
   PhotonTrackedTarget(
       double yaw, double pitch, double area, double skew, int fiducialID,
-      const frc::Transform3d& pose,
+      const frc::Transform3d& pose, const frc::Transform3d& alternatePose,
+      double ambiguity,
       const wpi::SmallVector<std::pair<double, double>, 4> corners);
 
   /**
@@ -87,7 +89,7 @@ class PhotonTrackedTarget {
    * Get the Fiducial ID of the target currently being tracked,
    * or -1 if not set.
    */
-  double GetFiducialId() const { return fiducialId; }
+  int GetFiducialId() const { return fiducialId; }
 
   /**
    * Returns the corners of the minimum area rectangle bounding this target.

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

@@ -0,0 +1,97 @@
+/*
+ * 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/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
+};
+
+/**
+ * A managing class to determine how an estimated pose should be chosen.
+ */
+class RobotPoseEstimator {
+  using map_value_type =
+      std::pair<std::shared_ptr<PhotonCamera>, frc::Transform3d>;
+  using size_type = std::vector<map_value_type>::size_type;
+
+ public:
+  explicit RobotPoseEstimator(std::map<int, frc::Pose3d> aprilTags,
+                              PoseStrategy strategy,
+                              std::vector<map_value_type>);
+
+  std::pair<frc::Pose3d, units::millisecond_t> Update();
+
+  void SetPoseStrategy(PoseStrategy strategy);
+
+  inline void SetReferencePose(frc::Pose3d referencePose) {
+    this->referencePose = referencePose;
+  }
+
+  inline void SetLastPose(frc::Pose3d lastPose) { this->lastPose = lastPose; }
+
+  inline void SetCameras(
+      const std::vector<std::pair<std::shared_ptr<PhotonCamera>,
+                                  frc::Transform3d>>& cameras) {
+    this->cameras = cameras;
+  }
+
+  PoseStrategy GetPoseStrategy() const { return strategy; }
+
+  frc::Pose3d GetLastPose() const { return lastPose; }
+
+  frc::Pose3d GetReferencePose() const { return referencePose; }
+
+ private:
+  std::map<int, frc::Pose3d> aprilTags;
+  PoseStrategy strategy;
+  std::vector<map_value_type> cameras;
+  frc::Pose3d lastPose;
+  frc::Pose3d referencePose;
+
+  std::pair<frc::Pose3d, units::millisecond_t> LowestAmbiguityStrategy();
+
+  std::pair<frc::Pose3d, units::millisecond_t> ClosestToCameraHeightStrategy();
+
+  std::pair<frc::Pose3d, units::millisecond_t> ClosestToReferencePoseStrategy();
+
+  std::pair<frc::Pose3d, units::millisecond_t> AverageBestTargetsStrategy();
+};
+
+}  // namespace photonlib

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

@@ -57,6 +57,8 @@ class SimPhotonCamera : public PhotonCamera {
                             nt::NetworkTableInstance::GetDefault()),
                         cameraName) {}
 
+  virtual ~SimPhotonCamera() = default;
+
   /**
    * Simulate one processed frame of vision data, putting one result to NT.
    *

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

@@ -181,6 +181,10 @@ class SimVisionSystem {
                                 0.0,
                                 target.targetId,
                                 camToTargetTransform,
+                                // TODO sim alternate pose
+                                camToTargetTransform,
+                                // TODO ambiguity
+                                0.0,
                                 {{0, 0}, {0, 0}, {0, 0}, {0, 0}}});
       }
 

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

@@ -0,0 +1,516 @@
+/*
+ * 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 static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.hal.JNIWrapper;
+import edu.wpi.first.math.Pair;
+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.net.WPINetJNI;
+import edu.wpi.first.networktables.NetworkTablesJNI;
+import edu.wpi.first.util.CombinedRuntimeLoader;
+import edu.wpi.first.util.WPIUtilJNI;
+import java.io.IOException;
+import java.util.ArrayList;
+import java.util.HashMap;
+import java.util.List;
+import java.util.Map;
+import org.junit.jupiter.api.BeforeAll;
+import org.junit.jupiter.api.Test;
+import org.photonvision.RobotPoseEstimator.PoseStrategy;
+import org.photonvision.targeting.PhotonPipelineResult;
+import org.photonvision.targeting.PhotonTrackedTarget;
+import org.photonvision.targeting.TargetCorner;
+
+class RobotPoseEstimatorTest {
+    @BeforeAll
+    public static void init() {
+        JNIWrapper.Helper.setExtractOnStaticLoad(false);
+        WPIUtilJNI.Helper.setExtractOnStaticLoad(false);
+        NetworkTablesJNI.Helper.setExtractOnStaticLoad(false);
+        WPINetJNI.Helper.setExtractOnStaticLoad(false);
+
+        try {
+            CombinedRuntimeLoader.loadLibraries(
+                    RobotPoseEstimatorTest.class, "wpiutiljni", "ntcorejni", "wpinetjni", "wpiHaljni");
+        } catch (IOException e) {
+            // TODO Auto-generated catch block
+            e.printStackTrace();
+        }
+    }
+
+    @Test
+    void testLowestAmbiguityStrategy() {
+        Map<Integer, Pose3d> aprilTags = new HashMap<>();
+        aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d()));
+        aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d()));
+
+        ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
+
+        PhotonCameraInjector cameraOne = new PhotonCameraInjector();
+        cameraOne.result =
+                new PhotonPipelineResult(
+                        2,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        4.0,
+                                        0,
+                                        new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
+                                        new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
+                                        0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.1,
+                                        6.7,
+                                        1,
+                                        new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(0, 0, 0)),
+                                        new Transform3d(new Translation3d(4, 2, 3), new Rotation3d(1, 5, 3)),
+                                        0.3,
+                                        List.of(
+                                                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 PhotonTrackedTarget(
+                                        9.0,
+                                        -2.0,
+                                        19.0,
+                                        3.0,
+                                        0,
+                                        new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
+                                        new Transform3d(new Translation3d(1, 2, 3), new Rotation3d(1, 2, 3)),
+                                        0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)))));
+
+        cameras.add(Pair.of(cameraOne, new Transform3d()));
+        cameras.add(Pair.of(cameraTwo, new Transform3d()));
+
+        RobotPoseEstimator estimator =
+                new RobotPoseEstimator(aprilTags, PoseStrategy.LOWEST_AMBIGUITY, cameras);
+
+        Pair<Pose3d, Double> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.getFirst();
+
+        assertEquals(2, estimatedPose.getSecond());
+        assertEquals(1, pose.getX(), .01);
+        assertEquals(3, pose.getY(), .01);
+        assertEquals(2, pose.getZ(), .01);
+    }
+
+    @Test
+    void testClosestToCameraHeightStrategy() {
+        Map<Integer, Pose3d> aprilTags = new HashMap<>();
+        aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d()));
+        aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d()));
+
+        ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
+
+        PhotonCameraInjector cameraOne = new PhotonCameraInjector();
+        cameraOne.result =
+                new PhotonPipelineResult(
+                        2,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        4.0,
+                                        1,
+                                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
+                                        new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
+                                        0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.1,
+                                        6.7,
+                                        1,
+                                        new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
+                                        0.3,
+                                        List.of(
+                                                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 PhotonTrackedTarget(
+                                        9.0,
+                                        -2.0,
+                                        19.0,
+                                        3.0,
+                                        0,
+                                        new Transform3d(new Translation3d(4, 4, 4), new Rotation3d()),
+                                        new Transform3d(new Translation3d(5, 5, 5), new Rotation3d()),
+                                        0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                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())));
+
+        RobotPoseEstimator estimator =
+                new RobotPoseEstimator(aprilTags, PoseStrategy.CLOSEST_TO_CAMERA_HEIGHT, cameras);
+
+        Pair<Pose3d, Double> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.getFirst();
+
+        assertEquals(2, estimatedPose.getSecond());
+        assertEquals(4, pose.getX(), .01);
+        assertEquals(4, pose.getY(), .01);
+        assertEquals(4, pose.getZ(), .01);
+    }
+
+    @Test
+    void closestToReferencePoseStrategy() {
+        Map<Integer, Pose3d> aprilTags = new HashMap<>();
+        aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d()));
+        aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d()));
+
+        ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
+
+        PhotonCameraInjector cameraOne = new PhotonCameraInjector();
+        cameraOne.result =
+                new PhotonPipelineResult(
+                        2,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        4.0,
+                                        1,
+                                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
+                                        new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
+                                        0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.1,
+                                        6.7,
+                                        1,
+                                        new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
+                                        0.3,
+                                        List.of(
+                                                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 PhotonTrackedTarget(
+                                        9.0,
+                                        -2.0,
+                                        19.0,
+                                        3.0,
+                                        0,
+                                        new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
+                                        0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                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_REFERENCE_POSE, cameras);
+        estimator.setReferencePose(new Pose3d(1, 1, 1, new Rotation3d()));
+
+        Pair<Pose3d, Double> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.getFirst();
+
+        assertEquals(4, estimatedPose.getSecond());
+        assertEquals(1, pose.getX(), .01);
+        assertEquals(1.1, pose.getY(), .01);
+        assertEquals(.9, pose.getZ(), .01);
+    }
+
+    @Test
+    void closestToLastPose() {
+        Map<Integer, Pose3d> aprilTags = new HashMap<>();
+        aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d()));
+        aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d()));
+
+        ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
+
+        PhotonCameraInjector cameraOne = new PhotonCameraInjector();
+        cameraOne.result =
+                new PhotonPipelineResult(
+                        2,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        4.0,
+                                        1,
+                                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
+                                        new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
+                                        0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.1,
+                                        6.7,
+                                        1,
+                                        new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
+                                        0.3,
+                                        List.of(
+                                                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 PhotonTrackedTarget(
+                                        9.0,
+                                        -2.0,
+                                        19.0,
+                                        3.0,
+                                        0,
+                                        new Transform3d(new Translation3d(2.2, 2.2, 2.2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
+                                        0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                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);
+
+        estimator.setLastPose(new Pose3d(1, 1, 1, new Rotation3d()));
+
+        Pair<Pose3d, Double> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.getFirst();
+
+        cameraOne.result =
+                new PhotonPipelineResult(
+                        2,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        4.0,
+                                        1,
+                                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
+                                        new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
+                                        0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))),
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.1,
+                                        6.7,
+                                        0,
+                                        new Transform3d(new Translation3d(2.1, 1.9, 2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
+                                        0.3,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)))));
+        cameraTwo.result =
+                new PhotonPipelineResult(
+                        4,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        9.0,
+                                        -2.0,
+                                        19.0,
+                                        3.0,
+                                        0,
+                                        new Transform3d(new Translation3d(2.4, 2.4, 2.2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(2, 1, 2), new Rotation3d()),
+                                        0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8)))));
+
+        estimatedPose = estimator.update();
+        pose = estimatedPose.getFirst();
+
+        assertEquals(2, estimatedPose.getSecond());
+        assertEquals(.9, pose.getX(), .01);
+        assertEquals(1.1, pose.getY(), .01);
+        assertEquals(1, pose.getZ(), .01);
+    }
+
+    @Test
+    void averageBestPoses() {
+        Map<Integer, Pose3d> aprilTags = new HashMap<>();
+        aprilTags.put(0, new Pose3d(3, 3, 3, new Rotation3d()));
+        aprilTags.put(1, new Pose3d(5, 5, 5, new Rotation3d()));
+
+        ArrayList<Pair<PhotonCamera, Transform3d>> cameras = new ArrayList<>();
+
+        PhotonCameraInjector cameraOne = new PhotonCameraInjector();
+        cameraOne.result =
+                new PhotonPipelineResult(
+                        2,
+                        List.of(
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.0,
+                                        4.0,
+                                        0,
+                                        new Transform3d(new Translation3d(2, 2, 2), new Rotation3d()),
+                                        new Transform3d(new Translation3d(1, 1, 1), new Rotation3d()),
+                                        0.7,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))), // 1 1 1 ambig: .7
+                                new PhotonTrackedTarget(
+                                        3.0,
+                                        -4.0,
+                                        9.1,
+                                        6.7,
+                                        1,
+                                        new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
+                                        new Transform3d(new Translation3d(3, 3, 3), new Rotation3d()),
+                                        0.3,
+                                        List.of(
+                                                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 PhotonTrackedTarget(
+                                        9.0,
+                                        -2.0,
+                                        19.0,
+                                        3.0,
+                                        0,
+                                        new Transform3d(new Translation3d(0, 0, 0), new Rotation3d()),
+                                        new Transform3d(new Translation3d(2, 1.9, 2.1), new Rotation3d()),
+                                        0.4,
+                                        List.of(
+                                                new TargetCorner(1, 2),
+                                                new TargetCorner(3, 4),
+                                                new TargetCorner(5, 6),
+                                                new TargetCorner(7, 8))))); // 3 3 3 ambig .4
+
+        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.AVERAGE_BEST_TARGETS, cameras);
+
+        Pair<Pose3d, Double> estimatedPose = estimator.update();
+        Pose3d pose = estimatedPose.getFirst();
+        assertEquals(2.6885245901639347, estimatedPose.getSecond(), .01);
+        assertEquals(2.15, pose.getX(), .01);
+        assertEquals(2.15, pose.getY(), .01);
+        assertEquals(2.15, pose.getZ(), .01);
+    }
+
+    private class PhotonCameraInjector extends PhotonCamera {
+        public PhotonCameraInjector() {
+            super("Test");
+        }
+
+        PhotonPipelineResult result;
+
+        @Override
+        public PhotonPipelineResult getLatestResult() {
+            return result;
+        }
+    }
+}

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

@@ -39,6 +39,9 @@ TEST(PacketTest, PhotonTrackedTarget) {
       -1,
       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)),
+      -1,
       {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}};
 
   photonlib::Packet p;
@@ -73,6 +76,9 @@ TEST(PacketTest, PhotonPipelineResult) {
           1,
           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)),
+          -1,
           {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
       photonlib::PhotonTrackedTarget{
           3.0,
@@ -82,6 +88,9 @@ TEST(PacketTest, PhotonPipelineResult) {
           -1,
           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)),
+          -1,
           {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
            std::pair{7, 8}}}};
 

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

@@ -0,0 +1,514 @@
+/*
+ * 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/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/PhotonTrackedTarget.h"
+#include "photonlib/RobotPoseEstimator.h"
+
+TEST(RobotPoseEstimatorTest, LowestAmbiguityStrategy) {
+  std::map<int, frc::Pose3d> aprilTags;
+  aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
+                                   units::meter_t(3), frc::Rotation3d())});
+  aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
+                                   units::meter_t(5), frc::Rotation3d())});
+
+  std::vector<
+      std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
+      cameras;
+
+  std::shared_ptr<photonlib::PhotonCamera> cameraOne =
+      std::make_shared<photonlib::PhotonCamera>("test");
+  std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
+      std::make_shared<photonlib::PhotonCamera>("test");
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> 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,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+      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,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
+           std::pair{7, 8}}}};
+
+  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,
+          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}}}};
+
+  cameraTwo->test = true;
+  cameraTwo->testResult = {4_s, targetsTwo};
+
+  cameras.push_back(std::make_pair(
+      cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
+  cameras.push_back(std::make_pair(
+      cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
+  photonlib::RobotPoseEstimator estimator(aprilTags,
+                                          photonlib::LOWEST_AMBIGUITY, cameras);
+  std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
+      estimator.Update();
+  frc::Pose3d pose = estimatedPose.first;
+
+  EXPECT_NEAR(2, units::unit_cast<double>(estimatedPose.second), .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);
+}
+
+TEST(RobotPoseEstimatorTest, ClosestToCameraHeightStrategy) {
+  std::map<int, frc::Pose3d> aprilTags;
+  aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
+                                   units::meter_t(3), frc::Rotation3d())});
+  aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
+                                   units::meter_t(5), frc::Rotation3d())});
+
+  std::vector<
+      std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
+      cameras;
+
+  std::shared_ptr<photonlib::PhotonCamera> cameraOne =
+      std::make_shared<photonlib::PhotonCamera>("test");
+  std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
+      std::make_shared<photonlib::PhotonCamera>("test");
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> 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,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+      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,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
+           std::pair{7, 8}}}};
+
+  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,
+          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}}}};
+
+  cameraTwo->test = true;
+  cameraTwo->testResult = {4_s, targetsTwo};
+
+  cameras.push_back(std::make_pair(
+      cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 4_m),
+                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
+  cameras.push_back(std::make_pair(
+      cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 2_m),
+                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
+  photonlib::RobotPoseEstimator estimator(
+      aprilTags, photonlib::CLOSEST_TO_CAMERA_HEIGHT, cameras);
+  std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
+      estimator.Update();
+  frc::Pose3d pose = estimatedPose.first;
+
+  EXPECT_NEAR(2, units::unit_cast<double>(estimatedPose.second), .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);
+}
+
+TEST(RobotPoseEstimatorTest, ClosestToReferencePoseStrategy) {
+  std::map<int, frc::Pose3d> aprilTags;
+  aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
+                                   units::meter_t(3), frc::Rotation3d())});
+  aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
+                                   units::meter_t(5), frc::Rotation3d())});
+
+  std::vector<
+      std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
+      cameras;
+
+  std::shared_ptr<photonlib::PhotonCamera> cameraOne =
+      std::make_shared<photonlib::PhotonCamera>("test");
+  std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
+      std::make_shared<photonlib::PhotonCamera>("test");
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> 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,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+      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,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
+           std::pair{7, 8}}}};
+
+  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,
+          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}}}};
+
+  cameraTwo->test = true;
+  cameraTwo->testResult = {4_s, targetsTwo};
+
+  cameras.push_back(std::make_pair(
+      cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
+  cameras.push_back(std::make_pair(
+      cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
+  photonlib::RobotPoseEstimator estimator(
+      aprilTags, photonlib::CLOSEST_TO_REFERENCE_POSE, cameras);
+  estimator.SetReferencePose(
+      frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
+  std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
+      estimator.Update();
+  frc::Pose3d pose = estimatedPose.first;
+
+  EXPECT_NEAR(4, units::unit_cast<double>(estimatedPose.second), .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(RobotPoseEstimatorTest, ClosestToLastPose) {
+  std::map<int, frc::Pose3d> aprilTags;
+  aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
+                                   units::meter_t(3), frc::Rotation3d())});
+  aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
+                                   units::meter_t(5), frc::Rotation3d())});
+
+  std::vector<
+      std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
+      cameras;
+
+  std::shared_ptr<photonlib::PhotonCamera> cameraOne =
+      std::make_shared<photonlib::PhotonCamera>("test");
+  std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
+      std::make_shared<photonlib::PhotonCamera>("test");
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> 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,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+      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,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
+           std::pair{7, 8}}}};
+
+  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,
+          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}}}};
+
+  cameraTwo->test = true;
+  cameraTwo->testResult = {4_s, targetsTwo};
+
+  cameras.push_back(std::make_pair(
+      cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
+  cameras.push_back(std::make_pair(
+      cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
+  photonlib::RobotPoseEstimator estimator(
+      aprilTags, photonlib::CLOSEST_TO_LAST_POSE, cameras);
+  estimator.SetLastPose(
+      frc::Pose3d(1_m, 1_m, 1_m, frc::Rotation3d(0_rad, 0_rad, 0_rad)));
+  std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
+      estimator.Update();
+  frc::Pose3d pose = estimatedPose.first;
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> 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,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+      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,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
+           std::pair{7, 8}}}};
+
+  cameraOne->testResult = {2_s, targetsThree};
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 1> targetsFour{
+      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,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
+           std::pair{7, 8}}}};
+
+  cameraTwo->testResult = {4_s, targetsFour};
+
+  std::vector<
+      std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
+      camerasUpdated;
+  camerasUpdated.push_back(std::make_pair(
+      cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
+  camerasUpdated.push_back(std::make_pair(
+      cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
+  estimator.SetCameras(camerasUpdated);
+  estimatedPose = estimator.Update();
+  pose = estimatedPose.first;
+
+  EXPECT_NEAR(2, units::unit_cast<double>(estimatedPose.second), .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(RobotPoseEstimatorTest, AverageBestPoses) {
+  std::map<int, frc::Pose3d> aprilTags;
+  aprilTags.insert({0, frc::Pose3d(units::meter_t(3), units::meter_t(3),
+                                   units::meter_t(3), frc::Rotation3d())});
+  aprilTags.insert({1, frc::Pose3d(units::meter_t(5), units::meter_t(5),
+                                   units::meter_t(5), frc::Rotation3d())});
+
+  std::vector<
+      std::pair<std::shared_ptr<photonlib::PhotonCamera>, frc::Transform3d>>
+      cameras;
+
+  std::shared_ptr<photonlib::PhotonCamera> cameraOne =
+      std::make_shared<photonlib::PhotonCamera>("test");
+  std::shared_ptr<photonlib::PhotonCamera> cameraTwo =
+      std::make_shared<photonlib::PhotonCamera>("test");
+
+  wpi::SmallVector<photonlib::PhotonTrackedTarget, 2> 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,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6}, std::pair{7, 8}}},
+      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,
+          {std::pair{1, 2}, std::pair{3, 4}, std::pair{5, 6},
+           std::pair{7, 8}}}};
+
+  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,
+          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}}}};
+
+  cameraTwo->test = true;
+  cameraTwo->testResult = {4_s, targetsTwo};
+
+  cameras.push_back(std::make_pair(
+      cameraOne, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
+  cameras.push_back(std::make_pair(
+      cameraTwo, frc::Transform3d(frc::Translation3d(0_m, 0_m, 0_m),
+                                  frc::Rotation3d(0_rad, 0_rad, 0_rad))));
+  photonlib::RobotPoseEstimator estimator(
+      aprilTags, photonlib::AVERAGE_BEST_TARGETS, cameras);
+  std::pair<frc::Pose3d, units::millisecond_t> estimatedPose =
+      estimator.Update();
+  frc::Pose3d pose = estimatedPose.first;
+
+  EXPECT_NEAR(2.6885245901639347,
+              units::unit_cast<double>(estimatedPose.second), .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);
+}