From 49284014c35327ce291c8d44baaa68c1c0450a3c Mon Sep 17 00:00:00 2001 From: Simon Drouin Date: Mon, 1 Feb 2016 15:16:13 -0500 Subject: [PATCH] Added a converter for the TRANSFORM message. --- Converter/CMakeLists.txt | 1 + Converter/igtlTransformConverter.cxx | 132 ++++++++++++++++++++++++++ Converter/igtlTransformConverter.h | 82 ++++++++++++++++ MRML/vtkIGTLToMRMLLinearTransform.cxx | 110 ++++----------------- MRML/vtkIGTLToMRMLLinearTransform.h | 2 + 5 files changed, 236 insertions(+), 91 deletions(-) create mode 100644 Converter/igtlTransformConverter.cxx create mode 100644 Converter/igtlTransformConverter.h diff --git a/Converter/CMakeLists.txt b/Converter/CMakeLists.txt index ff0c876..f61fdd2 100644 --- a/Converter/CMakeLists.txt +++ b/Converter/CMakeLists.txt @@ -22,6 +22,7 @@ set(${KIT}_INCLUDE_DIRECTORIES set(${KIT}_SRCS igtlImageConverter.cxx igtlPolyDataConverter.cxx + igtlTransformConverter.cxx ) set(${KIT}_TARGET_LIBRARIES diff --git a/Converter/igtlTransformConverter.cxx b/Converter/igtlTransformConverter.cxx new file mode 100644 index 0000000..1031ed9 --- /dev/null +++ b/Converter/igtlTransformConverter.cxx @@ -0,0 +1,132 @@ +#include "igtlTransformConverter.h" +#include + +namespace igtl +{ + +TransformConverter::TransformConverter() +{ +} + +//--------------------------------------------------------------------------- +TransformConverter::~TransformConverter() +{ +} + +//--------------------------------------------------------------------------- +void TransformConverter::PrintSelf(std::ostream &os) const +{ + this->LightObject::PrintSelf(os); +} + +//--------------------------------------------------------------------------- +int TransformConverter::IGTLToVTK(igtl::MessageBase::Pointer source, MessageContent* dest, bool checkCRC) +{ + // Create a message buffer to receive transform data + igtl::TransformMessage::Pointer transMsg; + transMsg = igtl::TransformMessage::New(); + transMsg->Copy(source); // !! TODO: copy makes performance issue. + + // Deserialize the transform data + // If CheckCRC==0, CRC check is skipped. + int c = transMsg->Unpack(checkCRC); + + if (!(c & igtl::MessageHeader::UNPACK_BODY)) // if CRC check fails + { + // TODO: error handling + return 0; + } + + vtkSmartPointer mat = vtkSmartPointer::New(); + + if (!this->IGTLToVTKMatrix4x4(transMsg, mat)) + return 0; + + dest->matrix = mat; + dest->deviceName = transMsg->GetDeviceName(); + + return 1; +} + +//--------------------------------------------------------------------------- +int TransformConverter::IGTLToVTKMatrix4x4(TransformMessage::Pointer transformMsg, vtkSmartPointer transform ) +{ + igtl::Matrix4x4 matrix; + transformMsg->GetMatrix(matrix); + + float tx = matrix[0][0]; + float ty = matrix[1][0]; + float tz = matrix[2][0]; + float sx = matrix[0][1]; + float sy = matrix[1][1]; + float sz = matrix[2][1]; + float nx = matrix[0][2]; + float ny = matrix[1][2]; + float nz = matrix[2][2]; + float px = matrix[0][3]; + float py = matrix[1][3]; + float pz = matrix[2][3]; + + //std::cerr << "\n\nmatrix = " << std::endl; + //std::cerr << tx << ", " << ty << ", " << tz << std::endl; + //std::cerr << sx << ", " << sy << ", " << sz << std::endl; + //std::cerr << nx << ", " << ny << ", " << nz << std::endl; + //std::cerr << px << ", " << py << ", " << pz << std::endl; + + // set volume orientation + transform->Identity(); + transform->Element[0][0] = tx; + transform->Element[1][0] = ty; + transform->Element[2][0] = tz; + transform->Element[0][1] = sx; + transform->Element[1][1] = sy; + transform->Element[2][1] = sz; + transform->Element[0][2] = nx; + transform->Element[1][2] = ny; + transform->Element[2][2] = nz; + transform->Element[0][3] = px; + transform->Element[1][3] = py; + transform->Element[2][3] = pz; + + return 1; +} + +//--------------------------------------------------------------------------- +int TransformConverter::VTKToIGTL(const MessageContent& source, igtl::TransformMessage::Pointer* dest) +{ + // Make sure we have a valid transform message + if (dest->IsNull()) + *dest = igtl::TransformMessage::New(); + igtl::TransformMessage::Pointer msg = *dest; + + // Set device name + msg->SetDeviceName( source.deviceName.c_str() ); + + // Convert matrix + vtkSmartPointer matrix = source.matrix; + igtl::Matrix4x4 igtlmatrix; + + igtlmatrix[0][0] = matrix->Element[0][0]; + igtlmatrix[1][0] = matrix->Element[1][0]; + igtlmatrix[2][0] = matrix->Element[2][0]; + igtlmatrix[3][0] = matrix->Element[3][0]; + igtlmatrix[0][1] = matrix->Element[0][1]; + igtlmatrix[1][1] = matrix->Element[1][1]; + igtlmatrix[2][1] = matrix->Element[2][1]; + igtlmatrix[3][1] = matrix->Element[3][1]; + igtlmatrix[0][2] = matrix->Element[0][2]; + igtlmatrix[1][2] = matrix->Element[1][2]; + igtlmatrix[2][2] = matrix->Element[2][2]; + igtlmatrix[3][2] = matrix->Element[3][2]; + igtlmatrix[0][3] = matrix->Element[0][3]; + igtlmatrix[1][3] = matrix->Element[1][3]; + igtlmatrix[2][3] = matrix->Element[2][3]; + igtlmatrix[3][3] = matrix->Element[3][3]; + + msg->SetMatrix(igtlmatrix); + msg->Pack(); + + return 1; +} + +} // namespace igtl diff --git a/Converter/igtlTransformConverter.h b/Converter/igtlTransformConverter.h new file mode 100644 index 0000000..d3adb10 --- /dev/null +++ b/Converter/igtlTransformConverter.h @@ -0,0 +1,82 @@ +/*========================================================================== + + Portions (c) Copyright 2008-2009 Brigham and Women's Hospital (BWH) All Rights Reserved. + + See Doc/copyright/copyright.txt + or http://www.slicer.org/copyright/copyright.txt for details. + + Program: 3D Slicer + Module: $HeadURL: http://svn.slicer.org/Slicer3/trunk/Modules/OpenIGTLinkIF/vtkIGTLToMRMLImage.h $ + Date: $Date: 2010-11-23 00:58:13 -0500 (Tue, 23 Nov 2010) $ + Version: $Revision: 15552 $ + +==========================================================================*/ + +#ifndef __igtlTransformConverter_h +#define __igtlTransformConverter_h + +#include "igtlSupportModuleConverterExport.h" + +// OpenIGTLink includes +#include + +// VTK includes +#include +#include + +class vtkMatrix4x4; + +#include "igtlLightObject.h" + +namespace igtl +{ + +/** Conversion between igtl::TransformMessage and vtk classes. + * + */ +class OPENIGTLINK_SUPPORT_MODULE_CONVERTER_EXPORT TransformConverter : public LightObject +{ +public: + /** Standard class typedefs. */ + typedef TransformConverter Self; + typedef LightObject Superclass; + typedef SmartPointer Pointer; + typedef SmartPointer ConstPointer; + + /** Method for creation through the object factory. */ + igtlNewMacro(Self); + + /** Run-time type information (and related methods). */ + igtlTypeMacro(TransformConverter, LightObject); + + /** + * This structure contains everything that igtl::PolyMessage is able to contain, + * in a vtk-friendly format. + */ + struct MessageContent + { + vtkSmartPointer matrix; + std::string deviceName; + }; + + virtual void PrintSelf(std::ostream& os) const; + + virtual const char* GetIGTLName() { return "TRANSFORM"; }; + + virtual int IGTLToVTK(igtl::MessageBase::Pointer source, MessageContent* dest, bool checkCRC); + virtual int VTKToIGTL(const MessageContent& source, igtl::TransformMessage::Pointer* dest); + +protected: + TransformConverter(); + ~TransformConverter(); + +private: + // Extract vtkMatrix4x4 from existing transformMsg, insert into existing vtkMatrix4x4. + int IGTLToVTKMatrix4x4(TransformMessage::Pointer transformMsg, vtkSmartPointer mat); + +}; + +} + + +#endif //__igtlTransformConverter_h diff --git a/MRML/vtkIGTLToMRMLLinearTransform.cxx b/MRML/vtkIGTLToMRMLLinearTransform.cxx index 8949230..e92bdab 100644 --- a/MRML/vtkIGTLToMRMLLinearTransform.cxx +++ b/MRML/vtkIGTLToMRMLLinearTransform.cxx @@ -50,6 +50,7 @@ vtkStandardNewMacro(vtkIGTLToMRMLLinearTransform); //--------------------------------------------------------------------------- vtkIGTLToMRMLLinearTransform::vtkIGTLToMRMLLinearTransform() { + Converter = igtl::TransformConverter::New(); } //--------------------------------------------------------------------------- @@ -100,73 +101,17 @@ int vtkIGTLToMRMLLinearTransform::IGTLToMRML(igtl::MessageBase::Pointer buffer, { vtkIGTLToMRMLBase::IGTLToMRML(buffer, node); - // Create a message buffer to receive transform data - igtl::TransformMessage::Pointer transMsg; - transMsg = igtl::TransformMessage::New(); - transMsg->Copy(buffer); // !! TODO: copy makes performance issue. + vtkMRMLLinearTransformNode * transformNode = vtkMRMLLinearTransformNode::SafeDownCast(node); + if (!transformNode) + return 0; - // Deserialize the transform data - // If CheckCRC==0, CRC check is skipped. - int c = transMsg->Unpack(this->CheckCRC); + igtl::TransformConverter::MessageContent content; + if (Converter->IGTLToVTK(buffer, &content, this->CheckCRC) == 0) + { + return 0; + } - if (!(c & igtl::MessageHeader::UNPACK_BODY)) // if CRC check fails - { - // TODO: error handling - return 0; - } - - if (node == NULL) - { - return 0; - } - - vtkMRMLLinearTransformNode* transformNode = - vtkMRMLLinearTransformNode::SafeDownCast(node); - - igtl::Matrix4x4 matrix; - transMsg->GetMatrix(matrix); - - float tx = matrix[0][0]; - float ty = matrix[1][0]; - float tz = matrix[2][0]; - float sx = matrix[0][1]; - float sy = matrix[1][1]; - float sz = matrix[2][1]; - float nx = matrix[0][2]; - float ny = matrix[1][2]; - float nz = matrix[2][2]; - float px = matrix[0][3]; - float py = matrix[1][3]; - float pz = matrix[2][3]; - - //std::cerr << "\n\nmatrix = " << std::endl; - //std::cerr << tx << ", " << ty << ", " << tz << std::endl; - //std::cerr << sx << ", " << sy << ", " << sz << std::endl; - //std::cerr << nx << ", " << ny << ", " << nz << std::endl; - //std::cerr << px << ", " << py << ", " << pz << std::endl; - - // set volume orientation - vtkNew transform; - transform->Identity(); - transform->Element[0][0] = tx; - transform->Element[1][0] = ty; - transform->Element[2][0] = tz; - transform->Element[0][1] = sx; - transform->Element[1][1] = sy; - transform->Element[2][1] = sz; - transform->Element[0][2] = nx; - transform->Element[1][2] = ny; - transform->Element[2][2] = nz; - transform->Element[0][3] = px; - transform->Element[1][3] = py; - transform->Element[2][3] = pz; - - transformNode->SetMatrixTransformToParent(transform.GetPointer()); - - //std::cerr << "IGTL matrix = " << std::endl; - //transform->Print(cerr); - //std::cerr << "MRML matrix = " << std::endl; - //transformToParent->Print(cerr); + transformNode->SetMatrixTransformToParent(content.matrix.GetPointer()); return 1; @@ -179,38 +124,21 @@ int vtkIGTLToMRMLLinearTransform::MRMLToIGTL(unsigned long event, vtkMRMLNode* m { vtkMRMLLinearTransformNode* transformNode = vtkMRMLLinearTransformNode::SafeDownCast(mrmlNode); - vtkNew matrix; - transformNode->GetMatrixTransformToParent(matrix.GetPointer()); - //igtl::TransformMessage::Pointer OutTransformMsg; + igtl::TransformConverter::MessageContent content; + content.matrix = vtkMatrix4x4::New(); + transformNode->GetMatrixTransformToParent(content.matrix.GetPointer()); + content.deviceName = mrmlNode->GetName(); + if (this->OutTransformMsg.IsNull()) { this->OutTransformMsg = igtl::TransformMessage::New(); } - this->OutTransformMsg->SetDeviceName(mrmlNode->GetName()); - - igtl::Matrix4x4 igtlmatrix; - - igtlmatrix[0][0] = matrix->Element[0][0]; - igtlmatrix[1][0] = matrix->Element[1][0]; - igtlmatrix[2][0] = matrix->Element[2][0]; - igtlmatrix[3][0] = matrix->Element[3][0]; - igtlmatrix[0][1] = matrix->Element[0][1]; - igtlmatrix[1][1] = matrix->Element[1][1]; - igtlmatrix[2][1] = matrix->Element[2][1]; - igtlmatrix[3][1] = matrix->Element[3][1]; - igtlmatrix[0][2] = matrix->Element[0][2]; - igtlmatrix[1][2] = matrix->Element[1][2]; - igtlmatrix[2][2] = matrix->Element[2][2]; - igtlmatrix[3][2] = matrix->Element[3][2]; - igtlmatrix[0][3] = matrix->Element[0][3]; - igtlmatrix[1][3] = matrix->Element[1][3]; - igtlmatrix[2][3] = matrix->Element[2][3]; - igtlmatrix[3][3] = matrix->Element[3][3]; - - this->OutTransformMsg->SetMatrix(igtlmatrix); - this->OutTransformMsg->Pack(); + if (Converter->VTKToIGTL(content,&(this->OutTransformMsg)) == 0) + { + return 0; + } *size = this->OutTransformMsg->GetPackSize(); *igtlMsg = (void*)this->OutTransformMsg->GetPackPointer(); diff --git a/MRML/vtkIGTLToMRMLLinearTransform.h b/MRML/vtkIGTLToMRMLLinearTransform.h index 1c3af5c..7e6bbab 100644 --- a/MRML/vtkIGTLToMRMLLinearTransform.h +++ b/MRML/vtkIGTLToMRMLLinearTransform.h @@ -21,6 +21,7 @@ // OpenIGTLink includes #include +#include "igtlTransformConverter.h" // MRML includes #include @@ -60,6 +61,7 @@ class VTK_SLICER_OPENIGTLINKIF_MODULE_MRML_EXPORT vtkIGTLToMRMLLinearTransform : protected: igtl::TransformMessage::Pointer OutTransformMsg; + igtl::TransformConverter::Pointer Converter; };