re #1010 Adding embedded image transform to tracked frame message

Optimizing message sending and receiving by not creating a factory for each receive

Adjusting timeout to be based in seconds

Author: Adam Rankin

PlusLib/src/PlusCommon/PlusTrackedFrame.cxx

@@ -98,6 +98,7 @@ PlusStatus PlusTrackedFrame::PrintToXML( vtkXMLDataElement* trackedFrame )
   trackedFrame->SetName( "TrackedFrame" );
   trackedFrame->SetDoubleAttribute( "Timestamp", this->Timestamp );
   trackedFrame->SetAttribute( "ImageDataValid", ( this->GetImageData()->IsImageValid() ? "true" : "false" ) );
+
   if ( this->GetImageData()->IsImageValid() )
   {
     trackedFrame->SetIntAttribute( "NumberOfBits", this->GetNumberOfBitsPerScalar() );
@@ -112,6 +113,7 @@ PlusStatus PlusTrackedFrame::PrintToXML( vtkXMLDataElement* trackedFrame )
     int frameSizeSigned[3] = { static_cast<int>( FrameSize[0] ), static_cast<int>( FrameSize[1] ), static_cast<int>( FrameSize[2] ) };
     trackedFrame->SetVectorAttribute( "FrameSize", 3, frameSizeSigned );
   }
+
   for ( FieldMapType::const_iterator it = this->CustomFrameFields.begin(); it != this->CustomFrameFields.end(); it++ )
   {
     vtkSmartPointer<vtkXMLDataElement> customField = vtkSmartPointer<vtkXMLDataElement>::New();
@@ -153,9 +155,11 @@ PlusStatus PlusTrackedFrame::PrintToXML( vtkXMLDataElement* trackedFrame )
       pointElement->SetVectorAttribute( "Position", 3, point );
       segmentedPoints->AddNestedElement( pointElement );
     }
+
     segmentation->AddNestedElement( segmentedPoints );
     trackedFrame->AddNestedElement( segmentation );
   }
+
   return PLUS_SUCCESS;
 }
 

PlusLib/src/PlusDataCollection/OpenIGTLink/vtkPlusOpenIGTLinkDevice.cxx

@@ -20,7 +20,7 @@ vtkPlusOpenIGTLinkDevice::vtkPlusOpenIGTLinkDevice()
   , ServerAddress( NULL )
   , ServerPort( -1 )
   , IgtlMessageCrcCheckEnabled( 0 )
-  , ReceiveTimeoutSec(0.5)
+  , ReceiveTimeoutSec( 0.5 )
   , NumberOfRetryAttempts( 3 ) // try a few times, but adding of data items is blocked while trying to reconnect, so don't make it too long
   , DelayBetweenRetryAttemptsSec( 0.100 ) // there is already a delay with a CLIENT_SOCKET_TIMEOUT_MSEC timeout, so we just add a little extra idle delay
   , ClientSocket( igtl::ClientSocket::New() )
@@ -142,7 +142,7 @@ PlusStatus vtkPlusOpenIGTLinkDevice::ClientSocketReconnect()
     LOG_DEBUG( "Client successfully connected to server (" << this->ServerAddress << ":" << this->ServerPort << ")."  );
   }
 
-  this->ClientSocket->SetTimeout(this->ReceiveTimeoutSec*1000); // *1000 because SetTimeout expects msec
+  this->ClientSocket->SetTimeout( this->ReceiveTimeoutSec * 1000 ); // *1000 because SetTimeout expects msec
 
   return SendRequestedMessageTypes();
 }

PlusLib/src/PlusDataCollection/OpenIGTLink/vtkPlusOpenIGTLinkVideoSource.cxx

@@ -16,11 +16,11 @@ See License.txt for details.
 #include "vtkPlusDataSource.h"
 #include "vtkPlusIgtlMessageCommon.h"
 
-vtkStandardNewMacro(vtkPlusOpenIGTLinkVideoSource);
+vtkStandardNewMacro( vtkPlusOpenIGTLinkVideoSource );
 
 //----------------------------------------------------------------------------
 vtkPlusOpenIGTLinkVideoSource::vtkPlusOpenIGTLinkVideoSource()
-  : IgtlMessageFactory(vtkSmartPointer<vtkPlusIgtlMessageFactory>::New())
+  : IgtlMessageFactory( vtkSmartPointer<vtkPlusIgtlMessageFactory>::New() )
 {
   this->RequireImageOrientationInConfiguration = true;
 }
@@ -31,118 +31,119 @@ vtkPlusOpenIGTLinkVideoSource::~vtkPlusOpenIGTLinkVideoSource()
 }
 
 //----------------------------------------------------------------------------
