47 full transform with error

Examples/CMakeLists.txt

@@ -27,6 +27,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2")
     Capability
     Trajectory
     SessionManager
+    FullTrackingDataWithError
     )
 endif (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2")
 

Examples/FullTrackingDataWithError/CMakeLists.txt

@@ -0,0 +1,16 @@
+PROJECT(FullTrackingDataWithError)
+
+cmake_minimum_required(VERSION 2.4)
+if(COMMAND cmake_policy)
+  cmake_policy(SET CMP0003 NEW)
+endif(COMMAND cmake_policy)
+
+find_package(OpenIGTLink REQUIRED)
+
+include(${OpenIGTLink_USE_FILE})
+
+ADD_EXECUTABLE(FullTrackingDataWithErrorServer   FullTrackingDataWithErrorServer.cxx)
+TARGET_LINK_LIBRARIES(FullTrackingDataWithErrorServer OpenIGTLink)
+
+
+

Examples/FullTrackingDataWithError/FullTrackingDataWithErrorServer.cxx

@@ -0,0 +1,143 @@
+/*=========================================================================
+
+  Program:   Open IGT Link -- Example for Full Tracking Data With Errors
+  Module:    $RCSfile: $
+  Language:  C++
+  Date:      $Date: $
+  Version:   $Revision: $
+
+  Copyright (c) Insight Software Consortium. All rights reserved.
+
+  This software is distributed WITHOUT ANY WARRANTY; without even
+  the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR
+  PURPOSE.  See the above copyright notices for more information.
+
+=========================================================================*/
+
+#include <iostream>
+#include <math.h>
+#include <cstdlib>
+
+#include "igtlOSUtil.h"
+#include "igtlTrackingDataMessage.h"
+#include "igtlFullTrackingDataWithErrorMessage.h"
+#include "igtlServerSocket.h"
+
+void GetRandomTestMatrix(igtl::Matrix4x4& matrix);
+
+int main(int argc, char* argv[])
+{
+  //------------------------------------------------------------
+  // Parse Arguments
+
+  if (argc != 3) // check number of arguments
+    {
+    // If not correct, print usage
+    std::cerr << "Usage: " << argv[0] << " <port> <fps>"    << std::endl;
+    std::cerr << "    <port>     : Port # (18944 in Slicer default)"   << std::endl;
+    std::cerr << "    <fps>      : Frequency (fps) to send coordinate" << std::endl;
+    exit(0);
+    }
+
+  int    port     = atoi(argv[1]);
+  double fps      = atof(argv[2]);
+  int    interval = (int) (1000.0 / fps);
+
+  igtl::FullTrackingDataWithErrorMessage::Pointer msg;
+  msg = igtl::FullTrackingDataWithErrorMessage::New();
+
+  igtl::TrackingDataWithErrorElement::Pointer elem;
+  elem = igtl::TrackingDataWithErrorElement::New();
+  elem->SetType(igtl::TrackingDataElement::TYPE_6D);
+  elem->SetName("Tracker");
+
+  igtl::TrackingDataElement::Pointer elemBase;
+  elemBase = dynamic_cast<igtl::TrackingDataElement*>(elem.GetPointer());
+
+  igtl::ServerSocket::Pointer serverSocket;
+  serverSocket = igtl::ServerSocket::New();
+  int r = serverSocket->CreateServer(port);
+
+  if (r < 0)
+    {
+    std::cerr << "Cannot create a server socket." << std::endl;
+    exit(0);
+    }
+
+  igtl::Socket::Pointer socket;
+
+  while (1)
+    {
+    //------------------------------------------------------------
+    // Waiting for Connection
+    socket = serverSocket->WaitForConnection(1000);
+
+    if (socket.IsNotNull()) // if client connected
+      {
+      //------------------------------------------------------------
+      // loop
+      for (int i = 0; i < 100; i ++)
+        {
+        igtl::Matrix4x4 matrix;
+        GetRandomTestMatrix(matrix);
+
+        // Geometrically a bit meaningless,
+        // but the whole point of this class is to transmit
+        // the full matrix. So we want to see these numbers at the client end.
+        matrix[3][0] = i;
+        matrix[3][1] = i;
+        matrix[3][2] = i;
+
+        msg->ClearTrackingDataElements();
+
+        elem->SetMatrix(matrix);
+        elem->SetError(i);
+
+        msg->AddTrackingDataElement(elemBase);
+
+        msg->Pack();
+
+        socket->Send(msg->GetPackPointer(), msg->GetPackSize());
+        igtl::Sleep(interval); // wait
+        }
+      }
+    }
+
+  //------------------------------------------------------------
+  // Close connection (The example code never reachs to this section ...)
+
+  socket->CloseSocket();
+
+}
+
+
+void GetRandomTestMatrix(igtl::Matrix4x4& matrix)
+{
+  float position[3];
+  float orientation[4];
+
+  // random position
+  static float phi = 0.0;
+  position[0] = 50.0 * cos(phi);
+  position[1] = 50.0 * sin(phi);
+  position[2] = 50.0 * cos(phi);
+  phi = phi + 0.2;
+
+  // random orientation
+  static float theta = 0.0;
+  orientation[0]=0.0;
+  orientation[1]=0.6666666666*cos(theta);
+  orientation[2]=0.577350269189626;
+  orientation[3]=0.6666666666*sin(theta);
+  theta = theta + 0.1;
+
+  //igtl::Matrix4x4 matrix;
+  igtl::QuaternionToMatrix(orientation, matrix);
+
+  matrix[0][3] = position[0];
+  matrix[1][3] = position[1];
+  matrix[2][3] = position[2];
+
+  igtl::PrintMatrix(matrix);
+}
+

Examples/Receiver/ReceiveClient.cxx

@@ -33,6 +33,7 @@
 #include "igtlTrajectoryMessage.h"
 #include "igtlStringMessage.h"
 #include "igtlTrackingDataMessage.h"
+#include "igtlFullTrackingDataWithErrorMessage.h"
 #include "igtlQuaternionTrackingDataMessage.h"
 #include "igtlCapabilityMessage.h"
 #endif // OpenIGTLink_PROTOCOL_VERSION >= 2
@@ -47,6 +48,7 @@ int ReceiveStatus(igtl::Socket * socket, igtl::MessageHeader::Pointer& header);
   int ReceiveTrajectory(igtl::Socket * socket, igtl::MessageHeader::Pointer& header);
   int ReceiveString(igtl::Socket * socket, igtl::MessageHeader::Pointer& header);
   int ReceiveTrackingData(igtl::ClientSocket::Pointer& socket, igtl::MessageHeader::Pointer& header);
+  int ReceiveFullTrackingDataWithError(igtl::ClientSocket::Pointer& socket, igtl::MessageHeader::Pointer& header);
   int ReceiveQuaternionTrackingData(igtl::ClientSocket::Pointer& socket, igtl::MessageHeader::Pointer& header);
   int ReceiveCapability(igtl::Socket * socket, igtl::MessageHeader * header);
 #endif //OpenIGTLink_PROTOCOL_VERSION >= 2
@@ -161,6 +163,10 @@ int main(int argc, char* argv[])
         {
         ReceiveTrackingData(socket, headerMsg);
         }
+      else if (strcmp(headerMsg->GetDeviceType(), "FULLTDATA") == 0)
+        {
+        ReceiveFullTrackingDataWithError(socket, headerMsg);
+        }
       else if (strcmp(headerMsg->GetDeviceType(), "QTDATA") == 0)
         {
         ReceiveQuaternionTrackingData(socket, headerMsg);
@@ -497,6 +503,52 @@ int ReceiveTrackingData(igtl::ClientSocket::Pointer& socket, igtl::MessageHeader
   return 0;
 }
 
+
+int ReceiveFullTrackingDataWithError(igtl::ClientSocket::Pointer& socket, igtl::MessageHeader::Pointer& header)
+{
+  std::cerr << "Receiving FULLTDATA data type." << std::endl;
+
+  // Create a message buffer to receive transform data
+  igtl::FullTrackingDataWithErrorMessage::Pointer trackingData;
+  trackingData = igtl::FullTrackingDataWithErrorMessage::New();
+  trackingData->SetMessageHeader(header);
+  trackingData->AllocatePack();
+
+  // Receive body from the socket
+  socket->Receive(trackingData->GetPackBodyPointer(), trackingData->GetPackBodySize());
+
+  // Deserialize the transform data
+  // If you want to skip CRC check, call Unpack() without argument.
+  int c = trackingData->Unpack(1);
+
+  if (c & igtl::MessageHeader::UNPACK_BODY) // if CRC check is OK
+    {
+    int nElements = trackingData->GetNumberOfTrackingDataElements();
+    for (int i = 0; i < nElements; i ++)
+      {
+      igtl::TrackingDataWithErrorElement::Pointer trackingElement;
+      trackingData->GetTrackingDataElement(i, trackingElement);
+
+      igtl::Matrix4x4 matrix;
+      trackingElement->GetMatrix(matrix);
+
+      float errorMeasure;
+      errorMeasure = trackingElement->GetError();
+
+      std::cerr << "========== Element #" << i << " ==========" << std::endl;
+      std::cerr << " Name       : " << trackingElement->GetName() << std::endl;
+      std::cerr << " Type       : " << (int) trackingElement->GetType() << std::endl;
+      std::cerr << " Matrix : " << std::endl;
+      igtl::PrintMatrix(matrix);
+      std::cerr << " Error      : " << trackingElement->GetError() << std::endl;
+      std::cerr << "================================" << std::endl << std::endl;
+      }
+    return 1;
+    }
+  return 0;
+}
+
+
 int ReceiveQuaternionTrackingData(igtl::ClientSocket::Pointer& socket, igtl::MessageHeader::Pointer& header)
 {
   std::cerr << "Receiving QTDATA data type." << std::endl;

Source/CMakeLists.txt

@@ -110,6 +110,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2")
     igtlutil/igtl_lbmeta.c
     igtlutil/igtl_point.c
     igtlutil/igtl_tdata.c
+    igtlutil/igtl_fulltdata.c
     igtlutil/igtl_qtdata.c
     igtlutil/igtl_trajectory.c
     igtlutil/igtl_unit.c
@@ -121,6 +122,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2")
     igtlutil/igtl_polydata.c
     igtlColorTableMessage.cxx
     igtlCapabilityMessage.cxx
+    igtlFullTrackingDataWithErrorMessage.cxx
     igtlImageMetaMessage.cxx
     igtlLabelMetaMessage.cxx
     igtlPointMessage.cxx
@@ -141,6 +143,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2")
     igtlutil/igtl_lbmeta.h
     igtlutil/igtl_point.h
     igtlutil/igtl_tdata.h
+    igtlutil/igtl_fulltdata.h
     igtlutil/igtl_qtdata.h
     igtlutil/igtl_trajectory.h
     igtlutil/igtl_unit.h
@@ -152,6 +155,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2")
     igtlutil/igtl_polydata.h
     igtlColorTableMessage.h
     igtlCapabilityMessage.h
+    igtlFullTrackingDataWithErrorMessage.h
     igtlImageMetaMessage.h
     igtlLabelMetaMessage.h
     igtlPointMessage.h