diff --git a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp
index 6ec9715e98..89c0b1e9cf 100644
--- a/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp
+++ b/Modules/IGT/TrackingDevices/mitkPolhemusInterface.cpp
@@ -1,465 +1,465 @@
/*============================================================================
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 <mitkPolhemusInterface.h>
#include <PDI.h>
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();
+ //m_pdiDev->StartPipeExport();
CPDImdat pdiMDat;
pdiMDat.Empty();
pdiMDat.Append(PDI_MODATA_FRAMECOUNT);
pdiMDat.Append(PDI_MODATA_POS);
pdiMDat.Append(PDI_MODATA_ORI);
pdiMDat.Append(PDI_MODATA_DISTLEV);
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();
+ MITK_INFO << "USB Connection";
break;
case PI_CNX_SERIAL:
- MITK_INFO << "Serial Connection: " << m_pdiDev->GetLastResultStr();
+ MITK_INFO << "Serial Connection";
break;
default:
- MITK_INFO << "DiscoverCnx result: " << m_pdiDev->GetLastResultStr();
+ MITK_INFO << "DiscoverCnx";
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<mitk::PolhemusInterface::trackingData> _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();
+ MITK_INFO << "Disconnect";
return returnValue;
}
std::vector<mitk::PolhemusInterface::trackingData> mitk::PolhemusInterface::AutoDetectTools()
{
OpenConnection();
std::vector<mitk::PolhemusInterface::trackingData> frame = GetSingleFrame();
m_pdiDev->Disconnect();
return frame;
}
unsigned int mitk::PolhemusInterface::GetNumberOfTools()
{
std::vector<mitk::PolhemusInterface::trackingData> _trackingData = GetFrame();
return _trackingData.size();
}
std::vector<mitk::PolhemusInterface::trackingData> mitk::PolhemusInterface::GetFrame()
{
if (m_continousTracking)
return this->GetLastFrame();
else
return this->GetSingleFrame();
}
std::vector<mitk::PolhemusInterface::trackingData> mitk::PolhemusInterface::GetLastFrame()
{
PBYTE pBuf;
DWORD dwSize;
//read one frame
- if (!m_pdiDev->LastPnoPtr(pBuf, dwSize)) { MITK_WARN << m_pdiDev->GetLastResultStr(); }
+ if (!m_pdiDev->LastPnoPtr(pBuf, dwSize)) { MITK_WARN << "There is an issue"; }
std::vector<mitk::PolhemusInterface::trackingData> returnValue = ParsePolhemusRawData(pBuf, dwSize);
if (returnValue.empty())
{
MITK_WARN << "Cannot parse data / no tools present";
}
return returnValue;
}
std::vector<mitk::PolhemusInterface::trackingData> mitk::PolhemusInterface::GetSingleFrame()
{
if (m_continousTracking)
{
MITK_WARN << "Cannot get a single frame when continuous tracking is on!";
return std::vector<mitk::PolhemusInterface::trackingData>();
}
PBYTE pBuf;
DWORD dwSize;
//read one frame
if (!m_pdiDev->ReadSinglePnoBuf(pBuf, dwSize)) {
- MITK_WARN << m_pdiDev->GetLastResultStr();
+ MITK_WARN << "There is an issue";
return std::vector<mitk::PolhemusInterface::trackingData>();
}
return ParsePolhemusRawData(pBuf, dwSize);
}
std::vector<mitk::PolhemusInterface::trackingData> mitk::PolhemusInterface::ParsePolhemusRawData(PBYTE pBuf, DWORD dwSize)
{
std::vector<mitk::PolhemusInterface::trackingData> 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]);
PINT pDistLevel = (PINT)(&pBuf[i + 28]);
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<double> eulerQuat(rollAngle, elevationAngle, azimuthAngle);
currentTrackingData.rot = eulerQuat;
currentTrackingData.distortionLevel = *pDistLevel;
returnValue.push_back(currentTrackingData);
i += shSize;
}
return returnValue;
}
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 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<mitk::PolhemusInterface::trackingData> _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<int> 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<int> mitk::PolhemusInterface::GetToolIterator(int _tool)
{
std::vector<int> _iterator;
if (_tool == -1)
{
for (int i = 0; i < static_cast<int>(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();
}