Return named type from PhotonPoseEstimator (#734)

Adds PhotonPoseEstimator class, and deprecates 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}}});
       }