33 #include "iir/Butterworth.h"
41 mResampleFrequency = 100;
43 fx.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency);
45 fy.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency);
47 fz.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency);
54 this->clearIfTimestampIsOlderThanHead(pos, timestamp);
55 this->clearIfJumpInTimestamps(pos, timestamp);
57 if (mResampled.empty()){
58 mResampled[timestamp] = pos;
59 mHistory[timestamp] = pos;
63 this->interpolateAndFilterPositions(pos, timestamp);
64 mHistory[timestamp] = pos;
69 if (mFiltered.size() > mResampleFrequency)
70 return mFiltered.rbegin()->second;
71 else if (!mHistory.empty())
72 return mHistory.rbegin()->second;
74 return Transform3D::Identity();
77 void TrackingPositionFilter::clearIfTimestampIsOlderThanHead(
Transform3D pos,
double timestamp)
79 if (mResampled.empty())
82 if (timestamp < mResampled.rbegin()->first){
87 fx.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency);
89 fy.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency);
91 fz.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency);
97 void TrackingPositionFilter::clearIfJumpInTimestamps(
Transform3D pos,
double timestamp)
99 if (mResampled.empty())
102 double timeStep = timestamp - mResampled.rbegin()->first;
103 if ( timeStep > 1000){
108 fx.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency);
110 fy.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency);
112 fz.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency);
117 void TrackingPositionFilter::interpolateAndFilterPositions(
Transform3D pos,
double timestamp)
119 Transform3D previousPositionMatrix = mHistory.rbegin()->second;
120 double deltaT = timestamp - mHistory.rbegin()->first;
121 int numberOfInterpolationPoints = floor( (timestamp - mResampled.rbegin()->first)/1000 * mResampleFrequency );
124 for (
int i=0; i < numberOfInterpolationPoints; i++)
126 double resampledTimestamp = mResampled.rbegin()->first + 1000/mResampleFrequency;
127 double deltaTpast = resampledTimestamp - mHistory.rbegin()->first;
128 double deltaTfuture = timestamp - resampledTimestamp;
129 interpolatedPosition = pos.matrix() * deltaTpast/deltaT + previousPositionMatrix.matrix() * deltaTfuture/deltaT;
130 mResampled[resampledTimestamp] = interpolatedPosition;
132 filteredPosition = interpolatedPosition;
133 filteredPosition(0,3) = fx.filter(interpolatedPosition(0,3));
134 filteredPosition(1,3) = fy.filter(interpolatedPosition(1,3));
135 filteredPosition(2,3) = fz.filter(interpolatedPosition(2,3));
136 mFiltered[resampledTimestamp] = filteredPosition;
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
Transform3D getFilteredPosition()
void addPosition(Transform3D pos, double timestamp)