[apriltags] Use map as internal data model (#4577)

This leaves the file format as a list, but internally will transform the collection of tags into a map on de/serialization. The serialization will probably happen once on startup, but the tag lookup can happen 100s of times a second. This honestly probably doesn't make too much of a performance hit since N is small, but this is a simple O(n) -> O(1) change for lookups.

wpilibc/src/main/native/cpp/apriltag/AprilTagFieldLayout.cpp

@@ -4,7 +4,6 @@
 
 #include "frc/apriltag/AprilTagFieldLayout.h"
 
-#include <algorithm>
 #include <system_error>
 
 #include <units/angle.h>
@@ -26,7 +25,9 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) {
   wpi::json json;
   input >> json;
 
-  m_apriltags = json.at("tags").get<std::vector<AprilTag>>();
+  for (const auto& tag : json.at("tags").get<std::vector<AprilTag>>()) {
+    m_apriltags[tag.ID] = tag;
+  }
   m_fieldWidth = units::meter_t{json.at("field").at("width").get<double>()};
   m_fieldLength = units::meter_t{json.at("field").at("height").get<double>()};
 }
@@ -34,28 +35,28 @@ AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) {
 AprilTagFieldLayout::AprilTagFieldLayout(std::vector<AprilTag> apriltags,
                                          units::meter_t fieldLength,
                                          units::meter_t fieldWidth)
-    : m_apriltags(std::move(apriltags)),
-      m_fieldLength(std::move(fieldLength)),
-      m_fieldWidth(std::move(fieldWidth)) {}
+    : m_fieldLength(std::move(fieldLength)),
+      m_fieldWidth(std::move(fieldWidth)) {
+  for (const auto& tag : apriltags) {
+    m_apriltags[tag.ID] = tag;
+  }
+}
 
 void AprilTagFieldLayout::SetAlliance(DriverStation::Alliance alliance) {
   m_mirror = alliance == DriverStation::Alliance::kRed;
 }
 
 std::optional<frc::Pose3d> AprilTagFieldLayout::GetTagPose(int ID) const {
-  Pose3d returnPose;
-  auto it = std::find_if(m_apriltags.begin(), m_apriltags.end(),
-                         [=](const auto& tag) { return tag.ID == ID; });
-  if (it != m_apriltags.end()) {
-    returnPose = it->pose;
-  } else {
-    return std::optional<Pose3d>();
+  const auto& it = m_apriltags.find(ID);
+  if (it == m_apriltags.end()) {
+    return std::nullopt;
   }
+  Pose3d returnPose = it->second.pose;
   if (m_mirror) {
     returnPose = returnPose.RelativeTo(Pose3d{
         m_fieldLength, m_fieldWidth, 0_m, Rotation3d{0_deg, 0_deg, 180_deg}});
   }
-  return std::make_optional(returnPose);
+  return returnPose;
 }
 
 void AprilTagFieldLayout::Serialize(std::string_view path) {
@@ -82,14 +83,24 @@ bool AprilTagFieldLayout::operator!=(const AprilTagFieldLayout& other) const {
 }
 
 void frc::to_json(wpi::json& json, const AprilTagFieldLayout& layout) {
+  std::vector<AprilTag> tagVector;
+  tagVector.reserve(layout.m_apriltags.size());
+  for (const auto& pair : layout.m_apriltags) {
+    tagVector.push_back(pair.second);
+  }
+
   json = wpi::json{{"field",
                     {{"length", layout.m_fieldLength.value()},
                      {"width", layout.m_fieldWidth.value()}}},
-                   {"tags", layout.m_apriltags}};
+                   {"tags", tagVector}};
 }
 
 void frc::from_json(const wpi::json& json, AprilTagFieldLayout& layout) {
-  layout.m_apriltags = json.at("tags").get<std::vector<AprilTag>>();
+  layout.m_apriltags.clear();
+  for (const auto& tag : json.at("tags").get<std::vector<AprilTag>>()) {
+    layout.m_apriltags[tag.ID] = tag;
+  }
+
   layout.m_fieldLength =
       units::meter_t{json.at("field").at("length").get<double>()};
   layout.m_fieldWidth =

wpilibc/src/main/native/include/frc/apriltag/AprilTagFieldLayout.h

@@ -6,6 +6,7 @@
 
 #include <optional>
 #include <string_view>
+#include <unordered_map>
 #include <vector>
 
 #include <units/length.h>
@@ -101,7 +102,7 @@ class WPILIB_DLLEXPORT AprilTagFieldLayout {
   bool operator!=(const AprilTagFieldLayout& other) const;
 
  private:
-  std::vector<AprilTag> m_apriltags;
+  std::unordered_map<int, AprilTag> m_apriltags;
   units::meter_t m_fieldLength;
   units::meter_t m_fieldWidth;
   bool m_mirror = false;

wpilibj/src/main/java/edu/wpi/first/wpilibj/apriltag/AprilTagFieldLayout.java

@@ -16,7 +16,9 @@ import edu.wpi.first.wpilibj.DriverStation;
 import java.io.IOException;
 import java.nio.file.Path;
 import java.util.ArrayList;
+import java.util.HashMap;
 import java.util.List;
+import java.util.Map;
 import java.util.Objects;
 import java.util.Optional;
 
@@ -38,8 +40,7 @@ import java.util.Optional;
 @JsonIgnoreProperties(ignoreUnknown = true)
 @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class AprilTagFieldLayout {
-  @JsonProperty(value = "tags")
-  private final List<AprilTag> m_apriltags = new ArrayList<>();
+  private final Map<Integer, AprilTag> m_apriltags = new HashMap<>();
 
   @JsonProperty(value = "field")
   private FieldDimensions m_fieldDimensions;
@@ -65,7 +66,7 @@ public class AprilTagFieldLayout {
   public AprilTagFieldLayout(Path path) throws IOException {
     AprilTagFieldLayout layout =
         new ObjectMapper().readValue(path.toFile(), AprilTagFieldLayout.class);
-    m_apriltags.addAll(layout.m_apriltags);
+    m_apriltags.putAll(layout.m_apriltags);
     m_fieldDimensions = layout.m_fieldDimensions;
   }
 
@@ -85,10 +86,17 @@ public class AprilTagFieldLayout {
       @JsonProperty(required = true, value = "tags") List<AprilTag> apriltags,
       @JsonProperty(required = true, value = "field") FieldDimensions fieldDimensions) {
     // To ensure the underlying semantics don't change with what kind of list is passed in
-    m_apriltags.addAll(apriltags);
+    for (AprilTag tag : apriltags) {
+      m_apriltags.put(tag.ID, tag);
+    }
     m_fieldDimensions = fieldDimensions;
   }
 
+  @JsonProperty("tags")
+  public List<AprilTag> getTags() {
+    return new ArrayList<>(m_apriltags.values());
+  }
+
   /**
    * Set the alliance that your team is on.
    *
@@ -110,16 +118,11 @@ public class AprilTagFieldLayout {
    */
   @SuppressWarnings("ParameterName")
   public Optional<Pose3d> getTagPose(int ID) {
-    Pose3d pose = null;
-    for (AprilTag apriltag : m_apriltags) {
-      if (apriltag.ID == ID) {
-        pose = apriltag.pose;
-        break;
-      }
-    }
-    if (pose == null) {
+    AprilTag tag = m_apriltags.get(ID);
+    if (tag == null) {
       return Optional.empty();
     }
+    Pose3d pose = tag.pose;
     if (m_mirror) {
       pose =
           pose.relativeTo(