[wpilib] Add AprilTag and AprilTagFieldLayout (#4421)

This is an API for looking up a Pose3d from a tag id, and includes functionality to load that map from a JSON file.

This also adds JSON support to Pose3d, Rotation3d. Translation3d, and Quaternion.

Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: AMereBagatelle <themerebagatelle@gmail.com>

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

@@ -0,0 +1,26 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/apriltag/AprilTag.h"
+
+#include <wpi/json.h>
+
+using namespace frc;
+
+bool AprilTag::operator==(const AprilTag& other) const {
+  return ID == other.ID && pose == other.pose;
+}
+
+bool AprilTag::operator!=(const AprilTag& other) const {
+  return !operator==(other);
+}
+
+void frc::to_json(wpi::json& json, const AprilTag& apriltag) {
+  json = wpi::json{{"ID", apriltag.ID}, {"pose", apriltag.pose}};
+}
+
+void frc::from_json(const wpi::json& json, AprilTag& apriltag) {
+  apriltag.ID = json.at("ID").get<int>();
+  apriltag.pose = json.at("pose").get<Pose3d>();
+}

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

@@ -0,0 +1,97 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "frc/apriltag/AprilTagFieldLayout.h"
+
+#include <algorithm>
+#include <system_error>
+
+#include <units/angle.h>
+#include <units/length.h>
+#include <wpi/json.h>
+#include <wpi/raw_istream.h>
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+AprilTagFieldLayout::AprilTagFieldLayout(std::string_view path) {
+  std::error_code error_code;
+
+  wpi::raw_fd_istream input{path, error_code};
+  if (error_code) {
+    throw std::runtime_error(fmt::format("Cannot open file: {}", path));
+  }
+
+  wpi::json json;
+  input >> json;
+
+  m_apriltags = json.at("tags").get<std::vector<AprilTag>>();
+  m_fieldWidth = units::meter_t{json.at("field").at("width").get<double>()};
+  m_fieldLength = units::meter_t{json.at("field").at("height").get<double>()};
+}
+
+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)) {}
+
+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>();
+  }
+  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);
+}
+
+void AprilTagFieldLayout::Serialize(std::string_view path) {
+  std::error_code error_code;
+
+  wpi::raw_fd_ostream output{path, error_code};
+  if (error_code) {
+    throw std::runtime_error(fmt::format("Cannot open file: {}", path));
+  }
+
+  wpi::json json = *this;
+  output << json;
+  output.flush();
+}
+
+bool AprilTagFieldLayout::operator==(const AprilTagFieldLayout& other) const {
+  return m_apriltags == other.m_apriltags && m_mirror == other.m_mirror &&
+         m_fieldLength == other.m_fieldLength &&
+         m_fieldWidth == other.m_fieldWidth;
+}
+
+bool AprilTagFieldLayout::operator!=(const AprilTagFieldLayout& other) const {
+  return !operator==(other);
+}
+
+void frc::to_json(wpi::json& json, const AprilTagFieldLayout& layout) {
+  json = wpi::json{{"field",
+                    {{"length", layout.m_fieldLength.value()},
+                     {"width", layout.m_fieldWidth.value()}}},
+                   {"tags", layout.m_apriltags}};
+}
+
+void frc::from_json(const wpi::json& json, AprilTagFieldLayout& layout) {
+  layout.m_apriltags = json.at("tags").get<std::vector<AprilTag>>();
+  layout.m_fieldLength =
+      units::meter_t{json.at("field").at("length").get<double>()};
+  layout.m_fieldWidth =
+      units::meter_t{json.at("field").at("width").get<double>()};
+}

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

