From c641983b5ea553da385407ae5373348de47d9602 Mon Sep 17 00:00:00 2001 From: Sylvain Bernhardt Date: Fri, 12 Sep 2025 09:41:00 +0200 Subject: [PATCH] Reworked AtracsysMarkerCreator One can easily create an Atracsys geometry file with automatic background subtraction and geometry refinement. It does not require to track an initial guess of the geometry. The geometry layout is consistent and generate simpler geometries: - The origin is the centroid C of all fiducials - The x-axis is defined by the largest unique distance from the origin - The y-axis is in the plane defined by the origin, the x-axis and the furthest fiducial from the x-axis - The z-axis naturally comes from x and y-axes This resulted in various changes in the wrapper: - Added a namespace Atracsys for classes AtracsysTracker (now Tracker), Marker and Fiducial - Added a namespace Geometry for IniFile class and related free functions - Added GetFiducialsInFrame function, which allows to retrieve 3D fiducials in the scene without tracking a specific geometry --- .../Atracsys/AtracsysMarkerCreator.cxx | 600 ++++++++++++----- .../Atracsys/AtracsysTracker.cxx | 611 ++++++++++-------- .../Atracsys/AtracsysTracker.h | 355 +++++----- .../Atracsys/vtkPlusAtracsysTracker.cxx | 274 ++++---- .../Atracsys/vtkPlusAtracsysTracker.h | 2 +- 5 files changed, 1105 insertions(+), 737 deletions(-) diff --git a/src/PlusDataCollection/Atracsys/AtracsysMarkerCreator.cxx b/src/PlusDataCollection/Atracsys/AtracsysMarkerCreator.cxx index 4d20a8186..0ba39347e 100644 --- a/src/PlusDataCollection/Atracsys/AtracsysMarkerCreator.cxx +++ b/src/PlusDataCollection/Atracsys/AtracsysMarkerCreator.cxx @@ -9,9 +9,12 @@ #include #include +//---------------------------------------------------------------------------- +using namespace Atracsys; + // global handle to Atracsys API wrapper -AtracsysTracker Tracker; -AtracsysTracker::DEVICE_TYPE DeviceType = AtracsysTracker::UNKNOWN_DEVICE; +Tracker AtrTracker; +Tracker::DEVICE_TYPE DeviceType = Atracsys::Tracker::DEVICE_TYPE::UNKNOWN_DEVICE; // constants #define NUM_BACKGROUND_FRAMES 100 @@ -19,57 +22,179 @@ AtracsysTracker::DEVICE_TYPE DeviceType = AtracsysTracker::UNKNOWN_DEVICE; #define ATRACSYS_MAX_FIDUCIALS 6 // for convenience -#define ATR_SUCCESS AtracsysTracker::ATRACSYS_RESULT::SUCCESS -typedef AtracsysTracker::ATRACSYS_RESULT ATRACSYS_RESULT; -typedef AtracsysTracker::Fiducial Fiducial; -typedef AtracsysTracker::Marker Marker; +typedef Atracsys::Tracker::RESULT ATR_RESULT; +#define ATR_SUCCESS ATR_RESULT::SUCCESS // data types -// list of all 3d fiducials in single frame, including triangulated background stray blobs -typedef std::vector fidsFrame; -// list of frames of 3d fiducials -typedef std::vector fidsFrameList; +typedef std::vector FiducialsSequence; +typedef std::vector MarkerSequence; // struct to hold geometry intrinsics -struct MarkerGeometry +struct CustomGeometry { std::string name; std::string description; std::string destPath; int geometryId; - fidsFrame fids; + Fiducials fids; +}; + +// forward declarations +PlusStatus CollectFiducialSequence(FiducialsSequence& fidFrameList, int numFrames); +PlusStatus ProcessBackgroundFiducials(FiducialsSequence& fidFrameList, Fiducials& backgroundFids); +PlusStatus CaptureInitialGeometry(Fiducials& backgroundFids, Fiducials& fids); +void TransformMarkerCoordinateSystem(Fiducials& fids); +std::string WriteGeometryToString(const CustomGeometry& geom); +PlusStatus CollectMarkerSequence(CustomGeometry& geom, MarkerSequence& markerFrameList, int numFrames); +PlusStatus RefineMarkerGeometry(CustomGeometry& geom, MarkerSequence& markerFrameList); +PlusStatus WriteGeometryToIniFile(const CustomGeometry& geom); + +// function to remove excessive precision for geometry coordinates writing +double remDecimals(double a, double prec) +{ + return (std::abs(a) > prec) ? a : 0.0; }; -// forward declare functions -PlusStatus CollectFiducials(fidsFrameList& fidFrameList, int numFrames); -PlusStatus ProcessFiducials(fidsFrameList& fidFrameList, fidsFrame& backgroundFids); -PlusStatus PerformBackgroundSubtraction(fidsFrame& backgroundFids, fidsFrame& dataFids); -PlusStatus ZeroMeanFids(fidsFrame& dataFids); -PlusStatus WriteGeometryIniFile(const MarkerGeometry geom); +//---------------------------------------------------------------------------- +// 3D point/vector struct to help in processing fiducial data +struct vec3 +{ + double x{ 0 }; + double y{ 0 }; + double z{ 0 }; + + vec3() = default; + vec3(const vec3& x1) = default; + vec3(const Fiducial& f) : x{ f.xMm }, y{ f.yMm }, z{ f.zMm } {}; + vec3(double _x, double _y, double _z) : x{ _x }, y{ _y }, z{ _z } {}; + + vec3& operator=(const vec3& other) = default; + vec3& operator=(vec3&& other) = default; + vec3 operator-() const + { + return { -this->x, -this->y, -this->z }; + } + + vec3& operator+=(const vec3& other) + { + this->x += other.x; + this->y += other.y; + this->z += other.z; + return *this; + } + + vec3& operator-=(const vec3& other) + { + this->x -= other.x; + this->y -= other.y; + this->z -= other.z; + return *this; + } + + vec3& operator/=(const double& value) + { + this->x /= value; + this->y /= value; + this->z /= value; + return *this; + } + + vec3& operator*=(const double& value) + { + this->x *= value; + this->y *= value; + this->z *= value; + return *this; + } + + vec3 operator+ (const vec3& x2) const + { + vec3 result{ *this }; + result += x2; + return result; + } + + vec3 operator- (const vec3& x1) const + { + vec3 result{ *this }; + result -= x1; + return result; + } + + vec3 operator* (const double value) const + { + return { this->x * value, this->y * value, this->z * value }; + } + + friend vec3 operator* (const double value, const vec3& x1) + { + return x1 * value; + } + + vec3 operator/ (const double value) const + { + vec3 result{ *this }; + result /= value; + return result; + } + + double operator* (const vec3& x1) const + { + return x1.x * this->x + x1.y * this->y + x1.z * this->z; + } + + double norm() const + { + return std::sqrt(this->normSqr()); + } + + double normSqr() const + { + return *this * *this; + } + + vec3& normalize() + { + // check if norm() is not 0 + if (this->norm()) + *this /= this->norm(); + return *this; + } + + vec3 cross(const vec3& x2) const + { + return { this->y * x2.z - this->z * x2.y, this->z * x2.x - this->x * x2.z, this->x * x2.y - this->y * x2.x }; + } +}; +//---------------------------------------------------------------------------- int main(int argc, char** argv) { // Check command line arguments. bool printHelp(false); - bool backgroundSubtraction(false); std::string markerName; std::string description; std::string destinationPath; int geometryId = -1; int verboseLevel = vtkPlusLogger::LOG_LEVEL_UNDEFINED; int numFrames = DEFAULT_NUM_DATA_FRAMES; + int intTime = -1; + int imgThresh = -1; vtksys::CommandLineArguments args; args.Initialize(argc, argv); args.AddArgument("--help", vtksys::CommandLineArguments::NO_ARGUMENT, &printHelp, "Print this help."); - args.AddArgument("--background-subtraction", vtksys::CommandLineArguments::NO_ARGUMENT, &backgroundSubtraction, "Remove background fiducials from data considered by the creator."); args.AddArgument("--marker-name", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &markerName, "Name of marker."); args.AddArgument("--description", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &description, "Decsription of marker (i.e. purpose, color, size, and any other desired metadata)."); args.AddArgument("--geometryId", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &geometryId, "Id of the geometry we are creating. Must be unique."); args.AddArgument("--destination-path", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &destinationPath, "Where the generated marker geometry ini file will be written to."); args.AddArgument("--verbose", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &verboseLevel, "Verbose level (1=error only, 2=warning, 3=info, 4=debug, 5=trace)."); args.AddArgument("--num-frames", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &numFrames, "Number of frames to use in generating marker geometry ini file."); + args.AddArgument("--int-time", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &intTime, "Integration time for the camera."); + args.AddArgument("--img-thresh", vtksys::CommandLineArguments::EQUAL_ARGUMENT, &imgThresh, "Image compression threshold."); + + std::this_thread::sleep_for(std::chrono::milliseconds(4000)); if (!args.Parse()) { @@ -110,122 +235,151 @@ int main(int argc, char** argv) LOG_INFO("Logging at level " << vtkPlusLogger::Instance()->GetLogLevel() << " (" << vtkPlusLogger::Instance()->GetLogLevelString() << ") to file: " << vtkPlusLogger::Instance()->GetLogFileName()); // Connect to tracker - ATRACSYS_RESULT result = Tracker.Connect(); - if (result != ATR_SUCCESS && result != AtracsysTracker::ATRACSYS_RESULT::WARNING_CONNECTED_IN_USB2) + ATR_RESULT result = AtrTracker.Connect(); + if (result != ATR_SUCCESS && result != ATR_RESULT::WARNING_CONNECTED_IN_USB2) { - LOG_ERROR(Tracker.ResultToString(result)); + LOG_ERROR(AtrTracker.ResultToString(result)); + AtrTracker.SetUserLEDState(255, 0, 0, 0); // red LED return PLUS_FAIL; } - else if (result == AtracsysTracker::ATRACSYS_RESULT::WARNING_CONNECTED_IN_USB2) + else if (result == ATR_RESULT::WARNING_CONNECTED_IN_USB2) { - LOG_WARNING(Tracker.ResultToString(result)); + LOG_WARNING(AtrTracker.ResultToString(result)); + AtrTracker.SetUserLEDState(255, 153, 0, 0); // orange LED } - // get device type - Tracker.GetDeviceType(DeviceType); - - // if spryTrack, setup for onboard processing and disable extraneous marker info streaming - if (DeviceType == AtracsysTracker::DEVICE_TYPE::SPRYTRACK_180) + // If spryTrack, setup for onboard processing and disable extraneous marker info streaming + AtrTracker.GetDeviceType(DeviceType); + if (DeviceType == Tracker::DEVICE_TYPE::SPRYTRACK_180) { - Tracker.SetSpryTrackProcessingType(AtracsysTracker::SPRYTRACK_IMAGE_PROCESSING_TYPE::PROCESSING_ONBOARD); + AtrTracker.SetSpryTrackProcessingType(Tracker::SPRYTRACK_IMAGE_PROCESSING_TYPE::PROCESSING_ONBOARD); } - - // set LED blue for collecting background - Tracker.SetUserLEDState(0, 0, 255, 0); - - // collect background frames - fidsFrameList backgroundfidsFrameList; - fidsFrame backgroundFids; // list of fids visible in background - if (backgroundSubtraction) + // Setting other options + if (intTime > 0) { - if (CollectFiducials(backgroundfidsFrameList, NUM_BACKGROUND_FRAMES) == PLUS_FAIL) + if (AtrTracker.SetOption("Image Integration Time", std::to_string(intTime)) != Atracsys::Tracker::SUCCESS) { - LOG_ERROR("Failed to collect background noise fiducial frames."); + AtrTracker.SetUserLEDState(255, 0, 0, 0); // red LED + return PLUS_FAIL; + } + } + if (imgThresh > 0) + { + if (AtrTracker.SetOption("Image Compression Threshold", std::to_string(imgThresh)) != Atracsys::Tracker::SUCCESS) + { + AtrTracker.SetUserLEDState(255, 0, 0, 0); // red LED return PLUS_FAIL; } - ProcessFiducials(backgroundfidsFrameList, backgroundFids); - - // set LED red and wait for user to place marker in front of the camera - Tracker.SetUserLEDState(255, 0, 0, 0); - LOG_INFO("Background collection successful. Place marker in FOV of camera." << std::endl << "Press to continue."); - std::cin.get(); } - LOG_INFO("Collecting data frames."); - // set LED blue for collecting frames - Tracker.SetUserLEDState(0, 0, 255, 0); + // Collect background frames + LOG_INFO("\nBackground acquisition. Do NOT place the marker in front of the camera."); + LOG_INFO("Press to continue."); + std::cin.get(); - // collect numFrames frames of fiducials - fidsFrameList datafidsFrameList; - if (CollectFiducials(datafidsFrameList, numFrames) == PLUS_FAIL) + FiducialsSequence backgroundfidsFrameList; + Fiducials backgroundFids; // list of fiducials visible in background + AtrTracker.SetUserLEDState(0, 0, 255, 0); // blue LED + if (CollectFiducialSequence(backgroundfidsFrameList, NUM_BACKGROUND_FRAMES) != PLUS_SUCCESS) { - LOG_ERROR("Failed to collect data fiducial frames."); + AtrTracker.SetUserLEDState(255, 0, 0, 0); // red LED + LOG_ERROR("Failed to collect background noise fiducial frames."); return PLUS_FAIL; } - fidsFrame dataFids; - ProcessFiducials(datafidsFrameList, dataFids); - - // perform background subtraction and fiducial filtering - if (PerformBackgroundSubtraction(backgroundFids, dataFids) != PLUS_SUCCESS) + std::cout << "\n"; + ProcessBackgroundFiducials(backgroundfidsFrameList, backgroundFids); + LOG_INFO("Background collection successful (" << backgroundFids.size() << " stray reflections).\n"); + + // Create initial marker geometry + AtrTracker.SetUserLEDState(0, 255, 0, 0); // green LED + LOG_INFO("Place marker facing the camera in the first half of the working volume\nand rotate slowly the marker until geometry file is generated (left LED off)."); + LOG_INFO("Press to continue."); + std::cin.get(); + + AtrTracker.SetUserLEDState(255, 153, 0, 0); // orange LED + LOG_INFO("Trying to get a good initial guess of the geometry..."); + Fiducials foregroundFids; + if (CaptureInitialGeometry(backgroundFids, foregroundFids) != PLUS_SUCCESS) { - // problem detected with captured fiducial data, don't generate marker - return EXIT_FAILURE; + AtrTracker.SetUserLEDState(255, 0, 0, 0); // red LED + LOG_ERROR("Failed to collect initial foreground fiducials."); + return PLUS_FAIL; } - // create marker - MarkerGeometry geom; + AtrTracker.SetUserLEDState(0, 0, 255, 0); // blue LED + LOG_INFO("Initial guess for marker geometry obtained."); + + CustomGeometry geom; geom.name = markerName; geom.description = description; geom.destPath = destinationPath; geom.geometryId = geometryId; - ZeroMeanFids(dataFids); - geom.fids = dataFids; + geom.fids = foregroundFids; - // if too many fids, return - if (geom.fids.size() > ATRACSYS_MAX_FIDUCIALS) + // Refining initial marker geometry + LOG_INFO("Collecting marker frames to refine geometry..."); + MarkerSequence markerFrameList; + if (CollectMarkerSequence(geom, markerFrameList, numFrames) != PLUS_SUCCESS) { - LOG_ERROR("Too many fiducials in frame (there were " << geom.fids.size() << " marker fids visible). Unable to create Atracsys marker with this many fiducials."); - return EXIT_FAILURE; + AtrTracker.SetUserLEDState(255, 0, 0, 0); // red LED + LOG_ERROR("Failed to collect marker frames with initial geometry guess."); + return PLUS_FAIL; } - else if (geom.fids.size() < 3) + std::cout << "\n"; + + LOG_INFO("Refining geometry..."); + if (RefineMarkerGeometry(geom, markerFrameList) != PLUS_SUCCESS) { - LOG_ERROR("Too few fiducials in frame. Ensure marker is fully visible and try again."); - return EXIT_FAILURE; + AtrTracker.SetUserLEDState(255, 0, 0, 0); // red LED + LOG_ERROR("Failed to refine geometry."); + return PLUS_FAIL; } - // write marker ini file - WriteGeometryIniFile(geom); + // Write marker ini file + WriteGeometryToIniFile(geom); // turn LED off & end program - Tracker.EnableUserLED(false); + AtrTracker.EnableUserLED(false); return EXIT_SUCCESS; } //---------------------------------------------------------------------------- -PlusStatus CollectFiducials(fidsFrameList& fidFrameList, int numFrames) +void ProgressBar(int i, int N, int barWidth = 70) +{ + std::cout << "["; + int pos = barWidth * i / N; + for (int i = 0; i < barWidth; ++i) { + if (i < pos) std::cout << "="; + else if (i == pos) std::cout << ">"; + else std::cout << " "; + } + std::cout << "] " << int(100 * i / N) << " %\r"; + std::cout.flush(); +} + +//---------------------------------------------------------------------------- +PlusStatus CollectFiducialSequence(FiducialsSequence& fidFrameList, int numFrames) { int m = 0; - fidsFrame fid3dFrame; - std::vector markerFrame; // unused + ProgressBar(m, numFrames); + Fiducials fid3dFrame; std::map events; // unused + uint64_t ts = 0; // unused while (m < numFrames) { - // ensure vector is empty - markerFrame.clear(); - uint64_t ts = 0; - ATRACSYS_RESULT result = Tracker.GetMarkersInFrame(markerFrame, events, ts); + ATR_RESULT result = AtrTracker.GetFiducialsInFrame(fid3dFrame, events, ts); if (result == ATR_SUCCESS) { - m++; + ProgressBar(++m, numFrames); fidFrameList.push_back(fid3dFrame); } - else if (result == AtracsysTracker::ERROR_NO_FRAME_AVAILABLE) + else if (result == ATR_RESULT::ERROR_NO_FRAME_AVAILABLE) { continue; } else { - LOG_ERROR(Tracker.ResultToString(result)); + LOG_ERROR(AtrTracker.ResultToString(result)); return PLUS_FAIL; } } @@ -233,95 +387,241 @@ PlusStatus CollectFiducials(fidsFrameList& fidFrameList, int numFrames) } //---------------------------------------------------------------------------- -PlusStatus ProcessFiducials(fidsFrameList& fidFrameList, fidsFrame& fids) +PlusStatus ProcessBackgroundFiducials(FiducialsSequence& fidFrameList, Fiducials& fids) { - fidsFrame frame; - // make sure backgroundFids is empty + Fiducials frame; + // make sure fids is empty fids.clear(); - for (fidsFrameList::size_type frameNum = 0; frameNum < fidFrameList.size(); frameNum++) + for (FiducialsSequence::size_type frameNum = 0; frameNum < fidFrameList.size(); frameNum++) { frame = fidFrameList[frameNum]; - // populate backgroundFids with list of fiducials appearing in the background + // populate fids with list of fiducials appearing in the background std::copy(frame.begin(), frame.end(), std::inserter(fids, fids.end())); } // remove duplicate points in backgroundFids && resize std::sort(fids.begin(), fids.end()); - fidsFrame::iterator it = std::unique(fids.begin(), fids.end()); + Fiducials::iterator it = std::unique(fids.begin(), fids.end()); fids.resize(std::distance(fids.begin(), it)); return PLUS_SUCCESS; } //---------------------------------------------------------------------------- -PlusStatus PerformBackgroundSubtraction(fidsFrame& backgroundFids, fidsFrame& dataFids) +PlusStatus CaptureInitialGeometry(Fiducials& backFids, Fiducials& fids) { - // subtract the backgroundFids from the dataFids - fidsFrame filteredDataFids; - fidsFrame::iterator backgroundIt, dataIt; - for (dataIt = begin(dataFids); dataIt != end(dataFids); dataIt++) + Fiducials fid3dFrame; + std::map events; // unused + uint64_t ts = 0; // unused + while (true) { - bool equalityFound = false; - for (backgroundIt = begin(backgroundFids); backgroundIt != end(backgroundFids); backgroundIt++) + ATR_RESULT result = AtrTracker.GetFiducialsInFrame(fid3dFrame, events, ts); + if (result == ATR_SUCCESS) { - if (*dataIt == *backgroundIt) + int deter = 0; + // Remove those also part of the background + fid3dFrame.erase( + std::remove_if(fid3dFrame.begin(), fid3dFrame.end(), + [&backFids](const Fiducial& item) { + return std::find(backFids.begin(), backFids.end(), item) != backFids.end(); + }), + fid3dFrame.end() + ); + + // Skip frame if foreground fiducial number too small or too large + if (fid3dFrame.size() < 3 || fid3dFrame.size() > ATRACSYS_MAX_FIDUCIALS) + { + continue; + } + // Check that all fiducial probabilities are 1 + for (const auto& f : fid3dFrame) + { + deter += std::floor(f.probability); + } + if (deter == fid3dFrame.size()) { - equalityFound = true; + fids = fid3dFrame; + break; } } - if (!equalityFound) + else if (result == ATR_RESULT::ERROR_NO_FRAME_AVAILABLE) + { + continue; + } + else { - // element of dataFids is not in background, add to filteredDataFids - filteredDataFids.push_back(*dataIt); + LOG_ERROR(AtrTracker.ResultToString(result)); + return PLUS_FAIL; } } - // check that no data fiducials have non 1 probability - for (fidsFrame::size_type i = 0; i < filteredDataFids.size(); i++) + return PLUS_SUCCESS; +} + +//---------------------------------------------------------------------------- +void TransformMarkerCoordinateSystem(Fiducials& fids) +{ + // Start processing fids positions to determine marker coordinate system. + vec3 c(0.0, 0.0, 0.0); + // Calculate the centroid of fiducials, also the origin of the new coordinate system. + for (const auto& fid : fids) + { + c += vec3(fid); + } + c /= static_cast(fids.size()); + + // Calculate all fiducial-to-centroid distances and automatically store them in descending order. + std::multimap> dists; + for (int i = 0; i < fids.size(); ++i) { - if (filteredDataFids[i].probability != 1) + dists.insert({ (c - vec3(fids[i])).norm(), i }); + } + // Look for the largest *unique* distance from the centroid, the corresponding fiducial will + // define the x-axis. + vec3 x; + for (auto prev = dists.begin(), curr = ++dists.begin(); curr != dists.end(); ++prev, ++curr) + { + if (std::abs(prev->first - curr->first) > 0.1) { - LOG_ERROR("Fiducial with non 1 probability in data fiducials. Please retry marker creation ensuring that the marker is not moving and is in a good view position for the camera."); - return PLUS_FAIL; + auto fidId = (prev == dists.begin()) ? prev->second : curr->second; + x = vec3(fids[fidId]); + break; + } + } + // Calculate all fiducial-to-x-axis distances and automatically store them in descending order. + std::multimap> dists2; + for (int i = 0; i < fids.size(); ++i) + { + double num = ((x - vec3(fids[i])).cross(x - c)).norm(); + dists2.insert({ num / (x - c).norm(), i }); + } + // Look for the largest *unique* distance from x-axis, it will define the plane of the y-axis. + vec3 p; + 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]); + break; } } + // Calculate z-axis and y-axis. + vec3 vx = (x - c).normalize(); + vec3 vz = (vx.cross(p - c)).normalize(); + vec3 vy = vz.cross(vx); + + // Store the coordinates of each fiducial in the new coordinate system. + for (auto& fid : fids) + { + Fiducial f = fid; + fid.xMm = (vec3(f) - c) * vx; + fid.yMm = (vec3(f) - c) * vy; + fid.zMm = (vec3(f) - c) * vz; + } +} + +//---------------------------------------------------------------------------- +std::string WriteGeometryToString(const CustomGeometry& geom) +{ + std::ostringstream os; + + // write metadata + os << "; " << geom.name << std::endl; + os << "; " << geom.description << std::endl; + auto t = std::time(nullptr); + auto tm = *std::localtime(&t); + os << "; " << std::put_time(&tm, "%Y-%m-%d %H:%M:%S") << std::endl; + + // write geometry + os << "[geometry]" << std::endl; + os << "count=" << geom.fids.size() << std::endl; + os << "id=" << geom.geometryId << std::endl; + + for (Fiducials::size_type i = 0; i < geom.fids.size(); i++) + { + os << "[fiducial" << i << "]" << std::endl; + os << "x=" << geom.fids[i].xMm << std::endl; + os << "y=" << geom.fids[i].yMm << std::endl; + os << "z=" << geom.fids[i].zMm << std::endl; + } + + return os.str(); +} + +//---------------------------------------------------------------------------- +PlusStatus CollectMarkerSequence(CustomGeometry& geom, MarkerSequence& markerFrameList, int numFrames) +{ + std::string geoStr = WriteGeometryToString(geom); + // Load initial marker for tracking + int gid; // unused + if (AtrTracker.LoadMarkerGeometryFromString(geoStr, gid) != Atracsys::Tracker::SUCCESS) + { + LOG_ERROR("Unable to load initial marker."); + return PLUS_FAIL; + } - // copy filteredDataFids back to dataFids - dataFids.clear(); - dataFids = filteredDataFids; + int m = 0; + ProgressBar(m, numFrames); + std::vector markersFrame; + std::map events; // unused + uint64_t ts = 0; // unused + while (m < numFrames) + { + ATR_RESULT result = AtrTracker.GetMarkersInFrame(markersFrame, events, ts); + if (result == ATR_SUCCESS) + { + if (markersFrame.size() == 1) // make sure there is only one marker tracked + { + ProgressBar(++m, numFrames); + markerFrameList.push_back(markersFrame.front()); + } + } + else if (result == ATR_RESULT::ERROR_NO_FRAME_AVAILABLE) + { + continue; + } + else + { + LOG_ERROR(AtrTracker.ResultToString(result)); + return PLUS_FAIL; + } + } return PLUS_SUCCESS; } //---------------------------------------------------------------------------- -PlusStatus ZeroMeanFids(fidsFrame& dataFids) +PlusStatus RefineMarkerGeometry(CustomGeometry& geom, MarkerSequence& markerFrameList) { - // zero-mean the fiducials - fidsFrame zeroMeanFids; - float cumulativeXmm = 0, cumulativeYmm = 0, cumulativeZmm = 0; - fidsFrame::const_iterator it; - for (it = begin(dataFids); it != end(dataFids); it++) - { - cumulativeXmm += it->xMm; - cumulativeYmm += it->yMm; - cumulativeZmm += it->zMm; - } - float aveXmm = cumulativeXmm /= dataFids.size(); - float aveYmm = cumulativeYmm /= dataFids.size(); - float aveZmm = cumulativeZmm /= dataFids.size(); - for (it = begin(dataFids); it != end(dataFids); it++) - { - zeroMeanFids.emplace_back(); - zeroMeanFids.back().xMm = it->xMm - aveXmm; - zeroMeanFids.back().yMm = it->yMm - aveYmm; - zeroMeanFids.back().zMm = it->zMm - aveZmm; - zeroMeanFids.back().probability = it->probability; - } - dataFids = zeroMeanFids; + Fiducials accFids(geom.fids.size()); + for (int m = 0; m < markerFrameList.size(); ++m) + { + Fiducials fids = markerFrameList[m].GetFiducials(); + TransformMarkerCoordinateSystem(fids); + // Accumulate fids coordinates for averaging + for (int i = 0; i < fids.size(); ++i) + { + accFids[i].xMm += fids[i].xMm; + accFids[i].yMm += fids[i].yMm; + accFids[i].zMm += fids[i].zMm; + } + } + + // Complete the averaging and output the result + for (int i = 0; i < accFids.size(); ++i) + { + accFids[i].xMm = remDecimals(accFids[i].xMm / markerFrameList.size(), 1e-4); + accFids[i].yMm = remDecimals(accFids[i].yMm / markerFrameList.size(), 1e-4); + accFids[i].zMm = remDecimals(accFids[i].zMm / markerFrameList.size(), 1e-4); + } + + geom.fids = accFids; + return PLUS_SUCCESS; } //---------------------------------------------------------------------------- -PlusStatus WriteGeometryIniFile(const MarkerGeometry geom) +PlusStatus WriteGeometryToIniFile(const CustomGeometry& geom) { // create file path std::string fileName; @@ -336,32 +636,8 @@ PlusStatus WriteGeometryIniFile(const MarkerGeometry geom) LOG_INFO("Writing marker geometry to: " << fileName); std::ofstream file; file.open(fileName); - - // write metadata - file << ";; " << geom.name << std::endl; - file << ";; " << geom.description << std::endl; - auto t = std::time(nullptr); - auto tm = *std::localtime(&t); - file << ";; " << std::put_time(&tm, "%Y-%m-%d %H:%M:%S") << std::endl; - - // write geometry - file << "[geometry]" << std::endl; - file << "count=" << geom.fids.size() << std::endl; - file << "id=" << geom.geometryId << std::endl; - - for (fidsFrame::size_type i = 0; i < geom.fids.size(); i++) - { - file << "[fiducial" << i << "]" << std::endl; - file << "x=" << geom.fids[i].xMm << std::endl; - file << "y=" << geom.fids[i].yMm << std::endl; - file << "z=" << geom.fids[i].zMm << std::endl; - } - - file << "[pivot]" << std::endl; - file << "x=0.0000" << std::endl; - file << "y=0.0000" << std::endl; - file << "z=0.0000" << std::endl; - + file << WriteGeometryToString(geom); file.close(); + LOG_INFO("Done."); return PLUS_SUCCESS; -} +} \ No newline at end of file diff --git a/src/PlusDataCollection/Atracsys/AtracsysTracker.cxx b/src/PlusDataCollection/Atracsys/AtracsysTracker.cxx index b075f72c0..a8ebec0a6 100644 --- a/src/PlusDataCollection/Atracsys/AtracsysTracker.cxx +++ b/src/PlusDataCollection/Atracsys/AtracsysTracker.cxx @@ -28,7 +28,24 @@ See License.txt for details. #define RESET_DROPPED_FRAME_COUNT 1 //---------------------------------------------------------------------------- -bool strToInt32(const std::string& str, int& var) +// this method from https://thispointer.com/find-and-replace-all-occurrences-of-a-sub-string-in-c/ +void stringFindAndReplaceAll(std::string& data, std::string toSearch, std::string replaceStr) +{ + // Get the first occurrence + size_t pos = data.find(toSearch); + + // Repeat till end is reached + while (pos != std::string::npos) + { + // Replace this occurrence of Sub String + data.replace(pos, toSearch.size(), replaceStr); + // Get the next occurrence from the current position + pos = data.find(toSearch, pos + replaceStr.size()); + } +} + +//---------------------------------------------------------------------------- +bool Atracsys::strToInt32(const std::string& str, int& var) { bool noexception = false; try @@ -48,7 +65,7 @@ bool strToInt32(const std::string& str, int& var) } //---------------------------------------------------------------------------- -bool strToFloat32(const std::string& str, float& var) +bool Atracsys::strToFloat32(const std::string& str, float& var) { bool noexception = false; try @@ -67,98 +84,10 @@ bool strToFloat32(const std::string& str, float& var) return noexception; } -//---------------------------------------------------------------------------- -class AtracsysTracker::AtracsysInternal +//============================================================================== +namespace Geometry { -public: - AtracsysInternal() - { - // populate result to string - ResultToStringMap[ERROR_UNABLE_TO_GET_FTK_HANDLE] = "Unable to get Atracsys library handle."; - ResultToStringMap[ERROR_NO_DEVICE_CONNECTED] = "No Atracsys device connected."; - ResultToStringMap[ERROR_UNABLE_TO_LOAD_MARKER] = "Unable to load marker."; - ResultToStringMap[ERROR_FAILURE_TO_LOAD_INI] = "Failed to load marker's ini file."; - ResultToStringMap[ERROR_OPTION_AVAILABLE_ONLY_ON_FTK] = "Attempted to call fusionTrack only option with non-fusionTrack device connected."; - ResultToStringMap[ERROR_OPTION_AVAILABLE_ONLY_ON_STK] = "Attempted to call spryTrack only option with non-spryTrack device connected."; - ResultToStringMap[ERROR_FAILED_TO_CLOSE_SDK] = "Failed to close the Atracsys SDK."; - ResultToStringMap[ERROR_FAILED_TO_EXPORT_CALIB] = "Failed to export cameras calibration."; - ResultToStringMap[ERROR_FAILED_TO_EXTRACT_FRAME_INFO] = "Failed to extract frame info."; - ResultToStringMap[ERROR_CANNOT_CREATE_FRAME_INSTANCE] = "Failed to create frame."; - ResultToStringMap[ERROR_CANNOT_INITIALIZE_FRAME] = "Failed to initialize frame."; - ResultToStringMap[ERROR_NO_FRAME_AVAILABLE] = "No frame available from tracker."; - ResultToStringMap[ERROR_INVALID_FRAME] = "Invalid frame received from tracker."; - ResultToStringMap[ERROR_TOO_MANY_MARKERS] = "Too many markers in frame."; - ResultToStringMap[ERROR_ENABLE_LASER] = "Failed to enable laser, this is a spryTrack only option."; - ResultToStringMap[ERROR_SET_USER_LED] = "Failed to set the user LED."; - ResultToStringMap[ERROR_ENABLE_USER_LED] = "Failed to enable / disable the user LED."; - ResultToStringMap[ERROR_ENABLE_IMAGE_STREAMING] = "Failed to enable / disable image streaming."; - ResultToStringMap[ERROR_ENABLE_WIRELESS_MARKER_PAIRING] = "Failed to enable / disable wireless marker pairing."; - ResultToStringMap[ERROR_ENABLE_WIRELESS_MARKER_STATUS_STREAMING] = "Failed to enable / disable wireless marker status streaming."; - ResultToStringMap[ERROR_ENABLE_WIRELESS_MARKER_BATTERY_STREAMING] = "Failed to enable / disable wireless marker battery streaming."; - ResultToStringMap[ERROR_DISCONNECT_ATTEMPT_WHEN_NOT_CONNECTED] = "Disconnect called when not connected to tracker."; - ResultToStringMap[ERROR_CANNOT_GET_MARKER_INFO] = "Cannot get info about paired wireless markers."; - ResultToStringMap[ERROR_FAILED_TO_SET_STK_PROCESSING_TYPE] = "Failed to set spryTrack image processing type."; - } - - virtual ~AtracsysInternal() - { - FtkLib = nullptr; - LibVersion = ""; - CalibrationDate = ""; - TrackerSN = 0; - } - - // is virtual device or not - bool isVirtual = false; - - // is paused or not - bool isPaused = true; - - // handle to FtkLib library - ftkLibrary FtkLib = nullptr; - - // library version - std::string LibVersion; - - // calibration date - std::string CalibrationDate; - - // serial number of tracker - uint64 TrackerSN = 0; - - // ftk frame data - ftkFrameQuery* Frame = nullptr; - - // mapping error code to user readable result string - std::map ResultToStringMap; - - // helper function to load ftkGeometry from file - ATRACSYS_RESULT LoadFtkGeometryFromFile(const std::string& filename, ftkGeometry& geom); - - // helper function to load ftkGeometry from string - ATRACSYS_RESULT LoadFtkGeometryFromString(const std::string& geomString, ftkGeometry& geom); - - std::map>> Geometries; - - // correspondence between atracsys option name and its actual id in the sdk - // this map is filled automatically by the sdk, DO NOT hardcode/change any id - std::map DeviceOptionMap{}; - - //---------------------------------------------------------------------------- - // callback function stores all option id - static void DeviceOptionEnumerator(uint64_t serialNumber, void* userData, ftkOptionsInfo* option) - { - AtracsysTracker::AtracsysInternal* ptr = - reinterpret_cast(userData); - if (!ptr) - { - return; - } - ptr->DeviceOptionMap.emplace(option->name, *option); - } - - // Code from ATRACSYS - class IniFile + class IniFile // Based on code from ATRACSYS { protected: //---------------------------------------------------------------------------- @@ -234,15 +163,7 @@ class AtracsysTracker::AtracsysInternal if (lineSize != 0) { - if (lineSize > 0) - { - strLine = std::string(addr, lineSize); - } - else - { - strLine = std::string(addr); - } - + strLine = (lineSize > 0) ? std::string(addr, lineSize) : std::string(addr); strLine.erase(remove(strLine.begin(), strLine.end(), '\r'), strLine.end()); if (!parseLine(strLine)) { @@ -364,8 +285,7 @@ class AtracsysTracker::AtracsysInternal } //---------------------------------------------------------------------------- - bool assignUint32(IniFile& p, const std::string& section, - const std::string& key, + bool assignUint32(IniFile& p, const std::string& section, const std::string& key, uint32* variable) { if (!checkKey(p, section, key)) @@ -382,8 +302,7 @@ class AtracsysTracker::AtracsysInternal } //---------------------------------------------------------------------------- - bool assignFloatXX(IniFile& p, const std::string& section, - const std::string& key, + bool assignFloatXX(IniFile& p, const std::string& section, const std::string& key, floatXX* variable) { if (!checkKey(p, section, key)) @@ -398,6 +317,99 @@ class AtracsysTracker::AtracsysInternal return true; } +} // end of Geometry namespace + +using namespace Atracsys; + +//============================================================================== +class Tracker::Internal +{ +public: + Internal() + { + // populate result to string + ResultToStringMap[ERROR_UNABLE_TO_GET_FTK_HANDLE] = "Unable to get Atracsys library handle."; + ResultToStringMap[ERROR_NO_DEVICE_CONNECTED] = "No Atracsys device connected."; + ResultToStringMap[ERROR_UNABLE_TO_LOAD_MARKER] = "Unable to load marker."; + ResultToStringMap[ERROR_FAILURE_TO_LOAD_INI] = "Failed to load marker's ini file."; + ResultToStringMap[ERROR_OPTION_AVAILABLE_ONLY_ON_FTK] = "Attempted to call fusionTrack only option with non-fusionTrack device connected."; + ResultToStringMap[ERROR_OPTION_AVAILABLE_ONLY_ON_STK] = "Attempted to call spryTrack only option with non-spryTrack device connected."; + ResultToStringMap[ERROR_FAILED_TO_CLOSE_SDK] = "Failed to close the Atracsys SDK."; + ResultToStringMap[ERROR_FAILED_TO_EXPORT_CALIB] = "Failed to export cameras calibration."; + ResultToStringMap[ERROR_FAILED_TO_EXTRACT_FRAME_INFO] = "Failed to extract frame info."; + ResultToStringMap[ERROR_CANNOT_CREATE_FRAME_INSTANCE] = "Failed to create frame."; + ResultToStringMap[ERROR_CANNOT_INITIALIZE_FRAME] = "Failed to initialize frame."; + ResultToStringMap[ERROR_NO_FRAME_AVAILABLE] = "No frame available from tracker."; + ResultToStringMap[ERROR_INVALID_FRAME] = "Invalid frame received from tracker."; + ResultToStringMap[ERROR_TOO_MANY_MARKERS] = "Too many markers in frame."; + ResultToStringMap[ERROR_ENABLE_LASER] = "Failed to enable laser, this is a spryTrack only option."; + ResultToStringMap[ERROR_SET_USER_LED] = "Failed to set the user LED."; + ResultToStringMap[ERROR_ENABLE_USER_LED] = "Failed to enable / disable the user LED."; + ResultToStringMap[ERROR_ENABLE_IMAGE_STREAMING] = "Failed to enable / disable image streaming."; + ResultToStringMap[ERROR_ENABLE_WIRELESS_MARKER_PAIRING] = "Failed to enable / disable wireless marker pairing."; + ResultToStringMap[ERROR_ENABLE_WIRELESS_MARKER_STATUS_STREAMING] = "Failed to enable / disable wireless marker status streaming."; + ResultToStringMap[ERROR_ENABLE_WIRELESS_MARKER_BATTERY_STREAMING] = "Failed to enable / disable wireless marker battery streaming."; + ResultToStringMap[ERROR_DISCONNECT_ATTEMPT_WHEN_NOT_CONNECTED] = "Disconnect called when not connected to tracker."; + ResultToStringMap[ERROR_CANNOT_GET_MARKER_INFO] = "Cannot get info about paired wireless markers."; + ResultToStringMap[ERROR_FAILED_TO_SET_STK_PROCESSING_TYPE] = "Failed to set spryTrack image processing type."; + } + + virtual ~Internal() + { + FtkLib = nullptr; + LibVersion = ""; + CalibrationDate = ""; + TrackerSN = 0; + } + + // is virtual device or not + bool isVirtual = false; + + // is paused or not + bool isPaused = true; + + // handle to FtkLib library + ftkLibrary FtkLib = nullptr; + + // library version + std::string LibVersion; + + // calibration date + std::string CalibrationDate; + + // serial number of tracker + uint64 TrackerSN = 0; + + // ftk frame data + ftkFrameQuery* Frame = nullptr; + + // mapping error code to user readable result string + std::map ResultToStringMap; + + // helper function to load ftkGeometry from file + RESULT LoadFtkGeometryFromFile(const std::string& filename, ftkGeometry& geom); + + // helper function to load ftkGeometry from string + RESULT LoadFtkGeometryFromString(const std::string& geomString, ftkGeometry& geom); + + std::map>> Geometries; + + // correspondence between atracsys option name and its actual id in the sdk + // this map is filled automatically by the sdk, DO NOT hardcode/change any id + std::map DeviceOptionMap{}; + + //---------------------------------------------------------------------------- + // callback function stores all option id + static void DeviceOptionEnumerator(uint64_t serialNumber, void* userData, ftkOptionsInfo* option) + { + Tracker::Internal* ptr = + reinterpret_cast(userData); + if (!ptr) + { + return; + } + ptr->DeviceOptionMap.emplace(option->name, *option); + } //---------------------------------------------------------------------------- bool LoadIniFile(std::ifstream& is, ftkGeometry& geometry) @@ -413,29 +425,12 @@ class AtracsysTracker::AtracsysInternal return ParseIniFile(fileContent, geometry); } - //---------------------------------------------------------------------------- - // this method from https://thispointer.com/find-and-replace-all-occurrences-of-a-sub-string-in-c/ - void stringFindAndReplaceAll(std::string& data, std::string toSearch, std::string replaceStr) - { - // Get the first occurrence - size_t pos = data.find(toSearch); - - // Repeat till end is reached - while (pos != std::string::npos) - { - // Replace this occurrence of Sub String - data.replace(pos, toSearch.size(), replaceStr); - // Get the next occurrence from the current position - pos = data.find(toSearch, pos + replaceStr.size()); - } - } - //---------------------------------------------------------------------------- bool ParseIniFile(std::string fileContent, ftkGeometry& geometry) { stringFindAndReplaceAll(fileContent, "\\n", "\n"); - IniFile parser; + Geometry::IniFile parser; if (!parser.parse(const_cast(fileContent.c_str()), fileContent.size())) @@ -499,7 +494,7 @@ class AtracsysTracker::AtracsysInternal return true; } -}; +}; // end of Tracker::Internal //---------------------------------------------------------------------------- struct DeviceData @@ -520,7 +515,7 @@ void FusionTrackEnumerator(uint64 sn, void* user, ftkDeviceType devType) } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::AtracsysInternal::LoadFtkGeometryFromFile(const std::string& filename, ftkGeometry& geom) +Tracker::RESULT Tracker::Internal::LoadFtkGeometryFromFile(const std::string& filename, ftkGeometry& geom) { std::ifstream input; input.open(filename.c_str()); @@ -551,10 +546,9 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::AtracsysInternal::LoadFtkGeome return ERROR_FAILURE_TO_LOAD_INI; } -// END CODE FROM ATRACSYS //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::AtracsysInternal::LoadFtkGeometryFromString(const std::string& geomString, ftkGeometry& geom) +Tracker::RESULT Tracker::Internal::LoadFtkGeometryFromString(const std::string& geomString, ftkGeometry& geom) { if (this->ParseIniFile(geomString, geom)) { @@ -567,10 +561,10 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::AtracsysInternal::LoadFtkGeome // provided an option name with Atracsys' nomenclature, this method returns the pointer // to the corresponding ftkOptionsInfo which contains various information about the option // (notably its id and value type) -bool AtracsysTracker::GetOptionInfo(const std::string& optionName, const ftkOptionsInfo*& info) +bool Tracker::GetOptionInfo(const std::string& optionName, const ftkOptionsInfo*& info) { - std::map::const_iterator it = this->Internal->DeviceOptionMap.find(optionName); - if (it == this->Internal->DeviceOptionMap.cend()) + std::map::const_iterator it = this->InternalObj->DeviceOptionMap.find(optionName); + if (it == this->InternalObj->DeviceOptionMap.cend()) { return false; } @@ -583,16 +577,16 @@ bool AtracsysTracker::GetOptionInfo(const std::string& optionName, const ftkOpti //---------------------------------------------------------------------------- // this method sets a value to an option in the device. The option name follows Atracsys' nomenclature. -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetOption(const std::string& optionName, const std::string& attributeValue) +Tracker::RESULT Tracker::SetOption(const std::string& optionName, const std::string& attributeValue) { - if (this->Internal->isVirtual) + if (this->InternalObj->isVirtual) { return SUCCESS; } std::string optionStr{ optionName }; // if Embedded processing is on and the option has an Embedded variant, add the prefix - if (isOnboardProcessing && this->Internal->DeviceOptionMap.find("Embedded " + optionName) != this->Internal->DeviceOptionMap.end()) + if (isOnboardProcessing && this->InternalObj->DeviceOptionMap.find("Embedded " + optionName) != this->InternalObj->DeviceOptionMap.end()) { optionStr = "Embedded " + optionStr; } @@ -613,10 +607,10 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetOption(const std::string& o return ERROR_SET_OPTION; } - if (ftkSetInt32(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, val) != ftkError::FTK_OK) + if (ftkSetInt32(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, val) != ftkError::FTK_OK) { ftkBuffer buffer{}; - if (ftkGetLastErrorString(this->Internal->FtkLib, sizeof(buffer.data), buffer.data) == ftkError::FTK_OK) + if (ftkGetLastErrorString(this->InternalObj->FtkLib, sizeof(buffer.data), buffer.data) == ftkError::FTK_OK) { LOG_WARNING(std::string(buffer.data)); } @@ -634,10 +628,10 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetOption(const std::string& o return ERROR_SET_OPTION; } - if (ftkSetFloat32(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, val) != ftkError::FTK_OK) + if (ftkSetFloat32(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, val) != ftkError::FTK_OK) { ftkBuffer buffer{}; - if (ftkGetLastErrorString(this->Internal->FtkLib, sizeof(buffer.data), buffer.data) == ftkError::FTK_OK) + if (ftkGetLastErrorString(this->InternalObj->FtkLib, sizeof(buffer.data), buffer.data) == ftkError::FTK_OK) { LOG_WARNING(std::string(buffer.data)); } @@ -660,46 +654,46 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetOption(const std::string& o // universally available options & methods // ------------------------------------------ -AtracsysTracker::AtracsysTracker() - : Internal(new AtracsysInternal()) {} +Tracker::Tracker() + : InternalObj(new Internal()) {} -AtracsysTracker::~AtracsysTracker() +Tracker::~Tracker() { - delete Internal; - Internal = nullptr; + delete InternalObj; + InternalObj = nullptr; } //---------------------------------------------------------------------------- -void AtracsysTracker::Pause(bool tof) +void Tracker::Pause(bool tof) { - this->Internal->isPaused = tof; + this->InternalObj->isPaused = tof; } //---------------------------------------------------------------------------- -bool AtracsysTracker::IsOnboardProcessing() +bool Tracker::IsOnboardProcessing() { return isOnboardProcessing; } //---------------------------------------------------------------------------- -bool AtracsysTracker::IsVirtual() +bool Tracker::IsVirtual() { - return this->Internal->isVirtual; + return this->InternalObj->isVirtual; } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::Connect() +Tracker::RESULT Tracker::Connect() { - if (this->Internal->FtkLib != nullptr && this->Internal->TrackerSN != 0) + if (this->InternalObj->FtkLib != nullptr && this->InternalObj->TrackerSN != 0) { // already connected return SUCCESS; } // initialize SDK - this->Internal->FtkLib = ftkInit(); + this->InternalObj->FtkLib = ftkInit(); - if (this->Internal->FtkLib == NULL) + if (this->InternalObj->FtkLib == NULL) { return ERROR_UNABLE_TO_GET_FTK_HANDLE; } @@ -708,26 +702,26 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::Connect() device.SerialNumber = 0uLL; // scan for devices - ftkError err = ftkEnumerateDevices(this->Internal->FtkLib, FusionTrackEnumerator, &device); + ftkError err = ftkEnumerateDevices(this->InternalObj->FtkLib, FusionTrackEnumerator, &device); if (err != ftkError::FTK_OK && err != ftkError::FTK_WAR_USB_TOO_SLOW) { - ftkClose(&this->Internal->FtkLib); - this->Internal->FtkLib = nullptr; + ftkClose(&this->InternalObj->FtkLib); + this->InternalObj->FtkLib = nullptr; return ERROR_NO_DEVICE_CONNECTED; } if (device.SerialNumber == 0uLL) { - ftkClose(&this->Internal->FtkLib); - this->Internal->FtkLib = nullptr; + ftkClose(&this->InternalObj->FtkLib); + this->InternalObj->FtkLib = nullptr; return ERROR_NO_DEVICE_CONNECTED; } - this->Internal->TrackerSN = device.SerialNumber; + this->InternalObj->TrackerSN = device.SerialNumber; ftkBuffer sdkVersion; ftkVersion(&sdkVersion); - this->Internal->LibVersion = sdkVersion.data; + this->InternalObj->LibVersion = sdkVersion.data; switch (device.Type) { @@ -748,28 +742,28 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::Connect() } // allocate memory for ftk frame to be used throughout life of the object - this->Internal->Frame = ftkCreateFrame(); + this->InternalObj->Frame = ftkCreateFrame(); - if (this->Internal->Frame == nullptr) + if (this->InternalObj->Frame == nullptr) { - ftkDeleteFrame(this->Internal->Frame); - this->Internal->Frame = nullptr; + ftkDeleteFrame(this->InternalObj->Frame); + this->InternalObj->Frame = nullptr; return ERROR_CANNOT_CREATE_FRAME_INSTANCE; } if (ftkSetFrameOptions(false, this->MaxAdditionalEventsNumber, this->Max2dFiducialsNumber, this->Max2dFiducialsNumber, this->Max3dFiducialsNumber, this->MaxMarkersNumber, - this->Internal->Frame) != ftkError::FTK_OK) + this->InternalObj->Frame) != ftkError::FTK_OK) { - ftkDeleteFrame(this->Internal->Frame); - this->Internal->Frame = nullptr; + ftkDeleteFrame(this->InternalObj->Frame); + this->InternalObj->Frame = nullptr; return ERROR_CANNOT_INITIALIZE_FRAME; } - if (ftkEnumerateOptions(this->Internal->FtkLib, this->Internal->TrackerSN, - &AtracsysTracker::AtracsysInternal::DeviceOptionEnumerator, this->Internal) != ftkError::FTK_OK - || this->Internal->DeviceOptionMap.find("Data Directory") == this->Internal->DeviceOptionMap.cend()) + if (ftkEnumerateOptions(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, + &Tracker::Internal::DeviceOptionEnumerator, this->InternalObj) != ftkError::FTK_OK + || this->InternalObj->DeviceOptionMap.find("Data Directory") == this->InternalObj->DeviceOptionMap.cend()) { return ERROR_OPTION_NOT_FOUND; } @@ -782,8 +776,8 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::Connect() return ERROR_OPTION_NOT_FOUND; } ftkBuffer buff; - ftkGetData(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, &buff); - this->Internal->CalibrationDate = std::string(buff.data); + ftkGetData(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, &buff); + this->InternalObj->CalibrationDate = std::string(buff.data); // Check whether onboard processing is off or on (spryTrack only) if (this->DeviceType == SPRYTRACK_180 || this->DeviceType == SPRYTRACK_300) @@ -796,7 +790,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::Connect() else { int32 val; - ftkGetInt32(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, &val, ftkOptionGetter::FTK_VALUE); + ftkGetInt32(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, &val, ftkOptionGetter::FTK_VALUE); isOnboardProcessing = (val == 1) ? true : false; LOG_INFO("Embedded processing is initially " << (isOnboardProcessing ? "enabled" : "disabled")); } @@ -806,23 +800,23 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::Connect() } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::Disconnect() +Tracker::RESULT Tracker::Disconnect() { - if (this->Internal->isVirtual) + if (this->InternalObj->isVirtual) { return SUCCESS; } - if (this->Internal->FtkLib == nullptr && this->Internal->TrackerSN == 0) + if (this->InternalObj->FtkLib == nullptr && this->InternalObj->TrackerSN == 0) { return ERROR_DISCONNECT_ATTEMPT_WHEN_NOT_CONNECTED; } // de-allocate memory for frame - ftkDeleteFrame(this->Internal->Frame); - this->Internal->Frame = nullptr; + ftkDeleteFrame(this->InternalObj->Frame); + this->InternalObj->Frame = nullptr; - ftkError err = ftkClose(&this->Internal->FtkLib); + ftkError err = ftkClose(&this->InternalObj->FtkLib); if (err != ftkError::FTK_OK) { return ERROR_FAILED_TO_CLOSE_SDK; @@ -831,39 +825,39 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::Disconnect() } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetSDKversion(std::string& version) +Tracker::RESULT Tracker::GetSDKversion(std::string& version) { - version = this->Internal->LibVersion; + version = this->InternalObj->LibVersion; return SUCCESS; } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetCalibrationDate(std::string& date) +Tracker::RESULT Tracker::GetCalibrationDate(std::string& date) { - date = this->Internal->CalibrationDate; + date = this->InternalObj->CalibrationDate; return SUCCESS; } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetDeviceType(DEVICE_TYPE& deviceType) +Tracker::RESULT Tracker::GetDeviceType(DEVICE_TYPE& deviceType) { deviceType = this->DeviceType; return SUCCESS; } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetCamerasCalibration( +Tracker::RESULT Tracker::GetCamerasCalibration( std::array& leftIntrinsic, std::array& rightIntrinsic, std::array& rightPosition, std::array& rightOrientation) { - if (!this->Internal->isVirtual || this->SetOption("Calibration export", "1") != SUCCESS) + if (!this->InternalObj->isVirtual || this->SetOption("Calibration export", "1") != SUCCESS) { LOG_ERROR("Could not export calibration."); return ERROR_FAILED_TO_EXPORT_CALIB; } else { - if (ftkGetLastFrame(this->Internal->FtkLib, this->Internal->TrackerSN, this->Internal->Frame, 20) != ftkError::FTK_OK) + if (ftkGetLastFrame(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, this->InternalObj->Frame, 20) != ftkError::FTK_OK) { return ERROR_NO_FRAME_AVAILABLE; } @@ -871,7 +865,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetCamerasCalibration( { ftkFrameInfoData info; info.WantedInformation = ftkInformationType::CalibrationParameters; - if (ftkExtractFrameInfo(this->Internal->Frame, &info) != ftkError::FTK_OK) + if (ftkExtractFrameInfo(this->InternalObj->Frame, &info) != ftkError::FTK_OK) { return ERROR_FAILED_TO_EXTRACT_FRAME_INFO; } @@ -895,19 +889,19 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetCamerasCalibration( } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetDeviceId(uint64_t& id) +Tracker::RESULT Tracker::GetDeviceId(uint64_t& id) { - id = this->Internal->TrackerSN; + id = this->InternalObj->TrackerSN; return SUCCESS; } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::LoadMarkerGeometryFromFile(std::string filePath, int& geometryId) +Tracker::RESULT Tracker::LoadMarkerGeometryFromFile(std::string filePath, int& geometryId) { ftkGeometry geom; - this->Internal->LoadFtkGeometryFromFile(filePath, geom); - if (!this->Internal->isVirtual && - ftkSetGeometry(this->Internal->FtkLib, this->Internal->TrackerSN, &geom) != ftkError::FTK_OK) + this->InternalObj->LoadFtkGeometryFromFile(filePath, geom); + if (!this->InternalObj->isVirtual && + ftkSetGeometry(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, &geom) != ftkError::FTK_OK) { return ERROR_UNABLE_TO_LOAD_MARKER; } @@ -916,12 +910,12 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::LoadMarkerGeometryFromFile(std } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::LoadMarkerGeometryFromString(std::string geomString, int& geometryId) +Tracker::RESULT Tracker::LoadMarkerGeometryFromString(std::string geomString, int& geometryId) { ftkGeometry geom; - this->Internal->LoadFtkGeometryFromString(geomString, geom); - if (!this->Internal->isVirtual && - ftkSetGeometry(this->Internal->FtkLib, this->Internal->TrackerSN, &geom) != ftkError::FTK_OK) + this->InternalObj->LoadFtkGeometryFromString(geomString, geom); + if (!this->InternalObj->isVirtual && + ftkSetGeometry(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, &geom) != ftkError::FTK_OK) { return ERROR_UNABLE_TO_LOAD_MARKER; } @@ -930,9 +924,9 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::LoadMarkerGeometryFromString(s } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetMarkerInfo(std::string& markerInfo) +Tracker::RESULT Tracker::GetMarkerInfo(std::string& markerInfo) { - if (this->Internal->isVirtual) + if (this->InternalObj->isVirtual) { return ERROR_CANNOT_GET_MARKER_INFO; } @@ -945,7 +939,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetMarkerInfo(std::string& mar } ftkBuffer buffer; - if (ftkGetData(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, &buffer) != ftkError::FTK_OK) + if (ftkGetData(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, &buffer) != ftkError::FTK_OK) { return ERROR_CANNOT_GET_MARKER_INFO; } @@ -954,18 +948,18 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetMarkerInfo(std::string& mar } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetLoadedGeometries(std::map>>& geometries) +Tracker::RESULT Tracker::GetLoadedGeometries(std::map>>& geometries) { - geometries = this->Internal->Geometries; + geometries = this->InternalObj->Geometries; return SUCCESS; } //---------------------------------------------------------------------------- -std::string AtracsysTracker::ResultToString(AtracsysTracker::ATRACSYS_RESULT result) +std::string Tracker::ResultToString(Tracker::RESULT result) { - std::map::iterator it; - it = this->Internal->ResultToStringMap.find(result); - if (it != end(this->Internal->ResultToStringMap)) + std::map::iterator it; + it = this->InternalObj->ResultToStringMap.find(result); + if (it != std::end(this->InternalObj->ResultToStringMap)) { return it->second; } @@ -973,16 +967,105 @@ std::string AtracsysTracker::ResultToString(AtracsysTracker::ATRACSYS_RESULT res } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetMarkersInFrame(std::vector& markers, +Tracker::RESULT Tracker::GetFiducialsInFrame(std::vector& fiducials, + std::map& events, uint64_t& sdkTimestamp) +{ + ftkError err = ftkGetLastFrame(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, this->InternalObj->Frame, 20); + if (err != ftkError::FTK_OK) + { + return ERROR_NO_FRAME_AVAILABLE; + } + + switch (this->InternalObj->Frame->markersStat) + { + case ftkQueryStatus::QS_WAR_SKIPPED: + return ERROR_INVALID_FRAME; + case ftkQueryStatus::QS_ERR_INVALID_RESERVED_SIZE: + return ERROR_INVALID_FRAME; + case ftkQueryStatus::QS_OK: + break; + default: + return ERROR_INVALID_FRAME; + } + + if (this->InternalObj->Frame->markersStat == ftkQueryStatus::QS_ERR_OVERFLOW) + { + return ERROR_TOO_MANY_MARKERS; + } + + // make sure fiducials vector is empty before populating + fiducials.clear(); + + for (uint32 f = 0; f < this->InternalObj->Frame->threeDFiducialsCount; f++) + { + Fiducial fid; + // 3D stuff + const ftk3DFiducial& ftkFid3d = this->InternalObj->Frame->threeDFiducials[f]; + fid.Fid3dStatus = ftkFid3d.status; + fid.xMm = ftkFid3d.positionMM.x; + fid.yMm = ftkFid3d.positionMM.y; + fid.zMm = ftkFid3d.positionMM.z; + fid.epipolarErrorPx = ftkFid3d.epipolarErrorPixels; + fid.probability = ftkFid3d.probability; + fid.triangulErrorMm = ftkFid3d.triangulationErrorMM; + // Left 2D stuff + const ftkRawData& leftRaw = this->InternalObj->Frame->rawDataLeft[ftkFid3d.leftIndex]; + fid.Fid2dLeftStatus = leftRaw.status; + fid.xLeftPx = leftRaw.centerXPixels; + fid.yLeftPx = leftRaw.centerYPixels; + fid.heightLeftPx = leftRaw.height; + fid.widthLeftPx = leftRaw.width; + fid.pixCountLeft = leftRaw.pixelsCount; + // Right 2D stuff + const ftkRawData& rightRaw = this->InternalObj->Frame->rawDataRight[ftkFid3d.rightIndex]; + fid.Fid2dRightStatus = rightRaw.status; + fid.xRightPx = rightRaw.centerXPixels; + fid.yRightPx = rightRaw.centerYPixels; + fid.heightRightPx = rightRaw.height; + fid.widthRightPx = rightRaw.width; + fid.pixCountRight = rightRaw.pixelsCount; + + fiducials.push_back(fid); + } + + // make sure events map is empty before populating + events.clear(); + + // Parse events + for (size_t e = 0; e < this->InternalObj->Frame->eventsCount; e++) + { + const ftkEvent& event = *this->InternalObj->Frame->events[e]; + + if (event.Type == FtkEventType::fetTempV4) + { + std::stringstream ss; + const EvtTemperatureV4Payload* ptr = reinterpret_cast(event.Data); + for (unsigned int i = 0; i < event.Payload / sizeof(EvtTemperatureV4Payload) - 1; i++, ++ptr) + { + ss << ptr->SensorId << " " << ptr->SensorValue << " "; + } + ss << ptr->SensorId << " " << ptr->SensorValue; + events.emplace("tempv4", ss.str()); + } + } + + // Save sdk timestamp + sdkTimestamp = this->InternalObj->Frame->imageHeader->timestampUS; + + return SUCCESS; +} + +//---------------------------------------------------------------------------- +Tracker::RESULT Tracker::GetMarkersInFrame(std::vector& markers, std::map& events, uint64_t& sdkTimestamp) { - ftkError err = ftkGetLastFrame(this->Internal->FtkLib, this->Internal->TrackerSN, this->Internal->Frame, 20); + ftkError err = ftkGetLastFrame(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, this->InternalObj->Frame, 20); if (err != ftkError::FTK_OK) { return ERROR_NO_FRAME_AVAILABLE; } - switch (this->Internal->Frame->markersStat) + switch (this->InternalObj->Frame->markersStat) { case ftkQueryStatus::QS_WAR_SKIPPED: return ERROR_INVALID_FRAME; @@ -994,7 +1077,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetMarkersInFrame(std::vector< return ERROR_INVALID_FRAME; } - if (this->Internal->Frame->markersStat == ftkQueryStatus::QS_ERR_OVERFLOW) + if (this->InternalObj->Frame->markersStat == ftkQueryStatus::QS_ERR_OVERFLOW) { return ERROR_TOO_MANY_MARKERS; } @@ -1002,9 +1085,9 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetMarkersInFrame(std::vector< // make sure markers vector is empty before populating markers.clear(); - for (size_t m = 0; m < this->Internal->Frame->markersCount; m++) + for (size_t m = 0; m < this->InternalObj->Frame->markersCount; m++) { - const ftkMarker& marker = this->Internal->Frame->markers[m]; + const ftkMarker& marker = this->InternalObj->Frame->markers[m]; // A marker vtkNew toolToTracker; @@ -1032,7 +1115,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetMarkersInFrame(std::vector< { Fiducial fid; // 3D stuff - const ftk3DFiducial& ftkFid3d = this->Internal->Frame->threeDFiducials[f]; + const ftk3DFiducial& ftkFid3d = this->InternalObj->Frame->threeDFiducials[f]; fid.Fid3dStatus = ftkFid3d.status; fid.xMm = ftkFid3d.positionMM.x; fid.yMm = ftkFid3d.positionMM.y; @@ -1041,7 +1124,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetMarkersInFrame(std::vector< fid.probability = ftkFid3d.probability; fid.triangulErrorMm = ftkFid3d.triangulationErrorMM; // Left 2D stuff - const ftkRawData& leftRaw = this->Internal->Frame->rawDataLeft[ftkFid3d.leftIndex]; + const ftkRawData& leftRaw = this->InternalObj->Frame->rawDataLeft[ftkFid3d.leftIndex]; fid.Fid2dLeftStatus = leftRaw.status; fid.xLeftPx = leftRaw.centerXPixels; fid.yLeftPx = leftRaw.centerYPixels; @@ -1049,7 +1132,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetMarkersInFrame(std::vector< fid.widthLeftPx = leftRaw.width; fid.pixCountLeft = leftRaw.pixelsCount; // Right 2D stuff - const ftkRawData& rightRaw = this->Internal->Frame->rawDataRight[ftkFid3d.rightIndex]; + const ftkRawData& rightRaw = this->InternalObj->Frame->rawDataRight[ftkFid3d.rightIndex]; fid.Fid2dRightStatus = rightRaw.status; fid.xRightPx = rightRaw.centerXPixels; fid.yRightPx = rightRaw.centerYPixels; @@ -1070,9 +1153,9 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetMarkersInFrame(std::vector< events.clear(); // Parse events - for (size_t e = 0; e < this->Internal->Frame->eventsCount; e++) + for (size_t e = 0; e < this->InternalObj->Frame->eventsCount; e++) { - const ftkEvent& event = *this->Internal->Frame->events[e]; + const ftkEvent& event = *this->InternalObj->Frame->events[e]; if (event.Type == FtkEventType::fetTempV4) { @@ -1088,15 +1171,15 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetMarkersInFrame(std::vector< } // Save sdk timestamp - sdkTimestamp = this->Internal->Frame->imageHeader->timestampUS; + sdkTimestamp = this->InternalObj->Frame->imageHeader->timestampUS; return SUCCESS; } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetUserLEDState(int red, int green, int blue, int frequency, bool enabled /* = true */) +Tracker::RESULT Tracker::SetUserLEDState(int red, int green, int blue, int frequency, bool enabled /* = true */) { - if (this->Internal->isVirtual) + if (this->InternalObj->isVirtual) { return SUCCESS; } @@ -1107,7 +1190,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetUserLEDState(int red, int g { return ERROR_OPTION_NOT_FOUND; } - if (ftkSetInt32(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, frequency) != ftkError::FTK_OK) + if (ftkSetInt32(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, frequency) != ftkError::FTK_OK) { return ERROR_SET_USER_LED; } @@ -1115,7 +1198,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetUserLEDState(int red, int g { return ERROR_OPTION_NOT_FOUND; } - if (ftkSetInt32(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, red) != ftkError::FTK_OK) + if (ftkSetInt32(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, red) != ftkError::FTK_OK) { return ERROR_SET_USER_LED; } @@ -1123,7 +1206,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetUserLEDState(int red, int g { return ERROR_OPTION_NOT_FOUND; } - if (ftkSetInt32(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, green) != ftkError::FTK_OK) + if (ftkSetInt32(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, green) != ftkError::FTK_OK) { return ERROR_SET_USER_LED; } @@ -1131,7 +1214,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetUserLEDState(int red, int g { return ERROR_OPTION_NOT_FOUND; } - if (ftkSetInt32(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, blue) != ftkError::FTK_OK) + if (ftkSetInt32(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, blue) != ftkError::FTK_OK) { return ERROR_SET_USER_LED; } @@ -1140,9 +1223,9 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetUserLEDState(int red, int g } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableUserLED(bool enabled) +Tracker::RESULT Tracker::EnableUserLED(bool enabled) { - if (this->Internal->isVirtual) + if (this->InternalObj->isVirtual) { return SUCCESS; } @@ -1153,7 +1236,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableUserLED(bool enabled) { return ERROR_OPTION_NOT_FOUND; } - if (ftkSetInt32(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, enabled) != ftkError::FTK_OK) + if (ftkSetInt32(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, enabled) != ftkError::FTK_OK) { return ERROR_ENABLE_USER_LED; } @@ -1161,9 +1244,9 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableUserLED(bool enabled) } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetLaserEnabled(bool enabled) +Tracker::RESULT Tracker::SetLaserEnabled(bool enabled) { - if (this->Internal->isVirtual) + if (this->InternalObj->isVirtual) { return SUCCESS; } @@ -1177,7 +1260,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetLaserEnabled(bool enabled) int laserEnabledValue = enabled ? 3 : 0; // 3 = both lasers on - if (ftkSetInt32(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, laserEnabledValue) != ftkError::FTK_OK) + if (ftkSetInt32(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, laserEnabledValue) != ftkError::FTK_OK) { return ERROR_ENABLE_LASER; } @@ -1185,9 +1268,9 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetLaserEnabled(bool enabled) } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableWirelessMarkerPairing(bool enabled) +Tracker::RESULT Tracker::EnableWirelessMarkerPairing(bool enabled) { - if (this->Internal->isVirtual) + if (this->InternalObj->isVirtual) { return SUCCESS; } @@ -1199,7 +1282,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableWirelessMarkerPairing(bo return ERROR_OPTION_NOT_FOUND; } - if (ftkSetInt32(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, enabled) != ftkError::FTK_OK) + if (ftkSetInt32(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, enabled) != ftkError::FTK_OK) { return ERROR_ENABLE_WIRELESS_MARKER_PAIRING; } @@ -1207,9 +1290,9 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableWirelessMarkerPairing(bo } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableWirelessMarkerStatusStreaming(bool enabled) +Tracker::RESULT Tracker::EnableWirelessMarkerStatusStreaming(bool enabled) { - if (this->Internal->isVirtual) + if (this->InternalObj->isVirtual) { return SUCCESS; } @@ -1221,7 +1304,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableWirelessMarkerStatusStre return ERROR_OPTION_NOT_FOUND; } - if (ftkSetInt32(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, enabled) != ftkError::FTK_OK) + if (ftkSetInt32(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, enabled) != ftkError::FTK_OK) { return ERROR_ENABLE_WIRELESS_MARKER_STATUS_STREAMING; } @@ -1229,9 +1312,9 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableWirelessMarkerStatusStre } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableWirelessMarkerBatteryStreaming(bool enabled) +Tracker::RESULT Tracker::EnableWirelessMarkerBatteryStreaming(bool enabled) { - if (this->Internal->isVirtual) + if (this->InternalObj->isVirtual) { return SUCCESS; } @@ -1243,7 +1326,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableWirelessMarkerBatteryStr return ERROR_OPTION_NOT_FOUND; } - if (ftkSetInt32(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, enabled) != ftkError::FTK_OK) + if (ftkSetInt32(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, enabled) != ftkError::FTK_OK) { return ERROR_ENABLE_WIRELESS_MARKER_BATTERY_STREAMING; } @@ -1251,7 +1334,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableWirelessMarkerBatteryStr } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetMaxAdditionalEventsNumber(int n) +Tracker::RESULT Tracker::SetMaxAdditionalEventsNumber(int n) { if (n < 0) { @@ -1262,7 +1345,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetMaxAdditionalEventsNumber(i } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetMax2dFiducialsNumber(int n) +Tracker::RESULT Tracker::SetMax2dFiducialsNumber(int n) { if (n < 0) { @@ -1273,7 +1356,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetMax2dFiducialsNumber(int n) } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetMax3dFiducialsNumber(int n) +Tracker::RESULT Tracker::SetMax3dFiducialsNumber(int n) { if (n < 0) { @@ -1284,7 +1367,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetMax3dFiducialsNumber(int n) } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetMaxMarkersNumber(int n) +Tracker::RESULT Tracker::SetMaxMarkersNumber(int n) { if (n < 0) { @@ -1298,9 +1381,9 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetMaxMarkersNumber(int n) // spryTrack only options // ------------------------------------------ -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableOnboardProcessing(bool enabled) +Tracker::RESULT Tracker::EnableOnboardProcessing(bool enabled) { - if (this->Internal->isVirtual) + if (this->InternalObj->isVirtual) { return SUCCESS; } @@ -1312,7 +1395,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableOnboardProcessing(bool e return ERROR_OPTION_NOT_FOUND; } - if (ftkSetInt32(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, enabled) != ftkError::FTK_OK) + if (ftkSetInt32(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, enabled) != ftkError::FTK_OK) { return ERROR_ENABLE_ONBOARD_PROCESSING; } @@ -1321,9 +1404,9 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableOnboardProcessing(bool e } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableImageStreaming(bool enabled) +Tracker::RESULT Tracker::EnableImageStreaming(bool enabled) { - if (this->Internal->isVirtual) + if (this->InternalObj->isVirtual) { return SUCCESS; } @@ -1335,7 +1418,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableImageStreaming(bool enab return ERROR_OPTION_NOT_FOUND; } - if (ftkSetInt32(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, enabled) != ftkError::FTK_OK) + if (ftkSetInt32(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, enabled) != ftkError::FTK_OK) { return ERROR_ENABLE_IMAGE_STREAMING; } @@ -1344,7 +1427,7 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::EnableImageStreaming(bool enab } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetSpryTrackProcessingType(AtracsysTracker::SPRYTRACK_IMAGE_PROCESSING_TYPE processingType) +Tracker::RESULT Tracker::SetSpryTrackProcessingType(Tracker::SPRYTRACK_IMAGE_PROCESSING_TYPE processingType) { if (this->DeviceType != SPRYTRACK_180 && this->DeviceType != SPRYTRACK_300) { @@ -1376,9 +1459,9 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::SetSpryTrackProcessingType(Atr // fusionTrack only options // ------------------------------------------ -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetDroppedFrameCount(int& droppedFrameCount) +Tracker::RESULT Tracker::GetDroppedFrameCount(int& droppedFrameCount) { - if (this->Internal->isVirtual) + if (this->InternalObj->isVirtual) { droppedFrameCount = 0; return SUCCESS; @@ -1393,12 +1476,12 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetDroppedFrameCount(int& drop { return ERROR_OPTION_NOT_FOUND; } - ftkGetInt32(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, &lost, ftkOptionGetter::FTK_VALUE); + ftkGetInt32(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, &lost, ftkOptionGetter::FTK_VALUE); if (!this->GetOptionInfo("Counter of corrupted frames", info)) { return ERROR_OPTION_NOT_FOUND; } - ftkGetInt32(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, &corrupted, ftkOptionGetter::FTK_VALUE); + ftkGetInt32(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, &corrupted, ftkOptionGetter::FTK_VALUE); droppedFrameCount = lost + corrupted; return SUCCESS; @@ -1407,9 +1490,9 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::GetDroppedFrameCount(int& drop } //---------------------------------------------------------------------------- -AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::ResetLostFrameCount() +Tracker::RESULT Tracker::ResetLostFrameCount() { - if (this->Internal->isVirtual) + if (this->InternalObj->isVirtual) { return SUCCESS; } @@ -1422,12 +1505,13 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::ResetLostFrameCount() { return ERROR_OPTION_NOT_FOUND; } - ftkSetInt32(this->Internal->FtkLib, this->Internal->TrackerSN, info->id, RESET_DROPPED_FRAME_COUNT); + ftkSetInt32(this->InternalObj->FtkLib, this->InternalObj->TrackerSN, info->id, RESET_DROPPED_FRAME_COUNT); return SUCCESS; } return ERROR_OPTION_AVAILABLE_ONLY_ON_FTK; } +//============================================================================ // ------------------------------------------ // Fiducial methods // ------------------------------------------ @@ -1435,15 +1519,14 @@ AtracsysTracker::ATRACSYS_RESULT AtracsysTracker::ResetLostFrameCount() // any 2 fiducials within this 3D distance in mm will be considered equal const float EQUALITY_DISTANCE_MM = 2.0; -bool AtracsysTracker::Fiducial::operator==(const Fiducial& f) +bool Fiducial::operator==(const Fiducial& f) const { // pow is much slower than just x*x for squaring numbers float dist2 = (this->xMm - f.xMm) * (this->xMm - f.xMm) + (this->yMm - f.yMm) * (this->yMm - f.yMm) + (this->zMm - f.zMm) * (this->zMm - f.zMm); return sqrt(dist2) < EQUALITY_DISTANCE_MM; } -// compare fiducials on distance from the origin -bool AtracsysTracker::Fiducial::operator<(const Fiducial& f) const +bool Fiducial::operator<(const Fiducial& f) const { float distF1 = sqrt(this->xMm * this->xMm + this->yMm * this->yMm + this->zMm * this->zMm); float distF2 = sqrt(f.xMm * f.xMm + f.yMm * f.yMm + f.zMm * f.zMm); @@ -1454,7 +1537,7 @@ bool AtracsysTracker::Fiducial::operator<(const Fiducial& f) const // Marker methods // ------------------------------------------ -AtracsysTracker::Marker::Marker() +Marker::Marker() { this->MarkerStatus = 0; this->TrackingId = -1; @@ -1462,7 +1545,7 @@ AtracsysTracker::Marker::Marker() this->GeometryPresenceMask = -1; this->RegistrationErrorMm = 0.0; } -AtracsysTracker::Marker::Marker(int status, int trackingId, int geometryId, +Marker::Marker(int status, int trackingId, int geometryId, vtkMatrix4x4* toolToTracker, int geometryPresenceMask, float registrationErrorMM) { this->MarkerStatus = status; @@ -1473,7 +1556,7 @@ AtracsysTracker::Marker::Marker(int status, int trackingId, int geometryId, this->RegistrationErrorMm = registrationErrorMM; } -AtracsysTracker::Marker::Marker(const AtracsysTracker::Marker& obj) +Marker::Marker(const Marker& obj) { this->MarkerStatus = obj.MarkerStatus; this->TrackingId = obj.TrackingId; @@ -1484,7 +1567,7 @@ AtracsysTracker::Marker::Marker(const AtracsysTracker::Marker& obj) this->fiducials = obj.fiducials; } -bool AtracsysTracker::Marker::AddFiducial(AtracsysTracker::Fiducial fid) +bool Marker::AddFiducial(Fiducial fid) { if (fiducials.size() < FTK_MAX_FIDUCIALS) { diff --git a/src/PlusDataCollection/Atracsys/AtracsysTracker.h b/src/PlusDataCollection/Atracsys/AtracsysTracker.h index e71f81978..d6bbbe4f4 100644 --- a/src/PlusDataCollection/Atracsys/AtracsysTracker.h +++ b/src/PlusDataCollection/Atracsys/AtracsysTracker.h @@ -15,65 +15,11 @@ See License.txt for details. struct ftkOptionsInfo; -// Functions to safely convert string to int32 or float -bool strToInt32(const std::string& str, int& var); -bool strToFloat32(const std::string& str, float& var); - -class AtracsysTracker +namespace Atracsys { -public: - /* Constructor & destructor */ - AtracsysTracker(); - virtual ~AtracsysTracker(); - - enum ATRACSYS_RESULT - { - SUCCESS = 0, - ERROR_UNABLE_TO_GET_FTK_HANDLE, - ERROR_NO_DEVICE_CONNECTED, - WARNING_CONNECTED_IN_USB2, - ERROR_UNABLE_TO_LOAD_MARKER, - ERROR_FAILURE_TO_LOAD_INI, - ERROR_OPTION_AVAILABLE_ONLY_ON_FTK, - ERROR_OPTION_AVAILABLE_ONLY_ON_STK, - ERROR_FAILED_TO_CLOSE_SDK, - ERROR_FAILED_TO_EXPORT_CALIB, - ERROR_FAILED_TO_EXTRACT_FRAME_INFO, - ERROR_CANNOT_CREATE_FRAME_INSTANCE, - ERROR_CANNOT_INITIALIZE_FRAME, - ERROR_NO_FRAME_AVAILABLE, - ERROR_INVALID_FRAME, - ERROR_TOO_MANY_MARKERS, - ERROR_TOO_MANY_FIDUCIALS, - ERROR_ENABLE_LASER, - ERROR_SET_USER_LED, - ERROR_ENABLE_USER_LED, - ERROR_ENABLE_ONBOARD_PROCESSING, - ERROR_ENABLE_IMAGE_STREAMING, - ERROR_ENABLE_WIRELESS_MARKER_PAIRING, - ERROR_ENABLE_WIRELESS_MARKER_STATUS_STREAMING, - ERROR_ENABLE_WIRELESS_MARKER_BATTERY_STREAMING, - ERROR_DISCONNECT_ATTEMPT_WHEN_NOT_CONNECTED, - ERROR_CANNOT_GET_MARKER_INFO, - ERROR_FAILED_TO_SET_STK_PROCESSING_TYPE, - ERROR_OPTION_NOT_FOUND, - ERROR_SET_OPTION - }; - - enum DEVICE_TYPE - { - UNKNOWN_DEVICE = 0, - SPRYTRACK_180, - SPRYTRACK_300, - FUSIONTRACK_500, - FUSIONTRACK_250 - }; - - enum SPRYTRACK_IMAGE_PROCESSING_TYPE - { - PROCESSING_ONBOARD, - PROCESSING_ON_PC - }; + // Functions to safely convert string to int32 or float + bool strToInt32(const std::string& str, int& var); + bool strToFloat32(const std::string& str, float& var); // Class to hold the 3D and 2D coordinates in mm and pixel respectively of // a single fiducial in the view of the camera @@ -81,8 +27,10 @@ class AtracsysTracker { public: Fiducial() {}; + // Overloaded operators, only for 3D coordinates // override equality operator to make fids less than EQUALITY_DISTANCE_MM considered equal - bool operator==(const Fiducial& f); + bool operator==(const Fiducial& f) const; + // compare fiducials on distance from the origin bool operator<(const Fiducial& f) const; uint32_t id = 0; // 3D @@ -139,138 +87,199 @@ class AtracsysTracker Fiducials fiducials; // fiducial coordinates }; - /* Is onboard/embedded processing on ?*/ - bool IsOnboardProcessing(); - - /*! Is virtual ? */ - bool IsVirtual(); - - /*! If virtual device, pause/unpause */ - void Pause(bool tof); - - /*! Connect to Atracsys tracker, must be called before any other function in this wrapper API. */ - ATRACSYS_RESULT Connect(); - - /*! Closes connections to Atracsys tracker, must be called at end of application. */ - ATRACSYS_RESULT Disconnect(); - - /*! */ - ATRACSYS_RESULT GetSDKversion(std::string& version); - - /*! */ - ATRACSYS_RESULT GetCalibrationDate(std::string& date); - - /*! */ - ATRACSYS_RESULT GetDeviceType(DEVICE_TYPE& deviceType); - - /*! */ - ATRACSYS_RESULT GetDeviceId(uint64_t& id); - - /*! Retrieves the cameras parameters : - * leftIntrinsic = left camera focal length [0-1], optical center [2-3], lens distorsion [4-8] and skew [9] - * rightIntrinsic = left camera focal length [0-1], optical center [2-3], lens distorsion [4-8] and skew [9] - * rightPosition = position of the right camera in the coordinate system of the left camera - * rightOrientation = orientation of the right camera in the coordinate system of the left camera - */ - ATRACSYS_RESULT GetCamerasCalibration(std::array& leftIntrinsic, std::array& rightIntrinsic, - std::array& rightPosition, std::array& rightOrientation); - - /*! */ - ATRACSYS_RESULT LoadMarkerGeometryFromFile(std::string filePath, int& geometryId); - - /*! */ - ATRACSYS_RESULT LoadMarkerGeometryFromString(std::string filePath, int& geometryId); - - /*! */ - ATRACSYS_RESULT GetMarkerInfo(std::string& markerInfo); - - /*! */ - ATRACSYS_RESULT GetLoadedGeometries(std::map>>& geometries); - - /*! */ - std::string ResultToString(ATRACSYS_RESULT result); - - /*! */ - ATRACSYS_RESULT GetMarkersInFrame(std::vector& markers, - std::map& events, uint64_t& sdkTimestamp); - - /*! */ - ATRACSYS_RESULT SetUserLEDState(int red, int green, int blue, int frequency, bool enabled = true); - - /*! */ - ATRACSYS_RESULT EnableUserLED(bool enabled); - - /*! */ - ATRACSYS_RESULT SetLaserEnabled(bool enabled); + class Tracker + { + public: + /* Constructor & destructor */ + Tracker(); + virtual ~Tracker(); + + enum RESULT + { + SUCCESS = 0, + ERROR_UNABLE_TO_GET_FTK_HANDLE, + ERROR_NO_DEVICE_CONNECTED, + WARNING_CONNECTED_IN_USB2, + ERROR_UNABLE_TO_LOAD_MARKER, + ERROR_FAILURE_TO_LOAD_INI, + ERROR_OPTION_AVAILABLE_ONLY_ON_FTK, + ERROR_OPTION_AVAILABLE_ONLY_ON_STK, + ERROR_FAILED_TO_CLOSE_SDK, + ERROR_FAILED_TO_EXPORT_CALIB, + ERROR_FAILED_TO_EXTRACT_FRAME_INFO, + ERROR_CANNOT_CREATE_FRAME_INSTANCE, + ERROR_CANNOT_INITIALIZE_FRAME, + ERROR_NO_FRAME_AVAILABLE, + ERROR_INVALID_FRAME, + ERROR_TOO_MANY_MARKERS, + ERROR_TOO_MANY_FIDUCIALS, + ERROR_ENABLE_LASER, + ERROR_SET_USER_LED, + ERROR_ENABLE_USER_LED, + ERROR_ENABLE_ONBOARD_PROCESSING, + ERROR_ENABLE_IMAGE_STREAMING, + ERROR_ENABLE_WIRELESS_MARKER_PAIRING, + ERROR_ENABLE_WIRELESS_MARKER_STATUS_STREAMING, + ERROR_ENABLE_WIRELESS_MARKER_BATTERY_STREAMING, + ERROR_DISCONNECT_ATTEMPT_WHEN_NOT_CONNECTED, + ERROR_CANNOT_GET_MARKER_INFO, + ERROR_FAILED_TO_SET_STK_PROCESSING_TYPE, + ERROR_OPTION_NOT_FOUND, + ERROR_SET_OPTION + }; + + enum DEVICE_TYPE + { + UNKNOWN_DEVICE = 0, + SPRYTRACK_180, + SPRYTRACK_300, + FUSIONTRACK_500, + FUSIONTRACK_250 + }; + + enum SPRYTRACK_IMAGE_PROCESSING_TYPE + { + PROCESSING_ONBOARD, + PROCESSING_ON_PC + }; + + /* Is onboard/embedded processing on ?*/ + bool IsOnboardProcessing(); + + /*! Is virtual ? */ + bool IsVirtual(); + + /*! If virtual device, pause/unpause */ + void Pause(bool tof); + + /*! Connect to Atracsys tracker, must be called before any other function in this wrapper API. */ + RESULT Connect(); + + /*! Closes connections to Atracsys tracker, must be called at end of application. */ + RESULT Disconnect(); + + /*! */ + RESULT GetSDKversion(std::string& version); + + /*! */ + RESULT GetCalibrationDate(std::string& date); + + /*! */ + RESULT GetDeviceType(DEVICE_TYPE& deviceType); + + /*! */ + RESULT GetDeviceId(uint64_t& id); + + /*! Retrieves the cameras parameters : + * leftIntrinsic = left camera focal length [0-1], optical center [2-3], lens distorsion [4-8] and skew [9] + * rightIntrinsic = left camera focal length [0-1], optical center [2-3], lens distorsion [4-8] and skew [9] + * rightPosition = position of the right camera in the coordinate system of the left camera + * rightOrientation = orientation of the right camera in the coordinate system of the left camera + */ + RESULT GetCamerasCalibration(std::array& leftIntrinsic, std::array& rightIntrinsic, + std::array& rightPosition, std::array& rightOrientation); + + /*! */ + RESULT LoadMarkerGeometryFromFile(std::string filePath, int& geometryId); + + /*! */ + RESULT LoadMarkerGeometryFromString(std::string filePath, int& geometryId); + + /*! */ + RESULT GetMarkerInfo(std::string& markerInfo); + + /*! */ + RESULT GetLoadedGeometries(std::map>>& geometries); + + /*! */ + std::string ResultToString(RESULT result); + + /*! */ + RESULT GetFiducialsInFrame(std::vector& fiducials, + std::map& events, uint64_t& sdkTimestamp); + + /*! */ + RESULT GetMarkersInFrame(std::vector& markers, + std::map& events, uint64_t& sdkTimestamp); + + /*! */ + RESULT SetUserLEDState(int red, int green, int blue, int frequency, bool enabled = true); + + /*! */ + RESULT EnableUserLED(bool enabled); + + /*! */ + RESULT SetLaserEnabled(bool enabled); - /*! */ - ATRACSYS_RESULT EnableWirelessMarkerPairing(bool enabled); + /*! */ + RESULT EnableWirelessMarkerPairing(bool enabled); - /*! */ - ATRACSYS_RESULT EnableWirelessMarkerStatusStreaming(bool enabled); + /*! */ + RESULT EnableWirelessMarkerStatusStreaming(bool enabled); - /*! */ - ATRACSYS_RESULT EnableWirelessMarkerBatteryStreaming(bool enabled); + /*! */ + RESULT EnableWirelessMarkerBatteryStreaming(bool enabled); - // ------------------------------------------ - // frame options - // ------------------------------------------ - /*! Set/get the maximum number of additional events per frame included in the device's output. - This extends the default allocation of 20, for a total of 20 + n events allowed per frame. */ - ATRACSYS_RESULT SetMaxAdditionalEventsNumber(int n); - int GetMaxAdditionalEventsNumber() { return MaxAdditionalEventsNumber; } + // ------------------------------------------ + // frame options + // ------------------------------------------ + /*! Set/get the maximum number of additional events per frame included in the device's output. + This extends the default allocation of 20, for a total of 20 + n events allowed per frame. */ + RESULT SetMaxAdditionalEventsNumber(int n); + int GetMaxAdditionalEventsNumber() { return MaxAdditionalEventsNumber; } - /*! Set/get the maximum number of 2D fiducials (in either left or right frame) included in the device's output */ - ATRACSYS_RESULT SetMax2dFiducialsNumber(int n); - int GetMax2dFiducialsNumber() { return Max2dFiducialsNumber; } + /*! Set/get the maximum number of 2D fiducials (in either left or right frame) included in the device's output */ + RESULT SetMax2dFiducialsNumber(int n); + int GetMax2dFiducialsNumber() { return Max2dFiducialsNumber; } - /*! Set/get the maximum number of 3D fiducials (after triangulation) included in the device's output */ - ATRACSYS_RESULT SetMax3dFiducialsNumber(int n); - int GetMax3dFiducialsNumber() { return Max3dFiducialsNumber; } + /*! Set/get the maximum number of 3D fiducials (after triangulation) included in the device's output */ + RESULT SetMax3dFiducialsNumber(int n); + int GetMax3dFiducialsNumber() { return Max3dFiducialsNumber; } - /*! Set/get the maximum number of markers included in the device's output */ - ATRACSYS_RESULT SetMaxMarkersNumber(int n); - int GetMaxMarkersNumber() { return MaxMarkersNumber; } + /*! Set/get the maximum number of markers included in the device's output */ + RESULT SetMaxMarkersNumber(int n); + int GetMaxMarkersNumber() { return MaxMarkersNumber; } - // ------------------------------------------ - // spryTrack only options - // ------------------------------------------ + // ------------------------------------------ + // spryTrack only options + // ------------------------------------------ - /*! */ - ATRACSYS_RESULT EnableOnboardProcessing(bool enabled); + /*! */ + RESULT EnableOnboardProcessing(bool enabled); - /*! */ - ATRACSYS_RESULT EnableImageStreaming(bool enabled); + /*! */ + RESULT EnableImageStreaming(bool enabled); - /*! */ - ATRACSYS_RESULT SetSpryTrackProcessingType(SPRYTRACK_IMAGE_PROCESSING_TYPE processingType); + /*! */ + RESULT SetSpryTrackProcessingType(SPRYTRACK_IMAGE_PROCESSING_TYPE processingType); - // ------------------------------------------ - // fusionTrack only options - // ------------------------------------------ + // ------------------------------------------ + // fusionTrack only options + // ------------------------------------------ - /*! Sum of lost and corrupted frames */ - ATRACSYS_RESULT GetDroppedFrameCount(int& droppedFrameCount); + /*! Sum of lost and corrupted frames */ + RESULT GetDroppedFrameCount(int& droppedFrameCount); - /*! */ - ATRACSYS_RESULT ResetLostFrameCount(); + /*! */ + RESULT ResetLostFrameCount(); - ATRACSYS_RESULT SetOption(const std::string&, const std::string&); + RESULT SetOption(const std::string&, const std::string&); -protected: - bool GetOptionInfo(const std::string&, const ftkOptionsInfo*&); + protected: + bool GetOptionInfo(const std::string&, const ftkOptionsInfo*&); -private: - DEVICE_TYPE DeviceType = UNKNOWN_DEVICE; + private: + DEVICE_TYPE DeviceType = UNKNOWN_DEVICE; - int MaxAdditionalEventsNumber = 0; // beyond the default allocation of 20 events - int Max2dFiducialsNumber = 256; - int Max3dFiducialsNumber = 256; - int MaxMarkersNumber = 16; + int MaxAdditionalEventsNumber = 0; // beyond the default allocation of 20 events + int Max2dFiducialsNumber = 256; + int Max3dFiducialsNumber = 256; + int MaxMarkersNumber = 16; - bool isOnboardProcessing = false; + bool isOnboardProcessing = false; - class AtracsysInternal; - AtracsysInternal* Internal; -}; + class Internal; + Internal* InternalObj; + }; +} // namespace Atracsys #endif diff --git a/src/PlusDataCollection/Atracsys/vtkPlusAtracsysTracker.cxx b/src/PlusDataCollection/Atracsys/vtkPlusAtracsysTracker.cxx index 8854e4a4c..8ef74335d 100644 --- a/src/PlusDataCollection/Atracsys/vtkPlusAtracsysTracker.cxx +++ b/src/PlusDataCollection/Atracsys/vtkPlusAtracsysTracker.cxx @@ -33,8 +33,8 @@ See License.txt for details. #include "ftkTypes.h" // for convenience -#define ATR_SUCCESS AtracsysTracker::ATRACSYS_RESULT::SUCCESS -typedef AtracsysTracker::ATRACSYS_RESULT ATRACSYS_RESULT; +typedef Atracsys::Tracker::RESULT ATR_RESULT; +#define ATR_SUCCESS ATR_RESULT::SUCCESS vtkStandardNewMacro(vtkPlusAtracsysTracker); @@ -90,16 +90,16 @@ class vtkPlusAtracsysTracker::vtkInternal std::map FtkGeometryIdMappedToToolId; // Atracsys API wrapper class handle - AtracsysTracker Tracker; + Atracsys::Tracker Tracker; // type of tracker connected - AtracsysTracker::DEVICE_TYPE DeviceType = AtracsysTracker::UNKNOWN_DEVICE; + Atracsys::Tracker::DEVICE_TYPE DeviceType = Atracsys::Tracker::DEVICE_TYPE::UNKNOWN_DEVICE; }; //---------------------------------------------------------------------------- vtkPlusAtracsysTracker::vtkPlusAtracsysTracker() : vtkPlusDevice() - , Internal(new vtkInternal(this)) + , InternalObj(new vtkInternal(this)) { LOG_TRACE("vtkPlusAtracsysTracker::vtkPlusAtracsysTracker()"); this->FrameNumber = 0; @@ -112,8 +112,8 @@ vtkPlusAtracsysTracker::~vtkPlusAtracsysTracker() { LOG_TRACE("vtkPlusAtracsysTracker::~vtkPlusAtracsysTracker()"); - delete Internal; - Internal = nullptr; + delete InternalObj; + InternalObj = nullptr; } //---------------------------------------------------------------------------- @@ -126,7 +126,7 @@ void vtkPlusAtracsysTracker::PrintSelf(ostream& os, vtkIndent indent) std::string vtkPlusAtracsysTracker::GetSdkVersion() { std::string v; - this->Internal->Tracker.GetSDKversion(v); + this->InternalObj->Tracker.GetSDKversion(v); return v; } @@ -134,22 +134,22 @@ std::string vtkPlusAtracsysTracker::GetSdkVersion() std::string vtkPlusAtracsysTracker::GetCalibrationDate() { std::string d; - this->Internal->Tracker.GetCalibrationDate(d); + this->InternalObj->Tracker.GetCalibrationDate(d); return d; } //---------------------------------------------------------------------------- std::string vtkPlusAtracsysTracker::GetDeviceType() { - switch (this->Internal->DeviceType) + switch (this->InternalObj->DeviceType) { - case AtracsysTracker::DEVICE_TYPE::FUSIONTRACK_250: + case Atracsys::Tracker::DEVICE_TYPE::FUSIONTRACK_250: return "fusionTrack250"; - case AtracsysTracker::DEVICE_TYPE::FUSIONTRACK_500: + case Atracsys::Tracker::DEVICE_TYPE::FUSIONTRACK_500: return "fusionTrack500"; - case AtracsysTracker::DEVICE_TYPE::SPRYTRACK_180: + case Atracsys::Tracker::DEVICE_TYPE::SPRYTRACK_180: return "spryTrack180"; - case AtracsysTracker::DEVICE_TYPE::SPRYTRACK_300: + case Atracsys::Tracker::DEVICE_TYPE::SPRYTRACK_300: return "spryTrack300"; default: return "unknown"; @@ -161,11 +161,11 @@ PlusStatus vtkPlusAtracsysTracker::GetCamerasCalibration( std::array& leftIntrinsic, std::array& rightIntrinsic, std::array& rightPosition, std::array& rightOrientation) { - ATRACSYS_RESULT result = this->Internal->Tracker.GetCamerasCalibration( + ATR_RESULT result = this->InternalObj->Tracker.GetCamerasCalibration( leftIntrinsic, rightIntrinsic, rightPosition, rightOrientation); if (result != ATR_SUCCESS) { - LOG_ERROR(this->Internal->Tracker.ResultToString(result)); + LOG_ERROR(this->InternalObj->Tracker.ResultToString(result)); return PLUS_FAIL; } return PLUS_SUCCESS; @@ -174,7 +174,7 @@ PlusStatus vtkPlusAtracsysTracker::GetCamerasCalibration( //---------------------------------------------------------------------------- PlusStatus vtkPlusAtracsysTracker::GetLoadedGeometries(std::map>>& geometries) { - if (this->Internal->Tracker.GetLoadedGeometries(geometries) != ATR_SUCCESS) + if (this->InternalObj->Tracker.GetLoadedGeometries(geometries) != ATR_SUCCESS) { LOG_ERROR("Could not get loaded geometries"); return PLUS_FAIL; @@ -190,7 +190,7 @@ PlusStatus vtkPlusAtracsysTracker::GetLoadedGeometries(std::mapInternal->Tracker.IsVirtual(); + return this->InternalObj->Tracker.IsVirtual(); } //---------------------------------------------------------------------------- @@ -201,7 +201,7 @@ PlusStatus vtkPlusAtracsysTracker::ReadConfiguration(vtkXMLDataElement* rootConf for (int i = 0; i < deviceConfig->GetNumberOfAttributes(); ++i) { - this->Internal->DeviceOptions.emplace(deviceConfig->GetAttributeName(i), deviceConfig->GetAttributeValue(i)); + this->InternalObj->DeviceOptions.emplace(deviceConfig->GetAttributeName(i), deviceConfig->GetAttributeValue(i)); // also store parameters with vtkPlusDevice method this->SetParameter(deviceConfig->GetAttributeName(i), deviceConfig->GetAttributeValue(i)); } @@ -244,7 +244,7 @@ PlusStatus vtkPlusAtracsysTracker::ReadConfiguration(vtkXMLDataElement* rootConf if (ftkGeometryId != -1) { std::pair thisTool(ftkGeometryId, toolId); - this->Internal->FtkGeometryIdMappedToToolId.insert(thisTool); + this->InternalObj->FtkGeometryIdMappedToToolId.insert(thisTool); } else { @@ -259,7 +259,7 @@ PlusStatus vtkPlusAtracsysTracker::ReadConfiguration(vtkXMLDataElement* rootConf if ((geometryFile = toolDataElement->GetAttribute("GeometryFile")) != NULL) { std::pair thisTool(toolId, geometryFile); - this->Internal->PlusIdMappedToGeometryFilename.insert(thisTool); + this->InternalObj->PlusIdMappedToGeometryFilename.insert(thisTool); } else { @@ -290,16 +290,16 @@ PlusStatus vtkPlusAtracsysTracker::Probe() //---------------------------------------------------------------------------- const std::map& vtkPlusAtracsysTracker::GetDeviceOptions() const { - return this->Internal->DeviceOptions; + return this->InternalObj->DeviceOptions; } //---------------------------------------------------------------------------- PlusStatus vtkPlusAtracsysTracker::GetOptionValue(const std::string& optionName, std::string& optionValue) { - std::map::const_iterator itd = this->Internal->DeviceOptions.find(optionName); - if (itd != this->Internal->DeviceOptions.cend()) + std::map::const_iterator itd = this->InternalObj->DeviceOptions.find(optionName); + if (itd != this->InternalObj->DeviceOptions.cend()) { - optionValue = this->Internal->DeviceOptions[optionName]; + optionValue = this->InternalObj->DeviceOptions[optionName]; return PLUS_SUCCESS; } return PLUS_FAIL; @@ -309,8 +309,8 @@ PlusStatus vtkPlusAtracsysTracker::GetOptionValue(const std::string& optionName, bool vtkPlusAtracsysTracker::TranslateOptionName(const std::string& optionName, std::string& translatedOptionName) { std::map::const_iterator itt = - this->Internal->DeviceOptionTranslator.find(optionName); - if (itt == this->Internal->DeviceOptionTranslator.cend()) + this->InternalObj->DeviceOptionTranslator.find(optionName); + if (itt == this->InternalObj->DeviceOptionTranslator.cend()) { return false; } @@ -327,139 +327,139 @@ PlusStatus vtkPlusAtracsysTracker::InternalConnect() LOG_TRACE("vtkPlusAtracsysTracker::InternalConnect"); // Connect to device - AtracsysTracker::ATRACSYS_RESULT result = this->Internal->Tracker.Connect(); - if (result != ATR_SUCCESS && result != AtracsysTracker::ATRACSYS_RESULT::WARNING_CONNECTED_IN_USB2) + ATR_RESULT result = this->InternalObj->Tracker.Connect(); + if (result != ATR_SUCCESS && result != ATR_RESULT::WARNING_CONNECTED_IN_USB2) { - LOG_ERROR(this->Internal->Tracker.ResultToString(result)); + LOG_ERROR(this->InternalObj->Tracker.ResultToString(result)); return PLUS_FAIL; } - else if (result == AtracsysTracker::ATRACSYS_RESULT::WARNING_CONNECTED_IN_USB2) + else if (result == ATR_RESULT::WARNING_CONNECTED_IN_USB2) { - LOG_WARNING(this->Internal->Tracker.ResultToString(result)); + LOG_WARNING(this->InternalObj->Tracker.ResultToString(result)); } // get device serial number in hex form std::ostringstream oss; uint64 sn; - this->Internal->Tracker.GetDeviceId(sn); + this->InternalObj->Tracker.GetDeviceId(sn); oss << "0x" << std::setw(16) << std::setfill('0') << std::hex << sn; this->DeviceId = oss.str(); // get device type - this->Internal->Tracker.GetDeviceType(this->Internal->DeviceType); + this->InternalObj->Tracker.GetDeviceType(this->InternalObj->DeviceType); std::map::const_iterator itd; // handling options that are used only internally // ------- time allocated for activer marker pairing - itd = this->Internal->DeviceOptions.find("ActiveMarkerPairingTimeSec"); - if (itd != this->Internal->DeviceOptions.cend()) + itd = this->InternalObj->DeviceOptions.find("ActiveMarkerPairingTimeSec"); + if (itd != this->InternalObj->DeviceOptions.cend()) { - if (strToInt32(itd->second, this->Internal->ActiveMarkerPairingTimeSec)) + if (Atracsys::strToInt32(itd->second, this->InternalObj->ActiveMarkerPairingTimeSec)) { - LOG_INFO("Marker pairing time is set to " << this->Internal->ActiveMarkerPairingTimeSec << " seconds, tracking will not start until this period is over."); + LOG_INFO("Marker pairing time is set to " << this->InternalObj->ActiveMarkerPairingTimeSec << " seconds, tracking will not start until this period is over."); } } // handling options that are used both internally and in the sdk // ------- max registration error in mm - itd = this->Internal->DeviceOptions.find("MaxMeanRegistrationErrorMm"); - if (itd != this->Internal->DeviceOptions.cend()) + itd = this->InternalObj->DeviceOptions.find("MaxMeanRegistrationErrorMm"); + if (itd != this->InternalObj->DeviceOptions.cend()) { - if (strToFloat32(itd->second, this->Internal->MaxMeanRegistrationErrorMm)) + if (Atracsys::strToFloat32(itd->second, this->InternalObj->MaxMeanRegistrationErrorMm)) { - if (this->Internal->MaxMeanRegistrationErrorMm < 0.1 || this->Internal->MaxMeanRegistrationErrorMm > 5.0) + if (this->InternalObj->MaxMeanRegistrationErrorMm < 0.1 || this->InternalObj->MaxMeanRegistrationErrorMm > 5.0) { - this->Internal->MaxMeanRegistrationErrorMm = 2.0; - 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."); + this->InternalObj->MaxMeanRegistrationErrorMm = 2.0; + 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->InternalObj->MaxMeanRegistrationErrorMm << "mm."); } } } else { - this->Internal->DeviceOptions.emplace("MaxMeanRegistrationErrorMm", - std::to_string(this->Internal->MaxMeanRegistrationErrorMm)); + this->InternalObj->DeviceOptions.emplace("MaxMeanRegistrationErrorMm", + std::to_string(this->InternalObj->MaxMeanRegistrationErrorMm)); } // ------- max number of missing fiducials - itd = this->Internal->DeviceOptions.find("MaxMissingFiducials"); - if (itd != this->Internal->DeviceOptions.cend()) + itd = this->InternalObj->DeviceOptions.find("MaxMissingFiducials"); + if (itd != this->InternalObj->DeviceOptions.cend()) { - if (strToInt32(itd->second, this->Internal->MaxMissingFiducials)) + if (Atracsys::strToInt32(itd->second, this->InternalObj->MaxMissingFiducials)) { - if (this->Internal->MaxMissingFiducials < 0 || this->Internal->MaxMissingFiducials > 3) + if (this->InternalObj->MaxMissingFiducials < 0 || this->InternalObj->MaxMissingFiducials > 3) { - this->Internal->MaxMissingFiducials = 0; - 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 << "."); + this->InternalObj->MaxMissingFiducials = 0; + 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->InternalObj->MaxMissingFiducials << "."); } } } else { - this->Internal->DeviceOptions.emplace("MaxMissingFiducials", - std::to_string(this->Internal->MaxMissingFiducials)); + this->InternalObj->DeviceOptions.emplace("MaxMissingFiducials", + std::to_string(this->InternalObj->MaxMissingFiducials)); } // set frame options (internally passed to sdk during connection) // ------- maximum number of events per frame included in the device's output - itd = this->Internal->DeviceOptions.find("MaxAdditionalEventsNumber"); - if (itd != this->Internal->DeviceOptions.cend()) + itd = this->InternalObj->DeviceOptions.find("MaxAdditionalEventsNumber"); + if (itd != this->InternalObj->DeviceOptions.cend()) { int value = -1; - strToInt32(itd->second, value); + Atracsys::strToInt32(itd->second, value); if (value < 0) { LOG_WARNING("Invalid value for max events number per frame in output: " << itd->second - << ". Default value used (" << this->Internal->Tracker.GetMaxAdditionalEventsNumber() << ")"); + << ". Default value used (" << this->InternalObj->Tracker.GetMaxAdditionalEventsNumber() << ")"); } else { - this->Internal->Tracker.SetMaxAdditionalEventsNumber(value); + this->InternalObj->Tracker.SetMaxAdditionalEventsNumber(value); } } // ------- maximum number of 2D fiducials (in either left or right frame) included in the device's output - itd = this->Internal->DeviceOptions.find("Max2dFiducialsNumber"); - if (itd != this->Internal->DeviceOptions.cend()) + itd = this->InternalObj->DeviceOptions.find("Max2dFiducialsNumber"); + if (itd != this->InternalObj->DeviceOptions.cend()) { int value = -1; - strToInt32(itd->second, value); + Atracsys::strToInt32(itd->second, value); if (value < 0) { LOG_WARNING("Invalid value for max 2D fiducials number in output: " << itd->second - << ". Default value used (" << this->Internal->Tracker.GetMax2dFiducialsNumber() << ")"); + << ". Default value used (" << this->InternalObj->Tracker.GetMax2dFiducialsNumber() << ")"); } else { - this->Internal->Tracker.SetMax2dFiducialsNumber(value); + this->InternalObj->Tracker.SetMax2dFiducialsNumber(value); } } // ------- maximum number of 3D fiducials (after triangulation) included in the device's output - itd = this->Internal->DeviceOptions.find("Max3dFiducialsNumber"); - if (itd != this->Internal->DeviceOptions.cend()) + itd = this->InternalObj->DeviceOptions.find("Max3dFiducialsNumber"); + if (itd != this->InternalObj->DeviceOptions.cend()) { int value = -1; - strToInt32(itd->second, value); + Atracsys::strToInt32(itd->second, value); if (value < 0) { LOG_WARNING("Invalid value for max 3D fiducials number in output: " << itd->second - << ". Default value used (" << this->Internal->Tracker.GetMax3dFiducialsNumber() << ")"); + << ". Default value used (" << this->InternalObj->Tracker.GetMax3dFiducialsNumber() << ")"); } else { - this->Internal->Tracker.SetMax3dFiducialsNumber(value); + this->InternalObj->Tracker.SetMax3dFiducialsNumber(value); } } // ------- maximum number of markers included in the device's output - itd = this->Internal->DeviceOptions.find("MaxMarkersNumber"); - if (itd != this->Internal->DeviceOptions.cend()) + itd = this->InternalObj->DeviceOptions.find("MaxMarkersNumber"); + if (itd != this->InternalObj->DeviceOptions.cend()) { int value = -1; - strToInt32(itd->second, value); + Atracsys::strToInt32(itd->second, value); if (value < 0) { LOG_WARNING("Invalid value for max markers number in output: " << itd->second - << ". Default value used (" << this->Internal->Tracker.GetMaxMarkersNumber() << ")"); + << ". Default value used (" << this->InternalObj->Tracker.GetMaxMarkersNumber() << ")"); } else { - this->Internal->Tracker.SetMaxMarkersNumber(value); + this->InternalObj->Tracker.SetMaxMarkersNumber(value); } } @@ -467,64 +467,64 @@ PlusStatus vtkPlusAtracsysTracker::InternalConnect() // SpryTrack's Embedded processing option needs to be considered first. // By default, the spryTrack is in Embedded processing mode. // In Embedded processing mode, some other options will require the "Embedded " prefix. - std::map::iterator itr = this->Internal->DeviceOptions.find("Enable_embedded_processing"); - if (itr != this->Internal->DeviceOptions.end()) + std::map::iterator itr = this->InternalObj->DeviceOptions.find("Enable_embedded_processing"); + if (itr != this->InternalObj->DeviceOptions.end()) { - if (this->Internal->DeviceOptions.at("Enable_embedded_processing") == "0") + if (this->InternalObj->DeviceOptions.at("Enable_embedded_processing") == "0") { - if (this->Internal->Tracker.SetSpryTrackProcessingType(AtracsysTracker::PROCESSING_ON_PC) != ATR_SUCCESS) + if (this->InternalObj->Tracker.SetSpryTrackProcessingType(Atracsys::Tracker::PROCESSING_ON_PC) != ATR_SUCCESS) { LOG_WARNING("Embedded processing could not be disabled."); - this->Internal->DeviceOptions.erase(itr); + this->InternalObj->DeviceOptions.erase(itr); } } - else if (this->Internal->DeviceOptions.at("Enable_embedded_processing") == "1") + else if (this->InternalObj->DeviceOptions.at("Enable_embedded_processing") == "1") { - if (this->Internal->Tracker.SetSpryTrackProcessingType(AtracsysTracker::PROCESSING_ONBOARD) != ATR_SUCCESS) + if (this->InternalObj->Tracker.SetSpryTrackProcessingType(Atracsys::Tracker::PROCESSING_ONBOARD) != ATR_SUCCESS) { - this->Internal->DeviceOptions.erase(itr); + this->InternalObj->DeviceOptions.erase(itr); LOG_WARNING("Embedded processing could not be enabled."); } } else { - 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)."); - this->Internal->DeviceOptions.erase(itr); + LOG_WARNING(this->InternalObj->DeviceOptions.at("Enable_embedded_processing") << " is not a correct value for Embedded processing.\nAccepted values are 0 (false) and 1 (true)."); + this->InternalObj->DeviceOptions.erase(itr); } } - itr = this->Internal->DeviceOptions.begin(); - while (itr != this->Internal->DeviceOptions.end()) + itr = this->InternalObj->DeviceOptions.begin(); + while (itr != this->InternalObj->DeviceOptions.end()) { std::string translatedOptionName; /* Dirty hack circumventing a nomenclature discrepancy between ftk and stk API's, this may be fixed in a future API release */ - if (itr->first == "EnableIRstrobe" && (this->Internal->DeviceType == AtracsysTracker::DEVICE_TYPE::FUSIONTRACK_500 - || this->Internal->DeviceType == AtracsysTracker::DEVICE_TYPE::FUSIONTRACK_250)) + if (itr->first == "EnableIRstrobe" && (this->InternalObj->DeviceType == Atracsys::Tracker::DEVICE_TYPE::FUSIONTRACK_500 + || this->InternalObj->DeviceType == Atracsys::Tracker::DEVICE_TYPE::FUSIONTRACK_250)) { // For fusiontracks, strobe on is 0 and strobe off is 1 (the opposite of sprytracks). // Fusiontracks also supports a mode 2 where the strobe turns on every other frame (unsupported by sprytracks at the moment) std::map strobCorresp{ {"0","1"},{"1","0"}, {"2","2"} }; if (strobCorresp.find(itr->second) != strobCorresp.end()) { - if (this->Internal->Tracker.SetOption("Strobe mode", strobCorresp.at(itr->second)) != ATR_SUCCESS) + if (this->InternalObj->Tracker.SetOption("Strobe mode", strobCorresp.at(itr->second)) != ATR_SUCCESS) { - itr = this->Internal->DeviceOptions.erase(itr); + itr = this->InternalObj->DeviceOptions.erase(itr); continue; } } else { LOG_WARNING("Unsupported strobe mode value " << itr->second); - itr = this->Internal->DeviceOptions.erase(itr); + itr = this->InternalObj->DeviceOptions.erase(itr); continue; } } else/* end of hack*/ if (this->TranslateOptionName(itr->first, translatedOptionName)) // looking for the option in the translation dictionary { - if (this->Internal->Tracker.SetOption(translatedOptionName, itr->second) != ATR_SUCCESS) + if (this->InternalObj->Tracker.SetOption(translatedOptionName, itr->second) != ATR_SUCCESS) { - itr = this->Internal->DeviceOptions.erase(itr); + itr = this->InternalObj->DeviceOptions.erase(itr); continue; } } @@ -533,9 +533,9 @@ PlusStatus vtkPlusAtracsysTracker::InternalConnect() { std::string inferredOptionName = itr->first; std::replace(inferredOptionName.begin(), inferredOptionName.end(), '_', ' '); - if (this->Internal->Tracker.SetOption(inferredOptionName, itr->second) != ATR_SUCCESS) + if (this->InternalObj->Tracker.SetOption(inferredOptionName, itr->second) != ATR_SUCCESS) { - itr = this->Internal->DeviceOptions.erase(itr); + itr = this->InternalObj->DeviceOptions.erase(itr); continue; } } @@ -543,74 +543,74 @@ PlusStatus vtkPlusAtracsysTracker::InternalConnect() && itr->first != "ToolReferenceFrame" && itr->first != "Enable_embedded_processing") { LOG_WARNING("Unknown option \"" << itr->first << "\"."); - itr = this->Internal->DeviceOptions.erase(itr); + itr = this->InternalObj->DeviceOptions.erase(itr); continue; } ++itr; } // disable marker status streaming and battery charge streaming, they cause momentary pauses in tracking while sending - if (this->Internal->DeviceType == AtracsysTracker::DEVICE_TYPE::SPRYTRACK_180) + if (this->InternalObj->DeviceType == Atracsys::Tracker::DEVICE_TYPE::SPRYTRACK_180) { - if ((result = this->Internal->Tracker.EnableWirelessMarkerStatusStreaming(false)) != ATR_SUCCESS) + if ((result = this->InternalObj->Tracker.EnableWirelessMarkerStatusStreaming(false)) != ATR_SUCCESS) { - LOG_WARNING(this->Internal->Tracker.ResultToString(result)); + LOG_WARNING(this->InternalObj->Tracker.ResultToString(result)); } - if ((result = this->Internal->Tracker.EnableWirelessMarkerBatteryStreaming(false)) != ATR_SUCCESS) + if ((result = this->InternalObj->Tracker.EnableWirelessMarkerBatteryStreaming(false)) != ATR_SUCCESS) { - LOG_WARNING(this->Internal->Tracker.ResultToString(result)); + LOG_WARNING(this->InternalObj->Tracker.ResultToString(result)); } } // load passive geometries onto Atracsys std::map::iterator it; - for (it = begin(this->Internal->PlusIdMappedToGeometryFilename); it != end(this->Internal->PlusIdMappedToGeometryFilename); it++) + for (it = begin(this->InternalObj->PlusIdMappedToGeometryFilename); it != end(this->InternalObj->PlusIdMappedToGeometryFilename); it++) { // load user defined geometry file // TODO: add check for conflicting marker IDs std::string geomFilePath = vtkPlusConfig::GetInstance()->GetDeviceSetConfigurationPath(it->second); int geometryId; - if ((result = this->Internal->Tracker.LoadMarkerGeometryFromFile(geomFilePath, geometryId)) != ATR_SUCCESS) + if ((result = this->InternalObj->Tracker.LoadMarkerGeometryFromFile(geomFilePath, geometryId)) != ATR_SUCCESS) { - LOG_ERROR(this->Internal->Tracker.ResultToString(result) << " This error occurred when trying to load geometry file at path: " << geomFilePath); + 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->Internal->FtkGeometryIdMappedToToolId.insert(newTool); + this->InternalObj->FtkGeometryIdMappedToToolId.insert(newTool); } // if active marker pairing is desired, then its time > 0 second - if (this->Internal->ActiveMarkerPairingTimeSec > 0) + if (this->InternalObj->ActiveMarkerPairingTimeSec > 0) { // make LED blue during pairing - this->Internal->Tracker.SetUserLEDState(0, 0, 255, 0); + this->InternalObj->Tracker.SetUserLEDState(0, 0, 255, 0); // pair active markers - if ((result = this->Internal->Tracker.EnableWirelessMarkerPairing(true)) != ATR_SUCCESS) + if ((result = this->InternalObj->Tracker.EnableWirelessMarkerPairing(true)) != ATR_SUCCESS) { - LOG_ERROR(this->Internal->Tracker.ResultToString(result)); + LOG_ERROR(this->InternalObj->Tracker.ResultToString(result)); return PLUS_FAIL; } - LOG_INFO("Active marker pairing period started for " << this->Internal->ActiveMarkerPairingTimeSec << " seconds."); + LOG_INFO("Active marker pairing period started for " << this->InternalObj->ActiveMarkerPairingTimeSec << " seconds."); // sleep while waiting for tracker to pair active markers - vtkIGSIOAccurateTimer::Delay(this->Internal->ActiveMarkerPairingTimeSec); + vtkIGSIOAccurateTimer::Delay(this->InternalObj->ActiveMarkerPairingTimeSec); LOG_INFO("Active marker pairing period ended."); - if ((result = this->Internal->Tracker.EnableWirelessMarkerPairing(false)) != ATR_SUCCESS) + if ((result = this->InternalObj->Tracker.EnableWirelessMarkerPairing(false)) != ATR_SUCCESS) { - LOG_ERROR(this->Internal->Tracker.ResultToString(result)); + LOG_ERROR(this->InternalObj->Tracker.ResultToString(result)); return PLUS_FAIL; } // make LED green, pairing is complete - this->Internal->Tracker.SetUserLEDState(0, 255, 0, 0); + this->InternalObj->Tracker.SetUserLEDState(0, 255, 0, 0); // TODO: check number of active markers paired std::string markerInfo; - this->Internal->Tracker.GetMarkerInfo(markerInfo); + this->InternalObj->Tracker.GetMarkerInfo(markerInfo); if (!markerInfo.empty()) { LOG_INFO("Additional info about paired markers:" << markerInfo << std::endl); @@ -624,12 +624,12 @@ PlusStatus vtkPlusAtracsysTracker::InternalConnect() PlusStatus vtkPlusAtracsysTracker::InternalDisconnect() { LOG_TRACE("vtkPlusAtracsysTracker::InternalDisconnect"); - this->Internal->Tracker.EnableUserLED(false); + this->InternalObj->Tracker.EnableUserLED(false); - ATRACSYS_RESULT result; - if ((result = this->Internal->Tracker.Disconnect()) != ATR_SUCCESS) + ATR_RESULT result; + if ((result = this->InternalObj->Tracker.Disconnect()) != ATR_SUCCESS) { - LOG_ERROR(this->Internal->Tracker.ResultToString(result)); + LOG_ERROR(this->InternalObj->Tracker.ResultToString(result)); return PLUS_FAIL; } return PLUS_SUCCESS; @@ -653,7 +653,7 @@ PlusStatus vtkPlusAtracsysTracker::InternalStopRecording() PlusStatus vtkPlusAtracsysTracker::PauseVirtualDevice() { LOG_TRACE("vtkPlusAtracsysTracker::PauseVirtualDevice"); - this->Internal->Tracker.Pause(true); + this->InternalObj->Tracker.Pause(true); return PLUS_SUCCESS; } @@ -661,7 +661,7 @@ PlusStatus vtkPlusAtracsysTracker::PauseVirtualDevice() PlusStatus vtkPlusAtracsysTracker::UnpauseVirtualDevice() { LOG_TRACE("vtkPlusAtracsysTracker::UnpauseVirtualDevice"); - this->Internal->Tracker.Pause(false); + this->InternalObj->Tracker.Pause(false); return PLUS_SUCCESS; } @@ -671,20 +671,20 @@ PlusStatus vtkPlusAtracsysTracker::InternalUpdate() LOG_TRACE("vtkPlusAtracsysTracker::InternalUpdate"); double unfilteredTimestamp = vtkIGSIOAccurateTimer::GetSystemTime(); - std::vector markers; + std::vector markers; std::map events; uint64_t sdkTimestamp = 0; - ATRACSYS_RESULT result = this->Internal->Tracker.GetMarkersInFrame(markers, events, sdkTimestamp); + ATR_RESULT result = this->InternalObj->Tracker.GetMarkersInFrame(markers, events, sdkTimestamp); - if (result == AtracsysTracker::ATRACSYS_RESULT::ERROR_NO_FRAME_AVAILABLE) + if (result == ATR_RESULT::ERROR_NO_FRAME_AVAILABLE) { // waiting for frame return PLUS_SUCCESS; } else if (result != ATR_SUCCESS) { - LOG_ERROR(this->Internal->Tracker.ResultToString(result)); + LOG_ERROR(this->InternalObj->Tracker.ResultToString(result)); return PLUS_FAIL; } @@ -702,7 +702,7 @@ PlusStatus vtkPlusAtracsysTracker::InternalUpdate() } std::map::iterator it; - for (it = this->Internal->FtkGeometryIdMappedToToolId.begin(); it != this->Internal->FtkGeometryIdMappedToToolId.end(); it++) + for (it = this->InternalObj->FtkGeometryIdMappedToToolId.begin(); it != this->InternalObj->FtkGeometryIdMappedToToolId.end(); it++) { if (std::find(this->DisabledToolIds.begin(), this->DisabledToolIds.end(), it->second) != this->DisabledToolIds.end()) { @@ -715,7 +715,7 @@ PlusStatus vtkPlusAtracsysTracker::InternalUpdate() } bool toolUpdated = false; - std::vector::iterator mit; + std::vector::iterator mit; for (mit = markers.begin(); mit != markers.end(); mit++) { if (it->first != (int)mit->GetGeometryID()) @@ -723,7 +723,7 @@ PlusStatus vtkPlusAtracsysTracker::InternalUpdate() continue; } // check if tool marker registration falls above maximum - if (mit->GetFiducialRegistrationErrorMm() > this->Internal->MaxMeanRegistrationErrorMm) + if (mit->GetFiducialRegistrationErrorMm() > this->InternalObj->MaxMeanRegistrationErrorMm) { LOG_WARNING("Maximum mean marker fiducial registration error exceeded for tool: " << it->second); continue; @@ -804,7 +804,7 @@ PlusStatus vtkPlusAtracsysTracker::InternalUpdate() // LED PlusStatus vtkPlusAtracsysTracker::SetLedEnabled(bool enabled) { - if (this->Internal->Tracker.EnableUserLED(enabled) != ATR_SUCCESS) + if (this->InternalObj->Tracker.EnableUserLED(enabled) != ATR_SUCCESS) { return PLUS_FAIL; } @@ -813,7 +813,7 @@ PlusStatus vtkPlusAtracsysTracker::SetLedEnabled(bool enabled) PlusStatus vtkPlusAtracsysTracker::SetUserLEDState(int red, int green, int blue, int frequency, bool enabled /* = true */) { - if (this->Internal->Tracker.SetUserLEDState(red, green, blue, frequency, enabled) != ATR_SUCCESS) + if (this->InternalObj->Tracker.SetUserLEDState(red, green, blue, frequency, enabled) != ATR_SUCCESS) { return PLUS_FAIL; } @@ -834,7 +834,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->Internal->FtkGeometryIdMappedToToolId.begin(); it != this->Internal->FtkGeometryIdMappedToToolId.end(); it++) + for (it = this->InternalObj->FtkGeometryIdMappedToToolId.begin(); it != this->InternalObj->FtkGeometryIdMappedToToolId.end(); it++) { if (igsioCommon::IsEqualInsensitive(it->second, toolId)) { @@ -859,7 +859,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->Internal->FtkGeometryIdMappedToToolId.begin(); it != this->Internal->FtkGeometryIdMappedToToolId.end(); it++) + for (it = this->InternalObj->FtkGeometryIdMappedToToolId.begin(); it != this->InternalObj->FtkGeometryIdMappedToToolId.end(); it++) { if (igsioCommon::IsEqualInsensitive(it->second, toolId)) { @@ -875,10 +875,10 @@ PlusStatus vtkPlusAtracsysTracker::AddToolGeometry(std::string toolId, std::stri } int geometryId; - ATRACSYS_RESULT result; - if ((result = this->Internal->Tracker.LoadMarkerGeometryFromString(geomString, geometryId)) != ATR_SUCCESS) + ATR_RESULT result; + if ((result = this->InternalObj->Tracker.LoadMarkerGeometryFromString(geomString, geometryId)) != ATR_SUCCESS) { - LOG_ERROR(this->Internal->Tracker.ResultToString(result) << " This error occurred when trying to load the following geometry information: " << geomString); + LOG_ERROR(this->InternalObj->Tracker.ResultToString(result) << " This error occurred when trying to load the following geometry information: " << geomString); return PLUS_FAIL; } @@ -925,7 +925,7 @@ PlusStatus vtkPlusAtracsysTracker::AddToolGeometry(std::string toolId, std::stri // register this tool internally std::pair newTool(geometryId, toolId); - this->Internal->FtkGeometryIdMappedToToolId.insert(newTool); + this->InternalObj->FtkGeometryIdMappedToToolId.insert(newTool); return PLUS_SUCCESS; } @@ -934,7 +934,7 @@ PlusStatus vtkPlusAtracsysTracker::AddToolGeometry(std::string toolId, std::stri // Other PlusStatus vtkPlusAtracsysTracker::SetLaserEnabled(bool enabled) { - if (this->Internal->Tracker.SetLaserEnabled(enabled) != ATR_SUCCESS) + if (this->InternalObj->Tracker.SetLaserEnabled(enabled) != ATR_SUCCESS) { return PLUS_FAIL; } diff --git a/src/PlusDataCollection/Atracsys/vtkPlusAtracsysTracker.h b/src/PlusDataCollection/Atracsys/vtkPlusAtracsysTracker.h index cea6dc65d..d6b7c98d8 100644 --- a/src/PlusDataCollection/Atracsys/vtkPlusAtracsysTracker.h +++ b/src/PlusDataCollection/Atracsys/vtkPlusAtracsysTracker.h @@ -121,7 +121,7 @@ class vtkPlusDataCollectionExport vtkPlusAtracsysTracker : public vtkPlusDevice std::vector DisabledToolIds; class vtkInternal; - vtkInternal* Internal; + vtkInternal* InternalObj; }; #endif