-void vtkPlusOpenIGTLinkVideoSource::PrintSelf(ostream& os, vtkIndent indent)
+void vtkPlusOpenIGTLinkVideoSource::PrintSelf( ostream& os, vtkIndent indent )
 {
-  this->Superclass::PrintSelf(os,indent);
+  this->Superclass::PrintSelf( os, indent );
 }
 
 //----------------------------------------------------------------------------
 PlusStatus vtkPlusOpenIGTLinkVideoSource::InternalUpdate()
 {
   LOG_TRACE( "vtkPlusOpenIGTLinkVideoSource::InternalUpdate" );
-  if (!this->Recording)
+  if ( !this->Recording )
   {
     // drop the frame, we are not recording data now
     return PLUS_SUCCESS;
   }
 
   igtl::MessageHeader::Pointer headerMsg;
-  ReceiveMessageHeaderWithErrorHandling(headerMsg);
-  
-  if (headerMsg.IsNull())
+  ReceiveMessageHeaderWithErrorHandling( headerMsg );
+
+  if ( headerMsg.IsNull() )
   {
     OnReceiveTimeout();
     return PLUS_FAIL;
   }
 
   // We've received valid header data
-  headerMsg->Unpack(this->IgtlMessageCrcCheckEnabled);
+  headerMsg->Unpack( this->IgtlMessageCrcCheckEnabled );
 
   // Set unfiltered and filtered timestamp by converting UTC to system timestamp
   double unfilteredTimestamp = vtkPlusAccurateTimer::GetSystemTime();
 
   PlusTrackedFrame trackedFrame;
-  igtl::MessageBase::Pointer bodyMsg = IgtlMessageFactory->CreateReceiveMessage(headerMsg);
+  vtkSmartPointer<vtkMatrix4x4> embeddedImageTransform;
+  igtl::MessageBase::Pointer bodyMsg = IgtlMessageFactory->CreateReceiveMessage( headerMsg );
 
-  if ( typeid(*bodyMsg) == typeid(igtl::ImageMessage) )
+  if ( typeid( *bodyMsg ) == typeid( igtl::ImageMessage ) )
   {
-    if (vtkPlusIgtlMessageCommon::UnpackImageMessage( bodyMsg, this->ClientSocket, trackedFrame, this->ImageMessageEmbeddedTransformName, this->IgtlMessageCrcCheckEnabled)!=PLUS_SUCCESS)
+    if ( vtkPlusIgtlMessageCommon::UnpackImageMessage( bodyMsg, this->ClientSocket, trackedFrame, this->ImageMessageEmbeddedTransformName, this->IgtlMessageCrcCheckEnabled ) != PLUS_SUCCESS )
     {
-      LOG_ERROR("Couldn't get image from OpenIGTLink server!"); 
+      LOG_ERROR( "Couldn't get image from OpenIGTLink server!" );
       return PLUS_FAIL;
     }
   }
-  else if ( typeid(*bodyMsg) == typeid(igtl::PlusTrackedFrameMessage) )
+  else if ( typeid( *bodyMsg ) == typeid( igtl::PlusTrackedFrameMessage ) )
   {
-    if ( vtkPlusIgtlMessageCommon::UnpackTrackedFrameMessage( bodyMsg, this->ClientSocket, trackedFrame, this->IgtlMessageCrcCheckEnabled ) != PLUS_SUCCESS )
+    if ( vtkPlusIgtlMessageCommon::UnpackTrackedFrameMessage( bodyMsg, this->ClientSocket, trackedFrame, embeddedImageTransform, this->IgtlMessageCrcCheckEnabled ) != PLUS_SUCCESS )
     {
-      LOG_ERROR("Couldn't get tracked frame from OpenIGTLink server!"); 
-      return PLUS_FAIL; 
+      LOG_ERROR( "Couldn't get tracked frame from OpenIGTLink server!" );
+      return PLUS_FAIL;
     }
     double unfilteredTimestampUtc = trackedFrame.GetTimestamp();
-    if (this->UseReceivedTimestamps)
+    if ( this->UseReceivedTimestamps )
     {
       // Use the timestamp in the OpenIGTLink message
       // The received timestamp is in UTC and timestamps in the buffer are in system time, so conversion is needed
-      unfilteredTimestamp = vtkPlusAccurateTimer::GetSystemTimeFromUniversalTime(unfilteredTimestampUtc); 
+      unfilteredTimestamp = vtkPlusAccurateTimer::GetSystemTimeFromUniversalTime( unfilteredTimestampUtc );
     }
   }
   else
   {
-    // if the data type is unknown, skip reading. 
-    this->ClientSocket->Skip(headerMsg->GetBodySizeToRead(), 0);
-    return PLUS_SUCCESS; 
+    // if the data type is unknown, skip reading.
+    this->ClientSocket->Skip( headerMsg->GetBodySizeToRead(), 0 );
+    return PLUS_SUCCESS;
   }
 
-  // No need to filter already filtered timestamped items received over OpenIGTLink 
+  // No need to filter already filtered timestamped items received over OpenIGTLink
   // If the original timestamps are not used it's still safer not to use filtering, as filtering assumes uniform frame rate, which is not guaranteed
   double filteredTimestamp = unfilteredTimestamp;
 
-  // The timestamps are already defined, so we don't need to filter them, 
+  // The timestamps are already defined, so we don't need to filter them,
   // for simplicity, we increase frame number always by 1.
   this->FrameNumber++;
 
-  vtkPlusDataSource* aSource=NULL;
-  if( this->GetFirstActiveOutputVideoSource(aSource) != PLUS_SUCCESS )
+  vtkPlusDataSource* aSource = NULL;
+  if( this->GetFirstActiveOutputVideoSource( aSource ) != PLUS_SUCCESS )
   {
-    LOG_ERROR("Unable to retrieve the video source in the OpenIGTLinkVideo device.");
+    LOG_ERROR( "Unable to retrieve the video source in the OpenIGTLinkVideo device." );
     return PLUS_FAIL;
   }
 
-  // If the buffer is empty, set the pixel type and frame size to the first received properties 
+  // If the buffer is empty, set the pixel type and frame size to the first received properties
   if ( aSource->GetNumberOfItems() == 0 )
   {
-    PlusVideoFrame* videoFrame=trackedFrame.GetImageData();
-    if (videoFrame==NULL)
+    PlusVideoFrame* videoFrame = trackedFrame.GetImageData();
+    if ( videoFrame == NULL )
     {
-      LOG_ERROR("Invalid video frame received, cannot use it to initialize the video buffer");
+      LOG_ERROR( "Invalid video frame received, cannot use it to initialize the video buffer" );
       return PLUS_FAIL;
     }
     aSource->SetPixelType( videoFrame->GetVTKScalarPixelType() );
     aSource->SetNumberOfScalarComponents( videoFrame->GetNumberOfScalarComponents() );
     aSource->SetImageType( videoFrame->GetImageType() );
     aSource->SetInputFrameSize( trackedFrame.GetFrameSize() );
   }
-  PlusTrackedFrame::FieldMapType customFields=trackedFrame.GetCustomFields();
-  PlusStatus status = aSource->AddItem( trackedFrame.GetImageData(), this->FrameNumber, unfilteredTimestamp, filteredTimestamp, &customFields); 
+  PlusTrackedFrame::FieldMapType customFields = trackedFrame.GetCustomFields();
+  PlusStatus status = aSource->AddItem( trackedFrame.GetImageData(), this->FrameNumber, unfilteredTimestamp, filteredTimestamp, &customFields );
   this->Modified();
 
   return status;
 }
 
 //-----------------------------------------------------------------------------
