19 #ifdef MITK_USE_OPTITRACK_TRACKER
24 #include <NPTrackingTools.h>
30 :
mitk::InternalTrackingTool(),
33 MITK_DEBUG <<
"Creating OptitrackTrackingTool Object";
42 delete this->m_calibrationPoints;
44 MITK_DEBUG <<
"Deleting OptitrackTrackingTool Object";
53 MITK_INFO<<
"Name of the file for configuration: "<<nameFile;
54 this->m_fileConfiguration = nameFile;
55 int resultFscan, resultUpdate, resultCreateTrackable, resultTrackableTranslatePivot;
58 if(this->m_fileConfiguration.empty())
60 MITK_INFO <<
"Calibration File for Tool is empty";
66 FILE* calib_file = fopen(this->m_fileConfiguration.c_str(),
"r");
67 if (calib_file == NULL)
77 this->m_ToolName =
"";
78 char* aux =
new char[200];
79 resultFscan = fscanf(calib_file,
"%s\n",aux);
80 this->m_ToolName.append(aux);
82 if ((resultFscan < 1) || this->m_ToolName.empty())
84 MITK_INFO <<
"No name found in the tool configuration file";
89 MITK_INFO<<
"ToolName: " << this->m_ToolName;
92 resultFscan = fscanf(calib_file,
"%i\n",&(this->m_numMarkers));
93 if (this->m_numMarkers < 3)
95 MITK_INFO <<
"The minimum number for define a tool is 3 markers";
100 MITK_INFO<<
"\tNumer of Markers: " << this->m_numMarkers;
103 this->m_calibrationPoints =
new float[3*this->m_numMarkers];
105 for(
int i=0; i<this->m_numMarkers; i++)
107 resultFscan = fscanf(calib_file,
"%fe ", &this->m_calibrationPoints[i*3+0]);
110 MITK_INFO <<
"Cannot read X location for marker " << i;
115 resultFscan = fscanf(calib_file,
"%fe ", &this->m_calibrationPoints[i*3+1]);
118 MITK_INFO <<
"Cannot read Y location for marker " << i;
123 resultFscan = fscanf(calib_file,
"%fe\n", &this->m_calibrationPoints[i*3+2]);
126 MITK_INFO <<
"Cannot read Z location for marker " << 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];
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;
140 this->m_pivotPoint =
new float[3];
141 resultFscan = fscanf(calib_file,
"%fe ", &this->m_pivotPoint[0]);
144 MITK_INFO <<
"Cannot read X location for Pivot Point ";
149 resultFscan = fscanf(calib_file,
"%fe ", &this->m_pivotPoint[1]);
152 MITK_INFO <<
"Cannot read Y location for Pivot Point " ;
157 resultFscan = fscanf(calib_file,
"%fe\n", &this->m_pivotPoint[2]);
160 MITK_INFO <<
"Cannot read Z location for Pivot Point " ;
166 MITK_INFO <<
"\t\t X: " << this->m_pivotPoint[0] <<
" Y: " << this->m_pivotPoint[1] <<
" Z: " << this->m_pivotPoint[2];
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;
174 this->m_ID = this->get_IDnext();
179 resultCreateTrackable = TT_CreateTrackable(m_ToolName.c_str(), this->m_ID,this->m_numMarkers,this->m_calibrationPoints);
180 if(NPRESULT_SUCCESS == resultCreateTrackable)
182 MITK_INFO <<
"Trackable Created Successfully";
194 resultUpdate = TT_Update();
195 if(NPRESULT_SUCCESS == resultUpdate)
197 resultTrackableTranslatePivot = TT_TrackableTranslatePivot(this->m_ID,this->m_pivotPoint[0],this->m_pivotPoint[1],this->m_pivotPoint[2]);
198 if(NPRESULT_SUCCESS == resultCreateTrackable)
200 MITK_INFO <<
"Pivot Translation Successfull";
229 int num_trackables = -1;
234 resultUpdate = TT_Update();
235 if(NPRESULT_SUCCESS == resultUpdate)
237 num_trackables = TT_TrackableCount();
239 if(num_trackables > -1)
241 return num_trackables;
258 return num_trackables;
267 int resultRemoveTrackable;
268 resultRemoveTrackable = TT_RemoveTrackable(this->m_ID);
270 if(resultRemoveTrackable != NPRESULT_SUCCESS)
279 MITK_INFO<<
"Trackable " << this->m_ToolName <<
" removed";
292 this->m_Position[0] = position[0];
293 this->m_Position[1] = position[1];
294 this->m_Position[2] = position[2];
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();
317 positionOutput[0] = this->m_Position[0];
318 positionOutput[1] = this->m_Position[1];
319 positionOutput[2] = this->m_Position[2];
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();
342 TT_SetTrackableEnabled(this->m_ID,
true);
344 if(TT_TrackableEnabled(this->m_ID) ==
true)
346 this->m_Enabled =
true;
351 this->m_Enabled =
false;
365 TT_SetTrackableEnabled(this->m_ID,
false);
367 if(TT_TrackableEnabled(this->m_ID) ==
true)
369 this->m_Enabled =
false;
374 this->m_Enabled =
true;
389 return TT_TrackableEnabled(this->m_ID);
399 return this->m_DataValid;
409 return this->m_TrackingError;
421 this->m_TrackingError = error;
431 this->m_DataValid = validate;
440 float yaw,pitch,roll;
443 if(TT_Update() == NPRESULT_SUCCESS)
445 if(this->IsEnabled())
447 TT_TrackableLocation(this->m_ID, &data[0], &data[1], &data[2],
448 &data[3], &data[4], &data[5], &data[6],
449 &yaw, &pitch, &roll);
462 this->m_Position[0] = data[0]*1000;
463 this->m_Position[1] = data[1]*1000;
464 this->m_Position[2] = -data[2]*1000;
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];
471 this->SetDataValid(
true);
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];
479 this->SetDataValid(
false);
480 MITK_DEBUG <<
"Trackable: "<< this->m_ToolName <<
"is not Tracked";
485 this->SetDataValid(
false);
An object of this class represents an exception of the MITK-IGT module.
DataCollection - Class to facilitate loading/accessing structured data.
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.
vnl_quaternion< ScalarType > Quaternion
#define mitkThrowException(classname)
MITKCORE_EXPORT const ScalarType eps