Drop unnecessary moving window filter pimpl

include/gz/math/MovingWindowFilter.hh

@@ -19,64 +19,31 @@
 
 #include <memory>
 #include <vector>
-#include "gz/math/Export.hh"
+
+#include <gz/math/config.hh>
+#include <gz/math/Export.hh>
+#include <gz/math/Vector3.hh>
+
+#include <gz/utils/SuppressWarning.hh>
 
 namespace gz
 {
   namespace math
   {
     // Inline bracket to help doxygen filtering.
     inline namespace GZ_MATH_VERSION_NAMESPACE {
-    //
-
-    /// \cond
-    /// \brief Private data members for MovingWindowFilter class.
-    /// This must be in the header due to templatization.
-    template< typename T>
-    class MovingWindowFilterPrivate
-    {
-      // \brief Constructor
-      public: MovingWindowFilterPrivate();
-
-      /// \brief For moving window smoothed value
-      public: unsigned int valWindowSize = 4;
-
-      /// \brief buffer history of raw values
-      public: std::vector<T> valHistory;
-
-      /// \brief iterator pointing to current value in buffer
-      public: typename std::vector<T>::iterator valIter;
-
-      /// \brief keep track of running sum
-      public: T sum;
-
-      /// \brief keep track of number of elements
-      public: unsigned int samples = 0;
-    };
-
-    //////////////////////////////////////////////////
-    template<typename T>
-    MovingWindowFilterPrivate<T>::MovingWindowFilterPrivate()
-    {
-      /// \TODO FIXME hardcoded initial value for now
-      this->valHistory.resize(this->valWindowSize);
-      this->valIter = this->valHistory.begin();
-      this->sum = T();
-    }
-    /// \endcond
-
     /// \brief Base class for MovingWindowFilter. This replaces the
     /// version of MovingWindowFilter in the Gazebo Common library.
     ///
     /// The default window size is 4.
     template< typename T>
-    class MovingWindowFilter
+    class GZ_MATH_VISIBLE MovingWindowFilter
     {
-      /// \brief Constructor
-      public: MovingWindowFilter();
+      // \brief Constructor
+      public: MovingWindowFilter(unsigned int _windowSize = 4);
 
-      /// \brief Destructor
-      public: virtual ~MovingWindowFilter();
+      // \brief Constructor
+      public: virtual ~MovingWindowFilter() = default;
 
       /// \brief Update value of filter
       /// \param[in] _val new raw value
@@ -98,93 +65,31 @@ namespace gz
       /// \return Latest filtered value
       public: T Value() const;
 
-      /// \brief Data pointer.
-      private: std::unique_ptr<MovingWindowFilterPrivate<T>> dataPtr;
-    };
+      /// \brief For moving window smoothed value
+      public: unsigned int valWindowSize = 4;
 
-    //////////////////////////////////////////////////
-    template<typename T>
-    MovingWindowFilter<T>::MovingWindowFilter()
-    : dataPtr(new MovingWindowFilterPrivate<T>())
-    {
-    }
+      /// \brief keep track of number of elements
+      public: unsigned int samples = 0;
 
-    //////////////////////////////////////////////////
-    template<typename T>
-    MovingWindowFilter<T>::~MovingWindowFilter()
-    {
-      this->dataPtr->valHistory.clear();
-    }
+      GZ_UTILS_WARN_IGNORE__DLL_INTERFACE_MISSING
+      /// \brief buffer history of raw values
+      public: std::vector<T> valHistory;
 
-    //////////////////////////////////////////////////
-    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;
-    }
+      /// \brief iterator pointing to current value in buffer
+      public: typename std::vector<T>::iterator valIter;
 
-    //////////////////////////////////////////////////
-    template<typename T>
-    bool MovingWindowFilter<T>::WindowFilled() const
-    {
-      return this->dataPtr->samples == this->dataPtr->valWindowSize;
-    }
+      /// \brief keep track of running sum
+      public: T sum;
+      GZ_UTILS_WARN_RESUME__DLL_INTERFACE_MISSING
+    };
 
-    //////////////////////////////////////////////////
-    template<typename T>
-    T MovingWindowFilter<T>::Value() const
-    {
-      return this->dataPtr->sum / static_cast<double>(this->dataPtr->samples);
-    }
-    }
+    using MovingWindowFilteri = MovingWindowFilter<int>;
+    using MovingWindowFilterf = MovingWindowFilter<float>;
+    using MovingWindowFilterd = MovingWindowFilter<double>;
+    using MovingWindowFilterVector3i = MovingWindowFilter<Vector3i>;
+    using MovingWindowFilterVector3f = MovingWindowFilter<Vector3f>;
+    using MovingWindowFilterVector3d = MovingWindowFilter<Vector3d>;
   }
