00001 /************************************************************************************ 00002 00003 PublicHeader: OVR.h 00004 Filename : OVR_SensorFilter.h 00005 Content : Basic filtering of sensor data 00006 Created : March 7, 2013 00007 Authors : Steve LaValle, Anna Yershova 00008 00009 Copyright : Copyright 2012 Oculus VR, Inc. All Rights reserved. 00010 00011 Use of this software is subject to the terms of the Oculus license 00012 agreement provided at the time of installation or download, or which 00013 otherwise accompanies this software in either electronic or hard copy form. 00014 00015 *************************************************************************************/ 00016 00017 #ifndef OVR_SensorFilter_h 00018 #define OVR_SensorFilter_h 00019 00020 #include "Kernel/OVR_Math.h" 00021 00022 00023 namespace OVR { 00024 00025 // This class maintains a sliding window of sensor data taken over time and implements 00026 // various simple filters, most of which are linear functions of the data history. 00027 class SensorFilter 00028 { 00029 enum 00030 { 00031 MaxFilterSize = 100, 00032 DefaultFilterSize = 20 00033 }; 00034 00035 private: 00036 int LastIdx; // The index of the last element that was added to the array 00037 int Size; // The window size (number of elements) 00038 Vector3f Elements[MaxFilterSize]; 00039 00040 public: 00041 // Create a new filter with default size 00042 SensorFilter() 00043 { 00044 LastIdx = -1; 00045 Size = DefaultFilterSize; 00046 }; 00047 00048 // Create a new filter with size i 00049 SensorFilter(int i) 00050 { 00051 OVR_ASSERT(i <= MaxFilterSize); 00052 LastIdx = -1; 00053 Size = i; 00054 }; 00055 00056 00057 // Create a new element to the filter 00058 void AddElement (const Vector3f &e) 00059 { 00060 if (LastIdx == Size - 1) 00061 LastIdx = 0; 00062 else 00063 LastIdx++; 00064 00065 Elements[LastIdx] = e; 00066 }; 00067 00068 // Get element i. 0 is the most recent, 1 is one step ago, 2 is two steps ago, ... 00069 Vector3f GetPrev(int i) const 00070 { 00071 OVR_ASSERT(i >= 0); // 00072 int idx = (LastIdx - i); 00073 if (idx < 0) // Fix the wraparound case 00074 idx += Size; 00075 OVR_ASSERT(idx >= 0); // Multiple wraparounds not allowed 00076 return Elements[idx]; 00077 }; 00078 00079 // Simple statistics 00080 Vector3f Total() const; 00081 Vector3f Mean() const; 00082 Vector3f Median() const; 00083 Vector3f Variance() const; // The diagonal of covariance matrix 00084 Matrix4f Covariance() const; 00085 Vector3f PearsonCoefficient() const; 00086 00087 // A popular family of smoothing filters and smoothed derivatives 00088 Vector3f SavitzkyGolaySmooth8() const; 00089 Vector3f SavitzkyGolayDerivative4() const; 00090 Vector3f SavitzkyGolayDerivative5() const; 00091 Vector3f SavitzkyGolayDerivative12() const; 00092 Vector3f SavitzkyGolayDerivativeN(int n) const; 00093 00094 ~SensorFilter() {}; 00095 }; 00096 00097 } //namespace OVR 00098 00099 #endif // OVR_SensorFilter_h