[wpimath] Add CoordinateSystem class (#4214)

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

@@ -0,0 +1,86 @@
+// 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.math.geometry;
+
+import edu.wpi.first.math.VecBuilder;
+import edu.wpi.first.math.Vector;
+import edu.wpi.first.math.numbers.N3;
+
+/** A class representing a coordinate system axis within the NWU coordinate system. */
+public class CoordinateAxis {
+  final Vector<N3> m_axis;
+
+  /**
+   * Constructs a coordinate system axis within the NWU coordinate system and normalizes it.
+   *
+   * @param x The x component.
+   * @param y The y component.
+   * @param z The z component.
+   */
+  public CoordinateAxis(double x, double y, double z) {
+    double norm = Math.sqrt(x * x + y * y + z * z);
+    m_axis = VecBuilder.fill(x / norm, y / norm, z / norm);
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to +X in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to +X in the NWU coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateAxis N() {
+    return new CoordinateAxis(1.0, 0.0, 0.0);
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to -X in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to -X in the NWU coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateAxis S() {
+    return new CoordinateAxis(-1.0, 0.0, 0.0);
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to -Y in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to -Y in the NWU coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateAxis E() {
+    return new CoordinateAxis(0.0, -1.0, 0.0);
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to +Y in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to +Y in the NWU coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateAxis W() {
+    return new CoordinateAxis(0.0, 1.0, 0.0);
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to +Z in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to +Z in the NWU coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateAxis U() {
+    return new CoordinateAxis(0.0, 0.0, 1.0);
+  }
+
+  /**
+   * Returns a coordinate axis corresponding to -Z in the NWU coordinate system.
+   *
+   * @return A coordinate axis corresponding to -Z in the NWU coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateAxis D() {
+    return new CoordinateAxis(0.0, 0.0, -1.0);
+  }
+}

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

@@ -0,0 +1,121 @@
+// 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.math.geometry;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
+
+/** A helper class that converts Pose3d objects between different standard coordinate frames. */
+public class CoordinateSystem {
+  // Rotation from this coordinate system to NWU coordinate system
+  private final Rotation3d m_rotation;
+
+  /**
+   * Constructs a coordinate system with the given cardinal directions for each axis.
+   *
+   * @param positiveX The cardinal direction of the positive x-axis.
+   * @param positiveY The cardinal direction of the positive y-axis.
+   * @param positiveZ The cardinal direction of the positive z-axis.
+   * @throws IllegalArgumentException if the coordinate system isn't special orthogonal
+   */
+  public CoordinateSystem(
+      CoordinateAxis positiveX, CoordinateAxis positiveY, CoordinateAxis positiveZ) {
+    // Construct a change of basis matrix from the source coordinate system to the
+    // NWU coordinate system. Each column vector in the change of basis matrix is
+    // one of the old basis vectors mapped to its representation in the new basis.
+    @SuppressWarnings("LocalVariableName")
+    var R = new Matrix<>(Nat.N3(), Nat.N3());
+    R.assignBlock(0, 0, positiveX.m_axis);
+    R.assignBlock(0, 1, positiveY.m_axis);
+    R.assignBlock(0, 2, positiveZ.m_axis);
+
+    // Require that the change of basis matrix is special orthogonal. This is true
+    // if the axes used are orthogonal and normalized. The Axis class already
+    // normalizes itself, so we just need to check for orthogonality.
+    if (!R.times(R.transpose()).equals(Matrix.eye(Nat.N3()))) {
+      throw new IllegalArgumentException("Coordinate system isn't special orthogonal");
+    }
+
+    // Turn change of basis matrix into a quaternion since it's a pure rotation
+    // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+    double trace = R.get(0, 0) + R.get(1, 1) + R.get(2, 2);
+    double w;
+    double x;
+    double y;
+    double z;
+
+    if (trace > 0.0) {
+      double s = 0.5 / Math.sqrt(trace + 1.0);
+      w = 0.25 / s;
+      x = (R.get(2, 1) - R.get(1, 2)) * s;
+      y = (R.get(0, 2) - R.get(2, 0)) * s;
+      z = (R.get(1, 0) - R.get(0, 1)) * s;
+    } else {
+      if (R.get(0, 0) > R.get(1, 1) && R.get(0, 0) > R.get(2, 2)) {
+        double s = 2.0 * Math.sqrt(1.0 + R.get(0, 0) - R.get(1, 1) - R.get(2, 2));
+        w = (R.get(2, 1) - R.get(1, 2)) / s;
+        x = 0.25 * s;
+        y = (R.get(0, 1) + R.get(1, 0)) / s;
+        z = (R.get(0, 2) + R.get(2, 0)) / s;
+      } else if (R.get(1, 1) > R.get(2, 2)) {
+        double s = 2.0 * Math.sqrt(1.0 + R.get(1, 1) - R.get(0, 0) - R.get(2, 2));
+        w = (R.get(0, 2) - R.get(2, 0)) / s;
+        x = (R.get(0, 1) + R.get(1, 0)) / s;
+        y = 0.25 * s;
+        z = (R.get(1, 2) + R.get(2, 1)) / s;
+      } else {
+        double s = 2.0 * Math.sqrt(1.0 + R.get(2, 2) - R.get(0, 0) - R.get(1, 1));
+        w = (R.get(1, 0) - R.get(0, 1)) / s;
+        x = (R.get(0, 2) + R.get(2, 0)) / s;
+        y = (R.get(1, 2) + R.get(2, 1)) / s;
+        z = 0.25 * s;
+      }
+    }
+
+    m_rotation = new Rotation3d(new Quaternion(w, x, y, z));
+  }
+
+  /**
+   * Returns an instance of the NWU coordinate system.
+   *
+   * @return An instance of the NWU coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateSystem NWU() {
+    return new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.W(), CoordinateAxis.U());
+  }
+
+  /**
+   * Returns an instance of the EDN coordinate system.
+   *
+   * @return An instance of the EDN coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateSystem EDN() {
+    return new CoordinateSystem(CoordinateAxis.E(), CoordinateAxis.D(), CoordinateAxis.N());
+  }
+
+  /**
+   * Returns an instance of the NED coordinate system.
+   *
+   * @return An instance of the NED coordinate system.
+   */
+  @SuppressWarnings("MethodName")
+  public static CoordinateSystem NED() {
+    return new CoordinateSystem(CoordinateAxis.N(), CoordinateAxis.E(), CoordinateAxis.D());
+  }
+
+  /**
+   * Converts the given pose from one coordinate system to another.
+   *
+   * @param pose The pose to convert.
+   * @param from The coordinate system the pose starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given pose in the desired coordinate system.
+   */
+  public static Pose3d convert(Pose3d pose, CoordinateSystem from, CoordinateSystem to) {
+    return pose.relativeTo(new Pose3d(new Translation3d(), to.m_rotation.minus(from.m_rotation)));
+  }
+}

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

@@ -0,0 +1,35 @@
+// 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/geometry/CoordinateAxis.h"
+
+using namespace frc;
+
+CoordinateAxis::CoordinateAxis(double x, double y, double z) : m_axis{x, y, z} {
+  m_axis /= m_axis.norm();
+}
+
+CoordinateAxis CoordinateAxis::N() {
+  return CoordinateAxis{1.0, 0.0, 0.0};
+}
+
+CoordinateAxis CoordinateAxis::S() {
+  return CoordinateAxis{-1.0, 0.0, 0.0};
+}
+
+CoordinateAxis CoordinateAxis::E() {
+  return CoordinateAxis{0.0, -1.0, 0.0};
+}
+
+CoordinateAxis CoordinateAxis::W() {
+  return CoordinateAxis{0.0, 1.0, 0.0};
+}
+
+CoordinateAxis CoordinateAxis::U() {
+  return CoordinateAxis{0.0, 0.0, 1.0};
+}
+
+CoordinateAxis CoordinateAxis::D() {
+  return CoordinateAxis{0.0, 0.0, -1.0};
+}

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

@@ -0,0 +1,92 @@
+// 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/geometry/CoordinateSystem.h"
+
+#include <cmath>
+#include <stdexcept>
+#include <utility>
+
+#include "Eigen/QR"
+
+using namespace frc;
+
+CoordinateSystem::CoordinateSystem(const CoordinateAxis& positiveX,
+                                   const CoordinateAxis& positiveY,
+                                   const CoordinateAxis& positiveZ) {
+  // Construct a change of basis matrix from the source coordinate system to the
+  // NWU coordinate system. Each column vector in the change of basis matrix is
+  // one of the old basis vectors mapped to its representation in the new basis.
+  Matrixd<3, 3> R;
+  R.block<3, 1>(0, 0) = positiveX.m_axis;
+  R.block<3, 1>(0, 1) = positiveY.m_axis;
+  R.block<3, 1>(0, 2) = positiveZ.m_axis;
+
+  // Require that the change of basis matrix is special orthogonal. This is true
+  // if the axes used are orthogonal and normalized. The Axis class already
+  // normalizes itself, so we just need to check for orthogonality.
+  if (R * R.transpose() != Matrixd<3, 3>::Identity()) {
+    throw std::domain_error("Coordinate system isn't special orthogonal");
+  }
+
+  // Turn change of basis matrix into a quaternion since it's a pure rotation
+  // https://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
+  double trace = R(0, 0) + R(1, 1) + R(2, 2);
+  double w;
+  double x;
+  double y;
+  double z;
+
+  if (trace > 0.0) {
+    double s = 0.5 / std::sqrt(trace + 1.0);
+    w = 0.25 / s;
+    x = (R(2, 1) - R(1, 2)) * s;
+    y = (R(0, 2) - R(2, 0)) * s;
+    z = (R(1, 0) - R(0, 1)) * s;
+  } else {
+    if (R(0, 0) > R(1, 1) && R(0, 0) > R(2, 2)) {
+      double s = 2.0 * std::sqrt(1.0 + R(0, 0) - R(1, 1) - R(2, 2));
+      w = (R(2, 1) - R(1, 2)) / s;
+      x = 0.25 * s;
+      y = (R(0, 1) + R(1, 0)) / s;
+      z = (R(0, 2) + R(2, 0)) / s;
+    } else if (R(1, 1) > R(2, 2)) {
+      double s = 2.0 * std::sqrt(1.0 + R(1, 1) - R(0, 0) - R(2, 2));
+      w = (R(0, 2) - R(2, 0)) / s;
+      x = (R(0, 1) + R(1, 0)) / s;
+      y = 0.25 * s;
+      z = (R(1, 2) + R(2, 1)) / s;
+    } else {
+      double s = 2.0 * std::sqrt(1.0 + R(2, 2) - R(0, 0) - R(1, 1));
+      w = (R(1, 0) - R(0, 1)) / s;
+      x = (R(0, 2) + R(2, 0)) / s;
+      y = (R(1, 2) + R(2, 1)) / s;
+      z = 0.25 * s;
+    }
+  }
+
+  m_rotation = Rotation3d{Quaternion{w, x, y, z}};
+}
+
+CoordinateSystem CoordinateSystem::NWU() {
+  return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::W(),
+                          CoordinateAxis::U()};
+}
+
+CoordinateSystem CoordinateSystem::EDN() {
+  return CoordinateSystem{CoordinateAxis::E(), CoordinateAxis::D(),
+                          CoordinateAxis::N()};
+}
+
+CoordinateSystem CoordinateSystem::NED() {
+  return CoordinateSystem{CoordinateAxis::N(), CoordinateAxis::E(),
+                          CoordinateAxis::D()};
+}
+
+Pose3d CoordinateSystem::Convert(const Pose3d& pose,
+                                 const CoordinateSystem& from,
+                                 const CoordinateSystem& to) {
+  return pose.RelativeTo(
+      Pose3d{Translation3d{}, to.m_rotation - from.m_rotation});
+}

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

@@ -0,0 +1,73 @@
+// 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/EigenCore.h"
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Rotation3d.h"
+
+namespace frc {
+
+/**
+ * A class representing a coordinate system axis within the NWU coordinate
+ * system.
+ */
+class WPILIB_DLLEXPORT CoordinateAxis {
+ public:
+  /**
+   * Constructs a coordinate system axis within the NWU coordinate system and
+   * normalizes it.
+   *
+   * @param x The x component.
+   * @param y The y component.
+   * @param z The z component.
+   */
+  CoordinateAxis(double x, double y, double z);
+
+  CoordinateAxis(const CoordinateAxis&) = default;
+  CoordinateAxis& operator=(const CoordinateAxis&) = default;
+
+  CoordinateAxis(CoordinateAxis&&) = default;
+  CoordinateAxis& operator=(CoordinateAxis&&) = default;
+
+  /**
+   * Returns a coordinate axis corresponding to +X in the NWU coordinate system.
+   */
+  static CoordinateAxis N();
+
+  /**
+   * Returns a coordinate axis corresponding to -X in the NWU coordinate system.
+   */
+  static CoordinateAxis S();
+
+  /**
+   * Returns a coordinate axis corresponding to -Y in the NWU coordinate system.
+   */
+  static CoordinateAxis E();
+
+  /**
+   * Returns a coordinate axis corresponding to +Y in the NWU coordinate system.
+   */
+  static CoordinateAxis W();
+
+  /**
+   * Returns a coordinate axis corresponding to +Z in the NWU coordinate system.
+   */
+  static CoordinateAxis U();
+
+  /**
+   * Returns a coordinate axis corresponding to -Z in the NWU coordinate system.
+   */
+  static CoordinateAxis D();
+
+ private:
+  friend class CoordinateSystem;
+
+  Vectord<3> m_axis;
+};
+
+}  // namespace frc

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

@@ -0,0 +1,71 @@
+// 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/EigenCore.h"
+#include "frc/geometry/CoordinateAxis.h"
+#include "frc/geometry/Pose3d.h"
+#include "frc/geometry/Rotation3d.h"
+
+namespace frc {
+
+/**
+ * A helper class that converts Pose3d objects between different standard
+ * coordinate frames.
+ */
+class WPILIB_DLLEXPORT CoordinateSystem {
+ public:
+  /**
+   * Constructs a coordinate system with the given cardinal directions for each
+   * axis.
+   *
+   * @param positiveX The cardinal direction of the positive x-axis.
+   * @param positiveY The cardinal direction of the positive y-axis.
+   * @param positiveZ The cardinal direction of the positive z-axis.
+   * @throws std::domain_error if the coordinate system isn't special orthogonal
+   */
+  CoordinateSystem(const CoordinateAxis& positiveX,
+                   const CoordinateAxis& positiveY,
+                   const CoordinateAxis& positiveZ);
+
+  CoordinateSystem(const CoordinateSystem&) = default;
+  CoordinateSystem& operator=(const CoordinateSystem&) = default;
+  CoordinateSystem(CoordinateSystem&&) = default;
+  CoordinateSystem& operator=(CoordinateSystem&&) = default;
+
+  /**
+   * Returns an instance of the NWU coordinate system.
+   */
+  static CoordinateSystem NWU();
+
+  /**
+   * Returns an instance of the EDN coordinate system.
+   */
+  static CoordinateSystem EDN();
+
+  /**
+   * Returns an instance of the NED coordinate system.
+   */
+  static CoordinateSystem NED();
+
+  /**
+   * Converts the given pose from one coordinate system to another.
+   *
+   * @param pose The pose to convert.
+   * @param from The coordinate system the pose starts in.
+   * @param to The coordinate system to which to convert.
+   * @return The given pose in the desired coordinate system.
+   */
+  static Pose3d Convert(const Pose3d& pose, const CoordinateSystem& from,
+                        const CoordinateSystem& to);
+
+ private:
+  // Rotation from this coordinate system to NWU coordinate system
+  Rotation3d m_rotation;
+};
+
+}  // namespace frc

wpimath/src/test/java/edu/wpi/first/math/geometry/CoordinateSystemTest.java

@@ -0,0 +1,128 @@
+// 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.math.geometry;
+
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+import edu.wpi.first.math.util.Units;
+import org.junit.jupiter.api.Test;
+
+class CoordinateSystemTest {
+  @Test
+  void testEDNtoNWU() {
+    var edn1 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d());
+    var nwu1 =
+        new Pose3d(
+            3.0,
+            -1.0,
+            -2.0,
+            new Rotation3d(-Units.degreesToRadians(90.0), 0.0, -Units.degreesToRadians(90.0)));
+
+    assertEquals(
+        nwu1, CoordinateSystem.convert(edn1, CoordinateSystem.EDN(), CoordinateSystem.NWU()));
+    assertEquals(
+        edn1, CoordinateSystem.convert(nwu1, CoordinateSystem.NWU(), CoordinateSystem.EDN()));
+
+    var edn2 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0));
+    var nwu2 =
+        new Pose3d(
+            3.0,
+            -1.0,
+            -2.0,
+            new Rotation3d(-Units.degreesToRadians(45.0), 0.0, -Units.degreesToRadians(90.0)));
+
+    assertEquals(
+        nwu2, CoordinateSystem.convert(edn2, CoordinateSystem.EDN(), CoordinateSystem.NWU()));
+    assertEquals(
+        edn2, CoordinateSystem.convert(nwu2, CoordinateSystem.NWU(), CoordinateSystem.EDN()));
+
+    var edn3 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0));
+    var nwu3 =
+        new Pose3d(
+            3.0,
+            -1.0,
+            -2.0,
+            new Rotation3d(-Units.degreesToRadians(90.0), 0.0, -Units.degreesToRadians(135.0)));
+
+    assertEquals(
+        nwu3, CoordinateSystem.convert(edn3, CoordinateSystem.EDN(), CoordinateSystem.NWU()));
+    assertEquals(
+        edn3, CoordinateSystem.convert(nwu3, CoordinateSystem.NWU(), CoordinateSystem.EDN()));
+
+    var edn4 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0)));
+    var nwu4 =
+        new Pose3d(
+            3.0,
+            -1.0,
+            -2.0,
+            new Rotation3d(
+                -Units.degreesToRadians(90.0),
+                Units.degreesToRadians(45.0),
+                -Units.degreesToRadians(90.0)));
+
+    assertEquals(
+        nwu4, CoordinateSystem.convert(edn4, CoordinateSystem.EDN(), CoordinateSystem.NWU()));
+    assertEquals(
+        edn4, CoordinateSystem.convert(nwu4, CoordinateSystem.NWU(), CoordinateSystem.EDN()));
+  }
+
+  @Test
+  void testEDNtoNED() {
+    var edn1 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d());
+    var ned1 =
+        new Pose3d(
+            3.0,
+            1.0,
+            2.0,
+            new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(90.0)));
+
+    assertEquals(
+        ned1, CoordinateSystem.convert(edn1, CoordinateSystem.EDN(), CoordinateSystem.NED()));
+    assertEquals(
+        edn1, CoordinateSystem.convert(ned1, CoordinateSystem.NED(), CoordinateSystem.EDN()));
+
+    var edn2 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(Units.degreesToRadians(45.0), 0.0, 0.0));
+    var ned2 =
+        new Pose3d(
+            3.0,
+            1.0,
+            2.0,
+            new Rotation3d(Units.degreesToRadians(135.0), 0.0, Units.degreesToRadians(90.0)));
+
+    assertEquals(
+        ned2, CoordinateSystem.convert(edn2, CoordinateSystem.EDN(), CoordinateSystem.NED()));
+    assertEquals(
+        edn2, CoordinateSystem.convert(ned2, CoordinateSystem.NED(), CoordinateSystem.EDN()));
+
+    var edn3 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, Units.degreesToRadians(45.0), 0.0));
+    var ned3 =
+        new Pose3d(
+            3.0,
+            1.0,
+            2.0,
+            new Rotation3d(Units.degreesToRadians(90.0), 0.0, Units.degreesToRadians(135.0)));
+
+    assertEquals(
+        ned3, CoordinateSystem.convert(edn3, CoordinateSystem.EDN(), CoordinateSystem.NED()));
+    assertEquals(
+        edn3, CoordinateSystem.convert(ned3, CoordinateSystem.NED(), CoordinateSystem.EDN()));
+
+    var edn4 = new Pose3d(1.0, 2.0, 3.0, new Rotation3d(0.0, 0.0, Units.degreesToRadians(45.0)));
+    var ned4 =
+        new Pose3d(
+            3.0,
+            1.0,
+            2.0,
+            new Rotation3d(
+                Units.degreesToRadians(90.0),
+                -Units.degreesToRadians(45.0),
+                Units.degreesToRadians(90.0)));
+
+    assertEquals(
+        ned4, CoordinateSystem.convert(edn4, CoordinateSystem.EDN(), CoordinateSystem.NED()));
+    assertEquals(
+        edn4, CoordinateSystem.convert(ned4, CoordinateSystem.NED(), CoordinateSystem.EDN()));
+  }
+}

