gz/math/MovingWindowFilter.hh

Go to the documentation of this file.

/*
 * Copyright (C) 2016 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.
 *
*/
#ifndef GZ_MATH_MOVINGWINDOWFILTER_HH_
#define GZ_MATH_MOVINGWINDOWFILTER_HH_
 
#include <memory>
#include <vector>
#include "gz/math/Export.hh"
 
namespace ignition
{
  namespace math
  {
    // Inline bracket to help doxygen filtering.
    inline namespace IGNITION_MATH_VERSION_NAMESPACE {
    //
 
    template< typename T>
    class MovingWindowFilterPrivate
    {
      // \brief Constructor
      public: MovingWindowFilterPrivate();
 
      public: unsigned int valWindowSize = 4;
 
      public: std::vector<T> valHistory;
 
      public: typename std::vector<T>::iterator valIter;
 
      public: T sum;
 
      public: unsigned int samples = 0;
    };
 
    template<typename T>
    MovingWindowFilterPrivate<T>::MovingWindowFilterPrivate()
    {
      this->valHistory.resize(this->valWindowSize);
      this->valIter = this->valHistory.begin();
      this->sum = T();
    }
 
    template< typename T>
    class MovingWindowFilter
    {
      public: MovingWindowFilter();
 
      public: virtual ~MovingWindowFilter();
 
      public: void Update(const T _val);
 
      public: void SetWindowSize(const unsigned int _n);
 
      public: unsigned int WindowSize() const;
 
      public: bool WindowFilled() const;
 
      public: T Value() const;
 
      private: std::unique_ptr<MovingWindowFilterPrivate<T>> dataPtr;
    };
 
    template<typename T>
    MovingWindowFilter<T>::MovingWindowFilter()
    : dataPtr(new MovingWindowFilterPrivate<T>())
    {
    }
 
    template<typename T>
    MovingWindowFilter<T>::~MovingWindowFilter()
    {
      this->dataPtr->valHistory.clear();
    }
 
    template<typename T>
    void MovingWindowFilter<T>::Update(const T _val)
    {
      // update sum and sample size with incoming _val
 
      // keep running sum
      this->dataPtr->sum += _val;
 
      // shift pointer, wrap around if end has been reached.
      ++this->dataPtr->valIter;
      if (this->dataPtr->valIter == this->dataPtr->valHistory.end())
      {
        // reset iterator to beginning of queue
        this->dataPtr->valIter = this->dataPtr->valHistory.begin();
      }
 
      // increment sample size
      ++this->dataPtr->samples;
 
      if (this->dataPtr->samples > this->dataPtr->valWindowSize)
      {
        // subtract old value if buffer already filled
        this->dataPtr->sum -= (*this->dataPtr->valIter);
        // put new value into queue
        (*this->dataPtr->valIter) = _val;
        // reduce sample size
        --this->dataPtr->samples;
      }
      else
      {
        // put new value into queue
        (*this->dataPtr->valIter) = _val;
      }
    }
 
    template<typename T>
    void MovingWindowFilter<T>::SetWindowSize(const unsigned int _n)
    {
      this->dataPtr->valWindowSize = _n;
      this->dataPtr->valHistory.clear();
      this->dataPtr->valHistory.resize(this->dataPtr->valWindowSize);
      this->dataPtr->valIter = this->dataPtr->valHistory.begin();
      this->dataPtr->sum = T();
      this->dataPtr->samples = 0;
    }
 
    template<typename T>
    unsigned int MovingWindowFilter<T>::WindowSize() const
    {
      return this->dataPtr->valWindowSize;
    }
 
    template<typename T>
    bool MovingWindowFilter<T>::WindowFilled() const
    {
      return this->dataPtr->samples == this->dataPtr->valWindowSize;
    }
 
    template<typename T>
    T MovingWindowFilter<T>::Value() const
    {
      return this->dataPtr->sum / static_cast<double>(this->dataPtr->samples);
    }
    }
  }
}
#endif