average quat function (#23)

* average quat function

* fixed pr feedback

Author: jorgenfj

cpp_test/test_math.cpp

@@ -338,4 +338,97 @@ TEST(anti_windup, test_anti_windup) {
     EXPECT_TRUE(expected.isApprox(result, 1e-10));
 }
 
+TEST(average_quaternions, empty_input_throws) {
+    std::vector<Eigen::Quaterniond> quats;
+    EXPECT_THROW(average_quaternions(quats), std::invalid_argument);
+}
+
+TEST(average_quaternions, single_quaternion_returns_same) {
+    Eigen::Quaterniond q =
+        Eigen::Quaterniond(Eigen::AngleAxisd(0.7, Eigen::Vector3d::UnitX()));
+
+    std::vector<Eigen::Quaterniond> quats{q};
+
+    Eigen::Quaterniond avg = average_quaternions(quats);
+
+    EXPECT_NEAR(avg.angularDistance(q), 0.0, 1e-12);
+}
+
+TEST(average_quaternions, identical_quaternions) {
+    Eigen::Quaterniond q =
+        Eigen::Quaterniond(Eigen::AngleAxisd(1.2, Eigen::Vector3d::UnitZ()));
+
+    std::vector<Eigen::Quaterniond> quats = {q, q, q};
+
+    Eigen::Quaterniond avg = average_quaternions(quats);
+
+    EXPECT_NEAR(avg.angularDistance(q), 0.0, 1e-12);
+}
+
+TEST(average_quaternions, antipodal_quaternions) {
+    Eigen::Quaterniond q(Eigen::AngleAxisd(0.8, Eigen::Vector3d::UnitY()));
+
+    Eigen::Quaterniond q_neg = q;
+    q_neg.coeffs() *= -1.0;
+
+    std::vector<Eigen::Quaterniond> quats = {q, q_neg};
+
+    Eigen::Quaterniond avg = average_quaternions(quats);
+
+    EXPECT_NEAR(avg.angularDistance(q), 0.0, 1e-12);
+}
+
+TEST(average_quaternions, average_of_small_rotations_near_identity) {
+    std::vector<Eigen::Quaterniond> quats;
+
+    quats.emplace_back(Eigen::AngleAxisd(0.05, Eigen::Vector3d::UnitX()));
+    quats.emplace_back(Eigen::AngleAxisd(-0.05, Eigen::Vector3d::UnitX()));
+    quats.emplace_back(Eigen::AngleAxisd(0.02, Eigen::Vector3d::UnitX()));
+
+    Eigen::Quaterniond avg = average_quaternions(quats);
+
+    EXPECT_TRUE(avg.isApprox(Eigen::Quaterniond::Identity(), 1e-2));
+}
+
+TEST(average_quaternions, noisy_measurements_about_known_rotation) {
+    const Eigen::Quaterniond truth =
+        Eigen::Quaterniond(Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitZ()));
+
+    std::vector<Eigen::Quaterniond> quats;
+    quats.emplace_back(Eigen::AngleAxisd(1.02, Eigen::Vector3d::UnitZ()));
+    quats.emplace_back(Eigen::AngleAxisd(0.98, Eigen::Vector3d::UnitZ()));
+    quats.emplace_back(Eigen::AngleAxisd(1.01, Eigen::Vector3d::UnitZ()));
+    quats.emplace_back(Eigen::AngleAxisd(0.99, Eigen::Vector3d::UnitZ()));
+
+    Eigen::Quaterniond avg = average_quaternions(quats);
+
+    EXPECT_NEAR(avg.angularDistance(truth), 0.0, 1e-2);
+}
+
+TEST(average_quaternions, symmetric_opposing_axes) {
+    Eigen::Quaterniond qx =
+        Eigen::Quaterniond(Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitX()));
+    Eigen::Quaterniond qy =
+        Eigen::Quaterniond(Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitY()));
+
+    std::vector<Eigen::Quaterniond> quats{qx, qy};
+
+    Eigen::Quaterniond avg = average_quaternions(quats);
+
+    double dx = avg.angularDistance(qx);
+    double dy = avg.angularDistance(qy);
+
+    EXPECT_NEAR(dx, dy, 1e-6);
+}
+
+TEST(average_quaternions, test_degeneracy) {
+    Eigen::Quaterniond q0 =
+        Eigen::Quaterniond(Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()));
+
+    Eigen::Quaterniond q180 =
+        Eigen::Quaterniond(Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitZ()));
+
+    EXPECT_THROW(average_quaternions({q0, q180}), std::runtime_error);
+}
+
 }  // namespace vortex::utils::math

include/vortex/utils/math.hpp

@@ -5,6 +5,7 @@
 #include <concepts>
 #include <eigen3/Eigen/Dense>
 #include <eigen3/Eigen/Geometry>
+#include <vector>
 
 namespace vortex::utils::math {
 
@@ -144,6 +145,19 @@ Eigen::VectorXd anti_windup(const double dt,
                             const Eigen::VectorXd& integral,
                             const double min_val,
                             const double max_val);
+
+/**
+ * @brief Computes the average orientation from a set of quaternions.
+ * @param quaternions A vector of Eigen::Quaterniond representing orientations.
+ * @return Eigen::Quaterniond The normalized average quaternion with positive
+ * scalar (w) part.
+ * @warning The average orientation is degenerate when input rotations
+ *          are antipodally distributed (≈180° apart).
+ * @throws std::invalid_argument if the input vector is empty.
+ */
+Eigen::Quaterniond average_quaternions(
+    const std::vector<Eigen::Quaterniond>& quaternions);
+
 }  // namespace vortex::utils::math
 
 #endif  // VORTEX_UTILS_MATH_HPP

src/math.cpp

@@ -131,4 +131,47 @@ Eigen::VectorXd anti_windup(const double dt,
     return integral_anti_windup;
 }
 
+Eigen::Quaterniond average_quaternions(
+    const std::vector<Eigen::Quaterniond>& quaternions) {
+    if (quaternions.empty()) {
+        throw std::invalid_argument(
+            "average_quaternions: input vector must not be empty");
+    }
+
+    Eigen::Matrix4d scatter_matrix = Eigen::Matrix4d::Zero();
+    std::ranges::for_each(quaternions, [&](const auto& q) {
+        scatter_matrix +=
+            q.normalized().coeffs() * q.normalized().coeffs().transpose();
+    });
+
+    Eigen::SelfAdjointEigenSolver<Eigen::Matrix4d> eigensolver(scatter_matrix);
+
+    if (eigensolver.info() != Eigen::Success) {
+        throw std::runtime_error(
+            "average_quaternions: eigen decomposition failed after bias");
+    }
+
+    const auto& eigenvalues = eigensolver.eigenvalues();
+    constexpr double eps = 1e-12;
+
+    if (std::abs(eigenvalues(3) - eigenvalues(2)) < eps) {
+        throw std::runtime_error(
+            "average_quaternions_weighted: average orientation is not unique");
+    }
+
+    const Eigen::Vector4d eigenvector = eigensolver.eigenvectors().col(3);
+
+    Eigen::Quaterniond avg_q;
+    avg_q.x() = eigenvector(0);
+    avg_q.y() = eigenvector(1);
+    avg_q.z() = eigenvector(2);
+    avg_q.w() = eigenvector(3);
+
+    if (avg_q.w() < 0.0) {
+        avg_q.coeffs() *= -1.0;
+    }
+
+    return avg_q.normalized();
+}
+
 }  // namespace vortex::utils::math