diff --git a/Modules/IGT/TrackingDevices/mitkOptitrackTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkOptitrackTrackingDevice.cpp index 093fcfde08..5901534b3b 100644 --- a/Modules/IGT/TrackingDevices/mitkOptitrackTrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkOptitrackTrackingDevice.cpp @@ -1,770 +1,770 @@ /*=================================================================== The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See LICENSE.txt or http://www.mitk.org for details. ===================================================================*/ #include "mitkOptitrackTrackingDevice.h" #include #ifdef MITK_USE_OPTITRACK_TRACKER /** * \brief API library header for Optitrack Systems */ #include //======================================================= // Static method: IsDeviceInstalled //======================================================= bool mitk::OptitrackTrackingDevice::IsDeviceInstalled() { return true; } //======================================================= // Constructor //======================================================= mitk::OptitrackTrackingDevice::OptitrackTrackingDevice() : mitk::TrackingDevice(), m_initialized(false) { // Set the MultiThread and Mutex this->m_MultiThreader = itk::MultiThreader::New(); this->m_ToolsMutex = itk::FastMutexLock::New(); //Set the mitk device information SetData(mitk::DeviceDataNPOptitrack); //Clear List of tools this->m_AllTools.clear(); } //======================================================= // Destructor //======================================================= mitk::OptitrackTrackingDevice::~OptitrackTrackingDevice() { MITK_DEBUG << "Deleting OptitrackTrackingDevice"; int result; // If device is in Tracking mode, stop the Tracking firts if (this->GetState() == mitk::TrackingDevice::Tracking) { MITK_DEBUG << "OptitrackTrackingDevice in Tracking State -> Stopping Tracking"; result = this->StopTracking(); if(result == NPRESULT_SUCCESS){ MITK_INFO << "OptitrackTrackingDevice Stopped"; } else { MITK_INFO << "Error during Stopping"; mitkThrowException(mitk::IGTException) << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(result); } } // If device is Ready, Close the connection to device and release the memory if (this->GetState() == mitk::TrackingDevice::Ready) { MITK_DEBUG << "OptitrackTrackingDevice in Ready State -> Closing the Connection"; result = this->CloseConnection(); if(result) { MITK_INFO << "OptitrackTrackingDevice Connection closed"; } else { MITK_DEBUG << "Error during Closing Connection"; mitkThrowException(mitk::IGTException) << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(result); } } // Set the device off m_initialized = false; // Change State to Setup this->SetState(mitk::TrackingDevice::Setup); MITK_DEBUG <<"OptitrackTrackingDevice deleted successfully"; } //======================================================= // OpenConnection //======================================================= bool mitk::OptitrackTrackingDevice::OpenConnection() { // Not initialize the system twice. if(!m_initialized) { MITK_DEBUG << "Initialize Optitrack Tracking System"; if( this->InitializeCameras() ) { m_initialized = true; // Set the initialized variable to true this->SetState(mitk::TrackingDevice::Ready); if(this->m_calibrationPath.empty()){ MITK_INFO << "Numer of connected cameras = " << TT_CameraCount(); MITK_WARN << "Attention: No calibration File defined !!"; return m_initialized; } else { this->LoadCalibration(); } } else { m_initialized = false; // Set the initialized variable to false this->SetState(mitk::TrackingDevice::Setup); // Set the State to Setup MITK_INFO << "Device initialization failed. Device is still in setup state"; mitkThrowException(mitk::IGTException) << "Device initialization failed. Device is still in setup state"; } } //this->LoadCalibration(); return m_initialized; } //======================================================= // InitializeCameras //======================================================= bool mitk::OptitrackTrackingDevice::InitializeCameras() { MITK_DEBUG << "Initialize Optitrack"; int result; result = TT_Initialize(); // Initialize the cameras if(result == NPRESULT_SUCCESS) { MITK_DEBUG << "Optitrack Initialization Succeed"; return true; } else { MITK_DEBUG << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(result); // If not succeed after OPTITRACK_ATTEMPTS times launch exception MITK_INFO << "Optitrack Tracking System cannot be initialized \n" << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(result); mitkThrowException(mitk::IGTException) << "Optitrack Tracking System cannot be initialized \n" << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(result); return false; } } //======================================================= // LoadCalibration //======================================================= bool mitk::OptitrackTrackingDevice::LoadCalibration() { MITK_DEBUG << "Loading System Calibration"; int resultLoadCalibration; // Check the file path if(this->m_calibrationPath.empty()){ MITK_INFO << "Calibration Path is empty"; mitkThrowException(mitk::IGTException) << "Calibration Path is empty"; return false; } // Once the system is ready and Initialized , a calibration file is loaded. if(this->m_initialized) { for( int i=OPTITRACK_ATTEMPTS; i>0; i--) { resultLoadCalibration = TT_LoadCalibration(this->m_calibrationPath.c_str()); if(resultLoadCalibration != NPRESULT_SUCCESS) { MITK_DEBUG << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultLoadCalibration); MITK_DEBUG << "Trying again..."; } else { MITK_DEBUG << "Calibration file has been loaded successfully"; return true; } } MITK_INFO << "System cannot load a calibration file"; mitkThrowException(mitk::IGTException) << "System cannot load a calibration file"; } else { MITK_INFO << "System is not ready for load a calibration file because it has not been initialized yet"; mitkThrowException(mitk::IGTException) << "System is not ready for load a calibration file because it has not been initialized yet"; return false; } // Never reach this point return false; } //======================================================= // SetCalibrationPath //======================================================= void mitk::OptitrackTrackingDevice::SetCalibrationPath(std::string calibrationPath){ MITK_DEBUG << "SetcalibrationPath"; MITK_DEBUG << calibrationPath; // Check the file path if(calibrationPath.empty()) { MITK_INFO << "Calibration Path is empty"; //mitkThrowException(mitk::IGTException) << "Calibration Path is empty"; return; } this->m_calibrationPath = calibrationPath; MITK_INFO << "Calibration Path has been updated to: " << this->m_calibrationPath; return; } //======================================================= // CloseConnection //======================================================= bool mitk::OptitrackTrackingDevice::CloseConnection() { MITK_DEBUG << "CloseConnection"; int resultStop, resultShutdown; if(m_initialized) // Close connection if the System was initialized first { if(this->GetState() == mitk::TrackingDevice::Tracking) { MITK_DEBUG << "Device state: Tracking -> Stoping the Tracking"; resultStop = this->StopTracking(); //Stop tracking on close } this->SetState(mitk::OptitrackTrackingDevice::Setup); for( int i=OPTITRACK_ATTEMPTS; i>0; i--) { TT_ClearTrackableList(); resultShutdown = TT_Shutdown(); if(resultShutdown == NPRESULT_SUCCESS) { MITK_DEBUG << "System has been Shutdown Correctly"; Sleep(2000); return true; } else { MITK_DEBUG << "System cannot ShutDown now. Trying again..."; } } MITK_INFO << "System cannot ShutDown now"; mitkThrowException(mitk::IGTException) << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultShutdown); return false; } else { MITK_INFO << "System has not been initialized. Close connection cannot be done"; mitkThrowException(mitk::IGTException) << "System has not been initialized. Close connection cannot be done"; return false; } return false; } //======================================================= // StartTracking //======================================================= bool mitk::OptitrackTrackingDevice::StartTracking() { MITK_DEBUG << "StartTracking"; bool resultIsTrackableTracked; if (this->GetState() != mitk::TrackingDevice::Ready) { MITK_INFO << "System is not in State Ready -> Cannot StartTracking"; mitkThrowException(mitk::IGTException) << "System is not in State Ready -> Cannot StartTracking"; return false; } this->SetState(mitk::TrackingDevice::Tracking); // Change the m_StopTracking Variable to false this->m_StopTrackingMutex->Lock(); this->m_StopTracking = false; this->m_StopTrackingMutex->Unlock(); /****************************************************************************** ############################################################################### TODO: check the timestamp from the Optitrack API ############################################################################### ******************************************************************************/ mitk::IGTTimeStamp::GetInstance()->Start(this); // Launch multiThreader using the Function ThreadStartTracking that executes the TrackTools() method m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method // Information for the user if(GetToolCount() == 0) MITK_INFO << "No tools are defined"; for ( int i = 0; i < GetToolCount(); ++i) // use mutexed methods to access tool container { resultIsTrackableTracked = TT_IsTrackableTracked(i); if(resultIsTrackableTracked) { MITK_DEBUG << "Trackable " << i << " is inside the Tracking Volume and it is Tracked"; } else { MITK_DEBUG << "Trackable " << i << " is not been tracked. Check if it is inside the Tracking volume"; } } return true; } //======================================================= // StopTracking //======================================================= bool mitk::OptitrackTrackingDevice::StopTracking() { MITK_DEBUG << "StopTracking"; if (this->GetState() == mitk::TrackingDevice::Tracking) // Only if the object is in the correct state { //Change the StopTracking value m_StopTrackingMutex->Lock(); // m_StopTracking is used by two threads, so we have to ensure correct thread handling m_StopTrackingMutex->Unlock(); this->SetState(mitk::TrackingDevice::Ready); } else { m_TrackingFinishedMutex->Unlock(); MITK_INFO << "System is not in State Tracking -> Cannot StopTracking"; mitkThrowException(mitk::IGTException) << "System is not in State Tracking -> Cannot StopTracking"; return false; } /****************************************************************************** ############################################################################### TODO: check the timestamp from the Optitrack API ############################################################################### ******************************************************************************/ mitk::IGTTimeStamp::GetInstance()->Stop(this); m_TrackingFinishedMutex->Unlock(); return true; } //======================================================= // ThreadStartTracking //======================================================= ITK_THREAD_RETURN_TYPE mitk::OptitrackTrackingDevice::ThreadStartTracking(void* pInfoStruct) { MITK_DEBUG << "ThreadStartTracking"; /* extract this pointer from Thread Info structure */ struct itk::MultiThreader::ThreadInfoStruct * pInfo = (struct itk::MultiThreader::ThreadInfoStruct*)pInfoStruct; if (pInfo == nullptr) { return ITK_THREAD_RETURN_VALUE; } if (pInfo->UserData == nullptr) { return ITK_THREAD_RETURN_VALUE; } OptitrackTrackingDevice *trackingDevice = static_cast(pInfo->UserData); if (trackingDevice != nullptr) { // Call the TrackTools function in this thread trackingDevice->TrackTools(); } else { mitkThrowException(mitk::IGTException) << "In ThreadStartTracking(): trackingDevice is nullptr"; } trackingDevice->m_ThreadID = -1; // reset thread ID because we end the thread here return ITK_THREAD_RETURN_VALUE; } //======================================================= // GetOptitrackTool //======================================================= mitk::OptitrackTrackingTool* mitk::OptitrackTrackingDevice::GetOptitrackTool( unsigned int toolNumber) const { MITK_DEBUG << "ThreadStartTracking"; OptitrackTrackingTool* t = nullptr; MutexLockHolder toolsMutexLockHolder(*m_ToolsMutex); // lock and unlock the mutex if(toolNumber < m_AllTools.size()) { t = m_AllTools.at(toolNumber); } else { MITK_INFO << "The tool numbered " << toolNumber << " does not exist"; mitkThrowException(mitk::IGTException) << "The tool numbered " << toolNumber << " does not exist"; } return t; } //======================================================= // GetToolCount //======================================================= unsigned int mitk::OptitrackTrackingDevice::GetToolCount() const { MITK_DEBUG << "GetToolCount"; MutexLockHolder lock(*m_ToolsMutex); // lock and unlock the mutex return ( int)(this->m_AllTools.size()); } //======================================================= // TrackTools //======================================================= void mitk::OptitrackTrackingDevice::TrackTools() { MITK_DEBUG << "TrackTools"; Point3D position; ScalarType t = 0.0; try { 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 this->m_StopTrackingMutex->Lock(); // update the local copy of m_StopTracking localStopTracking = this->m_StopTracking; /* lock the TrackingFinishedMutex to signal that the execution rights are now transfered to the tracking thread */ if (!localStopTracking) { m_TrackingFinishedMutex->Lock(); } this->m_StopTrackingMutex->Unlock(); while ((this->GetState() == mitk::TrackingDevice::Tracking) && (localStopTracking == false)) { // For each Tracked Tool update the position and orientation for ( int i = 0; i < GetToolCount(); ++i) // use mutexed methods to access tool container { OptitrackTrackingTool* currentTool = this->GetOptitrackTool(i); if(currentTool != nullptr) { currentTool->updateTool(); MITK_DEBUG << "Tool number " << i << " updated position"; } else { MITK_DEBUG << "Get data from tool number " << i << " failed"; mitkThrowException(mitk::IGTException) << "Get data from tool number " << i << " failed"; } } /* Update the local copy of m_StopTracking */ this->m_StopTrackingMutex->Lock(); localStopTracking = m_StopTracking; this->m_StopTrackingMutex->Unlock(); Sleep(OPTITRACK_FRAME_RATE); } // tracking ends if we pass this line m_TrackingFinishedMutex->Unlock(); // transfer control back to main thread } catch(...) { m_TrackingFinishedMutex->Unlock(); this->StopTracking(); mitkThrowException(mitk::IGTException) << "Error while trying to track tools. Thread stopped."; } } //======================================================= // SetCameraParams //======================================================= bool mitk::OptitrackTrackingDevice::SetCameraParams(int exposure, int threshold , int intensity, int videoType ) { MITK_DEBUG << "SetCameraParams"; if(this->m_initialized) { int num_cams = 0; int resultUpdate; bool resultSetCameraSettings; for( int i=OPTITRACK_ATTEMPTS; i>0; i--) { resultUpdate = TT_Update(); // Get Update for the Optitrack API if(resultUpdate == NPRESULT_SUCCESS) { MITK_DEBUG << "Update Succeed"; num_cams = TT_CameraCount(); i = 0; } else { MITK_DEBUG << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultUpdate); MITK_DEBUG << "Trying again..."; Sleep(30); } } // If no cameras are connected if(num_cams == 0) { MITK_DEBUG << "No cameras are connected to the device"; return false; mitkThrowException(mitk::IGTException) << "No cameras are connected to the device"; } for(int cam = 0; cam < num_cams; cam++) // for all connected cameras { for( int i=OPTITRACK_ATTEMPTS; i>0; i--) { resultUpdate = TT_Update(); // Get Update for the Optitrack API if(resultUpdate == NPRESULT_SUCCESS) { MITK_DEBUG << "Update Succeed for camera number " << cam; resultSetCameraSettings = TT_SetCameraSettings(cam,videoType,exposure,threshold,intensity); if(resultSetCameraSettings) { MITK_INFO << "Camera # "< System is not ready to perform the Camera Parameters Setting"; mitkThrowException(mitk::IGTException) << "System is not Initialized -> System is not ready to perform the Camera Parameters Setting"; return false; } return true; } //======================================================= // GetTool //======================================================= mitk::TrackingTool* mitk::OptitrackTrackingDevice::GetTool(unsigned int toolNumber) const { return static_cast(GetOptitrackTool(toolNumber)); } //======================================================= // AddToolByFileName //======================================================= bool mitk::OptitrackTrackingDevice::AddToolByDefinitionFile(std::string fileName) { bool resultSetToolByFileName; if(m_initialized) { OptitrackTrackingTool::Pointer t = OptitrackTrackingTool::New(); resultSetToolByFileName= t->SetToolByFileName(fileName); if(resultSetToolByFileName) { this->m_AllTools.push_back(t); MITK_INFO << "Added tool "<GetToolName()<< ". Tool vector size is now: "< Cannot Add tools"; mitkThrowException(mitk::IGTException) << "System is not Initialized -> Cannot Add tools"; return false; } } //======================================================= // IF Optitrack is not installed set functions to warnings //======================================================= #else //======================================================= // Static method: IsDeviceInstalled //======================================================= bool mitk::OptitrackTrackingDevice::IsDeviceInstalled() { return false; } //======================================================= // Constructor //======================================================= mitk::OptitrackTrackingDevice::OptitrackTrackingDevice() : mitk::TrackingDevice(), m_initialized(false) { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); } //======================================================= // Destructor //======================================================= mitk::OptitrackTrackingDevice::~OptitrackTrackingDevice() { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); } //======================================================= // OpenConnection //======================================================= bool mitk::OptitrackTrackingDevice::OpenConnection() { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return false; } //======================================================= // InitializeCameras //======================================================= bool mitk::OptitrackTrackingDevice::InitializeCameras() { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return false; } //======================================================= // LoadCalibration //======================================================= bool mitk::OptitrackTrackingDevice::LoadCalibration() { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return false; } //======================================================= // SetcalibrationPath //======================================================= void mitk::OptitrackTrackingDevice::SetCalibrationPath(std::string /*calibrationPath*/) { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); } //======================================================= // CloseConnection //======================================================= bool mitk::OptitrackTrackingDevice::CloseConnection() { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return false; } //======================================================= // StartTracking //======================================================= bool mitk::OptitrackTrackingDevice::StartTracking() { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return false; } //======================================================= // StopTracking //======================================================= bool mitk::OptitrackTrackingDevice::StopTracking() { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return false; } //======================================================= // ThreadStartTracking //======================================================= -ITK_THREAD_RETURN_TYPE mitk::OptitrackTrackingDevice::ThreadStartTracking(void* pInfoStruct) +ITK_THREAD_RETURN_TYPE mitk::OptitrackTrackingDevice::ThreadStartTracking(void*) { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return 0; } //======================================================= // GetOptitrackTool //======================================================= -mitk::OptitrackTrackingTool* mitk::OptitrackTrackingDevice::GetOptitrackTool( unsigned int toolNumber) const +mitk::OptitrackTrackingTool* mitk::OptitrackTrackingDevice::GetOptitrackTool(unsigned int) const { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return nullptr; } //======================================================= // GetToolCount //======================================================= unsigned int mitk::OptitrackTrackingDevice::GetToolCount() const { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return 0; } //======================================================= // TrackTools //======================================================= void mitk::OptitrackTrackingDevice::TrackTools() { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); } //======================================================= // SetCameraParams //======================================================= -bool mitk::OptitrackTrackingDevice::SetCameraParams(int exposure, int threshold , int intensity, int videoType ) +bool mitk::OptitrackTrackingDevice::SetCameraParams(int, int, int, int) { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return false; } //======================================================= // GetTool //======================================================= -mitk::TrackingTool* mitk::OptitrackTrackingDevice::GetTool(unsigned int toolNumber) const +mitk::TrackingTool* mitk::OptitrackTrackingDevice::GetTool(unsigned int) const { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return nullptr; } //======================================================= // AddToolByFileName //======================================================= -bool mitk::OptitrackTrackingDevice::AddToolByDefinitionFile(std::string fileName) +bool mitk::OptitrackTrackingDevice::AddToolByDefinitionFile(std::string) { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return false; } #endif diff --git a/Modules/IGT/TrackingDevices/mitkOptitrackTrackingTool.cpp b/Modules/IGT/TrackingDevices/mitkOptitrackTrackingTool.cpp index 288c2043ed..3fb6348a22 100644 --- a/Modules/IGT/TrackingDevices/mitkOptitrackTrackingTool.cpp +++ b/Modules/IGT/TrackingDevices/mitkOptitrackTrackingTool.cpp @@ -1,642 +1,642 @@ /*=================================================================== The Medical Imaging Interaction Toolkit (MITK) Copyright (c) German Cancer Research Center, Division of Medical and Biological Informatics. All rights reserved. This software is distributed WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See LICENSE.txt or http://www.mitk.org for details. ===================================================================*/ #include "mitkOptitrackTrackingTool.h" #ifdef MITK_USE_OPTITRACK_TRACKER /** * \brief API library header for Optitrack Systems */ #include //======================================================= // Constructor //======================================================= mitk::OptitrackTrackingTool::OptitrackTrackingTool() : mitk::InternalTrackingTool(), m_ID(-1) { MITK_DEBUG << "Creating OptitrackTrackingTool Object"; this->m_FLE = 0.0; } //======================================================= // Destructor //======================================================= mitk::OptitrackTrackingTool::~OptitrackTrackingTool() { delete this->m_calibrationPoints; delete m_pivotPoint; MITK_DEBUG << "Deleting OptitrackTrackingTool Object"; } //======================================================= // SetToolByFileName //======================================================= bool mitk::OptitrackTrackingTool::SetToolByFileName(std::string nameFile) { MITK_DEBUG << "SetToolByFileName"; MITK_INFO<<"Name of the file for configuration: "<m_fileConfiguration = nameFile; int resultFscan, resultUpdate, resultCreateTrackable, resultTrackableTranslatePivot; // Check the file path if(this->m_fileConfiguration.empty()) { MITK_INFO << "Calibration File for Tool is empty"; mitkThrowException(mitk::IGTException) << "Calibration File for Tool is empty"; return false; } // Open the file FILE* calib_file = fopen(this->m_fileConfiguration.c_str(),"r"); if (calib_file == nullptr) { MITK_INFO << "Error using opening file"; mitkThrowException(mitk::IGTException) << "Cannot open configuration file"; return false; } MITK_DEBUG<<"Reading configuration file..."; // Get the name this->m_ToolName = ""; char* aux = new char[200]; resultFscan = fscanf(calib_file,"%s\n",aux); this->m_ToolName.append(aux); delete aux; if ((resultFscan < 1) || this->m_ToolName.empty()) { MITK_INFO << "No name found in the tool configuration file"; mitkThrowException(mitk::IGTException) << "No name found in the tool configuration file"; return false; } MITK_INFO<<"ToolName: " << this->m_ToolName; // Get the number of of points resultFscan = fscanf(calib_file,"%i\n",&(this->m_numMarkers)); if (this->m_numMarkers < 3) { MITK_INFO << "The minimum number for define a tool is 3 markers"; mitkThrowException(mitk::IGTException) << "Tool has less than 3 markers"; return false; } MITK_INFO<<"\tNumer of Markers: " << this->m_numMarkers; // Read the Calibration Point locations and save them this->m_calibrationPoints = new float[3*this->m_numMarkers]; for(int i=0; im_numMarkers; i++) { resultFscan = fscanf(calib_file,"%fe ", &this->m_calibrationPoints[i*3+0]); if (resultFscan < 1) { MITK_INFO << "Cannot read X location for marker " << i; mitkThrowException(mitk::IGTException) << "Cannot read X location for marker " << i; return false; } resultFscan = fscanf(calib_file,"%fe ", &this->m_calibrationPoints[i*3+1]); if (resultFscan < 1) { MITK_INFO << "Cannot read Y location for marker " << i; mitkThrowException(mitk::IGTException) << "Cannot read Y location for marker " << i; return false; } resultFscan = fscanf(calib_file,"%fe\n", &this->m_calibrationPoints[i*3+2]); if (resultFscan < 1) { MITK_INFO << "Cannot read Z location for marker " << i; mitkThrowException(mitk::IGTException) << "Cannot read Z location for marker " << i; return false; } MITK_DEBUG << "\t\tMarker " << i; 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]; this->m_calibrationPoints[i*3+0] = this->m_calibrationPoints[i*3+0]/1000; this->m_calibrationPoints[i*3+1] = this->m_calibrationPoints[i*3+1]/1000; this->m_calibrationPoints[i*3+2] = -this->m_calibrationPoints[i*3+2]/1000;// Optitrack works with Left Handed System } // Read the Pivot Point location this->m_pivotPoint = new float[3]; resultFscan = fscanf(calib_file,"%fe ", &this->m_pivotPoint[0]); if (resultFscan < 1) { MITK_INFO << "Cannot read X location for Pivot Point "; mitkThrowException(mitk::IGTException) << "Cannot read X location for Pivot Point "; return false; } resultFscan = fscanf(calib_file,"%fe ", &this->m_pivotPoint[1]); if (resultFscan < 1) { MITK_INFO << "Cannot read Y location for Pivot Point " ; mitkThrowException(mitk::IGTException) << "Cannot read Y location for Pivot Point "; return false; } resultFscan = fscanf(calib_file,"%fe\n", &this->m_pivotPoint[2]); if (resultFscan < 1) { MITK_INFO << "Cannot read Z location for Pivot Point " ; mitkThrowException(mitk::IGTException) << "Cannot read Z location for Pivot Point "; return false; } MITK_INFO << "\tPivotPoint " ; MITK_INFO << "\t\t X: " << this->m_pivotPoint[0] << " Y: " << this->m_pivotPoint[1] << " Z: " << this->m_pivotPoint[2]; // mm -> m this->m_pivotPoint[0] = this->m_pivotPoint[0]/1000; this->m_pivotPoint[1] = this->m_pivotPoint[1]/1000; this->m_pivotPoint[2] = -this->m_pivotPoint[2]/1000; // get the ID for next tool in Optitrack System this->m_ID = this->get_IDnext(); // Create the Tool for( int i=OPTITRACK_ATTEMPTS; i>0; i--) { resultCreateTrackable = TT_CreateTrackable(m_ToolName.c_str(), this->m_ID,this->m_numMarkers,this->m_calibrationPoints); if(NPRESULT_SUCCESS == resultCreateTrackable) { MITK_INFO << "Trackable Created Successfully"; i = -1; } else { MITK_DEBUG << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultCreateTrackable); MITK_DEBUG << "Trying again..."; } } for( int i=OPTITRACK_ATTEMPTS; i>0; i--) { resultUpdate = TT_Update(); if(NPRESULT_SUCCESS == resultUpdate) { resultTrackableTranslatePivot = TT_TrackableTranslatePivot(this->m_ID,this->m_pivotPoint[0],this->m_pivotPoint[1],this->m_pivotPoint[2]); if(NPRESULT_SUCCESS == resultCreateTrackable) { MITK_INFO << "Pivot Translation Successfull"; fclose(calib_file); i=-1; return true; } else { MITK_INFO << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultUpdate); MITK_DEBUG << "Trying again..."; } } else { MITK_INFO << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultUpdate); MITK_DEBUG << "Trying again..."; } } MITK_INFO << "Cannot create tool "; mitkThrowException(mitk::IGTException) << "Cannot create tool "; return false; } //======================================================= // get_IDnext //======================================================= int mitk::OptitrackTrackingTool::get_IDnext() { MITK_DEBUG << "get_ID"; int num_trackables = -1; int resultUpdate; for( int i=OPTITRACK_ATTEMPTS; i>0; i--) { resultUpdate = TT_Update(); if(NPRESULT_SUCCESS == resultUpdate) { num_trackables = TT_TrackableCount(); MITK_DEBUG << " Next ID: " << num_trackables; if(num_trackables > -1) { return num_trackables; } else { MITK_DEBUG << "get_IDnext failed"; mitkThrowException(mitk::IGTException) << "get_IDnext failed"; } } else { MITK_DEBUG << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultUpdate); MITK_DEBUG << "Trying again..."; } } mitkThrowException(mitk::IGTException) << "get_IDnext failed"; return num_trackables; } //======================================================= // DeleteTrackable //======================================================= bool mitk::OptitrackTrackingTool::DeleteTrackable() { MITK_DEBUG << "DeleteTrackable"; int resultRemoveTrackable; resultRemoveTrackable = TT_RemoveTrackable(this->m_ID); if(resultRemoveTrackable != NPRESULT_SUCCESS) { MITK_INFO << "Cannot Remove Trackable"; MITK_INFO << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultRemoveTrackable); mitkThrowException(mitk::IGTException) << "Cannot Remove Trackable" << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultRemoveTrackable); return false; } else { MITK_INFO<<"Trackable " << this->m_ToolName << " removed"; } return true; } //======================================================= // SetPosition //======================================================= void mitk::OptitrackTrackingTool::SetPosition(mitk::Point3D position, ScalarType eps) { MITK_DEBUG << "SetPosition"; // sets the position this->m_Position[0] = position[0]; this->m_Position[1] = position[1]; this->m_Position[2] = position[2]; } //======================================================= // SetOrientation //======================================================= void mitk::OptitrackTrackingTool::SetOrientation(mitk::Quaternion orientation, ScalarType eps) { MITK_DEBUG << "SetOrientation"; // sets the orientation as a quaternion this->m_Orientation.x() = orientation.x(); this->m_Orientation.y() = orientation.y(); this->m_Orientation.z() = orientation.z(); this->m_Orientation.r() = orientation.r(); } //======================================================= // GetPosition //======================================================= void mitk::OptitrackTrackingTool::GetPosition(mitk::Point3D& positionOutput) const { MITK_DEBUG << "GetPosition"; // returns the current position of the tool as an array of three floats (in the tracking device coordinate system) positionOutput[0] = this->m_Position[0]; positionOutput[1] = this->m_Position[1]; positionOutput[2] = this->m_Position[2]; } //======================================================= // GetOrientation //======================================================= void mitk::OptitrackTrackingTool::GetOrientation(mitk::Quaternion& orientation) const { MITK_DEBUG << "GetOrientation"; // returns the current orientation of the tool as a quaternion (in the tracking device coordinate system) orientation.x() = this->m_Orientation.x(); orientation.y() = this->m_Orientation.y(); orientation.z() = this->m_Orientation.z(); orientation.r() = this->m_Orientation.r(); } //======================================================= // Enable //======================================================= bool mitk::OptitrackTrackingTool::Enable() { MITK_DEBUG << "Enable"; // enable the tool, so that it will be tracked. Returns true if enabling was successfull TT_SetTrackableEnabled(this->m_ID, true); if(TT_TrackableEnabled(this->m_ID) == true) { this->m_Enabled = true; return true; } else { this->m_Enabled = false; MITK_INFO << "Enable failed"; mitkThrowException(mitk::IGTException) << "Enable failed"; return false; } } //======================================================= // Disable //======================================================= bool mitk::OptitrackTrackingTool::Disable() { MITK_DEBUG << "Disable"; // disables the tool, so that it will not be tracked anymore. Returns true if disabling was successfull TT_SetTrackableEnabled(this->m_ID, false); if(TT_TrackableEnabled(this->m_ID) == true) { this->m_Enabled = false; return true; } else { this->m_Enabled = true; MITK_INFO << "Disable failed"; mitkThrowException(mitk::IGTException) << "Disable failed"; return false; } } //======================================================= // IsEnabled //======================================================= bool mitk::OptitrackTrackingTool::IsEnabled() const { MITK_DEBUG << "IsEnabled"; // returns whether the tool is enabled or disabled return TT_TrackableEnabled(this->m_ID); } //======================================================= // IsDataValid //======================================================= bool mitk::OptitrackTrackingTool::IsDataValid() const { MITK_DEBUG << "IsDataValid"; // returns true if the current position data is valid (no error during tracking, tracking error below threshold, ...) return this->m_DataValid; } //======================================================= // GetTrackingError //======================================================= float mitk::OptitrackTrackingTool::GetTrackingError() const { MITK_DEBUG << "GetTrackingError"; // return one value that corresponds to the overall tracking error. The dimension of this value is specific to each tracking device return this->m_TrackingError; } //======================================================= // SetTrackingError //======================================================= void mitk::OptitrackTrackingTool::SetTrackingError(float error) { MITK_DEBUG << "GetTrackingError"; //< sets the tracking error //this->m_FLE = error; //this->UpdateError; this->m_TrackingError = error; } //======================================================= // SetDataValid //======================================================= void mitk::OptitrackTrackingTool::SetDataValid(bool validate) { MITK_DEBUG << "SetDataValid"; // sets if the tracking data (position & Orientation) is valid this->m_DataValid = validate; } //======================================================= // updateTool //======================================================= void mitk::OptitrackTrackingTool::updateTool() { MITK_DEBUG << "updateTool"; float yaw,pitch,roll; float data[7]; if(TT_Update() == NPRESULT_SUCCESS) { if(this->IsEnabled()) { TT_TrackableLocation(this->m_ID, &data[0], &data[1], &data[2], // Position &data[3], &data[4], &data[5], &data[6], // Orientation &yaw, &pitch, &roll); // Orientation //for( int i=0; i<7; i++) //{ // if(boost::math::isinf(data[i])) // Possible Tracking check for INF numbers // { // this->SetDataValid(false); // MITK_DEBUG << "Data set to INF by the system"; // return; // } //} // m -> mm this->m_Position[0] = data[0]*1000; this->m_Position[1] = data[1]*1000; this->m_Position[2] = -data[2]*1000; // Correction from LeftHanded to RightHanded system this->m_Orientation.x() = data[3]; this->m_Orientation.y() = data[4]; this->m_Orientation.z() = -data[5]; this->m_Orientation.r() = data[6]; this->SetDataValid(true); MITK_DEBUG << this->m_Position[0] << " " << this->m_Position[1] << " " << this->m_Position[2]; MITK_DEBUG << data[3] << " " << data[4] << " " << data[5] << " " << data[6]; } else { this->SetDataValid(false); MITK_DEBUG << "Trackable: "<< this->m_ToolName << "is not Tracked"; } } else { this->SetDataValid(false); MITK_DEBUG << "Update Failed"; } } //======================================================= // IF Optitrack is not installed set functions to warnings //======================================================= #else //======================================================= // Constructor //======================================================= mitk::OptitrackTrackingTool::OptitrackTrackingTool() : mitk::InternalTrackingTool(), m_ID(-1) { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); } //======================================================= // Destructor //======================================================= mitk::OptitrackTrackingTool::~OptitrackTrackingTool() { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); } //======================================================= // SetToolByFileName //======================================================= -bool mitk::OptitrackTrackingTool::SetToolByFileName(std::string nameFile) +bool mitk::OptitrackTrackingTool::SetToolByFileName(std::string) { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return false; } //======================================================= // get_IDnext //======================================================= int mitk::OptitrackTrackingTool::get_IDnext() { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return -1; } //======================================================= // DeleteTrackable //======================================================= bool mitk::OptitrackTrackingTool::DeleteTrackable() { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return false; } //======================================================= // SetPosition //======================================================= -void mitk::OptitrackTrackingTool::SetPosition(mitk::Point3D position, ScalarType eps) +void mitk::OptitrackTrackingTool::SetPosition(mitk::Point3D, ScalarType) { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); } //======================================================= // SetOrientation //======================================================= -void mitk::OptitrackTrackingTool::SetOrientation(mitk::Quaternion orientation, ScalarType eps) +void mitk::OptitrackTrackingTool::SetOrientation(mitk::Quaternion, ScalarType) { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); } //======================================================= // GetPosition //======================================================= -void mitk::OptitrackTrackingTool::GetPosition(mitk::Point3D& positionOutput) const +void mitk::OptitrackTrackingTool::GetPosition(mitk::Point3D&) const { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); } //======================================================= // GetOrientation //======================================================= -void mitk::OptitrackTrackingTool::GetOrientation(mitk::Quaternion& orientation) const +void mitk::OptitrackTrackingTool::GetOrientation(mitk::Quaternion&) const { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); } //======================================================= // Enable //======================================================= bool mitk::OptitrackTrackingTool::Enable() { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return false; } //======================================================= // Disable //======================================================= bool mitk::OptitrackTrackingTool::Disable() { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return false; } //======================================================= // IsEnabled //======================================================= bool mitk::OptitrackTrackingTool::IsEnabled() const { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return false; } //======================================================= // IsDataValid //======================================================= bool mitk::OptitrackTrackingTool::IsDataValid() const { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return false; } //======================================================= // GetTrackingError //======================================================= float mitk::OptitrackTrackingTool::GetTrackingError() const { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return 0.0; } //======================================================= // SetTrackingError //======================================================= -void mitk::OptitrackTrackingTool::SetTrackingError(float error) +void mitk::OptitrackTrackingTool::SetTrackingError(float) { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); } //======================================================= // SetDataValid //======================================================= -void mitk::OptitrackTrackingTool::SetDataValid(bool validate) +void mitk::OptitrackTrackingTool::SetDataValid(bool) { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); } //======================================================= // updateTool //======================================================= void mitk::OptitrackTrackingTool::updateTool() { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); } #endif