Medical Imaging Interaction Toolkit  2016.11.0
Medical Imaging Interaction Toolkit
mitkOptitrackTrackingDevice.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,
6 Division of Medical and Biological Informatics.
7 All rights reserved.
8 
9 This software is distributed WITHOUT ANY WARRANTY; without
10 even the implied warranty of MERCHANTABILITY or FITNESS FOR
11 A PARTICULAR PURPOSE.
12 
13 See LICENSE.txt or http://www.mitk.org for details.
14 
15 ===================================================================*/
18 
19 #ifdef MITK_USE_OPTITRACK_TRACKER
20 
21 
22 
26 #include <NPTrackingTools.h>
27 
28 //=======================================================
29 // Static method: IsDeviceInstalled
30 //=======================================================
32 {
33  return true;
34 }
35 
36 //=======================================================
37 // Constructor
38 //=======================================================
40  : mitk::TrackingDevice(),
41  m_initialized(false)
42 {
43  // Set the MultiThread and Mutex
44  this->m_MultiThreader = itk::MultiThreader::New();
45  this->m_ToolsMutex = itk::FastMutexLock::New();
46 
47  //Set the mitk device information
48  SetData(mitk::DeviceDataNPOptitrack);
49 
50  //Clear List of tools
51  this->m_AllTools.clear();
52 }
53 
54 //=======================================================
55 // Destructor
56 //=======================================================
58 {
59  MITK_DEBUG << "Deleting OptitrackTrackingDevice";
60  int result;
61 
62  // If device is in Tracking mode, stop the Tracking firts
63  if (this->GetState() == mitk::TrackingDevice::Tracking)
64  {
65  MITK_DEBUG << "OptitrackTrackingDevice in Tracking State -> Stopping Tracking";
66  result = this->StopTracking();
67 
68  if(result == NPRESULT_SUCCESS){
69  MITK_INFO << "OptitrackTrackingDevice Stopped";
70  }
71  else
72  {
73  MITK_INFO << "Error during Stopping";
75  }
76  }
77 
78  // If device is Ready, Close the connection to device and release the memory
79  if (this->GetState() == mitk::TrackingDevice::Ready)
80  {
81  MITK_DEBUG << "OptitrackTrackingDevice in Ready State -> Closing the Connection";
82  result = this->CloseConnection();
83 
84  if(result)
85  {
86  MITK_INFO << "OptitrackTrackingDevice Connection closed";
87  }
88  else
89  {
90  MITK_DEBUG << "Error during Closing Connection";
92  }
93  }
94 
95  // Set the device off
96  m_initialized = false;
97  // Change State to Setup
98  this->SetState(mitk::TrackingDevice::Setup);
99  MITK_DEBUG <<"OptitrackTrackingDevice deleted successfully";
100 }
101 
102 //=======================================================
103 // OpenConnection
104 //=======================================================
106 {
107  // Not initialize the system twice.
108  if(!m_initialized)
109  {
110  MITK_DEBUG << "Initialize Optitrack Tracking System";
111 
112  if( this->InitializeCameras() )
113  {
114  m_initialized = true; // Set the initialized variable to true
115  this->SetState(mitk::TrackingDevice::Ready);
116  if(this->m_calibrationPath.empty()){
117  MITK_INFO << "Numer of connected cameras = " << TT_CameraCount();
118  MITK_WARN << "Attention: No calibration File defined !!";
119  return m_initialized;
120  }
121  else
122  {
123  this->LoadCalibration();
124  }
125  }
126  else
127  {
128  m_initialized = false; // Set the initialized variable to false
129  this->SetState(mitk::TrackingDevice::Setup); // Set the State to Setup
130  MITK_INFO << "Device initialization failed. Device is still in setup state";
131  mitkThrowException(mitk::IGTException) << "Device initialization failed. Device is still in setup state";
132  }
133  }
134  //this->LoadCalibration();
135  return m_initialized;
136 }
137 
138 //=======================================================
139 // InitializeCameras
140 //=======================================================
142 {
143  MITK_DEBUG << "Initialize Optitrack";
144  int result;
145 
146  result = TT_Initialize(); // Initialize the cameras
147 
148  if(result == NPRESULT_SUCCESS)
149  {
150  MITK_DEBUG << "Optitrack Initialization Succeed";
151  return true;
152  }
153  else
154  {
156  // If not succeed after OPTITRACK_ATTEMPTS times launch exception
157  MITK_INFO << "Optitrack Tracking System cannot be initialized \n" << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(result);
158  mitkThrowException(mitk::IGTException) << "Optitrack Tracking System cannot be initialized \n" << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(result);
159  return false;
160  }
161 }
162 
163 //=======================================================
164 // LoadCalibration
165 //=======================================================
167 {
168  MITK_DEBUG << "Loading System Calibration";
169  int resultLoadCalibration;
170 
171  // Check the file path
172  if(this->m_calibrationPath.empty()){
173  MITK_INFO << "Calibration Path is empty";
174  mitkThrowException(mitk::IGTException) << "Calibration Path is empty";
175  return false;
176  }
177 
178  // Once the system is ready and Initialized , a calibration file is loaded.
179  if(this->m_initialized)
180  {
181 
182  for( int i=OPTITRACK_ATTEMPTS; i>0; i--)
183  {
184  resultLoadCalibration = TT_LoadCalibration(this->m_calibrationPath.c_str());
185 
186  if(resultLoadCalibration != NPRESULT_SUCCESS)
187  {
189  MITK_DEBUG << "Trying again...";
190  }
191  else
192  {
193  MITK_DEBUG << "Calibration file has been loaded successfully";
194  return true;
195  }
196 
197  }
198 
199  MITK_INFO << "System cannot load a calibration file";
200  mitkThrowException(mitk::IGTException) << "System cannot load a calibration file";
201 
202  }
203  else
204  {
205  MITK_INFO << "System is not ready for load a calibration file because it has not been initialized yet";
206  mitkThrowException(mitk::IGTException) << "System is not ready for load a calibration file because it has not been initialized yet";
207  return false;
208  }
209 
210  // Never reach this point
211  return false;
212 }
213 
214 //=======================================================
215 // SetCalibrationPath
216 //=======================================================
217 void mitk::OptitrackTrackingDevice::SetCalibrationPath(std::string calibrationPath){
218 
219  MITK_DEBUG << "SetcalibrationPath";
220  MITK_DEBUG << calibrationPath;
221 
222  // Check the file path
223  if(calibrationPath.empty())
224  {
225  MITK_INFO << "Calibration Path is empty";
226  //mitkThrowException(mitk::IGTException) << "Calibration Path is empty";
227  return;
228  }
229 
230  this->m_calibrationPath = calibrationPath;
231  MITK_INFO << "Calibration Path has been updated to: " << this->m_calibrationPath;
232  return;
233 }
234 
235 //=======================================================
236 // CloseConnection
237 //=======================================================
239 {
240  MITK_DEBUG << "CloseConnection";
241  int resultStop, resultShutdown;
242 
243  if(m_initialized) // Close connection if the System was initialized first
244  {
245  if(this->GetState() == mitk::TrackingDevice::Tracking)
246  {
247  MITK_DEBUG << "Device state: Tracking -> Stoping the Tracking";
248  resultStop = this->StopTracking(); //Stop tracking on close
249  }
250 
251  this->SetState(mitk::OptitrackTrackingDevice::Setup);
252 
253  for( int i=OPTITRACK_ATTEMPTS; i>0; i--)
254  {
255 
256  TT_ClearTrackableList();
257  resultShutdown = TT_Shutdown();
258 
259  if(resultShutdown == NPRESULT_SUCCESS)
260  {
261  MITK_DEBUG << "System has been Shutdown Correctly";
262  Sleep(2000);
263  return true;
264  }
265  else
266  {
267  MITK_DEBUG << "System cannot ShutDown now. Trying again...";
268  }
269  }
270 
271  MITK_INFO << "System cannot ShutDown now";
273  return false;
274  }
275  else
276  {
277  MITK_INFO << "System has not been initialized. Close connection cannot be done";
278  mitkThrowException(mitk::IGTException) << "System has not been initialized. Close connection cannot be done";
279  return false;
280  }
281 
282  return false;
283 }
284 
285 //=======================================================
286 // StartTracking
287 //=======================================================
289 {
290  MITK_DEBUG << "StartTracking";
291  bool resultIsTrackableTracked;
292 
293  if (this->GetState() != mitk::TrackingDevice::Ready)
294  {
295  MITK_INFO << "System is not in State Ready -> Cannot StartTracking";
296  mitkThrowException(mitk::IGTException) << "System is not in State Ready -> Cannot StartTracking";
297  return false;
298  }
299 
300  this->SetState(mitk::TrackingDevice::Tracking);
301 
302  // Change the m_StopTracking Variable to false
303  this->m_StopTrackingMutex->Lock();
304  this->m_StopTracking = false;
305  this->m_StopTrackingMutex->Unlock();
306 
307  /******************************************************************************
308  ###############################################################################
309  TODO: check the timestamp from the Optitrack API
310  ###############################################################################
311  ******************************************************************************/
313 
314  // Launch multiThreader using the Function ThreadStartTracking that executes the TrackTools() method
315  m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method
316 
317  // Information for the user
318  if(GetToolCount() == 0) MITK_INFO << "No tools are defined";
319 
320  for ( int i = 0; i < GetToolCount(); ++i) // use mutexed methods to access tool container
321  {
322  resultIsTrackableTracked = TT_IsTrackableTracked(i);
323  if(resultIsTrackableTracked)
324  {
325  MITK_DEBUG << "Trackable " << i << " is inside the Tracking Volume and it is Tracked";
326  }
327  else
328  {
329  MITK_DEBUG << "Trackable " << i << " is not been tracked. Check if it is inside the Tracking volume";
330  }
331 
332  }
333 
334  return true;
335 }
336 
337 //=======================================================
338 // StopTracking
339 //=======================================================
341 {
342  MITK_DEBUG << "StopTracking";
343 
344  if (this->GetState() == mitk::TrackingDevice::Tracking) // Only if the object is in the correct state
345  {
346  //Change the StopTracking value
347  m_StopTrackingMutex->Lock(); // m_StopTracking is used by two threads, so we have to ensure correct thread handling
348  m_StopTrackingMutex->Unlock();
349  this->SetState(mitk::TrackingDevice::Ready);
350  }
351  else
352  {
353  m_TrackingFinishedMutex->Unlock();
354  MITK_INFO << "System is not in State Tracking -> Cannot StopTracking";
355  mitkThrowException(mitk::IGTException) << "System is not in State Tracking -> Cannot StopTracking";
356  return false;
357  }
358 
359  /******************************************************************************
360  ###############################################################################
361  TODO: check the timestamp from the Optitrack API
362  ###############################################################################
363  ******************************************************************************/
365 
366  m_TrackingFinishedMutex->Unlock();
367  return true;
368 }
369 
370 //=======================================================
371 // ThreadStartTracking
372 //=======================================================
373 ITK_THREAD_RETURN_TYPE mitk::OptitrackTrackingDevice::ThreadStartTracking(void* pInfoStruct)
374 {
375  MITK_DEBUG << "ThreadStartTracking";
376 
377  /* extract this pointer from Thread Info structure */
378  struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct;
379 
380  if (pInfo == NULL)
381  {
382  return ITK_THREAD_RETURN_VALUE;
383  }
384 
385  if (pInfo->UserData == NULL)
386  {
387  return ITK_THREAD_RETURN_VALUE;
388  }
389 
390  OptitrackTrackingDevice *trackingDevice = static_cast<OptitrackTrackingDevice*>(pInfo->UserData);
391 
392  if (trackingDevice != NULL)
393  {
394  // Call the TrackTools function in this thread
395  trackingDevice->TrackTools();
396  }
397  else
398  {
399  mitkThrowException(mitk::IGTException) << "In ThreadStartTracking(): trackingDevice is NULL";
400  }
401 
402  trackingDevice->m_ThreadID = -1; // reset thread ID because we end the thread here
403  return ITK_THREAD_RETURN_VALUE;
404 }
405 
406 //=======================================================
407 // GetOptitrackTool
408 //=======================================================
410 {
411  MITK_DEBUG << "ThreadStartTracking";
412  OptitrackTrackingTool* t = nullptr;
413 
414  MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex
415  if(toolNumber < m_AllTools.size())
416  {
417  t = m_AllTools.at(toolNumber);
418  }
419  else
420  {
421  MITK_INFO << "The tool numbered " << toolNumber << " does not exist";
422  mitkThrowException(mitk::IGTException) << "The tool numbered " << toolNumber << " does not exist";
423  }
424  return t;
425 }
426 
427 //=======================================================
428 // GetToolCount
429 //=======================================================
431 {
432  MITK_DEBUG << "GetToolCount";
433  MutexLockHolder lock(*m_ToolsMutex); // lock and unlock the mutex
434  return ( int)(this->m_AllTools.size());
435 }
436 
437 //=======================================================
438 // TrackTools
439 //=======================================================
441 {
442  MITK_DEBUG << "TrackTools";
443 
444  Point3D position;
445  ScalarType t = 0.0;
446 
447  try
448  {
449  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
450  this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking
451  localStopTracking = this->m_StopTracking;
452 
453  /* lock the TrackingFinishedMutex to signal that the execution rights are now transfered to the tracking thread */
454  if (!localStopTracking)
455  {
456  m_TrackingFinishedMutex->Lock();
457  }
458 
459  this->m_StopTrackingMutex->Unlock();
460  while ((this->GetState() == mitk::TrackingDevice::Tracking) && (localStopTracking == false))
461  {
462  // For each Tracked Tool update the position and orientation
463  for ( int i = 0; i < GetToolCount(); ++i) // use mutexed methods to access tool container
464  {
465  OptitrackTrackingTool* currentTool = this->GetOptitrackTool(i);
466  if(currentTool != nullptr)
467  {
468  currentTool->updateTool();
469  MITK_DEBUG << "Tool number " << i << " updated position";
470  }
471  else
472  {
473  MITK_DEBUG << "Get data from tool number " << i << " failed";
474  mitkThrowException(mitk::IGTException) << "Get data from tool number " << i << " failed";
475  }
476  }
477 
478  /* Update the local copy of m_StopTracking */
479  this->m_StopTrackingMutex->Lock();
480  localStopTracking = m_StopTracking;
481  this->m_StopTrackingMutex->Unlock();
482  Sleep(OPTITRACK_FRAME_RATE);
483  } // tracking ends if we pass this line
484 
485  m_TrackingFinishedMutex->Unlock(); // transfer control back to main thread
486  }
487  catch(...)
488  {
489  m_TrackingFinishedMutex->Unlock();
490  this->StopTracking();
491  mitkThrowException(mitk::IGTException) << "Error while trying to track tools. Thread stopped.";
492  }
493 }
494 
495 //=======================================================
496 // SetCameraParams
497 //=======================================================
498 bool mitk::OptitrackTrackingDevice::SetCameraParams(int exposure, int threshold , int intensity, int videoType )
499 {
500  MITK_DEBUG << "SetCameraParams";
501 
502  if(this->m_initialized)
503  {
504  int num_cams = 0;
505  int resultUpdate;
506  bool resultSetCameraSettings;
507 
508  for( int i=OPTITRACK_ATTEMPTS; i>0; i--)
509  {
510  resultUpdate = TT_Update(); // Get Update for the Optitrack API
511  if(resultUpdate == NPRESULT_SUCCESS)
512  {
513  MITK_DEBUG << "Update Succeed";
514  num_cams = TT_CameraCount();
515  i = 0;
516  }
517  else
518  {
520  MITK_DEBUG << "Trying again...";
521  Sleep(30);
522  }
523  }
524 
525  // If no cameras are connected
526  if(num_cams == 0)
527  {
528  MITK_DEBUG << "No cameras are connected to the device";
529  return false;
530  mitkThrowException(mitk::IGTException) << "No cameras are connected to the device";
531  }
532 
533  for(int cam = 0; cam < num_cams; cam++) // for all connected cameras
534  {
535  for( int i=OPTITRACK_ATTEMPTS; i>0; i--)
536  {
537  resultUpdate = TT_Update(); // Get Update for the Optitrack API
538 
539  if(resultUpdate == NPRESULT_SUCCESS)
540  {
541  MITK_DEBUG << "Update Succeed for camera number " << cam;
542  resultSetCameraSettings = TT_SetCameraSettings(cam,videoType,exposure,threshold,intensity);
543 
544  if(resultSetCameraSettings)
545  {
546  MITK_INFO << "Camera # "<<cam<< " params are set";
547  i = 0; // End attempts for camera #cam
548  }
549  else
550  {
552  if(i == 1)
553  mitkThrowException(mitk::IGTException) << "Camera number " << cam << " failed during setting the params. Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultSetCameraSettings);
554  }
555  }
556  else
557  {
559  MITK_DEBUG << "Update: Trying again...";
560  }
561  }
562  }
563  }
564  else
565  {
566  MITK_INFO << "System is not Initialized -> System is not ready to perform the Camera Parameters Setting";
567  mitkThrowException(mitk::IGTException) << "System is not Initialized -> System is not ready to perform the Camera Parameters Setting";
568  return false;
569  }
570  return true;
571 }
572 
573 //=======================================================
574 // GetTool
575 //=======================================================
576 mitk::TrackingTool* mitk::OptitrackTrackingDevice::GetTool(unsigned int toolNumber) const
577 {
578  return static_cast<mitk::TrackingTool*>(GetOptitrackTool(toolNumber));
579 }
580 
581 //=======================================================
582 // AddToolByFileName
583 //=======================================================
585 {
586  bool resultSetToolByFileName;
587  if(m_initialized)
588  {
590  resultSetToolByFileName= t->SetToolByFileName(fileName);
591 
592  if(resultSetToolByFileName)
593  {
594  this->m_AllTools.push_back(t);
595  MITK_INFO << "Added tool "<<t->GetToolName()<< ". Tool vector size is now: "<<m_AllTools.size();
596  return true;
597  }
598  else
599  {
600  MITK_INFO << "Tool could not be added";
601  mitkThrowException(mitk::IGTException) << "Tool could not be added";
602  return false;
603  }
604  }
605  else
606  {
607  MITK_INFO << "System is not Initialized -> Cannot Add tools";
608  mitkThrowException(mitk::IGTException) << "System is not Initialized -> Cannot Add tools";
609  return false;
610  }
611 }
612 
613 
614 //=======================================================
615 // IF Optitrack is not installed set functions to warnings
616 //=======================================================
617 
618 #else
619 
620 //=======================================================
621 // Static method: IsDeviceInstalled
622 //=======================================================
624 {
625  return false;
626 }
627 
628 //=======================================================
629 // Constructor
630 //=======================================================
632  : mitk::TrackingDevice(),
633  m_initialized(false)
634 {
636 }
637 
638 //=======================================================
639 // Destructor
640 //=======================================================
642 {
644 }
645 
646 //=======================================================
647 // OpenConnection
648 //=======================================================
650 {
652  return false;
653 }
654 
655 //=======================================================
656 // InitializeCameras
657 //=======================================================
659 {
661  return false;
662 }
663 
664 //=======================================================
665 // LoadCalibration
666 //=======================================================
668 {
670  return false;
671 }
672 
673 //=======================================================
674 // SetcalibrationPath
675 //=======================================================
676 void mitk::OptitrackTrackingDevice::SetCalibrationPath(std::string /*calibrationPath*/)
677 {
679 }
680 
681 //=======================================================
682 // CloseConnection
683 //=======================================================
685 {
687  return false;
688 }
689 
690 //=======================================================
691 // StartTracking
692 //=======================================================
694 {
696  return false;
697 }
698 
699 //=======================================================
700 // StopTracking
701 //=======================================================
703 {
705  return false;
706 }
707 
708 //=======================================================
709 // ThreadStartTracking
710 //=======================================================
711 ITK_THREAD_RETURN_TYPE mitk::OptitrackTrackingDevice::ThreadStartTracking(void* pInfoStruct)
712 {
714  return NULL;
715 }
716 
717 //=======================================================
718 // GetOptitrackTool
719 //=======================================================
721 {
723  return nullptr;
724 }
725 
726 //=======================================================
727 // GetToolCount
728 //=======================================================
730 {
732  return 0;
733 }
734 
735 //=======================================================
736 // TrackTools
737 //=======================================================
739 {
741 }
742 
743 //=======================================================
744 // SetCameraParams
745 //=======================================================
746 bool mitk::OptitrackTrackingDevice::SetCameraParams(int exposure, int threshold , int intensity, int videoType )
747 {
749  return false;
750 }
751 
752 //=======================================================
753 // GetTool
754 //=======================================================
756 {
758  return nullptr;
759 }
760 
761 //=======================================================
762 // AddToolByFileName
763 //=======================================================
765 {
767  return false;
768 }
769 
770 #endif
#define OPTITRACK_FRAME_RATE
Time to refresh the tools location (ms)
virtual bool CloseConnection() override
Close the Connection with the Tracker. Also CleanUp the Optitrack variables using the API: TT_CleanUp...
Interface for all Tracking Tools.
itk::SmartPointer< Self > Pointer
OptitrackTrackingTool * GetOptitrackTool(unsigned int toolNumber) const
Return the tool pointer of the tool number toolNumber.
#define MITK_INFO
Definition: mitkLogMacros.h:22
double ScalarType
virtual bool StopTracking() override
Stop the Tracking Thread and tools will not longer be updated.
An object of this class represents an exception of the MITK-IGT module.
#define MITK_DEBUG
Definition: mitkLogMacros.h:26
DataCollection - Class to facilitate loading/accessing structured data.
bool LoadCalibration()
Load the Calibration file to the Optitrack System and set the cameras in calibrated locations...
static std::string GetOptitrackErrorMessage(int result)
Helper function to get the error messages from Optitrack API.
static ITK_THREAD_RETURN_TYPE ThreadStartTracking(void *data)
Start the Tracking Thread for the tools.
void SetCalibrationPath(std::string calibrationPath)
Sets the file where the calibration of the OptitrackTracker can be found.
#define OPTITRACK_ATTEMPTS
Maximum number of attempts for Initialization, Shutdown and CleanUp.
virtual bool StartTracking() override
Start to Track the tools already defined. If no tools are defined for this tracker, it returns an error. Tools can be added using either AddToolByDescriptionFile or AddToolsByConfigurationFiles.
bool SetCameraParams(int exposure, int threshold, int intensity, int videoType=4)
Set the Cameras Exposure, Threshold and Intensity of IR LEDs. By Default it set the Video type to 4: ...
virtual bool OpenConnection() override
Open the Connection with the Tracker. Calls LoadCalibration function and set the system up with the c...
#define MITK_WARN
Definition: mitkLogMacros.h:23
OptitrackTrackingDevice()
This function load a file with Tools definitions provided for the software.
An object of this class represents the a Tool tracked by Optitrack System. You can define the tool by...
Interface for all Tracking Devices.
static IGTTimeStamp * GetInstance()
returns a pointer to the current instance of mitkTimeStamp
TrackingTool * GetTool(unsigned int toolNumber) const override
Return the tool pointer of the tool number toolNumber.
void Start(itk::Object::Pointer device)
starts the time-acquisition
bool AddToolByDefinitionFile(std::string fileName)
Add a new tool using a text file which described the tool. The file must to have the next structure T...
Point< ScalarType, 3 > Point3D
Definition: mitkPoint.h:99
unsigned int GetToolCount() const override
Returns the number of defined tools.
#define mitkThrowException(classname)
void TrackTools()
Update each tool location in the list m_AllTools.
void Stop(itk::Object::Pointer device)
stops the time-acqusition
bool InitializeCameras()
Initialize the Optitrack System.
static itkEventMacro(BoundingShapeInteractionEvent, itk::AnyEvent) class MITKBOUNDINGSHAPE_EXPORT BoundingShapeInteractor Pointer New()
Basic interaction methods for mitk::GeometryData.