Add 1D particle filter tutorial code (#362)

### Proposed changes

Related to #305, split from #340. 

#### Type of change

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

### 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/)

---------

Signed-off-by: Alon Druck <alon.druck@ekumenlabs.com>

beluga/include/beluga/algorithm/estimation.hpp

@@ -15,6 +15,8 @@
 #ifndef BELUGA_ALGORITHM_ESTIMATION_HPP
 #define BELUGA_ALGORITHM_ESTIMATION_HPP
 
+#include <range/v3/algorithm/count_if.hpp>
+#include <range/v3/numeric/accumulate.hpp>
 #include <range/v3/range/access.hpp>
 #include <range/v3/range/primitives.hpp>
 #include <range/v3/view/common.hpp>
@@ -169,9 +171,51 @@ std::pair<Sophus::SE2<Scalar>, Sophus::Matrix3<Scalar>> estimate(Poses&& poses,
   return std::pair{estimated_pose, covariance_matrix};
 }
 
-/// Computes mean and covariance Returns a pair consisting of the estimated mean pose and its covariance.
+/// Computes mean and standard deviation of a range of weighted scalars.
 /**
- * Given a range of 2D poses, computes the estimated pose by averaging the translation
+ * Given a range of scalars, computes the scalar mean and standard deviation.
+ *
+ * \tparam Scalars A [sized range](https://en.cppreference.com/w/cpp/ranges/sized_range) type whose
+ *  value type is `std::vector<Scalar>`.
+ * \tparam Weights A [sized range](https://en.cppreference.com/w/cpp/ranges/sized_range) type whose
+ *  value type is `Scalar`.
+ * \tparam Scalar The scalar value type of the given range of Scalars.
+ * \param scalars Range of scalars.
+ * \param weights Range of weights.
+ * \return The estimated mean and its standard deviation.
+ */
+template <
+    class Scalars,
+    class Weights,
+    class Scalar = ranges::range_value_t<Scalars>,
+    typename = std::enable_if_t<std::is_arithmetic_v<Scalar>>>
+std::pair<Scalar, Scalar> estimate(Scalars&& scalars, Weights&& weights) {
+  auto weights_view = weights | ranges::views::common;
+  const auto weights_sum = ranges::accumulate(weights, 0.0, std::plus<>{});
+  auto normalized_weights_view =
+      weights_view | ranges::views::transform([weights_sum](auto weight) { return weight / weights_sum; });
+
+  const Scalar weighted_mean = std::transform_reduce(
+      scalars.begin(), scalars.end(), normalized_weights_view.begin(), 0.0, std::plus<>{}, std::multiplies<>{});
+
+  const Scalar weighted_squared_deviations = std::transform_reduce(
+      scalars.begin(), scalars.end(), normalized_weights_view.begin(), 0.0, std::plus<>{},
+      [weighted_mean](const auto& scalar, const auto& weight) {
+        return weight * (scalar - weighted_mean) * (scalar - weighted_mean);
+      });
+
+  const auto number_of_non_zero_weights =
+      static_cast<Scalar>(ranges::count_if(weights_view, [&](auto weight) { return weight > 0; }));
+
+  const Scalar weighted_sd =
+      std::sqrt(weighted_squared_deviations * number_of_non_zero_weights / (number_of_non_zero_weights - 1));
+
+  return std::pair{weighted_mean, weighted_sd};
+}
+
+/// Returns a pair consisting of the estimated mean pose and its covariance.
+/**
+ * Given a range of poses, computes the estimated pose by averaging the translation
  * and rotation parts, assuming all poses are equally weighted.
  * Computes the covariance matrix of the translation parts and the circular variance
  * of the rotation angles to create a 3x3 covariance matrix.

beluga/test/beluga/algorithm/test_estimation.cpp

@@ -229,4 +229,26 @@ TEST_F(PoseCovarianceEstimation, RandomWalkWithSmoothRotationAndNonUniformWeight
   ASSERT_THAT(covariance.col(2).eval(), Vector3Near({0.0000, 0.0000, 0.0855}, kTolerance));
 }
 
+struct MeanStandardDeviationEstimation : public testing::Test {};
+
+TEST_F(MeanStandardDeviationEstimation, UniformWeightOverload) {
+  // Mean and standard deviation estimation with uniform weights.
+  const auto states = std::vector{0.0, 1.0, 1.0, 2.0, 2.0, 3.0, 4.0, 4.0, 5.0, 5.0, 6.0, 7.0, 7.0, 8.0, 9.0};
+  const auto weights = std::vector(states.size(), 1.0);
+  const auto estimation = beluga::estimate(states, weights);
+  constexpr double kTolerance = 0.001;
+  ASSERT_NEAR(std::get<0>(estimation), 4.266, kTolerance);
+  ASSERT_NEAR(std::get<1>(estimation), 2.763, kTolerance);
+}
+
+TEST_F(MeanStandardDeviationEstimation, NonUniformWeightOverload) {
+  // Mean and standard deviation estimation with non-uniform weights.
+  const auto states = std::vector{0.0, 1.0, 1.0, 2.0, 2.0, 3.0, 4.0, 4.0, 5.0, 5.0, 6.0, 7.0, 7.0, 8.0, 9.0};
+  const auto weights = std::vector{0.1, 0.15, 0.15, 0.3, 0.3, 0.4, 0.8, 0.8, 0.4, 0.4, 0.35, 0.3, 0.3, 0.15, 0.1};
+  const auto estimation = beluga::estimate(states, weights);
+  constexpr double kTolerance = 0.001;
+  ASSERT_NEAR(std::get<0>(estimation), 4.300, kTolerance);
+  ASSERT_NEAR(std::get<1>(estimation), 2.026, kTolerance);
+}
+
 }  // namespace

beluga_tutorial/CMakeLists.txt

@@ -0,0 +1,51 @@
+# Copyright 2024 Ekumen, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+cmake_minimum_required(VERSION 3.16)
+
+project(beluga_tutorial)
+
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+if(NOT CMAKE_BUILD_TYPE)
+  message(STATUS "Setting build type to 'Release' as none was specified.")
+  set(CMAKE_BUILD_TYPE
+      "Release"
+      CACHE STRING "Build type" FORCE)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(
+    -Wall
+    -Wconversion
+    -Wextra
+    -Werror
+    -Wpedantic)
+endif()
+
+find_package(beluga REQUIRED)
+find_package(yaml_cpp_vendor REQUIRED)
+
+include_directories(include)
+
+add_executable(${PROJECT_NAME} src/main.cpp)
+
+target_include_directories(
+  ${PROJECT_NAME}
+  PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include> ${yaml_cpp_vendor_INCLUDE_DIRS})
+
+target_link_libraries(${PROJECT_NAME} beluga::beluga yaml-cpp)
+
+install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION lib/${PROJECT_NAME})

beluga_tutorial/package.xml

@@ -0,0 +1,24 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>beluga_tutorial</name>
+  <version>1.0.0</version>
+
+  <description>Primer on Particle Filtering with Beluga.</description>
+
+  <maintainer email="glpuga@ekumenlabs.com">Gerardo Puga</maintainer>
+  <maintainer email="michel@ekumenlabs.com">Michel Hidalgo</maintainer>
+  <maintainer email="nespinosa@ekumenlabs.com">Nahuel Espinosa</maintainer>
+  <maintainer email="alon.druck@ekumenlabs.com">Alon Druck</maintainer>
+
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>cmake</buildtool_depend>
+
+  <depend>beluga</depend>
+  <depend>yaml_cpp_vendor</depend>
+
+  <export>
+    <build_type>cmake</build_type>
+  </export>
+</package>

beluga_tutorial/params/tutorial.yaml

@@ -0,0 +1,26 @@
+map_size: 100 # Size of the map in [m]
+number_of_doors: 15
+number_of_particles: 300 # Fixed number of particles
+number_of_cycles: 100
+initial_position: 0.0 #[m]
+initial_position_sd: 20.0
+dt: 1.0 #[s]
+velocity: 1.0 #[m/s]
+translation_sd: 1.0
+sensor_range: 3.0 # Sensor range of view in [m]
+sensor_model_sigma: 1.0
+min_particle_weight: 0.08
+record_path: "./record.yaml"
+
+landmark_map:
+  - 5
+  - 12
+  - 25
+  - 37
+  - 52
+  - 55
+  - 65
+  - 74
+  - 75
+  - 87
+  - 97