@@ -0,0 +1,45 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <wpi/SymbolExports.h>
+
+#include "frc/geometry/Pose3d.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace frc {
+
+struct WPILIB_DLLEXPORT AprilTag {
+  int ID;
+
+  Pose3d pose;
+
+  /**
+   * Checks equality between this AprilTag and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const AprilTag& other) const;
+
+  /**
+   * Checks inequality between this AprilTag and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const AprilTag& other) const;
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const AprilTag& apriltag);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, AprilTag& apriltag);
+
+}  // namespace frc

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

@@ -0,0 +1,122 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <optional>
+#include <string_view>
+#include <vector>
+
+#include <units/length.h>
+#include <wpi/SymbolExports.h>
+
+#include "frc/DriverStation.h"
+#include "frc/apriltag/AprilTag.h"
+#include "frc/geometry/Pose3d.h"
+
+namespace wpi {
+class json;
+}  // namespace wpi
+
+namespace frc {
+/**
+ * Class for representing a layout of AprilTags on a field and reading them from
+ * a JSON format.
+ *
+ * The JSON format contains two top-level objects, "tags" and "field".
+ * The "tags" object is a list of all AprilTags contained within a layout. Each
+ * AprilTag serializes to a JSON object containing an ID and a Pose3d. The
+ * "field" object is a descriptor of the size of the field in meters with
+ * "width" and "height" values.  This is to account for arbitrary field sizes
+ * when mirroring the poses.
+ *
+ * Pose3ds are assumed to be measured from the bottom-left corner of the field,
+ * when the blue alliance is at the left. Pose3ds will automatically be returned
+ * as passed in when calling GetTagPose(int).  Setting an alliance color via
+ * SetAlliance(DriverStation::Alliance) will mirror the poses returned from
+ * GetTagPose(int) to be correct relative to the other alliance.
+ */
+class WPILIB_DLLEXPORT AprilTagFieldLayout {
+ public:
+  AprilTagFieldLayout() = default;
+
+  /**
+   * Construct a new AprilTagFieldLayout with values imported from a JSON file.
+   *
+   * @param path Path of the JSON file to import from.
+   */
+  explicit AprilTagFieldLayout(std::string_view path);
+
+  /**
+   * Construct a new AprilTagFieldLayout from a vector of AprilTag objects.
+   *
+   * @param apriltags Vector of AprilTags.
+   * @param fieldLength Length of field the layout of representing.
+   * @param fieldWidth Width of field the layout is representing.
+   */
+  AprilTagFieldLayout(std::vector<AprilTag> apriltags,
+                      units::meter_t fieldLength, units::meter_t fieldWidth);
+
+  /**
+   * Set the alliance that your team is on.
+   *
+   * This changes the GetTagPose(int) method to return the correct pose for your
+   * alliance.
+   *
+   * @param alliance The alliance to mirror poses for.
+   */
+  void SetAlliance(DriverStation::Alliance alliance);
+
+  /**
+   * Gets an AprilTag pose by its ID.
+   *
+   * @param ID The ID of the tag.
+   * @return The pose corresponding to the ID that was passed in or an empty
+   * optional if a tag with that ID is not found.
+   */
+  std::optional<Pose3d> GetTagPose(int ID) const;
+
+  /**
+   * Serializes an AprilTagFieldLayout to a JSON file.
+   *
+   * @param path The path to write the JSON file to.
+   */
+  void Serialize(std::string_view path);
+
+  /*
+   * Checks equality between this AprilTagFieldLayout and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are equal.
+   */
+  bool operator==(const AprilTagFieldLayout& other) const;
+
+  /**
+   * Checks inequality between this AprilTagFieldLayout and another object.
+   *
+   * @param other The other object.
+   * @return Whether the two objects are not equal.
+   */
+  bool operator!=(const AprilTagFieldLayout& other) const;
+
+ private:
+  std::vector<AprilTag> m_apriltags;
+  units::meter_t m_fieldLength;
+  units::meter_t m_fieldWidth;
+  bool m_mirror = false;
+
+  friend WPILIB_DLLEXPORT void to_json(wpi::json& json,
+                                       const AprilTagFieldLayout& layout);
+
+  friend WPILIB_DLLEXPORT void from_json(const wpi::json& json,
+                                         AprilTagFieldLayout& layout);
+};
+
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const AprilTagFieldLayout& layout);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, AprilTagFieldLayout& layout);
+
+}  // namespace frc

