From 91347918b38cc17fc7cec080aa2d4bd0a5f32d81 Mon Sep 17 00:00:00 2001 From: Alon Druck <44757262+Alondruck@users.noreply.github.com> Date: Thu, 23 May 2024 08:47:11 -0300 Subject: [PATCH] Add 1D particle filter tutorial code (#362) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit ### 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 --- .../include/beluga/algorithm/estimation.hpp | 48 +++- .../test/beluga/algorithm/test_estimation.cpp | 22 ++ beluga_tutorial/CMakeLists.txt | 51 ++++ beluga_tutorial/package.xml | 24 ++ beluga_tutorial/params/tutorial.yaml | 26 ++ beluga_tutorial/src/main.cpp | 260 ++++++++++++++++++ 6 files changed, 429 insertions(+), 2 deletions(-) create mode 100644 beluga_tutorial/CMakeLists.txt create mode 100644 beluga_tutorial/package.xml create mode 100644 beluga_tutorial/params/tutorial.yaml create mode 100644 beluga_tutorial/src/main.cpp diff --git a/beluga/include/beluga/algorithm/estimation.hpp b/beluga/include/beluga/algorithm/estimation.hpp index 5397d5397..392ebf0b4 100644 --- a/beluga/include/beluga/algorithm/estimation.hpp +++ b/beluga/include/beluga/algorithm/estimation.hpp @@ -15,6 +15,8 @@ #ifndef BELUGA_ALGORITHM_ESTIMATION_HPP #define BELUGA_ALGORITHM_ESTIMATION_HPP +#include +#include #include #include #include @@ -169,9 +171,51 @@ std::pair, Sophus::Matrix3> 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`. + * \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, + typename = std::enable_if_t>> +std::pair 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(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. diff --git a/beluga/test/beluga/algorithm/test_estimation.cpp b/beluga/test/beluga/algorithm/test_estimation.cpp index 7a5518632..26acfcdc1 100644 --- a/beluga/test/beluga/algorithm/test_estimation.cpp +++ b/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 diff --git a/beluga_tutorial/CMakeLists.txt b/beluga_tutorial/CMakeLists.txt new file mode 100644 index 000000000..d6d525607 --- /dev/null +++ b/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 $ + $ ${yaml_cpp_vendor_INCLUDE_DIRS}) + +target_link_libraries(${PROJECT_NAME} beluga::beluga yaml-cpp) + +install(TARGETS ${PROJECT_NAME} RUNTIME DESTINATION lib/${PROJECT_NAME}) diff --git a/beluga_tutorial/package.xml b/beluga_tutorial/package.xml new file mode 100644 index 000000000..8c9832f0f --- /dev/null +++ b/beluga_tutorial/package.xml @@ -0,0 +1,24 @@ + + + + beluga_tutorial + 1.0.0 + + Primer on Particle Filtering with Beluga. + + Gerardo Puga + Michel Hidalgo + Nahuel Espinosa + Alon Druck + + Apache License 2.0 + + cmake + + beluga + yaml_cpp_vendor + + + cmake + + diff --git a/beluga_tutorial/params/tutorial.yaml b/beluga_tutorial/params/tutorial.yaml new file mode 100644 index 000000000..69370eff8 --- /dev/null +++ b/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 diff --git a/beluga_tutorial/src/main.cpp b/beluga_tutorial/src/main.cpp new file mode 100644 index 000000000..9efdce6c9 --- /dev/null +++ b/beluga_tutorial/src/main.cpp @@ -0,0 +1,260 @@ +// Copyright 2022-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. + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +struct Parameters { + /// Size of the 1D map in (m) + /** + * In the simulation, the 1D map is continuous, + * but the limit is defined as an integer to simplify data visualization. + */ + std::size_t map_size{100}; + + /// Maximum number of doors (or landmarks) in the map. + std::size_t number_of_doors{33}; + + /// Fixed number of particles used by the algorithm. + std::size_t number_of_particles{200}; + + /// Number of simulation cycles. + std::size_t number_of_cycles{100}; + + /// Robot's initial position in meters (m). + double initial_position{0.0}; + + /// Standard deviation of the robot's initial position. + /** + * Represents the uncertainty of the robot's initial position. + */ + double initial_position_sd{1.0}; + + /// Delta time in seconds (s). + double dt{1.0}; + + /// Translation velocity in meters per second (m/s) of the robot in 1D. + double velocity{1.0}; + + /// Translation standard deviation. + /** + * Represents the robot's translation noise due to drift and slippage. + */ + double translation_sd{1.0}; + + /// Sensor range of view in meters (m) + double sensor_range{2.0}; + + /// Sensor model sigma + /** + * Represents the precision of the modeled sensor. + */ + double sensor_model_sigma{1.0}; + + /// Minimum particle weight. + /** + * Used to keep all particles "alive" in the reweight step. + */ + double min_particle_weight{0.08}; + + /// Dataset path. + /** + * The dataset file is used to save the data produced by the simulation for posterior analisys. + */ + std::filesystem::path record_path{"./record.yaml"}; +}; + +using Particle = std::tuple; + +struct RobotRecord { + double ground_truth; + beluga::TupleVector current; + beluga::TupleVector propagate; + beluga::TupleVector reweight; + beluga::TupleVector resample; + std::pair estimation; +}; + +struct SimulationRecord { + std::vector landmark_map; + std::vector robot_record; +}; + +void load_parameters(const std::filesystem::path& path, Parameters& parameters) { + try { + YAML::Node params = YAML::LoadFile(path); + parameters.map_size = params["map_size"].as(); + parameters.number_of_doors = params["number_of_doors"].as(); + parameters.number_of_particles = params["number_of_particles"].as(); + parameters.number_of_cycles = params["number_of_cycles"].as(); + parameters.initial_position = params["initial_position"].as(); + parameters.initial_position_sd = params["initial_position_sd"].as(); + parameters.dt = params["dt"].as(); + parameters.velocity = params["velocity"].as(); + parameters.translation_sd = params["translation_sd"].as(); + parameters.sensor_range = params["sensor_range"].as(); + parameters.sensor_model_sigma = params["sensor_model_sigma"].as(); + parameters.min_particle_weight = params["min_particle_weight"].as(); + parameters.record_path = params["record_path"].as(); + } catch (YAML::BadFile& e) { + std::cout << e.what() << "\n"; + } +} + +void load_landmark_map(const std::filesystem::path& path, std::vector& landmark_map) { + try { + YAML::Node node = YAML::LoadFile(path); + landmark_map = node["landmark_map"].as>(); + } catch (YAML::BadFile& e) { + std::cout << e.what() << "\n"; + } +} + +void save_simulation_records(const std::filesystem::path& path, const SimulationRecord& simulation_record) { + YAML::Node node; + + for (const auto& landmark : simulation_record.landmark_map) { + node["landmark_map"].push_back(landmark); + } + + for (auto&& [cycle, record] : simulation_record.robot_record | ranges::views::enumerate) { + node["simulation_records"][cycle]["ground_truth"] = record.ground_truth; + + auto current = node["simulation_records"][cycle]["particles"]["current"]; + current["states"] = beluga::views::states(record.current) | ranges::to>; + current["weights"] = beluga::views::weights(record.current) | ranges::to>; + + auto propagate = node["simulation_records"][cycle]["particles"]["propagate"]; + propagate["states"] = beluga::views::states(record.propagate) | ranges::to>; + propagate["weights"] = beluga::views::weights(record.propagate) | ranges::to>; + + auto reweight = node["simulation_records"][cycle]["particles"]["reweight"]; + reweight["states"] = beluga::views::states(record.reweight) | ranges::to>; + reweight["weights"] = beluga::views::weights(record.reweight) | ranges::to>; + + auto resample = node["simulation_records"][cycle]["particles"]["resample"]; + resample["states"] = beluga::views::states(record.resample) | ranges::to>; + resample["weights"] = beluga::views::weights(record.resample) | ranges::to>; + + auto estimation = node["simulation_records"][cycle]["estimation"]; + estimation["mean"] = std::get<0>(record.estimation); + estimation["sd"] = std::get<1>(record.estimation); + } + + std::ofstream fout(path, std::ios::trunc); + fout << node; + fout.close(); +} + +int main(int argc, char* argv[]) { + if (argc < 2) { + std::cerr << "Usage: " << argv[0] << "\n"; + return 1; + } + + Parameters parameters; + load_parameters(argv[1], parameters); + + SimulationRecord simulation_record; + + std::vector landmark_map; + load_landmark_map(argv[1], landmark_map); + simulation_record.landmark_map = landmark_map; + + std::normal_distribution initial_distribution(parameters.initial_position, parameters.initial_position_sd); + auto particles = beluga::views::sample(initial_distribution) | // + ranges::views::transform(beluga::make_from_state) | // + ranges::views::take_exactly(parameters.number_of_particles) | // + ranges::to; + + double current_position{parameters.initial_position}; + for (std::size_t n = 0; n < parameters.number_of_cycles; ++n) { + RobotRecord robot_record; + + current_position += parameters.velocity * parameters.dt; + robot_record.ground_truth = current_position; + + if (current_position > static_cast(parameters.map_size)) { + break; + } + + auto motion_model = [&](double particle_position, auto& random_engine) { + const double distance = parameters.velocity * parameters.dt; + std::normal_distribution distribution(distance, parameters.translation_sd); + return particle_position + distribution(random_engine); + }; + + auto detections = + landmark_map | // + ranges::views::transform([&](double landmark) { return landmark - current_position; }) | // + ranges::views::remove_if([&](double range) { return std::abs(range) > parameters.sensor_range; }) | // + ranges::to; + + auto sensor_model = [&](double particle_position) { + auto particle_detections = + landmark_map | // + ranges::views::transform([&](double landmark) { return landmark - particle_position; }) | // + ranges::to; + + return parameters.min_particle_weight + + std::transform_reduce( + detections.begin(), detections.end(), 1.0, std::multiplies<>{}, [&](double detection) { + auto distances = // + particle_detections | // + ranges::views::transform( + [&](double particle_detection) { return std::abs(detection - particle_detection); }); + const auto min_distance = ranges::min(distances); + return std::exp((-1 * std::pow(min_distance, 2)) / (2 * parameters.sensor_model_sigma)); + }); + }; + + robot_record.current = particles; + + particles |= beluga::actions::propagate(std::execution::seq, motion_model); + robot_record.propagate = particles; + + particles |= beluga::actions::reweight(std::execution::seq, sensor_model) | beluga::actions::normalize; + robot_record.reweight = particles; + + particles |= beluga::views::sample | // + ranges::views::take_exactly(parameters.number_of_particles) | // + beluga::actions::assign; + robot_record.resample = particles; + + const auto estimation = beluga::estimate(beluga::views::states(particles), beluga::views::weights(particles)); + robot_record.estimation = estimation; + + simulation_record.robot_record.push_back(std::move(robot_record)); + } + + save_simulation_records(parameters.record_path, simulation_record); + + return 0; +}