ENH: Added network configuration for Atracsys and other improvements (#1269)

Author: atracsys-sbt

src/PlusDataCollection/Atracsys/AtracsysMarkerCreator.cxx

@@ -479,12 +479,13 @@ void TransformMarkerCoordinateSystem(Fiducials& fids)
   // Look for the largest *unique* distance from the centroid, the corresponding fiducial will
   // define the x-axis.
   vec3 x;
+  int xfidId{ -1 };
   for (auto prev = dists.begin(), curr = ++dists.begin(); curr != dists.end(); ++prev, ++curr)
   {
     if (std::abs(prev->first - curr->first) > 0.1)
     {
-      auto fidId = (prev == dists.begin()) ? prev->second : curr->second;
-      x = vec3(fids[fidId]);
+      xfidId = (prev == dists.begin()) ? prev->second : curr->second;
+      x = vec3(fids[xfidId]);
       break;
     }
   }
@@ -497,12 +498,13 @@ void TransformMarkerCoordinateSystem(Fiducials& fids)
   }
   // Look for the largest *unique* distance from x-axis, it will define the plane of the y-axis.
   vec3 p;
+  int yfidId{ -1 };
   for (auto prev = dists2.begin(), curr = ++dists2.begin(); curr != dists2.end(); ++prev, ++curr)
   {
     if (std::abs(prev->first - curr->first) > 0.1)
     {
-      auto fidId = (prev == dists2.begin()) ? prev->second : curr->second;
-      p = vec3(fids[fidId]);
+      yfidId = (prev == dists2.begin()) ? prev->second : curr->second;
+      p = vec3(fids[yfidId]);
       break;
     }
   }
@@ -511,6 +513,24 @@ void TransformMarkerCoordinateSystem(Fiducials& fids)
   vec3 vz = (vx.cross(p - c)).normalize();
   vec3 vy = vz.cross(vx);
 
+  // Create new fiducial order:
+  // {fiducial defining x-axis, fiducial defining y-axis, remaining fiducials in descending order
+  // of distance to x-axis}
+  std::vector<int> newFidOrder{ xfidId, yfidId };
+  for (auto d = dists2.begin(); d != dists2.end(); ++d)
+  {
+      if (d->second != xfidId && d->second != yfidId)
+          newFidOrder.push_back(d->second);
+  }
+  // Apply new order to fiducials
+  for (int i = 0; i < newFidOrder.size(); ++i) {
+      size_t j = newFidOrder[i];
+      while (j != i) {
+          std::swap(fids[i], fids[j]);
+          std::swap(newFidOrder[i], newFidOrder[j]);
+          j = newFidOrder[i];
+      }
+  }
   // Store the coordinates of each fiducial in the new coordinate system.
   for (auto& fid : fids)
   {

src/PlusDataCollection/Atracsys/AtracsysTracker.cxx

@@ -374,6 +374,9 @@ class Tracker::Internal
   // library version 
   std::string LibVersion;
 
+  // firmware soft version
+  std::string FirmwareSoftwareVersion;
+
   // calibration date
   std::string CalibrationDate;
 
@@ -682,7 +685,7 @@ bool Tracker::IsVirtual()
 }
 
 //----------------------------------------------------------------------------
