PlusLib  2.9.0
Software library for tracked ultrasound image acquisition, calibration, and processing.
vtkPlusAtracsysTracker.cxx
Go to the documentation of this file.
1 /*=Plus=header=begin======================================================
2 Program: Plus
3 Copyright (c) Laboratory for Percutaneous Surgery. All rights reserved.
4 See License.txt for details.
5 =========================================================Plus=header=end*/
6 
7 #include "PlusConfigure.h"
8 
9 // Local includes
10 #include "AtracsysTracker.h"
11 #include "vtkIGSIOAccurateTimer.h"
12 #include "vtkPlusAtracsysTracker.h"
13 #include "vtkPlusDataSource.h"
14 
15 // VTK includes
16 #include <vtkMath.h>
17 #include <vtkMatrix4x4.h>
18 #include <vtkNew.h>
19 #include <vtkSmartPointer.h>
20 
21 // System includes
22 #include <fstream>
23 #include <iostream>
24 #include <map>
25 #include <string>
26 
27 // Atracsys includes
28 #include "ftkErrors.h"
29 #include "ftkEvent.h"
30 #include "ftkInterface.h"
31 #include "ftkOptions.h"
32 #include "ftkPlatform.h"
33 #include "ftkTypes.h"
34 
35 // for convenience
36 #define ATR_SUCCESS AtracsysTracker::ATRACSYS_RESULT::SUCCESS
38 
40 
41 //----------------------------------------------------------------------------
42 // Define command strings
50 
51 
52 //----------------------------------------------------------------------------
53 class vtkPlusAtracsysTracker::vtkInternal
54 {
55 public:
56  vtkPlusAtracsysTracker* External;
57 
58  vtkInternal(vtkPlusAtracsysTracker* external)
59  : External(external)
60  {
61  // stores correspondences between config file option names and atracsys option names
62  this->DeviceOptionTranslator["MaxMeanRegistrationErrorMm"] = "Registration Mean Error";
63  this->DeviceOptionTranslator["MaxMissingFiducials"] = "Matching Maximum Missing Points";
64  this->DeviceOptionTranslator["SymmetriseCoordinates"] = "Symmetrise coordinates";
65  this->DeviceOptionTranslator["EnableLasers"] = "Enables lasers";
66  this->DeviceOptionTranslator["EnableUserLED"] = "Enables the user-LED";
67  this->DeviceOptionTranslator["EnableIRstrobe"] = "Enables IR strobe";
68  this->DeviceOptionTranslator["ImageCompressionThreshold"] = "Image Compression Threshold";
69  this->DeviceOptionTranslator["ImageIntegrationTime"] = "Image Integration Time";
70  }
71 
72  virtual ~vtkInternal()
73  {
74  }
75 
76  // mapping option names as written in config file to Atracsys option names
77  std::map<std::string, std::string> DeviceOptionTranslator;
78  // the actual options as read in the config file
79  std::map<std::string, std::string> DeviceOptions{};
80 
81  // parameters used internally and their default values
82  int ActiveMarkerPairingTimeSec = 0; // pairing time in seconds
83  float MaxMeanRegistrationErrorMm = 2.0; // maximum mean registration error
84  int MaxMissingFiducials = 0; // maximum number of missing fiducials
85 
86  // matches plus tool id to .ini geometry file names/paths
87  std::map<std::string, std::string> PlusIdMappedToGeometryFilename;
88 
89  // matches fusionTrack internal tool geometry ID to Plus tool ID for updating tools
90  std::map<int, std::string> FtkGeometryIdMappedToToolId;
91 
92  // Atracsys API wrapper class handle
94 
95  // type of tracker connected
97 };
98 
99 //----------------------------------------------------------------------------
101  : vtkPlusDevice()
102  , Internal(new vtkInternal(this))
103 {
104  LOG_TRACE("vtkPlusAtracsysTracker::vtkPlusAtracsysTracker()");
105  this->FrameNumber = 0;
106  this->StartThreadForInternalUpdates = true;
107  this->InternalUpdateRate = 300;
108 }
109 
110 //----------------------------------------------------------------------------
112 {
113  LOG_TRACE("vtkPlusAtracsysTracker::~vtkPlusAtracsysTracker()");
114 
115  delete Internal;
116  Internal = nullptr;
117 }
118 
119 //----------------------------------------------------------------------------
120 void vtkPlusAtracsysTracker::PrintSelf(ostream& os, vtkIndent indent)
121 {
122  Superclass::PrintSelf(os, indent);
123 }
124 
125 //----------------------------------------------------------------------------
127 {
128  std::string v;
129  this->Internal->Tracker.GetSDKversion(v);
130  return v;
131 }
132 
133 //----------------------------------------------------------------------------
135 {
136  std::string d;
137  this->Internal->Tracker.GetCalibrationDate(d);
138  return d;
139 }
140 
141 //----------------------------------------------------------------------------
143 {
144  switch (this->Internal->DeviceType)
145  {
146  case AtracsysTracker::DEVICE_TYPE::FUSIONTRACK_250:
147  return "fusionTrack250";
148  case AtracsysTracker::DEVICE_TYPE::FUSIONTRACK_500:
149  return "fusionTrack500";
150  case AtracsysTracker::DEVICE_TYPE::SPRYTRACK_180:
151  return "spryTrack180";
152  case AtracsysTracker::DEVICE_TYPE::SPRYTRACK_300:
153  return "spryTrack300";
154  default:
155  return "unknown";
156  }
157 }
158 
159 //----------------------------------------------------------------------------
161  std::array<float, 10>& leftIntrinsic, std::array<float, 10>& rightIntrinsic,
162  std::array<float, 3>& rightPosition, std::array<float, 3>& rightOrientation)
163 {
164  ATRACSYS_RESULT result = this->Internal->Tracker.GetCamerasCalibration(
165  leftIntrinsic, rightIntrinsic, rightPosition, rightOrientation);
166  if (result != ATR_SUCCESS)
167  {
168  LOG_ERROR(this->Internal->Tracker.ResultToString(result));
169  return PLUS_FAIL;
170  }
171  return PLUS_SUCCESS;
172 }
173 
174 //----------------------------------------------------------------------------
175 PlusStatus vtkPlusAtracsysTracker::GetLoadedGeometries(std::map<int, std::vector<std::array<float, 3>>>& geometries)
176 {
177  if (this->Internal->Tracker.GetLoadedGeometries(geometries) != ATR_SUCCESS)
178  {
179  LOG_ERROR("Could not get loaded geometries");
180  return PLUS_FAIL;
181  }
182  if (geometries.size() == 0)
183  {
184  LOG_ERROR("No loaded geometries");
185  return PLUS_FAIL;
186  }
187  return PLUS_SUCCESS;
188 }
189 
190 //----------------------------------------------------------------------------
192 {
193  return this->Internal->Tracker.IsVirtual();
194 }
195 
196 //----------------------------------------------------------------------------
197 PlusStatus vtkPlusAtracsysTracker::ReadConfiguration(vtkXMLDataElement* rootConfigElement)
198 {
199  // Read and store all device options read in the xml config file
200  XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_READING(deviceConfig, rootConfigElement);
201 
202  for (int i = 0; i < deviceConfig->GetNumberOfAttributes(); ++i)
203  {
204  this->Internal->DeviceOptions.emplace(deviceConfig->GetAttributeName(i), deviceConfig->GetAttributeValue(i));
205  // also store parameters with vtkPlusDevice method
206  this->SetParameter(deviceConfig->GetAttributeName(i), deviceConfig->GetAttributeValue(i));
207  }
208 
209  XML_FIND_NESTED_ELEMENT_REQUIRED(dataSourcesElement, deviceConfig, "DataSources");
210  for (int nestedElementIndex = 0; nestedElementIndex < dataSourcesElement->GetNumberOfNestedElements(); nestedElementIndex++)
211  {
212  vtkXMLDataElement* toolDataElement = dataSourcesElement->GetNestedElement(nestedElementIndex);
213  if (STRCASECMP(toolDataElement->GetName(), "DataSource") != 0)
214  {
215  // if this is not a data source element, skip it
216  continue;
217  }
218  if (toolDataElement->GetAttribute("Type") != NULL && STRCASECMP(toolDataElement->GetAttribute("Type"), "Tool") != 0)
219  {
220  // if this is not a Tool element, skip it
221  continue;
222  }
223  std::string toolId(toolDataElement->GetAttribute("Id"));
224  if (toolId.empty())
225  {
226  // tool doesn't have ID needed to generate transform
227  LOG_ERROR("Failed to initialize Atracsys tool: DataSource Id is missing.");
228  continue;
229  }
230 
231  enum TRACKING_TYPE
232  {
233  ACTIVE,
234  PASSIVE
235  };
236 
237  TRACKING_TYPE toolTrackingType;
238  XML_READ_ENUM2_ATTRIBUTE_NONMEMBER_OPTIONAL(TrackingType, toolTrackingType, toolDataElement, "ACTIVE", ACTIVE, "PASSIVE", PASSIVE);
239  if (toolTrackingType == ACTIVE)
240  {
241  // active tool, can be loaded directly into FtkGeometryIdMappedToToolId
242  int ftkGeometryId = -1;
243  XML_READ_SCALAR_ATTRIBUTE_NONMEMBER_OPTIONAL(int, GeometryId, ftkGeometryId, toolDataElement);
244  if (ftkGeometryId != -1)
245  {
246  std::pair<int, std::string> thisTool(ftkGeometryId, toolId);
247  this->Internal->FtkGeometryIdMappedToToolId.insert(thisTool);
248  }
249  else
250  {
251  LOG_ERROR("Active tool with Id " << toolId << " is missing a GeometryId");
252  return PLUS_FAIL;
253  }
254  }
255  else if (toolTrackingType == PASSIVE)
256  {
257  // passive tool, load into PlusIdMappedToGeometryFilename to have geometry loaded in InternalConnect
258  const char* geometryFile;
259  if ((geometryFile = toolDataElement->GetAttribute("GeometryFile")) != NULL)
260  {
261  std::pair<std::string, std::string> thisTool(toolId, geometryFile);
262  this->Internal->PlusIdMappedToGeometryFilename.insert(thisTool);
263  }
264  else
265  {
266  LOG_ERROR("Passive tool with Id " << toolId << " is missing GeometryFile (.ini).");
267  return PLUS_FAIL;
268  }
269  }
270  }
271 
272  return PLUS_SUCCESS;
273 }
274 
275 //----------------------------------------------------------------------------
276 PlusStatus vtkPlusAtracsysTracker::WriteConfiguration(vtkXMLDataElement* rootConfigElement)
277 {
278  LOG_TRACE("vtkPlusAtracsysTracker::WriteConfiguration");
279  XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_WRITING(deviceConfig, rootConfigElement);
280  return PLUS_SUCCESS;
281 }
282 
283 //----------------------------------------------------------------------------
285 {
286  LOG_TRACE("vtkPlusAtracsysTracker::Probe");
287  return PLUS_SUCCESS;
288 }
289 
290 //----------------------------------------------------------------------------
291 const std::map<std::string, std::string>& vtkPlusAtracsysTracker::GetDeviceOptions() const
292 {
293  return this->Internal->DeviceOptions;
294 }
295 
296 //----------------------------------------------------------------------------
297 PlusStatus vtkPlusAtracsysTracker::GetOptionValue(const std::string& optionName, std::string& optionValue)
298 {
299  std::map<std::string, std::string>::const_iterator itd = this->Internal->DeviceOptions.find(optionName);
300  if (itd != this->Internal->DeviceOptions.cend())
301  {
302  optionValue = this->Internal->DeviceOptions[optionName];
303  return PLUS_SUCCESS;
304  }
305  return PLUS_FAIL;
306 }
307 
308 //----------------------------------------------------------------------------
309 bool vtkPlusAtracsysTracker::TranslateOptionName(const std::string& optionName, std::string& translatedOptionName)
310 {
311  std::map<std::string, std::string>::const_iterator itt =
312  this->Internal->DeviceOptionTranslator.find(optionName);
313  if (itt == this->Internal->DeviceOptionTranslator.cend())
314  {
315  return false;
316  }
317  else
318  {
319  translatedOptionName = itt->second;
320  return true;
321  }
322 }
323 
324 //----------------------------------------------------------------------------
326 {
327  LOG_TRACE("vtkPlusAtracsysTracker::InternalConnect");
328 
329  // Connect to device
330  AtracsysTracker::ATRACSYS_RESULT result = this->Internal->Tracker.Connect();
331  if (result != ATR_SUCCESS && result != AtracsysTracker::ATRACSYS_RESULT::WARNING_CONNECTED_IN_USB2)
332  {
333  LOG_ERROR(this->Internal->Tracker.ResultToString(result));
334  return PLUS_FAIL;
335  }
336  else if (result == AtracsysTracker::ATRACSYS_RESULT::WARNING_CONNECTED_IN_USB2)
337  {
338  LOG_WARNING(this->Internal->Tracker.ResultToString(result));
339  }
340 
341  // get device serial number in hex form
342  std::ostringstream oss;
343  uint64 sn;
344  this->Internal->Tracker.GetDeviceId(sn);
345  oss << "0x" << std::setw(16) << std::setfill('0') << std::hex << sn;
346  this->DeviceId = oss.str();
347 
348  // get device type
349  this->Internal->Tracker.GetDeviceType(this->Internal->DeviceType);
350 
351  std::map<std::string, std::string>::const_iterator itd;
352  // handling options that are used only internally
353  // ------- time allocated for activer marker pairing
354  itd = this->Internal->DeviceOptions.find("ActiveMarkerPairingTimeSec");
355  if (itd != this->Internal->DeviceOptions.cend())
356  {
357  if (strToInt32(itd->second, this->Internal->ActiveMarkerPairingTimeSec))
358  {
359  LOG_INFO("Marker pairing time is set to " << this->Internal->ActiveMarkerPairingTimeSec << " seconds, tracking will not start until this period is over.");
360  }
361  }
362 
363  // handling options that are used both internally and in the sdk
364  // ------- max registration error in mm
365  itd = this->Internal->DeviceOptions.find("MaxMeanRegistrationErrorMm");
366  if (itd != this->Internal->DeviceOptions.cend())
367  {
368  if (strToFloat32(itd->second, this->Internal->MaxMeanRegistrationErrorMm))
369  {
370  if (this->Internal->MaxMeanRegistrationErrorMm < 0.1 || this->Internal->MaxMeanRegistrationErrorMm > 5.0)
371  {
372  this->Internal->MaxMeanRegistrationErrorMm = 2.0;
373  LOG_WARNING("Invalid maximum mean registration error provided. Must be between 0.1 and 5 mm inclusive. Maximum mean registration error has been set to its default value of " << this->Internal->MaxMeanRegistrationErrorMm << "mm.");
374  }
375  }
376  }
377  else
378  {
379  this->Internal->DeviceOptions.emplace("MaxMeanRegistrationErrorMm",
380  std::to_string(this->Internal->MaxMeanRegistrationErrorMm));
381  }
382  // ------- max number of missing fiducials
383  itd = this->Internal->DeviceOptions.find("MaxMissingFiducials");
384  if (itd != this->Internal->DeviceOptions.cend())
385  {
386  if (strToInt32(itd->second, this->Internal->MaxMissingFiducials))
387  {
388  if (this->Internal->MaxMissingFiducials < 0 || this->Internal->MaxMissingFiducials > 3)
389  {
390  this->Internal->MaxMissingFiducials = 0;
391  LOG_WARNING("Invalid maximum number of missing fiducials provided. Must be between 0 and 3 inclusive. Maximum missing fiducials has been set to its default value of " << this->Internal->MaxMissingFiducials << ".");
392  }
393  }
394  }
395  else
396  {
397  this->Internal->DeviceOptions.emplace("MaxMissingFiducials",
398  std::to_string(this->Internal->MaxMissingFiducials));
399  }
400 
401  // set frame options (internally passed to sdk during connection)
402  // ------- maximum number of events per frame included in the device's output
403  itd = this->Internal->DeviceOptions.find("MaxAdditionalEventsNumber");
404  if (itd != this->Internal->DeviceOptions.cend())
405  {
406  int value = -1;
407  strToInt32(itd->second, value);
408  if (value < 0) {
409  LOG_WARNING("Invalid value for max events number per frame in output: " << itd->second
410  << ". Default value used (" << this->Internal->Tracker.GetMaxAdditionalEventsNumber() << ")");
411  }
412  else
413  {
414  this->Internal->Tracker.SetMaxAdditionalEventsNumber(value);
415  }
416  }
417  // ------- maximum number of 2D fiducials (in either left or right frame) included in the device's output
418  itd = this->Internal->DeviceOptions.find("Max2dFiducialsNumber");
419  if (itd != this->Internal->DeviceOptions.cend())
420  {
421  int value = -1;
422  strToInt32(itd->second, value);
423  if (value < 0)
424  {
425  LOG_WARNING("Invalid value for max 2D fiducials number in output: " << itd->second
426  << ". Default value used (" << this->Internal->Tracker.GetMax2dFiducialsNumber() << ")");
427  }
428  else
429  {
430  this->Internal->Tracker.SetMax2dFiducialsNumber(value);
431  }
432  }
433  // ------- maximum number of 3D fiducials (after triangulation) included in the device's output
434  itd = this->Internal->DeviceOptions.find("Max3dFiducialsNumber");
435  if (itd != this->Internal->DeviceOptions.cend())
436  {
437  int value = -1;
438  strToInt32(itd->second, value);
439  if (value < 0)
440  {
441  LOG_WARNING("Invalid value for max 3D fiducials number in output: " << itd->second
442  << ". Default value used (" << this->Internal->Tracker.GetMax3dFiducialsNumber() << ")");
443  }
444  else
445  {
446  this->Internal->Tracker.SetMax3dFiducialsNumber(value);
447  }
448  }
449  // ------- maximum number of markers included in the device's output
450  itd = this->Internal->DeviceOptions.find("MaxMarkersNumber");
451  if (itd != this->Internal->DeviceOptions.cend())
452  {
453  int value = -1;
454  strToInt32(itd->second, value);
455  if (value < 0)
456  {
457  LOG_WARNING("Invalid value for max markers number in output: " << itd->second
458  << ". Default value used (" << this->Internal->Tracker.GetMaxMarkersNumber() << ")");
459  }
460  else
461  {
462  this->Internal->Tracker.SetMaxMarkersNumber(value);
463  }
464  }
465 
466  // Set actual device options and remove those unsuccessfully set
467  // SpryTrack's Embedded processing option needs to be considered first.
468  // By default, the spryTrack is in Embedded processing mode.
469  // In Embedded processing mode, some other options will require the "Embedded " prefix.
470  std::map<std::string, std::string>::iterator itr = this->Internal->DeviceOptions.find("Enable_embedded_processing");
471  if (itr != this->Internal->DeviceOptions.end())
472  {
473  if (this->Internal->DeviceOptions.at("Enable_embedded_processing") == "0")
474  {
475  if (this->Internal->Tracker.SetSpryTrackProcessingType(AtracsysTracker::PROCESSING_ON_PC) != ATR_SUCCESS)
476  {
477  LOG_WARNING("Embedded processing could not be disabled.");
478  this->Internal->DeviceOptions.erase(itr);
479  }
480  }
481  else if (this->Internal->DeviceOptions.at("Enable_embedded_processing") == "1")
482  {
483  if (this->Internal->Tracker.SetSpryTrackProcessingType(AtracsysTracker::PROCESSING_ONBOARD) != ATR_SUCCESS)
484  {
485  this->Internal->DeviceOptions.erase(itr);
486  LOG_WARNING("Embedded processing could not be enabled.");
487  }
488  }
489  else
490  {
491  LOG_WARNING(this->Internal->DeviceOptions.at("Enable_embedded_processing") << " is not a correct value for Embedded processing.\nAccepted values are 0 (false) and 1 (true).");
492  this->Internal->DeviceOptions.erase(itr);
493  }
494  }
495 
496  itr = this->Internal->DeviceOptions.begin();
497  while (itr != this->Internal->DeviceOptions.end())
498  {
499  std::string translatedOptionName;
500 
501  /* Dirty hack circumventing a nomenclature discrepancy between ftk and stk API's, this may be fixed in a future API release */
502  if (itr->first == "EnableIRstrobe" && (this->Internal->DeviceType == AtracsysTracker::DEVICE_TYPE::FUSIONTRACK_500
503  || this->Internal->DeviceType == AtracsysTracker::DEVICE_TYPE::FUSIONTRACK_250))
504  {
505  // For fusiontracks, strobe on is 0 and strobe off is 1 (the opposite of sprytracks).
506  // Fusiontracks also supports a mode 2 where the strobe turns on every other frame (unsupported by sprytracks at the moment)
507  std::map<std::string, std::string> strobCorresp{ {"0","1"},{"1","0"}, {"2","2"} };
508  if (strobCorresp.find(itr->second) != strobCorresp.end())
509  {
510  if (this->Internal->Tracker.SetOption("Strobe mode", strobCorresp.at(itr->second)) != ATR_SUCCESS)
511  {
512  itr = this->Internal->DeviceOptions.erase(itr);
513  continue;
514  }
515  }
516  else
517  {
518  LOG_WARNING("Unsupported strobe mode value " << itr->second);
519  itr = this->Internal->DeviceOptions.erase(itr);
520  continue;
521  }
522  }
523  else/* end of hack*/ if (this->TranslateOptionName(itr->first, translatedOptionName)) // looking for the option in the translation dictionary
524  {
525  if (this->Internal->Tracker.SetOption(translatedOptionName, itr->second) != ATR_SUCCESS)
526  {
527  itr = this->Internal->DeviceOptions.erase(itr);
528  continue;
529  }
530  }
531  // option not found in dictionary, let's try to infer the option name by replacing _ by spaces
532  else if (itr->first.find('_') != std::string::npos && itr->first != "Enable_embedded_processing")
533  {
534  std::string inferredOptionName = itr->first;
535  std::replace(inferredOptionName.begin(), inferredOptionName.end(), '_', ' ');
536  if (this->Internal->Tracker.SetOption(inferredOptionName, itr->second) != ATR_SUCCESS)
537  {
538  itr = this->Internal->DeviceOptions.erase(itr);
539  continue;
540  }
541  }
542  else if (itr->first != "AcquisitionRate" && itr->first != "Id" && itr->first != "Type"
543  && itr->first != "ToolReferenceFrame" && itr->first != "Enable_embedded_processing")
544  {
545  LOG_WARNING("Unknown option \"" << itr->first << "\".");
546  itr = this->Internal->DeviceOptions.erase(itr);
547  continue;
548  }
549  ++itr;
550  }
551 
552  // disable marker status streaming and battery charge streaming, they cause momentary pauses in tracking while sending
553  if (this->Internal->DeviceType == AtracsysTracker::DEVICE_TYPE::SPRYTRACK_180)
554  {
555  if ((result = this->Internal->Tracker.EnableWirelessMarkerStatusStreaming(false)) != ATR_SUCCESS)
556  {
557  LOG_WARNING(this->Internal->Tracker.ResultToString(result));
558  }
559  if ((result = this->Internal->Tracker.EnableWirelessMarkerBatteryStreaming(false)) != ATR_SUCCESS)
560  {
561  LOG_WARNING(this->Internal->Tracker.ResultToString(result));
562  }
563  }
564 
565  // load passive geometries onto Atracsys
566  std::map<std::string, std::string>::iterator it;
567  for (it = begin(this->Internal->PlusIdMappedToGeometryFilename); it != end(this->Internal->PlusIdMappedToGeometryFilename); it++)
568  {
569  // load user defined geometry file
570  // TODO: add check for conflicting marker IDs
571  std::string geomFilePath = vtkPlusConfig::GetInstance()->GetDeviceSetConfigurationPath(it->second);
572  int geometryId;
573  if ((result = this->Internal->Tracker.LoadMarkerGeometryFromFile(geomFilePath, geometryId)) != ATR_SUCCESS)
574  {
575  LOG_ERROR(this->Internal->Tracker.ResultToString(result) << " This error occurred when trying to load geometry file at path: " << geomFilePath);
576  return PLUS_FAIL;
577  }
578  std::pair<int, std::string> newTool(geometryId, it->first);
579  this->Internal->FtkGeometryIdMappedToToolId.insert(newTool);
580  }
581 
582  // if active marker pairing is desired, then its time > 0 second
583  if (this->Internal->ActiveMarkerPairingTimeSec > 0)
584  {
585  // make LED blue during pairing
586  this->Internal->Tracker.SetUserLEDState(0, 0, 255, 0);
587 
588  // pair active markers
589  if ((result = this->Internal->Tracker.EnableWirelessMarkerPairing(true)) != ATR_SUCCESS)
590  {
591  LOG_ERROR(this->Internal->Tracker.ResultToString(result));
592  return PLUS_FAIL;
593  }
594  LOG_INFO("Active marker pairing period started for " << this->Internal->ActiveMarkerPairingTimeSec << " seconds.");
595 
596  // sleep while waiting for tracker to pair active markers
597  vtkIGSIOAccurateTimer::Delay(this->Internal->ActiveMarkerPairingTimeSec);
598 
599  LOG_INFO("Active marker pairing period ended.");
600 
601  if ((result = this->Internal->Tracker.EnableWirelessMarkerPairing(false)) != ATR_SUCCESS)
602  {
603  LOG_ERROR(this->Internal->Tracker.ResultToString(result));
604  return PLUS_FAIL;
605  }
606 
607  // make LED green, pairing is complete
608  this->Internal->Tracker.SetUserLEDState(0, 255, 0, 0);
609 
610  // TODO: check number of active markers paired
611 
612  std::string markerInfo;
613  this->Internal->Tracker.GetMarkerInfo(markerInfo);
614  if (!markerInfo.empty())
615  {
616  LOG_INFO("Additional info about paired markers:" << markerInfo << std::endl);
617  }
618  }
619 
620  return PLUS_SUCCESS;
621 }
622 
623 //----------------------------------------------------------------------------
625 {
626  LOG_TRACE("vtkPlusAtracsysTracker::InternalDisconnect");
627  this->Internal->Tracker.EnableUserLED(false);
628 
629  ATRACSYS_RESULT result;
630  if ((result = this->Internal->Tracker.Disconnect()) != ATR_SUCCESS)
631  {
632  LOG_ERROR(this->Internal->Tracker.ResultToString(result));
633  return PLUS_FAIL;
634  }
635  return PLUS_SUCCESS;
636 }
637 
638 //----------------------------------------------------------------------------
639 PlusStatus vtkPlusAtracsysTracker::InternalStartRecording()
640 {
641  LOG_TRACE("vtkPlusAtracsysTracker::InternalStartRecording");
642  return PLUS_SUCCESS;
643 }
644 
645 //----------------------------------------------------------------------------
646 PlusStatus vtkPlusAtracsysTracker::InternalStopRecording()
647 {
648  LOG_TRACE("vtkPlusAtracsysTracker::InternalStopRecording");
649  return PLUS_SUCCESS;
650 }
651 
652 //----------------------------------------------------------------------------
654 {
655  LOG_TRACE("vtkPlusAtracsysTracker::PauseVirtualDevice");
656  this->Internal->Tracker.Pause(true);
657  return PLUS_SUCCESS;
658 }
659 
660 //----------------------------------------------------------------------------
662 {
663  LOG_TRACE("vtkPlusAtracsysTracker::UnpauseVirtualDevice");
664  this->Internal->Tracker.Pause(false);
665  return PLUS_SUCCESS;
666 }
667 
668 //----------------------------------------------------------------------------
670 {
671  LOG_TRACE("vtkPlusAtracsysTracker::InternalUpdate");
672  double unfilteredTimestamp = vtkIGSIOAccurateTimer::GetSystemTime();
673 
674  std::vector<AtracsysTracker::Marker> markers;
675  std::map<std::string, std::string> events;
676 
677  uint64_t sdkTimestamp = 0;
678  ATRACSYS_RESULT result = this->Internal->Tracker.GetMarkersInFrame(markers, events, sdkTimestamp);
679 
680  if (result == AtracsysTracker::ATRACSYS_RESULT::ERROR_NO_FRAME_AVAILABLE)
681  {
682  // waiting for frame
683  return PLUS_SUCCESS;
684  }
685  else if (result != ATR_SUCCESS)
686  {
687  LOG_ERROR(this->Internal->Tracker.ResultToString(result));
688  return PLUS_FAIL;
689  }
690 
691  igsioFieldMapType customFields;
692 
693  // save sdk timestamp in customfield
694  customFields["SdkTimestamp"].first = FRAMEFIELD_NONE;
695  customFields["SdkTimestamp"].second = std::to_string(sdkTimestamp);
696 
697  // save event data in custom field
698  for (const auto& it : events)
699  {
700  customFields["Event_" + it.first].first = FRAMEFIELD_NONE;
701  customFields["Event_" + it.first].second = it.second;
702  }
703 
704  std::map<int, std::string>::iterator it;
705  for (it = this->Internal->FtkGeometryIdMappedToToolId.begin(); it != this->Internal->FtkGeometryIdMappedToToolId.end(); it++)
706  {
707  if (std::find(this->DisabledToolIds.begin(), this->DisabledToolIds.end(), it->second) != this->DisabledToolIds.end())
708  {
709  // tracking of this tool has been disabled
710  vtkNew<vtkMatrix4x4> emptyTransform;
711  igsioTransformName toolTransformName(it->second, this->GetToolReferenceFrameName());
712  std::string toolSourceId = toolTransformName.GetTransformName();
713  ToolTimeStampedUpdate(toolSourceId, emptyTransform.GetPointer(), TOOL_OUT_OF_VIEW, this->FrameNumber, unfilteredTimestamp);
714  continue;
715  }
716  bool toolUpdated = false;
717 
718  std::vector<AtracsysTracker::Marker>::iterator mit;
719  for (mit = markers.begin(); mit != markers.end(); mit++)
720  {
721  if (it->first != (int)mit->GetGeometryID())
722  {
723  continue;
724  }
725  // check if tool marker registration falls above maximum
726  if (mit->GetFiducialRegistrationErrorMm() > this->Internal->MaxMeanRegistrationErrorMm)
727  {
728  LOG_WARNING("Maximum mean marker fiducial registration error exceeded for tool: " << it->second);
729  continue;
730  }
731 
732  // tool is seen with acceptable registration error
733  toolUpdated = true;
734 
735  // save marker data in custom field
736  std::ostringstream mos;
737  mos.precision(3);
738  mos << std::fixed << mit->GetMarkerStatus() << " " << mit->GetTrackingID() << " ";
739  mos << mit->GetGeometryID() << " " << mit->GetGeometryPresenceMask() << " ";
740  mos << mit->GetFiducialRegistrationErrorMm() << " ";
741  mos << mit->GetFiducials().size(); // add number of fiducials
742  customFields[it->second + "_info"].first = FRAMEFIELD_NONE;
743  customFields[it->second + "_info"].second = mos.str();
744 
745  // save fiducial data in custom field
746  const auto& fids = mit->GetFiducials();
747  for (unsigned int f = 0; f < fids.size(); f++)
748  {
749  // 3D
750  std::ostringstream f3dos;
751  f3dos.precision(3);
752  f3dos << std::fixed << fids[f].Fid3dStatus << " ";
753  f3dos << fids[f].xMm << " " << fids[f].yMm << " " << fids[f].zMm << " ";
754  f3dos << fids[f].epipolarErrorPx << " " << fids[f].probability << " " << fids[f].triangulErrorMm;
755  customFields[it->second + "_fid" + std::to_string(f) + "_3D"].first = FRAMEFIELD_NONE;
756  customFields[it->second + "_fid" + std::to_string(f) + "_3D"].second = f3dos.str();
757  // 2D left
758  std::ostringstream f2dLos;
759  f2dLos.precision(3);
760  f2dLos << std::fixed << fids[f].Fid2dLeftStatus << " ";
761  f2dLos << fids[f].xLeftPx << " " << fids[f].yLeftPx << " ";
762  f2dLos << fids[f].heightLeftPx << " " << fids[f].widthLeftPx << " ";
763  f2dLos << fids[f].pixCountLeft;
764  customFields[it->second + "_fid" + std::to_string(f) + "_2D_L"].first = FRAMEFIELD_NONE;
765  customFields[it->second + "_fid" + std::to_string(f) + "_2D_L"].second = f2dLos.str();
766  // 2D right
767  std::ostringstream f2dRos;
768  f2dRos.precision(3);
769  f2dRos << std::fixed << fids[f].Fid2dRightStatus << " ";
770  f2dRos << fids[f].xRightPx << " " << fids[f].yRightPx << " ";
771  f2dRos << fids[f].heightRightPx << " " << fids[f].widthRightPx << " ";
772  f2dRos << fids[f].pixCountRight;
773  customFields[it->second + "_fid" + std::to_string(f) + "_2D_R"].first = FRAMEFIELD_NONE;
774  customFields[it->second + "_fid" + std::to_string(f) + "_2D_R"].second = f2dRos.str();
775  }
776 
777  // dump marker transform and custom data to buffer
778  igsioTransformName toolTransformName(it->second, this->GetToolReferenceFrameName());
779  std::string toolSourceId = toolTransformName.GetTransformName();
780  ToolTimeStampedUpdate(toolSourceId, mit->GetTransformToTracker(), TOOL_OK, this->FrameNumber,
781  unfilteredTimestamp, &customFields);
782  }
783 
784  if (!toolUpdated)
785  {
786  // tool is not seen in this frame
787  vtkNew<vtkMatrix4x4> emptyTransform;
788  igsioTransformName toolTransformName(it->second, this->GetToolReferenceFrameName());
789  std::string toolSourceId = toolTransformName.GetTransformName();
790  ToolTimeStampedUpdate(toolSourceId, emptyTransform.GetPointer(), TOOL_OUT_OF_VIEW, this->FrameNumber,
791  unfilteredTimestamp, &customFields);
792  }
793  }
794 
795  this->FrameNumber++;
796 
797  return PLUS_SUCCESS;
798 }
799 
800 //----------------------------------------------------------------------------
801 // Command methods
802 //----------------------------------------------------------------------------
803 
804 // LED
806 {
807  if (this->Internal->Tracker.EnableUserLED(enabled) != ATR_SUCCESS)
808  {
809  return PLUS_FAIL;
810  }
811  return PLUS_SUCCESS;
812 }
813 
814 PlusStatus vtkPlusAtracsysTracker::SetUserLEDState(int red, int green, int blue, int frequency, bool enabled /* = true */)
815 {
816  if (this->Internal->Tracker.SetUserLEDState(red, green, blue, frequency, enabled) != ATR_SUCCESS)
817  {
818  return PLUS_FAIL;
819  }
820  return PLUS_SUCCESS;
821 }
822 
823 //----------------------------------------------------------------------------
824 // Tools
826 {
827  if (enabled)
828  {
829  // remove any occurances of ToolId in DisabledToolIds
830  this->DisabledToolIds.erase(std::remove(this->DisabledToolIds.begin(), this->DisabledToolIds.end(), toolId), this->DisabledToolIds.end());
831  return PLUS_SUCCESS;
832  }
833 
834  // tool should be disabled, if it exists add to disabled list
835  bool toolExists = false;
836  std::map<int, std::string>::iterator it;
837  for (it = this->Internal->FtkGeometryIdMappedToToolId.begin(); it != this->Internal->FtkGeometryIdMappedToToolId.end(); it++)
838  {
839  if (igsioCommon::IsEqualInsensitive(it->second, toolId))
840  {
841  toolExists = true;
842  }
843  }
844 
845  if (!toolExists)
846  {
847  // trying to disable non-existant tool
848  LOG_ERROR("Tried to disable non-existant tool.");
849  return PLUS_FAIL;
850  }
851 
852  this->DisabledToolIds.push_back(toolId);
853  return PLUS_SUCCESS;
854 }
855 
856 //----------------------------------------------------------------------------
857 PlusStatus vtkPlusAtracsysTracker::AddToolGeometry(std::string toolId, std::string geomString)
858 {
859  // make sure geometry with toolId doesn't already exist
860  bool toolExists = false;
861  std::map<int, std::string>::iterator it;
862  for (it = this->Internal->FtkGeometryIdMappedToToolId.begin(); it != this->Internal->FtkGeometryIdMappedToToolId.end(); it++)
863  {
864  if (igsioCommon::IsEqualInsensitive(it->second, toolId))
865  {
866  toolExists = true;
867  }
868  }
869 
870  if (toolExists)
871  {
872  // trying to add a tool with an already-existing ID
873  LOG_ERROR("Tried to add tool with conflicting ToolId (" << toolId << ").");
874  return PLUS_FAIL;
875  }
876 
877  int geometryId;
878  ATRACSYS_RESULT result;
879  if ((result = this->Internal->Tracker.LoadMarkerGeometryFromString(geomString, geometryId)) != ATR_SUCCESS)
880  {
881  LOG_ERROR(this->Internal->Tracker.ResultToString(result) << " This error occurred when trying to load the following geometry information: " << geomString);
882  return PLUS_FAIL;
883  }
884 
885  // add datasources for this tool
886  vtkSmartPointer<vtkPlusDataSource> aToolSource = vtkSmartPointer<vtkPlusDataSource>::New();
887  aToolSource->SetReferenceCoordinateFrameName(this->ToolReferenceFrameName);
888  aToolSource->SetType(DATA_SOURCE_TYPE_TOOL);
889  aToolSource->SetId(toolId);
890  this->AddTool(aToolSource);
891 
892  // TO DO: add fiducial data as separate sources ?
893  //vtkSmartPointer<vtkPlusDataSource> aFids3dSource = vtkSmartPointer<vtkPlusDataSource>::New();
894  //aFids3dSource->SetReferenceCoordinateFrameName(this->ToolReferenceFrameName);
895  //aFids3dSource->SetType(DATA_SOURCE_TYPE_FIELDDATA);
896  //aFids3dSource->SetId(toolId + "_Fids3d");
897  //this->AddFieldDataSource(aFids3dSource);
898 
899  //vtkSmartPointer<vtkPlusDataSource> aFids2dLefSource = vtkSmartPointer<vtkPlusDataSource>::New();
900  //aFids2dLefSource->SetReferenceCoordinateFrameName(this->ToolReferenceFrameName);
901  //aFids2dLefSource->SetType(DATA_SOURCE_TYPE_FIELDDATA);
902  //aFids2dLefSource->SetId(toolId + "_Fids2dLeft");
903  //this->AddFieldDataSource(aFids2dLefSource);
904 
905  //vtkSmartPointer<vtkPlusDataSource> aFids2dRightSource = vtkSmartPointer<vtkPlusDataSource>::New();
906  //aFids2dRightSource->SetReferenceCoordinateFrameName(this->ToolReferenceFrameName);
907  //aFids2dRightSource->SetType(DATA_SOURCE_TYPE_FIELDDATA);
908  //aFids2dRightSource->SetId(toolId + "_Fids2dRight");
909  //this->AddFieldDataSource(aFids2dRightSource);
910 
911  // add output channel for this tool
912  vtkPlusChannel* outChannel;
913  if (this->GetFirstOutputChannel(outChannel) != PLUS_SUCCESS)
914  {
915  LOG_ERROR("Failed to get output channel when adding Atracsys geometry.");
916  return PLUS_FAIL;
917  }
918  outChannel->AddTool(aToolSource);
919  //outChannel->AddFieldDataSource(aFids3dSource);
920  //outChannel->AddFieldDataSource(aFids2dLefSource);
921  //outChannel->AddFieldDataSource(aFids2dRightSource);
922 
923  // enable tool in Plus
924 
925 
926  // register this tool internally
927  std::pair<int, std::string> newTool(geometryId, toolId);
928  this->Internal->FtkGeometryIdMappedToToolId.insert(newTool);
929 
930  return PLUS_SUCCESS;
931 }
932 
933 //----------------------------------------------------------------------------
934 // Other
936 {
937  if (this->Internal->Tracker.SetLaserEnabled(enabled) != ATR_SUCCESS)
938  {
939  return PLUS_FAIL;
940  }
941  return PLUS_SUCCESS;
942 }
943 
945 {
946  // not implemented yet
947  return PLUS_FAIL;
948 }
std::string ToolReferenceFrameName
virtual void PrintSelf(ostream &os, vtkIndent indent) VTK_OVERRIDE
virtual PlusStatus WriteConfiguration(vtkXMLDataElement *config)
static const char * ATRACSYS_COMMAND_SET_FLAG
PlusStatus AddToolGeometry(std::string toolId, std::string geomString)
Abstract interface for tracker and video devices.
Definition: vtkPlusDevice.h:60
const std::map< std::string, std::string > & GetDeviceOptions() const
PlusStatus SetLedEnabled(bool enabled)
#define XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_WRITING(deviceConfig, rootConfigElement)
PlusStatus GetFirstOutputChannel(vtkPlusChannel *&aChannel)
virtual PlusStatus InternalDisconnect()
static const char * ATRACSYS_COMMAND_VIDEO_ENABLED
virtual PlusStatus ReadConfiguration(vtkXMLDataElement *config)
virtual PlusStatus SetParameter(const std::string &key, const std::string &value)
igsioStatus PlusStatus
Definition: PlusCommon.h:40
AtracsysTracker Tracker
PlusStatus AddTool(vtkPlusDataSource *tool, bool requireUniquePortName=true)
virtual PlusStatus ToolTimeStampedUpdate(const std::string &aToolSourceId, vtkMatrix4x4 *matrix, ToolStatus status, unsigned long frameNumber, double unfilteredtimestamp, const igsioFieldMapType *customFields=NULL)
std::string to_string(ClariusAvailability avail)
for i
PlusStatus SetLaserEnabled(bool enabled)
vtkStandardNewMacro(vtkPlusAtracsysTracker)
static const char * ATRACSYS_COMMAND_ENABLE_TOOL
PlusStatus AddTool(vtkPlusDataSource *aTool)
#define PLUS_FAIL
Definition: PlusCommon.h:43
PlusStatus GetLoadedGeometries(std::map< int, std::vector< std::array< float, 3 >>> &geometries)
PlusStatus SetUserLEDState(int red, int green, int blue, int frequency, bool enabled=true)
static vtkPlusConfig * GetInstance()
static const char * ATRACSYS_COMMAND_LED_ENABLED
#define ATR_SUCCESS
bool strToInt32(const std::string &str, int &var)
unsigned long FrameNumber
PlusStatus GetCamerasCalibration(std::array< float, 10 > &leftIntrinsic, std::array< float, 10 > &rightIntrinsic, std::array< float, 3 > &rightPosition, std::array< float, 3 > &rightOrientation)
#define PLUS_SUCCESS
Definition: PlusCommon.h:44
static const char * ATRACSYS_COMMAND_LASER_ENABLED
int enabled
Definition: phidget22.h:3369
double InternalUpdateRate
bool strToFloat32(const std::string &str, float &var)
#define XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_READING(deviceConfig, rootConfigElement)
bool TranslateOptionName(const std::string &optionName, std::string &translatedOptionName)
AtracsysTracker::ATRACSYS_RESULT ATRACSYS_RESULT
std::string DeviceId
const char const char * value
Definition: phidget22.h:5111
static const char * ATRACSYS_COMMAND_SET_LED_RGBF
bool StartThreadForInternalUpdates
PlusStatus GetOptionValue(const std::string &optionName, std::string &optionValue)
void PrintSelf(ostream &os, vtkIndent indent)
Contains an optional timestamped circular buffer containing the video images and a number of timestam...
std::string GetDeviceSetConfigurationPath(const std::string &subPath)
static const char * ATRACSYS_COMMAND_ADD_TOOL
double frequency
Definition: phidget22.h:3246
PlusStatus SetToolEnabled(std::string toolId, bool enabled)
AtracsysTracker::DEVICE_TYPE DeviceType
PlusStatus SetVideoEnabled(bool enabled)
virtual bool IsVirtual() const
Interface to the Atracsys trackers This class talks with a Atracsys Tracker over the sTk Passive Trac...
Position vectors of rods v
Definition: algo3.m:14