diff --git a/src/PlusDataCollection/Atracsys/AtracsysMarkerCreator.cxx b/src/PlusDataCollection/Atracsys/AtracsysMarkerCreator.cxx index 0ba39347e..ec64e0cb6 100644 --- a/src/PlusDataCollection/Atracsys/AtracsysMarkerCreator.cxx +++ b/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 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) { diff --git a/src/PlusDataCollection/Atracsys/AtracsysTracker.cxx b/src/PlusDataCollection/Atracsys/AtracsysTracker.cxx index a8ebec0a6..21a6a6ac2 100644 --- a/src/PlusDataCollection/Atracsys/AtracsysTracker.cxx +++ b/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>& 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) { diff --git a/src/PlusDataCollection/Atracsys/AtracsysTracker.h b/src/PlusDataCollection/Atracsys/AtracsysTracker.h index d6bbbe4f4..b11a54699 100644 --- a/src/PlusDataCollection/Atracsys/AtracsysTracker.h +++ b/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,7 +154,7 @@ 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(); @@ -161,6 +162,9 @@ namespace Atracsys /*! */ 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>>& geometries); + /*! */ + RESULT GetFiducialCoordinates(int geomId, std::vector>& fidCoords); + /*! */ std::string ResultToString(RESULT result); diff --git a/src/PlusDataCollection/Atracsys/vtkPlusAtracsysTracker.cxx b/src/PlusDataCollection/Atracsys/vtkPlusAtracsysTracker.cxx index 8ef74335d..9b803b71e 100644 --- a/src/PlusDataCollection/Atracsys/vtkPlusAtracsysTracker.cxx +++ b/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 PlusIdMappedToGeometryFilename; + std::map ToolIdMappedToGeometryFilename; - // matches fusionTrack internal tool geometry ID to Plus tool ID for updating tools - std::map FtkGeometryIdMappedToToolId; + // matches Atracsys internal tool geometry ID to Plus tool ID for updating tools + std::map AtracGeomIdMappedToToolId; + // reverse lookup, Plus tool ID to Atracsys internal tool geometry ID + std::map 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>& 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 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 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::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 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::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::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::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 aFids3dSource = vtkSmartPointer::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 newTool(geometryId, toolId); - this->InternalObj->FtkGeometryIdMappedToToolId.insert(newTool); + this->InternalObj->AtracGeomIdMappedToToolId.insert(newTool); return PLUS_SUCCESS; } diff --git a/src/PlusDataCollection/Atracsys/vtkPlusAtracsysTracker.h b/src/PlusDataCollection/Atracsys/vtkPlusAtracsysTracker.h index d6b7c98d8..a8031b068 100644 --- a/src/PlusDataCollection/Atracsys/vtkPlusAtracsysTracker.h +++ b/src/PlusDataCollection/Atracsys/vtkPlusAtracsysTracker.h @@ -29,6 +29,9 @@ class vtkPlusDataCollectionExport vtkPlusAtracsysTracker : public vtkPlusDevice /* Get SDK version */ std::string GetSdkVersion(); + /* Get Firmware Software Version */ + std::string GetFirmwareSoftwareVersion(); + /*! Retrieves the device calibration date in ISO format: YYYY-MM-DDTHH:MM:SSZ+XX Example: "2020-04-08T21:24:15Z+00" is April 8th, 2020 at 9:24:15pm UTC (+00) */ @@ -53,6 +56,12 @@ class vtkPlusDataCollectionExport vtkPlusAtracsysTracker : public vtkPlusDevice */ PlusStatus GetLoadedGeometries(std::map>>& geometries); + /*! Get marker geometry details from tool id : + * Based on tool id, retrieves the provided relative file path, the associated Atracsys geometry id + * and the vector of x,y,z coordinates of each fiducial that composes the marker + */ + PlusStatus GetMarkerGeometry(std::string toolId, std::string &relPath, int &geomId, std::vector>& fidCoords); + /* Device is a hardware tracker. */ virtual bool IsTracker() const { return true; } virtual bool IsVirtual() const;