diff --git a/Examples/CMakeLists.txt b/Examples/CMakeLists.txt index 356342bb..7641172b 100644 --- a/Examples/CMakeLists.txt +++ b/Examples/CMakeLists.txt @@ -27,6 +27,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") Capability Trajectory SessionManager + FullTrackingDataWithError ) endif (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") diff --git a/Examples/FullTrackingDataWithError/CMakeLists.txt b/Examples/FullTrackingDataWithError/CMakeLists.txt new file mode 100644 index 00000000..1a66af8f --- /dev/null +++ b/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) + + + diff --git a/Examples/FullTrackingDataWithError/FullTrackingDataWithErrorServer.cxx b/Examples/FullTrackingDataWithError/FullTrackingDataWithErrorServer.cxx new file mode 100644 index 00000000..4a06934f --- /dev/null +++ b/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 +#include +#include + +#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] << " " << std::endl; + std::cerr << " : Port # (18944 in Slicer default)" << std::endl; + std::cerr << " : 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(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); +} + diff --git a/Examples/Receiver/ReceiveClient.cxx b/Examples/Receiver/ReceiveClient.cxx index 56e58922..06a03555 100644 --- a/Examples/Receiver/ReceiveClient.cxx +++ b/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; diff --git a/Source/CMakeLists.txt b/Source/CMakeLists.txt index df08e6ba..912107d0 100644 --- a/Source/CMakeLists.txt +++ b/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 diff --git a/Source/igtlFullTrackingDataWithErrorMessage.cxx b/Source/igtlFullTrackingDataWithErrorMessage.cxx new file mode 100644 index 00000000..c28af3a7 --- /dev/null +++ b/Source/igtlFullTrackingDataWithErrorMessage.cxx @@ -0,0 +1,164 @@ +/*========================================================================= + + Program: The OpenIGTLink Library + Language: C++ + Web page: http://openigtlink.org/ + + 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 "igtlFullTrackingDataWithErrorMessage.h" + +#include "igtl_header.h" +#include "igtl_fulltdata.h" +#include "igtlMacro.h" + +// Disable warning C4996 (strncpy() may be unsafe) in Windows. +#define _CRT_SECURE_NO_WARNINGS + +#include +#include + +namespace igtl { + +//---------------------------------------------------------------------- +FullTrackingDataWithErrorMessage::FullTrackingDataWithErrorMessage() + : TrackingDataMessage() +{ + this->m_DefaultBodyType = "FULLTDATA"; +} + + +//---------------------------------------------------------------------- +FullTrackingDataWithErrorMessage::~FullTrackingDataWithErrorMessage() +{ +} + + +//---------------------------------------------------------------------- +int FullTrackingDataWithErrorMessage::GetBodyPackSize() +{ + // The body size sum of the header size and status message size. + return IGTL_FULLTDATA_ELEMENT_SIZE * this->m_TrackingDataList.size(); +} + + +//---------------------------------------------------------------------- +int FullTrackingDataWithErrorMessage::PackBody() +{ + // allocate pack + AllocatePack(); + + igtl_fulltdata_element* element; + element = (igtl_fulltdata_element*)this->m_Body; + std::vector::iterator iter; + + for (iter = this->m_TrackingDataList.begin(); iter != this->m_TrackingDataList.end(); iter ++) + { + TrackingDataWithErrorElement* elem = dynamic_cast((*iter).GetPointer()); + if (elem == NULL) + { + igtlExceptionMacro("Found list item that is not a TrackingDataWithErrorElement"); + } + + strncpy((char*)element->name, (*iter)->GetName(), IGTL_FULLTDATA_LEN_NAME); + element->type = (*iter)->GetType(); + + Matrix4x4 matrix; + (*iter)->GetMatrix(matrix); + for (int i = 0; i < 4; i ++) + { + element->transform[i] = matrix[i][0]; + element->transform[i+4] = matrix[i][1]; + element->transform[i+8] = matrix[i][2]; + element->transform[i+12] = matrix[i][3]; + } + + float error; + error = elem->GetError(); + element->error = error; + + element ++; + } + + igtl_fulltdata_convert_byte_order((igtl_fulltdata_element*)this->m_Body, this->m_TrackingDataList.size()); + + return 1; +} + + +//---------------------------------------------------------------------- +int FullTrackingDataWithErrorMessage::UnpackBody() +{ + + this->m_TrackingDataList.clear(); + + igtl_fulltdata_element* element = (igtl_fulltdata_element*) this->m_Body; + int nElement = igtl_fulltdata_get_data_n(this->m_BodySizeToRead); + + igtl_fulltdata_convert_byte_order(element, nElement); + + char strbuf[128]; + for (int i = 0; i < nElement; i ++) + { + igtl::TrackingDataWithErrorElement::Pointer elemClass = TrackingDataWithErrorElement::New(); + + // Add '\n' at the end of each string + // (neccesary for a case, where a string reaches the maximum length.) + strbuf[IGTL_FULLTDATA_LEN_NAME] = '\n'; + strncpy(strbuf, (char*)element->name, IGTL_FULLTDATA_LEN_NAME); + elemClass->SetName((const char*)strbuf); + elemClass->SetType(element->type); + + Matrix4x4 matrix; + IdentityMatrix(matrix); + for (int j = 0; j < 4; j ++) + { + matrix[j][0] = element->transform[j]; + matrix[j][1] = element->transform[j+4]; + matrix[j][2] = element->transform[j+8]; + matrix[j][3] = element->transform[j+12]; + } + elemClass->SetMatrix(matrix); + + float error; + error = element->error; + elemClass->SetError(error); + + igtl::TrackingDataElement::Pointer pointerToBase = igtl::TrackingDataElement::New(); + pointerToBase = elemClass.GetPointer(); + + this->m_TrackingDataList.push_back(pointerToBase); + + element ++; + } + + return 1; +} + +//---------------------------------------------------------------------- +void FullTrackingDataWithErrorMessage::GetTrackingDataElement(int index, TrackingDataWithErrorElement::Pointer& elem) +{ + if (index >= 0 && index < (int)this->m_TrackingDataList.size()) + { + TrackingDataWithErrorElement* element = dynamic_cast(this->m_TrackingDataList[index].GetPointer()); + if (element == NULL) + { + igtlExceptionMacro("Found list item that is not a TrackingDataWithErrorElement"); + } + igtl::TrackingDataWithErrorElement::Pointer tmp = element; + elem = tmp; + } +} + +} // namespace igtl + + + + + diff --git a/Source/igtlFullTrackingDataWithErrorMessage.h b/Source/igtlFullTrackingDataWithErrorMessage.h new file mode 100644 index 00000000..b1697cd1 --- /dev/null +++ b/Source/igtlFullTrackingDataWithErrorMessage.h @@ -0,0 +1,83 @@ +/*========================================================================= + + Program: The OpenIGTLink Library + Language: C++ + Web page: http://openigtlink.org/ + + 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. + +=========================================================================*/ + +#ifndef __igtlFullTrackingDataWithErrorMessage_h +#define __igtlFullTrackingDataWithErrorMessage_h + +#include "igtlTrackingDataMessage.h" +#include "igtlMacro.h" + +namespace igtl +{ + +class IGTLCommon_EXPORT TrackingDataWithErrorElement: public TrackingDataElement +{ +public: + typedef TrackingDataWithErrorElement Self; + typedef TrackingDataElement Superclass; + typedef SmartPointer Pointer; + typedef SmartPointer ConstPointer; + + igtlTypeMacro(igtl::TrackingDataWithErrorElement, igtl::TrackingDataElement); + igtlNewMacro(igtl::TrackingDataWithErrorElement); + +public: + igtlGetConstMacro(Error, igtlFloat32); + igtlSetMacro(Error, igtlFloat32); + +protected: + TrackingDataWithErrorElement() : m_Error(0) {} + ~TrackingDataWithErrorElement() {} + +protected: + + /// Some tracking systems (e.g. NDI Certus) give the tracking error. + igtlFloat32 m_Error; +}; + + +class IGTLCommon_EXPORT FullTrackingDataWithErrorMessage: public TrackingDataMessage +{ +public: + typedef FullTrackingDataWithErrorMessage Self; + typedef TrackingDataMessage Superclass; + typedef SmartPointer Pointer; + typedef SmartPointer ConstPointer; + + igtlTypeMacro(igtl::FullTrackingDataWithErrorMessage, igtl::TrackingDataMessage); + igtlNewMacro(igtl::FullTrackingDataWithErrorMessage); + +public: + + /// Gets the tracking data element specified by 'index'. + void GetTrackingDataElement(int index, TrackingDataWithErrorElement::Pointer& elem); + +protected: + FullTrackingDataWithErrorMessage(); + ~FullTrackingDataWithErrorMessage(); + +protected: + + virtual int GetBodyPackSize(); + virtual int PackBody(); + virtual int UnpackBody(); + +}; + +} // namespace igtl + +#endif // __igtlFullTrackingDataWithErrorMessage_h + + + diff --git a/Source/igtlutil/CMakeLists.txt b/Source/igtlutil/CMakeLists.txt index d90b36a4..e6649d2c 100644 --- a/Source/igtlutil/CMakeLists.txt +++ b/Source/igtlutil/CMakeLists.txt @@ -39,6 +39,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") igtl_lbmeta.h igtl_point.h igtl_tdata.h + igtl_fulltdata.h igtl_qtdata.h igtl_trajectory.h igtl_unit.h @@ -56,6 +57,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") igtl_lbmeta.c igtl_point.c igtl_tdata.c + igtl_fulltdata.c igtl_qtdata.c igtl_trajectory.c igtl_unit.c diff --git a/Source/igtlutil/igtl_fulltdata.c b/Source/igtlutil/igtl_fulltdata.c new file mode 100644 index 00000000..9a439693 --- /dev/null +++ b/Source/igtlutil/igtl_fulltdata.c @@ -0,0 +1,62 @@ +/*========================================================================= + + Program: The OpenIGTLink Library + Language: C + Web page: http://openigtlink.org/ + + 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 +#include + +#include "igtl_fulltdata.h" +#include "igtl_util.h" + +void igtl_export igtl_fulltdata_convert_byte_order(igtl_fulltdata_element* tdatalist, int nitem) +{ + igtl_fulltdata_element* elem; + int i; + int j; + igtl_int32* tmp; + + for (i = 0; i < nitem; i ++) + { + elem = &(tdatalist[i]); + if (igtl_is_little_endian()) + { + for (j = 0; j < 16; j ++) + { + tmp = (igtl_int32*)&(elem->transform[j]); + *tmp = BYTE_SWAP_INT32(*tmp); + } + tmp = ( igtl_int32*)&(elem->error); + *tmp = BYTE_SWAP_INT32(*tmp); + } + + } +} + + +igtl_uint64 igtl_export igtl_fulltdata_get_crc(igtl_fulltdata_element* tdatalist, int nitem) +{ + igtl_fulltdata_element* elem; + int i; + igtl_uint64 crc; + + crc = crc64(0, 0, 0); + for (i = 0; i < nitem; i ++) + { + elem = &(tdatalist[i]); + crc = crc64((unsigned char*) elem, IGTL_FULLTDATA_ELEMENT_SIZE, crc); + } + + return crc; +} + + diff --git a/Source/igtlutil/igtl_fulltdata.h b/Source/igtlutil/igtl_fulltdata.h new file mode 100644 index 00000000..d7185a3e --- /dev/null +++ b/Source/igtlutil/igtl_fulltdata.h @@ -0,0 +1,65 @@ +/*========================================================================= + + Program: The OpenIGTLink Library + Language: C + Web page: http://openigtlink.org/ + + 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. + +=========================================================================*/ + +#ifndef __IGTL_FULLTDATA_H +#define __IGTL_FULLTDATA_H + +#include "igtl_win32header.h" +#include "igtl_util.h" +#include "igtl_types.h" + +#define IGTL_FULLTDATA_ELEMENT_SIZE 90 +#define IGTL_FULLTDATA_LEN_NAME 20 /* Maximum length of tracking instrument name */ + +#ifdef __cplusplus +extern "C" { +#endif + +#pragma pack(1) /* For 1-byte boundary in memroy */ + +/** Status data header for OpenIGTLinik protocol */ +typedef struct { + char name[IGTL_FULLTDATA_LEN_NAME]; /* Name of instrument / tracker */ + igtl_uint8 type; /* Tracking data type (1-4) */ + igtl_uint8 reserved; /* Reserved byte */ + igtl_float32 transform[16]; /* same as TRANSFORM */ + igtl_float32 error; /* the tracking error */ +} igtl_fulltdata_element; + + +#pragma pack() + +/** igtl_tdata_get_data_size(n) calculates the size of body based on the number + * of tdatas. The size of body is used in the message header.*/ +#define igtl_fulltdata_get_data_size(n) ((n) * IGTL_FULLTDATA_ELEMENT_SIZE) + +/** igtl_tdata_get_data_n(size) calculates the number of tdatas in the body, based on + * the body size. This function may be used when a client program parses a TDATA message. */ +#define igtl_fulltdata_get_data_n(size) ((size) / IGTL_FULLTDATA_ELEMENT_SIZE) + +/** Converts endianness of each element in an array of + * igtl_tdata_element from host byte order to network byte order, + * or vice versa.*/ +void igtl_export igtl_fulltdata_convert_byte_order(igtl_fulltdata_element* tdatalist, int nelem); + +/** Calculates CRC of TDATA, STT_TDATA and RTS_TDATA messages.*/ +igtl_uint64 igtl_export igtl_fulltdata_get_crc(igtl_fulltdata_element* tdatalist, int nelem); + +#ifdef __cplusplus +} +#endif + +#endif /* __IGTL_FULLTDATA_H */ + + diff --git a/Source/igtlutil/igtl_fulltransform.c b/Source/igtlutil/igtl_fulltransform.c new file mode 100644 index 00000000..27426179 --- /dev/null +++ b/Source/igtlutil/igtl_fulltransform.c @@ -0,0 +1,45 @@ +/*========================================================================= + + Program: The OpenIGTLink Library + Language: C + Web page: http://openigtlink.org/ + + 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 +#include "igtl_transform.h" +#include "igtl_util.h" +/*#include "crc32.h"*/ + +#include + +void igtl_export igtl_transform_convert_byte_order(igtl_float32* transform) +{ + int i; + igtl_uint32 tmp[12]; + + if (igtl_is_little_endian()) { + memcpy(tmp, transform, sizeof(igtl_uint32)*12); + for (i = 0; i < 12; i ++) { + tmp[i] = BYTE_SWAP_INT32(tmp[i]); + } + memcpy(transform, tmp, sizeof(igtl_uint32)*12); + } +} + + +igtl_uint64 igtl_export igtl_transform_get_crc(igtl_float32* transform) +{ + + igtl_uint64 crc = crc64(0, 0, 0); + + crc = crc64((unsigned char*)transform, sizeof(igtl_float32)*12, crc); + + return crc; +} diff --git a/Source/igtlutil/igtl_fulltransform.h b/Source/igtlutil/igtl_fulltransform.h new file mode 100644 index 00000000..182ff525 --- /dev/null +++ b/Source/igtlutil/igtl_fulltransform.h @@ -0,0 +1,43 @@ +/*========================================================================= + + Program: The OpenIGTLink Library + Language: C + Web page: http://openigtlink.org/ + + 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. + +=========================================================================*/ + +#ifndef __IGTL_TRANSFORM_H +#define __IGTL_TRANSFORM_H + +#include "igtl_win32header.h" +#include "igtl_util.h" + +#define IGTL_TRANSFORM_SIZE 48 + +#ifdef __cplusplus +extern "C" { +#endif + +/* +typedef igtl_float32[12] transform; +*/ + +/** Converts endianness of each member variable + * in igtl_image_header from host byte order to network byte order, + * or vice versa. */ +void igtl_export igtl_transform_convert_byte_order(igtl_float32* transform); + +/** Calculates CRC of transform data. */ +igtl_uint64 igtl_export igtl_transform_get_crc(igtl_float32* transform); + +#ifdef __cplusplus +} +#endif +#endif /*__IGTL_TRANSFORM_H*/ + diff --git a/Source/igtlutil/igtl_util.c b/Source/igtlutil/igtl_util.c index 51cde514..2b95d8e7 100644 --- a/Source/igtlutil/igtl_util.c +++ b/Source/igtlutil/igtl_util.c @@ -233,7 +233,8 @@ void igtl_export igtl_message_dump_hex(FILE* stream, const void* message, int by for (j = 0; j < cols_in_last_row; j ++) { - fprintf(stream, "%02x ", p[i]); + fprintf(stream, "%02x ", *p); + p++; } fprintf(stream, "\n"); diff --git a/Testing/igtlutil/CMakeLists.txt b/Testing/igtlutil/CMakeLists.txt index 42de4157..38e31dd9 100644 --- a/Testing/igtlutil/CMakeLists.txt +++ b/Testing/igtlutil/CMakeLists.txt @@ -15,6 +15,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") ADD_EXECUTABLE(igtl_lbmeta_test igtl_lbmeta_test.c) ADD_EXECUTABLE(igtl_point_test igtl_point_test.c) ADD_EXECUTABLE(igtl_tdata_test igtl_tdata_test.c) + ADD_EXECUTABLE(igtl_fulltdata_test igtl_fulltdata_test.c) ADD_EXECUTABLE(igtl_trajectory_test igtl_trajectory_test.c) ADD_EXECUTABLE(igtl_sensor_test igtl_sensor_test.c) ADD_EXECUTABLE(igtl_string_test igtl_string_test.c) @@ -37,6 +38,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") TARGET_LINK_LIBRARIES(igtl_lbmeta_test OpenIGTLink) TARGET_LINK_LIBRARIES(igtl_point_test OpenIGTLink) TARGET_LINK_LIBRARIES(igtl_tdata_test OpenIGTLink) + TARGET_LINK_LIBRARIES(igtl_fulltdata_test OpenIGTLink) TARGET_LINK_LIBRARIES(igtl_trajectory_test OpenIGTLink) TARGET_LINK_LIBRARIES(igtl_sensor_test OpenIGTLink) TARGET_LINK_LIBRARIES(igtl_string_test OpenIGTLink) @@ -59,6 +61,7 @@ if (${OpenIGTLink_PROTOCOL_VERSION} STREQUAL "2") ADD_TEST( igtl_lbmeta_test_01 ${OpenIGTLink_EXECUTABLE_PATH}/igtl_lbmeta_test ) ADD_TEST( igtl_point_test_01 ${OpenIGTLink_EXECUTABLE_PATH}/igtl_point_test ) ADD_TEST( igtl_tdata_test_01 ${OpenIGTLink_EXECUTABLE_PATH}/igtl_tdata_test ) + ADD_TEST( igtl_fulltdata_test_01 ${OpenIGTLink_EXECUTABLE_PATH}/igtl_fulltdata_test ) ADD_TEST( igtl_trajectory_test_01 ${OpenIGTLink_EXECUTABLE_PATH}/igtl_trajectory_test ) ADD_TEST( igtl_sensor_test_01 ${OpenIGTLink_EXECUTABLE_PATH}/igtl_sensor_test ) ADD_TEST( igtl_string_test_01 ${OpenIGTLink_EXECUTABLE_PATH}/igtl_string_test ) diff --git a/Testing/igtlutil/igtl_fulltdata_test.c b/Testing/igtlutil/igtl_fulltdata_test.c new file mode 100644 index 00000000..2b493df0 --- /dev/null +++ b/Testing/igtlutil/igtl_fulltdata_test.c @@ -0,0 +1,167 @@ +/*========================================================================= + + Program: OpenIGTLink Library + 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 +#include +#include "igtl_types.h" +#include "igtl_header.h" +#include "igtl_tdata.h" +#include "igtl_fulltdata.h" +#include "igtl_util.h" + +/* include test fulltdata data and serialized fulltdata message */ +#include "igtl_test_data_fulltdata.h" + +#define EXIT_SUCCESS 0 +#define EXIT_FAILURE 1 + +#define TEST_TDATA_NUM 3 + +#pragma pack(1) +struct fulltdata_message { + igtl_header header; + igtl_fulltdata_element tlist[TEST_TDATA_NUM]; +}; +#pragma pack() + + +int main( int argc, char * argv [] ) +{ + + struct fulltdata_message message; + int r; + int s; + + // Test structure size + if (sizeof(message) != IGTL_HEADER_SIZE+IGTL_FULLTDATA_ELEMENT_SIZE*TEST_TDATA_NUM) + { + fprintf(stdout, "Invalid size of fulltdata message structure.\n"); + return EXIT_FAILURE; + } + + /* Tracking data 0 */ + strncpy((char*)&(message.tlist[0].name), "Tracker0", 20); + message.tlist[0].type = IGTL_TDATA_TYPE_6D; + message.tlist[0].reserved = 0; + message.tlist[0].transform[0] = -0.954892f; + message.tlist[0].transform[1] = 0.196632f; + message.tlist[0].transform[2] = -0.222525f; + message.tlist[0].transform[3] = 0.0f; + message.tlist[0].transform[4] = -0.196632f; + message.tlist[0].transform[5] = 0.142857f; + message.tlist[0].transform[6] = 0.970014f; + message.tlist[0].transform[7] = 0.0f; + message.tlist[0].transform[8] = 0.222525f; + message.tlist[0].transform[9] = 0.970014f; + message.tlist[0].transform[10] = -0.0977491f; + message.tlist[0].transform[11] = 0.0f; + message.tlist[0].transform[12] = 46.0531f; + message.tlist[0].transform[13] = 19.4709f; + message.tlist[0].transform[14] = 46.0531f; + message.tlist[0].transform[15] = 1.0f; + + message.tlist[0].error = 46.0531f; + + /* Tracking data 1 */ + strncpy((char*)&(message.tlist[1].name), "Tracker1", 20); + message.tlist[1].type = IGTL_TDATA_TYPE_6D; + message.tlist[1].reserved = 0; + message.tlist[1].transform[0] = -0.954892f; + message.tlist[1].transform[1] = 0.196632f; + message.tlist[1].transform[2] = -0.222525f; + message.tlist[1].transform[3] = 0.0f; + message.tlist[1].transform[4] = -0.196632f; + message.tlist[1].transform[5] = 0.142857f; + message.tlist[1].transform[6] = 0.970014f; + message.tlist[1].transform[7] = 0.0f; + message.tlist[1].transform[8] = 0.222525f; + message.tlist[1].transform[9] = 0.970014f; + message.tlist[1].transform[10] = -0.0977491f; + message.tlist[1].transform[11] = 0.0f; + message.tlist[1].transform[12] = 46.0531f; + message.tlist[1].transform[13] = 19.4709f; + message.tlist[1].transform[14] = 46.0531f; + message.tlist[1].transform[15] = 1.0f; + message.tlist[1].error = 46.0531f; + + /* Tracking data 2 */ + strncpy((char*)&(message.tlist[2].name), "Tracker2", 20); + message.tlist[2].type = IGTL_TDATA_TYPE_6D; + message.tlist[2].reserved = 0; + message.tlist[2].transform[0] = -0.954892f; + message.tlist[2].transform[1] = 0.196632f; + message.tlist[2].transform[2] = -0.222525f; + message.tlist[2].transform[3] = 0.0f; + message.tlist[2].transform[4] = -0.196632f; + message.tlist[2].transform[5] = 0.142857f; + message.tlist[2].transform[6] = 0.970014f; + message.tlist[2].transform[7] = 0.0f; + message.tlist[2].transform[8] = 0.222525f; + message.tlist[2].transform[9] = 0.970014f; + message.tlist[2].transform[10] = -0.0977491f; + message.tlist[2].transform[11] = 0.0f; + message.tlist[2].transform[12] = 46.0531f; + message.tlist[2].transform[13] = 19.4709f; + message.tlist[2].transform[14] = 46.0531f; + message.tlist[2].transform[15] = 1.0f; + message.tlist[2].error = 46.0531f; + + /* Swap byte order if necessary */ + igtl_fulltdata_convert_byte_order(message.tlist, TEST_TDATA_NUM); + + /* Create OpenIGTLink header */ + message.header.version = 1; + strncpy( (char*)&(message.header.name), "FULLTDATA", 12 ); + strncpy( (char*)&(message.header.device_name), "DeviceName", 20 ); + message.header.timestamp = 1234567890; + message.header.body_size = IGTL_FULLTDATA_ELEMENT_SIZE*TEST_TDATA_NUM; + message.header.crc = igtl_fulltdata_get_crc(message.tlist, TEST_TDATA_NUM); + igtl_header_convert_byte_order( &(message.header) ); + + /* Dumping data -- for debugging */ + /* + FILE *fp; + fp = fopen("tdata.bin", "w"); + fwrite(&(message), IGTL_HEADER_SIZE+IGTL_TDATA_ELEMENT_SIZE*TEST_TDATA_NUM, 1, fp); + fclose(fp); + */ + + + /* Compare the serialized byte array with the gold standard */ + r = memcmp((const void*)&message, (const void*)test_fulltdata_message, + (size_t)(IGTL_HEADER_SIZE+IGTL_FULLTDATA_ELEMENT_SIZE*TEST_TDATA_NUM)); + + if (r == 0) + { + return EXIT_SUCCESS; + } + else + { + /* Print first 256 bytes as HEX values in STDERR for debug */ + s = IGTL_HEADER_SIZE+IGTL_FULLTDATA_ELEMENT_SIZE*TEST_TDATA_NUM; + if (s > 256) + { + // s = 256; + } + + fprintf(stdout, "\n===== First %d bytes of the test message =====\n", s); + igtl_message_dump_hex(stdout, (const void*)&message, s); + + fprintf(stdout, "\n===== First %d bytes of the gs message =====\n", s); + igtl_message_dump_hex(stdout, (const void*)&test_fulltdata_message, s); + return EXIT_FAILURE; + } +} diff --git a/Testing/igtlutil/igtl_test_data_fulltdata.h b/Testing/igtlutil/igtl_test_data_fulltdata.h new file mode 100644 index 00000000..1d558455 --- /dev/null +++ b/Testing/igtlutil/igtl_test_data_fulltdata.h @@ -0,0 +1,86 @@ +/*========================================================================= + + Program: OpenIGTLLink Library -- Dummy tdata data + 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. + +=========================================================================*/ + +#ifndef __IGTL_TEST_DATA_FULLTDATA_H +#define __IGTL_TEST_DATA_FULLTDATA_H + +char test_fulltdata_message[] = { + /*------- OpenIGTLink message header --------*/ + 0x00, 0x01, /* Version number */ + 0x46, 0x55, 0x4c, 0x4c, 0x54, 0x44, 0x41, 0x54, + 0x41, 0x00, 0x00, 0x00, /* FULLTDATA */ + 0x44, 0x65, 0x76, 0x69, 0x63, 0x65, 0x4e, 0x61, + 0x6d, 0x65, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, /* Device name */ + 0x00, 0x00, 0x00, 0x00, 0x49, 0x96, 0x02, 0xd2, /* Time stamp */ + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x0e, /* Body size */ + 0x87, 0x6d, 0x4f, 0xb9, 0x60, 0x77, 0x63, 0xc2, /* CRC */ + + /*---------- TDATA message body ------------*/ + /* Tracker data 0 */ + 0x54, 0x72, 0x61, 0x63, 0x6b, 0x65, 0x72, 0x30, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, /* Name */ + 0x02, /* Instrument type */ + 0x00, + 0xbf, 0x74, 0x73, 0xcd, 0x3e, 0x49, 0x59, 0xe6, /*4x4 Matrix*/ + 0xbe, 0x63, 0xdd, 0x98, 0x00, 0x00, 0x00, 0x00, + 0xbe, 0x49, 0x59, 0xe6, 0x3e, 0x12, 0x49, 0x1b, + 0x3f, 0x78, 0x52, 0xd6, 0x00, 0x00, 0x00, 0x00, + 0x3e, 0x63, 0xdd, 0x98, 0x3f, 0x78, 0x52, 0xd6, + 0xbd, 0xc8, 0x30, 0xae, 0x00, 0x00, 0x00, 0x00, + 0x42, 0x38, 0x36, 0x60, 0x41, 0x9b, 0xc4, 0x67, + 0x42, 0x38, 0x36, 0x60, 0x3f, 0x80, 0x00, 0x00, + 0x42, 0x38, 0x36, 0x60, /* error */ + + /* Tracker data 1 */ + 0x54, 0x72, 0x61, 0x63, 0x6b, 0x65, 0x72, 0x31, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, /* Name */ + 0x02, /* Instrument type */ + 0x00, + 0xbf, 0x74, 0x73, 0xcd, 0x3e, 0x49, 0x59, 0xe6, /*4x4 Matrix*/ + 0xbe, 0x63, 0xdd, 0x98, 0x00, 0x00, 0x00, 0x00, + 0xbe, 0x49, 0x59, 0xe6, 0x3e, 0x12, 0x49, 0x1b, + 0x3f, 0x78, 0x52, 0xd6, 0x00, 0x00, 0x00, 0x00, + 0x3e, 0x63, 0xdd, 0x98, 0x3f, 0x78, 0x52, 0xd6, + 0xbd, 0xc8, 0x30, 0xae, 0x00, 0x00, 0x00, 0x00, + 0x42, 0x38, 0x36, 0x60, 0x41, 0x9b, 0xc4, 0x67, + 0x42, 0x38, 0x36, 0x60, 0x3f, 0x80, 0x00, 0x00, + 0x42, 0x38, 0x36, 0x60, /* error */ + + /* Tracker data 2 */ + 0x54, 0x72, 0x61, 0x63, 0x6b, 0x65, 0x72, 0x32, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, /* Name */ + 0x02, /* Instrument type */ + 0x00, + 0xbf, 0x74, 0x73, 0xcd, 0x3e, 0x49, 0x59, 0xe6, /*4x4 Matrix*/ + 0xbe, 0x63, 0xdd, 0x98, 0x00, 0x00, 0x00, 0x00, + 0xbe, 0x49, 0x59, 0xe6, 0x3e, 0x12, 0x49, 0x1b, + 0x3f, 0x78, 0x52, 0xd6, 0x00, 0x00, 0x00, 0x00, + 0x3e, 0x63, 0xdd, 0x98, 0x3f, 0x78, 0x52, 0xd6, + 0xbd, 0xc8, 0x30, 0xae, 0x00, 0x00, 0x00, 0x00, + 0x42, 0x38, 0x36, 0x60, 0x41, 0x9b, 0xc4, 0x67, + 0x42, 0x38, 0x36, 0x60, 0x3f, 0x80, 0x00, 0x00, + 0x42, 0x38, 0x36, 0x60, /* error */ + +}; + +#endif /* __IGTL_TEST_DATA_FULLTDATA_H */ + + +