From 23811ef33dffc7048b88fd6ba5dc1ebcb490a75f Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sat, 20 Dec 2025 19:45:59 +0100 Subject: [PATCH 1/2] average quat function --- cpp_test/test_math.cpp | 93 +++++++++++++++++++++++++++++++++++ include/vortex/utils/math.hpp | 14 ++++++ src/math.cpp | 42 ++++++++++++++++ 3 files changed, 149 insertions(+) diff --git a/cpp_test/test_math.cpp b/cpp_test/test_math.cpp index 7f05ca0..6c95fa3 100644 --- a/cpp_test/test_math.cpp +++ b/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 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 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 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 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 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 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 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 diff --git a/include/vortex/utils/math.hpp b/include/vortex/utils/math.hpp index 8155287..f9377de 100644 --- a/include/vortex/utils/math.hpp +++ b/include/vortex/utils/math.hpp @@ -5,6 +5,7 @@ #include #include #include +#include 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& quaternions); + } // namespace vortex::utils::math #endif // VORTEX_UTILS_MATH_HPP diff --git a/src/math.cpp b/src/math.cpp index 02517f1..a177002 100644 --- a/src/math.cpp +++ b/src/math.cpp @@ -131,4 +131,46 @@ Eigen::VectorXd anti_windup(const double dt, return integral_anti_windup; } +Eigen::Quaterniond average_quaternions( + const std::vector& quaternions) { + if (quaternions.empty()) { + throw std::invalid_argument( + "average_quaternions: input vector must not be empty"); + } + + Eigen::Matrix4d M = Eigen::Matrix4d::Zero(); + std::ranges::for_each(quaternions, [&](const auto& q) { + M += q.normalized().coeffs() * q.normalized().coeffs().transpose(); + }); + + Eigen::SelfAdjointEigenSolver eigensolver(M); + + 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"); + } + + 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 From 89508ea7127fdba5ce2bdc4a929e6aaef6314cdd Mon Sep 17 00:00:00 2001 From: Jorgen Fjermedal Date: Sun, 21 Dec 2025 14:15:49 +0100 Subject: [PATCH 2/2] fixed pr feedback --- src/math.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/src/math.cpp b/src/math.cpp index a177002..aab8c41 100644 --- a/src/math.cpp +++ b/src/math.cpp @@ -138,12 +138,13 @@ Eigen::Quaterniond average_quaternions( "average_quaternions: input vector must not be empty"); } - Eigen::Matrix4d M = Eigen::Matrix4d::Zero(); + Eigen::Matrix4d scatter_matrix = Eigen::Matrix4d::Zero(); std::ranges::for_each(quaternions, [&](const auto& q) { - M += q.normalized().coeffs() * q.normalized().coeffs().transpose(); + scatter_matrix += + q.normalized().coeffs() * q.normalized().coeffs().transpose(); }); - Eigen::SelfAdjointEigenSolver eigensolver(M); + Eigen::SelfAdjointEigenSolver eigensolver(scatter_matrix); if (eigensolver.info() != Eigen::Success) { throw std::runtime_error( @@ -158,7 +159,7 @@ Eigen::Quaterniond average_quaternions( "average_quaternions_weighted: average orientation is not unique"); } - Eigen::Vector4d eigenvector = eigensolver.eigenvectors().col(3); + const Eigen::Vector4d eigenvector = eigensolver.eigenvectors().col(3); Eigen::Quaterniond avg_q; avg_q.x() = eigenvector(0);