diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp index 31eb691c2b..20c0647d40 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp +++ b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp @@ -1,466 +1,466 @@ /*=================================================================== 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 #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::OpenConnection() { 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(); } } return returnValue; } bool mitk::PolhemusInterface::Connect() { bool returnValue = OpenConnection(); if (!returnValue) { return returnValue; } m_numberOfTools = this->GetNumberOfTools(); //Get the tracking data to find out which tools are available. std::vector _trackingData = GetFrame(); //if we have more/less tools than before, reset all data. //check with toolStorage changes is nto enough, 'cause a sensor could just have been unplugged. if (m_ToolPorts.size() != _trackingData.size()) { m_ToolPorts.clear(); m_Hemispheres.clear(); m_HemisphereTracking.clear(); } //if we have the same number of tools as before, check if they are still the same. if (m_ToolPorts.size() == _trackingData.size()) { for (size_t 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(); m_HemisphereTracking.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 (size_t i = 0; i < _trackingData.size(); ++i) { m_ToolPorts.push_back(_trackingData.at(i).id); } //and reset the hemisphere parameters m_Hemispheres.clear(); m_HemisphereTracking.clear(); mitk::Vector3D temp; mitk::FillVector3D(temp, 1, 0, 0); m_Hemispheres.assign(m_numberOfTools, temp); m_HemisphereTracking.assign(m_numberOfTools, false); } 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::AutoDetectTools() { OpenConnection(); std::vector frame = GetSingleFrame(); m_pdiDev->Disconnect(); return frame; } unsigned int mitk::PolhemusInterface::GetNumberOfTools() { std::vector _trackingData = GetFrame(); return _trackingData.size(); } std::vector mitk::PolhemusInterface::GetFrame() { if (m_continousTracking) return this->GetLastFrame(); else return this->GetSingleFrame(); } 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; } 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 * itk::Math::pi; //from degree to rad double elevationAngle = pPno[4] / 180 * itk::Math::pi; double rollAngle = pPno[5] / 180 * itk::Math::pi; vnl_quaternion eulerQuat(rollAngle, elevationAngle, azimuthAngle); currentTrackingData.rot = eulerQuat; returnValue.push_back(currentTrackingData); i += shSize; } return returnValue; } -void mitk::PolhemusInterface::SetHemisphereTrackingEnabled(bool _HeisphereTrackingEnabeled, int _tool) +void mitk::PolhemusInterface::SetHemisphereTrackingEnabled(bool _HemisphereTrackingEnabled, int _tool) { //only if connection is ready! if (!this->m_pdiDev->CnxReady()) return; if (m_Hemispheres.empty()) { MITK_ERROR << "No Hemispheres. This should never happen when connected. Check your code!"; } //HemisphereTracking is switched on by SetSHemiTrack(-1). "-1" means for all sensors. - //To switch heisphere tracking of, you need to set a hemisphere vector e.g. by calling SetSHemisphere(-1, { (float)1,0,0 }) - if (_HeisphereTrackingEnabeled) + //To switch hemisphere tracking off, you need to set a hemisphere vector e.g. by calling SetSHemisphere(-1, { (float)1,0,0 }) + if (_HemisphereTrackingEnabled) { m_pdiDev->SetSHemiTrack(_tool); if (_tool != -1) { m_HemisphereTracking.at(GetToolIndex(_tool)) = true; } else { m_HemisphereTracking.assign(m_numberOfTools, true); } } //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 = GetFrame(); for (int index : GetToolIterator(_tool)) { //Scalar product between mitk::point and mitk::vector double _scalarProduct = _position.at(index).pos.GetVectorFromOrigin() * m_Hemispheres.at(index); //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(index) = -1. * m_Hemispheres.at(index); } else if (_scalarProduct == 0) MITK_ERROR << "Something went wrong. Hemisphere or Position should not be zero."; SetHemisphere(m_ToolPorts[index], m_Hemispheres.at(index)); } } } void mitk::PolhemusInterface::ToggleHemisphere(int _tool) { //only if connection is ready! if (!this->m_pdiDev->CnxReady()) return; //toggle. for (int index : GetToolIterator(_tool)) { if (m_HemisphereTracking.at(index)) { SetHemisphereTrackingEnabled(false, m_ToolPorts[index]); this->SetHemisphere(m_ToolPorts[index], -1.*m_Hemispheres.at(index)); SetHemisphereTrackingEnabled(true, m_ToolPorts[index]); } else { this->SetHemisphere(m_ToolPorts[index], -1.*m_Hemispheres.at(index)); } } } void mitk::PolhemusInterface::AdjustHemisphere(int _tool) { //only if connection is ready! if (!this->m_pdiDev->CnxReady()) return; mitk::Vector3D _hemisphere; mitk::FillVector3D(_hemisphere, 1, 0, 0); for (int index : GetToolIterator(_tool)) { if (m_HemisphereTracking.at(index)) { SetHemisphereTrackingEnabled(false, m_ToolPorts[index]); this->SetHemisphere(m_ToolPorts[index], _hemisphere); SetHemisphereTrackingEnabled(true, m_ToolPorts[index]); } else { this->SetHemisphere(m_ToolPorts[index], _hemisphere); } } } 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] }); for (int index : GetToolIterator(_tool)) { if (_hemisphere.GetNorm() != 0) { m_HemisphereTracking.at(index) = false; m_Hemispheres.at(index) = _hemisphere; } else { m_HemisphereTracking.at(index) = true; //don't set the Hemisphere to (0|0|0), as we want to remember the old one. } } } mitk::Vector3D mitk::PolhemusInterface::GetHemisphere(int _tool) { if (_tool == -1) { MITK_WARN << "Can't return hemisphere for all tools. Returning Hemisphere of first tool " << m_ToolPorts[0]; return m_Hemispheres.at(0); } return m_Hemispheres.at(GetToolIndex(_tool)); } bool mitk::PolhemusInterface::GetHemisphereTrackingEnabled(int _tool) { //if tool is -1, this means "All Tools". We return true if HemiTracking is enabled for all tools, and false if it is off for at least one tool. if (_tool == -1) { bool _returnValue = true; for (bool currentValue : m_HemisphereTracking) _returnValue = _returnValue && currentValue; return _returnValue; } else return m_HemisphereTracking.at(GetToolIndex(_tool)); } std::vector mitk::PolhemusInterface::GetToolPorts() { return m_ToolPorts; } int mitk::PolhemusInterface::GetToolIndex(int _tool) { if (_tool == -1) return -1; else return std::find(m_ToolPorts.begin(), m_ToolPorts.end(), _tool) - m_ToolPorts.begin(); } std::vector mitk::PolhemusInterface::GetToolIterator(int _tool) { std::vector _iterator; if (_tool == -1) { for (int i = 0; i < static_cast(m_numberOfTools); ++i) _iterator.push_back(i); } else { _iterator.push_back(GetToolIndex(_tool)); } return _iterator; } void mitk::PolhemusInterface::PrintStatus() { MITK_INFO << "Polhemus status: " << this->m_pdiDev->CnxReady(); } \ No newline at end of file diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.h b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.h index 4123d66038..b8612548ca 100644 --- a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.h +++ b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.h @@ -1,174 +1,174 @@ /*=================================================================== 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 the number of tools. Returns 0 if no information is avialable.*/ + /** @return Returns the number of tools. Returns 0 if no information is available.*/ unsigned int GetNumberOfTools(); /** Enables/disables hemisphere tracking for all stations/tools. */ - void SetHemisphereTrackingEnabled(bool _HeisphereTrackingEnabeled, int _tool = -1); + void SetHemisphereTrackingEnabled(bool _HemisphereTrackingEnabled, int _tool = -1); /** 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? * if tool is -1, this means "All Tools". We return true if HemiTracking is enabled for all tools, and false if it is off for at least one tool.*/ bool GetHemisphereTrackingEnabled(int _tool); /** Adjust the Hemisphere for this tool. User needs to make sure, that the tool is located in hemisphere (1|0|0) when calling this function. In contrast to SetHemisphere(1,0,0), this method restores the original HemisphereTracking settings at the end. */ void AdjustHemisphere(int _tool); - /** @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.*/ + /** @return Returns a single frame. Only works if the tracking device is not in continuous tracking mode. Returns an empty vector in case of an error.*/ std::vector GetSingleFrame(); /** @return Returns a single frame with all tools, which could be auto detected.*/ std::vector AutoDetectTools(); /** @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(); 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); bool InitializeDevice(); bool SetupDevice(); //returns the index in the arrays of tool _tool. Eg. sensor 3 (_tool = 3) is the second tool --> index 1 in m_Hemispheres etc. int GetToolIndex(int _tool); /** @brief Convenient method to get a frame from the tracking device. * @return Returns a single OR the last frame depending on m_continuousTracking. * @warning Don't use this function if you use different threads. You need to make sure, that you are still in the right mode! */ std::vector GetFrame(); private: //returns vector with tool index as only element if tool != -1, else returns vector from 0 to numberOfTools std::vector GetToolIterator(int _tool); //helper method to open connection bool OpenConnection(); private: //Stores the hemispheres for all sensors. Default is (1|0|0). std::vector m_Hemispheres; //Stores, if hemisphereTracking is on for this Sensor. std::vector m_HemisphereTracking; //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; unsigned int m_numberOfTools; bool m_continousTracking; }; }//mitk #endif