| 302 | } |
| 303 | |
| 304 | TEST(AverageQuaternion, AgainstSophusImpl) { |
| 305 | const auto quaternions = std::vector{ |
| 306 | Eigen::Quaterniond::UnitRandom(), |
| 307 | Eigen::Quaterniond::UnitRandom(), |
| 308 | Eigen::Quaterniond::UnitRandom(), |
| 309 | }; |
| 310 | |
| 311 | { |
| 312 | auto quats_as_so3_view = |
| 313 | quaternions | ranges::views::transform([](const Eigen::Quaterniond& q) { return SO3d{q}; }); |
| 314 | const auto avg_quaternion = beluga::mean(quaternions, std::array{1., 1., 1.}); |
| 315 | const auto avg_quaternion_neg = Eigen::Quaterniond{-avg_quaternion.coeffs()}; |
| 316 | const auto avg_quat_sophus = Sophus::details::averageUnitQuaternion(quats_as_so3_view | ranges::to<std::vector>); |
| 317 | ASSERT_TRUE(avg_quaternion.isApprox(avg_quat_sophus) || avg_quaternion_neg.isApprox(avg_quat_sophus)) |
| 318 | << "Expected: " << avg_quat_sophus.coeffs().transpose() |
| 319 | << "\n Actual: " << avg_quaternion.coeffs().transpose(); |
| 320 | } |
| 321 | |
| 322 | { |
| 323 | constexpr double kTolerance = 0.01; |
| 324 | auto quats_as_so3_view = |
| 325 | quaternions | ranges::views::transform([](const Eigen::Quaterniond& q) { return SO3d{q}; }); |
| 326 | const auto avg_quaternion = beluga::mean(quaternions, std::array{1e-3, 1e-3, 1. - 2e-3}); |
| 327 | const auto avg_quat_sophus = Sophus::details::averageUnitQuaternion(quats_as_so3_view | ranges::to<std::vector>); |
| 328 | ASSERT_FALSE(avg_quaternion.isApprox(avg_quat_sophus)); |
| 329 | ASSERT_TRUE(avg_quaternion.isApprox(quaternions.back(), kTolerance)); |
| 330 | } |
| 331 | } |
| 332 | |
| 333 | TEST_F(PoseCovarianceEstimation, SE3EquallyWeighted) { |
| 334 | constexpr double kTolerance = 0.001; |