-
Notifications
You must be signed in to change notification settings - Fork 20
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Add two new resampling policies and refactor resample on motion into …
…a policy Signed-off-by: Gerardo Puga <[email protected]>
- Loading branch information
Showing
20 changed files
with
1,110 additions
and
23 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,46 @@ | ||
| // 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. | ||
|
|
||
| #ifndef BELUGA_RESAMPLING_POLICIES_HPP | ||
| #define BELUGA_RESAMPLING_POLICIES_HPP | ||
|
|
||
| #include <beluga/resampling_policies/resample_interval_policy.hpp> | ||
| #include <beluga/resampling_policies/resample_on_motion_policy.hpp> | ||
| #include <beluga/resampling_policies/resampling_policies_poller.hpp> | ||
| #include <beluga/resampling_policies/selective_resampling_policy.hpp> | ||
|
|
||
| /** | ||
| * \file | ||
| * \brief Includes all resampling policy related headers. | ||
| */ | ||
|
|
||
| /** | ||
| * \page ResamplingPolicyPage beluga named requirements: ResamplingPolicy | ||
| * Requirements for a resampling policy to be used a beluga `ParticleFilter`. | ||
| * | ||
| * \section ResamplingPolicyRequirements Requirements | ||
| * A type `T` satisfies the `ResamplingPolicy` requirements if the following is satisfied. | ||
| * | ||
| * Given: | ||
| * - An instance `p` of `T`. | ||
| * - An instance `c` of `C`, where is C a particle filter that meets the requirements listed in the policy. | ||
| * | ||
| * Then: | ||
| * - `p.do_resampling(c)` will return true if resampling must be done according to the policy, false otherwise. This | ||
| * function is called in cascade when multiple policies are installed in the filter, and a given filter's | ||
| * do_resampling() function may not be called during a given iteration if a previously queried policy has already voted | ||
| * "false" (short-circuit evaluation of policies). | ||
| * */ | ||
|
|
||
| #endif |
67 changes: 67 additions & 0 deletions
67
beluga/include/beluga/resampling_policies/resample_interval_policy.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,67 @@ | ||
| // Copyright 2023 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. | ||
|
|
||
| #ifndef BELUGA_RESAMPLING_POLICIES_RESAMPLE_INTERVAL_POLICY_HPP | ||
| #define BELUGA_RESAMPLING_POLICIES_RESAMPLE_INTERVAL_POLICY_HPP | ||
|
|
||
| #include <optional> | ||
|
|
||
| #include <beluga/type_traits/resampling_policy_traits.hpp> | ||
|
|
||
| namespace beluga { | ||
|
|
||
| /// Parameters used to construct a ResampleIntervalPolicy instance. | ||
| struct ResampleIntervalPolicyParam { | ||
| /// Interval of calls to do_resampling() out of which only the last iteration will do resampling. | ||
| std::size_t resample_interval_count{1}; | ||
| }; | ||
|
|
||
| /// Implementation of the Resample Interval algorithm for resampling. | ||
| /** | ||
| * ResampleIntervalPolicy is an implementation of the \ref ResamplingPolicyPage "ResamplingPolicy" named requirements. | ||
| * */ | ||
| struct ResampleIntervalPolicy { | ||
| public: | ||
| /// Parameter type that the constructor uses to configure the policy | ||
| using param_type = ResampleIntervalPolicyParam; | ||
|
|
||
| /// @brief Constructor | ||
| /// @param configuration Policy configuration data. | ||
| explicit ResampleIntervalPolicy(const param_type& configuration) : configuration_{configuration} {} | ||
|
|
||
| /// Vote whether resampling must be done according to this policy./ | ||
| /** | ||
| * \tparam Concrete Type representing the concrete implementation of the filter. | ||
| */ | ||
| template <typename Concrete> | ||
| [[nodiscard]] bool do_resampling([[maybe_unused]] Concrete& filter) { | ||
| filter_update_counter_ = (filter_update_counter_ + 1) % configuration_.resample_interval_count; | ||
| return (filter_update_counter_ == 0); | ||
| } | ||
|
|
||
| private: | ||
| param_type configuration_; //< Policy configuration | ||
| std::size_t filter_update_counter_{0}; //< Current cycle phase | ||
| }; | ||
|
|
||
| /// Specialization for a ResampleIntervalPolicy, see also \ref resampling_policy_traits.hpp. | ||
| template <> | ||
| struct resampling_policy_traits<ResampleIntervalPolicy> { | ||
| /// configuration struct type associated to the policy | ||
| using config_type = ResampleIntervalPolicyParam; | ||
| }; | ||
|
|
||
| } // namespace beluga | ||
|
|
||
| #endif |
97 changes: 97 additions & 0 deletions
97
beluga/include/beluga/resampling_policies/resample_on_motion_policy.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,97 @@ | ||
| // Copyright 2023 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. | ||
|
|
||
| #ifndef BELUGA_RESAMPLING_POLICIES_RESAMPLE_ON_MOTION_POLICY_HPP | ||
| #define BELUGA_RESAMPLING_POLICIES_RESAMPLE_ON_MOTION_POLICY_HPP | ||
|
|
||
| #include <optional> | ||
|
|
||
| #include <sophus/se2.hpp> | ||
|
|
||
| #include <beluga/type_traits/resampling_policy_traits.hpp> | ||
|
|
||
| namespace beluga { | ||
|
|
||
| /// Parameters used to construct a ResampleOnMotionPolicy instance. | ||
| struct ResampleOnMotionPolicyParam { | ||
| /// distance threshold along x an y (independently) to trigger a resample | ||
| double update_min_d{0.}; | ||
| /// angular threshold to trigger a resample | ||
| double update_min_a{0.}; | ||
| }; | ||
|
|
||
| /// Implementation of the Resample-On-Motion algorithm for resampling. | ||
| /** | ||
| * ResampleOnMotionPolicy is an implementation of the \ref ResamplingPolicyPage "ResamplingPolicy" named requirements. | ||
| * */ | ||
| struct ResampleOnMotionPolicy { | ||
| public: | ||
| /// Parameter type that the constructor uses to configure the policy | ||
| using param_type = ResampleOnMotionPolicyParam; | ||
| /// Type used to exchange and store motion updates by the motion model | ||
| using motion_event = Sophus::SE2d; | ||
|
|
||
| /// @brief Constructor | ||
| /// @param configuration Policy configuration data. | ||
| explicit ResampleOnMotionPolicy(const param_type& configuration) : configuration_{configuration} {} | ||
|
|
||
| /// Vote whether resampling must be done according to this policy./ | ||
| /** | ||
| * \tparam Concrete Type representing the concrete implementation of the filter. | ||
| * It must satisfy the \ref MotionModelPage MotionModel requirements. | ||
| */ | ||
| template <typename Concrete> | ||
| [[nodiscard]] bool do_resampling(Concrete& filter) { | ||
| // To avoid loss of diversity in the particle population, don't | ||
| // resample when the state is known to be static. | ||
| // Probabilistic Robotics \cite thrun2005probabilistic Chapter 4.2.4. | ||
| auto current_pose = filter.latest_motion_update(); | ||
|
|
||
| // default to letting other policies decide | ||
| bool must_do_resample{true}; | ||
|
|
||
| if (current_pose && latest_resample_pose_) { | ||
| // calculate relative transform between previous pose and the current one | ||
| const auto delta = latest_resample_pose_->inverse() * current_pose.value(); | ||
|
|
||
| // only resample if movement is above thresholds | ||
| must_do_resample = // | ||
| std::abs(delta.translation().x()) > configuration_.update_min_d || | ||
| std::abs(delta.translation().y()) > configuration_.update_min_d || | ||
| std::abs(delta.so2().log()) > configuration_.update_min_a; | ||
| } | ||
|
|
||
| // we always measure the distance traveled since the last time we did resample | ||
| if (must_do_resample) { | ||
| latest_resample_pose_ = current_pose; | ||
| } | ||
|
|
||
| return must_do_resample; | ||
| } | ||
|
|
||
| private: | ||
| param_type configuration_; | ||
| std::optional<motion_event> latest_resample_pose_; | ||
| }; | ||
|
|
||
| /// Specialization for a ResampleOnMotionPolicy, see also \ref resampling_policy_traits.hpp. | ||
| template <> | ||
| struct resampling_policy_traits<ResampleOnMotionPolicy> { | ||
| /// configuration struct type associated to the policy | ||
| using config_type = ResampleOnMotionPolicyParam; | ||
| }; | ||
|
|
||
| } // namespace beluga | ||
|
|
||
| #endif |
Oops, something went wrong.