Fraxinus  2023.01.05-dev+develop.0da12
An IGT application
cxIgstkTool.cpp
Go to the documentation of this file.
1 /*=========================================================================
2 This file is part of CustusX, an Image Guided Therapy Application.
3 
4 Copyright (c) SINTEF Department of Medical Technology.
5 All rights reserved.
6 
7 CustusX is released under a BSD 3-Clause license.
8 
9 See Lisence.txt (https://github.com/SINTEFMedtek/CustusX/blob/master/License.txt) for details.
10 =========================================================================*/
11 
12 #include "cxIgstkTool.h"
13 
14 #include <QDateTime>
15 #include <QStringList>
16 #include <QTextStream>
17 #include <vtkTransform.h>
18 #include "cxLogger.h"
19 #include "cxTypeConversions.h"
20 #include "cxSettings.h"
21 #include "cxTransformFile.h"
22 #include "cxIgstkToolManager.h"
23 
24 namespace cx
25 {
26 
28 {
29  igstk::Transform retval;
30  retval.ImportTransform(*transform.getVtkMatrix());
31  return retval;
32 }
33 
35 {
36  Transform3D retval;
37  vtkMatrix4x4Ptr matrix = vtkMatrix4x4Ptr::New();
38  transform.ExportTransform(*(matrix.GetPointer()));
39  retval = Transform3D::fromVtkMatrix(matrix);
40  return retval;
41 }
42 
44 {
45  //apply the calibration
46  mInternalStructure->mCalibration = cal;
47  this->setCalibrationTransform(mInternalStructure->mCalibration);
48 
49  Transform3D sMt = mInternalStructure->getCalibrationAsSSC();
50  report("Set " + mInternalStructure->mName + "s calibration to \n" + qstring_cast(sMt));
51 
52  //write to file
53  mInternalStructure->saveCalibrationToFile();
54 }
55 
59 
61  mToolObserver(itk::ReceptorMemberCommand<IgstkTool>::New()), mValid(false), mVisible(false), mAttachedToTracker(
62  false)
63 {
64  qRegisterMetaType<Transform3D>("Transform3D");
65  qRegisterMetaType<ToolPositionMetadata>("ToolPositionMetadata");
66 
67  mLatestEmittedTimestamp = 0;
68 
69  mInternalStructure = internalStructure;
70 
71  mToolObserver->SetCallbackFunction(this, &IgstkTool::toolTransformCallback);
72 
73  if (mInternalStructure->verify())
74  {
75  mTool = this->buildInternalTool();
76  this->addLogging();
77  mValid = true;
78  }
79  else
80  {
81  reportError(mInternalStructure->mUid + " was created with invalid internal structure.");
82  mValid = false;
83  }
84 }
85 
87 {
88 }
89 
91 {
92  return mInternalStructure;
93 }
94 
96 {
97  return mInternalStructure->mUid;
98 }
99 
100 igstk::TrackerTool::Pointer IgstkTool::getPointer() const
101 {
102  return mTool;
103 }
104 
105 TRACKING_SYSTEM IgstkTool::getTrackerType()
106 {
107  return mInternalStructure->mTrackerType;
108 }
109 
110 bool IgstkTool::isValid() const
111 {
112  return mValid;
113 }
114 
116 {
117  return mAttachedToTracker;
118 }
119 
121 {
122  return mTracked;
123 }
124 
126 {
127  return mVisible;
128 }
129 
131 {
132  mReferenceTool = refTool;
133 }
134 
136 {
137  mTracker = tracker;
138 }
139 
140 igstk::NDITracker::TrackingSampleInfo IgstkTool::getSampleInfo()
141 {
142  igstk::NDITracker::TrackingSampleInfo retval;
143  retval.m_FrameNumber = 0;
144 
145  TrackerPtr tracker = mTracker.lock();
146  if (!tracker)
147  return retval;
148  igstk::NDITracker* ndiTracker = dynamic_cast<igstk::NDITracker*>(tracker->getPointer());
149  if (!ndiTracker)
150  return retval;
151 
152  // hacking into the IGSTK internals...
153  std::map<std::string,igstk::NDITracker::TrackingSampleInfo> info = ndiTracker->GetTrackingSampleInfo();
154  return info[mTool->GetTrackerToolIdentifier()];
155 }
156 
157 bool IgstkTool::validReferenceForResult(igstk::CoordinateSystemTransformToResult result)
158 {
159  const igstk::CoordinateSystem* destination = result.GetDestination();
160  IgstkToolPtr refTool = mReferenceTool.lock();
161 
162  if (refTool) //if we are tracking with a reftool it must be visible
163  {
164  if (!refTool->getPointer()->IsCoordinateSystem(destination))
165  return false;
166  }
167  else //if we dont have a reftool we use the tracker as patientref
168  {
169  TrackerPtr tracker = mTracker.lock();
170  if (!tracker || !tracker->getPointer()->IsCoordinateSystem(destination))
171  return false;
172  }
173 
174  return true;
175 }
176 
177 Transform3D IgstkTool::igstk2Transform3D(const igstk::Transform& input) const
178 {
179  vtkMatrix4x4Ptr vtkMatrix = vtkMatrix4x4Ptr::New();
180  input.ExportTransform(*vtkMatrix.GetPointer());
181  return Transform3D(vtkMatrix.GetPointer());
182 }
183 
184 void IgstkTool::processReceivedTransformResult(igstk::CoordinateSystemTransformToResult result)
185 {
186  if (!this->validReferenceForResult(result))
187  return;
188 
189  // emit even if not visible: need error metadata
190 
191  igstk::NDITracker::TrackingSampleInfo sampleInfo = this->getSampleInfo();
192 
193  // ignore duplicate positions
194  if (similar(mLatestEmittedTimestamp, sampleInfo.m_TimeStamp,1.0E-3))
195  {
196  return;
197  }
198  mLatestEmittedTimestamp = sampleInfo.m_TimeStamp;
199 
200  QDomDocument doc;
201  QDomElement root = doc.createElement("info");
202  doc.appendChild(root);
203  sampleInfo2xml(sampleInfo, root);
204  ToolPositionMetadata metadata;
205  metadata.mData = doc.toString();
206 
207  igstk::Transform transform = result.GetTransform();
208  Transform3D prMt = igstk2Transform3D(transform);
209  double timestamp = transform.GetStartTime();
210 
211  emit toolTransformAndTimestamp(prMt, timestamp, metadata);
212 }
213 
214 void IgstkTool::toolTransformCallback(const itk::EventObject &event)
215 {
216  if (igstk::CoordinateSystemTransformToEvent().CheckEvent(&event))
217  {
218  const igstk::CoordinateSystemTransformToEvent *transformEvent;
219  transformEvent = dynamic_cast<const igstk::CoordinateSystemTransformToEvent*>(&event);
220  if (!transformEvent)
221  return;
222  this->processReceivedTransformResult(transformEvent->Get());
223  }
224  //Successes
225  else if (igstk::TrackerToolConfigurationEvent().CheckEvent(&event))
226  {
227  report(QString("Configured [%1] with the tracking system").arg(mInternalStructure->mUid));
228  }
229  else if (igstk::TrackerToolAttachmentToTrackerEvent().CheckEvent(&event))
230  {
231  this->internalAttachedToTracker(true);
232  }
233  else if (igstk::TrackerToolDetachmentFromTrackerEvent().CheckEvent(&event))
234  {
235  this->internalAttachedToTracker(false);
236  }
237  else if (igstk::TrackerToolMadeTransitionToTrackedStateEvent().CheckEvent(&event))
238  {
239  this->internalVisible(true);
240  }
241  else if (igstk::TrackerToolNotAvailableToBeTrackedEvent().CheckEvent(&event))
242  {
243  this->internalVisible(false);
244  }
245  else if (igstk::ToolTrackingStartedEvent().CheckEvent(&event))
246  {
247  this->internalTracked(true);
248  report(mInternalStructure->mUid + " is tracked.");
249  }
250  else if (igstk::ToolTrackingStoppedEvent().CheckEvent(&event))
251  {
252  this->internalTracked(false);
253  report(mInternalStructure->mUid + " is not tracked anymore.");
254  }
255  //Failures
256  else if (igstk::InvalidRequestErrorEvent().CheckEvent(&event))
257  {
259  mInternalStructure->mUid
260  + " received an invalid request. This means that the internal igstk trackertool did not accept the request. Do not know which request.");
261  }
262  else if (igstk::TrackerToolConfigurationErrorEvent().CheckEvent(&event))
263  {
264  reportError(mInternalStructure->mUid + " could not configure with the tracking system.");
265  }
266  else if (igstk::InvalidRequestToAttachTrackerToolErrorEvent().CheckEvent(&event))
267  {
268  reportError(mInternalStructure->mUid + " could not request to attach to tracker.");
269  }
270  else if (igstk::InvalidRequestToDetachTrackerToolErrorEvent().CheckEvent(&event))
271  {
272  reportError(mInternalStructure->mUid + " could not request to detach from tracker.");
273  }
274  else if (igstk::TrackerToolAttachmentToTrackerErrorEvent().CheckEvent(&event))
275  {
276  reportError(mInternalStructure->mUid + " could not attach to tracker.");
277  }
278  else if (igstk::TrackerToolDetachmentFromTrackerErrorEvent().CheckEvent(&event))
279  {
280  reportError(mInternalStructure->mUid + " could not detach from tracker.");
281  }
282  //Polaris specific failures
283  else if (igstk::InvalidPolarisPortNumberErrorEvent().CheckEvent(&event))
284  {
285  reportError(
286  mInternalStructure->mUid + " sendt invalid Polaris port number: "
287  + qstring_cast(mInternalStructure->mPortNumber) + ".");
288  }
289  else if (igstk::InvalidPolarisSROMFilenameErrorEvent().CheckEvent(&event))
290  {
291  reportError(
292  mInternalStructure->mUid + " sendt invalid ROM file: " + mInternalStructure->mSROMFilename);
293  }
294  else if (igstk::InvalidPolarisPartNumberErrorEvent().CheckEvent(&event))
295  {
296  reportError(mInternalStructure->mUid + " has an invalid part number.");
297  }
298  //Aurora specific failures
299  else if (igstk::InvalidAuroraPortNumberErrorEvent().CheckEvent(&event))
300  {
301  reportError(
302  mInternalStructure->mUid + " has an invalid port number: "
303  + qstring_cast(mInternalStructure->mPortNumber) + ".");
304  }
305  else if (igstk::InvalidAuroraSROMFilenameErrorEvent().CheckEvent(&event))
306  {
307  reportError(
308  mInternalStructure->mUid + " sendt invalid ROM file: " + mInternalStructure->mSROMFilename);
309  }
310  else if (igstk::InvalidAuroraPartNumberErrorEvent().CheckEvent(&event))
311  {
312  reportError(mInternalStructure->mUid + " has an invalid part number.");
313  }
314  else if (igstk::InvalidAuroraChannelNumberErrorEvent().CheckEvent(&event))
315  {
316  reportError(
317  mInternalStructure->mUid + " has an invalid channel number:"
318  + qstring_cast(mInternalStructure->mChannelNumber) + ".");
319  }
320 }
321 
322 igstk::TrackerTool::Pointer IgstkTool::buildInternalTool()
323 {
324  igstk::TrackerTool::Pointer tool;
325 
326  igstk::PolarisTrackerTool::Pointer tempPolarisTool;
327  igstk::AuroraTrackerTool::Pointer tempAuroraTool;
328 
329  igstk::Transform calibration = toIgstkTransform(mInternalStructure->mCalibration);
330  switch (mInternalStructure->mTrackerType)
331  {
332  case tsNONE:
333  break;
334  case tsPOLARIS_SPECTRA:
335  case tsPOLARIS_VICRA:
336  case tsPOLARIS:
337  tempPolarisTool = igstk::PolarisTrackerTool::New();
338  tempPolarisTool->AddObserver(igstk::IGSTKEvent(), mToolObserver);
339  if (!mInternalStructure->mWireless) //we only support wireless atm
340  return tool = tempPolarisTool.GetPointer();
341  tempPolarisTool->RequestSelectWirelessTrackerTool();
342  tempPolarisTool->RequestSetSROMFileName(string_cast(mInternalStructure->mSROMFilename));
343  tempPolarisTool->RequestConfigure();
344  tempPolarisTool->SetCalibrationTransform(calibration);
345  tool = tempPolarisTool;
346  break;
347  case tsAURORA:
348  tempAuroraTool = igstk::AuroraTrackerTool::New();
349  tempAuroraTool->AddObserver(igstk::IGSTKEvent(), mToolObserver);
350  if (mInternalStructure->m5DOF)
351  {
352  tempAuroraTool->RequestSelect5DOFTrackerTool();
353  tempAuroraTool->RequestSetPortNumber(mInternalStructure->mPortNumber);
354  tempAuroraTool->RequestSetChannelNumber(mInternalStructure->mChannelNumber);
355  }
356  else
357  {
358  tempAuroraTool->RequestSelect6DOFTrackerTool();
359  tempAuroraTool->RequestSetPortNumber(mInternalStructure->mPortNumber);
360  }
361  tempAuroraTool->RequestConfigure();
362  tempAuroraTool->SetCalibrationTransform(calibration);
363  tool = tempAuroraTool;
364  break;
365  case tsMICRON:
366  //TODO: implement
367  break;
368  default:
369  break;
370  }
371  return tool;
372 }
373 
375 {
376  mInternalStructure->mCalibration = calibration;
377  igstk::Transform transform = toIgstkTransform(mInternalStructure->mCalibration);
378  mTool->SetCalibrationTransform(transform);
379 }
380 
381 void IgstkTool::internalAttachedToTracker(bool value)
382 {
383  if (mAttachedToTracker == value)
384  return;
385  mAttachedToTracker = value;
386  report(
387  mInternalStructure->mUid + " is " + (value ? "at" : "de") + "tached " + (value ? "to" : "from")
388  + " the tracker.");
389  emit attachedToTracker(mAttachedToTracker);
390 }
391 
392 void IgstkTool::internalTracked(bool value)
393 {
394  if (mTracked == value)
395  return;
396  mTracked = value;
397 
398  if (!mTracked && mVisible)
399  this->internalVisible(false); //Make sure tool is invisible when not tracked
400 
401  emit tracked(mTracked);
402 }
403 
404 void IgstkTool::internalVisible(bool value)
405 {
406  if (mVisible == value)
407  return;
408  mVisible = value;
409  emit toolVisible(mVisible);
410 }
411 
412 void IgstkTool::addLogging()
413 {
414  bool logging = settings()->value("IGSTKDebugLogging", true).toBool();
415  if (logging)
416  {
417  std::ofstream* loggerFile = new std::ofstream();
418  QString logFile = mInternalStructure->mLoggingFolderName + "Tool_" + mInternalStructure->mName + "_Logging.txt";
419  loggerFile->open(cstring_cast(logFile));
420  mLogger = igstk::Logger::New();
421  mLogOutput = itk::StdStreamLogOutput::New();
422  mLogOutput->SetStream(*loggerFile);
423  mLogger->AddLogOutput(mLogOutput);
424  mLogger->SetPriorityLevel(itk::Logger::DEBUG);
425 
426  mTool->SetLogger(mLogger);
427  }
428 }
429 
431 {
432  std::cout << "------------------------------------------------------------------" << std::endl;
433  std::cout << "mIsProbe: " << mInternalStructure->mIsProbe << std::endl;
434  std::cout << "mIsReference: " << mInternalStructure->mIsReference << std::endl;
435  std::cout << "mIsPointer: " << mInternalStructure->mIsPointer << std::endl;
436  std::cout << "mName: " << mInternalStructure->mName << std::endl;
437  std::cout << "mUid: " << mInternalStructure->mUid << std::endl;
438  std::cout << "mTrackerType: " << mInternalStructure->mTrackerType << std::endl;
439  std::cout << "mSROMFilename: " << mInternalStructure->mSROMFilename << std::endl;
440  std::cout << "mPortNumber: " << mInternalStructure->mPortNumber << std::endl;
441  std::cout << "mChannelNumber: " << mInternalStructure->mChannelNumber << std::endl;
442  std::cout << "mReferencePoints: " << string_cast(mInternalStructure->mReferencePoints.size()) << std::endl;
443  std::cout << "mWireless: " << mInternalStructure->mWireless << std::endl;
444  std::cout << "m5DOF: " << mInternalStructure->m5DOF << std::endl;
445  std::cout << "mCalibration: " << mInternalStructure->mCalibration << std::endl;
446  //mInternalStructure.mCalibration.Print(std::cout, itk::Indent());"
447  std::cout << "mCalibrationFilename: " << mInternalStructure->mCalibrationFilename << std::endl;
448  std::cout << "mGraphicsFileName: " << mInternalStructure->mGraphicsFileName << std::endl;
449  std::cout << "mTransformSaveFileName: " << mInternalStructure->mTransformSaveFileName << std::endl;
450  std::cout << "mLoggingFolderName: " << mInternalStructure->mLoggingFolderName << std::endl;
451  std::cout << "------------------------------------------------------------------" << std::endl;
452 }
453 
454 } //namespace cx
QString qstring_cast(const T &val)
vtkSmartPointer< class vtkMatrix4x4 > vtkMatrix4x4Ptr
Definition: cxMathBase.h:37
TRACKING_SYSTEM getTrackerType()
DoubleBoundingBox3D transform(const Transform3D &m, const DoubleBoundingBox3D &bb)
boost::shared_ptr< ToolInternalStructure > ToolInternalStructurePtr
void reportError(QString msg)
Definition: cxLogger.cpp:71
QString getUid()
Definition: cxIgstkTool.cpp:95
tsNONE
Not specified.
tsPOLARIS
NDIs Polaris tracker.
void attachedToTracker(bool)
static Transform3D toTransform3D(igstk::Transform transform)
Definition: cxIgstkTool.cpp:34
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
boost::shared_ptr< class IgstkTool > IgstkToolPtr
bool isVisible() const
Thread safe, volatile.
virtual ~IgstkTool()
Definition: cxIgstkTool.cpp:86
ToolFileParser::ToolInternalStructurePtr getInternalStructure()
Definition: cxIgstkTool.cpp:90
cstring_cast_Placeholder cstring_cast(const T &val)
QVariant value(const QString &key, const QVariant &defaultValue=QVariant()) const
Definition: cxSettings.cpp:66
void toolVisible(bool)
std::string string_cast(const T &val)
void toolTransformAndTimestamp(Transform3D matrix, double timestamp, ToolPositionMetadata metadata)
tsPOLARIS_SPECTRA
NDIs Polaris Spectra tracker.
void printInternalStructure()
void setCalibrationTransform(Transform3D calibration)
void setReference(IgstkToolPtr)
void reportWarning(QString msg)
Definition: cxLogger.cpp:70
static igstk::Transform toIgstkTransform(Transform3D transform)
Definition: cxIgstkTool.cpp:27
bool isValid() const
Thread safe, volatile.
void sampleInfo2xml(const igstk::NDITracker::TrackingSampleInfo &info, QDomElement &node)
igstk::TrackerTool::Pointer getPointer() const
return a pointer to the internal tools base object
Class for controlling the igstk tracking (hardware) interface.
Definition: cxIgstkTool.h:52
Settings * settings()
Shortcut for accessing the settings instance.
Definition: cxSettings.cpp:21
bool isInitialized() const
Thread safe, volatile.
void updateCalibration(const Transform3D &sMt)
Definition: cxIgstkTool.cpp:43
tsAURORA
NDIs Aurora tracker.
void report(QString msg)
Definition: cxLogger.cpp:69
void tracked(bool)
void setTracker(TrackerPtr tracker)
bool similar(const CameraInfo &lhs, const CameraInfo &rhs, double tol)
IgstkTool(ToolFileParser::ToolInternalStructurePtr internalStructure)
Definition: cxIgstkTool.cpp:60
tsMICRON
Claron Technologys Micron tracker.
bool isTracked() const
Thread safe, volatile.
boost::shared_ptr< IgstkTracker > TrackerPtr
Namespace for all CustusX production code.
tsPOLARIS_VICRA
NDIs Polaris Vicra tracker.