-PlusStatus vtkPlusOpenIGTLinkVideoSource::ReadConfiguration(vtkXMLDataElement* rootConfigElement)
+PlusStatus vtkPlusOpenIGTLinkVideoSource::ReadConfiguration( vtkXMLDataElement* rootConfigElement )
 {
-  XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_READING(deviceConfig, rootConfigElement);
-  XML_READ_STRING_ATTRIBUTE_OPTIONAL(ImageMessageEmbeddedTransformName, deviceConfig);
+  XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_READING( deviceConfig, rootConfigElement );
+  XML_READ_STRING_ATTRIBUTE_OPTIONAL( ImageMessageEmbeddedTransformName, deviceConfig );
   return PLUS_SUCCESS;
 }
 
 //----------------------------------------------------------------------------
-PlusStatus vtkPlusOpenIGTLinkVideoSource::WriteConfiguration(vtkXMLDataElement* rootConfigElement)
+PlusStatus vtkPlusOpenIGTLinkVideoSource::WriteConfiguration( vtkXMLDataElement* rootConfigElement )
 {
-  XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_WRITING(deviceConfig, rootConfigElement);
-  deviceConfig->SetAttribute("ImageMessageEmbeddedTransformName", this->ImageMessageEmbeddedTransformName.GetTransformName().c_str());
+  XML_FIND_DEVICE_ELEMENT_REQUIRED_FOR_WRITING( deviceConfig, rootConfigElement );
+  deviceConfig->SetAttribute( "ImageMessageEmbeddedTransformName", this->ImageMessageEmbeddedTransformName.GetTransformName().c_str() );
   return PLUS_SUCCESS;
 }
 
