Medical Imaging Interaction Toolkit  2018.4.99-12ad79a3
Medical Imaging Interaction Toolkit
mitkPolhemusInterface.cpp
Go to the documentation of this file.
1 /*============================================================================
2 
3 The Medical Imaging Interaction Toolkit (MITK)
4 
5 Copyright (c) German Cancer Research Center (DKFZ)
6 All rights reserved.
7 
8 Use of this source code is governed by a 3-clause BSD license that can be
9 found in the LICENSE file.
10 
11 ============================================================================*/
12 
13 #include <mitkPolhemusInterface.h>
14 #include <PDI.h>
15 
16 BYTE MotionBuf[0x1FA400];
17 
18 mitk::PolhemusInterface::PolhemusInterface() : m_continousTracking(false)
19 {
20  m_pdiDev = new CPDIdev();
21  m_numberOfTools = 0;
22 }
23 
25 {
26  delete m_pdiDev;
27 }
28 
30 {
31  m_pdiDev->ResetTracker();
32  m_pdiDev->ResetSAlignment(-1);
33  m_pdiDev->Trace(TRUE, 7);
34  m_continousTracking = false;
35  return true;
36 }
37 
39 {
40  m_pdiDev->SetPnoBuffer(MotionBuf, 0x1FA400);
41  m_pdiDev->SetMetric(true); //use cm instead of inches
42 
43  m_pdiDev->StartPipeExport();
44 
45  CPDImdat pdiMDat;
46  pdiMDat.Empty();
47  pdiMDat.Append(PDI_MODATA_FRAMECOUNT);
48  pdiMDat.Append(PDI_MODATA_POS);
49  pdiMDat.Append(PDI_MODATA_ORI);
50  pdiMDat.Append(PDI_MODATA_DISTLEV);
51  m_pdiDev->SetSDataList(-1, pdiMDat);
52 
53  CPDIbiterr cBE;
54  m_pdiDev->GetBITErrs(cBE);
55 
56  if (!(cBE.IsClear())) { m_pdiDev->ClearBITErrs(); }
57 
58  return true;
59 }
60 
62 {
63  m_continousTracking = true;
64  return m_pdiDev->StartContPno(0);
65 }
66 
68 {
69  m_continousTracking = false;
70  m_pdiDev->StopContPno();
71  return true;
72 }
73 
74 bool mitk::PolhemusInterface::OpenConnection()
75 {
76  bool returnValue;
77  //Initialize, and if it is not successful, return false.
78  if (!InitializeDevice())
79  {
80  returnValue = false;
81  }
82  //Connect
83  else if (m_pdiDev->CnxReady())
84  {
85  returnValue = true;
86  }
87  //If it is not successful, search for connections.
88  else
89  {
90  CPDIser pdiSer;
91  m_pdiDev->SetSerialIF(&pdiSer);
92 
93  ePiCommType eType = m_pdiDev->DiscoverCnx();
94  switch (eType)
95  {
96  case PI_CNX_USB:
97  MITK_INFO << "USB Connection: " << m_pdiDev->GetLastResultStr();
98  break;
99  case PI_CNX_SERIAL:
100  MITK_INFO << "Serial Connection: " << m_pdiDev->GetLastResultStr();
101  break;
102  default:
103  MITK_INFO << "DiscoverCnx result: " << m_pdiDev->GetLastResultStr();
104  break;
105  }
106 
107  //Setup device
108  if (!SetupDevice())
109  {
110  returnValue = false;
111  }
112  else
113  {
114  returnValue = m_pdiDev->CnxReady();
115  }
116  }
117  return returnValue;
118 }
119 
121 {
122  bool returnValue = OpenConnection();
123 
124  if (!returnValue)
125  {
126  return returnValue;
127  }
128 
129  m_numberOfTools = this->GetNumberOfTools();
130 
131  //Get the tracking data to find out which tools are available.
132  std::vector<mitk::PolhemusInterface::trackingData> _trackingData = GetFrame();
133 
134  //if we have more/less tools than before, reset all data.
135  //check with toolStorage changes is nto enough, 'cause a sensor could just have been unplugged.
136  if (m_ToolPorts.size() != _trackingData.size())
137  {
138  m_ToolPorts.clear();
139  m_Hemispheres.clear();
140  m_HemisphereTracking.clear();
141  }
142 
143  //if we have the same number of tools as before, check if they are still the same.
144  if (m_ToolPorts.size() == _trackingData.size())
145  {
146  for (size_t i = 0; i < _trackingData.size(); ++i)
147  {
148  //if they are not the same, clear hemispheres and toolNames and break.
149  if (m_ToolPorts[i] != _trackingData.at(i).id)
150  {
151  m_ToolPorts.clear();
152  m_Hemispheres.clear();
153  m_HemisphereTracking.clear();
154  break;
155  }
156  }
157  }
158 
159  //if we don't have old tool names or if the old ones don't match any more, assign them again.
160  if (m_ToolPorts.size() == 0)
161  {
162  for (size_t i = 0; i < _trackingData.size(); ++i)
163  {
164  m_ToolPorts.push_back(_trackingData.at(i).id);
165  }
166  //and reset the hemisphere parameters
167  m_Hemispheres.clear();
168  m_HemisphereTracking.clear();
169  mitk::Vector3D temp;
170  mitk::FillVector3D(temp, 1, 0, 0);
171  m_Hemispheres.assign(m_numberOfTools, temp);
172  m_HemisphereTracking.assign(m_numberOfTools, false);
173  }
174 
175  return returnValue;
176 }
177 
179 {
180  bool returnValue = true;
181  //If Tracking is running, stop tracking first
182  if (m_continousTracking)
183  {
184  this->StopTracking();
185  }
186 
187  returnValue = m_pdiDev->Disconnect();
188  MITK_INFO << "Disconnect: " << m_pdiDev->GetLastResultStr();
189  return returnValue;
190 }
191 
192 std::vector<mitk::PolhemusInterface::trackingData> mitk::PolhemusInterface::AutoDetectTools()
193 {
194  OpenConnection();
195  std::vector<mitk::PolhemusInterface::trackingData> frame = GetSingleFrame();
196  m_pdiDev->Disconnect();
197  return frame;
198 }
199 
201 {
202  std::vector<mitk::PolhemusInterface::trackingData> _trackingData = GetFrame();
203  return _trackingData.size();
204 }
205 
206 std::vector<mitk::PolhemusInterface::trackingData> mitk::PolhemusInterface::GetFrame()
207 {
208  if (m_continousTracking)
209  return this->GetLastFrame();
210  else
211  return this->GetSingleFrame();
212 }
213 
214 std::vector<mitk::PolhemusInterface::trackingData> mitk::PolhemusInterface::GetLastFrame()
215 {
216  PBYTE pBuf;
217  DWORD dwSize;
218 
219  //read one frame
220  if (!m_pdiDev->LastPnoPtr(pBuf, dwSize)) { MITK_WARN << m_pdiDev->GetLastResultStr(); }
221 
222  std::vector<mitk::PolhemusInterface::trackingData> returnValue = ParsePolhemusRawData(pBuf, dwSize);
223 
224  if (returnValue.empty())
225  {
226  MITK_WARN << "Cannot parse data / no tools present";
227  }
228 
229  return returnValue;
230 }
231 
232 std::vector<mitk::PolhemusInterface::trackingData> mitk::PolhemusInterface::GetSingleFrame()
233 {
234  if (m_continousTracking)
235  {
236  MITK_WARN << "Cannot get a single frame when continuous tracking is on!";
237  return std::vector<mitk::PolhemusInterface::trackingData>();
238  }
239  PBYTE pBuf;
240  DWORD dwSize;
241 
242  //read one frame
243  if (!m_pdiDev->ReadSinglePnoBuf(pBuf, dwSize)) {
244  MITK_WARN << m_pdiDev->GetLastResultStr();
245  return std::vector<mitk::PolhemusInterface::trackingData>();
246  }
247 
248  return ParsePolhemusRawData(pBuf, dwSize);
249 }
250 
251 std::vector<mitk::PolhemusInterface::trackingData> mitk::PolhemusInterface::ParsePolhemusRawData(PBYTE pBuf, DWORD dwSize)
252 {
253  std::vector<mitk::PolhemusInterface::trackingData> returnValue;
254 
255  DWORD i = 0;
256 
257  while (i < dwSize)
258  {
259  BYTE ucSensor = pBuf[i + 2];
260  SHORT shSize = pBuf[i + 6];
261 
262  // skip rest of header
263  i += 8;
264 
265  PDWORD pFC = (PDWORD)(&pBuf[i]);
266  PFLOAT pPno = (PFLOAT)(&pBuf[i + 4]);
267  PINT pDistLevel = (PINT)(&pBuf[i + 28]);
268 
269  mitk::PolhemusInterface::trackingData currentTrackingData;
270 
271  currentTrackingData.id = ucSensor;
272 
273  currentTrackingData.pos[0] = pPno[0] * 10; //from cm to mm
274  currentTrackingData.pos[1] = pPno[1] * 10;
275  currentTrackingData.pos[2] = pPno[2] * 10;
276 
277  double azimuthAngle = pPno[3] / 180 * itk::Math::pi; //from degree to rad
278  double elevationAngle = pPno[4] / 180 * itk::Math::pi;
279  double rollAngle = pPno[5] / 180 * itk::Math::pi;
280  vnl_quaternion<double> eulerQuat(rollAngle, elevationAngle, azimuthAngle);
281  currentTrackingData.rot = eulerQuat;
282  currentTrackingData.distortionLevel = *pDistLevel;
283 
284  returnValue.push_back(currentTrackingData);
285  i += shSize;
286  }
287  return returnValue;
288 }
289 
290 void mitk::PolhemusInterface::SetHemisphereTrackingEnabled(bool _HemisphereTrackingEnabled, int _tool)
291 {
292  //only if connection is ready!
293  if (!this->m_pdiDev->CnxReady())
294  return;
295 
296  if (m_Hemispheres.empty())
297  {
298  MITK_ERROR << "No Hemispheres. This should never happen when connected. Check your code!";
299  }
300 
301  //HemisphereTracking is switched on by SetSHemiTrack(-1). "-1" means for all sensors.
302  //To switch hemisphere tracking off, you need to set a hemisphere vector e.g. by calling SetSHemisphere(-1, { (float)1,0,0 })
303  if (_HemisphereTrackingEnabled)
304  {
305  m_pdiDev->SetSHemiTrack(_tool);
306  if (_tool != -1)
307  {
308  m_HemisphereTracking.at(GetToolIndex(_tool)) = true;
309  }
310  else
311  {
312  m_HemisphereTracking.assign(m_numberOfTools, true);
313  }
314  }
315 
316  //switch HemiTracking OFF
317  else
318  {
319  //Get Tool Position. ToDo, this should not be the tool tip but the sensor position. Any chance, to get that from Polhemus interface?!
320  std::vector<mitk::PolhemusInterface::trackingData> _position = GetFrame();
321 
322  for (int index : GetToolIterator(_tool))
323  {
324  //Scalar product between mitk::point and mitk::vector
325  double _scalarProduct = _position.at(index).pos.GetVectorFromOrigin() * m_Hemispheres.at(index);
326  //if scalar product is negative, then the tool is in the opposite sphere then when we started to track.
327  //Hence, we have to set the inverted hemisphere.
328  //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...
329  if (_scalarProduct < 0)
330  {
331  m_Hemispheres.at(index) = -1. * m_Hemispheres.at(index);
332  }
333  else if (_scalarProduct == 0)
334  MITK_ERROR << "Something went wrong. Hemisphere or Position should not be zero.";
335 
336  SetHemisphere(m_ToolPorts[index], m_Hemispheres.at(index));
337  }
338  }
339 }
340 
342 {
343  //only if connection is ready!
344  if (!this->m_pdiDev->CnxReady())
345  return;
346 
347  //toggle.
348  for (int index : GetToolIterator(_tool))
349  {
350  if (m_HemisphereTracking.at(index))
351  {
352  SetHemisphereTrackingEnabled(false, m_ToolPorts[index]);
353  this->SetHemisphere(m_ToolPorts[index], -1.*m_Hemispheres.at(index));
354  SetHemisphereTrackingEnabled(true, m_ToolPorts[index]);
355  }
356  else
357  {
358  this->SetHemisphere(m_ToolPorts[index], -1.*m_Hemispheres.at(index));
359  }
360  }
361 }
362 
364 {
365  //only if connection is ready!
366  if (!this->m_pdiDev->CnxReady())
367  return;
368 
369  mitk::Vector3D _hemisphere;
370  mitk::FillVector3D(_hemisphere, 1, 0, 0);
371 
372  for (int index : GetToolIterator(_tool))
373  {
374  if (m_HemisphereTracking.at(index))
375  {
376  SetHemisphereTrackingEnabled(false, m_ToolPorts[index]);
377  this->SetHemisphere(m_ToolPorts[index], _hemisphere);
378  SetHemisphereTrackingEnabled(true, m_ToolPorts[index]);
379  }
380  else
381  {
382  this->SetHemisphere(m_ToolPorts[index], _hemisphere);
383  }
384  }
385 }
386 
388 {
389  //only if connection is ready!
390  if (!this->m_pdiDev->CnxReady())
391  return;
392 
393  m_pdiDev->SetSHemisphere(_tool, { (float)_hemisphere[0], (float)_hemisphere[1], (float)_hemisphere[2] });
394 
395  for (int index : GetToolIterator(_tool))
396  {
397  if (_hemisphere.GetNorm() != 0)
398  {
399  m_HemisphereTracking.at(index) = false;
400  m_Hemispheres.at(index) = _hemisphere;
401  }
402  else
403  {
404  m_HemisphereTracking.at(index) = true;
405  //don't set the Hemisphere to (0|0|0), as we want to remember the old one.
406  }
407  }
408 }
409 
411 {
412  if (_tool == -1)
413  {
414  MITK_WARN << "Can't return hemisphere for all tools. Returning Hemisphere of first tool " << m_ToolPorts[0];
415  return m_Hemispheres.at(0);
416  }
417  return m_Hemispheres.at(GetToolIndex(_tool));
418 }
419 
421 {
422  //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.
423  if (_tool == -1)
424  {
425  bool _returnValue = true;
426  for (bool currentValue : m_HemisphereTracking)
427  _returnValue = _returnValue && currentValue;
428  return _returnValue;
429  }
430  else
431  return m_HemisphereTracking.at(GetToolIndex(_tool));
432 }
433 
435 {
436  return m_ToolPorts;
437 }
438 
440 {
441  if (_tool == -1)
442  return -1;
443  else
444  return std::find(m_ToolPorts.begin(), m_ToolPorts.end(), _tool) - m_ToolPorts.begin();
445 }
446 
447 std::vector<int> mitk::PolhemusInterface::GetToolIterator(int _tool)
448 {
449  std::vector<int> _iterator;
450  if (_tool == -1)
451  {
452  for (int i = 0; i < static_cast<int>(m_numberOfTools); ++i)
453  _iterator.push_back(i);
454  }
455  else
456  {
457  _iterator.push_back(GetToolIndex(_tool));
458  }
459  return _iterator;
460 }
461 
463 {
464  MITK_INFO << "Polhemus status: " << this->m_pdiDev->CnxReady();
465 }
std::vector< trackingData > GetSingleFrame()
#define MITK_INFO
Definition: mitkLogMacros.h:18
PolhemusInterface()
standard constructor
bool StopTracking()
Clears all resources. After this method have been called the system isn&#39;t ready to track any longer...
mitk::Vector3D GetHemisphere(int _tool)
~PolhemusInterface()
standard destructor
#define MITK_ERROR
Definition: mitkLogMacros.h:20
std::vector< mitk::PolhemusInterface::trackingData > ParsePolhemusRawData(PBYTE pBuf, DWORD dwSize)
std::vector< trackingData > GetFrame()
Convenient method to get a frame from the tracking device.
bool GetHemisphereTrackingEnabled(int _tool)
void FillVector3D(Tout &out, mitk::ScalarType x, mitk::ScalarType y, mitk::ScalarType z)
Definition: mitkArray.h:106
BYTE MotionBuf[0x1FA400]
std::vector< trackingData > AutoDetectTools()
#define MITK_WARN
Definition: mitkLogMacros.h:19
bool StartTracking()
Opens the connection to the device and makes it ready to track tools.
void ToggleHemisphere(int _tool=-1)
void SetHemisphereTrackingEnabled(bool _HemisphereTrackingEnabled, int _tool=-1)
void SetHemisphere(int _tool, mitk::Vector3D _hemisphere)
std::vector< int > GetToolPorts()
std::vector< trackingData > GetLastFrame()