CustusX  18.04
An IGT application
cxTrackingPositionFilter.cpp
Go to the documentation of this file.
1 /*=========================================================================
2 This file is part of CustusX, an Image Guided Therapy Application.
3 
4 Copyright (c) SINTEF Department of Medical Technology.
5 All rights reserved.
6 
7 CustusX is released under a BSD 3-Clause license.
8 
9 See Lisence.txt (https://github.com/SINTEFMedtek/CustusX/blob/master/License.txt) for details.
10 =========================================================================*/
12 #include "iir/Butterworth.h"
13 
14 namespace cx
15 {
16 
18 {
19  mCutOffFrequency = 3;
20  mResampleFrequency = 100;
21  this->reset();
22 }
23 
25 {
26  mCutOffFrequency = freq;
27  this->reset();
28 }
29 
31 {
32  this->clearIfTimestampIsOlderThanHead(pos, timestamp);
33  this->clearIfJumpInTimestamps(pos, timestamp);
34 
35  if (mResampled.empty())
36  {
37  mResampled[timestamp] = pos;
38  mHistory[timestamp] = pos;
39  return;
40  }
41 
42  this->interpolateAndFilterPositions(pos, timestamp);
43  mHistory[timestamp] = pos;
44 }
45 
47 {
48  if (mFiltered.size() > mResampleFrequency) //check if mFiltered contains enough positions for the filter to be stable
49  return mFiltered.rbegin()->second;
50  else if (!mHistory.empty())
51  return mHistory.rbegin()->second;
52  else
53  return Transform3D::Identity();
54 }
55 
56 void TrackingPositionFilter::clearIfTimestampIsOlderThanHead(Transform3D pos, double timestamp)
57 {
58  if (mResampled.empty())
59  return;
60 
61  if (timestamp < mResampled.rbegin()->first)
62  {
63  // clear history if old timestamps appear
64  this->reset();
65  }
66 }
67 
68 void TrackingPositionFilter::clearIfJumpInTimestamps(Transform3D pos, double timestamp)
69 {
70  if (mResampled.empty())
71  return;
72 
73  double timeStep = timestamp - mResampled.rbegin()->first;
74  if ( timeStep > 1000)
75  {
76  // clear history of resampled and filtered data if jump in timestamps of more than 1 second
77  this->reset();
78  }
79 }
80 
81 void TrackingPositionFilter::interpolateAndFilterPositions(Transform3D pos, double timestamp)
82 {
83  Transform3D previousPositionMatrix = mHistory.rbegin()->second;
84  double deltaT = timestamp - mHistory.rbegin()->first; //time from previous measured position to this position
85  int numberOfInterpolationPoints = floor( (timestamp - mResampled.rbegin()->first)/1000 * mResampleFrequency ); // interpolate from last resampled position to current measured position
86  Transform3D interpolatedPosition;
87  Transform3D filteredPosition;
88  for (int i=0; i < numberOfInterpolationPoints; i++)
89  {
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; // linear interpolation between previous and current measured position
94  mResampled[resampledTimestamp] = interpolatedPosition;
95 
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;
101  }
102 }
103 
104 void TrackingPositionFilter::reset()
105 {
106  mHistory.clear();
107  mResampled.clear();
108  mFiltered.clear();
109 
110  fx.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency); // Lag perker isteden
111  fx.reset ();
112  fy.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency);
113  fy.reset ();
114  fz.setup (mFilterOrder, mResampleFrequency, mCutOffFrequency);
115  fz.reset ();
116 }
117 
118 
119 } // namespace cx
120 
121 
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
void addPosition(Transform3D pos, double timestamp)
Namespace for all CustusX production code.