-}
+  }  // namespace math
+}  // namespace gz
 #endif

src/MovingWindowFilter.cc

@@ -0,0 +1,149 @@
+/*
+ * 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<typename T>
+MovingWindowFilter<T>::MovingWindowFilter(unsigned int _windowSize)
+{
+  this->SetWindowSize(_windowSize);
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+void MovingWindowFilter<T>::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<typename T>
+void MovingWindowFilter<T>::SetWindowSize(const unsigned int _n)
+{
+  this->valWindowSize = _n;
+  this->valHistory = std::vector<T>(_n, T());
+  this->valIter = this->valHistory.begin();
+  this->sum = T();
+  this->samples = 0;
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+unsigned int MovingWindowFilter<T>::WindowSize() const
+{
+  return this->valWindowSize;
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+bool MovingWindowFilter<T>::WindowFilled() const
+{
+  return this->samples == this->valWindowSize;
+}
+
+//////////////////////////////////////////////////
+template<typename T>
+T MovingWindowFilter<T>::Value() const
+{
+  if (std::is_integral_v<T>)
+  {
+    auto value = this->sum / this->samples;
+    return T(value);
+  }
+  else
+  {
+    auto value = this->sum / static_cast<double>(this->samples);
+    return T(value);
+  }
+}
+
+//////////////////////////////////////////////////
+template<>
+gz::math::Vector3i
+MovingWindowFilter<gz::math::Vector3i>::Value() const
+{
+  auto value = this->sum / this->samples;
+  return value;
+}
+
+//////////////////////////////////////////////////
+template<>
+gz::math::Vector3f
+MovingWindowFilter<gz::math::Vector3f>::Value() const
+{
+  gz::math::Vector3f divisor;
+  divisor = static_cast<float>(this->samples);
+  auto value = this->sum / divisor;
+  return value;
+}
+
+//////////////////////////////////////////////////
+template<>
+gz::math::Vector3d
+MovingWindowFilter<gz::math::Vector3d>::Value() const
+{
+  auto value = this->sum / this->samples;
+  return value;
+}
+
+template class MovingWindowFilter<int>;
+template class MovingWindowFilter<float>;
+template class MovingWindowFilter<double>;
+template class MovingWindowFilter<gz::math::Vector3i>;
+template class MovingWindowFilter<gz::math::Vector3f>;
+template class MovingWindowFilter<gz::math::Vector3d>;
+
+}
+}  // namespace math
+}  // namespace gz

src/MovingWindowFilter_TEST.cc

@@ -34,6 +34,53 @@ TEST(MovingWindowFilterTest, SetWindowSize)
   EXPECT_FALSE(filterInt.WindowFilled());
 }
 
+/////////////////////////////////////////////////
+TEST(MovingWindowFilterTest, FilterIntegral)
+{
+  math::MovingWindowFilter<int> mwf;
+
+  mwf.SetWindowSize(2);
+  mwf.Update(1);
+  mwf.Update(1);
+  EXPECT_EQ(1, mwf.Value());
+
+  // Integer cast
+  mwf.Update(1);
+  mwf.Update(2);
+  EXPECT_EQ(1, mwf.Value());
+}
+
+/////////////////////////////////////////////////
+TEST(MovingWindowFilterTest, FilterIntegralVector)
+{
+  math::MovingWindowFilter<math::Vector3i> mwf;
+
+  mwf.SetWindowSize(2);
+  mwf.Update({1, 1, 1});
+  mwf.Update({1, 1, 1});
+  EXPECT_EQ(math::Vector3i(1, 1, 1), mwf.Value());
+
+  // Integer cast
+  mwf.Update({1, 1, 1});
+  mwf.Update({2, 2, 2});
+  EXPECT_EQ(math::Vector3i(1, 1, 1), mwf.Value());
+}
+
+/////////////////////////////////////////////////
+TEST(MovingWindowFilterTest, FilterFloatVector)
+{
+  math::MovingWindowFilter<math::Vector3f> mwf;
+
+  mwf.SetWindowSize(2);
+  mwf.Update({1, 1, 1});
+  mwf.Update({1, 1, 1});
+  EXPECT_EQ(math::Vector3f(1, 1, 1), mwf.Value());
+
+  mwf.Update({1, 1, 1});
+  mwf.Update({2, 2, 2});
+  EXPECT_EQ(math::Vector3f(1.5, 1.5, 1.5), mwf.Value());
+}
+
 /////////////////////////////////////////////////
 TEST(MovingWindowFilterTest, FilterSomething)
 {