Medical Imaging Interaction Toolkit  2018.4.99-a3d2e8fb
Medical Imaging Interaction Toolkit
mitkVirtualTrackingDevice.cpp
Go to the documentation of this file.
1 /*============================================================================
2 
3 The Medical Imaging Interaction Toolkit (MITK)
4 
5 Copyright (c) German Cancer Research Center (DKFZ)
6 All rights reserved.
7 
8 Use of this source code is governed by a 3-clause BSD license that can be
9 found in the LICENSE file.
10 
11 ============================================================================*/
12 
14 #include "mitkIGTTimeStamp.h"
15 #include "mitkIGTException.h"
16 
17 #include <cstdlib>
18 #include <cstdio>
19 #include <ctime>
20 #include <itksys/SystemTools.hxx>
21 #include <itkMutexLockHolder.h>
22 #include <random>
23 
25 
27 
29 m_AllTools(), m_ToolsMutex(nullptr), m_MultiThreader(nullptr), m_ThreadID(-1), m_RefreshRate(100), m_NumberOfControlPoints(20), m_GaussianNoiseEnabled(false),
30 m_MeanDistributionParam(0.0), m_DeviationDistributionParam(1.0)
31 {
33  m_Bounds[0] = m_Bounds[2] = m_Bounds[4] = -400.0; // initialize bounds to -400 ... +400 (mm) cube
34  m_Bounds[1] = m_Bounds[3] = m_Bounds[5] = 400.0;
35  m_ToolsMutex = itk::FastMutexLock::New();
36 }
37 
39 {
40  if (this->GetState() == Tracking)
41  {
42  this->StopTracking();
43  }
44  if (this->GetState() == Ready)
45  {
46  this->CloseConnection();
47  }
48  /* cleanup tracking thread */
49  if (m_MultiThreader.IsNotNull() && (m_ThreadID != -1))
50  {
51  m_MultiThreader->TerminateThread(m_ThreadID);
52  m_MultiThreader = nullptr;
53  }
54  m_AllTools.clear();
55 }
56 
58 {
59  //if (this->GetState() == Tracking)
60  //{
61  // return nullptr;
62  //}
63  mitk::VirtualTrackingTool::Pointer t = mitk::VirtualTrackingTool::New();
64  t->SetToolName(toolName);
65  t->SetVelocity(0.1);
66  this->InitializeSpline(t);
67  MutexLockHolder lock(*m_ToolsMutex); // lock and unlock the mutex
68  m_AllTools.push_back(t);
69  return t;
70 }
71 
72 
74 {
75  if (this->GetState() != Ready)
76  return false;
77  this->SetState(Tracking); // go to mode Tracking
78  this->m_StopTrackingMutex->Lock();
79  this->m_StopTracking = false;
80  this->m_StopTrackingMutex->Unlock();
81 
83 
84  if (m_MultiThreader.IsNotNull() && (m_ThreadID != -1))
85  m_MultiThreader->TerminateThread(m_ThreadID);
86  if (m_MultiThreader.IsNull())
87  m_MultiThreader = itk::MultiThreader::New();
88 
89  m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method
90  return true;
91 }
92 
94 {
95  if (this->GetState() == Tracking) // Only if the object is in the correct state
96  {
97  m_StopTrackingMutex->Lock(); // m_StopTracking is used by two threads, so we have to ensure correct thread handling
98  m_StopTracking = true;
99  m_StopTrackingMutex->Unlock();
100 
101  m_TrackingFinishedMutex->Lock();
102  this->SetState(Ready);
103  m_TrackingFinishedMutex->Unlock();
104  }
105 
107 
108  return true;
109 }
110 
112 {
113  MutexLockHolder lock(*m_ToolsMutex); // lock and unlock the mutex
114  return static_cast<unsigned int>(this->m_AllTools.size());
115 }
116 
118 {
119  MutexLockHolder lock(*m_ToolsMutex); // lock and unlock the mutex
120  if (toolNumber < m_AllTools.size())
121  return this->m_AllTools.at(toolNumber);
122  return nullptr;
123 }
124 
126 {
127  if (m_NumberOfControlPoints < 1)
128  {
129  mitkThrowException(mitk::IGTException) << "to few control points for spline interpolation";
130  }
131  srand(time(nullptr)); //Init random number generator
132 
133  this->SetState(Ready);
134  return true;
135 }
136 
138 {
139  if (t == nullptr)
140  return;
141 
142  typedef mitk::VirtualTrackingTool::SplineType SplineType;
143  /* create random control points */
144  SplineType::ControlPointListType controlPoints;
145  controlPoints.reserve(m_NumberOfControlPoints + 1);
146 
147  controlPoints.push_back(this->GetRandomPoint()); // insert point 0
148  double length = 0.0; // estimate spline length by calculating line segments lengths
149  for (unsigned int i = 1; i < m_NumberOfControlPoints - 1; ++i) // set points 1..n-2
150  {
151  SplineType::ControlPointType pos;
152  pos = this->GetRandomPoint();
153  length += controlPoints.at(i - 1).EuclideanDistanceTo(pos);
154  controlPoints.push_back(pos);
155  }
156  controlPoints.push_back(controlPoints.at(0)); // close spline --> insert point last control point with same value as first control point
157  length += controlPoints.at(controlPoints.size() - 2).EuclideanDistanceTo(controlPoints.at(controlPoints.size() - 1));
158 
159  /* Create knot list. TODO: rethink knot list values and list size. Is there a better solution? */
160  SplineType::KnotListType knotList;
161  knotList.push_back(0.0);
162  for (unsigned int i = 1; i < controlPoints.size() + t->GetSpline()->GetSplineOrder() + 1; ++i)
163  knotList.push_back(i);
164  knotList.push_back(controlPoints.size() + t->GetSpline()->GetSplineOrder() + 1);
165 
166  t->GetSpline()->SetControlPoints(controlPoints);
167  t->GetSpline()->SetKnots(knotList);
168  t->SetSplineLength(length);
169 }
170 
172 {
173  bool returnValue = true;
174  if (this->GetState() == Setup)
175  return true;
176 
177  this->SetState(Setup);
178  return returnValue;
179 }
180 
182 {
184  if (t != nullptr)
185  return t->GetSplineLength();
186  else
187  throw std::invalid_argument("invalid index");
188 }
189 
190 void mitk::VirtualTrackingDevice::SetToolSpeed(unsigned int idx, mitk::ScalarType roundsPerSecond)
191 {
192  if (roundsPerSecond < 0.0001)
193  throw std::invalid_argument("Minimum tool speed is 0.0001 rounds per second");
194 
196  if (t != nullptr)
197  t->SetVelocity(roundsPerSecond);
198  else
199  throw std::invalid_argument("invalid index");
200 }
201 
203 {
204  MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex
205  if (idx < m_AllTools.size())
206  return m_AllTools.at(idx);
207  else
208  return nullptr;
209 }
210 
212 {
213  /* lock the TrackingFinishedMutex to signal that the execution rights are now transfered to the tracking thread */
214  MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope
215 
216  if (this->GetState() != Tracking)
217  return;
218 
219  bool localStopTracking; // Because m_StopTracking is used by two threads, access has to be guarded by a mutex. To minimize thread locking, a local copy is used here
220 
221  this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking
222  localStopTracking = this->m_StopTracking;
223  this->m_StopTrackingMutex->Unlock();
224 
225  mitk::ScalarType t = 0.0;
226  while ((this->GetState() == Tracking) && (localStopTracking == false))
227  {
228  //for (ToolContainer::iterator itAllTools = m_AllTools.begin(); itAllTools != m_AllTools.end(); itAllTools++)
229  for (unsigned int i = 0; i < this->GetToolCount(); ++i) // use mutexed methods to access tool container
230  {
231  mitk::VirtualTrackingTool::Pointer currentTool = this->GetInternalTool(i);
233  /* calculate tool position with spline interpolation */
234  pos = currentTool->GetSpline()->EvaluateSpline(t);
235  mitk::Point3D mp;
236  mitk::itk2vtk(pos, mp); // convert from SplineType::PointType to mitk::Point3D
237 
238  //Add Gaussian Noise to Tracking Coordinates if enabled
239  if (this->m_GaussianNoiseEnabled)
240  {
241  std::random_device rd;
242  std::mt19937 generator(rd());
243  std::normal_distribution<double> dist(this->m_MeanDistributionParam, this->m_DeviationDistributionParam);
244  double noise = dist(generator);
245  mp = mp + noise;
246  }
247 
248  currentTool->SetPosition(mp);
249  // Currently, a constant speed is used. TODO: use tool velocity setting
250  t += 0.001;
251  if (t >= 1.0)
252  t = 0.0;
253 
254  mitk::Quaternion quat;
255  /* fix quaternion rotation */
256  quat.x() = 0.0;
257  quat.y() = 0.0;
258  quat.z() = 0.0;
259  quat.r() = 1.0;
260  quat.normalize();
261  currentTool->SetOrientation(quat);
262  // TODO: rotate once per cycle around a fixed rotation vector
263 
264  currentTool->SetTrackingError(2 * (rand() / (RAND_MAX + 1.0))); // tracking error in 0 .. 2 Range
265  currentTool->SetDataValid(true);
266  currentTool->Modified();
267  }
268  itksys::SystemTools::Delay(m_RefreshRate);
269  /* Update the local copy of m_StopTracking */
270  this->m_StopTrackingMutex->Lock();
271  localStopTracking = m_StopTracking;
272  this->m_StopTrackingMutex->Unlock();
273  } // tracking ends if we pass this line
274 }
275 
276 ITK_THREAD_RETURN_TYPE mitk::VirtualTrackingDevice::ThreadStartTracking(void* pInfoStruct)
277 {
278  /* extract this pointer from Thread Info structure */
279  struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct;
280  if (pInfo == nullptr)
281  {
282  return ITK_THREAD_RETURN_VALUE;
283  }
284  if (pInfo->UserData == nullptr)
285  {
286  return ITK_THREAD_RETURN_VALUE;
287  }
288  VirtualTrackingDevice *trackingDevice = static_cast<VirtualTrackingDevice*>(pInfo->UserData);
289 
290  if (trackingDevice != nullptr)
291  trackingDevice->TrackTools();
292 
293  trackingDevice->m_ThreadID = -1; // reset thread ID because we end the thread here
294  return ITK_THREAD_RETURN_VALUE;
295 }
296 
298 {
299  ControlPointType pos;
300  pos[0] = m_Bounds[0] + (m_Bounds[1] - m_Bounds[0]) * (rand() / (RAND_MAX + 1.0)); // X = xMin + xRange * (random number between 0 and 1)
301  pos[1] = m_Bounds[2] + (m_Bounds[3] - m_Bounds[2]) * (rand() / (RAND_MAX + 1.0)); // Y
302  pos[2] = m_Bounds[4] + (m_Bounds[5] - m_Bounds[4]) * (rand() / (RAND_MAX + 1.0)); // Z
303 
304  return pos;
305 }
306 
308 {
309  this->m_GaussianNoiseEnabled = true;
310 }
311 
313 {
314  this->m_GaussianNoiseEnabled = false;
315 }
316 
317 void mitk::VirtualTrackingDevice::SetParamsForGaussianNoise(double meanDistribution, double deviationDistribution)
318 {
319  this->m_MeanDistributionParam = meanDistribution;
320  this->m_DeviationDistributionParam = deviationDistribution;
321 }
322 
324 {
326 }
327 
329 {
331 }
void EnableGaussianNoise()
enable addition of Gaussian Noise to tracking coordinates
Interface for all Tracking Tools.
itk::FastMutexLock::Pointer m_ToolsMutex
mutex for coordinated access of tool container
double GetDeviationDistribution()
returns the deviation distribution for the Gaussian Noise
unsigned int m_NumberOfControlPoints
number of control points for the random path generation
void SetControlPoints(ControlPointListType &ctrlpts)
ToolContainer m_AllTools
container for all tracking tools
virtual SplineType * GetSpline()
itk::FastMutexLock::Pointer m_TrackingFinishedMutex
mutex to manage control flow of StopTracking()
static ITK_THREAD_RETURN_TYPE ThreadStartTracking(void *data)
static start method for tracking thread
TrackingTool * AddTool(const char *toolName)
Adds a tool to the tracking device.
double ScalarType
virtual mitk::ScalarType GetSplineLength()
bool m_GaussianNoiseEnabled
adding Gaussian Noise to tracking coordinates or not, false by default
double m_DeviationDistributionParam
mean distribution for Gaussion Noise, 0.0 by default
TrackingTool * GetTool(unsigned int toolNumber) const override
An object of this class represents an exception of the MITK-IGT module.
DataCollection - Class to facilitate loading/accessing structured data.
void SetParamsForGaussianNoise(double meanDistribution, double deviationDistribution)
sets the mean distribution and the standard deviation for the Gaussian Noise
itk::MutexLockHolder< itk::FastMutexLock > MutexLockHolder
static Pointer New()
virtual void SetSplineLength(mitk::ScalarType _arg)
itk::Point< ScalarType, TDimension > PointType
TrackingDeviceState GetState() const
return current object state (Setup, Ready or Tracking)
itk::MultiThreader::Pointer m_MultiThreader
MultiThreader that starts continuous tracking update.
void SetKnots(KnotListType &newKnots)
double GetMeanDistribution()
returns the mean distribution for the Gaussian Noise
void DisableGaussianNoise()
disable addition of Gaussian Noise to Trackin coordinates
mitk::VirtualTrackingTool * GetInternalTool(unsigned int idx)
virtual const unsigned int & GetSplineOrder()
BSpline with nonuniform knot spacing.
bool m_StopTracking
signal stop to tracking thread
void TrackTools()
This method tracks tools as long as the variable m_Mode is set to "Tracking". Tracking tools means ge...
bool CloseConnection() override
Closes the connection and clears all resources.
Interface for all Tracking Devices.
static IGTTimeStamp * GetInstance()
returns a pointer to the current instance of mitkTimeStamp
vnl_quaternion< ScalarType > Quaternion
void Start(itk::Object::Pointer device)
starts the time-acquisition
bool OpenConnection() override
Opens the connection to the device. This have to be done before the tracking is started.
mitk::VirtualTrackingTool::SplineType::ControlPointType ControlPointType
bool StartTracking() override
Starts the tracking.
unsigned int m_RefreshRate
refresh rate of the internal tracking thread in milliseconds (NOT refreshs per second!) ...
void InitializeSpline(mitk::VirtualTrackingTool *t)
initializes the spline path of the tool t with random control points inside the current tracking volu...
TrackingDeviceData m_Data
current device Data
void itk2vtk(const Tin &in, Tout &out)
#define mitkThrowException(classname)
mitk::ScalarType m_Bounds[6]
bounding box of the tracking volume stored as {xMin, xMax, yMin, yMax, zMin, zMax} ...
itk::FastMutexLock::Pointer m_StopTrackingMutex
mutex to control access to m_StopTracking
unsigned int GetToolCount() const override
void Stop(itk::Object::Pointer device)
stops the time-acqusition
virtual void SetVelocity(mitk::ScalarType _arg)
void SetToolSpeed(unsigned int idx, mitk::ScalarType roundsPerSecond)
sets the speed of the tool idx in rounds per second
void SetState(TrackingDeviceState state)
change object state
ControlPointType GetRandomPoint()
returns a random position inside the tracking volume (defined by m_Bounds)
implements TrackingTool interface
Class representing a tracking device which generates random positions / orientations. No hardware is needed for tracking device.
bool StopTracking() override
Stops the tracking.
mitk::ScalarType GetSplineChordLength(unsigned int idx)
return the approximate length of the spline for tool with index idx in millimeter ...