Refactor estimation library (#445)

### Proposed changes

Related to #385.

As the title says, refactors the estimation library to reduce compile
times, improve consistency, and make it easier to extend.

#### Type of change

- [ ] 🐛 Bugfix (change which fixes an issue)
- [x] 🚀 Feature (change which adds functionality)
- [ ] 📚 Documentation (change which fixes or extends documentation)

💥 **Breaking change!** `beluga::calculate_covariance` is
`beluga::covariance` and the algorithms are niebloids now.

### Checklist

- [x] Lint and unit tests (if any) pass locally with my changes
- [x] I have added tests that prove my fix is effective or that my
feature works
- [x] I have added necessary documentation (if appropriate)
- [x] All commits have been signed for
[DCO](https://developercertificate.org/)

### Additional comments

There is room for improvement around this design (i.e. optional return
types, more overloads).

---------

Signed-off-by: Nahuel Espinosa <[email protected]>

.clang-tidy

@@ -22,6 +22,7 @@ Checks: >
   -performance-move-const-arg,
   -performance-unnecessary-value-param,
   -readability-braces-around-statements,
+  -readability-qualified-auto,
   -readability-identifier-length,
   -readability-magic-numbers,
   -readability-named-parameter,

beluga/include/beluga/algorithm/amcl_core.hpp

@@ -89,8 +89,7 @@ class Amcl {
   using map_type = typename SensorModel::map_type;
   using spatial_hasher_type = spatial_hash<state_type>;
   using random_state_generator_type = RandomStateGenerator;
-  using estimation_type =
-      std::invoke_result_t<decltype(beluga::estimate<std::vector<state_type>>), std::vector<state_type>>;
+  using estimation_type = std::invoke_result_t<beluga::detail::estimate_fn, std::vector<state_type>>;
 
  public:
   /// Construct a AMCL instance.

beluga/include/beluga/algorithm/cluster_based_estimation.hpp

@@ -29,6 +29,7 @@
 #include <range/v3/numeric/accumulate.hpp>
 #include <range/v3/range/conversion.hpp>
 #include <range/v3/view/cache1.hpp>
+#include <range/v3/view/common.hpp>
 #include <range/v3/view/filter.hpp>
 #include <range/v3/view/map.hpp>
 #include <range/v3/view/zip.hpp>

beluga/test/beluga/algorithm/test_cluster_based_estimation.cpp

@@ -333,8 +333,7 @@ TEST_F(ClusterBasedEstimationDetailTesting, ClusterEstimation) {
 
   const auto particles = ranges::views::zip(states, weights, clusters);
 
-  auto cluster_0_particles =
-      particles | ranges::views::cache1 | ranges::views::filter([](const auto& p) { return std::get<2>(p) == 0; });
+  auto cluster_0_particles = particles | ranges::views::filter([](const auto& p) { return std::get<2>(p) == 0; });
 
   auto cluster_0_states = cluster_0_particles | beluga::views::elements<0>;
   auto cluster_0_weights = cluster_0_particles | beluga::views::elements<1>;

beluga/test/beluga/algorithm/test_estimation.cpp

@@ -67,7 +67,7 @@ TEST_F(CovarianceCalculation, UniformWeightOverload) {
       Vector2d{2, 0}, Vector2d{0, 2}, Vector2d{2, 2}, Vector2d{0, 2}, Vector2d{2, 0},
   };
   const auto translation_mean = Vector2d{1, 1};
-  const auto covariance = beluga::calculate_covariance(translation_vector, translation_mean);
+  const auto covariance = beluga::covariance(translation_vector, translation_mean);
   ASSERT_NEAR(covariance(0, 0), 1.1111, 0.001);
   ASSERT_NEAR(covariance(0, 1), 0.2222, 0.001);
   ASSERT_NEAR(covariance(1, 0), 0.2222, 0.001);
@@ -93,7 +93,7 @@ TEST_F(CovarianceCalculation, NonUniformWeightOverload) {
   const auto total_weight = std::accumulate(weights.begin(), weights.end(), 0.0);
   std::for_each(weights.begin(), weights.end(), [total_weight](auto& weight) { weight /= total_weight; });
   const auto translation_mean = Vector2d{1.1111, 1.1111};
-  const auto covariance = beluga::calculate_covariance(translation_vector, weights, translation_mean);
+  const auto covariance = beluga::covariance(translation_vector, weights, translation_mean);
   ASSERT_NEAR(covariance(0, 0), 1.1765, 0.001);
   ASSERT_NEAR(covariance(0, 1), 0.1176, 0.001);
   ASSERT_NEAR(covariance(1, 0), 0.1176, 0.001);
@@ -265,7 +265,7 @@ TEST_F(ScalarEstimation, NonUniformWeightOverload) {
   const auto standard_deviation = std::sqrt(variance);
   constexpr double kTolerance = 0.001;
   ASSERT_NEAR(mean, 4.300, kTolerance);
-  ASSERT_NEAR(standard_deviation, 2.026, kTolerance);
+  ASSERT_NEAR(standard_deviation, 2.055, kTolerance);
 }
 
 TEST_F(PoseCovarianceEstimation, MultiVariateNormalSE3) {
@@ -311,7 +311,7 @@ TEST(AverageQuaternion, AgainstSophusImpl) {
   {
     auto quats_as_so3_view =
         quaternions | ranges::views::transform([](const Eigen::Quaterniond& q) { return SO3d{q}; });
-    const auto avg_quaternion = beluga::detail::weighted_average_quaternion(quaternions, std::array{1., 1., 1.});
+    const auto avg_quaternion = beluga::mean(quaternions, std::array{1., 1., 1.});
     const auto avg_quat_sophus = Sophus::details::averageUnitQuaternion(quats_as_so3_view | ranges::to<std::vector>);
     ASSERT_TRUE(avg_quaternion.isApprox(avg_quat_sophus));
   }
@@ -320,8 +320,7 @@ TEST(AverageQuaternion, AgainstSophusImpl) {
     constexpr double kTolerance = 0.01;
     auto quats_as_so3_view =
         quaternions | ranges::views::transform([](const Eigen::Quaterniond& q) { return SO3d{q}; });
-    const auto avg_quaternion =
-        beluga::detail::weighted_average_quaternion(quaternions, std::array{1e-3, 1e-3, 1. - 2e-3});
+    const auto avg_quaternion = beluga::mean(quaternions, std::array{1e-3, 1e-3, 1. - 2e-3});
     const auto avg_quat_sophus = Sophus::details::averageUnitQuaternion(quats_as_so3_view | ranges::to<std::vector>);
     ASSERT_FALSE(avg_quaternion.isApprox(avg_quat_sophus));
     ASSERT_TRUE(avg_quaternion.isApprox(quaternions.back(), kTolerance));

beluga/test/beluga/random/test_multivariate_normal_distribution.cpp

@@ -82,7 +82,7 @@ TEST_P(MultivariateNormalDistributionWithParam, DistributionCovarianceAndMean) {
   const auto mean = Eigen::Vector2d{sum / sequence.size()};
   ASSERT_NEAR(mean(0), expected_mean(0), kTolerance);
   ASSERT_NEAR(mean(1), expected_mean(1), kTolerance);
-  const auto covariance = beluga::calculate_covariance(sequence, mean);
+  const auto covariance = beluga::covariance(sequence, mean);
   ASSERT_NEAR(covariance(0, 0), expected_covariance(0, 0), kTolerance);
   ASSERT_NEAR(covariance(0, 1), expected_covariance(0, 1), kTolerance);
   ASSERT_NEAR(covariance(1, 0), expected_covariance(1, 0), kTolerance);