12 #include "iir/Butterworth.h"
20 mResampleFrequency = 100;
26 mCutOffFrequency = freq;
32 this->clearIfTimestampIsOlderThanHead(pos, timestamp);
33 this->clearIfJumpInTimestamps(pos, timestamp);
35 if (mResampled.empty())
37 mResampled[timestamp] = pos;
38 mHistory[timestamp] = pos;
42 this->interpolateAndFilterPositions(pos, timestamp);
43 mHistory[timestamp] = pos;
48 if (mFiltered.size() > mResampleFrequency)
49 return mFiltered.rbegin()->second;
50 else if (!mHistory.empty())
51 return mHistory.rbegin()->second;
53 return Transform3D::Identity();
56 void TrackingPositionFilter::clearIfTimestampIsOlderThanHead(
Transform3D pos,
double timestamp)
58 if (mResampled.empty())
61 if (timestamp < mResampled.rbegin()->first)
68 void TrackingPositionFilter::clearIfJumpInTimestamps(
Transform3D pos,
double timestamp)
70 if (mResampled.empty())
73 double timeStep = timestamp - mResampled.rbegin()->first;
81 void TrackingPositionFilter::interpolateAndFilterPositions(
Transform3D pos,
double timestamp)
83 Transform3D previousPositionMatrix = mHistory.rbegin()->second;
84 double deltaT = timestamp - mHistory.rbegin()->first;
85 int numberOfInterpolationPoints = floor( (timestamp - mResampled.rbegin()->first)/1000 * mResampleFrequency );
88 for (
int i=0; i < numberOfInterpolationPoints; i++)
90 double resampledTimestamp = mResampled.rbegin()->first + 1000/mResampleFrequency;
91 double deltaTpast = resampledTimestamp - mHistory.rbegin()->first;
92 double deltaTfuture = timestamp - resampledTimestamp;
93 interpolatedPosition = pos.matrix() * deltaTpast/deltaT + previousPositionMatrix.matrix() * deltaTfuture/deltaT;
94 mResampled[resampledTimestamp] = interpolatedPosition;
96 filteredPosition = interpolatedPosition;
97 filteredPosition(0,3) = fx.filter(interpolatedPosition(0,3));
98 filteredPosition(1,3) = fy.filter(interpolatedPosition(1,3));
99 filteredPosition(2,3) = fz.filter(interpolatedPosition(2,3));
100 mFiltered[resampledTimestamp] = filteredPosition;
104 void TrackingPositionFilter::reset()
110 fx.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency);
112 fy.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency);
114 fz.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency);