AprilTag ID to Pose API

wpimath/src/main/java/edu/wpi/first/math/WPIMathJNI.java

@@ -138,6 +138,22 @@ public static native void discreteAlgebraicRiccatiEquation(
   public static native void rankUpdate(
       double[] mat, int rows, double[] vec, double sigma, boolean lowerTriangular);
 
+  /**
+   * Deserializes an Apriltag layout JSON into a double[] of elements.
+   *
+   * @param json The JSON containing the serialized trajectory.
+   * @return A double array with the trajectory states.
+   */
+  public static native double[] deserializeApriltagLayout(String json);
+
+  /**
+   * Serializes the Apriltag layout into a JSON string.
+   *
+   * @param elements The elements of the layout.
+   * @return A JSON containing the serialized layout.
+   */
+  public static native String serializeApriltagLayout(double[] elements);
+
   public static class Helper {
     private static AtomicBoolean extractOnStaticLoad = new AtomicBoolean(true);
 

wpimath/src/main/java/edu/wpi/first/math/apriltag/ApriltagFieldLayout.java

@@ -0,0 +1,34 @@
+package edu.wpi.first.math.apriltag;
+
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+
+import java.util.HashMap;
+import java.util.Map;
+
+public class ApriltagFieldLayout {
+    private final Map<Integer, Pose3d> m_tags = new HashMap<>();
+    private boolean m_mirror = false;
+
+    public ApriltagFieldLayout(Map<Integer, Pose3d> tags) {
+        // To ensure the underlying semantics don't change with what kind of map is passed in
+        m_tags.putAll(tags);
+    }
+
+    public void setShouldMirror(boolean mirror) {
+        m_mirror = mirror;
+    }
+
+    public Map<Integer, Pose3d> getTags() {
+        return m_tags;
+    }
+
+    public Pose3d getTag(int id) {
+        Pose3d tag = m_tags.get(id);
+        if(m_mirror) {
+            tag = tag.relativeTo(new Pose3d(new Translation3d(16.4592, 8.2296, 0), new Rotation3d(0, 0, 180)));
+        }
+        return tag;
+    }
+}

wpimath/src/main/java/edu/wpi/first/math/apriltag/ApriltagUtil.java

@@ -0,0 +1,83 @@
+package edu.wpi.first.math.apriltag;
+
+import edu.wpi.first.math.WPIMathJNI;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Quaternion;
+import edu.wpi.first.math.geometry.Rotation3d;
+
+import java.util.HashMap;
+import java.util.Map;
+
+public class ApriltagUtil {
+    /**
+     * Creates an Apriltag layout from a double[] of elements.
+     *
+     * @param elements A double[] containing the raw elements of the Apriltag layout.
+     * @return An Apriltag layout created from the elements.
+     */
+    private static ApriltagFieldLayout createApriltagFieldLayoutFromElements(double[] elements) {
+        // Make sure that the elements have the correct length.
+        if (elements.length % 7 != 0) {
+            throw new ApriltagLayoutSerializationException(
+                    "An error occurred when converting trajectory elements into a trajectory.");
+        }
+
+        // Create a list of states from the elements.
+        Map<Integer, Pose3d> apriltagLayout = new HashMap<>();
+        for (int i = 0; i < elements.length; i += 7) {
+            apriltagLayout.put(
+                    (int) elements[i],
+                    new Pose3d(
+                            elements[i + 1],
+                            elements[i + 2],
+                            elements[i + 3],
+                            new Rotation3d(new Quaternion(
+                                    elements[i + 4],
+                                    elements[i + 5],
+                                    elements[i + 6],
+                                    elements[i + 7]
+                            ))
+                    )
+            );
+        }
+        return new ApriltagFieldLayout(apriltagLayout);
+    }
+
+    /**
+     * Returns a double[] of elements from the given Apriltag layout.
+     *
+     * @param apriltagFieldLayout The Apriltag field layout to retrieve raw elements from.
+     * @return A double[] of elements from the given trajectory.
+     */
+    private static double[] getElementsFromApriltagFieldLayout(ApriltagFieldLayout apriltagFieldLayout) {
+        // Create a double[] of elements and fill it from the trajectory states.
+        double[] elements = new double[apriltagFieldLayout.getTags().size() * 7];
+
+        for (int i = 0; i < apriltagFieldLayout.getTags().size() * 7; i += 7) {
+            var entry = apriltagFieldLayout.getTags().entrySet().stream().toList().get(i / 7);
+            elements[i] = entry.getKey();
+            elements[i + 1] = entry.getValue().getX();
+            elements[i + 2] = entry.getValue().getY();
+            elements[i + 3] = entry.getValue().getZ();
+            elements[i + 4] = entry.getValue().getRotation().getQuaternion().getW();
+            elements[i + 5] = entry.getValue().getRotation().getQuaternion().getX();
+            elements[i + 6] = entry.getValue().getRotation().getQuaternion().getY();
+            elements[i + 7] = entry.getValue().getRotation().getQuaternion().getZ();
+        }
+        return elements;
+    }
+
+    public static ApriltagFieldLayout deserializeApriltagFieldLayout(String json) {
+        return createApriltagFieldLayoutFromElements(WPIMathJNI.deserializeApriltagLayout(json));
+    }
+
+    public static String serializeApriltagFieldLayout(ApriltagFieldLayout apriltagFieldLayout) {
+        return WPIMathJNI.serializeApriltagLayout(getElementsFromApriltagFieldLayout(apriltagFieldLayout));
+    }
+
+    public static class ApriltagLayoutSerializationException extends RuntimeException {
+        public ApriltagLayoutSerializationException(String message) {
+            super(message);
+        }
+    }
+}

wpimath/src/main/native/cpp/apriltag/ApriltagFieldLayout.cpp

@@ -0,0 +1,37 @@
+// 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 <wpi/json.h>
+#include "frc/geometry/Pose3d.h"
+#include "units/angle.h"
+#include "units/length.h"
+
+using namespace frc;
+
+frc::Pose3d ApriltagFieldLayout::GetTagPose(int id) const {
+    auto returnPose = Pose3d{};
+    for(Apriltag tag : m_apriltags) {
+        if(tag.id == id) {
+            returnPose = tag.pose;
+        }
+    }
+    if(m_mirror) {
+        returnPose = returnPose.RelativeTo(Pose3d{units::inch_t{54}, units::inch_t{27}, returnPose.Z(), Rotation3d{units::radian_t{0}, units::radian_t{0}, units::radian_t{180}}});
+    }
+    return returnPose;
+}
+
+void frc::to_json(wpi::json& json, const ApriltagFieldLayout::Apriltag& apriltag) {
+    json = wpi::json{
+        {"id", apriltag.id},
+        {"pose", apriltag.pose}
+    };
+}
+
+void frc::from_json(const wpi::json& json, ApriltagFieldLayout::Apriltag& apriltag) {
+    apriltag.id = json.at("id").get<int>();
+    apriltag.pose = json.at("pose").get<Pose3d>();
+}

wpimath/src/main/native/cpp/apriltag/ApriltagUtil.cpp

@@ -0,0 +1,54 @@
+// 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/ApriltagUtil.h"
+
+#include <system_error>
+#include "frc/apriltag/ApriltagFieldLayout.h"
+
+#include <fmt/format.h>
+#include <wpi/SmallString.h>
+#include <wpi/json.h>
+#include <wpi/raw_istream.h>
+#include <wpi/raw_ostream.h>
+
+using namespace frc;
+
+void ApriltagUtil::ToJson(const ApriltagFieldLayout& apriltagFieldLayout,
+                                      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 = apriltagFieldLayout.GetTags();
+  output << json;
+  output.flush();
+}
+
+ApriltagFieldLayout ApriltagUtil::FromJson(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;
+
+  return ApriltagFieldLayout{json.get<std::vector<ApriltagFieldLayout::Apriltag>>()};
+}
+
+std::string ApriltagUtil::SerializeApriltagLayout(const ApriltagFieldLayout& apriltagFieldLayout) {
+  wpi::json json = apriltagFieldLayout.GetTags();
+  return json.dump();
+}
+
+ApriltagFieldLayout ApriltagUtil::DeserializeApriltagLayout(std::string_view jsonStr) {
+  wpi::json json = wpi::json::parse(jsonStr);
+  return ApriltagFieldLayout{json.get<std::vector<ApriltagFieldLayout::Apriltag>>()};
+}

wpimath/src/main/native/cpp/jni/WPIMathJNI.cpp

@@ -14,7 +14,13 @@
 #include "Eigen/QR"
 #include "drake/math/discrete_algebraic_riccati_equation.h"
 #include "edu_wpi_first_math_WPIMathJNI.h"
+#include "frc/apriltag/ApriltagFieldLayout.h"
+#include "frc/apriltag/ApriltagUtil.h"
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Rotation3d.h"
 #include "frc/trajectory/TrajectoryUtil.h"
+#include "units/angle.h"
+#include "units/length.h"
 #include "unsupported/Eigen/MatrixFunctions"
 
 using namespace wpi::java;
@@ -93,6 +99,51 @@ frc::Trajectory CreateTrajectoryFromElements(wpi::span<const double> elements) {
   return frc::Trajectory(states);
 }
 
+std::vector<double> GetElementsFromApriltagLayout(
+    const frc::ApriltagFieldLayout& apriltagFieldLayout) {
+  std::vector<double> elements;
+  elements.reserve(apriltagFieldLayout.GetTags().size() * 8);
+
+  for (auto&& apriltag : apriltagFieldLayout.GetTags()) {
+    elements.push_back(apriltag.id);
+    elements.push_back(apriltag.pose.X().value());
+    elements.push_back(apriltag.pose.Y().value());
+    elements.push_back(apriltag.pose.Z().value());
+    elements.push_back(apriltag.pose.Rotation().GetQuaternion().W());
+    elements.push_back(apriltag.pose.Rotation().GetQuaternion().X());
+    elements.push_back(apriltag.pose.Rotation().GetQuaternion().Y());
+    elements.push_back(apriltag.pose.Rotation().GetQuaternion().Z());
+  }
+
+  return elements;
+}
+
+frc::ApriltagFieldLayout CreateApriltagLayoutFromElements(wpi::span<const double> elements) {
+  // Make sure that the elements have the correct length.
+  assert(elements.size() % 8 == 0);
+
+  // Create a vector of states from the elements.
+  std::vector<frc::ApriltagFieldLayout::Apriltag> apriltags;
+  apriltags.reserve(elements.size() / 8);
+
+  for (size_t i = 0; i < elements.size(); i += 8) {
+    apriltags.emplace_back(frc::ApriltagFieldLayout::Apriltag{
+      static_cast<int>(elements[0]),
+      frc::Pose3d{units::meter_t{elements[i + 1]},
+                  units::meter_t{elements[i + 2]},
+                  units::meter_t{elements[i + 3]},
+                  frc::Rotation3d{frc::Quaternion{
+                                  elements[i + 4],
+                                  elements[i + 5],
+                                  elements[i + 6],
+                                  elements[i + 7]}}}});
+  }
+
+  return frc::ApriltagFieldLayout(apriltags);
+}
+
+
+
 extern "C" {
 
 /*
@@ -308,6 +359,56 @@ Java_edu_wpi_first_math_WPIMathJNI_serializeTrajectory
   }
 }
 
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    deserializeApriltagLayout
+ * Signature: (Ljava/lang/String;)[D
+ */
+JNIEXPORT jdoubleArray JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_deserializeApriltagLayout
+  (JNIEnv* env, jclass, jstring json)
+{
+  try {
+    auto apriltagFieldLayout = frc::ApriltagUtil::DeserializeApriltagLayout(
+        JStringRef{env, json}.c_str());
+    std::vector<double> elements = GetElementsFromApriltagLayout(apriltagFieldLayout);
+    return MakeJDoubleArray(env, elements);
+  } catch (std::exception& e) {
+    jclass cls = env->FindClass(
+        "edu/wpi/first/math/apriltag/ApriltagUtil$"
+        "ApriltagLayoutSerializationException");
+    if (cls) {
+      env->ThrowNew(cls, e.what());
+    }
+    return nullptr;
+  }
+}
+
+/*
+ * Class:     edu_wpi_first_math_WPIMathJNI
+ * Method:    serializeApriltagLayout
+ * Signature: ([D)Ljava/lang/String;
+ */
+JNIEXPORT jstring JNICALL
+Java_edu_wpi_first_math_WPIMathJNI_serializeApriltagLayout
+  (JNIEnv* env, jclass, jdoubleArray elements)
+{
+  try {
+    auto apriltagFieldLayout =
+        CreateApriltagLayoutFromElements(JDoubleArrayRef{env, elements});
+    return MakeJString(env,
+                       frc::ApriltagUtil::SerializeApriltagLayout(apriltagFieldLayout));
+  } catch (std::exception& e) {
+    jclass cls = env->FindClass(
+        "edu/wpi/first/math/apriltag/ApriltagUtil$"
+        "ApriltagLayoutSerializationException");
+    if (cls) {
+      env->ThrowNew(cls, e.what());
+    }
+    return nullptr;
+  }
+}
+
 /*
  * Class:     edu_wpi_first_math_WPIMathJNI
  * Method:    rankUpdate