diff --git a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
index 461d9a6f080..b27e974fb86 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/StateSpaceUtil.java
@@ -131,7 +131,9 @@ public static <States extends Num, Outputs extends Num> boolean isDetectable(
    *
    * @param pose A pose to convert to a vector.
    * @return The given pose in vector form, with the third element, theta, in radians.
+   * @deprecated Use {@link Pose2d#toMatrix()} instead.
    */
+  @Deprecated
   public static Matrix<N3, N1> poseToVector(Pose2d pose) {
     return VecBuilder.fill(pose.getX(), pose.getY(), pose.getRotation().getRadians());
   }
@@ -180,7 +182,9 @@ public static <I extends Num> Matrix<I, N1> desaturateInputVector(
    *
    * @param pose A pose to convert to a vector.
    * @return The given pose in as a 4x1 vector of x, y, cos(theta), and sin(theta).
+   * @deprecated Use {@link Pose2d#toMatrix()} instead.
    */
+  @Deprecated
   public static Matrix<N4, N1> poseTo4dVector(Pose2d pose) {
     return VecBuilder.fill(
         pose.getTranslation().getX(),
@@ -194,7 +198,9 @@ public static Matrix<N4, N1> poseTo4dVector(Pose2d pose) {
    *
    * @param pose A pose to convert to a vector.
    * @return The given pose in vector form, with the third element, theta, in radians.
+   * @deprecated Use {@link Pose2d#toMatrix()} instead.
    */
+  @Deprecated
   public static Matrix<N3, N1> poseTo3dVector(Pose2d pose) {
     return VecBuilder.fill(
         pose.getTranslation().getX(),
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
index 89c404fddbd..0705d61ea7f 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose2d.java
@@ -10,9 +10,13 @@
 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;
 import edu.wpi.first.math.geometry.proto.Pose2dProto;
 import edu.wpi.first.math.geometry.struct.Pose2dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
@@ -79,6 +83,20 @@ public Pose2d(Distance x, Distance y, Rotation2d rotation) {
     this(x.in(Meters), y.in(Meters), rotation);
   }
 
+  /**
+   * Constructs a pose with the specified affine transformation matrix.
+   *
+   * @param matrix The affine transformation matrix.
+   * @throws IllegalArgumentException if the affine transformation matrix is invalid.
+   */
+  public Pose2d(Matrix<N3, N3> matrix) {
+    m_translation = new Translation2d(matrix.get(0, 2), matrix.get(1, 2));
+    m_rotation = new Rotation2d(matrix.block(2, 2, 0, 0));
+    if (matrix.get(2, 0) != 0.0 || matrix.get(2, 1) != 0.0 || matrix.get(2, 2) != 1.0) {
+      throw new IllegalArgumentException("Affine transformation matrix is invalid");
+    }
+  }
+
   /**
    * Transforms the pose by the given transformation and returns the new transformed pose.
    *
@@ -295,6 +313,28 @@ public Twist2d log(Pose2d end) {
     return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
   }
 
+  /**
+   * Returns an affine transformation matrix representation of this pose.
+   *
+   * @return An affine transformation matrix representation of this pose.
+   */
+  public Matrix<N3, N3> toMatrix() {
+    var vec = m_translation.toVector();
+    var mat = m_rotation.toMatrix();
+    return MatBuilder.fill(
+        Nat.N3(),
+        Nat.N3(),
+        mat.get(0, 0),
+        mat.get(0, 1),
+        vec.get(0),
+        mat.get(1, 0),
+        mat.get(1, 1),
+        vec.get(1),
+        0.0,
+        0.0,
+        1.0);
+  }
+
   /**
    * Returns the nearest Pose2d from a list of poses. If two or more poses in the list have the same
    * distance from this pose, return the one with the closest rotation component.
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
index ada3e9059db..02c2b9ba4eb 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Pose3d.java
@@ -10,10 +10,14 @@
 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;
 import edu.wpi.first.math.geometry.proto.Pose3dProto;
 import edu.wpi.first.math.geometry.struct.Pose3dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
 import edu.wpi.first.math.jni.Pose3dJNI;
+import edu.wpi.first.math.numbers.N4;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
@@ -79,6 +83,23 @@ public Pose3d(Distance x, Distance y, Distance z, Rotation3d rotation) {
     this(x.in(Meters), y.in(Meters), z.in(Meters), rotation);
   }
 
+  /**
+   * Constructs a pose with the specified affine transformation matrix.
+   *
+   * @param matrix The affine transformation matrix.
+   * @throws IllegalArgumentException if the affine transformation matrix is invalid.
+   */
+  public Pose3d(Matrix<N4, N4> matrix) {
+    m_translation = new Translation3d(matrix.get(0, 3), matrix.get(1, 3), matrix.get(2, 3));
+    m_rotation = new Rotation3d(matrix.block(3, 3, 0, 0));
+    if (matrix.get(3, 0) != 0.0
+        || matrix.get(3, 1) != 0.0
+        || matrix.get(3, 2) != 0.0
+        || matrix.get(3, 3) != 1.0) {
+      throw new IllegalArgumentException("Affine transformation matrix is invalid");
+    }
+  }
+
   /**
    * Constructs a 3D pose from a 2D pose in the X-Y plane.
    *
@@ -326,6 +347,35 @@ public Twist3d log(Pose3d end) {
         resultArray[5]);
   }
 
+  /**
+   * Returns an affine transformation matrix representation of this pose.
+   *
+   * @return An affine transformation matrix representation of this pose.
+   */
+  public Matrix<N4, N4> toMatrix() {
+    var vec = m_translation.toVector();
+    var mat = m_rotation.toMatrix();
+    return MatBuilder.fill(
+        Nat.N4(),
+        Nat.N4(),
+        mat.get(0, 0),
+        mat.get(0, 1),
+        mat.get(0, 2),
+        vec.get(0),
+        mat.get(1, 0),
+        mat.get(1, 1),
+        mat.get(1, 2),
+        vec.get(1),
+        mat.get(2, 0),
+        mat.get(2, 1),
+        mat.get(2, 2),
+        vec.get(2),
+        0.0,
+        0.0,
+        0.0,
+        1.0);
+  }
+
   /**
    * Returns a Pose2d representing this Pose3d projected into the X-Y plane.
    *
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
index 49cf4c3c7d0..693a7f2bfc2 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation2d.java
@@ -10,11 +10,15 @@
 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;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.geometry.proto.Rotation2dProto;
 import edu.wpi.first.math.geometry.struct.Rotation2dStruct;
 import edu.wpi.first.math.interpolation.Interpolatable;
+import edu.wpi.first.math.numbers.N2;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.units.measure.Angle;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
@@ -133,6 +137,39 @@ public Rotation2d(Angle angle) {
     this(angle.in(Radians));
   }
 
+  /**
+   * Constructs a Rotation2d from a rotation matrix.
+   *
+   * @param rotationMatrix The rotation matrix.
+   * @throws IllegalArgumentException if the rotation matrix isn't special orthogonal.
+   */
+  public Rotation2d(Matrix<N2, N2> rotationMatrix) {
+    final var R = rotationMatrix;
+
+    // Require that the rotation matrix is special orthogonal. This is true if
+    // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
+    if (R.times(R.transpose()).minus(Matrix.eye(Nat.N2())).normF() > 1e-9) {
+      var msg = "Rotation matrix isn't orthogonal\n\nR =\n" + R.getStorage().toString() + '\n';
+      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
+      throw new IllegalArgumentException(msg);
+    }
+    if (Math.abs(R.det() - 1.0) > 1e-9) {
+      var msg =
+          "Rotation matrix is orthogonal but not special orthogonal\n\nR =\n"
+              + R.getStorage().toString()
+              + '\n';
+      MathSharedStore.reportError(msg, Thread.currentThread().getStackTrace());
+      throw new IllegalArgumentException(msg);
+    }
+
+    // R = [cosθ  −sinθ]
+    //     [sinθ   cosθ]
+    m_cos = R.get(0, 0);
+    m_sin = R.get(1, 0);
+
+    m_value = Math.atan2(m_sin, m_cos);
+  }
+
   /**
    * Constructs and returns a Rotation2d with the given radian value.
    *
@@ -238,6 +275,17 @@ public Rotation2d rotateBy(Rotation2d other) {
         m_cos * other.m_cos - m_sin * other.m_sin, m_cos * other.m_sin + m_sin * other.m_cos);
   }
 
+  /**
+   * Returns matrix representation of this rotation.
+   *
+   * @return Matrix representation of this rotation.
+   */
+  public Matrix<N2, N2> toMatrix() {
+    // R = [cosθ  −sinθ]
+    //     [sinθ   cosθ]
+    return MatBuilder.fill(Nat.N2(), Nat.N2(), m_cos, -m_sin, m_sin, m_cos);
+  }
+
   /**
    * Returns the measure of the Rotation2d.
    *
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
index 8883e665389..7d134774957 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Rotation3d.java
@@ -10,6 +10,7 @@
 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;
 import edu.wpi.first.math.Matrix;
@@ -458,6 +459,32 @@ public Angle getMeasureZ() {
     return Radians.of(getZ());
   }
 
+  /**
+   * Returns rotation matrix representation of this rotation.
+   *
+   * @return Rotation matrix representation of this rotation.
+   */
+  public Matrix<N3, N3> toMatrix() {
+    double w = m_q.getW();
+    double x = m_q.getX();
+    double y = m_q.getY();
+    double z = m_q.getZ();
+
+    // https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix
+    return MatBuilder.fill(
+        Nat.N3(),
+        Nat.N3(),
+        1.0 - 2.0 * (y * y + z * z),
+        2.0 * (x * y - w * z),
+        2.0 * (x * z + w * y),
+        2.0 * (x * y + w * z),
+        1.0 - 2.0 * (x * x + z * z),
+        2.0 * (y * z - w * x),
+        2.0 * (x * z - w * y),
+        2.0 * (y * z + w * x),
+        1.0 - 2.0 * (x * x + y * y));
+  }
+
   /**
    * Returns the axis in the axis-angle representation of this rotation.
    *
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
index 79dc16939e9..7a4cf8f4f9a 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform2d.java
@@ -6,8 +6,12 @@
 
 import static edu.wpi.first.units.Units.Meters;
 
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.geometry.proto.Transform2dProto;
 import edu.wpi.first.math.geometry.struct.Transform2dStruct;
+import edu.wpi.first.math.numbers.N3;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
@@ -78,6 +82,20 @@ public Transform2d(Distance x, Distance y, Rotation2d rotation) {
     this(x.in(Meters), y.in(Meters), rotation);
   }
 
+  /**
+   * Constructs a transform with the specified affine transformation matrix.
+   *
+   * @param matrix The affine transformation matrix.
+   * @throws IllegalArgumentException if the affine transformation matrix is invalid.
+   */
+  public Transform2d(Matrix<N3, N3> matrix) {
+    m_translation = new Translation2d(matrix.get(0, 2), matrix.get(1, 2));
+    m_rotation = new Rotation2d(matrix.block(2, 2, 0, 0));
+    if (matrix.get(2, 0) != 0.0 || matrix.get(2, 1) != 0.0 || matrix.get(2, 2) != 1.0) {
+      throw new IllegalArgumentException("Affine transformation matrix is invalid");
+    }
+  }
+
   /** Constructs the identity transform -- maps an initial pose to itself. */
   public Transform2d() {
     m_translation = Translation2d.kZero;
@@ -160,6 +178,28 @@ public Distance getMeasureY() {
     return m_translation.getMeasureY();
   }
 
+  /**
+   * Returns an affine transformation matrix representation of this transformation.
+   *
+   * @return An affine transformation matrix representation of this transformation.
+   */
+  public Matrix<N3, N3> toMatrix() {
+    var vec = m_translation.toVector();
+    var mat = m_rotation.toMatrix();
+    return MatBuilder.fill(
+        Nat.N3(),
+        Nat.N3(),
+        mat.get(0, 0),
+        mat.get(0, 1),
+        vec.get(0),
+        mat.get(1, 0),
+        mat.get(1, 1),
+        vec.get(1),
+        0.0,
+        0.0,
+        1.0);
+  }
+
   /**
    * Returns the rotational component of the transformation.
    *
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
index 8332b607c20..d993d861001 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Transform3d.java
@@ -6,8 +6,12 @@
 
 import static edu.wpi.first.units.Units.Meters;
 
+import edu.wpi.first.math.MatBuilder;
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.Nat;
 import edu.wpi.first.math.geometry.proto.Transform3dProto;
 import edu.wpi.first.math.geometry.struct.Transform3dStruct;
+import edu.wpi.first.math.numbers.N4;
 import edu.wpi.first.units.measure.Distance;
 import edu.wpi.first.util.protobuf.ProtobufSerializable;
 import edu.wpi.first.util.struct.StructSerializable;
@@ -80,6 +84,23 @@ public Transform3d(Distance x, Distance y, Distance z, Rotation3d rotation) {
     this(x.in(Meters), y.in(Meters), z.in(Meters), rotation);
   }
 
+  /**
+   * Constructs a transform with the specified affine transformation matrix.
+   *
+   * @param matrix The affine transformation matrix.
+   * @throws IllegalArgumentException if the affine transformation matrix is invalid.
+   */
+  public Transform3d(Matrix<N4, N4> matrix) {
+    m_translation = new Translation3d(matrix.get(0, 3), matrix.get(1, 3), matrix.get(2, 3));
+    m_rotation = new Rotation3d(matrix.block(3, 3, 0, 0));
+    if (matrix.get(3, 0) != 0.0
+        || matrix.get(3, 1) != 0.0
+        || matrix.get(3, 2) != 0.0
+        || matrix.get(3, 3) != 1.0) {
+      throw new IllegalArgumentException("Affine transformation matrix is invalid");
+    }
+  }
+
   /** Constructs the identity transform -- maps an initial pose to itself. */
   public Transform3d() {
     m_translation = Translation3d.kZero;
@@ -192,6 +213,35 @@ public Distance getMeasureZ() {
     return m_translation.getMeasureZ();
   }
 
+  /**
+   * Returns an affine transformation matrix representation of this transformation.
+   *
+   * @return An affine transformation matrix representation of this transformation.
+   */
+  public Matrix<N4, N4> toMatrix() {
+    var vec = m_translation.toVector();
+    var mat = m_rotation.toMatrix();
+    return MatBuilder.fill(
+        Nat.N4(),
+        Nat.N4(),
+        mat.get(0, 0),
+        mat.get(0, 1),
+        mat.get(0, 2),
+        vec.get(0),
+        mat.get(1, 0),
+        mat.get(1, 1),
+        mat.get(1, 2),
+        vec.get(1),
+        mat.get(2, 0),
+        mat.get(2, 1),
+        mat.get(2, 2),
+        vec.get(2),
+        0.0,
+        0.0,
+        0.0,
+        1.0);
+  }
+
   /**
    * Returns the rotational component of the transformation.
    *
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
index e3857eb14e6..9b4af815c72 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation2d.java
@@ -88,10 +88,10 @@ public Translation2d(Distance x, Distance y) {
   }
 
   /**
-   * Constructs a Translation2d from the provided translation vector's X and Y components. The
-   * values are assumed to be in meters.
+   * Constructs a Translation2d from a 2D translation vector. The values are assumed to be in
+   * meters.
    *
-   * @param vector The translation vector to represent.
+   * @param vector The translation vector.
    */
   public Translation2d(Vector<N2> vector) {
     this(vector.get(0), vector.get(1));
@@ -148,9 +148,9 @@ public Distance getMeasureY() {
   }
 
   /**
-   * Returns a vector representation of this translation.
+   * Returns a 2D translation vector representation of this translation.
    *
-   * @return A Vector representation of this translation.
+   * @return A 2D translation vector representation of this translation.
    */
   public Vector<N2> toVector() {
     return VecBuilder.fill(m_x, m_y);
diff --git a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
index 48a70c7f45a..20cede3d3da 100644
--- a/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
+++ b/wpimath/src/main/java/edu/wpi/first/math/geometry/Translation3d.java
@@ -104,10 +104,10 @@ public Translation3d(Translation2d translation) {
   }
 
   /**
-   * Constructs a Translation3d from the provided translation vector's X, Y, and Z components. The
-   * values are assumed to be in meters.
+   * Constructs a Translation3d from a 3D translation vector. The values are assumed to be in
+   * meters.
    *
-   * @param vector The translation vector to represent.
+   * @param vector The translation vector.
    */
   public Translation3d(Vector<N3> vector) {
     this(vector.get(0), vector.get(1), vector.get(2));
@@ -184,9 +184,9 @@ public Distance getMeasureZ() {
   }
 
   /**
-   * Returns a vector representation of this translation.
+   * Returns a 2D translation vector representation of this translation.
    *
-   * @return A Vector representation of this translation.
+   * @return A 2D translation vector representation of this translation.
    */
   public Vector<N3> toVector() {
     return VecBuilder.fill(m_x, m_y, m_z);
diff --git a/wpimath/src/main/native/include/frc/StateSpaceUtil.h b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
index 785255d6748..f5435545232 100644
--- a/wpimath/src/main/native/include/frc/StateSpaceUtil.h
+++ b/wpimath/src/main/native/include/frc/StateSpaceUtil.h
@@ -206,9 +206,10 @@ Vectord<N> MakeWhiteNoiseVector(const std::array<double, N>& stdDevs) {
  * @param pose The pose that is being represented.
  *
  * @return The vector.
+ * @deprecated Use Pose2d.ToMatrix() instead.
  */
-WPILIB_DLLEXPORT
-constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) {
+[[deprecated("Use Pose2d.ToMatrix() instead.")]]
+WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) {
   return Eigen::Vector3d{{pose.Translation().X().value(),
                           pose.Translation().Y().value(),
                           pose.Rotation().Radians().value()}};
@@ -220,9 +221,10 @@ constexpr Eigen::Vector3d PoseTo3dVector(const Pose2d& pose) {
  * @param pose The pose that is being represented.
  *
  * @return The vector.
+ * @deprecated Use Pose2d.ToMatrix() instead.
  */
-WPILIB_DLLEXPORT
-constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) {
+[[deprecated("Use Pose2d.ToMatrix() instead.")]]
+WPILIB_DLLEXPORT constexpr Eigen::Vector4d PoseTo4dVector(const Pose2d& pose) {
   return Eigen::Vector4d{{pose.Translation().X().value(),
                           pose.Translation().Y().value(), pose.Rotation().Cos(),
                           pose.Rotation().Sin()}};
@@ -311,9 +313,10 @@ bool IsDetectable(const Matrixd<States, States>& A,
  * @param pose The pose that is being represented.
  *
  * @return The vector.
+ * @deprecated Use Pose2d.ToMatrix() instead.
  */
-WPILIB_DLLEXPORT
-constexpr Eigen::Vector3d PoseToVector(const Pose2d& pose) {
+[[deprecated("Use Pose2d.ToMatrix() instead.")]]
+WPILIB_DLLEXPORT constexpr Eigen::Vector3d PoseToVector(const Pose2d& pose) {
   return Eigen::Vector3d{
       {pose.X().value(), pose.Y().value(), pose.Rotation().Radians().value()}};
 }
diff --git a/wpimath/src/main/native/include/frc/ct_matrix.h b/wpimath/src/main/native/include/frc/ct_matrix.h
index 7b6f9056e7c..ce339db276e 100644
--- a/wpimath/src/main/native/include/frc/ct_matrix.h
+++ b/wpimath/src/main/native/include/frc/ct_matrix.h
@@ -308,6 +308,23 @@ class ct_matrix {
                             (*this)(0) * rhs(1) - rhs(0) * (*this)(1)}};
   }
 
+  /**
+   * Constexpr version of Eigen's 2x2 matrix determinant member function.
+   *
+   * @return Determinant of matrix.
+   */
+  constexpr Scalar determinant() const
+    requires(Rows == 2 && Cols == 2)
+  {
+    // |a  b|
+    // |c  d| = ad - bc
+    Scalar a = (*this)(0, 0);
+    Scalar b = (*this)(0, 1);
+    Scalar c = (*this)(1, 0);
+    Scalar d = (*this)(1, 1);
+    return a * d - b * c;
+  }
+
   /**
    * Constexpr version of Eigen's 3x3 matrix determinant member function.
    *
@@ -364,7 +381,9 @@ using ct_vector = ct_matrix<Scalar, Rows, 1>;
 template <typename Scalar, int Cols>
 using ct_row_vector = ct_matrix<Scalar, 1, Cols>;
 
+using ct_matrix2d = ct_matrix<double, 2, 2>;
 using ct_matrix3d = ct_matrix<double, 3, 3>;
+using ct_vector2d = ct_vector<double, 2>;
 using ct_vector3d = ct_vector<double, 3>;
 
 }  // namespace frc
diff --git a/wpimath/src/main/native/include/frc/geometry/Pose2d.h b/wpimath/src/main/native/include/frc/geometry/Pose2d.h
index 016de906245..0664161d6d3 100644
--- a/wpimath/src/main/native/include/frc/geometry/Pose2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Pose2d.h
@@ -53,6 +53,21 @@ class WPILIB_DLLEXPORT Pose2d {
   constexpr Pose2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
       : m_translation{x, y}, m_rotation{std::move(rotation)} {}
 
+  /**
+   * Constructs a pose with the specified affine transformation matrix.
+   *
+   * @param matrix The affine transformation matrix.
+   * @throws std::domain_error if the affine transformation matrix is invalid.
+   */
+  constexpr explicit Pose2d(const Eigen::Matrix3d& matrix)
+      : m_translation{Eigen::Vector2d{{matrix(0, 2)}, {matrix(1, 2)}}},
+        m_rotation{Eigen::Matrix2d{{matrix(0, 0), matrix(0, 1)},
+                                   {matrix(1, 0), matrix(1, 1)}}} {
+    if (matrix(2, 0) != 0.0 || matrix(2, 1) != 0.0 || matrix(2, 2) != 1.0) {
+      throw std::domain_error("Affine transformation matrix is invalid");
+    }
+  }
+
   /**
    * Transforms the pose by the given transformation and returns the new
    * transformed pose.
@@ -202,6 +217,17 @@ class WPILIB_DLLEXPORT Pose2d {
    */
   constexpr Twist2d Log(const Pose2d& end) const;
 
+  /**
+   * Returns an affine transformation matrix representation of this pose.
+   */
+  constexpr Eigen::Matrix3d ToMatrix() const {
+    auto vec = m_translation.ToVector();
+    auto mat = m_rotation.ToMatrix();
+    return Eigen::Matrix3d{{mat(0, 0), mat(0, 1), vec(0)},
+                           {mat(1, 0), mat(1, 1), vec(1)},
+                           {0.0, 0.0, 1.0}};
+  }
+
   /**
    * Returns the nearest Pose2d from a collection of poses
    * @param poses The collection of poses.
diff --git a/wpimath/src/main/native/include/frc/geometry/Pose3d.h b/wpimath/src/main/native/include/frc/geometry/Pose3d.h
index dc6a7416fd0..1f377a8ce71 100644
--- a/wpimath/src/main/native/include/frc/geometry/Pose3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Pose3d.h
@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <stdexcept>
 #include <type_traits>
 #include <utility>
 
@@ -54,6 +55,25 @@ class WPILIB_DLLEXPORT Pose3d {
                    Rotation3d rotation)
       : m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
 
+  /**
+   * Constructs a pose with the specified affine transformation matrix.
+   *
+   * @param matrix The affine transformation matrix.
+   * @throws std::domain_error if the affine transformation matrix is invalid.
+   */
+  constexpr explicit Pose3d(const Eigen::Matrix4d& matrix)
+      : m_translation{Eigen::Vector3d{
+            {matrix(0, 3)}, {matrix(1, 3)}, {matrix(2, 3)}}},
+        m_rotation{
+            Eigen::Matrix3d{{matrix(0, 0), matrix(0, 1), matrix(0, 2)},
+                            {matrix(1, 0), matrix(1, 1), matrix(1, 2)},
+                            {matrix(2, 0), matrix(2, 1), matrix(2, 2)}}} {
+    if (matrix(3, 0) != 0.0 || matrix(3, 1) != 0.0 || matrix(3, 2) != 0.0 ||
+        matrix(3, 3) != 1.0) {
+      throw std::domain_error("Affine transformation matrix is invalid");
+    }
+  }
+
   /**
    * Constructs a 3D pose from a 2D pose in the X-Y plane.
    *
@@ -218,6 +238,18 @@ class WPILIB_DLLEXPORT Pose3d {
    */
   constexpr Twist3d Log(const Pose3d& end) const;
 
+  /**
+   * Returns an affine transformation matrix representation of this pose.
+   */
+  constexpr Eigen::Matrix4d ToMatrix() const {
+    auto vec = m_translation.ToVector();
+    auto mat = m_rotation.ToMatrix();
+    return Eigen::Matrix4d{{mat(0, 0), mat(0, 1), mat(0, 2), vec(0)},
+                           {mat(1, 0), mat(1, 1), mat(1, 2), vec(1)},
+                           {mat(2, 0), mat(2, 1), mat(2, 2), vec(2)},
+                           {0.0, 0.0, 0.0, 1.0}};
+  }
+
   /**
    * Returns a Pose2d representing this Pose3d projected into the X-Y plane.
    */
diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
index a8899304f23..d2500a9a21c 100644
--- a/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation2d.h
@@ -5,12 +5,16 @@
 #pragma once
 
 #include <type_traits>
+#include <utility>
 
+#include <Eigen/Core>
+#include <Eigen/LU>
 #include <gcem.hpp>
 #include <wpi/StackTrace.h>
 #include <wpi/SymbolExports.h>
 #include <wpi/json_fwd.h>
 
+#include "frc/ct_matrix.h"
 #include "units/angle.h"
 #include "wpimath/MathShared.h"
 
@@ -66,6 +70,41 @@ class WPILIB_DLLEXPORT Rotation2d {
     m_value = units::radian_t{gcem::atan2(m_sin, m_cos)};
   }
 
+  /**
+   * Constructs a Rotation2d from a rotation matrix.
+   *
+   * @param rotationMatrix The rotation matrix.
+   * @throws std::domain_error if the rotation matrix isn't special orthogonal.
+   */
+  constexpr explicit Rotation2d(const Eigen::Matrix2d& rotationMatrix) {
+    auto impl =
+        []<typename Matrix2d>(const Matrix2d& R) -> std::pair<double, double> {
+      // Require that the rotation matrix is special orthogonal. This is true if
+      // the matrix is orthogonal (RRᵀ = I) and normalized (determinant is 1).
+      if ((R * R.transpose() - Matrix2d::Identity()).norm() > 1e-9) {
+        throw std::domain_error("Rotation matrix isn't orthogonal");
+      }
+      if (gcem::abs(R.determinant() - 1.0) > 1e-9) {
+        throw std::domain_error(
+            "Rotation matrix is orthogonal but not special orthogonal");
+      }
+
+      // R = [cosθ  −sinθ]
+      //     [sinθ   cosθ]
+      return {R(0, 0), R(1, 0)};
+    };
+
+    if (std::is_constant_evaluated()) {
+      auto cossin = impl(ct_matrix2d{rotationMatrix});
+      m_cos = std::get<0>(cossin);
+      m_sin = std::get<1>(cossin);
+    } else {
+      auto cossin = impl(rotationMatrix);
+      m_cos = std::get<0>(cossin);
+      m_sin = std::get<1>(cossin);
+    }
+  }
+
   /**
    * Adds two rotations together, with the result being bounded between -π and
    * π.
@@ -154,6 +193,15 @@ class WPILIB_DLLEXPORT Rotation2d {
             Cos() * other.Sin() + Sin() * other.Cos()};
   }
 
+  /**
+   * Returns matrix representation of this rotation.
+   */
+  constexpr Eigen::Matrix2d ToMatrix() const {
+    // R = [cosθ  −sinθ]
+    //     [sinθ   cosθ]
+    return Eigen::Matrix2d{{m_cos, -m_sin}, {m_sin, m_cos}};
+  }
+
   /**
    * Returns the radian value of the rotation.
    *
diff --git a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
index f27d1d94131..47aa165b7c4 100644
--- a/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Rotation3d.h
@@ -403,6 +403,24 @@ class WPILIB_DLLEXPORT Rotation3d {
     return units::radian_t{2.0 * gcem::atan2(norm, m_q.W())};
   }
 
+  /**
+   * Returns rotation matrix representation of this rotation.
+   */
+  constexpr Eigen::Matrix3d ToMatrix() const {
+    double w = m_q.W();
+    double x = m_q.X();
+    double y = m_q.Y();
+    double z = m_q.Z();
+
+    // https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation#Quaternion-derived_rotation_matrix
+    return Eigen::Matrix3d{{1.0 - 2.0 * (y * y + z * z), 2.0 * (x * y - w * z),
+                            2.0 * (x * z + w * y)},
+                           {2.0 * (x * y + w * z), 1.0 - 2.0 * (x * x + z * z),
+                            2.0 * (y * z - w * x)},
+                           {2.0 * (x * z - w * y), 2.0 * (y * z + w * x),
+                            1.0 - 2.0 * (x * x + y * y)}};
+  }
+
   /**
    * Returns a Rotation2d representing this Rotation3d projected into the X-Y
    * plane.
diff --git a/wpimath/src/main/native/include/frc/geometry/Transform2d.h b/wpimath/src/main/native/include/frc/geometry/Transform2d.h
index abf5c1ff4de..88c00550d04 100644
--- a/wpimath/src/main/native/include/frc/geometry/Transform2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Transform2d.h
@@ -49,6 +49,21 @@ class WPILIB_DLLEXPORT Transform2d {
   constexpr Transform2d(units::meter_t x, units::meter_t y, Rotation2d rotation)
       : m_translation{x, y}, m_rotation{std::move(rotation)} {}
 
+  /**
+   * Constructs a pose with the specified affine transformation matrix.
+   *
+   * @param matrix The affine transformation matrix.
+   * @throws std::domain_error if the affine transformation matrix is invalid.
+   */
+  constexpr explicit Transform2d(const Eigen::Matrix3d& matrix)
+      : m_translation{Eigen::Vector2d{{matrix(0, 2)}, {matrix(1, 2)}}},
+        m_rotation{Eigen::Matrix2d{{matrix(0, 0), matrix(0, 1)},
+                                   {matrix(1, 0), matrix(1, 1)}}} {
+    if (matrix(2, 0) != 0.0 || matrix(2, 1) != 0.0 || matrix(2, 2) != 1.0) {
+      throw std::domain_error("Affine transformation matrix is invalid");
+    }
+  }
+
   /**
    * Constructs the identity transform -- maps an initial pose to itself.
    */
@@ -75,6 +90,18 @@ class WPILIB_DLLEXPORT Transform2d {
    */
   constexpr units::meter_t Y() const { return m_translation.Y(); }
 
+  /**
+   * Returns an affine transformation matrix representation of this
+   * transformation.
+   */
+  constexpr Eigen::Matrix3d ToMatrix() const {
+    auto vec = m_translation.ToVector();
+    auto mat = m_rotation.ToMatrix();
+    return Eigen::Matrix3d{{mat(0, 0), mat(0, 1), vec(0)},
+                           {mat(1, 0), mat(1, 1), vec(1)},
+                           {0.0, 0.0, 1.0}};
+  }
+
   /**
    * Returns the rotational component of the transformation.
    *
diff --git a/wpimath/src/main/native/include/frc/geometry/Transform3d.h b/wpimath/src/main/native/include/frc/geometry/Transform3d.h
index 4716b7181e5..bd6ca9c41e0 100644
--- a/wpimath/src/main/native/include/frc/geometry/Transform3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Transform3d.h
@@ -51,6 +51,25 @@ class WPILIB_DLLEXPORT Transform3d {
                         Rotation3d rotation)
       : m_translation{x, y, z}, m_rotation{std::move(rotation)} {}
 
+  /**
+   * Constructs a transform with the specified affine transformation matrix.
+   *
+   * @param matrix The affine transformation matrix.
+   * @throws std::domain_error if the affine transformation matrix is invalid.
+   */
+  constexpr explicit Transform3d(const Eigen::Matrix4d& matrix)
+      : m_translation{Eigen::Vector3d{
+            {matrix(0, 3)}, {matrix(1, 3)}, {matrix(2, 3)}}},
+        m_rotation{
+            Eigen::Matrix3d{{matrix(0, 0), matrix(0, 1), matrix(0, 2)},
+                            {matrix(1, 0), matrix(1, 1), matrix(1, 2)},
+                            {matrix(2, 0), matrix(2, 1), matrix(2, 2)}}} {
+    if (matrix(3, 0) != 0.0 || matrix(3, 1) != 0.0 || matrix(3, 2) != 0.0 ||
+        matrix(3, 3) != 1.0) {
+      throw std::domain_error("Affine transformation matrix is invalid");
+    }
+  }
+
   /**
    * Constructs the identity transform -- maps an initial pose to itself.
    */
@@ -95,6 +114,19 @@ class WPILIB_DLLEXPORT Transform3d {
    */
   constexpr units::meter_t Z() const { return m_translation.Z(); }
 
+  /**
+   * Returns an affine transformation matrix representation of this
+   * transformation.
+   */
+  constexpr Eigen::Matrix4d ToMatrix() const {
+    auto vec = m_translation.ToVector();
+    auto mat = m_rotation.ToMatrix();
+    return Eigen::Matrix4d{{mat(0, 0), mat(0, 1), mat(0, 2), vec(0)},
+                           {mat(1, 0), mat(1, 1), mat(1, 2), vec(1)},
+                           {mat(2, 0), mat(2, 1), mat(2, 2), vec(2)},
+                           {0.0, 0.0, 0.0, 1.0}};
+  }
+
   /**
    * Returns the rotational component of the transformation.
    *
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation2d.h b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
index a8a26b8bff5..aa6c4753ef7 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation2d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation2d.h
@@ -54,13 +54,13 @@ class WPILIB_DLLEXPORT Translation2d {
       : m_x{distance * angle.Cos()}, m_y{distance * angle.Sin()} {}
 
   /**
-   * Constructs a Translation2d from the provided translation vector's X and Y
-   * components. The values are assumed to be in meters.
+   * Constructs a Translation2d from a 2D translation vector. The values are
+   * assumed to be in meters.
    *
-   * @param vector The translation vector to represent.
+   * @param vector The translation vector.
    */
   constexpr explicit Translation2d(const Eigen::Vector2d& vector)
-      : m_x{units::meter_t{vector(0)}}, m_y{units::meter_t{vector(1)}} {}
+      : m_x{units::meter_t{vector.x()}}, m_y{units::meter_t{vector.y()}} {}
 
   /**
    * Calculates the distance between two translations in 2D space.
@@ -90,9 +90,9 @@ class WPILIB_DLLEXPORT Translation2d {
   constexpr units::meter_t Y() const { return m_y; }
 
   /**
-   * Returns a vector representation of this translation.
+   * Returns a 2D translation vector representation of this translation.
    *
-   * @return A Vector representation of this translation.
+   * @return A 2D translation vector representation of this translation.
    */
   constexpr Eigen::Vector2d ToVector() const {
     return Eigen::Vector2d{{m_x.value(), m_y.value()}};
diff --git a/wpimath/src/main/native/include/frc/geometry/Translation3d.h b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
index fb3194b8ced..c16fc56a5b8 100644
--- a/wpimath/src/main/native/include/frc/geometry/Translation3d.h
+++ b/wpimath/src/main/native/include/frc/geometry/Translation3d.h
@@ -56,10 +56,10 @@ class WPILIB_DLLEXPORT Translation3d {
   }
 
   /**
-   * Constructs a Translation3d from the provided translation vector's X, Y, and
-   * Z components. The values are assumed to be in meters.
+   * Constructs a Translation3d from a 3D translation vector. The values are
+   * assumed to be in meters.
    *
-   * @param vector The translation vector to represent.
+   * @param vector The translation vector.
    */
   constexpr explicit Translation3d(const Eigen::Vector3d& vector)
       : m_x{units::meter_t{vector.x()}},
@@ -114,9 +114,9 @@ class WPILIB_DLLEXPORT Translation3d {
   constexpr units::meter_t Z() const { return m_z; }
 
   /**
-   * Returns a vector representation of this translation.
+   * Returns a 3D translation vector representation of this translation.
    *
-   * @return A Vector representation of this translation.
+   * @return A 3D translation vector representation of this translation.
    */
   constexpr Eigen::Vector3d ToVector() const {
     return Eigen::Vector3d{{m_x.value(), m_y.value(), m_z.value()}};
diff --git a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
index 4883e6f5ce0..f9333406386 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/StateSpaceUtilTest.java
@@ -153,6 +153,7 @@ void testMatrixExp() {
   }
 
   @Test
+  @SuppressWarnings("deprecation")
   void testPoseToVector() {
     Pose2d pose = new Pose2d(1, 2, new Rotation2d(3));
     var vector = StateSpaceUtil.poseToVector(pose);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
index 0844e1321e9..81f81509929 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose2dTest.java
@@ -86,6 +86,14 @@ void testInequality() {
     assertNotEquals(one, two);
   }
 
+  @Test
+  void testToMatrix() {
+    var before = new Pose2d(1.0, 2.0, Rotation2d.fromDegrees(20.0));
+    var after = new Pose2d(before.toMatrix());
+
+    assertEquals(before, after);
+  }
+
   @Test
   void testMinus() {
     var initial = new Pose2d(0.0, 0.0, Rotation2d.fromDegrees(45.0));
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
index b333d24592f..00c63eae080 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Pose3dTest.java
@@ -169,6 +169,22 @@ void testMinus() {
         () -> assertEquals(0.0, transform.getRotation().getZ(), kEpsilon));
   }
 
+  @Test
+  void testToMatrix() {
+    var before =
+        new Pose3d(
+            1.0,
+            2.0,
+            3.0,
+            new Rotation3d(
+                Units.degreesToRadians(20.0),
+                Units.degreesToRadians(30.0),
+                Units.degreesToRadians(40.0)));
+    var after = new Pose3d(before.toMatrix());
+
+    assertEquals(before, after);
+  }
+
   @Test
   void testToPose2d() {
     var pose =
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
index b75182edb7e..6eaa2a3f98c 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation2dTest.java
@@ -100,6 +100,14 @@ void testInequality() {
     assertNotEquals(rot1, rot2);
   }
 
+  @Test
+  void testToMatrix() {
+    var before = Rotation2d.fromDegrees(20.0);
+    var after = new Rotation2d(before.toMatrix());
+
+    assertEquals(before, after);
+  }
+
   @Test
   void testInterpolate() {
     // 50 + (70 - 50) * 0.5 = 60
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
index 43b7e9b4c5b..3e96a1cb3df 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Rotation3dTest.java
@@ -344,6 +344,18 @@ void testInequality() {
     assertNotEquals(rot1, rot2);
   }
 
+  @Test
+  void testToMatrix() {
+    var before =
+        new Rotation3d(
+            Units.degreesToRadians(10.0),
+            Units.degreesToRadians(20.0),
+            Units.degreesToRadians(30.0));
+    var after = new Rotation3d(before.toMatrix());
+
+    assertEquals(before, after);
+  }
+
   @Test
   void testInterpolate() {
     final var xAxis = VecBuilder.fill(1.0, 0.0, 0.0);
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java
index 5817f4e2825..19d4b4f396f 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform2dTest.java
@@ -22,6 +22,14 @@ void testNewWithMeasures() {
     assertEquals(Math.PI / 4, transform.getRotation().getRadians(), kEpsilon);
   }
 
+  @Test
+  void testToMatrix() {
+    var before = new Transform2d(1.0, 2.0, Rotation2d.fromDegrees(20.0));
+    var after = new Transform2d(before.toMatrix());
+
+    assertEquals(before, after);
+  }
+
   @Test
   void testInverse() {
     var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
diff --git a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java
index 51d5f071fc3..dc251512fd1 100644
--- a/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java
+++ b/wpimath/src/test/java/edu/wpi/first/math/geometry/Transform3dTest.java
@@ -11,6 +11,22 @@
 import org.junit.jupiter.api.Test;
 
 class Transform3dTest {
+  @Test
+  void testToMatrix() {
+    var before =
+        new Transform3d(
+            1.0,
+            2.0,
+            3.0,
+            new Rotation3d(
+                Units.degreesToRadians(20.0),
+                Units.degreesToRadians(30.0),
+                Units.degreesToRadians(40.0)));
+    var after = new Transform3d(before.toMatrix());
+
+    assertEquals(before, after);
+  }
+
   @Test
   void testInverse() {
     var zAxis = VecBuilder.fill(0.0, 0.0, 1.0);
diff --git a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
index bd5a9aa8a28..9a038590b1a 100644
--- a/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Pose2dTest.cpp
@@ -141,6 +141,13 @@ TEST(Pose2dTest, Nearest) {
                        .value());
 }
 
+TEST(Pose2dTest, ToMatrix) {
+  Pose2d before{1_m, 2_m, 20_deg};
+  Pose2d after{before.ToMatrix()};
+
+  EXPECT_EQ(before, after);
+}
+
 TEST(Pose2dTest, Constexpr) {
   constexpr Pose2d defaultConstructed;
   constexpr Pose2d translationRotation{Translation2d{0_m, 1_m},
diff --git a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
index 88acb1fb897..f0168e95435 100644
--- a/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Pose3dTest.cpp
@@ -112,6 +112,13 @@ TEST(Pose3dTest, Minus) {
   EXPECT_NEAR(0.0, transform.Rotation().Z().value(), 1e-9);
 }
 
+TEST(Pose3dTest, ToMatrix) {
+  Pose3d before{1_m, 2_m, 3_m, Rotation3d{10_deg, 20_deg, 30_deg}};
+  Pose3d after{before.ToMatrix()};
+
+  EXPECT_EQ(before, after);
+}
+
 TEST(Pose3dTest, ToPose2d) {
   Pose3d pose{1_m, 2_m, 3_m, Rotation3d{20_deg, 30_deg, 40_deg}};
   Pose2d expected{1_m, 2_m, 40_deg};
diff --git a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
index 65d3016d18d..00065df514f 100644
--- a/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Rotation2dTest.cpp
@@ -78,6 +78,13 @@ TEST(Rotation2dTest, Inequality) {
   EXPECT_NE(rot1, rot2);
 }
 
+TEST(Rotation2dTest, ToMatrix) {
+  Rotation2d before{20_deg};
+  Rotation2d after{before.ToMatrix()};
+
+  EXPECT_EQ(before, after);
+}
+
 TEST(Rotation2dTest, Constexpr) {
   constexpr Rotation2d defaultCtor;
   constexpr Rotation2d radianCtor{5_rad};
diff --git a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
index 16c033723d1..6088bdd7590 100644
--- a/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Rotation3dTest.cpp
@@ -307,6 +307,13 @@ TEST(Rotation3dTest, Inequality) {
   EXPECT_NE(rot1, rot2);
 }
 
+TEST(Rotation3dTest, ToMatrix) {
+  Rotation3d before{10_deg, 20_deg, 30_deg};
+  Rotation3d after{before.ToMatrix()};
+
+  EXPECT_EQ(before, after);
+}
+
 TEST(Rotation3dTest, Interpolate) {
   const Eigen::Vector3d xAxis{1.0, 0.0, 0.0};
   const Eigen::Vector3d yAxis{0.0, 1.0, 0.0};
diff --git a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
index 49c9466201c..5a562405cc6 100644
--- a/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Transform2dTest.cpp
@@ -13,6 +13,13 @@
 
 using namespace frc;
 
+TEST(Transform2dTest, ToMatrix) {
+  Transform2d before{1_m, 2_m, 20_deg};
+  Transform2d after{before.ToMatrix()};
+
+  EXPECT_EQ(before, after);
+}
+
 TEST(Transform2dTest, Inverse) {
   const Pose2d initial{1_m, 2_m, 45_deg};
   const Transform2d transform{{5_m, 0_m}, 5_deg};
diff --git a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
index 8a6bbc039f0..9aa7eda331e 100644
--- a/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
+++ b/wpimath/src/test/native/cpp/geometry/Transform3dTest.cpp
@@ -13,6 +13,13 @@
 
 using namespace frc;
 
+TEST(Transform3dTest, ToMatrix) {
+  Transform3d before{1_m, 2_m, 3_m, Rotation3d{10_deg, 20_deg, 30_deg}};
+  Transform3d after{before.ToMatrix()};
+
+  EXPECT_EQ(before, after);
+}
+
 TEST(Transform3dTest, Inverse) {
   Eigen::Vector3d zAxis{0.0, 0.0, 1.0};