From 3edc8750ecfb020f8bbc5b2cf5206549d0889dde Mon Sep 17 00:00:00 2001
From: David Vo <auscompgeek@users.noreply.github.com>
Date: Sat, 11 Feb 2023 23:44:22 +1100
Subject: [PATCH] PhotonPoseEstimator: Stop manually iterating targets (#796)

---
 .../cpp/photonlib/PhotonPoseEstimator.cpp     | 20 +++++++++----------
 .../include/photonlib/PhotonPoseEstimator.h   |  6 ------
 2 files changed, 9 insertions(+), 17 deletions(-)

diff --git a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp
index ecdc03eb1..16ef76611 100644
--- a/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp
+++ b/photon-lib/src/main/native/cpp/photonlib/PhotonPoseEstimator.cpp
@@ -98,21 +98,21 @@ std::optional<EstimatedRobotPose> PhotonPoseEstimator::Update(
 
 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();
+  auto foundIt = targets.end();
+  for (auto it = targets.begin(); it != targets.end(); ++it) {
+    if (it->GetPoseAmbiguity() < lowestAmbiguityScore) {
+      foundIt = it;
+      lowestAmbiguityScore = it->GetPoseAmbiguity();
     }
   }
 
-  if (lowestAJ == -1) {
+  if (foundIt == targets.end()) {
     return std::nullopt;
   }
 
-  PhotonTrackedTarget bestTarget = targets[lowestAJ];
+  auto& bestTarget = *foundIt;
 
   std::optional<frc::Pose3d> fiducialPose =
       aprilTags.GetTagPose(bestTarget.GetFiducialId());
@@ -186,8 +186,7 @@ PhotonPoseEstimator::ClosestToReferencePoseStrategy(
   frc::Pose3d pose = lastPose;
 
   auto targets = result.GetTargets();
-  for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
-    PhotonTrackedTarget target = targets[j];
+  for (auto& target : targets) {
     std::optional<frc::Pose3d> fiducialPose =
         aprilTags.GetTagPose(target.GetFiducialId());
     if (!fiducialPose) {
@@ -232,8 +231,7 @@ PhotonPoseEstimator::AverageBestTargetsStrategy(PhotonPipelineResult result) {
   double totalAmbiguity = 0;
 
   auto targets = result.GetTargets();
-  for (PhotonPoseEstimator::size_type j = 0; j < targets.size(); ++j) {
-    PhotonTrackedTarget target = targets[j];
+  for (auto& target : targets) {
     std::optional<frc::Pose3d> fiducialPose =
         aprilTags.GetTagPose(target.GetFiducialId());
     if (!fiducialPose) {
diff --git a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h
index fffd7c35f..367043d73 100644
--- a/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h
+++ b/photon-lib/src/main/native/include/photonlib/PhotonPoseEstimator.h
@@ -25,8 +25,6 @@
 #pragma once
 
 #include <memory>
-#include <utility>
-#include <vector>
 
 #include <frc/apriltag/AprilTagFieldLayout.h>
 #include <frc/geometry/Pose3d.h>
@@ -63,10 +61,6 @@ struct EstimatedRobotPose {
  */
 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.
    *