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