From d32e60233fe516e8f67e0e94d3de87615e09f00f Mon Sep 17 00:00:00 2001
From: Lucien Morey <lucien@thedropbears.org.au>
Date: Fri, 16 May 2025 11:43:46 +1000
Subject: [PATCH] [wpimath] Add dynamic size support for angle statistics
 (#7964)

---
 .../cpp/estimator/AngleStatisticsTest.cpp     | 31 +++++++++++++++++++
 1 file changed, 31 insertions(+)

diff --git a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
index d62decda61..1c4bdaa58b 100644
--- a/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
+++ b/wpimath/src/test/native/cpp/estimator/AngleStatisticsTest.cpp
@@ -22,6 +22,21 @@ TEST(AngleStatisticsTest, Mean) {
                   .isApprox(frc::AngleMean<3, 1>(sigmas, weights, 1), 1e-3));
 }
 
+TEST(AngleStatisticsTest, Mean_DynamicSize) {
+  Eigen::MatrixXd sigmas{
+      {1, 1.2, 0},
+      {359 * std::numbers::pi / 180, 3 * std::numbers::pi / 180, 0},
+      {1, 2, 0}};
+  // Weights need to produce the mean of the sigmas
+  Eigen::VectorXd weights{3};
+  weights.fill(1.0 / sigmas.cols());
+
+  EXPECT_TRUE(Eigen::Vector3d(0.7333333, 0.01163323, 1)
+                  .isApprox(frc::AngleMean<Eigen::Dynamic, Eigen::Dynamic>(
+                                sigmas, weights, 1),
+                            1e-3));
+}
+
 TEST(AngleStatisticsTest, Residual) {
   Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2};
   Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1};
@@ -30,9 +45,25 @@ TEST(AngleStatisticsTest, Residual) {
       Eigen::Vector3d{0, 2 * std::numbers::pi / 180, 1}));
 }
 
+TEST(AngleStatisticsTest, Residual_DynamicSize) {
+  Eigen::VectorXd a{{1, 1 * std::numbers::pi / 180, 2}};
+  Eigen::VectorXd b{{1, 359 * std::numbers::pi / 180, 1}};
+
+  EXPECT_TRUE(frc::AngleResidual<Eigen::Dynamic>(a, b, 1).isApprox(
+      Eigen::VectorXd{{0, 2 * std::numbers::pi / 180, 1}}));
+}
+
 TEST(AngleStatisticsTest, Add) {
   Eigen::Vector3d a{1, 1 * std::numbers::pi / 180, 2};
   Eigen::Vector3d b{1, 359 * std::numbers::pi / 180, 1};
 
   EXPECT_TRUE(frc::AngleAdd<3>(a, b, 1).isApprox(Eigen::Vector3d{2, 0, 3}));
 }
+
+TEST(AngleStatisticsTest, Add_DynamicSize) {
+  Eigen::VectorXd a{{1, 1 * std::numbers::pi / 180, 2}};
+  Eigen::VectorXd b{{1, 359 * std::numbers::pi / 180, 1}};
+
+  EXPECT_TRUE(frc::AngleAdd<Eigen::Dynamic>(a, b, 1).isApprox(
+      Eigen::VectorXd{{2, 0, 3}}));
+}