33 #include "iir/Butterworth.h" 41 mResampleFrequency = 100;
47 mCutOffFrequency = freq;
53 this->clearIfTimestampIsOlderThanHead(pos, timestamp);
54 this->clearIfJumpInTimestamps(pos, timestamp);
56 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)
89 void TrackingPositionFilter::clearIfJumpInTimestamps(
Transform3D pos,
double timestamp)
91 if (mResampled.empty())
94 double timeStep = timestamp - mResampled.rbegin()->first;
102 void TrackingPositionFilter::interpolateAndFilterPositions(
Transform3D pos,
double timestamp)
104 Transform3D previousPositionMatrix = mHistory.rbegin()->second;
105 double deltaT = timestamp - mHistory.rbegin()->first;
106 int numberOfInterpolationPoints = floor( (timestamp - mResampled.rbegin()->first)/1000 * mResampleFrequency );
109 for (
int i=0; i < numberOfInterpolationPoints; i++)
111 double resampledTimestamp = mResampled.rbegin()->first + 1000/mResampleFrequency;
112 double deltaTpast = resampledTimestamp - mHistory.rbegin()->first;
113 double deltaTfuture = timestamp - resampledTimestamp;
114 interpolatedPosition = pos.matrix() * deltaTpast/deltaT + previousPositionMatrix.matrix() * deltaTfuture/deltaT;
115 mResampled[resampledTimestamp] = interpolatedPosition;
117 filteredPosition = interpolatedPosition;
118 filteredPosition(0,3) = fx.filter(interpolatedPosition(0,3));
119 filteredPosition(1,3) = fy.filter(interpolatedPosition(1,3));
120 filteredPosition(2,3) = fz.filter(interpolatedPosition(2,3));
121 mFiltered[resampledTimestamp] = filteredPosition;
125 void TrackingPositionFilter::reset()
131 fx.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency);
133 fy.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency);
135 fz.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency);
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
void setCutOffFrequency(double freq)
Transform3D getFilteredPosition()
void addPosition(Transform3D pos, double timestamp)
Namespace for all CustusX production code.