Initial implementation of P. Gorry Savitzky-Golay

.clang-format

@@ -0,0 +1,5 @@
+---
+BasedOnStyle: Google
+BreakBeforeBraces: Allman
+ColumnLimit: 0
+...

.gitignore

@@ -0,0 +1,3 @@
+compile_commands.json
+*.pyc
+build/

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "cmake"]
+	path = cmake
+	url = git://github.com/jrl-umi3218/jrl-cmakemodules.git

CMakeLists.txt

@@ -0,0 +1,25 @@
+cmake_minimum_required(VERSION 2.8)
+
+include(cmake/base.cmake)
+include(cmake/eigen.cmake)
+include(cmake/boost.cmake)
+
+set(PROJECT_NAME gram_savitzky_golay)
+set(PROJECT_DESCRIPTION "Implementation of Savitzky-Golay Filtering using Peter A. Gory Gram Polynomials method")
+set(PROJECT_URL "")
+set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
+
+setup_project()
+search_for_eigen()
+enable_testing()
+
+find_package(Boost 1.54 COMPONENTS unit_test_framework REQUIRED)
+
+# Enable C++11
+add_definitions(-std=c++11)
+
+include_directories(include)
+add_subdirectory(src)
+
+setup_project_finalize()
+

README.md

@@ -0,0 +1,26 @@
+gram_savitzky_golay
+==
+
+C++ Implementation of Savitzky-Golay filtering based on Gram polynomials, as described in 
+- [General Least-Squares Smoothing and Differentiation by the Convolution (Savitzky-Golay) Method](http://pubs.acs.org/doi/pdf/10.1021/ac00205a007)
+
+Example
+==
+
+```cpp
+
+int m = floor(sg7_gram.size() / 2);
+// Window size is 2*m+1
+const size_t m = 3;
+// Polynomial Order
+const size_t n = 2;
+// Initial Point Smoothing (ie evaluate polynomial at first point in the window)
+// Points are defined in range [-m;m]
+const size_t t = -m;
+// Derivate? 0: no derivation, 1: first derivative...
+SavitzkyGolayFilter filter(m, -m, 2, 0);
+
+// Filter some data
+std::vector<double> data = {.1, .7, .9, .7, .8, .5, -.3};
+double result = filter.filter(data);
+```

include/gram_savitzky_golay/gram_savitzky_golay.h

@@ -0,0 +1,70 @@
+#pragma once
+
+#include <cassert>
+#include <cstddef>
+#include <vector>
+
+namespace gram_sg
+{
+/*! GRAMPOLY Calculates the Gram Polynomial (s=0) or its sth derivative
+  *  evaluated at i, order k, over 2m+1 points
+  */
+double GramPoly(const int i, const int m, const int k, const int s);
+
+/*! GenFact Calculates the generalized factorial (a)(a-1)(a-2)...(a-b+1)
+ *    Detailed explanation goes here
+ */
+double GenFact(const int a, const int b);
+
+/*!
+     * Weight Calculates the weight of the ith data point for the t'th
+     * Least-Square point of the s'th derivative, over 2m+1 points, order n
+     */
+double Weight(const int i, const int t, const int m, const int n, const int s);
+
+/*!
+ * Computes the weights for each data point over the window of size 2*m+1,
+ * evaluated at time t, with order n and derivative s
+ */
+std::vector<double> ComputeWeights(const int m, const int t, const int n, const int s);
+
+struct SavitzkyGolayFilterConfig
+{
+  //! Window size is 2*m+1
+  int m;
+  //! Polynomial order
+  int n;
+  //! Derivation order (0 for no derivation)
+  int s;
+  //! Time at which the filter is applied
+  // For real-time, should be t=m
+  int t;
+
+  SavitzkyGolayFilterConfig(const int m, const int n, const int s, const int t) : m(m), n(n), s(s), t(t)
+  {
+  }
+};
+
+class SavitzkyGolayFilter
+{
+ public:
+ private:
+  const SavitzkyGolayFilterConfig conf_;
+  std::vector<double> weights_;
+
+ public:
+  SavitzkyGolayFilter(const int m, const int t, const int n, const int s);
+
+  /*!
+     * Apply Savitzky-Golay convolution to the data
+     * x should have size 2*m+1
+     */
+  double filter(const std::vector<double>& x);
+
+  std::vector<double> weights() const
+  {
+    return weights_;
+  }
+};
+
+} /* gram_sg */

matlab/gram_savitzky_golay/GenFact.m

@@ -0,0 +1,9 @@
+function [ gf ] = GenFact( a, b )
+%GenFact Calculates the generalized factorial (a)(a-1)(a-2)...(a-b+1)
+%   Detailed explanation goes here
+    gf = 1;
+    for j=(a-b+1):a
+        gf = gf * j;
+    end
+end
+

matlab/gram_savitzky_golay/GramPoly.m

@@ -0,0 +1,18 @@
+function [ gp ] = GramPoly( i, m, k, s )
+%GRAMPOLY Calculates the Gram Polynomial (s=0) or its sth derivative 
+% evaluated at i, order k, over 2m+1 points
+
+gp = 0;
+if(k>0)
+    gp = (4*k-2)/(k*(2*m-k+1)) * (i* GramPoly(i,m,k-1,s) + ...
+    s * GramPoly(i,m,k-1,s-1)) - ((k-1)*(2*m+k))/(k*(2*m-k+1))*GramPoly(i,m,k-2,s);
+else
+    if (k==0) && (s==0)
+        gp = 1;
+    else
+        gp = 0;
+    end
+end
+
+end
+

matlab/gram_savitzky_golay/Weight.m

@@ -0,0 +1,10 @@
+function [ w ] = Weight( i, t, m, n, s )
+%Weight Calculates the weight of the ith data point for the t'th
+%Least-Square point of the s'th derivative, over 2m+1 points, order n
+
+w = 0;
+for k=0:n
+    w = w + (2*k+1) * (GenFact(2*m,k)/GenFact(2*m+k+1,k+1))*GramPoly(i,m,k,0)*GramPoly(t,m,k,s);
+end
+end
+

matlab/gram_savitzky_golay/generate_weights.m

@@ -0,0 +1,9 @@
+function [ w ] = generate_weights( m, t, n, s )
+%generate_weights Generate weights for filtering at the end of window
+
+w = [];
+for i=-m:m
+w = [w Weight(i, t, m, 2, 0)];
+end
+end
+

matlab/gram_savitzky_golay/savitzky_golay.m

@@ -0,0 +1,10 @@
+function [ res ] = savitzky_golay( x, sg )
+%savitzky_golay Apply Gram Convolution weights to x
+%   sg: weights
+%    x: data
+  res = 0;
+  for i=1:length(sg)
+    res = res + sg(i) * x(i);
+  end
+end
+

src/CMakeLists.txt

@@ -0,0 +1,25 @@
+add_library(gram_savitzky_golay SHARED
+  gram_savitzky_golay.cpp
+  )
+
+install(TARGETS gram_savitzky_golay
+  RUNTIME DESTINATION bin
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib)
+
+install(
+  DIRECTORY ${CMAKE_SOURCE_DIR}/include/ DESTINATION include)
+
+
+#Add compile target
+add_executable(test_gram_savitzky_golay test/test_gram_savitzky_golay.cpp)
+target_link_libraries(test_gram_savitzky_golay gram_savitzky_golay)
+
+#link to Boost libraries AND your targets and dependencies
+target_link_libraries(test_gram_savitzky_golay ${Boost_LIBRARIES} gram_savitzky_golay)
+
+#Finally add it to test execution - 
+#Notice the WORKING_DIRECTORY and COMMAND
+add_test(NAME test_gram_savitzky_golay
+              WORKING_DIRECTORY ${CMAKE_CURRENT_DIRECTORY}
+              COMMAND ${CMAKE_CURRENT_DIRECTORY}/test_gram_savitzky_golay )

src/gram_savitzky_golay.cpp

@@ -0,0 +1,71 @@
+#include "gram_savitzky_golay/gram_savitzky_golay.h"
+#include <iostream>
+
+namespace gram_sg
+{
+// OK
+double GramPoly(const int i, const int m, const int k, const int s)
+{
+  if (k > 0)
+  {
+    return (4. * k - 2.) / (k * (2. * m - k + 1.)) * (i * GramPoly(i, m, k - 1, s) + s * GramPoly(i, m, k - 1, s - 1)) - ((k - 1.) * (2. * m + k)) / (k * (2. * m - k + 1.)) * GramPoly(i, m, k - 2, s);
+  }
+  else
+  {
+    if (k == 0 && s == 0)
+      return 1.;
+    else
+      return 0.;
+  }
+}
+
+// OK
+double GenFact(const int a, const int b)
+{
+  double gf = 1.;
+
+  for (int j = (a - b) + 1; j <= a; j++)
+  {
+    gf *= j;
+  }
+  return gf;
+}
+
+double Weight(const int i, const int t, const int m, const int n, const int s)
+{
+  double w = 0;
+  for (int k = 0; k <= n; ++k)
+  {
+    w = w + (2 * k + 1) * (GenFact(2 * m, k) / GenFact(2 * m + k + 1, k + 1)) * GramPoly(i, m, k, 0) * GramPoly(t, m, k, s);
+  }
+  return w;
+}
+
+std::vector<double> ComputeWeights(const int m, const int t, const int n, const int s)
+{
+  std::vector<double> weights(2 * m + 1);
+  for (int i = 0; i < 2 * m + 1; ++i)
+  {
+    weights[i] = Weight(i - m, t, m, n, s);
+  }
+  return weights;
+}
+
+SavitzkyGolayFilter::SavitzkyGolayFilter(const int m, const int t, const int n, const int s) : conf_(m, n, s, t)
+{
+  // Compute weights for the time window 2*m+1, for the t'th least-square
+  // point of the s'th derivative
+  weights_ = ComputeWeights(m, t, n, s);
+}
+
+double SavitzkyGolayFilter::filter(const std::vector<double>& x)
+{
+  assert(x.size() == weights_.size());
+  double res = 0;
+  for (unsigned i = 0; i < x.size(); i++)
+  {
+    res += weights_[i] * x[i];
+  }
+  return res;
+}
+} /* gram_sg */

src/test/test_gram_savitzky_golay.cpp

@@ -0,0 +1,64 @@
+//Link to Boost
+#define BOOST_TEST_DYN_LINK
+
+//Define our Module name (prints at testing)
+#define BOOST_TEST_MODULE MyTest
+
+#include <boost/test/unit_test.hpp>
+#include <cmath>
+#include <iostream>
+#include "gram_savitzky_golay/gram_savitzky_golay.h"
+
+using namespace gram_sg;
+
+BOOST_AUTO_TEST_CASE(TestGorryTables)
+{
+  // Compare with tables in the paper from Gorry.
+  // Convolution weights for quadratic initial-point smoothing:
+  // polynomial order = 2, derivative = 0
+  std::vector<double> sg7_gram{
+      32,
+      15,
+      3,
+      -4,
+      -6,
+      -3,
+      5};
+
+  int m = 3;
+  SavitzkyGolayFilter filter(m, -m, 2, 0);
+
+  const auto& filter_weights = filter.weights();
+  for (unsigned int i = 0; i < sg7_gram.size(); i++)
+  {
+    std::cout << "ref: " << sg7_gram[i] << ", computed: " << filter_weights[i] * 42 << std::endl;
+    BOOST_REQUIRE_CLOSE(sg7_gram[i], filter_weights[i] * 42, 10e-6);
+  }
+
+  // BOOST_CHECK( test_object.is_valid() );
+}
+
+BOOST_AUTO_TEST_CASE(TestGorryDerivative)
+{
+  // Compare with tables in the paper from Gorry.
+  // Convolution weights for quadratic initial-point first derivative:
+  // polynomial order = 2, derivative = 1
+  std::vector<double> sg7_deriv_gram{
+      -13,
+      -2,
+      5,
+      8,
+      7,
+      2,
+      -7};
+
+  int m = 3;
+  SavitzkyGolayFilter filter(m, -m, 2, 1);
+
+  const auto& filter_weights = filter.weights();
+  for (unsigned int i = 0; i < sg7_deriv_gram.size(); i++)
+  {
+    std::cout << "ref: " << sg7_deriv_gram[i] << ", computed: " << filter_weights[i] * 28 << std::endl;
+    BOOST_REQUIRE_CLOSE(sg7_deriv_gram[i], filter_weights[i] * 28, 10e-6);
+  }
+}