diff --git a/CMakeLists.txt b/CMakeLists.txt index 439088a0..46ffb056 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -128,6 +128,7 @@ set(FAKE_INSTALL_PREFIX "${CMAKE_BINARY_DIR}/fake/install") # Configure the build #============================================================================ gz_configure_build(QUIT_IF_BUILD_ERRORS + HIDE_SYMBOLS_BY_DEFAULT COMPONENTS eigen3) diff --git a/include/gz/math/MovingWindowFilter.hh b/include/gz/math/MovingWindowFilter.hh index 3a24f4cf..2a0b331d 100644 --- a/include/gz/math/MovingWindowFilter.hh +++ b/include/gz/math/MovingWindowFilter.hh @@ -37,7 +37,7 @@ namespace gz /// /// The default window size is 4. template< typename T> - class GZ_MATH_VISIBLE MovingWindowFilter + class MovingWindowFilter { /// \brief Constructor public: MovingWindowFilter(unsigned int _windowSize = 4); @@ -90,6 +90,119 @@ namespace gz using MovingWindowFilterVector3f = MovingWindowFilter; using MovingWindowFilterVector3d = MovingWindowFilter; } + + ////////////////////////////////////////////////// + template + MovingWindowFilter::MovingWindowFilter(unsigned int _windowSize) + { + this->SetWindowSize(_windowSize); + } + + ////////////////////////////////////////////////// + template + void MovingWindowFilter::Update(const T _val) + { + // update sum and sample size with incoming _val + + // keep running sum + this->sum += _val; + + // shift pointer, wrap around if end has been reached. + ++this->valIter; + if (this->valIter == this->valHistory.end()) + { + // reset iterator to beginning of queue + this->valIter = this->valHistory.begin(); + } + + // increment sample size + ++this->samples; + + if (this->samples > this->valWindowSize) + { + // subtract old value if buffer already filled + this->sum -= (*this->valIter); + // put new value into queue + (*this->valIter) = _val; + // reduce sample size + --this->samples; + } + else + { + // put new value into queue + (*this->valIter) = _val; + } + } + + ////////////////////////////////////////////////// + template + void MovingWindowFilter::SetWindowSize(const unsigned int _n) + { + this->valWindowSize = _n; + this->valHistory = std::vector(_n, T()); + this->valIter = this->valHistory.begin(); + this->sum = T(); + this->samples = 0; + } + + ////////////////////////////////////////////////// + template + unsigned int MovingWindowFilter::WindowSize() const + { + return this->valWindowSize; + } + + ////////////////////////////////////////////////// + template + bool MovingWindowFilter::WindowFilled() const + { + return this->samples == this->valWindowSize; + } + + ////////////////////////////////////////////////// + template<> + inline gz::math::Vector3i + MovingWindowFilter::Value() const + { + auto value = this->sum / this->samples; + return value; + } + + ////////////////////////////////////////////////// + template<> + inline gz::math::Vector3f + MovingWindowFilter::Value() const + { + gz::math::Vector3f divisor; + divisor = static_cast(this->samples); + auto value = this->sum / divisor; + return value; + } + + ////////////////////////////////////////////////// + template<> + inline gz::math::Vector3d + MovingWindowFilter::Value() const + { + auto value = this->sum / this->samples; + return value; + } + + ////////////////////////////////////////////////// + template + T MovingWindowFilter::Value() const + { + if (std::is_integral_v) + { + auto value = this->sum / this->samples; + return T(value); + } + else + { + auto value = this->sum / static_cast(this->samples); + return T(value); + } + } } // namespace math } // namespace gz #endif diff --git a/src/MovingWindowFilter.cc b/src/MovingWindowFilter.cc deleted file mode 100644 index b8724086..00000000 --- a/src/MovingWindowFilter.cc +++ /dev/null @@ -1,149 +0,0 @@ -/* - * Copyright (C) 2021 Open Source Robotics Foundation - * - * 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 "gz/math/MovingWindowFilter.hh" -#include "gz/math/Vector3.hh" - -namespace gz -{ -namespace math -{ -inline namespace GZ_MATH_VERSION_NAMESPACE { - -////////////////////////////////////////////////// -template -MovingWindowFilter::MovingWindowFilter(unsigned int _windowSize) -{ - this->SetWindowSize(_windowSize); -} - -////////////////////////////////////////////////// -template -void MovingWindowFilter::Update(const T _val) -{ - // update sum and sample size with incoming _val - - // keep running sum - this->sum += _val; - - // shift pointer, wrap around if end has been reached. - ++this->valIter; - if (this->valIter == this->valHistory.end()) - { - // reset iterator to beginning of queue - this->valIter = this->valHistory.begin(); - } - - // increment sample size - ++this->samples; - - if (this->samples > this->valWindowSize) - { - // subtract old value if buffer already filled - this->sum -= (*this->valIter); - // put new value into queue - (*this->valIter) = _val; - // reduce sample size - --this->samples; - } - else - { - // put new value into queue - (*this->valIter) = _val; - } -} - -////////////////////////////////////////////////// -template -void MovingWindowFilter::SetWindowSize(const unsigned int _n) -{ - this->valWindowSize = _n; - this->valHistory = std::vector(_n, T()); - this->valIter = this->valHistory.begin(); - this->sum = T(); - this->samples = 0; -} - -////////////////////////////////////////////////// -template -unsigned int MovingWindowFilter::WindowSize() const -{ - return this->valWindowSize; -} - -////////////////////////////////////////////////// -template -bool MovingWindowFilter::WindowFilled() const -{ - return this->samples == this->valWindowSize; -} - -////////////////////////////////////////////////// -template -T MovingWindowFilter::Value() const -{ - if (std::is_integral_v) - { - auto value = this->sum / this->samples; - return T(value); - } - else - { - auto value = this->sum / static_cast(this->samples); - return T(value); - } -} - -////////////////////////////////////////////////// -template<> -gz::math::Vector3i -MovingWindowFilter::Value() const -{ - auto value = this->sum / this->samples; - return value; -} - -////////////////////////////////////////////////// -template<> -gz::math::Vector3f -MovingWindowFilter::Value() const -{ - gz::math::Vector3f divisor; - divisor = static_cast(this->samples); - auto value = this->sum / divisor; - return value; -} - -////////////////////////////////////////////////// -template<> -gz::math::Vector3d -MovingWindowFilter::Value() const -{ - auto value = this->sum / this->samples; - return value; -} - -template class MovingWindowFilter; -template class MovingWindowFilter; -template class MovingWindowFilter; -template class MovingWindowFilter; -template class MovingWindowFilter; -template class MovingWindowFilter; - -} -} // namespace math -} // namespace gz