diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp index ef172e90ad..87c5594839 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp +++ b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp @@ -1,426 +1,434 @@ /*=================================================================== 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 #define _USE_MATH_DEFINES #include #include BYTE MotionBuf[0x1FA400]; mitk::PolhemusInterface::PolhemusInterface() : m_continousTracking(false) { m_pdiDev = new CPDIdev(); m_numberOfTools = 0; } mitk::PolhemusInterface::~PolhemusInterface() { delete m_pdiDev; } bool mitk::PolhemusInterface::InitializeDevice() { m_pdiDev->ResetTracker(); m_pdiDev->ResetSAlignment(-1); m_pdiDev->Trace(TRUE, 7); m_continousTracking = false; return true; } bool mitk::PolhemusInterface::SetupDevice() { m_pdiDev->SetPnoBuffer(MotionBuf, 0x1FA400); m_pdiDev->SetMetric(true); //use cm instead of inches m_pdiDev->StartPipeExport(); CPDImdat pdiMDat; pdiMDat.Empty(); pdiMDat.Append(PDI_MODATA_FRAMECOUNT); pdiMDat.Append(PDI_MODATA_POS); pdiMDat.Append(PDI_MODATA_ORI); m_pdiDev->SetSDataList(-1, pdiMDat); CPDIbiterr cBE; m_pdiDev->GetBITErrs(cBE); if (!(cBE.IsClear())) { m_pdiDev->ClearBITErrs(); } return true; } bool mitk::PolhemusInterface::StartTracking() { m_continousTracking = true; return m_pdiDev->StartContPno(0); } bool mitk::PolhemusInterface::StopTracking() { m_continousTracking = false; m_pdiDev->StopContPno(); return true; } bool mitk::PolhemusInterface::Connect() { bool returnValue; //Initialize, and if it is not successful, return false. if (!InitializeDevice()) { returnValue = false; } //Connect else if (m_pdiDev->CnxReady()) { returnValue = true; } //If it is not successful, search for connections. else { CPDIser pdiSer; m_pdiDev->SetSerialIF(&pdiSer); ePiCommType eType = m_pdiDev->DiscoverCnx(); switch (eType) { case PI_CNX_USB: MITK_INFO << "USB Connection: " << m_pdiDev->GetLastResultStr(); break; case PI_CNX_SERIAL: MITK_INFO << "Serial Connection: " << m_pdiDev->GetLastResultStr(); break; default: MITK_INFO << "DiscoverCnx result: " << m_pdiDev->GetLastResultStr(); break; } //Setup device if (!SetupDevice()) { returnValue = false; } else { returnValue = m_pdiDev->CnxReady(); } } if (returnValue) { m_numberOfTools = this->GetNumberOfTools(); } //Get the tracking data to find out which tools are available. std::vector _trackingData; if (m_continousTracking) { _trackingData = GetLastFrame(); } else { _trackingData = GetSingleFrame(); } //if we had tool before, check if they are still the same. if (m_ToolPorts.size() == _trackingData.size()) { for (int i = 0; i < _trackingData.size(); ++i) { //if they are not the same, clear hemispheres and toolNames and break. if (m_ToolPorts[i] != _trackingData.at(i).id) { m_ToolPorts.clear(); m_Hemispheres.clear(); break; } } } //if we don't have old tool names or if the old ones don't match any more, assign them again. if (m_ToolPorts.size() == 0) { for (int i = 0; i < _trackingData.size(); ++i) { m_ToolPorts.push_back(_trackingData.at(i).id); } } return returnValue; } bool mitk::PolhemusInterface::Disconnect() { bool returnValue = true; //If Tracking is running, stop tracking first if (m_continousTracking) { this->StopTracking(); } returnValue = m_pdiDev->Disconnect(); MITK_INFO << "Disconnect: " << m_pdiDev->GetLastResultStr(); return returnValue; } std::vector mitk::PolhemusInterface::GetLastFrame() { PBYTE pBuf; DWORD dwSize; //read one frame if (!m_pdiDev->LastPnoPtr(pBuf, dwSize)) { MITK_WARN << m_pdiDev->GetLastResultStr(); } std::vector returnValue = ParsePolhemusRawData(pBuf, dwSize); if (returnValue.empty()) { MITK_WARN << "Cannot parse data / no tools present"; } return returnValue; } unsigned int mitk::PolhemusInterface::GetNumberOfTools() { if (m_continousTracking) return GetLastFrame().size(); else return GetSingleFrame().size(); } std::vector mitk::PolhemusInterface::GetSingleFrame() { if (m_continousTracking) { MITK_WARN << "Cannot get a single frame when continuous tracking is on!"; return std::vector(); } PBYTE pBuf; DWORD dwSize; //read one frame if (!m_pdiDev->ReadSinglePnoBuf(pBuf, dwSize)) { MITK_WARN << m_pdiDev->GetLastResultStr(); return std::vector(); } return ParsePolhemusRawData(pBuf, dwSize); } std::vector mitk::PolhemusInterface::ParsePolhemusRawData(PBYTE pBuf, DWORD dwSize) { std::vector returnValue; DWORD i = 0; while (i < dwSize) { BYTE ucSensor = pBuf[i + 2]; SHORT shSize = pBuf[i + 6]; // skip rest of header i += 8; PDWORD pFC = (PDWORD)(&pBuf[i]); PFLOAT pPno = (PFLOAT)(&pBuf[i + 4]); mitk::PolhemusInterface::trackingData currentTrackingData; currentTrackingData.id = ucSensor; currentTrackingData.pos[0] = pPno[0] * 10; //from cm to mm currentTrackingData.pos[1] = pPno[1] * 10; currentTrackingData.pos[2] = pPno[2] * 10; double azimuthAngle = pPno[3] / 180 * M_PI; //from degree to rad double elevationAngle = pPno[4] / 180 * M_PI; double rollAngle = pPno[5] / 180 * M_PI; vnl_quaternion eulerQuat(rollAngle, elevationAngle, azimuthAngle); currentTrackingData.rot = eulerQuat; returnValue.push_back(currentTrackingData); i += shSize; } return returnValue; } void mitk::PolhemusInterface::SetHemisphereTrackingEnabled(bool _HeisphereTrackingEnabeled) { //only if connection is ready! if (!this->m_pdiDev->CnxReady()) return; //HemisphereTracking is switched on by SetSHemiTrack(-1). "-1" means for all sensors. //To switch heisphere tracking of, you need to set a hemisphere vector by calling SetSHemisphere(-1, { (float)1,0,0 }) if (_HeisphereTrackingEnabeled) { //Remember the Hemisphere and Position when switching on to avoid wrong positions ("jumps") when swithing HemiTracking off. if (m_Hemispheres.empty()) { //only if it is empty. Otherwise, it might already be on and we overwrite it with (0|0|0). for (int i = 0; i < m_numberOfTools; ++i) { m_Hemispheres.push_back(GetHemisphere(m_ToolPorts[i])); } } m_pdiDev->SetSHemiTrack(-1); } //switch HemiTracking OFF else { //Get Tool Position. ToDo, this should not be the tool tip but the sensor position. Any chance, to get that from Polhemus interface?! std::vector _position; if (m_continousTracking) { _position = GetLastFrame(); } else { _position = GetSingleFrame(); } if (m_Hemispheres.empty()) { //Default Hemisphere for all tools, maybe the first setup. //But we still check the position, 'cause maybe the tool is in negative space. //We can't do that every time, in case the user wants to use e.g. (0|1|0). Therefore, storing the last one makes sense... mitk::Vector3D temp; mitk::FillVector3D(temp, 1, 0, 0); m_Hemispheres.assign(m_numberOfTools, temp); } for (int i = 0; i < m_numberOfTools; ++i) { if (m_Hemispheres.at(i).GetNorm() == 0) { //hemisphere vector can be 0 if Polhemus was in "HemiTracking" status when MITK connected or when user manually set it... mitk::FillVector3D(m_Hemispheres.at(i), 1, 0, 0); } //Scalar product between mitk::point and mitk::vector double _scalarProduct = _position.at(i).pos.GetVectorFromOrigin() * m_Hemispheres.at(i); //if scalar product is negative, then the tool is in the opposite sphere then when we started to track. //Hence, we have to set the inverted hemisphere. //For default (1|0|0) this means, if x is negative, we have to set (-1|0|0). But we want to keep it generic if user sets different hemisphere... if (_scalarProduct < 0) { m_Hemispheres.at(i) = -1. * m_Hemispheres.at(i); } SetHemisphere(m_ToolPorts[i], m_Hemispheres.at(i)); } //clean up hemispheres! m_Hemispheres.clear(); } } void mitk::PolhemusInterface::ToggleHemisphere(int _tool) { //only if connection is ready! if (!this->m_pdiDev->CnxReady()) return; //we have a single tool number, which is identical with Polhemus index, i.e. first tool is "1", not "0"... //is hemiTracking on? - //Get function again doesn't work in continuous mode... - BOOL _hemiTrack; - if (m_continousTracking) - { - m_pdiDev->StopContPno(); - } - m_pdiDev->GetSHemiTrack(_tool, _hemiTrack); - if (m_continousTracking) - { - m_pdiDev->StartContPno(0); - } + bool _hemiTrack = GetHemisphereTrackingEnabled(_tool); - MITK_INFO << "HemisphereTracking: " << m_pdiDev->GetLastResultStr(); + //MITK_INFO << "HemisphereTracking: " << m_pdiDev->GetLastResultStr(); //if hemiTracing is on, switch it off. if (_hemiTrack) SetHemisphereTrackingEnabled(false); //toggel. if (_tool == -1) { //GetHemisphere(-1) returns the first tool. Hence, we have to loop over all tools manually... for (int i = 0; i < m_numberOfTools; ++i) { this->SetHemisphere(m_ToolPorts[i], -1.*this->GetHemisphere(m_ToolPorts[i])); } } else { this->SetHemisphere(_tool, -1.*this->GetHemisphere(_tool)); } //if hemiTracing was on, switch it on again. if (_hemiTrack) SetHemisphereTrackingEnabled(true); } void mitk::PolhemusInterface::SetHemisphere(int _tool, mitk::Vector3D _hemisphere) { //only if connection is ready! if (!this->m_pdiDev->CnxReady()) return; m_pdiDev->SetSHemisphere(_tool, { (float)_hemisphere[0], (float)_hemisphere[1], (float)_hemisphere[2] }); } mitk::Vector3D mitk::PolhemusInterface::GetHemisphere(int _tool) { //only if connection is ready! if (!this->m_pdiDev->CnxReady()) return nullptr; PDI3vec _hemisphere; mitk::Vector3D _returnVector; //Doesn't work in continuous mode. Don't know why, but so it is... Hence: stop and restart... if (m_continousTracking) { m_continousTracking = false; m_pdiDev->StopContPno(); m_pdiDev->GetSHemisphere(_tool, _hemisphere); MITK_DEBUG << "Get Hemisphere: " << m_pdiDev->GetLastResultStr(); mitk::FillVector3D(_returnVector, _hemisphere[0], _hemisphere[1], _hemisphere[2]); m_pdiDev->StartContPno(0); m_continousTracking = true; } else { m_pdiDev->GetSHemisphere(_tool, _hemisphere); MITK_DEBUG << "Get Hemisphere: " << m_pdiDev->GetLastResultStr(); mitk::FillVector3D(_returnVector, _hemisphere[0], _hemisphere[1], _hemisphere[2]); } return _returnVector; } void mitk::PolhemusInterface::PrintStatus() { MITK_INFO << "Polhemus status: " << this->m_pdiDev->CnxReady(); } std::vector mitk::PolhemusInterface::GetToolPorts() { return m_ToolPorts; +} + + +bool mitk::PolhemusInterface::GetHemisphereTrackingEnabled(int _tool) +{ + BOOL _hemiTrack; + //Get function again doesn't work in continuous mode... + if (m_continousTracking) + { + m_pdiDev->StopContPno(); + } + m_pdiDev->GetSHemiTrack(_tool, _hemiTrack); + if (m_continousTracking) + { + m_pdiDev->StartContPno(0); + } + + return _hemiTrack; } \ No newline at end of file diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.h b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.h index b98f329193..183a0deaef 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.h +++ b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.h @@ -1,147 +1,150 @@ /*=================================================================== 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. ===================================================================*/ #ifndef MITKPolhemusINTERFACE_H_HEADER_INCLUDED_ #define MITKPolhemusINTERFACE_H_HEADER_INCLUDED_ #include #include #include #include "mitkCommon.h" #include #include #include #include #include #include class CPDIdev; namespace mitk { /** Documentation: * \brief An object of this class represents the interface to Polhemus trackers. * All variables with the name "tool" start with index 1, which is the station number of Polhemus. * Make sure to call functions in this class with parameter "1" if you want to loop over all tools. * If you need to access an array (e.g. m_Hemisphere), you need to use "_tool -1" and adapt your index for loops... * \ingroup IGT */ class MITKIGT_EXPORT PolhemusInterface : public itk::Object { public: mitkClassMacroItkParent(PolhemusInterface,itk::Object); itkFactorylessNewMacro(Self); itkCloneMacro(Self); struct trackingData { mitk::Point3D pos; mitk::Quaternion rot; BYTE id; }; /** * \brief Opens the connection to the device and makes it ready to track tools. * \return Returns true if there is a connection to the device and the device is ready to track tools, false if not. */ bool StartTracking(); /** * \brief Clears all resources. After this method have been called the system isn't ready to track any longer. * \return Returns true if the operation was succesful, false if not. */ bool StopTracking(); bool Connect(); bool Disconnect(); /** @return Returns a single frame. Only works if the tracking device is not in continous tracking mode. Returns an empty vector in case of an error.*/ std::vector GetSingleFrame(); /** @return Returns the last frame when the tracking device is in continous tracking mode. Returns an empty vector in case of an error.*/ std::vector GetLastFrame(); /** @return Returns the number of tools. Returns 0 if no information is avialable.*/ unsigned int GetNumberOfTools(); /** Enables/disables hemisphere tracking for all stations/tools. */ void SetHemisphereTrackingEnabled(bool _HeisphereTrackingEnabeled); /** Toggles the current hemisphere. Parameter _tool describes, for which tool the hemisphere should change. Default -1 toggles all tools. Index starts at "1" for the first tool (i.e. station number of Polhemus). Not 0! */ void ToggleHemisphere(int _tool = -1); /** Convenient method to print the status of the tracking device (true/false) if connection is established. For debugging...*/ void PrintStatus(); /** Sets the Hemisphere of tool _tool to the vector _hemisphere. "-1" sets all tools. Index starts at "1" for the first tool (i.e. station number of Polhemus). Not 0! */ void SetHemisphere(int _tool, mitk::Vector3D _hemisphere); /** Get the Hemisphere for _tool as mitk vector. -1 ("all tools") returns hemisphere of first tool. Index starts at "1" for the first tool (i.e. station number of Polhemus). Not 0! */ mitk::Vector3D GetHemisphere(int _tool); /** Get the ports on which tools are connected. Returns empty vector if device is not connected! */ std::vector GetToolPorts(); + /** Is Hemisphere Tracking Enabled for this tool? */ + bool GetHemisphereTrackingEnabled(int _tool); + protected: /** * \brief standard constructor */ PolhemusInterface(); /** * \brief standard destructor */ ~PolhemusInterface(); /** Polhemus liberty/patriot tracker object*/ CPDIdev* m_pdiDev; /** Parses polhemus raw data to a collection of tracking data of single tools. */ std::vector ParsePolhemusRawData(PBYTE pBuf, DWORD dwSize); unsigned int m_numberOfTools; bool m_continousTracking; bool InitializeDevice(); bool SetupDevice(); private: std::vector m_Hemispheres; //This vector stores the order of tools, which are available. //E.g. only Sensor 1 and 3 are attached, then this vector maps the first tool (0) to Polhemus identifier 1 and the second tool (1) to Polhemus 3. std::vector m_ToolPorts; }; }//mitk #endif diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp index 94b116fc02..b9a7bb8273 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp +++ b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.cpp @@ -1,326 +1,335 @@ /*=================================================================== 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 "mitkPolhemusTrackingDevice.h" #include "mitkPolhemusTool.h" #include "mitkIGTConfig.h" #include "mitkIGTTimeStamp.h" #include "mitkIGTHardwareException.h" #include #include #include #include "mitkPolhemusTrackerTypeInformation.h" #include typedef itk::MutexLockHolder MutexLockHolder; mitk::PolhemusTrackingDevice::PolhemusTrackingDevice(): mitk::TrackingDevice() { //set the type of this tracking device this->m_Data = mitk::PolhemusTrackerTypeInformation::GetDeviceDataPolhemusTrackerLiberty(); this->m_MultiThreader = itk::MultiThreader::New(); m_ThreadID = 0; m_Device = mitk::PolhemusInterface::New(); } mitk::PolhemusTrackingDevice::~PolhemusTrackingDevice() { } bool mitk::PolhemusTrackingDevice::IsDeviceInstalled() { return true; } mitk::TrackingTool* mitk::PolhemusTrackingDevice::AddTool(const char* toolName, int toolPort) { //Only add tool if port isn't already used. for (auto _tool : m_AllTools) { if (_tool->GetToolPort() == toolPort) { MITK_DEBUG << "There is already a tool connected to this port. Returning existing tool"; return _tool; } } mitk::PolhemusTool::Pointer t = mitk::PolhemusTool::New(); t->SetToolName(toolName); t->SetToolPort(toolPort); if (this->InternalAddTool(t) == false) return nullptr; return t.GetPointer(); } bool mitk::PolhemusTrackingDevice::InternalAddTool(PolhemusTool::Pointer tool) { m_AllTools.push_back(tool); return true; } bool mitk::PolhemusTrackingDevice::StartTracking() { bool success = m_Device->StartTracking(); if (success) { mitk::IGTTimeStamp::GetInstance()->Start(this); this->SetState(Tracking); this->m_StopTrackingMutex->Lock(); this->m_StopTracking = false; this->m_StopTrackingMutex->Unlock(); m_ThreadID = m_MultiThreader->SpawnThread(this->ThreadStartTracking, this); // start a new thread that executes the TrackTools() method return true; } else { this->SetState(Ready); mitkThrowException(mitk::IGTHardwareException) << "Error while trying to start the device!"; } return success; } bool mitk::PolhemusTrackingDevice::StopTracking() { m_Device->StopTracking(); return Superclass::StopTracking(); } unsigned int mitk::PolhemusTrackingDevice::GetToolCount() const { return (unsigned int)this->m_AllTools.size(); } mitk::TrackingTool* mitk::PolhemusTrackingDevice::GetTool(unsigned int toolNumber) const { if ( toolNumber >= this->GetToolCount()) return nullptr; else return this->m_AllTools[toolNumber]; } bool mitk::PolhemusTrackingDevice::OpenConnection() { //reset everything if (m_Device.IsNull()) {m_Device = mitk::PolhemusInterface::New();} m_Device->Connect(); m_Device->SetHemisphereTrackingEnabled(m_HemisphereTrackingEnabled); //check if connected ports of Polhemus matches the tools in the toolStorage. std::vector toolPorts = m_Device->GetToolPorts(); //first, check size. if (this->GetToolCount() != toolPorts.size()) { MITK_ERROR << "Cannot connect device, number of tools in toolstorage doesn't match the number of tools connected to Polhemus device!"; CloseConnection(); return false; } //second, check if toolStorage identifier is included in this port. for (auto _tool : m_AllTools) { if (std::find(toolPorts.begin(), toolPorts.end(), _tool->GetToolPort()) == toolPorts.end()) { MITK_ERROR << "Cannot connect device, tool " << _tool->GetToolPort() << " is not connected to its port."; CloseConnection(); return false; } else { //erase this port to avoid that two tools want to connect to the same port. toolPorts.erase(std::find(toolPorts.begin(), toolPorts.end(), _tool->GetToolPort())); } } this->SetState(Ready); return true; } bool mitk::PolhemusTrackingDevice::CloseConnection() { bool returnValue = true; if (this->GetState() == Setup) return true; returnValue = m_Device->Disconnect(); this->SetState(Setup); return returnValue; } mitk::PolhemusInterface* mitk::PolhemusTrackingDevice::GetDevice() { return m_Device; } std::vector mitk::PolhemusTrackingDevice::GetAllTools() { return this->m_AllTools; } void mitk::PolhemusTrackingDevice::TrackTools() { try { /* lock the TrackingFinishedMutex to signal that the execution rights are now transfered to the tracking thread */ MutexLockHolder trackingFinishedLockHolder(*m_TrackingFinishedMutex); // keep lock until end of scope 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; this->m_StopTrackingMutex->Unlock(); Sleep(100);//Wait a bit until the tracker is ready... while ((this->GetState() == Tracking) && (localStopTracking == false)) { std::vector lastData = this->GetDevice()->GetLastFrame(); if (lastData.size() != m_AllTools.size()) { MITK_WARN << "Tool count is corrupt. Aborting!"; } else { std::vector allTools = this->GetAllTools(); for (int i = 0; i < allTools.size(); i++) { mitk::PolhemusTool::Pointer currentTool = allTools.at(i); currentTool->SetDataValid(true); currentTool->SetPosition(lastData.at(i).pos); currentTool->SetOrientation(lastData.at(i).rot); currentTool->SetIGTTimeStamp(mitk::IGTTimeStamp::GetInstance()->GetElapsed()); } } /* Update the local copy of m_StopTracking */ this->m_StopTrackingMutex->Lock(); localStopTracking = m_StopTracking; this->m_StopTrackingMutex->Unlock(); } } catch(...) { this->StopTracking(); mitkThrowException(mitk::IGTHardwareException) << "Error while trying to track tools. Thread stopped."; } } ITK_THREAD_RETURN_TYPE mitk::PolhemusTrackingDevice::ThreadStartTracking(void* pInfoStruct) { /* 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; } PolhemusTrackingDevice *trackingDevice = (PolhemusTrackingDevice*)pInfo->UserData; if (trackingDevice != nullptr) trackingDevice->TrackTools(); return ITK_THREAD_RETURN_VALUE; } bool mitk::PolhemusTrackingDevice::AutoDetectToolsAvailable() { return true; } mitk::NavigationToolStorage::Pointer mitk::PolhemusTrackingDevice::AutoDetectTools() { this->OpenConnection(); std::vector singeFrameData = this->m_Device->GetSingleFrame(); MITK_INFO << "Found " << singeFrameData.size() << " tools."; this->CloseConnection(); mitk::NavigationToolStorage::Pointer returnValue = mitk::NavigationToolStorage::New(); for each (mitk::PolhemusInterface::trackingData t in singeFrameData) { mitk::DataNode::Pointer newNode = mitk::DataNode::New(); std::stringstream name; name << "Sensor-" << ((int)t.id); newNode->SetName(name.str()); mitk::Surface::Pointer myCone = mitk::Surface::New(); vtkConeSource *vtkData = vtkConeSource::New(); vtkData->SetAngle(5.0); vtkData->SetResolution(50); vtkData->SetHeight(6.0f); vtkData->SetRadius(2.0f); vtkData->SetCenter(0.0, 0.0, 0.0); vtkData->Update(); myCone->SetVtkPolyData(vtkData->GetOutput()); vtkData->Delete(); newNode->SetData(myCone); mitk::NavigationTool::Pointer newTool = mitk::NavigationTool::New(); newTool->SetDataNode(newNode); //The identifier defines, which plug is used (e.g. "Sens 2" --> 2). std::stringstream identifier; identifier << ((int)t.id); newTool->SetIdentifier(identifier.str()); newTool->SetTrackingDeviceType(mitk::PolhemusTrackerTypeInformation::GetDeviceDataPolhemusTrackerLiberty().Line); returnValue->AddTool(newTool); } return returnValue; } void mitk::PolhemusTrackingDevice::SetHemisphereTrackingEnabled(bool _HemisphereTrackingEnabled) { if (m_HemisphereTrackingEnabled != _HemisphereTrackingEnabled) { m_HemisphereTrackingEnabled = _HemisphereTrackingEnabled; this->m_Device->SetHemisphereTrackingEnabled(_HemisphereTrackingEnabled); } } void mitk::PolhemusTrackingDevice::ToggleHemisphere(int _tool) { this->m_Device->ToggleHemisphere(_tool); } void mitk::PolhemusTrackingDevice::SetHemisphere(int _tool, mitk::Vector3D _hemisphere) { this->m_Device->SetHemisphere(_tool, _hemisphere); } mitk::Vector3D mitk::PolhemusTrackingDevice::GetHemisphere(int _tool) { return this->m_Device->GetHemisphere(_tool); } + +/** Is Hemisphere Tracking Enabled for this tool? */ +bool mitk::PolhemusTrackingDevice::GetHemisphereTrackingEnabled(int _tool) +{ + if (_tool == -1) + return this->m_HemisphereTrackingEnabled; + else + return this->m_Device->GetHemisphereTrackingEnabled(_tool); +} diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.h b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.h index fa562439b7..bc775fe8b6 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.h +++ b/Modules/IGT/TrackingDevices/mitkPolhemusTrackingDevice.h @@ -1,156 +1,159 @@ /*=================================================================== 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. ===================================================================*/ #ifndef MITKPolhemusTRACKINGDEVICE_H_HEADER_INCLUDED_ #define MITKPolhemusTRACKINGDEVICE_H_HEADER_INCLUDED_ #include #include #include #include #include namespace mitk { /** Documentation: * \brief An object of this class represents Polhemus tracking device. You can add tools to this * device, then open the connection and start tracking. The tracking device will then * continuously update the tool coordinates. * \ingroup IGT */ class MITKIGT_EXPORT PolhemusTrackingDevice : public TrackingDevice { public: mitkClassMacro(PolhemusTrackingDevice, TrackingDevice); itkFactorylessNewMacro(Self) itkCloneMacro(Self) /** * \brief Starts the tracking. * \return Returns true if the tracking is started. Throws an exception if an error occures. * @throw mitk::IGTHardwareException Throws an exception if there is an error during start tracking. */ virtual bool StartTracking() override; /** * \brief Stops the tracking. * \return Returns true if the tracking is stopped. */ virtual bool StopTracking() override; /** * \brief Opens the connection to the device. This have to be done before the tracking is started. * @throw mitk::IGTHardwareException Throws an exception if there is an error during open connection. */ virtual bool OpenConnection() override; /** * \brief Closes the connection and clears all resources. */ virtual bool CloseConnection() override; /** * \return Returns the number of tools which have been added to the device. */ virtual unsigned int GetToolCount() const override; /** * \param toolNumber The number of the tool which should be given back. * \return Returns the tool which the number "toolNumber". Returns NULL, if there is * no tool with this number. */ TrackingTool* GetTool(unsigned int toolNumber) const override; /** * \brief Create a new Polhemus tool with toolName and add it to the list of tools * * This method will create a new PolhemusTool object, * set the tool name toolName and then add it to the list of tools. * It returns a pointer of type mitk::TrackingTool to the tool * that can be used to read tracking data from it. * This is the only way to add tools to PolhemusTrackingDevice. * * \warning adding tools is not possible in tracking mode, only in setup and ready. */ mitk::TrackingTool* AddTool(const char* toolName, int toolPort); bool IsDeviceInstalled(); /** @return Returns true if this device can autodetects its tools. */ virtual bool AutoDetectToolsAvailable(); /** Autodetects tools from this device and returns them as a navigation tool storage. * @return Returns the detected tools. Returns an empty storage if no tools are present * or if detection is not possible */ virtual mitk::NavigationToolStorage::Pointer AutoDetectTools(); /** Enables/disables hemisphere tracking for all sensors. */ void SetHemisphereTrackingEnabled(bool _HemisphereTrackingEnabled); + /** Is Hemisphere Tracking Enabled for this tool? */ + bool GetHemisphereTrackingEnabled(int _tool); + /** Toggles the current hemisphere. Parameter _tool describes, for which tool the hemisphere should change. Default -1 toggles all tools.*/ void ToggleHemisphere(int _tool = -1); /** Sets the Hemisphere of tool _tool to the vector _hemisphere */ void SetHemisphere(int _tool, mitk::Vector3D _hemisphere); /** Get the Hemisphere for _tool as mitk vector */ mitk::Vector3D GetHemisphere(int _tool); protected: PolhemusTrackingDevice(); ~PolhemusTrackingDevice(); /** * \brief Adds a tool to the tracking device. * * \param tool The tool which will be added. * \return Returns true if the tool has been added, false otherwise. */ bool InternalAddTool(PolhemusTool::Pointer tool); /** * \brief This method tracks tools as long as the variable m_Mode is set to "Tracking". * Tracking tools means grabbing frames from the camera an updating the tools. * @throw mitk::IGTHardwareException Throws an exception if there is an error during tracking of tools. */ void TrackTools(); /** * \return Returns all tools of the tracking device. */ std::vector GetAllTools(); /** * \return Gives back the device which is represented by an object of the class PolhemusInterface. */ PolhemusInterface* GetDevice(); static ITK_THREAD_RETURN_TYPE ThreadStartTracking(void* data); std::vector m_AllTools; ///< vector holding all tools PolhemusInterface::Pointer m_Device; ///< represents the interface to the tracking hardware itk::MultiThreader::Pointer m_MultiThreader; int m_ThreadID; bool m_HemisphereTrackingEnabled; }; }//mitk #endif /* MITKPolhemusTRACKINGDEVICE_H_HEADER_INCLUDED_ */ diff --git a/Modules/IGTUI/Qmitk/QmitkPolhemusTrackerWidget.cpp b/Modules/IGTUI/Qmitk/QmitkPolhemusTrackerWidget.cpp index 3b691a2e2c..44a32c16ce 100644 --- a/Modules/IGTUI/Qmitk/QmitkPolhemusTrackerWidget.cpp +++ b/Modules/IGTUI/Qmitk/QmitkPolhemusTrackerWidget.cpp @@ -1,180 +1,225 @@ /*=================================================================== 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 "QmitkPolhemusTrackerWidget.h" #include #include +#include #include #include #include #include const std::string QmitkPolhemusTrackerWidget::VIEW_ID = "org.mitk.views.PolhemusTrackerWidget"; QmitkPolhemusTrackerWidget::QmitkPolhemusTrackerWidget(QWidget* parent, Qt::WindowFlags f) : QmitkAbstractTrackingDeviceWidget(parent, f) , m_Controls(nullptr) { } void QmitkPolhemusTrackerWidget::Initialize() { InitializeSuperclassWidget(); CreateQtPartControl(this); SetAdvancedSettingsVisible(false); } QmitkPolhemusTrackerWidget::~QmitkPolhemusTrackerWidget() { delete m_Controls; } void QmitkPolhemusTrackerWidget::CreateQtPartControl(QWidget *parent) { if (!m_Controls) { // create GUI widgets m_Controls = new Ui::QmitkPolhemusTrackerWidget; m_Controls->setupUi(parent); } - } void QmitkPolhemusTrackerWidget::CreateConnections() { if (m_Controls) { connect((QObject*)(m_Controls->m_hemisphereTracking), SIGNAL(clicked()), this, SLOT(on_m_hemisphereTracking_clicked())); connect((QObject*)(m_Controls->m_ToggleHemisphere), SIGNAL(clicked()), this, SLOT(on_m_ToggleHemisphere_clicked())); connect((QObject*)(m_Controls->m_SetHemisphere), SIGNAL(clicked()), this, SLOT(on_m_SetHemisphere_clicked())); connect((QObject*)(m_Controls->m_GetHemisphere), SIGNAL(clicked()), this, SLOT(on_m_GetHemisphere_clicked())); connect((QObject*)(m_Controls->m_AdjustHemisphere), SIGNAL(clicked()), this, SLOT(on_m_AdjustHemisphere_clicked())); } } - mitk::TrackingDevice::Pointer QmitkPolhemusTrackerWidget::ConstructTrackingDevice() { if (m_TrackingDevice.IsNull()) { m_TrackingDevice = mitk::PolhemusTrackingDevice::New(); m_TrackingDevice->SetHemisphereTrackingEnabled(m_Controls->m_hemisphereTracking->isChecked()); } return static_cast(m_TrackingDevice); } - - QmitkPolhemusTrackerWidget* QmitkPolhemusTrackerWidget::Clone(QWidget* parent) const { QmitkPolhemusTrackerWidget* clonedWidget = new QmitkPolhemusTrackerWidget(parent); clonedWidget->Initialize(); return clonedWidget; } void QmitkPolhemusTrackerWidget::on_m_hemisphereTracking_clicked() { m_TrackingDevice->SetHemisphereTrackingEnabled(m_Controls->m_hemisphereTracking->isChecked()); } void QmitkPolhemusTrackerWidget::on_m_ToggleHemisphere_clicked() { // Index 0 == All Tools == -1 for Polhemus interface; Index 2 == Tool 2 == 1 for Polhemus; etc... m_TrackingDevice->ToggleHemisphere(GetSelectedToolIndex()); + + MITK_INFO << "Toggle Hemisphere for tool " << m_Controls->m_ToolSelection->currentText().toStdString(); } void QmitkPolhemusTrackerWidget::on_m_SetHemisphere_clicked() { mitk::Vector3D _hemisphere; mitk::FillVector3D(_hemisphere, m_Controls->m_Hemisphere_X->value(), m_Controls->m_Hemisphere_Y->value(), m_Controls->m_Hemisphere_Z->value()); m_TrackingDevice->SetHemisphere(GetSelectedToolIndex(), _hemisphere); //If you set a hemisphere vector which is unequal (0|0|0), this means, that there is no hemisphere tracing any more //disable the checkbox in case it was on before, so that it can be reactivated... if (_hemisphere.GetNorm() != 0) m_Controls->m_hemisphereTracking->setChecked(false); + + MITK_INFO << "Hemisphere set for tool " << m_Controls->m_ToolSelection->currentText().toStdString(); } void QmitkPolhemusTrackerWidget::on_m_GetHemisphere_clicked() { - - mitk::Vector3D _hemisphere = m_TrackingDevice->GetHemisphere(GetSelectedToolIndex()); m_Controls->m_Hemisphere_X->setValue(_hemisphere[0]); m_Controls->m_Hemisphere_Y->setValue(_hemisphere[1]); m_Controls->m_Hemisphere_Z->setValue(_hemisphere[2]); + + MITK_INFO << "Updated SpinBox for Hemisphere of tool " << m_Controls->m_ToolSelection->currentText().toStdString(); } void QmitkPolhemusTrackerWidget::on_m_AdjustHemisphere_clicked() { - MITK_INFO << "Not implemented yet!"; + int _tool = GetSelectedToolIndex(); + QMessageBox msgBox; + QString _text; + if (_tool == -1) + { + _text.append("Adjusting hemisphere for all tools."); + msgBox.setText(_text); + _text.clear(); + _text = tr("Please make sure, that the entire tools (including tool tip AND sensor) are placed in the positive x hemisphere. Press 'Adjust hemisphere' if you are ready."); + msgBox.setInformativeText(_text); + } + else + { + _text.append("Adjusting hemisphere for tool '"); + _text.append(m_Controls->m_ToolSelection->currentText()); + _text.append(tr("' at port %2.").arg(_tool)); + msgBox.setText(_text); + _text.clear(); + _text = tr("Please make sure, that the entire tool (including tool tip AND sensor) is placed in the positive x hemisphere. Press 'Adjust hemisphere' if you are ready."); + msgBox.setInformativeText(_text); + } + + QPushButton *adjustButton = msgBox.addButton(tr("Adjust hemisphere"), QMessageBox::ActionRole); + QPushButton *cancelButton = msgBox.addButton(QMessageBox::Cancel); + msgBox.exec(); + if (msgBox.clickedButton() == adjustButton) { + // adjust + mitk::Vector3D _hemisphere; + mitk::FillVector3D(_hemisphere, 1, 0, 0); + + //Was HemiTracking on before? + bool _hemiTrack = m_TrackingDevice->GetHemisphereTrackingEnabled(_tool); + + //Set Hemisphere to (1|0|0) where user placed the tool + m_TrackingDevice->SetHemisphere(_tool, _hemisphere); + + //If HemiTrack was on, switch it on again by setting (0|0|0). Don't use general fuction SetHemisphereTrackingEnabled, as we might only adapt a single tool. + if (_hemiTrack) + { + mitk::FillVector3D(_hemisphere, 0, 0, 0); + m_TrackingDevice->SetHemisphere(_tool, _hemisphere); + } + MITK_INFO << "Adjusting Hemisphere for tool " << m_Controls->m_ToolSelection->currentText().toStdString(); + } + else if (msgBox.clickedButton() == cancelButton) { + // abort + MITK_INFO << "Cancel 'Adjust hemisphere'. No harm done..."; + } } void QmitkPolhemusTrackerWidget::OnConnected() { SetAdvancedSettingsVisible(true); if (m_TrackingDevice->GetToolCount() != m_Controls->m_ToolSelection->count()) { m_Controls->m_ToolSelection->clear(); m_Controls->m_ToolSelection->addItem("All Tools"); for (int i = 0; i < m_TrackingDevice->GetToolCount(); ++i) { m_Controls->m_ToolSelection->addItem(m_TrackingDevice->GetTool(i)->GetToolName()); } } - } - void QmitkPolhemusTrackerWidget::OnDisconnected() { SetAdvancedSettingsVisible(false); } void QmitkPolhemusTrackerWidget::SetAdvancedSettingsVisible(bool _enable) { m_Controls->m_ToolSelection->setVisible(_enable); m_Controls->label_toolsToChange->setVisible(_enable); m_Controls->label_UpdateOnRequest->setVisible(_enable); m_Controls->m_GetHemisphere->setVisible(_enable); m_Controls->m_Hemisphere_X->setVisible(_enable); m_Controls->m_Hemisphere_Y->setVisible(_enable); m_Controls->m_Hemisphere_Z->setVisible(_enable); m_Controls->m_SetHemisphere->setVisible(_enable); m_Controls->m_ToggleHemisphere->setVisible(_enable); m_Controls->m_AdjustHemisphere->setVisible(_enable); } int QmitkPolhemusTrackerWidget::GetSelectedToolIndex() { // Index 0 == All Tools == -1 for Polhemus interface; Index 1 == Tool 1 == 1 for Polhemus Interface; etc... - int _index = m_Controls->m_ToolSelection->currentIndex()-1; + int _index = m_Controls->m_ToolSelection->currentIndex() - 1; if (_index != -1) { //we need to find the internal Polhemus index for this tool. This is stored in the identifier of a navigation tool or as Port in PolhemusTool. mitk::PolhemusTool* _tool = dynamic_cast(m_TrackingDevice->GetToolByName(m_Controls->m_ToolSelection->currentText().toStdString())); _index = _tool->GetToolPort(); - } return _index; } \ No newline at end of file