From bb0187f3d55fe2fb5f59e22acda71bb38e3e16f6 Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Mon, 19 Dec 2022 20:28:54 -0300 Subject: [PATCH] Decouple ROS message types from the observation model (#27) This patch decouples ROS message types from the likelihood field sensor model, allowing it to be moved to the main package. It also replaces the custom state class with SE(2) group elements using the sophus library. The only functional change with respect to the previous implementation is that we now support different orientations for the map origin. --- beluga/CMakeLists.txt | 9 +- beluga/include/beluga/beluga.h | 2 + beluga/include/beluga/motion.h | 17 + .../include/beluga/motion/stationary_model.h | 39 +++ beluga/include/beluga/sensor.h | 17 + .../beluga/sensor/likelihood_field_model.h | 125 +++++++ beluga/include/beluga/spatial_hash.h | 11 + beluga/package.xml | 2 + beluga/test/beluga/CMakeLists.txt | 1 + .../sensor/test_likelihood_field_model.cpp | 228 +++++++++++++ beluga/test/benchmark/CMakeLists.txt | 5 +- .../benchmark_likelihood_field_model.cpp | 69 ++++ beluga_amcl/CMakeLists.txt | 2 + beluga_amcl/include/beluga_amcl/amcl_node.hpp | 11 +- beluga_amcl/include/beluga_amcl/models.hpp | 304 ------------------ .../include/beluga_amcl/occupancy_grid.hpp | 145 +++++++++ beluga_amcl/package.xml | 1 + beluga_amcl/src/amcl_node.cpp | 71 +++- 18 files changed, 736 insertions(+), 323 deletions(-) create mode 100644 beluga/include/beluga/motion.h create mode 100644 beluga/include/beluga/motion/stationary_model.h create mode 100644 beluga/include/beluga/sensor.h create mode 100644 beluga/include/beluga/sensor/likelihood_field_model.h create mode 100644 beluga/test/beluga/sensor/test_likelihood_field_model.cpp create mode 100644 beluga/test/benchmark/benchmark_likelihood_field_model.cpp delete mode 100644 beluga_amcl/include/beluga_amcl/models.hpp create mode 100644 beluga_amcl/include/beluga_amcl/occupancy_grid.hpp diff --git a/beluga/CMakeLists.txt b/beluga/CMakeLists.txt index aac22d921..7a3d46a43 100644 --- a/beluga/CMakeLists.txt +++ b/beluga/CMakeLists.txt @@ -14,14 +14,18 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake REQUIRED) +find_package(Eigen3 REQUIRED NO_MODULE) find_package(range-v3 REQUIRED) +find_package(Sophus REQUIRED) add_library(${PROJECT_NAME} INTERFACE) target_include_directories(${PROJECT_NAME} INTERFACE "$" "$") ament_target_dependencies(${PROJECT_NAME} range-v3) +target_link_libraries(${PROJECT_NAME} INTERFACE Eigen3::Eigen Sophus::Sophus) target_compile_features(${PROJECT_NAME} INTERFACE cxx_std_17) +target_compile_definitions(${PROJECT_NAME} INTERFACE EIGEN_NO_DEBUG SOPHUS_USE_BASIC_LOGGING) add_executable(clang_tidy_findable) target_sources(clang_tidy_findable PRIVATE src/clang_tidy_findable.cpp) @@ -58,7 +62,10 @@ install( ament_export_include_directories("include/${PROJECT_NAME}") ament_export_targets(${PROJECT_NAME} HAS_LIBRARY_TARGET) -ament_export_dependencies(range-v3) +ament_export_dependencies( + Eigen3 + range-v3 + Sophus) ament_package() diff --git a/beluga/include/beluga/beluga.h b/beluga/include/beluga/beluga.h index bdc5b7486..aaf79d1cb 100644 --- a/beluga/include/beluga/beluga.h +++ b/beluga/include/beluga/beluga.h @@ -15,6 +15,8 @@ #pragma once #include +#include +#include #include #include #include diff --git a/beluga/include/beluga/motion.h b/beluga/include/beluga/motion.h new file mode 100644 index 000000000..c1f8ec05a --- /dev/null +++ b/beluga/include/beluga/motion.h @@ -0,0 +1,17 @@ +// Copyright 2022 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. + +#pragma once + +#include diff --git a/beluga/include/beluga/motion/stationary_model.h b/beluga/include/beluga/motion/stationary_model.h new file mode 100644 index 000000000..30a8e247a --- /dev/null +++ b/beluga/include/beluga/motion/stationary_model.h @@ -0,0 +1,39 @@ +// Copyright 2022 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. + +#pragma once + +#include + +#include +#include + +namespace beluga { + +template +class StationaryModel : public Mixin { + public: + template + explicit StationaryModel(Args&&... args) : Mixin(std::forward(args)...) {} + + [[nodiscard]] Sophus::SE2d apply_motion(const Sophus::SE2d& state) const { + static thread_local std::mt19937 generator{std::random_device()()}; + auto distribution = std::normal_distribution<>{0, 0.02}; + return state * Sophus::SE2d{ + Sophus::SO2d{distribution(generator)}, + Eigen::Vector2d{distribution(generator), distribution(generator)}}; + } +}; + +} // namespace beluga diff --git a/beluga/include/beluga/sensor.h b/beluga/include/beluga/sensor.h new file mode 100644 index 000000000..7b37bf6e5 --- /dev/null +++ b/beluga/include/beluga/sensor.h @@ -0,0 +1,17 @@ +// Copyright 2022 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. + +#pragma once + +#include diff --git a/beluga/include/beluga/sensor/likelihood_field_model.h b/beluga/include/beluga/sensor/likelihood_field_model.h new file mode 100644 index 000000000..122055176 --- /dev/null +++ b/beluga/include/beluga/sensor/likelihood_field_model.h @@ -0,0 +1,125 @@ +// Copyright 2022 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. + +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +namespace beluga { + +struct LikelihoodFieldModelParam { + double max_obstacle_distance; + double min_laser_distance; + double max_laser_distance; + double z_hit; + double z_random; + double sigma_hit; +}; + +template +class LikelihoodFieldModel : public Mixin { + public: + template + explicit LikelihoodFieldModel(const LikelihoodFieldModelParam& params, const OccupancyGrid& grid, Args&&... rest) + : Mixin(std::forward(rest)...), + grid_{grid}, + free_cells_{make_free_cells_vector(grid)}, + likelihood_field_{make_likelihood_field(params, grid)} {} + + const auto& likelihood_field() const { return likelihood_field_; } + + template + [[nodiscard]] Sophus::SE2d generate_random_state(Generator& generator) const { + auto index_distribution = std::uniform_int_distribution{0, free_cells_.size() - 1}; + return Sophus::SE2d{ + Sophus::SO2d::sampleUniform(generator), + grid_.origin() * grid_.point(free_cells_[index_distribution(generator)])}; + } + + [[nodiscard]] double importance_weight(const Sophus::SE2d& state) const { + const auto transform = grid_.origin().inverse() * state; + const auto lock = std::shared_lock{points_mutex_}; + return std::transform_reduce( + points_.cbegin(), points_.cend(), 0.0, std::plus{}, + [this, x_offset = transform.translation().x(), y_offset = transform.translation().y(), + cos_theta = transform.so2().unit_complex().x(), + sin_theta = transform.so2().unit_complex().y()](const auto& point) { + // Transform the end point of the laser to the global coordinate system. + // Not using Eigen/Sophus because they make the routine x10 slower. + // See `benchmark_likelihood_field_model.cpp` for reference. + const auto x = point.first * cos_theta - point.second * sin_theta + x_offset; + const auto y = point.first * sin_theta + point.second * cos_theta + y_offset; + const auto index = grid_.index(x, y); + return index < likelihood_field_.size() ? likelihood_field_[index] : 0.0; + }); + } + + void update_sensor(std::vector> points) { + const auto lock = std::lock_guard{points_mutex_}; + points_ = std::move(points); + } + + private: + OccupancyGrid grid_; + std::vector free_cells_; + std::vector likelihood_field_; + std::vector> points_; + mutable std::shared_mutex points_mutex_; + + static std::vector make_free_cells_vector(const OccupancyGrid& grid) { + auto free_cells = std::vector{}; + free_cells.reserve(grid.size()); + for (std::size_t index = 0; index < grid.size(); ++index) { + if (OccupancyGrid::Traits::is_free(grid.data()[index])) { + free_cells.push_back(index); + } + } + return free_cells; + } + + static std::vector make_likelihood_field(const LikelihoodFieldModelParam& params, const OccupancyGrid& grid) { + const auto obstacle_map = grid.data() | ranges::views::transform(&OccupancyGrid::Traits::is_occupied); + + auto squared_distance = [&grid, squared_max_distance = params.max_obstacle_distance * params.max_obstacle_distance]( + std::size_t first, std::size_t second) { + return std::min((grid.point(first) - grid.point(second)).squaredNorm(), squared_max_distance); + }; + + const auto distance_map = nearest_obstacle_distance_map( + obstacle_map, squared_distance, std::bind(&OccupancyGrid::neighbors, &grid, std::placeholders::_1)); + + auto to_likelihood = [amplitude = + params.z_hit / (params.sigma_hit * std::sqrt(2 * Sophus::Constants::pi())), + two_squared_sigma = 2 * params.sigma_hit * params.sigma_hit, + offset = params.z_random / params.max_laser_distance](double squared_distance) { + return amplitude * std::exp(-squared_distance / two_squared_sigma) + offset; + }; + + return distance_map | ranges::views::transform(to_likelihood) | ranges::to; + } +}; + +} // namespace beluga diff --git a/beluga/include/beluga/spatial_hash.h b/beluga/include/beluga/spatial_hash.h index 44b750b69..eafce8ec2 100644 --- a/beluga/include/beluga/spatial_hash.h +++ b/beluga/include/beluga/spatial_hash.h @@ -22,6 +22,8 @@ #include #include +#include + namespace beluga { namespace detail { @@ -64,4 +66,13 @@ struct spatial_hash, std::enable_if_t<(std::is_arithmetic_v +struct spatial_hash { + public: + std::size_t operator()(const Sophus::SE2d& state, double resolution = 1.) const { + const auto& position = state.translation(); + return spatial_hash>{}(std::make_tuple(position.x(), position.y()), resolution); + } +}; + } // namespace beluga diff --git a/beluga/package.xml b/beluga/package.xml index f61550d29..ae34eaea1 100644 --- a/beluga/package.xml +++ b/beluga/package.xml @@ -13,7 +13,9 @@ ament_cmake + eigen range-v3 + sophus ament_cmake_clang_format ament_cmake_clang_tidy diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index 285e1e9d9..f1c61d79a 100644 --- a/beluga/test/beluga/CMakeLists.txt +++ b/beluga/test/beluga/CMakeLists.txt @@ -5,6 +5,7 @@ ament_add_gmock(test_beluga algorithm/test_exponential_filter.cpp algorithm/test_particle_filter.cpp algorithm/test_sampling.cpp + sensor/test_likelihood_field_model.cpp test_tuple_vector.cpp test_spatial_hash.cpp) target_link_libraries(test_beluga ${PROJECT_NAME}) diff --git a/beluga/test/beluga/sensor/test_likelihood_field_model.cpp b/beluga/test/beluga/sensor/test_likelihood_field_model.cpp new file mode 100644 index 000000000..3c6cde954 --- /dev/null +++ b/beluga/test/beluga/sensor/test_likelihood_field_model.cpp @@ -0,0 +1,228 @@ +// Copyright 2022 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 + +namespace { + +template +class StaticOccupancyGrid { + public: + struct Traits { + static bool is_free(bool value) { return !value; } + static bool is_unknown(bool) { return false; } + static bool is_occupied(bool value) { return value; } + }; + + explicit StaticOccupancyGrid( + std::array array, + double resolution = 1.0, + const Sophus::SE2d& origin = Sophus::SE2d{}) + : grid_{array}, origin_{origin}, origin_inverse_{origin.inverse()}, resolution_{resolution} {} + + std::size_t size() const { return grid_.size(); } + const auto& data() const { return grid_; } + const Sophus::SE2d& origin() const { return origin_; } + const Sophus::SE2d& origin_inverse() const { return origin_inverse_; } + + std::size_t index(double x, double y) const { + const auto x_index = static_cast(std::floor(x / resolution())); + const auto y_index = static_cast(std::floor(y / resolution())); + if (x_index >= width() || y_index >= height()) { + return size(); // If the point is outside the map, return an invalid index + } + return x_index + y_index * width(); + } + + std::size_t index(const Eigen::Vector2d& point) const { return index(point.x(), point.y()); } + + Eigen::Vector2d point(std::size_t index) const { + return Eigen::Vector2d{ + (static_cast(index % width()) + 0.5) * resolution(), + (static_cast(index / width()) + 0.5) * resolution()}; + } + + auto neighbors(std::size_t index) const { + auto result = std::vector{}; + const std::size_t row = index / width(); + const std::size_t col = index % width(); + if (row < (height() - 1)) { + result.push_back(index + width()); + } + if (row > 0) { + result.push_back(index - width()); + } + if (col < (width() - 1)) { + result.push_back(index + 1); + } + if (col > 0) { + result.push_back(index - 1); + } + return result; + } + + private: + std::array grid_; + Sophus::SE2d origin_; + Sophus::SE2d origin_inverse_; + double resolution_; + + std::size_t width() const { return Cols; } + std::size_t height() const { return Rows; } + double resolution() const { return resolution_; } +}; + +template