Do not require initial value for filtering

include/gram_savitzky_golay/gram_savitzky_golay.h

@@ -20,7 +20,6 @@
 
 #include <Eigen/Core>
 #include <cassert>
-#include <cstddef>
 #include <sstream>
 #include <vector>
 
@@ -95,16 +94,8 @@ struct SavitzkyGolayFilterConfig
   friend std::ostream& operator<<(std::ostream& os, const SavitzkyGolayFilterConfig& conf);
 };
 
-class SavitzkyGolayFilter
+struct SavitzkyGolayFilter
 {
- public:
- private:
-  SavitzkyGolayFilterConfig conf_;
-  std::vector<double> weights_;
-  void init();
-  double dt_;
-
- public:
   SavitzkyGolayFilter(const int m, const int t, const int n, const int s, const double dt = 1.);
   SavitzkyGolayFilter(const SavitzkyGolayFilterConfig& conf);
   SavitzkyGolayFilter();
@@ -122,21 +113,16 @@ class SavitzkyGolayFilter
    * and addition with itself
    * Common types would be std::vector<double>, std::vector<Eigen::VectorXd>, boost::circular_buffer<Eigen::Vector3d>...
    *
-   * @param initial_value Initial value for the filter's accumulator. It should be the zero.
-   *
    * @return Filtered value according to the precomputed filter weights.
    */
   template <typename ContainerT>
-  typename ContainerT::value_type filter(const ContainerT& v, typename ContainerT::value_type&& initial_value) const
+  typename ContainerT::value_type filter(const ContainerT& v) const
   {
-    assert(v.size() == weights_.size());
+    assert(v.size() == weights_.size() && v.size() > 0);
     using T = typename ContainerT::value_type;
-    T res = initial_value;
-    int i = 0;
-    for (const T& value : v)
-    {
-      res += weights_[i] * value;
-      ++i;
+    T res = weights_[0]*v[0];
+    for (int i = 1; i < v.size(); ++i) {
+      res += weights_[i] * v[i];
     }
     return res / dt_;
   }
@@ -145,10 +131,17 @@ class SavitzkyGolayFilter
   {
     return weights_;
   }
+
   SavitzkyGolayFilterConfig config() const
   {
     return conf_;
   }
+
+ private:
+  SavitzkyGolayFilterConfig conf_;
+  std::vector<double> weights_;
+  void init();
+  double dt_;
 };
 
 } /* gram_sg */

include/gram_savitzky_golay/spatial_filters.h

@@ -64,7 +64,7 @@ class EigenVectorFilter
   }
 
   void add(const T& data) { buffer.push_back(data); }
-  T filter() const { return sg_filter.filter(buffer, T::Zero()); }
+  T filter() const { return sg_filter.filter(buffer); }
   gram_sg::SavitzkyGolayFilterConfig config() const
   {
     return sg_conf;
@@ -106,7 +106,7 @@ class RotationFilter
 /**
  * @brief Filters Affine3d
  * The transformations are first converted to their translation and RPY
- * compenents, and then each component is filtered individually
+ * components, and then each component is filtered individually
  * Finally the result is converted back to an Affine3d
  */
 class TransformFilter
@@ -132,28 +132,4 @@ class TransformFilter
   }
 };
 
-class VelocityFilter
-{
- private:
-  EigenVectorFilter<Vector6d> vfilter;
-
-  Vector6d convert(const Vector6d& T);
-
- public:
-  VelocityFilter(const gram_sg::SavitzkyGolayFilterConfig& conf);
-  void reset(const Vector6d& T);
-  void reset();
-  void clear();
-  void add(const Vector6d& T);
-  Vector6d filter() const;
-  gram_sg::SavitzkyGolayFilterConfig config() const
-  {
-    return vfilter.config();
-  }
-  bool ready() const
-  {
-    return vfilter.ready();
-  }
-};
-
 }

src/spatial_filters.cpp

@@ -55,7 +55,7 @@ Eigen::Matrix3d RotationFilter::filter() const
 {
   // Apply a temporal (savitzky-golay) convolution,
   // followed by an orthogonalization
-  const Eigen::Matrix3d& result = sg_filter.filter(buffer, Eigen::Matrix3d::Zero());
+  const Eigen::Matrix3d& result = sg_filter.filter(buffer);
   Eigen::JacobiSVD<Eigen::Matrix3d> svd(result, Eigen::ComputeFullV | Eigen::ComputeFullU);
   Eigen::Matrix3d res = svd.matrixU() * svd.matrixV().transpose();
   return res;
@@ -99,11 +99,4 @@ Eigen::Affine3d TransformFilter::filter() const
   return Eigen::Affine3d(rot);
 }
 
