From a800d8850411dbfe7e427be9301d91b7431f3c58 Mon Sep 17 00:00:00 2001 From: Nahuel Espinosa Date: Sun, 18 Dec 2022 02:58:15 +0000 Subject: [PATCH 1/3] Add differential motion model --- beluga/include/beluga/motion.h | 1 + .../beluga/motion/differential_drive_model.h | 102 ++++++++++++ beluga/test/beluga/CMakeLists.txt | 1 + .../motion/test_differential_drive_model.cpp | 157 ++++++++++++++++++ beluga/test/utils/sophus_matchers.hpp | 48 ++++++ 5 files changed, 309 insertions(+) create mode 100644 beluga/include/beluga/motion/differential_drive_model.h create mode 100644 beluga/test/beluga/motion/test_differential_drive_model.cpp create mode 100644 beluga/test/utils/sophus_matchers.hpp diff --git a/beluga/include/beluga/motion.h b/beluga/include/beluga/motion.h index c1f8ec05a..3037d6d5c 100644 --- a/beluga/include/beluga/motion.h +++ b/beluga/include/beluga/motion.h @@ -14,4 +14,5 @@ #pragma once +#include #include diff --git a/beluga/include/beluga/motion/differential_drive_model.h b/beluga/include/beluga/motion/differential_drive_model.h new file mode 100644 index 000000000..41c6fdafb --- /dev/null +++ b/beluga/include/beluga/motion/differential_drive_model.h @@ -0,0 +1,102 @@ +// 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 + +namespace beluga { + +struct DifferentialDriveModelParam { + double alpha1; + double alpha2; + double alpha3; + double alpha4; + double distance_threshold = 0.01; /* Distance threshold to detect in-place rotation */ +}; + +template +class DifferentialDriveModel : public Mixin { + public: + using DistributionParam = typename std::normal_distribution::param_type; + + template + explicit DifferentialDriveModel(const DifferentialDriveModelParam& params, Args&&... args) + : Mixin(std::forward(args)...), params_{params} {} + + [[nodiscard]] Sophus::SE2d apply_motion(const Sophus::SE2d& state) const { + static thread_local std::mt19937 generator{std::random_device()()}; + auto distribution = std::normal_distribution{}; + using Eigen::Vector2d; + using Sophus::SE2d; + using Sophus::SO2d; + const auto lock = std::shared_lock{params_mutex_}; + const auto first_rotation = SO2d{distribution(generator, first_rotation_params_)}; + const auto translation = Vector2d{distribution(generator, translation_params_), 0.0}; + const auto second_rotation = SO2d{distribution(generator, second_rotation_params_)}; + return state * SE2d{first_rotation, Vector2d{0.0, 0.0}} * SE2d{second_rotation, translation}; + } + + void update_motion(const Sophus::SE2d& pose) { + if (last_pose_) { + const auto translation = pose.translation() - last_pose_.value().translation(); + const double distance = translation.norm(); + const double distance_variance = distance * distance; + + const auto& previous_orientation = last_pose_.value().so2(); + const auto& current_orientation = pose.so2(); + const auto first_rotation = + distance > params_.distance_threshold + ? Sophus::SO2d{std::atan2(translation.y(), translation.x())} * previous_orientation.inverse() + : Sophus::SO2d{0.0}; + const auto second_rotation = current_orientation * previous_orientation.inverse() * first_rotation.inverse(); + const auto combined_rotation = first_rotation * second_rotation; + + const auto lock = std::lock_guard{params_mutex_}; + first_rotation_params_ = DistributionParam{ + first_rotation.log(), + std::sqrt(params_.alpha1 * rotation_variance(first_rotation) + params_.alpha2 * distance_variance)}; + translation_params_ = DistributionParam{ + distance, + std::sqrt(params_.alpha3 * distance_variance + params_.alpha4 * rotation_variance(combined_rotation))}; + second_rotation_params_ = DistributionParam{ + second_rotation.log(), + std::sqrt(params_.alpha1 * rotation_variance(second_rotation) + params_.alpha2 * distance_variance)}; + } + last_pose_ = pose; + } + + private: + DifferentialDriveModelParam params_; + std::optional last_pose_; + + DistributionParam first_rotation_params_{0.0, 0.0}; + DistributionParam second_rotation_params_{0.0, 0.0}; + DistributionParam translation_params_{0.0, 0.0}; + mutable std::shared_mutex params_mutex_; + + static double rotation_variance(const Sophus::SO2d& rotation) { + // Treat backward and forward motion symmetrically for the noise models. + const auto flipped_rotation = rotation * Sophus::SO2d{Sophus::Constants::pi()}; + const auto delta = std::min(std::abs(rotation.log()), std::abs(flipped_rotation.log())); + return delta * delta; + } +}; + +} // namespace beluga diff --git a/beluga/test/beluga/CMakeLists.txt b/beluga/test/beluga/CMakeLists.txt index f1c61d79a..044dc5435 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 + motion/test_differential_drive_model.cpp sensor/test_likelihood_field_model.cpp test_tuple_vector.cpp test_spatial_hash.cpp) diff --git a/beluga/test/beluga/motion/test_differential_drive_model.cpp b/beluga/test/beluga/motion/test_differential_drive_model.cpp new file mode 100644 index 000000000..5dbd35850 --- /dev/null +++ b/beluga/test/beluga/motion/test_differential_drive_model.cpp @@ -0,0 +1,157 @@ +// 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 +#include +#include + +#include "../../utils/sophus_matchers.hpp" + +namespace { + +using Constants = Sophus::Constants; +using Eigen::Vector2d; +using Sophus::SE2d; +using Sophus::SO2d; + +template