@@ -151,22 +152,22 @@ PlusStatus vtkPlusOpenIGTLinkVideoSource::NotifyConfigured()
 {
   if( this->OutputChannels.size() > 1 )
   {
-    LOG_WARNING("vtkPlusOpenIGTLinkVideoSource is expecting one output channel and there are " << this->OutputChannels.size() << " channels. First output channel will be used.");
+    LOG_WARNING( "vtkPlusOpenIGTLinkVideoSource is expecting one output channel and there are " << this->OutputChannels.size() << " channels. First output channel will be used." );
     return PLUS_FAIL;
   }
 
   if( this->OutputChannels.empty() )
   {
-    LOG_ERROR("No output channels defined for vtkPlusOpenIGTLinkVideoSource. Cannot proceed." );
-    this->SetCorrectlyConfigured(false);
+    LOG_ERROR( "No output channels defined for vtkPlusOpenIGTLinkVideoSource. Cannot proceed." );
+    this->SetCorrectlyConfigured( false );
     return PLUS_FAIL;
   }
 
   return PLUS_SUCCESS;
 }
 
 //----------------------------------------------------------------------------
-PlusStatus vtkPlusOpenIGTLinkVideoSource::SetImageMessageEmbeddedTransformName(const char* nameString)
+PlusStatus vtkPlusOpenIGTLinkVideoSource::SetImageMessageEmbeddedTransformName( const char* nameString )
 {
-  return this->ImageMessageEmbeddedTransformName.SetTransformName(nameString);
+  return this->ImageMessageEmbeddedTransformName.SetTransformName( nameString );
 }

PlusLib/src/PlusOpenIGTLink/igtlPlusTrackedFrameMessage.cxx

@@ -7,6 +7,7 @@ See License.txt for details.
 #include "PlusConfigure.h"
 #include "igtlPlusTrackedFrameMessage.h"
 #include "vtkImageData.h"
+#include "vtkMatrix4x4.h"
 #include "vtkPlusIgtlMessageFactory.h"
 
 namespace igtl
@@ -98,6 +99,34 @@ namespace igtl
     return this->m_TrackedFrame;
   }
 
+  //----------------------------------------------------------------------------
+  PlusStatus PlusTrackedFrameMessage::SetEmbeddedImageTransform(vtkSmartPointer<vtkMatrix4x4> matrix)
+  {
+    for (int i = 0; i < 4; ++i)
+    {
+      for (int j = 0; j < 4; ++j)
+      {
+        m_MessageHeader.m_EmbeddedImageTransform[i][j] = matrix->GetElement(i, j);
+      }
+    }
+    
+    return PLUS_SUCCESS;
+  }
+
+  //----------------------------------------------------------------------------
+  vtkSmartPointer<vtkMatrix4x4> PlusTrackedFrameMessage::GetEmbeddedImageTransform()
+  {
+    vtkSmartPointer<vtkMatrix4x4> mat(vtkSmartPointer<vtkMatrix4x4>::New());
+    for (int i = 0; i < 4; ++i)
+    {
+      for (int j = 0; j < 4; ++j)
+      {
+        mat->SetElement(i, j, m_MessageHeader.m_EmbeddedImageTransform[i][j]);
+      }
+    }
+    return mat;
+  }
+
   //----------------------------------------------------------------------------
   int PlusTrackedFrameMessage::CalculateContentBufferSize()
   {
@@ -121,6 +150,7 @@ namespace igtl
     header->m_FrameSize[2] = this->m_MessageHeader.m_FrameSize[2];
     header->m_ImageDataSizeInBytes = this->m_MessageHeader.m_ImageDataSizeInBytes;
     header->m_XmlDataSizeInBytes = this->m_MessageHeader.m_XmlDataSizeInBytes;
+    memcpy( header->m_EmbeddedImageTransform, this->m_MessageHeader.m_EmbeddedImageTransform, sizeof( igtl::Matrix4x4 ) );
 
     // Copy xml data
     char* xmlData = ( char* )( this->m_Content + header->GetMessageHeaderSize() );
@@ -159,6 +189,7 @@ namespace igtl
     this->m_MessageHeader.m_FrameSize[2] = header->m_FrameSize[2];
     this->m_MessageHeader.m_ImageDataSizeInBytes = header->m_ImageDataSizeInBytes;
     this->m_MessageHeader.m_XmlDataSizeInBytes = header->m_XmlDataSizeInBytes;
+    memcpy( this->m_MessageHeader.m_EmbeddedImageTransform, header->m_EmbeddedImageTransform, sizeof( igtl::Matrix4x4 ) );
 
     // Copy xml data
     char* xmlData = ( char* )( this->m_Content + header->GetMessageHeaderSize() );