wpilibc/src/test/native/cpp/apriltag/AprilTagJsonTest.cpp

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <vector>
+
+#include <wpi/json.h>
+
+#include "frc/apriltag/AprilTag.h"
+#include "frc/apriltag/AprilTagFieldLayout.h"
+#include "frc/geometry/Pose3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(AprilTagJsonTest, DeserializeMatches) {
+  auto layout = AprilTagFieldLayout{
+      std::vector{
+          AprilTag{1, Pose3d{}},
+          AprilTag{3, Pose3d{0_m, 1_m, 0_m, Rotation3d{0_deg, 0_deg, 0_deg}}}},
+      54_ft, 27_ft};
+
+  AprilTagFieldLayout deserialized;
+  wpi::json json = layout;
+  EXPECT_NO_THROW(deserialized = json.get<AprilTagFieldLayout>());
+  EXPECT_EQ(layout, deserialized);
+}

wpilibc/src/test/native/cpp/apriltag/AprilTagPoseMirroringTest.cpp

@@ -0,0 +1,32 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <vector>
+
+#include <wpi/json.h>
+
+#include "frc/apriltag/AprilTag.h"
+#include "frc/apriltag/AprilTagFieldLayout.h"
+#include "frc/geometry/Pose3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(AprilTagPoseMirroringTest, MirroringMatches) {
+  auto layout = AprilTagFieldLayout{
+      std::vector<AprilTag>{
+          AprilTag{1,
+                   Pose3d{0_ft, 0_ft, 0_ft, Rotation3d{0_deg, 0_deg, 0_deg}}},
+          AprilTag{
+              2, Pose3d{4_ft, 4_ft, 4_ft, Rotation3d{0_deg, 0_deg, 180_deg}}}},
+      54_ft, 27_ft};
+
+  layout.SetAlliance(DriverStation::Alliance::kRed);
+
+  auto mirrorPose =
+      Pose3d{54_ft, 27_ft, 0_ft, Rotation3d{0_deg, 0_deg, 180_deg}};
+  EXPECT_EQ(mirrorPose, *layout.GetTagPose(1));
+  mirrorPose = Pose3d{50_ft, 23_ft, 4_ft, Rotation3d{0_deg, 0_deg, 0_deg}};
+  EXPECT_EQ(mirrorPose, *layout.GetTagPose(2));
+}

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

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.apriltag;
+
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import edu.wpi.first.math.geometry.Pose3d;
+import java.util.Objects;
+
+@SuppressWarnings("MemberName")
+public class AprilTag {
+  @JsonProperty(value = "ID")
+  public int ID;
+
+  @JsonProperty(value = "pose")
+  public Pose3d pose;
+
+  @SuppressWarnings("ParameterName")
+  @JsonCreator
+  public AprilTag(
+      @JsonProperty(required = true, value = "ID") int ID,
+      @JsonProperty(required = true, value = "pose") Pose3d pose) {
+    this.ID = ID;
+    this.pose = pose;
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof AprilTag) {
+      var other = (AprilTag) obj;
+      return ID == other.ID && pose.equals(other.pose);
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(ID, pose);
+  }
+
+  @Override
+  public String toString() {
+    return "AprilTag(ID: " + ID + ", pose: " + pose + ")";
+  }
+}

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

