Fraxinus  17.12-rc4
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) 2008-2014, SINTEF Department of Medical Technology
5 All rights reserved.
6 
7 Redistribution and use in source and binary forms, with or without
8 modification, are permitted provided that the following conditions are met:
9 
10 1. Redistributions of source code must retain the above copyright notice,
11  this list of conditions and the following disclaimer.
12 
13 2. Redistributions in binary form must reproduce the above copyright notice,
14  this list of conditions and the following disclaimer in the documentation
15  and/or other materials provided with the distribution.
16 
17 3. Neither the name of the copyright holder nor the names of its contributors
18  may be used to endorse or promote products derived from this software
19  without specific prior written permission.
20 
21 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
22 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
23 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
24 DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE
25 FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
26 DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
27 SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
28 CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29 OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
30 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 =========================================================================*/
32 
33 #include "cxIgstkTool.h"
34 
35 #include <QDateTime>
36 #include <QStringList>
37 #include <QTextStream>
38 #include <vtkTransform.h>
39 #include "cxLogger.h"
40 #include "cxTypeConversions.h"
41 #include "cxSettings.h"
42 #include "cxTransformFile.h"
43 #include "cxIgstkToolManager.h"
44 
45 namespace cx
46 {
47 
49 {
50  igstk::Transform retval;
51  retval.ImportTransform(*transform.getVtkMatrix());
52  return retval;
53 }
54 
56 {
57  Transform3D retval;
58  vtkMatrix4x4Ptr matrix = vtkMatrix4x4Ptr::New();
59  transform.ExportTransform(*(matrix.GetPointer()));
60  retval = Transform3D::fromVtkMatrix(matrix);
61  return retval;
62 }
63 
65 {
66  //apply the calibration
67  mInternalStructure.mCalibration = cal;
68  this->setCalibrationTransform(mInternalStructure.mCalibration);
69 
70  Transform3D sMt = mInternalStructure.getCalibrationAsSSC();
71  report("Set " + mInternalStructure.mName + "s calibration to \n" + qstring_cast(sMt));
72 
73  //write to file
74  mInternalStructure.saveCalibrationToFile();
75 }
76 
80 
82  mToolObserver(itk::ReceptorMemberCommand<IgstkTool>::New()), mValid(false), mVisible(false), mAttachedToTracker(
83  false)
84 {
85  qRegisterMetaType<Transform3D>("Transform3D");
86  qRegisterMetaType<ToolPositionMetadata>("ToolPositionMetadata");
87 
88  mLatestEmittedTimestamp = 0;
89 
90  mInternalStructure = internalStructure;
91 
92  mToolObserver->SetCallbackFunction(this, &IgstkTool::toolTransformCallback);
93 
94  if (mInternalStructure.verify())
95  {
96  mTool = this->buildInternalTool();
97  this->addLogging();
98  mValid = true;
99  }
100  else
101  {
102  reportError(mInternalStructure.mUid + " was created with invalid internal structure.");
103  mValid = false;
104  }
105 }
106 
108 {
109 }
110 
112 {
113  return mInternalStructure;
114 }
115 
117 {
118  return mInternalStructure.mUid;
119 }
120 
121 igstk::TrackerTool::Pointer IgstkTool::getPointer() const
122 {
123  return mTool;
124 }
125 
126 TRACKING_SYSTEM IgstkTool::getTrackerType()
127 {
128  return mInternalStructure.mTrackerType;
129 }
130 
131 bool IgstkTool::isValid() const
132 {
133  return mValid;
134 }
135 
137 {
138  return mAttachedToTracker;
139 }
140 
142 {
143  return mTracked;
144 }
145 
147 {
148  return mVisible;
149 }
150 
152 {
153  mReferenceTool = refTool;
154 }
155 
157 {
158  mTracker = tracker;
159 }
160 
161 igstk::NDITracker::TrackingSampleInfo IgstkTool::getSampleInfo()
162 {
163  igstk::NDITracker::TrackingSampleInfo retval;
164  retval.m_FrameNumber = 0;
165 
166  TrackerPtr tracker = mTracker.lock();
167  if (!tracker)
168  return retval;
169  igstk::NDITracker* ndiTracker = dynamic_cast<igstk::NDITracker*>(tracker->getPointer());
170  if (!ndiTracker)
171  return retval;
172 
173  // hacking into the IGSTK internals...
174  std::map<std::string,igstk::NDITracker::TrackingSampleInfo> info = ndiTracker->GetTrackingSampleInfo();
175  return info[mTool->GetTrackerToolIdentifier()];
176 }
177 
178 bool IgstkTool::validReferenceForResult(igstk::CoordinateSystemTransformToResult result)
179 {
180  const igstk::CoordinateSystem* destination = result.GetDestination();
181  IgstkToolPtr refTool = mReferenceTool.lock();
182 
183  if (refTool) //if we are tracking with a reftool it must be visible
184  {
185  if (!refTool->getPointer()->IsCoordinateSystem(destination))
186  return false;
187  }
188  else //if we dont have a reftool we use the tracker as patientref
189  {
190  TrackerPtr tracker = mTracker.lock();
191  if (!tracker || !tracker->getPointer()->IsCoordinateSystem(destination))
192  return false;
193  }
194 
195  return true;
196 }
197 
198 Transform3D IgstkTool::igstk2Transform3D(const igstk::Transform& input) const
199 {
200  vtkMatrix4x4Ptr vtkMatrix = vtkMatrix4x4Ptr::New();
201  input.ExportTransform(*vtkMatrix.GetPointer());
202  return Transform3D(vtkMatrix.GetPointer());
203 }
204 
205 void IgstkTool::processReceivedTransformResult(igstk::CoordinateSystemTransformToResult result)
206 {
207  if (!this->validReferenceForResult(result))
208  return;
209 
210  // emit even if not visible: need error metadata
211 
212  igstk::NDITracker::TrackingSampleInfo sampleInfo = this->getSampleInfo();
213 
214  // ignore duplicate positions
215  if (similar(mLatestEmittedTimestamp, sampleInfo.m_TimeStamp,1.0E-3))
216  {
217  return;
218  }
219  mLatestEmittedTimestamp = sampleInfo.m_TimeStamp;
220 
221  QDomDocument doc;
222  QDomElement root = doc.createElement("info");
223  doc.appendChild(root);
224  sampleInfo2xml(sampleInfo, root);
225  ToolPositionMetadata metadata;
226  metadata.mData = doc.toString();
227 
228  igstk::Transform transform = result.GetTransform();
229  Transform3D prMt = igstk2Transform3D(transform);
230  double timestamp = transform.GetStartTime();
231 
232  emit toolTransformAndTimestamp(prMt, timestamp, metadata);
233 }
234 
235 void IgstkTool::toolTransformCallback(const itk::EventObject &event)
236 {
237  if (igstk::CoordinateSystemTransformToEvent().CheckEvent(&event))
238  {
239  const igstk::CoordinateSystemTransformToEvent *transformEvent;
240  transformEvent = dynamic_cast<const igstk::CoordinateSystemTransformToEvent*>(&event);
241  if (!transformEvent)
242  return;
243  this->processReceivedTransformResult(transformEvent->Get());
244  }
245  //Successes
246  else if (igstk::TrackerToolConfigurationEvent().CheckEvent(&event))
247  {
248  report(QString("Configured [%1] with the tracking system").arg(mInternalStructure.mUid));
249  }
250  else if (igstk::TrackerToolAttachmentToTrackerEvent().CheckEvent(&event))
251  {
252  this->internalAttachedToTracker(true);
253  }
254  else if (igstk::TrackerToolDetachmentFromTrackerEvent().CheckEvent(&event))
255  {
256  this->internalAttachedToTracker(false);
257  }
258  else if (igstk::TrackerToolMadeTransitionToTrackedStateEvent().CheckEvent(&event))
259  {
260  this->internalVisible(true);
261  }
262  else if (igstk::TrackerToolNotAvailableToBeTrackedEvent().CheckEvent(&event))
263  {
264  this->internalVisible(false);
265  }
266  else if (igstk::ToolTrackingStartedEvent().CheckEvent(&event))
267  {
268  this->internalTracked(true);
269  report(mInternalStructure.mUid + " is tracked.");
270  }
271  else if (igstk::ToolTrackingStoppedEvent().CheckEvent(&event))
272  {
273  this->internalTracked(false);
274  report(mInternalStructure.mUid + " is not tracked anymore.");
275  }
276  //Failures
277  else if (igstk::InvalidRequestErrorEvent().CheckEvent(&event))
278  {
280  mInternalStructure.mUid
281  + " received an invalid request. This means that the internal igstk trackertool did not accept the request. Do not know which request.");
282  }
283  else if (igstk::TrackerToolConfigurationErrorEvent().CheckEvent(&event))
284  {
285  reportError(mInternalStructure.mUid + " could not configure with the tracking system.");
286  }
287  else if (igstk::InvalidRequestToAttachTrackerToolErrorEvent().CheckEvent(&event))
288  {
289  reportError(mInternalStructure.mUid + " could not request to attach to tracker.");
290  }
291  else if (igstk::InvalidRequestToDetachTrackerToolErrorEvent().CheckEvent(&event))
292  {
293  reportError(mInternalStructure.mUid + " could not request to detach from tracker.");
294  }
295  else if (igstk::TrackerToolAttachmentToTrackerErrorEvent().CheckEvent(&event))
296  {
297  reportError(mInternalStructure.mUid + " could not attach to tracker.");
298  }
299  else if (igstk::TrackerToolDetachmentFromTrackerErrorEvent().CheckEvent(&event))
300  {
301  reportError(mInternalStructure.mUid + " could not detach from tracker.");
302  }
303  //Polaris specific failures
304  else if (igstk::InvalidPolarisPortNumberErrorEvent().CheckEvent(&event))
305  {
306  reportError(
307  mInternalStructure.mUid + " sendt invalid Polaris port number: "
308  + qstring_cast(mInternalStructure.mPortNumber) + ".");
309  }
310  else if (igstk::InvalidPolarisSROMFilenameErrorEvent().CheckEvent(&event))
311  {
312  reportError(
313  mInternalStructure.mUid + " sendt invalid ROM file: " + mInternalStructure.mSROMFilename);
314  }
315  else if (igstk::InvalidPolarisPartNumberErrorEvent().CheckEvent(&event))
316  {
317  reportError(mInternalStructure.mUid + " has an invalid part number.");
318  }
319  //Aurora specific failures
320  else if (igstk::InvalidAuroraPortNumberErrorEvent().CheckEvent(&event))
321  {
322  reportError(
323  mInternalStructure.mUid + " has an invalid port number: "
324  + qstring_cast(mInternalStructure.mPortNumber) + ".");
325  }
326  else if (igstk::InvalidAuroraSROMFilenameErrorEvent().CheckEvent(&event))
327  {
328  reportError(
329  mInternalStructure.mUid + " sendt invalid ROM file: " + mInternalStructure.mSROMFilename);
330  }
331  else if (igstk::InvalidAuroraPartNumberErrorEvent().CheckEvent(&event))
332  {
333  reportError(mInternalStructure.mUid + " has an invalid part number.");
334  }
335  else if (igstk::InvalidAuroraChannelNumberErrorEvent().CheckEvent(&event))
336  {
337  reportError(
338  mInternalStructure.mUid + " has an invalid channel number:"
339  + qstring_cast(mInternalStructure.mChannelNumber) + ".");
340  }
341 }
342 
343 igstk::TrackerTool::Pointer IgstkTool::buildInternalTool()
344 {
345  igstk::TrackerTool::Pointer tool;
346 
347  igstk::PolarisTrackerTool::Pointer tempPolarisTool;
348  igstk::AuroraTrackerTool::Pointer tempAuroraTool;
349 
350  igstk::Transform calibration = toIgstkTransform(mInternalStructure.mCalibration);
351  switch (mInternalStructure.mTrackerType)
352  {
353  case tsNONE:
354  break;
355  case tsPOLARIS_SPECTRA:
356  case tsPOLARIS_VICRA:
357  case tsPOLARIS:
358  tempPolarisTool = igstk::PolarisTrackerTool::New();
359  tempPolarisTool->AddObserver(igstk::IGSTKEvent(), mToolObserver);
360  if (!mInternalStructure.mWireless) //we only support wireless atm
361  return tool = tempPolarisTool.GetPointer();
362  tempPolarisTool->RequestSelectWirelessTrackerTool();
363  tempPolarisTool->RequestSetSROMFileName(string_cast(mInternalStructure.mSROMFilename));
364  tempPolarisTool->RequestConfigure();
365  tempPolarisTool->SetCalibrationTransform(calibration);
366  tool = tempPolarisTool;
367  break;
368  case tsAURORA:
369  tempAuroraTool = igstk::AuroraTrackerTool::New();
370  tempAuroraTool->AddObserver(igstk::IGSTKEvent(), mToolObserver);
371  if (mInternalStructure.m5DOF)
372  {
373  tempAuroraTool->RequestSelect5DOFTrackerTool();
374  tempAuroraTool->RequestSetPortNumber(mInternalStructure.mPortNumber);
375  tempAuroraTool->RequestSetChannelNumber(mInternalStructure.mChannelNumber);
376  }
377  else
378  {
379  tempAuroraTool->RequestSelect6DOFTrackerTool();
380  tempAuroraTool->RequestSetPortNumber(mInternalStructure.mPortNumber);
381  }
382  tempAuroraTool->RequestConfigure();
383  tempAuroraTool->SetCalibrationTransform(calibration);
384  tool = tempAuroraTool;
385  break;
386  case tsMICRON:
387  //TODO: implement
388  break;
389  default:
390  break;
391  }
392  return tool;
393 }
394 
396 {
397  mInternalStructure.mCalibration = calibration;
398  igstk::Transform transform = toIgstkTransform(mInternalStructure.mCalibration);
399  mTool->SetCalibrationTransform(transform);
400 }
401 
402 void IgstkTool::internalAttachedToTracker(bool value)
403 {
404  if (mAttachedToTracker == value)
405  return;
406  mAttachedToTracker = value;
407  report(
408  mInternalStructure.mUid + " is " + (value ? "at" : "de") + "tached " + (value ? "to" : "from")
409  + " the tracker.");
410  emit attachedToTracker(mAttachedToTracker);
411 }
412 
413 void IgstkTool::internalTracked(bool value)
414 {
415  if (mTracked == value)
416  return;
417  mTracked = value;
418 
419  if (!mTracked && mVisible)
420  this->internalVisible(false); //Make sure tool is invisible when not tracked
421 
422  emit tracked(mTracked);
423 }
424 
425 void IgstkTool::internalVisible(bool value)
426 {
427  if (mVisible == value)
428  return;
429  mVisible = value;
430  emit toolVisible(mVisible);
431 }
432 
433 void IgstkTool::addLogging()
434 {
435  bool logging = settings()->value("IGSTKDebugLogging", true).toBool();
436  if (logging)
437  {
438  std::ofstream* loggerFile = new std::ofstream();
439  QString logFile = mInternalStructure.mLoggingFolderName + "Tool_" + mInternalStructure.mName + "_Logging.txt";
440  loggerFile->open(cstring_cast(logFile));
441  mLogger = igstk::Logger::New();
442  mLogOutput = itk::StdStreamLogOutput::New();
443  mLogOutput->SetStream(*loggerFile);
444  mLogger->AddLogOutput(mLogOutput);
445  mLogger->SetPriorityLevel(itk::Logger::DEBUG);
446 
447  mTool->SetLogger(mLogger);
448  }
449 }
450 
452 {
453  std::cout << "------------------------------------------------------------------" << std::endl;
454  std::cout << "mIsProbe: " << mInternalStructure.mIsProbe << std::endl;
455  std::cout << "mIsReference: " << mInternalStructure.mIsReference << std::endl;
456  std::cout << "mIsPointer: " << mInternalStructure.mIsPointer << std::endl;
457  std::cout << "mName: " << mInternalStructure.mName << std::endl;
458  std::cout << "mUid: " << mInternalStructure.mUid << std::endl;
459  std::cout << "mTrackerType: " << mInternalStructure.mTrackerType << std::endl;
460  std::cout << "mSROMFilename: " << mInternalStructure.mSROMFilename << std::endl;
461  std::cout << "mPortNumber: " << mInternalStructure.mPortNumber << std::endl;
462  std::cout << "mChannelNumber: " << mInternalStructure.mChannelNumber << std::endl;
463  std::cout << "mReferencePoints: " << string_cast(mInternalStructure.mReferencePoints.size()) << std::endl;
464  std::cout << "mWireless: " << mInternalStructure.mWireless << std::endl;
465  std::cout << "m5DOF: " << mInternalStructure.m5DOF << std::endl;
466  std::cout << "mCalibration: " << mInternalStructure.mCalibration << std::endl;
467  //mInternalStructure.mCalibration.Print(std::cout, itk::Indent());"
468  std::cout << "mCalibrationFilename: " << mInternalStructure.mCalibrationFilename << std::endl;
469  std::cout << "mGraphicsFileName: " << mInternalStructure.mGraphicsFileName << std::endl;
470  std::cout << "mTransformSaveFileName: " << mInternalStructure.mTransformSaveFileName << std::endl;
471  std::cout << "mLoggingFolderName: " << mInternalStructure.mLoggingFolderName << std::endl;
472  std::cout << "------------------------------------------------------------------" << std::endl;
473 }
474 
475 } //namespace cx
QString qstring_cast(const T &val)
vtkSmartPointer< class vtkMatrix4x4 > vtkMatrix4x4Ptr
Definition: cxMathBase.h:58
TRACKING_SYSTEM getTrackerType()
DoubleBoundingBox3D transform(const Transform3D &m, const DoubleBoundingBox3D &bb)
void reportError(QString msg)
Definition: cxLogger.cpp:92
QString getUid()
tsNONE
Not specified.
tsPOLARIS
NDIs Polaris tracker.
void attachedToTracker(bool)
bool m5DOF
whether or not the tool have 5 DOF
static Transform3D toTransform3D(igstk::Transform transform)
Definition: cxIgstkTool.cpp:55
Transform3D Transform3D
Transform3D is a representation of an affine 3D transform.
boost::shared_ptr< class IgstkTool > IgstkToolPtr
QString mTransformSaveFileName
path to where transforms should be saved
bool isVisible() const
Thread safe, volatile.
virtual ~IgstkTool()
cstring_cast_Placeholder cstring_cast(const T &val)
QVariant value(const QString &key, const QVariant &defaultValue=QVariant()) const
Definition: cxSettings.cpp:87
QString mSROMFilename
path to the tools SROM file
ToolFileParser::ToolInternalStructure getInternalStructure()
void toolVisible(bool)
unsigned int mPortNumber
the port number the tool is connected to
std::string string_cast(const T &val)
void toolTransformAndTimestamp(Transform3D matrix, double timestamp, ToolPositionMetadata metadata)
IgstkTool(ToolFileParser::ToolInternalStructure internalStructure)
Definition: cxIgstkTool.cpp:81
tsPOLARIS_SPECTRA
NDIs Polaris Spectra tracker.
void printInternalStructure()
void setCalibrationTransform(Transform3D calibration)
unsigned int mChannelNumber
the channel the tool is connected to
void setReference(IgstkToolPtr)
void reportWarning(QString msg)
Definition: cxLogger.cpp:91
static igstk::Transform toIgstkTransform(Transform3D transform)
Definition: cxIgstkTool.cpp:48
bool isValid() const
Thread safe, volatile.
bool mWireless
whether or not the tool is wireless
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:73
TRACKING_SYSTEM mTrackerType
what product the tool belongs to
Settings * settings()
Shortcut for accessing the settings instance.
Definition: cxSettings.cpp:42
QString mCalibrationFilename
path to the tools calibration file
bool isInitialized() const
Thread safe, volatile.
QString mGraphicsFileName
path to this tools graphics file
void updateCalibration(const Transform3D &sMt)
Definition: cxIgstkTool.cpp:64
tsAURORA
NDIs Aurora tracker.
void report(QString msg)
Definition: cxLogger.cpp:90
void tracked(bool)
QString mLoggingFolderName
path to where log should be saved
void setTracker(TrackerPtr tracker)
bool similar(const CameraInfo &lhs, const CameraInfo &rhs, double tol)
Transform3D mCalibration
transform read from mCalibrationFilename
std::map< int, Vector3D > mReferencePoints
optional point on the frame, specifying a known reference point, 0,0,0 is default, in sensor space
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.