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