re PlusToolkit#241: Tracking of passive and active markers using Atra…

…csys wrapper API

src/PlusDataCollection/Atracsys/AtracsysConstants.h

@@ -21,7 +21,12 @@ namespace Atracsys
     ERROR_OPTION_NOT_AVAILABLE_ON_FTK,
     ERROR_FAILED_TO_SET_OPTION,
     ERROR_FAILED_TO_LOAD_GEOMETRY,
-    ERROR_FAILED_TO_CLOSE_SDK
+    ERROR_FAILED_TO_CLOSE_SDK,
+    ERROR_CANNOT_CREATE_FRAME_INSTANCE,
+    ERROR_CANNOT_INITIALIZE_FRAME,
+    ERROR_NO_FRAME_AVAILABLE,
+    ERROR_INVALID_FRAME,
+    ERROR_TOO_MANY_MARKERS
   };
 
   enum OPTIONS
@@ -43,7 +48,11 @@ namespace Atracsys
     OPTION_MAX_TRACKING_RANGE = 3005,
     OPTION_ONBOARD_PROCESSING = 6000,
     OPTION_IMAGE_STREAMING = 6003,
-    OPTION_WIRELESS_MARKER_PAIRING = 7000
+    OPTION_WIRELESS_MARKER_PAIRING = 7000,
+    OPTION_WIRELESS_MARKER_STATUS_STREAMING = 7001,
+    OPTION_WIRELESS_MARKER_BATTERY_STREAMING = 7002,
+    OPTION_DEV_MARKERS_INFO = 7005,
+    OPTION_MAXIMUM_MISSING_POINTS = 10004
   };
 
   enum DEVICE_TYPE

src/PlusDataCollection/Atracsys/AtracsysMarker.cxx

@@ -8,27 +8,30 @@ See License.txt for details.
 
 using namespace Atracsys;
 
-ATRACSYS_ERROR Marker::GetID(int& id)
+Marker::Marker(int geometryId, vtkSmartPointer<vtkMatrix4x4> toolToTracker, int gpm, int freMm)
 {
-  return SUCCESS;
+  this->GeometryId = geometryId;
+  this->ToolToTracker = toolToTracker;
+  this->GeometryPresenceMask = gpm;
+  this->FreMm = freMm;
 }
 
-ATRACSYS_ERROR Marker::GetGeometryID(int& geometryID)
+int Marker::GetGeometryID()
 {
-  return SUCCESS;
+  return this->GeometryId;
 }
 
-ATRACSYS_ERROR Marker::GetGeometryPrecsenceMask(int& geometryMask)
+int Marker::GetGeometryPrecsenceMask()
 {
-  return SUCCESS;
+  return this->GeometryPresenceMask;
 }
 
-ATRACSYS_ERROR Marker::GetTransformToTracker(vtkSmartPointer<vtkMatrix4x4> MarkerToTracker)
+vtkSmartPointer<vtkMatrix4x4> Marker::GetTransformToTracker()
 {
-  return SUCCESS;
+  return this->ToolToTracker;
 }
 
-ATRACSYS_ERROR Marker::GetFiducialRegistrationErrorMm(float& fre)
+float Marker::GetFiducialRegistrationErrorMm()
 {
-  return SUCCESS;
+  return this->FreMm;
 }

src/PlusDataCollection/Atracsys/AtracsysMarker.h

@@ -9,31 +9,25 @@ See License.txt for details.
 
 #include "AtracsysConstants.h"
 #include <vtkSmartPointer.h>