-VelocityFilter::VelocityFilter(const gram_sg::SavitzkyGolayFilterConfig& conf) : vfilter(conf) {}
-Vector6d VelocityFilter::convert(const Vector6d& T) { return T; }
-void VelocityFilter::reset(const Vector6d& T) { vfilter.reset(convert(T)); }
-void VelocityFilter::reset() { vfilter.reset(); }
-void VelocityFilter::clear() { vfilter.clear(); }
-void VelocityFilter::add(const Vector6d& T) { vfilter.add(convert(T)); }
-Vector6d VelocityFilter::filter() const { return Vector6d(vfilter.filter()); }
 }

test/test_gram_savitzky_golay.cpp

@@ -88,7 +88,7 @@ BOOST_AUTO_TEST_CASE(TestIdentity)
   int m = 3;
   SavitzkyGolayFilter filter(m, 0, 2, 0);
   std::vector<double> data = {1, 1, 1, 1, 1, 1, 1};
-  double res = filter.filter(data, 0.);
+  double res = filter.filter(data);
   BOOST_REQUIRE_CLOSE(res, 1, 10e-6);
 }
 
@@ -106,7 +106,7 @@ BOOST_AUTO_TEST_CASE(TestRealTimeFilter)
 
   // Filter some data
   std::vector<double> data = {.1, .7, .9, .7, .8, .5, -.3};
-  double result = filter.filter(data, 0.);
+  double result = filter.filter(data);
   double result_ref = -0.22619047619047616;
   BOOST_REQUIRE_CLOSE(result, result_ref, 10e-6);
 }
@@ -129,23 +129,23 @@ BOOST_AUTO_TEST_CASE(TestRealTimeDerivative)
 
   // Filter some data
   std::vector<double> data = {.1, .2, .3, .4, .5, .6, .7};
-  double result = filter.filter(data, 0.);
+  double result = filter.filter(data);
   double result_ref = 0.1;
   BOOST_REQUIRE_CLOSE(result, result_ref, 10e-6);
 
   // Test filtering with timestep=0.005
   data = {.1, .2, .3, .4, .5, .6, .7};
-  result = filter_dt.filter(data, 0.);
+  result = filter_dt.filter(data);
   result_ref = 0.1/filter_dt.config().time_step();
   BOOST_REQUIRE_CLOSE(result, result_ref, 10e-6);
 
   // Filter some data
   data = {-1, -2, -3, -4, -5, -6, -7};
-  result = filter.filter(data, 0.);
+  result = filter.filter(data);
   result_ref = -1;
   BOOST_REQUIRE_CLOSE(result, result_ref, 10e-6);
   // Test filtering with timestep=0.005
-  result = filter_dt.filter(data, 0.);
+  result = filter_dt.filter(data);
   result_ref = -1./filter_dt.config().time_step();
   BOOST_REQUIRE_CLOSE(result, result_ref, 10e-6);
 
@@ -156,14 +156,13 @@ BOOST_AUTO_TEST_CASE(TestRealTimeDerivative)
 
   // Filter some data
   data = {.1, .2, .3, .4, .5, .6, .7};
-  result = second_order_filter.filter(data, 0.);
+  result = second_order_filter.filter(data);
   BOOST_CHECK_SMALL(result, 10e-6);
 
   // Filter some data
   data = {-1, -2, -3, -4, -5, -6, -7};
-  result = second_order_filter.filter(data, 0.);
+  result = second_order_filter.filter(data);
   BOOST_CHECK_SMALL(result, 10e-6);
-
 }
 
 // Test derivation on a known polynomial function
@@ -198,9 +197,9 @@ BOOST_AUTO_TEST_CASE(TestPolynomialDerivative)
     derivative_order1[x] = (3*a*std::pow(x, 2) + 2*b*std::pow(x,1) + c)/timeStep;
     derivative_order2[x] = (6*a*std::pow(x, 1) + 2*b) / std::pow(timeStep,2);
   }
-  const auto result_order1 = filter_order1.filter(data, 0.);
+  const auto result_order1 = filter_order1.filter(data);
   const auto expected_result_order1 = derivative_order1[m];
-  const auto result_order2 = filter_order2.filter(data, 0.);
+  const auto result_order2 = filter_order2.filter(data);
   const auto expected_result_order2 = derivative_order2[m];
 
   BOOST_REQUIRE_CLOSE(result_order1, expected_result_order1, 10e-8);
@@ -217,7 +216,7 @@ BOOST_AUTO_TEST_CASE(FilterSpeed)
     int Nsample = 1000;
     for (int i = 0; i < Nsample; ++i) {
       auto start_time = std::chrono::high_resolution_clock::now();
-      filter.filter(data, 0.);
+      filter.filter(data);
       auto end_time = std::chrono::high_resolution_clock::now();
       std::chrono::duration<double, std::milli> elapsed = end_time - start_time;
       totalTime += elapsed.count();