@@ -0,0 +1,187 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.apriltag;
+
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.wpilibj.DriverStation;
+import java.io.IOException;
+import java.nio.file.Path;
+import java.util.ArrayList;
+import java.util.List;
+import java.util.Objects;
+import java.util.Optional;
+
+/**
+ * Class for representing a layout of AprilTags on a field and reading them from a JSON format.
+ *
+ * <p>The JSON format contains two top-level objects, "tags" and "field". The "tags" object is a
+ * list of all AprilTags contained within a layout. Each AprilTag serializes to a JSON object
+ * containing an ID and a Pose3d. The "field" object is a descriptor of the size of the field in
+ * meters with "width" and "height" values. This is to account for arbitrary field sizes when
+ * mirroring the poses.
+ *
+ * <p>Pose3ds are assumed to be measured from the bottom-left corner of the field, when the blue
+ * alliance is at the left. Pose3ds will automatically be returned as passed in when calling {@link
+ * AprilTagFieldLayout#getTagPose(int)}. Setting an alliance color via {@link
+ * AprilTagFieldLayout#setAlliance(DriverStation.Alliance)} will mirror the poses returned from
+ * {@link AprilTagFieldLayout#getTagPose(int)} to be correct relative to the other alliance.
+ */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+public class AprilTagFieldLayout {
+  @JsonProperty(value = "tags")
+  private final List<AprilTag> m_apriltags = new ArrayList<>();
+
+  @JsonProperty(value = "field")
+  private FieldDimensions m_fieldDimensions;
+
+  private boolean m_mirror;
+
+  /**
+   * Construct a new AprilTagFieldLayout with values imported from a JSON file.
+   *
+   * @param path Path of the JSON file to import from.
+   * @throws IOException If reading from the file fails.
+   */
+  public AprilTagFieldLayout(String path) throws IOException {
+    this(Path.of(path));
+  }
+
+  /**
+   * Construct a new AprilTagFieldLayout with values imported from a JSON file.
+   *
+   * @param path Path of the JSON file to import from.
+   * @throws IOException If reading from the file fails.
+   */
+  public AprilTagFieldLayout(Path path) throws IOException {
+    AprilTagFieldLayout layout =
+        new ObjectMapper().readValue(path.toFile(), AprilTagFieldLayout.class);
+    m_apriltags.addAll(layout.m_apriltags);
+    m_fieldDimensions = layout.m_fieldDimensions;
+  }
+
+  /**
+   * Construct a new AprilTagFieldLayout from a list of {@link AprilTag} objects.
+   *
+   * @param apriltags List of {@link AprilTag}.
+   * @param fieldLength Length of the field in meters.
+   * @param fieldWidth Width of the field in meters.
+   */
+  public AprilTagFieldLayout(List<AprilTag> apriltags, double fieldLength, double fieldWidth) {
+    this(apriltags, new FieldDimensions(fieldLength, fieldWidth));
+  }
+
+  @JsonCreator
+  private 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);
+    m_fieldDimensions = fieldDimensions;
+  }
+
+  /**
+   * Set the alliance that your team is on.
+   *
+   * <p>This changes the {@link #getTagPose(int)} method to return the correct pose for your
+   * alliance.
+   *
+   * @param alliance The alliance to mirror poses for.
+   */
+  public void setAlliance(DriverStation.Alliance alliance) {
+    m_mirror = alliance == DriverStation.Alliance.Red;
+  }
+
+  /**
+   * Gets an AprilTag pose by its ID.
+   *
+   * @param ID The ID of the tag.
+   * @return The pose corresponding to the ID passed in or an empty optional if a tag with that ID
+   *     was not found.
+   */
+  @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) {
+      return Optional.empty();
+    }
+    if (m_mirror) {
+      pose =
+          pose.relativeTo(
+              new Pose3d(
+                  new Translation3d(
+                      m_fieldDimensions.fieldWidth, m_fieldDimensions.fieldLength, 0.0),
+                  new Rotation3d(0.0, 0.0, Math.PI)));
+    }
+    return Optional.of(pose);
+  }
+
+  /**
+   * Serializes a AprilTagFieldLayout to a JSON file.
+   *
+   * @param path The path to write to.
+   * @throws IOException If writing to the file fails.
+   */
+  public void serialize(String path) throws IOException {
+    serialize(Path.of(path));
+  }
+
+  /**
+   * Serializes a AprilTagFieldLayout to a JSON file.
+   *
+   * @param path The path to write to.
+   * @throws IOException If writing to the file fails.
+   */
+  public void serialize(Path path) throws IOException {
+    new ObjectMapper().writeValue(path.toFile(), this);
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (obj instanceof AprilTagFieldLayout) {
+      var other = (AprilTagFieldLayout) obj;
+      return m_apriltags.equals(other.m_apriltags) && m_mirror == other.m_mirror;
+    }
+    return false;
+  }
+
+  @Override
+  public int hashCode() {
+    return Objects.hash(m_apriltags, m_mirror);
+  }
+
+  @JsonIgnoreProperties(ignoreUnknown = true)
+  @JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
+  private static class FieldDimensions {
+    @SuppressWarnings("MemberName")
+    @JsonProperty(value = "width")
+    public double fieldWidth;
+
+    @SuppressWarnings("MemberName")
+    @JsonProperty(value = "height")
+    public double fieldLength;
+
+    @JsonCreator()
+    FieldDimensions(
+        @JsonProperty(required = true, value = "width") double fieldWidth,
+        @JsonProperty(required = true, value = "height") double fieldLength) {
+      this.fieldWidth = fieldWidth;
+      this.fieldLength = fieldLength;
+    }
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagPoseMirroringTest.java

@@ -0,0 +1,47 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.apriltag;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.DriverStation;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class AprilTagPoseMirroringTest {
+  @Test
+  void poseMirroring() {
+    var layout =
+        new AprilTagFieldLayout(
+            List.of(
+                new AprilTag(1, new Pose3d(new Translation3d(0, 0, 0), new Rotation3d(0, 0, 0))),
+                new AprilTag(
+                    2,
+                    new Pose3d(
+                        new Translation3d(
+                            Units.feetToMeters(4.0), Units.feetToMeters(4), Units.feetToMeters(4)),
+                        new Rotation3d(0, 0, Units.degreesToRadians(180))))),
+            Units.feetToMeters(54.0),
+            Units.feetToMeters(27.0));
+    layout.setAlliance(DriverStation.Alliance.Red);
+
+    assertEquals(
+        new Pose3d(
+            new Translation3d(Units.feetToMeters(54.0), Units.feetToMeters(27.0), 0.0),
+            new Rotation3d(0.0, 0.0, Units.degreesToRadians(180.0))),
+        layout.getTagPose(1).orElse(null));
+
+    assertEquals(
+        new Pose3d(
+            new Translation3d(
+                Units.feetToMeters(50.0), Units.feetToMeters(23.0), Units.feetToMeters(4)),
+            new Rotation3d(0.0, 0.0, 0)),
+        layout.getTagPose(2).orElse(null));
+  }
+}

wpilibj/src/test/java/edu/wpi/first/wpilibj/apriltag/AprilTagSerializationTest.java

@@ -0,0 +1,38 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package edu.wpi.first.wpilibj.apriltag;
+
+import static org.junit.jupiter.api.Assertions.assertDoesNotThrow;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import com.fasterxml.jackson.databind.ObjectMapper;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.util.Units;
+import java.util.List;
+import org.junit.jupiter.api.Test;
+
+class AprilTagSerializationTest {
+  @Test
+  void deserializeMatches() {
+    var layout =
+        new AprilTagFieldLayout(
+            List.of(
+                new AprilTag(1, new Pose3d(0, 0, 0, new Rotation3d(0, 0, 0))),
+                new AprilTag(3, new Pose3d(0, 1, 0, new Rotation3d(0, 0, 0)))),
+            Units.feetToMeters(54.0),
+            Units.feetToMeters(27.0));
+
+    var objectMapper = new ObjectMapper();
+
+    var deserialized =
+        assertDoesNotThrow(
+            () ->
+                objectMapper.readValue(
+                    objectMapper.writeValueAsString(layout), AprilTagFieldLayout.class));
+
+    assertEquals(layout, deserialized);
+  }
+}

wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java

@@ -4,6 +4,10 @@
 
 package edu.wpi.first.math.geometry;
 
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.Matrix;
 import edu.wpi.first.math.Nat;
@@ -14,6 +18,8 @@ import edu.wpi.first.math.numbers.N3;
 import java.util.Objects;
 
 /** Represents a 3D pose containing translational and rotational elements. */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Pose3d implements Interpolatable<Pose3d> {
   private final Translation3d m_translation;
   private final Rotation3d m_rotation;
@@ -30,7 +36,10 @@ public class Pose3d implements Interpolatable<Pose3d> {
    * @param translation The translational component of the pose.
    * @param rotation The rotational component of the pose.
    */
-  public Pose3d(Translation3d translation, Rotation3d rotation) {
+  @JsonCreator
+  public Pose3d(
+      @JsonProperty(required = true, value = "translation") Translation3d translation,
+      @JsonProperty(required = true, value = "rotation") Rotation3d rotation) {
     m_translation = translation;
     m_rotation = rotation;
   }
@@ -84,6 +93,7 @@ public class Pose3d implements Interpolatable<Pose3d> {
    *
    * @return The translational component of the pose.
    */
+  @JsonProperty
   public Translation3d getTranslation() {
     return m_translation;
   }
@@ -120,6 +130,7 @@ public class Pose3d implements Interpolatable<Pose3d> {
    *
    * @return The rotational component of the pose.
    */
+  @JsonProperty
   public Rotation3d getRotation() {
     return m_rotation;
   }

wpimath/src/main/java/edu/wpi/first/math/geometry/Quaternion.java

@@ -4,11 +4,17 @@
 
 package edu.wpi.first.math.geometry;
 
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.VecBuilder;
 import edu.wpi.first.math.Vector;
 import edu.wpi.first.math.numbers.N3;
 import java.util.Objects;
 
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Quaternion {
   private final double m_r;
   private final Vector<N3> m_v;
@@ -27,7 +33,12 @@ public class Quaternion {
    * @param y Y component of the quaternion.
    * @param z Z component of the quaternion.
    */
-  public Quaternion(double w, double x, double y, double z) {
+  @JsonCreator
+  public Quaternion(
+      @JsonProperty(required = true, value = "W") double w,
+      @JsonProperty(required = true, value = "X") double x,
+      @JsonProperty(required = true, value = "Y") double y,
+      @JsonProperty(required = true, value = "Z") double z) {
     m_r = w;
     m_v = VecBuilder.fill(x, y, z);
   }
@@ -113,6 +124,7 @@ public class Quaternion {
    *
    * @return W component of the quaternion.
    */
+  @JsonProperty(value = "W")
   public double getW() {
     return m_r;
   }
@@ -122,6 +134,7 @@ public class Quaternion {
    *
    * @return X component of the quaternion.
    */
+  @JsonProperty(value = "X")
   public double getX() {
     return m_v.get(0, 0);
   }
@@ -131,6 +144,7 @@ public class Quaternion {
    *
    * @return Y component of the quaternion.
    */
+  @JsonProperty(value = "Y")
   public double getY() {
     return m_v.get(1, 0);
   }
@@ -140,6 +154,7 @@ public class Quaternion {
    *
    * @return Z component of the quaternion.
    */
+  @JsonProperty(value = "Z")
   public double getZ() {
     return m_v.get(2, 0);
   }

wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java

@@ -4,6 +4,10 @@
 
 package edu.wpi.first.math.geometry;
 
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MatBuilder;
 import edu.wpi.first.math.MathSharedStore;
 import edu.wpi.first.math.MathUtil;
@@ -17,6 +21,8 @@ import java.util.Objects;
 import org.ejml.dense.row.factory.DecompositionFactory_DDRM;
 
 /** A rotation in a 3D coordinate frame represented by a quaternion. */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Rotation3d implements Interpolatable<Rotation3d> {
   private Quaternion m_q = new Quaternion();
 
@@ -28,7 +34,8 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
    *
    * @param q The quaternion.
    */
-  public Rotation3d(Quaternion q) {
+  @JsonCreator
+  public Rotation3d(@JsonProperty(required = true, value = "quaternion") Quaternion q) {
     m_q = q.normalize();
   }
 
@@ -270,6 +277,7 @@ public class Rotation3d implements Interpolatable<Rotation3d> {
    *
    * @return The quaternion representation of the Rotation3d.
    */
+  @JsonProperty(value = "quaternion")
   public Quaternion getQuaternion() {
     return m_q;
   }

wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java

@@ -4,6 +4,10 @@
 
 package edu.wpi.first.math.geometry;
 
+import com.fasterxml.jackson.annotation.JsonAutoDetect;
+import com.fasterxml.jackson.annotation.JsonCreator;
+import com.fasterxml.jackson.annotation.JsonIgnoreProperties;
+import com.fasterxml.jackson.annotation.JsonProperty;
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import java.util.Objects;
@@ -15,6 +19,8 @@ import java.util.Objects;
  * origin facing in the positive X direction, forward is positive X, left is positive Y, and up is
  * positive Z.
  */
+@JsonIgnoreProperties(ignoreUnknown = true)
+@JsonAutoDetect(getterVisibility = JsonAutoDetect.Visibility.NONE)
 public class Translation3d implements Interpolatable<Translation3d> {
   private final double m_x;
   private final double m_y;
@@ -32,7 +38,11 @@ public class Translation3d implements Interpolatable<Translation3d> {
    * @param y The y component of the translation.
    * @param z The z component of the translation.
    */
-  public Translation3d(double x, double y, double z) {
+  @JsonCreator
+  public Translation3d(
+      @JsonProperty(required = true, value = "x") double x,
+      @JsonProperty(required = true, value = "y") double y,
+      @JsonProperty(required = true, value = "z") double z) {
     m_x = x;
     m_y = y;
     m_z = z;
@@ -70,6 +80,7 @@ public class Translation3d implements Interpolatable<Translation3d> {
    *
    * @return The X component of the translation.
    */
+  @JsonProperty
   public double getX() {
     return m_x;
   }
@@ -79,6 +90,7 @@ public class Translation3d implements Interpolatable<Translation3d> {
    *
    * @return The Y component of the translation.
    */
+  @JsonProperty
   public double getY() {
     return m_y;
   }
@@ -88,6 +100,7 @@ public class Translation3d implements Interpolatable<Translation3d> {
    *
    * @return The Z component of the translation.
    */
+  @JsonProperty
   public double getZ() {
     return m_z;
   }

wpimath/src/main/native/cpp/geometry/Pose3d.cpp

@@ -6,6 +6,8 @@
 
 #include <cmath>
 
+#include <wpi/json.h>
+
 using namespace frc;
 
 namespace {
@@ -149,3 +151,13 @@ Twist3d Pose3d::Log(const Pose3d& end) const {
 Pose2d Pose3d::ToPose2d() const {
   return Pose2d{m_translation.X(), m_translation.Y(), m_rotation.Z()};
 }
+
+void frc::to_json(wpi::json& json, const Pose3d& pose) {
+  json = wpi::json{{"translation", pose.Translation()},
+                   {"rotation", pose.Rotation()}};
+}
+
+void frc::from_json(const wpi::json& json, Pose3d& pose) {
+  pose = Pose3d{json.at("translation").get<Translation3d>(),
+                json.at("rotation").get<Rotation3d>()};
+}

wpimath/src/main/native/cpp/geometry/Quaternion.cpp

@@ -4,6 +4,8 @@
 
 #include "frc/geometry/Quaternion.h"
 
+#include <wpi/json.h>
+
 using namespace frc;
 
 Quaternion::Quaternion(double w, double x, double y, double z)
@@ -81,3 +83,16 @@ Eigen::Vector3d Quaternion::ToRotationVector() const {
     }
   }
 }
+
+void frc::to_json(wpi::json& json, const Quaternion& quaternion) {
+  json = wpi::json{{"W", quaternion.W()},
+                   {"X", quaternion.X()},
+                   {"Y", quaternion.Y()},
+                   {"Z", quaternion.Z()}};
+}
+
+void frc::from_json(const wpi::json& json, Quaternion& quaternion) {
+  quaternion =
+      Quaternion{json.at("W").get<double>(), json.at("X").get<double>(),
+                 json.at("Y").get<double>(), json.at("Z").get<double>()};
+}

wpimath/src/main/native/cpp/geometry/Rotation3d.cpp

@@ -7,6 +7,8 @@
 #include <cmath>
 #include <numbers>
 
+#include <wpi/json.h>
+
 #include "Eigen/Core"
 #include "Eigen/LU"
 #include "Eigen/QR"
@@ -238,3 +240,11 @@ units::radian_t Rotation3d::Angle() const {
 Rotation2d Rotation3d::ToRotation2d() const {
   return Rotation2d{Z()};
 }
+
+void frc::to_json(wpi::json& json, const Rotation3d& rotation) {
+  json = wpi::json{{"quaternion", rotation.GetQuaternion()}};
+}
+
+void frc::from_json(const wpi::json& json, Rotation3d& rotation) {
+  rotation = Rotation3d{json.at("quaternion").get<Quaternion>()};
+}

wpimath/src/main/native/cpp/geometry/Translation3d.cpp

@@ -4,6 +4,11 @@
 
 #include "frc/geometry/Translation3d.h"
 
+#include <wpi/json.h>
+
+#include "units/length.h"
+#include "units/math.h"
+
 using namespace frc;
 
 Translation3d::Translation3d(units::meter_t distance, const Rotation3d& angle) {
@@ -39,3 +44,15 @@ bool Translation3d::operator==(const Translation3d& other) const {
 bool Translation3d::operator!=(const Translation3d& other) const {
   return !operator==(other);
 }
+
+void frc::to_json(wpi::json& json, const Translation3d& translation) {
+  json = wpi::json{{"x", translation.X().value()},
+                   {"y", translation.Y().value()},
+                   {"z", translation.Z().value()}};
+}
+
+void frc::from_json(const wpi::json& json, Translation3d& translation) {
+  translation = Translation3d{units::meter_t{json.at("x").get<double>()},
+                              units::meter_t{json.at("y").get<double>()},
+                              units::meter_t{json.at("z").get<double>()}};
+}

wpimath/src/main/native/include/frc/geometry/Pose3d.h

@@ -11,6 +11,10 @@
 #include "Translation3d.h"
 #include "Twist3d.h"
 
+namespace wpi {
+class json;
+}  // namespace wpi
+
 namespace frc {
 
 /**
@@ -202,4 +206,10 @@ class WPILIB_DLLEXPORT Pose3d {
   Rotation3d m_rotation;
 };
 
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Pose3d& pose);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Pose3d& pose);
+
 }  // namespace frc

wpimath/src/main/native/include/frc/geometry/Quaternion.h

@@ -8,6 +8,10 @@
 
 #include "frc/EigenCore.h"
 
+namespace wpi {
+class json;
+}  // namespace wpi
+
 namespace frc {
 
 class WPILIB_DLLEXPORT Quaternion {
@@ -92,4 +96,10 @@ class WPILIB_DLLEXPORT Quaternion {
   Eigen::Vector3d m_v{0.0, 0.0, 0.0};
 };
 
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Quaternion& quaternion);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Quaternion& quaternion);
+
 }  // namespace frc

wpimath/src/main/native/include/frc/geometry/Rotation3d.h

@@ -11,6 +11,10 @@
 #include "frc/EigenCore.h"
 #include "units/angle.h"
 
+namespace wpi {
+class json;
+}  // namespace wpi
+
 namespace frc {
 
 /**
@@ -180,4 +184,10 @@ class WPILIB_DLLEXPORT Rotation3d {
   Quaternion m_q;
 };
 
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Rotation3d& rotation);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Rotation3d& rotation);
+
 }  // namespace frc

wpimath/src/main/native/include/frc/geometry/Translation3d.h

@@ -10,6 +10,10 @@
 #include "Translation2d.h"
 #include "units/length.h"
 
+namespace wpi {
+class json;
+}  // namespace wpi
+
 namespace frc {
 
 /**
@@ -182,6 +186,12 @@ class WPILIB_DLLEXPORT Translation3d {
   units::meter_t m_z = 0_m;
 };
 
+WPILIB_DLLEXPORT
+void to_json(wpi::json& json, const Translation3d& state);
+
+WPILIB_DLLEXPORT
+void from_json(const wpi::json& json, Translation3d& state);
+
 }  // namespace frc
 
 #include "Translation3d.inc"