mitkOptitrackTrackingDevice.cpp

Go to the documentation of this file.

/*============================================================================
 
The Medical Imaging Interaction Toolkit (MITK)
 
Copyright (c) German Cancer Research Center (DKFZ)
All rights reserved.
 
Use of this source code is governed by a 3-clause BSD license that can be
found in the LICENSE file.
 
============================================================================*/
#include "mitkOptitrackTrackingDevice.h"
#include <mitkOptitrackErrorMessages.h>
 
#ifdef MITK_USE_OPTITRACK_TRACKER
 
 
 
#include <NPTrackingTools.h>
 
//=======================================================
// Static method: IsDeviceInstalled
//=======================================================
bool mitk::OptitrackTrackingDevice::IsDeviceInstalled()
{
  return true;
}
 
//=======================================================
// Constructor
//=======================================================
mitk::OptitrackTrackingDevice::OptitrackTrackingDevice()
    : mitk::TrackingDevice(),
      m_initialized(false)
{
  //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_Thread = std::thread(&OptitrackTrackingDevice::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
//=======================================================
void mitk::OptitrackTrackingDevice::ThreadStartTracking()
{
  MITK_DEBUG << "ThreadStartTracking";
  this->TrackTools();
}
 
//=======================================================
// GetOptitrackTool
//=======================================================
mitk::OptitrackTrackingTool* mitk::OptitrackTrackingDevice::GetOptitrackTool( unsigned int toolNumber) const
{
  MITK_DEBUG << "ThreadStartTracking";
  OptitrackTrackingTool* t = nullptr;
 
  std::lock_guard<std::mutex> 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";
  std::lock_guard<std::mutex> 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 # "<<cam<< " params are set";
            i = 0; // End attempts for camera #cam
          }
          else
          {
            MITK_DEBUG << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultSetCameraSettings);
            if(i == 1)
            mitkThrowException(mitk::IGTException) << "Camera number " << cam << " failed during setting the params. Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultSetCameraSettings);
          }
        }
        else
        {
          MITK_DEBUG << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(resultUpdate);
          MITK_DEBUG << "Update: Trying again...";
        }
      }
    }
  }
  else
  {
    MITK_INFO << "System is not Initialized -> 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<mitk::TrackingTool*>(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 "<<t->GetToolName()<< ". Tool vector size is now: "<<m_AllTools.size();
      return true;
    }
    else
    {
      MITK_INFO << "Tool could not be added";
      mitkThrowException(mitk::IGTException) << "Tool could not be added";
      return false;
  }
  }
  else
  {
    MITK_INFO << "System is not Initialized -> 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
//=======================================================
void mitk::OptitrackTrackingDevice::ThreadStartTracking()
{
  MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100);
}
 
//=======================================================
// GetOptitrackTool
//=======================================================
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, int, int, int)
{
  MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100);
  return false;
}
 
//=======================================================
// GetTool
//=======================================================
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)
{
  MITK_WARN("IGT") << "Error: " << mitk::OptitrackErrorMessages::GetOptitrackErrorMessage(100);
  return false;
}
 
#endif