Medical Imaging Interaction Toolkit  2016.11.0
Medical Imaging Interaction Toolkit
mitkOptitrackTrackingTool.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 ===================================================================*/
16 
18 
19 #ifdef MITK_USE_OPTITRACK_TRACKER
20 
24 #include <NPTrackingTools.h>
25 
26 //=======================================================
27 // Constructor
28 //=======================================================
30  : mitk::InternalTrackingTool(),
31  m_ID(-1)
32 {
33  MITK_DEBUG << "Creating OptitrackTrackingTool Object";
34  this->m_FLE = 0.0;
35 }
36 
37 //=======================================================
38 // Destructor
39 //=======================================================
41 {
42  delete this->m_calibrationPoints;
43  delete m_pivotPoint;
44  MITK_DEBUG << "Deleting OptitrackTrackingTool Object";
45 }
46 
47 //=======================================================
48 // SetToolByFileName
49 //=======================================================
50 bool mitk::OptitrackTrackingTool::SetToolByFileName(std::string nameFile)
51 {
52  MITK_DEBUG << "SetToolByFileName";
53  MITK_INFO<<"Name of the file for configuration: "<<nameFile;
54  this->m_fileConfiguration = nameFile;
55  int resultFscan, resultUpdate, resultCreateTrackable, resultTrackableTranslatePivot;
56 
57  // Check the file path
58  if(this->m_fileConfiguration.empty())
59  {
60  MITK_INFO << "Calibration File for Tool is empty";
61  mitkThrowException(mitk::IGTException) << "Calibration File for Tool is empty";
62  return false;
63  }
64 
65  // Open the file
66  FILE* calib_file = fopen(this->m_fileConfiguration.c_str(),"r");
67  if (calib_file == NULL)
68  {
69  MITK_INFO << "Error using opening file";
70  mitkThrowException(mitk::IGTException) << "Cannot open configuration file";
71  return false;
72  }
73 
74  MITK_DEBUG<<"Reading configuration file...";
75 
76  // Get the name
77  this->m_ToolName = "";
78  char* aux = new char[200];
79  resultFscan = fscanf(calib_file,"%s\n",aux);
80  this->m_ToolName.append(aux);
81  delete aux;
82  if ((resultFscan < 1) || this->m_ToolName.empty())
83  {
84  MITK_INFO << "No name found in the tool configuration file";
85  mitkThrowException(mitk::IGTException) << "No name found in the tool configuration file";
86  return false;
87  }
88 
89  MITK_INFO<<"ToolName: " << this->m_ToolName;
90 
91  // Get the number of of points
92  resultFscan = fscanf(calib_file,"%i\n",&(this->m_numMarkers));
93  if (this->m_numMarkers < 3)
94  {
95  MITK_INFO << "The minimum number for define a tool is 3 markers";
96  mitkThrowException(mitk::IGTException) << "Tool has less than 3 markers";
97  return false;
98  }
99 
100  MITK_INFO<<"\tNumer of Markers: " << this->m_numMarkers;
101 
102  // Read the Calibration Point locations and save them
103  this->m_calibrationPoints = new float[3*this->m_numMarkers];
104 
105  for(int i=0; i<this->m_numMarkers; i++)
106  {
107  resultFscan = fscanf(calib_file,"%fe ", &this->m_calibrationPoints[i*3+0]);
108  if (resultFscan < 1)
109  {
110  MITK_INFO << "Cannot read X location for marker " << i;
111  mitkThrowException(mitk::IGTException) << "Cannot read X location for marker " << i;
112  return false;
113  }
114 
115  resultFscan = fscanf(calib_file,"%fe ", &this->m_calibrationPoints[i*3+1]);
116  if (resultFscan < 1)
117  {
118  MITK_INFO << "Cannot read Y location for marker " << i;
119  mitkThrowException(mitk::IGTException) << "Cannot read Y location for marker " << i;
120  return false;
121  }
122 
123  resultFscan = fscanf(calib_file,"%fe\n", &this->m_calibrationPoints[i*3+2]);
124  if (resultFscan < 1)
125  {
126  MITK_INFO << "Cannot read Z location for marker " << i;
127  mitkThrowException(mitk::IGTException) << "Cannot read Z location for marker " << i;
128  return false;
129  }
130  MITK_DEBUG << "\t\tMarker " << i;
131  MITK_DEBUG << "\t\t X: " << this->m_calibrationPoints[i*3+0] << " Y: " << this->m_calibrationPoints[i*3+1] << " Z: " << this->m_calibrationPoints[i*3+2];
132 
133  this->m_calibrationPoints[i*3+0] = this->m_calibrationPoints[i*3+0]/1000;
134  this->m_calibrationPoints[i*3+1] = this->m_calibrationPoints[i*3+1]/1000;
135  this->m_calibrationPoints[i*3+2] = -this->m_calibrationPoints[i*3+2]/1000;// Optitrack works with Left Handed System
136 
137  }
138 
139  // Read the Pivot Point location
140  this->m_pivotPoint = new float[3];
141  resultFscan = fscanf(calib_file,"%fe ", &this->m_pivotPoint[0]);
142  if (resultFscan < 1)
143  {
144  MITK_INFO << "Cannot read X location for Pivot Point ";
145  mitkThrowException(mitk::IGTException) << "Cannot read X location for Pivot Point ";
146  return false;
147  }
148 
149  resultFscan = fscanf(calib_file,"%fe ", &this->m_pivotPoint[1]);
150  if (resultFscan < 1)
151  {
152  MITK_INFO << "Cannot read Y location for Pivot Point " ;
153  mitkThrowException(mitk::IGTException) << "Cannot read Y location for Pivot Point ";
154  return false;
155  }
156 
157  resultFscan = fscanf(calib_file,"%fe\n", &this->m_pivotPoint[2]);
158  if (resultFscan < 1)
159  {
160  MITK_INFO << "Cannot read Z location for Pivot Point " ;
161  mitkThrowException(mitk::IGTException) << "Cannot read Z location for Pivot Point ";
162  return false;
163  }
164 
165  MITK_INFO << "\tPivotPoint " ;
166  MITK_INFO << "\t\t X: " << this->m_pivotPoint[0] << " Y: " << this->m_pivotPoint[1] << " Z: " << this->m_pivotPoint[2];
167 
168  // mm -> m
169  this->m_pivotPoint[0] = this->m_pivotPoint[0]/1000;
170  this->m_pivotPoint[1] = this->m_pivotPoint[1]/1000;
171  this->m_pivotPoint[2] = -this->m_pivotPoint[2]/1000;
172 
173  // get the ID for next tool in Optitrack System
174  this->m_ID = this->get_IDnext();
175 
176  // Create the Tool
177  for( int i=OPTITRACK_ATTEMPTS; i>0; i--)
178  {
179  resultCreateTrackable = TT_CreateTrackable(m_ToolName.c_str(), this->m_ID,this->m_numMarkers,this->m_calibrationPoints);
180  if(NPRESULT_SUCCESS == resultCreateTrackable)
181  {
182  MITK_INFO << "Trackable Created Successfully";
183  i = -1;
184  }
185  else
186  {
188  MITK_DEBUG << "Trying again...";
189  }
190  }
191 
192  for( int i=OPTITRACK_ATTEMPTS; i>0; i--)
193  {
194  resultUpdate = TT_Update();
195  if(NPRESULT_SUCCESS == resultUpdate)
196  {
197  resultTrackableTranslatePivot = TT_TrackableTranslatePivot(this->m_ID,this->m_pivotPoint[0],this->m_pivotPoint[1],this->m_pivotPoint[2]);
198  if(NPRESULT_SUCCESS == resultCreateTrackable)
199  {
200  MITK_INFO << "Pivot Translation Successfull";
201  fclose(calib_file);
202  i=-1;
203  return true;
204  }
205  else
206  {
208  MITK_DEBUG << "Trying again...";
209  }
210  }
211  else
212  {
214  MITK_DEBUG << "Trying again...";
215  }
216  }
217 
218  MITK_INFO << "Cannot create tool ";
219  mitkThrowException(mitk::IGTException) << "Cannot create tool ";
220  return false;
221 }
222 
223 //=======================================================
224 // get_IDnext
225 //=======================================================
227 {
228  MITK_DEBUG << "get_ID";
229  int num_trackables = -1;
230  int resultUpdate;
231 
232  for( int i=OPTITRACK_ATTEMPTS; i>0; i--)
233  {
234  resultUpdate = TT_Update();
235  if(NPRESULT_SUCCESS == resultUpdate)
236  {
237  num_trackables = TT_TrackableCount();
238  MITK_DEBUG << " Next ID: " << num_trackables;
239  if(num_trackables > -1)
240  {
241  return num_trackables;
242  }
243  else
244  {
245  MITK_DEBUG << "get_IDnext failed";
246  mitkThrowException(mitk::IGTException) << "get_IDnext failed";
247  }
248 
249  }
250  else
251  {
253  MITK_DEBUG << "Trying again...";
254  }
255  }
256 
257  mitkThrowException(mitk::IGTException) << "get_IDnext failed";
258  return num_trackables;
259 }
260 
261 //=======================================================
262 // DeleteTrackable
263 //=======================================================
265 {
266  MITK_DEBUG << "DeleteTrackable";
267  int resultRemoveTrackable;
268  resultRemoveTrackable = TT_RemoveTrackable(this->m_ID);
269 
270  if(resultRemoveTrackable != NPRESULT_SUCCESS)
271  {
272  MITK_INFO << "Cannot Remove Trackable";
274  mitkThrowException(mitk::IGTException) << "Cannot Remove Trackable" << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultRemoveTrackable);
275  return false;
276  }
277  else
278  {
279  MITK_INFO<<"Trackable " << this->m_ToolName << " removed";
280  }
281 
282  return true;
283 }
284 
285 //=======================================================
286 // SetPosition
287 //=======================================================
289 {
290  MITK_DEBUG << "SetPosition";
291  // sets the position
292  this->m_Position[0] = position[0];
293  this->m_Position[1] = position[1];
294  this->m_Position[2] = position[2];
295 }
296 
297 //=======================================================
298 // SetOrientation
299 //=======================================================
301 {
302  MITK_DEBUG << "SetOrientation";
303  // sets the orientation as a quaternion
304  this->m_Orientation.x() = orientation.x();
305  this->m_Orientation.y() = orientation.y();
306  this->m_Orientation.z() = orientation.z();
307  this->m_Orientation.r() = orientation.r();
308 }
309 
310 //=======================================================
311 // GetPosition
312 //=======================================================
314 {
315  MITK_DEBUG << "GetPosition";
316  // returns the current position of the tool as an array of three floats (in the tracking device coordinate system)
317  positionOutput[0] = this->m_Position[0];
318  positionOutput[1] = this->m_Position[1];
319  positionOutput[2] = this->m_Position[2];
320 }
321 
322 //=======================================================
323 // GetOrientation
324 //=======================================================
326 {
327  MITK_DEBUG << "GetOrientation";
328  // returns the current orientation of the tool as a quaternion (in the tracking device coordinate system)
329  orientation.x() = this->m_Orientation.x();
330  orientation.y() = this->m_Orientation.y();
331  orientation.z() = this->m_Orientation.z();
332  orientation.r() = this->m_Orientation.r();
333 }
334 
335 //=======================================================
336 // Enable
337 //=======================================================
339 {
340  MITK_DEBUG << "Enable";
341  // enable the tool, so that it will be tracked. Returns true if enabling was successfull
342  TT_SetTrackableEnabled(this->m_ID, true);
343 
344  if(TT_TrackableEnabled(this->m_ID) == true)
345  {
346  this->m_Enabled = true;
347  return true;
348  }
349  else
350  {
351  this->m_Enabled = false;
352  MITK_INFO << "Enable failed";
353  mitkThrowException(mitk::IGTException) << "Enable failed";
354  return false;
355  }
356 }
357 
358 //=======================================================
359 // Disable
360 //=======================================================
362 {
363  MITK_DEBUG << "Disable";
364  // disables the tool, so that it will not be tracked anymore. Returns true if disabling was successfull
365  TT_SetTrackableEnabled(this->m_ID, false);
366 
367  if(TT_TrackableEnabled(this->m_ID) == true)
368  {
369  this->m_Enabled = false;
370  return true;
371  }
372  else
373  {
374  this->m_Enabled = true;
375  MITK_INFO << "Disable failed";
376  mitkThrowException(mitk::IGTException) << "Disable failed";
377  return false;
378  }
379 
380 }
381 
382 //=======================================================
383 // IsEnabled
384 //=======================================================
386 {
387  MITK_DEBUG << "IsEnabled";
388  // returns whether the tool is enabled or disabled
389  return TT_TrackableEnabled(this->m_ID);
390 }
391 
392 //=======================================================
393 // IsDataValid
394 //=======================================================
396 {
397  MITK_DEBUG << "IsDataValid";
398  // returns true if the current position data is valid (no error during tracking, tracking error below threshold, ...)
399  return this->m_DataValid;
400 }
401 
402 //=======================================================
403 // GetTrackingError
404 //=======================================================
406 {
407  MITK_DEBUG << "GetTrackingError";
408  // return one value that corresponds to the overall tracking error. The dimension of this value is specific to each tracking device
409  return this->m_TrackingError;
410 }
411 
412 //=======================================================
413 // SetTrackingError
414 //=======================================================
416 {
417  MITK_DEBUG << "GetTrackingError";
418  //< sets the tracking error
419  //this->m_FLE = error;
420  //this->UpdateError;
421  this->m_TrackingError = error;
422 }
423 
424 //=======================================================
425 // SetDataValid
426 //=======================================================
428 {
429  MITK_DEBUG << "SetDataValid";
430  // sets if the tracking data (position & Orientation) is valid
431  this->m_DataValid = validate;
432 }
433 
434 //=======================================================
435 // updateTool
436 //=======================================================
438 {
439  MITK_DEBUG << "updateTool";
440  float yaw,pitch,roll;
441  float data[7];
442 
443  if(TT_Update() == NPRESULT_SUCCESS)
444  {
445  if(this->IsEnabled())
446  {
447  TT_TrackableLocation(this->m_ID, &data[0], &data[1], &data[2], // Position
448  &data[3], &data[4], &data[5], &data[6], // Orientation
449  &yaw, &pitch, &roll); // Orientation
450 
451  //for( int i=0; i<7; i++)
452  //{
453  // if(boost::math::isinf<float>(data[i])) // Possible Tracking check for INF numbers
454  // {
455  // this->SetDataValid(false);
456  // MITK_DEBUG << "Data set to INF by the system";
457  // return;
458  // }
459  //}
460 
461  // m -> mm
462  this->m_Position[0] = data[0]*1000;
463  this->m_Position[1] = data[1]*1000;
464  this->m_Position[2] = -data[2]*1000; // Correction from LeftHanded to RightHanded system
465 
466  this->m_Orientation.x() = data[3];
467  this->m_Orientation.y() = data[4];
468  this->m_Orientation.z() = -data[5];
469  this->m_Orientation.r() = data[6];
470 
471  this->SetDataValid(true);
472 
473  MITK_DEBUG << this->m_Position[0] << " " << this->m_Position[1] << " " << this->m_Position[2];
474  MITK_DEBUG << data[3] << " " << data[4] << " " << data[5] << " " << data[6];
475 
476  }
477  else
478  {
479  this->SetDataValid(false);
480  MITK_DEBUG << "Trackable: "<< this->m_ToolName << "is not Tracked";
481  }
482  }
483  else
484  {
485  this->SetDataValid(false);
486  MITK_DEBUG << "Update Failed";
487  }
488 }
489 
490 //=======================================================
491 // IF Optitrack is not installed set functions to warnings
492 //=======================================================
493 #else
494 
495 //=======================================================
496 // Constructor
497 //=======================================================
500  m_ID(-1)
501 {
503 }
504 
505 //=======================================================
506 // Destructor
507 //=======================================================
509 {
511 }
512 
513 //=======================================================
514 // SetToolByFileName
515 //=======================================================
517 {
519  return false;
520 }
521 
522 //=======================================================
523 // get_IDnext
524 //=======================================================
526 {
528  return -1;
529 }
530 
531 //=======================================================
532 // DeleteTrackable
533 //=======================================================
535 {
537  return false;
538 }
539 
540 //=======================================================
541 // SetPosition
542 //=======================================================
544 {
546 }
547 
548 //=======================================================
549 // SetOrientation
550 //=======================================================
552 {
554 }
555 
556 //=======================================================
557 // GetPosition
558 //=======================================================
560 {
562 }
563 
564 //=======================================================
565 // GetOrientation
566 //=======================================================
568 {
570 }
571 
572 //=======================================================
573 // Enable
574 //=======================================================
576 {
578  return false;
579 }
580 
581 //=======================================================
582 // Disable
583 //=======================================================
585 {
587  return false;
588 }
589 
590 //=======================================================
591 // IsEnabled
592 //=======================================================
594 {
596  return false;
597 }
598 
599 //=======================================================
600 // IsDataValid
601 //=======================================================
603 {
605  return false;
606 }
607 
608 //=======================================================
609 // GetTrackingError
610 //=======================================================
612 {
614  return 0.0;
615 }
616 
617 //=======================================================
618 // SetTrackingError
619 //=======================================================
621 {
623 }
624 
625 //=======================================================
626 // SetDataValid
627 //=======================================================
629 {
631 }
632 
633 //=======================================================
634 // updateTool
635 //=======================================================
637 {
639 }
640 
641 
642 #endif
bool Enable() override
Set the tool enabled for tracking.
#define MITK_INFO
Definition: mitkLogMacros.h:22
int get_IDnext()
Ask API the next number of defined tool.
double ScalarType
void SetTrackingError(float FLEerror) override
Set the FLE (Fiducial Localization Error) for the tool.
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.
void SetPosition(mitk::Point3D position, ScalarType eps=0.0)
Set the position to a given one.
void GetOrientation(mitk::Quaternion &orientation) const override
Get the orientation of the tool using quaternion nomenclature.
static std::string GetOptitrackErrorMessage(int result)
Helper function to get the error messages from Optitrack API.
#define OPTITRACK_ATTEMPTS
Maximum number of attempts for Initialization, Shutdown and CleanUp.
OptitrackTrackingTool()
Constructor of the class.
bool DeleteTrackable()
Delete the tool from the list of tools inside API Optitrack.
bool SetToolByFileName(std::string nameFile)
Define the tool by a calibration File. The file must to have the next structure. Makers locations mus...
implements TrackingTool interface
bool Disable() override
Set the tool disabled for tracking.
#define MITK_WARN
Definition: mitkLogMacros.h:23
bool IsDataValid() const override
Check if the data of the tool is valid.
bool IsEnabled() const override
Check if the tool is enabled (true) or not.
float GetTrackingError() const override
Get the expectated error in the tracked tool.
vnl_quaternion< ScalarType > Quaternion
void updateTool()
Update location and orientation of the tool.
void SetOrientation(mitk::Quaternion orientation, ScalarType eps=0.0)
Set the orientation to a given one using a quaternion nomenclature.
void GetPosition(mitk::Point3D &position) const override
Get the position of the tool.
#define mitkThrowException(classname)
MITKCORE_EXPORT const ScalarType eps
void SetDataValid(bool _arg) override
Set the valid flag for tracking data to true.
~OptitrackTrackingTool()
Destructor of the class.