-#include "vtkMatrix4x4.h"
+#include <vtkMatrix4x4.h>
 
 namespace Atracsys
 {
   class Marker
   {
   public:
-    ATRACSYS_ERROR GetID(int& id);
-    ATRACSYS_ERROR GetGeometryID(int& geometryID);
-    ATRACSYS_ERROR GetGeometryPrecsenceMask(int& geometryMask);
-    ATRACSYS_ERROR GetTransformToTracker(vtkSmartPointer<vtkMatrix4x4> MarkerToTracker);
-    ATRACSYS_ERROR GetFiducialRegistrationErrorMm(float& fre);
+    Marker(int geometryId, vtkSmartPointer<vtkMatrix4x4> toolToTracker, int gpm, int freMm);
+    int GetGeometryID();
+    int GetGeometryPrecsenceMask();
+    vtkSmartPointer<vtkMatrix4x4> GetTransformToTracker();
+    float GetFiducialRegistrationErrorMm();
   private:
-    int id; /*!< Tracking id */
-    int geometryId; /*!< Geometric id, i.e. the unique id of the used
-                       * geometry. */
-    int geometryPresenceMask; /*!< Presence mask of fiducials expressed as
+    int Id; /*!< Tracking id */
+    int GeometryId;
+    vtkSmartPointer<vtkMatrix4x4> ToolToTracker;
+    int GeometryPresenceMask; /*!< Presence mask of fiducials expressed as
                                  * their geometrical indexes */
-    int fiducialCorresp[FTK_MAX_FIDUCIALS]; /*!< Correspondence between
-                                               * geometry index and 3D
-                                               * fiducials indexes or
-                                               * INVALID_ID */
-    float rotation[3][3]; /*!< Rotation matrix: format [row][column] */
-    float translationMM[3]; /*!< translation vector (unit mm) */
-    float registrationErrorMM; /*!< Registration mean ATRACSYS_ERROR (unit mm) */
+    float FreMm; /*!< Registration mean ATRACSYS_ERROR (unit mm) */
   };
 }
 

src/PlusDataCollection/Atracsys/AtracsysTracker.cxx

@@ -4,8 +4,9 @@ Copyright (c) Laboratory for Percutaneous Surgery. All rights reserved.
 See License.txt for details.
 =========================================================Plus=header=end*/
 
-
+#include "PlusConfigure.h"
 #include "AtracsysTracker.h"
+#include "AtracsysMarker.h"
 
 // System includes
 #include <string>
@@ -21,8 +22,10 @@ See License.txt for details.
 #include "ftkOptions.h"
 #include "ftkPlatform.h"
 
-using namespace Atracsys;
+// vtk includes
+#include <vtkMatrix4x4.h>
 
+using namespace Atracsys;
 
 namespace
 {
@@ -481,10 +484,21 @@ ATRACSYS_ERROR Tracker::Disconnect()
   return SUCCESS;
 }
 
+std::string Tracker::GetMarkerInfo()
+{
+  ftkBuffer buffer;
+  if (ftkGetData(this->FtkLib, this->TrackerSN, OPTION_DEV_MARKERS_INFO, &buffer) != FTK_OK)
+  {
+    std::cout << "Cannot get additional infos on the markers.";
+  }
+  std::string info(buffer.data, buffer.data + buffer.size);
+  return info;
+}
+
 //----------------------------------------------------------------------------
 // Some spryTrack 180 only options
 
-ATRACSYS_ERROR Tracker::SetSTKOnlyOption(OPTIONS option, int value)
+ATRACSYS_ERROR Tracker::SetSTKOnlyOption(int option, int value)
 {
   // this option is only available on spryTrack
   if (this->DeviceType == SPRYTRACK_180)
@@ -527,6 +541,16 @@ ATRACSYS_ERROR Tracker::DisableWirelessMarkerPairing()
   return this->SetSTKOnlyOption(OPTION_WIRELESS_MARKER_PAIRING, 0);
 }
 
+ATRACSYS_ERROR Tracker::DisableWirelessMarkerStatusStreaming()
+{
+  return this->SetSTKOnlyOption(OPTION_WIRELESS_MARKER_STATUS_STREAMING, 0);
+}
+
+ATRACSYS_ERROR Tracker::DisableWirelessMarkerBatteryStreaming()
+{
+  return this->SetSTKOnlyOption(OPTION_WIRELESS_MARKER_BATTERY_STREAMING, 0);
+}
+
 //----------------------------------------------------------------------------
 ATRACSYS_ERROR Tracker::EnableIRStrobe()
 {
@@ -573,6 +597,9 @@ ATRACSYS_ERROR Tracker::DisableUserLED()
   return SUCCESS;
 }
 
