1 files changed, 122 insertions, 0 deletions
diff --git a/matchblox/common/C_Smoother.h b/matchblox/common/C_Smoother.h
new file mode 100644
index 0000000..e7d9508
--- /dev/null
+++ b/matchblox/common/C_Smoother.h
@@ -0,0 +1,122 @@
+#ifndef C_3D_POINT_SMOOTHER_H_
+
+#define C_3D_POINT_SMOOTHER_H_
+
+#include <iostream>
+#include <list>
+#include <iostream>
+#include <iomanip>
+
+using namespace std;
+
+typedef enum
+{                 //           k-1
+  SimpMovingAvg,  //S(t) = (1/k)*sum(X(t-n))  =  S(t-1) + (X(t) - X(t-k))/k
+                  //           n=0
+
+  ExpMovingAvg    //S(0) = X(0)
+                  //S(t) = a*X(t) + (1 - a)*S(t-1) = S(t-1) + a*(X(t) - S(t-1))
+
+} SmoothingMethod;
+
+
+template<typename T>
+class C_Smoother
+{
+public:
+  C_Smoother(T f_ZeroVal) : 
+    m_Method(SimpMovingAvg), 
+    m_iWindowSize(2), 
+    m_iSampCount(0),
+    m_ZeroVal(f_ZeroVal),
+    m_dExponent(0.0) {}
+
+  void SetSimpleMovingAverage(int f_iWindowSize);
+  void SetExponentialMovingAverage(double f_dExponent);
+
+  T Smooth(const T &f_Xt);
+
+private:
+  SmoothingMethod m_Method;
+  int             m_iWindowSize, //window size for SimpMovingAvg
+                  m_iSampCount;  //number of processed samples
+  list<T>         m_SampWindow;
+  double          m_dExponent;   //exponent for ExpMovingAvg
+  T               m_ZeroVal,     //the value that is supposed to be zero
+                  m_St;          //last smoothed statistic S(t)
+
+  T SmoothSample_SimpMovingAvg(const T &f_Xt);
+  T SmoothSample_ExpMovingAvg(const T &f_Xt);
+};
+
+template <typename T> 
+void C_Smoother<T>::SetSimpleMovingAverage(int f_iWindowSize)
+{
+  //reset smoothing stuff for Simple moving average smoothing
+  m_Method = SimpMovingAvg;
+  m_iWindowSize = f_iWindowSize;
+  m_iSampCount = 0;
+  m_St = m_ZeroVal; 
+  m_SampWindow.clear();
+}
+
+template <typename T> 
+void C_Smoother<T>::SetExponentialMovingAverage(double f_dExponent)
+{
+  m_Method = ExpMovingAvg;
+  m_dExponent = f_dExponent;
+  m_iSampCount = 0;
+  m_St = m_ZeroVal;
+}
+
+template <typename T> 
+T C_Smoother<T>::SmoothSample_SimpMovingAvg(const T &f_Xt)
+{
+  //add sample to the window
+  m_SampWindow.push_front(f_Xt);
+
+  if (m_iSampCount < m_iWindowSize)
+  {
+    //we don't have enough cmopute the average of the points currently in the window
+    T l_sum = m_ZeroVal;
+    for (std::list<T>::iterator it = m_SampWindow.begin(); it != m_SampWindow.end(); it++)
+      l_sum += *it;
+    return l_sum / (double)m_SampWindow.size();
+  }
+  else
+  {
+    //there are mote than m_iWindowSize samples, incrementally update smoothed sample
+    T l_Xtk = m_SampWindow.back(); //pop the last point from the (window) list
+    m_SampWindow.pop_back(); 
+    return m_St + ((f_Xt - l_Xtk)/(double)m_iWindowSize);
+  }
+}
+
+template <typename T> 
+T C_Smoother<T>::SmoothSample_ExpMovingAvg(const T &f_Xt)
+{
+  if (m_iSampCount == 0)
+    return f_Xt;
+  else
+    return m_St + m_dExponent * (f_Xt - m_St);
+}
+
+template <typename T>
+T C_Smoother<T>::Smooth(const T &f_Xt)
+{
+  switch(m_Method)
+  {
+  case SimpMovingAvg:
+      m_St = SmoothSample_SimpMovingAvg(f_Xt);
+    break;
+
+  case ExpMovingAvg:
+      m_St = SmoothSample_ExpMovingAvg(f_Xt);
+    break;
+  }
+
+  m_iSampCount++;
+  return m_St;
+}
+
+#endif //C_3D_POINT_SMOOTHER_H_
+\ No newline at end of file