diff --git a/Modules/IGT/TrackingDevices/mitkOptitrackTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkOptitrackTrackingDevice.cpp index db82056a56..917ad5b761 100644 --- a/Modules/IGT/TrackingDevices/mitkOptitrackTrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkOptitrackTrackingDevice.cpp @@ -1,771 +1,771 @@ /*=================================================================== 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(); m_TrackingFinishedMutex->Unlock(); // transfer the execution rights to tracking thread /****************************************************************************** ############################################################################### 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 { 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->Lock(); 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 == NULL) { return ITK_THREAD_RETURN_VALUE; } if (pInfo->UserData == NULL) { return ITK_THREAD_RETURN_VALUE; } OptitrackTrackingDevice *trackingDevice = static_cast(pInfo->UserData); if (trackingDevice != NULL) { // Call the TrackTools function in this thread trackingDevice->TrackTools(); } else { mitkThrowException(mitk::IGTException) << "In ThreadStartTracking(): trackingDevice is NULL"; } 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) { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); - return nullptr; + return NULL; } //======================================================= // GetOptitrackTool //======================================================= mitk::OptitrackTrackingTool* mitk::OptitrackTrackingDevice::GetOptitrackTool( unsigned int toolNumber) 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 ) { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return false; } //======================================================= // GetTool //======================================================= mitk::TrackingTool* mitk::OptitrackTrackingDevice::GetTool(unsigned int toolNumber) const { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return nullptr; } //======================================================= // AddToolByFileName //======================================================= bool mitk::OptitrackTrackingDevice::AddToolByDefinitionFile(std::string fileName) { MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100); return false; } #endif