+
+
+
 ATRACSYS_ERROR Tracker::LoadMarkerGeometry(std::string filePath, int& geometryId)
 {
   ftkGeometry geom;
@@ -587,5 +614,89 @@ ATRACSYS_ERROR Tracker::LoadMarkerGeometry(std::string filePath, int& geometryId
 
 ATRACSYS_ERROR Tracker::GetMarkersInFrame(std::vector<Marker>& markers)
 {
+
+
+
+  ftkFrameQuery* frame = ftkCreateFrame();
+
+  if (frame == 0)
+  {
+    ftkDeleteFrame(frame);
+    return ERROR_CANNOT_CREATE_FRAME_INSTANCE;
+  }
+
+  ftkError err(ftkSetFrameOptions(false, false, 128u, 128u,
+    4u * FTK_MAX_FIDUCIALS, 4u, frame));
+
+  if (err != FTK_OK)
+  {
+    ftkDeleteFrame(frame);
+    return ERROR_CANNOT_INITIALIZE_FRAME;
+  }
+
+  if (ftkGetLastFrame(this->FtkLib, this->TrackerSN, frame, 0) != FTK_OK)
+  {
+    // block until next frame is available from camera
+    return ERROR_NO_FRAME_AVAILABLE;
+  }
+
+  switch (frame->markersStat)
+  {
+  case QS_WAR_SKIPPED:
+    ftkDeleteFrame(frame);
+    LOG_ERROR("marker fields in the frame are not set correctly");
+    LOG_ERROR(this->GetFtkLastErrorString());
+    return ERROR_INVALID_FRAME;
+
+  case QS_ERR_INVALID_RESERVED_SIZE:
+    ftkDeleteFrame(frame);
+    LOG_ERROR("frame -> markersVersionSize is invalid");
+    LOG_ERROR(this->GetFtkLastErrorString());
+    return ERROR_INVALID_FRAME;
+
+  default:
+    ftkDeleteFrame(frame);
+    LOG_ERROR("invalid status");
+    LOG_ERROR(this->GetFtkLastErrorString());
+    return ERROR_INVALID_FRAME;
+
+  case QS_OK:
+    break;
+  }
+
+  if (frame->markersStat == QS_ERR_OVERFLOW)
+  {
+    LOG_ERROR("Marker overflow. Too many markers in frame.");
+    return ERROR_TOO_MANY_MARKERS;
+  }
+
+  ftkMarker* marker;
+
+  for (size_t m = 0; m < frame->markersCount; m++)
+  {
+    marker = &(frame->markers[m]);
+    int geometryId = marker->geometryId;
+    vtkSmartPointer<vtkMatrix4x4> toolToTracker = vtkSmartPointer<vtkMatrix4x4>::New();
+    toolToTracker->Identity();
+
+    for (int i = 0; i < 3; i++)
+    {
+      toolToTracker->SetElement(i, 3, marker->translationMM[i]);
+      for (int j = 0; j < 3; j++)
+      {
+        toolToTracker->SetElement(i, j, marker->rotation[i][j]);
+      }
+    }
+
+    int gpm = marker->geometryPresenceMask;
+    float freMm = marker->registrationErrorMM;
+
+    Marker atracsysMarker(geometryId, toolToTracker, gpm, freMm);
+    markers.push_back(atracsysMarker);
+  }
+
+  // close frame
+  ftkDeleteFrame(frame);
+
   return SUCCESS;
 }

src/PlusDataCollection/Atracsys/AtracsysTracker.h

@@ -74,9 +74,18 @@ namespace Atracsys
     /*! */
     ATRACSYS_ERROR DisableWirelessMarkerPairing();
 
+    /*! */
+    ATRACSYS_ERROR DisableWirelessMarkerStatusStreaming();
+
+    /*! */
+    ATRACSYS_ERROR DisableWirelessMarkerBatteryStreaming();
+
     /*! */
     ATRACSYS_ERROR GetMarkersInFrame(std::vector<Marker>& markers);
 
+    /*! */
+    std::string GetMarkerInfo();
+
     /*! */
     std::string GetFtkLastErrorString();
 
@@ -91,7 +100,7 @@ namespace Atracsys
     bool LoadIniFile(std::ifstream& is, ftkGeometry& geometry);
     //----------------------------------------------------------------------------
 
-    ATRACSYS_ERROR SetSTKOnlyOption(OPTIONS option, int value);
+    ATRACSYS_ERROR SetSTKOnlyOption(int option, int value);
   };
 }
 #endif