wpimath/src/test/native/cpp/geometry/CoordinateSystemTest.cpp

@@ -0,0 +1,77 @@
+// 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/geometry/CoordinateSystem.h"
+#include "frc/geometry/Pose3d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+TEST(CoordinateSystemTest, EDNtoNWU) {
+  Pose3d edn1{1_m, 2_m, 3_m, Rotation3d{}};
+  Pose3d nwu1{3_m, -1_m, -2_m, Rotation3d{-90_deg, 0_deg, -90_deg}};
+
+  EXPECT_EQ(nwu1, CoordinateSystem::Convert(edn1, CoordinateSystem::EDN(),
+                                            CoordinateSystem::NWU()));
+  EXPECT_EQ(edn1, CoordinateSystem::Convert(nwu1, CoordinateSystem::NWU(),
+                                            CoordinateSystem::EDN()));
+
+  Pose3d edn2{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}};
+  Pose3d nwu2{3_m, -1_m, -2_m, Rotation3d{-45_deg, 0_deg, -90_deg}};
+
+  EXPECT_EQ(nwu2, CoordinateSystem::Convert(edn2, CoordinateSystem::EDN(),
+                                            CoordinateSystem::NWU()));
+  EXPECT_EQ(edn2, CoordinateSystem::Convert(nwu2, CoordinateSystem::NWU(),
+                                            CoordinateSystem::EDN()));
+
+  Pose3d edn3{1_m, 2_m, 3_m, Rotation3d{0_deg, 45_deg, 0_deg}};
+  Pose3d nwu3{3_m, -1_m, -2_m, Rotation3d{-90_deg, 0_deg, -135_deg}};
+
+  EXPECT_EQ(nwu3, CoordinateSystem::Convert(edn3, CoordinateSystem::EDN(),
+                                            CoordinateSystem::NWU()));
+  EXPECT_EQ(edn3, CoordinateSystem::Convert(nwu3, CoordinateSystem::NWU(),
+                                            CoordinateSystem::EDN()));
+
+  Pose3d edn4{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}};
+  Pose3d nwu4{3_m, -1_m, -2_m, Rotation3d{-90_deg, 45_deg, -90_deg}};
+
+  EXPECT_EQ(nwu4, CoordinateSystem::Convert(edn4, CoordinateSystem::EDN(),
+                                            CoordinateSystem::NWU()));
+  EXPECT_EQ(edn4, CoordinateSystem::Convert(nwu4, CoordinateSystem::NWU(),
+                                            CoordinateSystem::EDN()));
+}
+
+TEST(CoordinateSystemTest, EDNtoNED) {
+  Pose3d edn1{1_m, 2_m, 3_m, Rotation3d{}};
+  Pose3d ned1{3_m, 1_m, 2_m, Rotation3d{90_deg, 0_deg, 90_deg}};
+
+  EXPECT_EQ(ned1, CoordinateSystem::Convert(edn1, CoordinateSystem::EDN(),
+                                            CoordinateSystem::NED()));
+  EXPECT_EQ(edn1, CoordinateSystem::Convert(ned1, CoordinateSystem::NED(),
+                                            CoordinateSystem::EDN()));
+
+  Pose3d edn2{1_m, 2_m, 3_m, Rotation3d{45_deg, 0_deg, 0_deg}};
+  Pose3d ned2{3_m, 1_m, 2_m, Rotation3d{135_deg, 0_deg, 90_deg}};
+
+  EXPECT_EQ(ned2, CoordinateSystem::Convert(edn2, CoordinateSystem::EDN(),
+                                            CoordinateSystem::NED()));
+  EXPECT_EQ(edn2, CoordinateSystem::Convert(ned2, CoordinateSystem::NED(),
+                                            CoordinateSystem::EDN()));
+
+  Pose3d edn3{1_m, 2_m, 3_m, Rotation3d{0_deg, 45_deg, 0_deg}};
+  Pose3d ned3{3_m, 1_m, 2_m, Rotation3d{90_deg, 0_deg, 135_deg}};
+
+  EXPECT_EQ(ned3, CoordinateSystem::Convert(edn3, CoordinateSystem::EDN(),
+                                            CoordinateSystem::NED()));
+  EXPECT_EQ(edn3, CoordinateSystem::Convert(ned3, CoordinateSystem::NED(),
+                                            CoordinateSystem::EDN()));
+
+  Pose3d edn4{1_m, 2_m, 3_m, Rotation3d{0_deg, 0_deg, 45_deg}};
+  Pose3d ned4{3_m, 1_m, 2_m, Rotation3d{90_deg, -45_deg, 90_deg}};
+
+  EXPECT_EQ(ned4, CoordinateSystem::Convert(edn4, CoordinateSystem::EDN(),
+                                            CoordinateSystem::NED()));
+  EXPECT_EQ(edn4, CoordinateSystem::Convert(ned4, CoordinateSystem::NED(),
+                                            CoordinateSystem::EDN()));
+}