-Tracker::RESULT Tracker::Connect()
+Tracker::RESULT Tracker::Connect(const std::string& networkConfigFile)
 {
   if (this->InternalObj->FtkLib != nullptr && this->InternalObj->TrackerSN != 0)
   {
@@ -691,11 +694,24 @@ Tracker::RESULT Tracker::Connect()
   }
 
   // initialize SDK
-  this->InternalObj->FtkLib = ftkInit();
-
-  if (this->InternalObj->FtkLib == NULL)
+  if (networkConfigFile.empty())
   {
-    return ERROR_UNABLE_TO_GET_FTK_HANDLE;
+    this->InternalObj->FtkLib = ftkInit();
+    if (this->InternalObj->FtkLib == NULL)
+    {
+      return ERROR_UNABLE_TO_GET_FTK_HANDLE;
+    }
+  }
+  else
+  {
+    ftkBuffer buffer;
+    this->InternalObj->FtkLib = ftkInitExt(networkConfigFile.c_str(), &buffer);
+    if (this->InternalObj->FtkLib == NULL)
+    {
+      LOG_ERROR(buffer.data);
+      LOG_ERROR("Cannot initialize library with the provided network configuration.");
+      return ERROR_UNABLE_TO_GET_FTK_HANDLE;
+    }
   }
 
   DeviceData device;
@@ -779,6 +795,14 @@ Tracker::RESULT Tracker::Connect()
   ftkGetData(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, &buff);
   this->InternalObj->CalibrationDate = std::string(buff.data);
 
+  if (!this->GetOptionInfo("Device software version", info))
+  {
+    LOG_ERROR("Option unknown: \"Device software version\"");
+    return ERROR_OPTION_NOT_FOUND;
+  }
+  ftkGetData(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, &buff);
+  this->InternalObj->FirmwareSoftwareVersion = std::string(buff.data);
+
   // Check whether onboard processing is off or on (spryTrack only)
   if (this->DeviceType == SPRYTRACK_180 || this->DeviceType == SPRYTRACK_300)
   {
@@ -831,6 +855,13 @@ Tracker::RESULT Tracker::GetSDKversion(std::string& version)
   return SUCCESS;
 }
 
+//----------------------------------------------------------------------------
+Tracker::RESULT Tracker::GetFirmwareSoftwareVersion(std::string& version)
+{
+  version = this->InternalObj->FirmwareSoftwareVersion;
+  return SUCCESS;
+}
+
 //----------------------------------------------------------------------------
 Tracker::RESULT Tracker::GetCalibrationDate(std::string& date)
 {
@@ -954,6 +985,16 @@ Tracker::RESULT Tracker::GetLoadedGeometries(std::map<int, std::vector<std::arra
   return SUCCESS;
 }
 
+//----------------------------------------------------------------------------
+Tracker::RESULT Tracker::GetFiducialCoordinates(int geomId, std::vector<std::array<float, 3>>& fidCoords)
+{
+  if (this->InternalObj->Geometries.find(geomId) == this->InternalObj->Geometries.cend())
+    return ERROR_REQUESTED_GEOMETRY_NOT_FOUND;
+
+  fidCoords = this->InternalObj->Geometries.at(geomId);
+  return SUCCESS;
+}
+
 //----------------------------------------------------------------------------
 std::string Tracker::ResultToString(Tracker::RESULT result)
 {

src/PlusDataCollection/Atracsys/AtracsysTracker.h

@@ -125,7 +125,8 @@ namespace Atracsys
       ERROR_CANNOT_GET_MARKER_INFO,
       ERROR_FAILED_TO_SET_STK_PROCESSING_TYPE,
       ERROR_OPTION_NOT_FOUND,
-      ERROR_SET_OPTION
+      ERROR_SET_OPTION,
+      ERROR_REQUESTED_GEOMETRY_NOT_FOUND
     };
 
     enum DEVICE_TYPE
@@ -153,14 +154,17 @@ namespace Atracsys
     void Pause(bool tof);
 
     /*! Connect to Atracsys tracker, must be called before any other function in this wrapper API. */
-    RESULT Connect();
+    RESULT Connect(const std::string& networkConfigFile = "");
 
     /*! Closes connections to Atracsys tracker, must be called at end of application. */
     RESULT Disconnect();
 
     /*! */
     RESULT GetSDKversion(std::string& version);
 
+    /*! */
+    RESULT GetFirmwareSoftwareVersion(std::string& version);
+
     /*! */
     RESULT GetCalibrationDate(std::string& date);
 
@@ -191,6 +195,9 @@ namespace Atracsys
     /*! */
     RESULT GetLoadedGeometries(std::map<int, std::vector<std::array<float,3>>>& geometries);
 
+    /*! */
+    RESULT GetFiducialCoordinates(int geomId, std::vector<std::array<float, 3>>& fidCoords);
+
     /*! */
     std::string ResultToString(RESULT result);
 

src/PlusDataCollection/Atracsys/vtkPlusAtracsysTracker.cxx

@@ -84,10 +84,12 @@ class vtkPlusAtracsysTracker::vtkInternal
   int MaxMissingFiducials = 0; // maximum number of missing fiducials
 
   // matches plus tool id to .ini geometry file names/paths
-  std::map<std::string, std::string> PlusIdMappedToGeometryFilename;
+  std::map<std::string, std::string> ToolIdMappedToGeometryFilename;
 
-  // matches fusionTrack internal tool geometry ID to Plus tool ID for updating tools
-  std::map<int, std::string> FtkGeometryIdMappedToToolId;
+  // matches Atracsys internal tool geometry ID to Plus tool ID for updating tools
+  std::map<int, std::string> AtracGeomIdMappedToToolId;
+  // reverse lookup, Plus tool ID to Atracsys internal tool geometry ID
+  std::map<std::string, int> ToolIdMappedToAtracGeomId;
 
   // Atracsys API wrapper class handle
   Atracsys::Tracker Tracker;
@@ -130,6 +132,14 @@ std::string vtkPlusAtracsysTracker::GetSdkVersion()
   return v;
 }
 
+//----------------------------------------------------------------------------
+std::string vtkPlusAtracsysTracker::GetFirmwareSoftwareVersion()
+{
+    std::string d;
+    this->InternalObj->Tracker.GetFirmwareSoftwareVersion(d);
+    return d;
+}
+
 //----------------------------------------------------------------------------
 std::string vtkPlusAtracsysTracker::GetCalibrationDate()
 {
@@ -187,6 +197,32 @@ PlusStatus vtkPlusAtracsysTracker::GetLoadedGeometries(std::map<int, std::vector
   return PLUS_SUCCESS;
 }
 
+//----------------------------------------------------------------------------
+PlusStatus vtkPlusAtracsysTracker::GetMarkerGeometry(std::string toolId, std::string& relPath,
+  int& geomId, std::vector<std::array<float, 3>>& fidCoords)
+{
+  if (this->InternalObj->ToolIdMappedToGeometryFilename.find(toolId) == this->InternalObj->ToolIdMappedToGeometryFilename.cend())
+  {
+    LOG_ERROR("Could not get geometry filename for tool " << toolId);
+    return PLUS_FAIL;
+  }
+  relPath = this->InternalObj->ToolIdMappedToGeometryFilename.at(toolId);
+
+  if (this->InternalObj->ToolIdMappedToAtracGeomId.find(toolId) == this->InternalObj->ToolIdMappedToAtracGeomId.cend())
+  {
+    LOG_ERROR("Could not get marker geometry for tool " << toolId);
+    return PLUS_FAIL;
+  }
+  geomId = this->InternalObj->ToolIdMappedToAtracGeomId.at(toolId);
+
+  if (this->InternalObj->Tracker.GetFiducialCoordinates(geomId, fidCoords) != ATR_SUCCESS)
+  {
+    LOG_ERROR("Could not get fiducial coordinates for geometry id " << geomId);
+    return PLUS_FAIL;
+  }
+  return PLUS_SUCCESS;
+}
+
 //----------------------------------------------------------------------------
 bool vtkPlusAtracsysTracker::IsVirtual() const
 {
@@ -238,13 +274,13 @@ PlusStatus vtkPlusAtracsysTracker::ReadConfiguration(vtkXMLDataElement* rootConf
     XML_READ_ENUM2_ATTRIBUTE_NONMEMBER_OPTIONAL(TrackingType, toolTrackingType, toolDataElement, "ACTIVE", ACTIVE, "PASSIVE", PASSIVE);
     if (toolTrackingType == ACTIVE)
     {
-      // active tool, can be loaded directly into FtkGeometryIdMappedToToolId
+      // active tool, can be loaded directly into AtracGeomIdMappedToToolId
       int ftkGeometryId = -1;
       XML_READ_SCALAR_ATTRIBUTE_NONMEMBER_OPTIONAL(int, GeometryId, ftkGeometryId, toolDataElement);
       if (ftkGeometryId != -1)
       {
-        std::pair<int, std::string> thisTool(ftkGeometryId, toolId);
-        this->InternalObj->FtkGeometryIdMappedToToolId.insert(thisTool);
+        this->InternalObj->AtracGeomIdMappedToToolId.insert(std::make_pair(ftkGeometryId, toolId));
+        this->InternalObj->ToolIdMappedToAtracGeomId.insert(std::make_pair(toolId, ftkGeometryId));
       }
       else
       {
@@ -254,12 +290,11 @@ PlusStatus vtkPlusAtracsysTracker::ReadConfiguration(vtkXMLDataElement* rootConf
     }
     else if (toolTrackingType == PASSIVE)
     {
-      // passive tool, load into PlusIdMappedToGeometryFilename to have geometry loaded in InternalConnect
+      // passive tool, load into ToolIdMappedToGeometryFilename to have geometry loaded in InternalConnect
       const char* geometryFile;
       if ((geometryFile = toolDataElement->GetAttribute("GeometryFile")) != NULL)
       {
-        std::pair<std::string, std::string> thisTool(toolId, geometryFile);
-        this->InternalObj->PlusIdMappedToGeometryFilename.insert(thisTool);
+        this->InternalObj->ToolIdMappedToGeometryFilename.insert(std::make_pair(toolId, geometryFile));
       }
       else
       {
@@ -326,8 +361,17 @@ PlusStatus vtkPlusAtracsysTracker::InternalConnect()
 {
   LOG_TRACE("vtkPlusAtracsysTracker::InternalConnect");
 
+  // Check if network configuration file is provided
+  std::string networkConfigPath;
+  if (this->InternalObj->DeviceOptions.find("NetworkConfigFile") != this->InternalObj->DeviceOptions.end())
+  {
+    networkConfigPath = vtkPlusConfig::GetInstance()->GetDeviceSetConfigurationPath(
+        this->InternalObj->DeviceOptions["NetworkConfigFile"]);
+  }
+
   // Connect to device
-  ATR_RESULT result = this->InternalObj->Tracker.Connect();
+  ATR_RESULT result = this->InternalObj->Tracker.Connect(networkConfigPath.empty() ? "" : networkConfigPath.c_str());
+
   if (result != ATR_SUCCESS && result != ATR_RESULT::WARNING_CONNECTED_IN_USB2)
   {
     LOG_ERROR(this->InternalObj->Tracker.ResultToString(result));
@@ -540,7 +584,8 @@ PlusStatus vtkPlusAtracsysTracker::InternalConnect()
       }
     }
     else if (itr->first != "AcquisitionRate" && itr->first != "Id" && itr->first != "Type"
-      && itr->first != "ToolReferenceFrame" && itr->first != "Enable_embedded_processing")
+      && itr->first != "ToolReferenceFrame" && itr->first != "Enable_embedded_processing"
+      && itr->first != "NetworkConfigFile")
     {
       LOG_WARNING("Unknown option \"" << itr->first << "\".");
       itr = this->InternalObj->DeviceOptions.erase(itr);
@@ -564,7 +609,7 @@ PlusStatus vtkPlusAtracsysTracker::InternalConnect()
 
   // load passive geometries onto Atracsys
   std::map<std::string, std::string>::iterator it;
-  for (it = begin(this->InternalObj->PlusIdMappedToGeometryFilename); it != end(this->InternalObj->PlusIdMappedToGeometryFilename); it++)
+  for (it = begin(this->InternalObj->ToolIdMappedToGeometryFilename); it != end(this->InternalObj->ToolIdMappedToGeometryFilename); it++)
   {
     // load user defined geometry file
     // TODO: add check for conflicting marker IDs
@@ -575,8 +620,8 @@ PlusStatus vtkPlusAtracsysTracker::InternalConnect()
       LOG_ERROR(this->InternalObj->Tracker.ResultToString(result) << " This error occurred when trying to load geometry file at path: " << geomFilePath);
       return PLUS_FAIL;
     }
-    std::pair<int, std::string> newTool(geometryId, it->first);
-    this->InternalObj->FtkGeometryIdMappedToToolId.insert(newTool);
+    this->InternalObj->AtracGeomIdMappedToToolId.insert(std::make_pair(geometryId, it->first));
+    this->InternalObj->ToolIdMappedToAtracGeomId.insert(std::make_pair(it->first, geometryId));
   }
 
   // if active marker pairing is desired, then its time > 0 second
@@ -702,7 +747,7 @@ PlusStatus vtkPlusAtracsysTracker::InternalUpdate()
   }
 
   std::map<int, std::string>::iterator it;
-  for (it = this->InternalObj->FtkGeometryIdMappedToToolId.begin(); it != this->InternalObj->FtkGeometryIdMappedToToolId.end(); it++)
+  for (it = this->InternalObj->AtracGeomIdMappedToToolId.begin(); it != this->InternalObj->AtracGeomIdMappedToToolId.end(); it++)
   {
     if (std::find(this->DisabledToolIds.begin(), this->DisabledToolIds.end(), it->second) != this->DisabledToolIds.end())
     {
@@ -834,7 +879,7 @@ PlusStatus vtkPlusAtracsysTracker::SetToolEnabled(std::string toolId, bool enabl
   // tool should be disabled, if it exists add to disabled list
   bool toolExists = false;
   std::map<int, std::string>::iterator it;
-  for (it = this->InternalObj->FtkGeometryIdMappedToToolId.begin(); it != this->InternalObj->FtkGeometryIdMappedToToolId.end(); it++)
+  for (it = this->InternalObj->AtracGeomIdMappedToToolId.begin(); it != this->InternalObj->AtracGeomIdMappedToToolId.end(); it++)
   {
     if (igsioCommon::IsEqualInsensitive(it->second, toolId))
     {
@@ -859,7 +904,7 @@ PlusStatus vtkPlusAtracsysTracker::AddToolGeometry(std::string toolId, std::stri
   // make sure geometry with toolId doesn't already exist
   bool toolExists = false;
   std::map<int, std::string>::iterator it;
-  for (it = this->InternalObj->FtkGeometryIdMappedToToolId.begin(); it != this->InternalObj->FtkGeometryIdMappedToToolId.end(); it++)
+  for (it = this->InternalObj->AtracGeomIdMappedToToolId.begin(); it != this->InternalObj->AtracGeomIdMappedToToolId.end(); it++)
   {
     if (igsioCommon::IsEqualInsensitive(it->second, toolId))
     {
@@ -889,7 +934,7 @@ PlusStatus vtkPlusAtracsysTracker::AddToolGeometry(std::string toolId, std::stri
   aToolSource->SetId(toolId);
   this->AddTool(aToolSource);
 
-  // TO DO: add fiducial data as separate sources ?
+  // TODO: add fiducial data as separate sources ?
   //vtkSmartPointer<vtkPlusDataSource> aFids3dSource = vtkSmartPointer<vtkPlusDataSource>::New();
   //aFids3dSource->SetReferenceCoordinateFrameName(this->ToolReferenceFrameName);
   //aFids3dSource->SetType(DATA_SOURCE_TYPE_FIELDDATA);
@@ -925,7 +970,7 @@ PlusStatus vtkPlusAtracsysTracker::AddToolGeometry(std::string toolId, std::stri
 
   // register this tool internally
   std::pair<int, std::string> newTool(geometryId, toolId);
-  this->InternalObj->FtkGeometryIdMappedToToolId.insert(newTool);
+  this->InternalObj->AtracGeomIdMappedToToolId.insert(newTool);
 
   return PLUS_SUCCESS;
 }