diff --git a/Modules/IGT/Algorithms/mitkNavigationDataToIGTLMessageFilter.cpp b/Modules/IGT/Algorithms/mitkNavigationDataToIGTLMessageFilter.cpp
index 0a0e7cc46b..55e82f25da 100644
--- a/Modules/IGT/Algorithms/mitkNavigationDataToIGTLMessageFilter.cpp
+++ b/Modules/IGT/Algorithms/mitkNavigationDataToIGTLMessageFilter.cpp
@@ -1,360 +1,354 @@
/*===================================================================
The Medical Imaging Interaction Toolkit (MITK)
Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.
This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.
See LICENSE.txt or http://www.mitk.org for details.
===================================================================*/
#include "mitkNavigationDataToIGTLMessageFilter.h"
#include "igtlQuaternionTrackingDataMessage.h"
#include "igtlTrackingDataMessage.h"
#include "igtlTransformMessage.h"
#include "igtlPositionMessage.h"
#include <mitkInteractionConst.h>
#include <itksys/SystemTools.hxx>
mitk::NavigationDataToIGTLMessageFilter::NavigationDataToIGTLMessageFilter()
{
mitk::IGTLMessage::Pointer output = mitk::IGTLMessage::New();
this->SetNumberOfRequiredOutputs(1);
this->SetNthOutput(0, output.GetPointer());
this->SetNumberOfRequiredInputs(1);
// m_OperationMode = Mode3D;
m_CurrentTimeStep = 0;
// m_RingBufferSize = 50; //the default ring buffer size
// m_NumberForMean = 100;
}
mitk::NavigationDataToIGTLMessageFilter::~NavigationDataToIGTLMessageFilter()
{
}
void mitk::NavigationDataToIGTLMessageFilter::GenerateData()
{
switch (m_OperationMode)
{
case ModeSendQTDataMsg:
this->GenerateDataModeSendQTDataMsg();
break;
case ModeSendTDataMsg:
this->GenerateDataModeSendTDataMsg();
break;
case ModeSendQTransMsg:
this->GenerateDataModeSendQTransMsg();
break;
case ModeSendTransMsg:
this->GenerateDataModeSendTransMsg();
break;
default:
break;
}
igtl::MessageBase::Pointer curMessage = this->GetOutput()->GetMessage();
if (dynamic_cast<igtl::TrackingDataMessage*>(curMessage.GetPointer()) != nullptr)
{
igtl::TrackingDataMessage* tdMsg =
(igtl::TrackingDataMessage*)(curMessage.GetPointer());
}
}
void mitk::NavigationDataToIGTLMessageFilter::SetInput(const NavigationData* nd)
{
// Process object is not const-correct so the const_cast is required here
this->ProcessObject::SetNthInput(0, const_cast<NavigationData*>(nd));
this->CreateOutputsForAllInputs();
}
void mitk::NavigationDataToIGTLMessageFilter::SetInput(unsigned int idx, const NavigationData* nd)
{
// Process object is not const-correct so the const_cast is required here
this->ProcessObject::SetNthInput(idx, const_cast<NavigationData*>(nd));
this->CreateOutputsForAllInputs();
}
const mitk::NavigationData* mitk::NavigationDataToIGTLMessageFilter::GetInput(void)
{
if (this->GetNumberOfInputs() < 1)
return NULL;
return static_cast<const NavigationData*>(this->ProcessObject::GetInput(0));
}
const mitk::NavigationData* mitk::NavigationDataToIGTLMessageFilter::GetInput(unsigned int idx)
{
if (this->GetNumberOfInputs() < 1)
return NULL;
return static_cast<const NavigationData*>(this->ProcessObject::GetInput(idx));
}
void mitk::NavigationDataToIGTLMessageFilter::CreateOutputsForAllInputs()
{
switch (m_OperationMode)
{
case ModeSendQTDataMsg:
// create one message output for all navigation data inputs
this->SetNumberOfIndexedOutputs(this->GetNumberOfIndexedInputs());
// set the type for this filter
this->SetType("QTDATA");
break;
case ModeSendTDataMsg:
// create one message output for all navigation data inputs
this->SetNumberOfIndexedOutputs(this->GetNumberOfIndexedInputs());
// set the type for this filter
this->SetType("TDATA");
break;
case ModeSendQTransMsg:
// create one message output for all navigation data input together
this->SetNumberOfIndexedOutputs(this->GetNumberOfIndexedInputs());
// set the type for this filter
this->SetType("POSITION");
break;
case ModeSendTransMsg:
// create one message output for all navigation data input together
this->SetNumberOfIndexedOutputs(this->GetNumberOfIndexedInputs());
// set the type for this filter
this->SetType("TRANS");
break;
default:
break;
}
for (unsigned int idx = 0; idx < this->GetNumberOfIndexedOutputs(); ++idx)
{
if (this->GetOutput(idx) == NULL)
{
DataObjectPointer newOutput = this->MakeOutput(idx);
this->SetNthOutput(idx, newOutput);
}
this->Modified();
}
}
void ConvertAffineTransformationIntoIGTLMatrix(mitk::AffineTransform3D* trans,
igtl::Matrix4x4 igtlTransform)
{
const mitk::AffineTransform3D::MatrixType& matrix = trans->GetMatrix();
mitk::Vector3D position = trans->GetOffset();
//copy the data into a matrix type that igtl understands
for (unsigned int r = 0; r < 3; r++)
{
for (unsigned int c = 0; c < 3; c++)
{
igtlTransform[r][c] = matrix(r, c);
}
igtlTransform[r][3] = position[r];
}
for (unsigned int c = 0; c < 3; c++)
{
igtlTransform[3][c] = 0.0;
}
igtlTransform[3][3] = 1.0;
}
void mitk::NavigationDataToIGTLMessageFilter::GenerateDataModeSendQTransMsg()
{
// for each output message
for (unsigned int i = 0; i < this->GetNumberOfIndexedOutputs(); ++i)
{
mitk::IGTLMessage* output = this->GetOutput(i);
assert(output);
const mitk::NavigationData* input = this->GetInput(i);
assert(input);
// do not add navigation data to message if input is invalid
if (input->IsDataValid() == false)
continue;
//get the navigation data components
mitk::NavigationData::PositionType pos = input->GetPosition();
mitk::NavigationData::OrientationType ori = input->GetOrientation();
//insert this information into the message
igtl::PositionMessage::Pointer posMsg = igtl::PositionMessage::New();
posMsg->SetPosition(pos[0], pos[1], pos[2]);
posMsg->SetQuaternion(ori[0], ori[1], ori[2], ori[3]);
- igtl::TimeStamp::Pointer timestamp = igtl::TimeStamp::New();
- timestamp->SetTime(input->GetTimeStamp().GetMTime() / 1000, input->GetTimeStamp().GetMTime() % 1000);
- posMsg->SetTimeStamp(timestamp);
+ posMsg->SetTimeStamp(ConvertToIGTLTimeStamp(input->GetIGTTimeStamp()));
posMsg->SetDeviceName(input->GetName());
posMsg->Pack();
//add the igtl message to the mitk::IGTLMessage
output->SetMessage(posMsg.GetPointer());
}
}
void mitk::NavigationDataToIGTLMessageFilter::GenerateDataModeSendTransMsg()
{
// for each output message
for (unsigned int i = 0; i < this->GetNumberOfIndexedOutputs(); ++i)
{
mitk::IGTLMessage* output = this->GetOutput(i);
assert(output);
const mitk::NavigationData* input = this->GetInput(i);
assert(input);
// do not add navigation data to message if input is invalid
if (input->IsDataValid() == false)
continue;
//get the navigation data components
mitk::AffineTransform3D::Pointer transform = input->GetAffineTransform3D();
mitk::NavigationData::PositionType position = transform->GetOffset();
//convert the transform into a igtl type
igtl::Matrix4x4 igtlTransform;
ConvertAffineTransformationIntoIGTLMatrix(transform, igtlTransform);
//insert this information into the message
igtl::TransformMessage::Pointer transMsg = igtl::TransformMessage::New();
transMsg->SetMatrix(igtlTransform);
transMsg->SetPosition(position[0], position[1], position[2]);
- igtl::TimeStamp::Pointer timestamp = igtl::TimeStamp::New();
- timestamp->SetTime(input->GetTimeStamp().GetMTime() / 1000, input->GetTimeStamp().GetMTime() % 1000);
- transMsg->SetTimeStamp(timestamp);
+ transMsg->SetTimeStamp(ConvertToIGTLTimeStamp(input->GetIGTTimeStamp()));
transMsg->SetDeviceName(input->GetName());
transMsg->Pack();
//add the igtl message to the mitk::IGTLMessage
output->SetMessage(transMsg.GetPointer());
}
}
+igtl::TimeStamp::Pointer mitk::NavigationDataToIGTLMessageFilter::ConvertToIGTLTimeStamp(double IGTTimeStamp)
+{
+ igtl::TimeStamp::Pointer timestamp = igtl::TimeStamp::New();
+ timestamp->SetTime(IGTTimeStamp / 1000, (int)(IGTTimeStamp) % 1000);
+ return timestamp;
+}
void mitk::NavigationDataToIGTLMessageFilter::GenerateDataModeSendQTDataMsg()
{
mitk::IGTLMessage* output = this->GetOutput();
assert(output);
//create a output igtl message
igtl::QuaternionTrackingDataMessage::Pointer qtdMsg =
igtl::QuaternionTrackingDataMessage::New();
mitk::NavigationData::PositionType pos;
mitk::NavigationData::OrientationType ori;
for (unsigned int index = 0; index < this->GetNumberOfIndexedInputs(); index++)
{
const mitk::NavigationData* nd = GetInput(index);
assert(nd);
//get the navigation data components
pos = nd->GetPosition();
ori = nd->GetOrientation();
//insert the information into the tracking element
igtl::QuaternionTrackingDataElement::Pointer tde =
igtl::QuaternionTrackingDataElement::New();
tde->SetPosition(pos[0], pos[1], pos[2]);
tde->SetQuaternion(ori[0], ori[1], ori[2], ori[3]);
tde->SetName(nd->GetName());
//insert this element into the tracking data message
qtdMsg->AddQuaternionTrackingDataElement(tde);
- //copy the time stamp
- //todo find a better way to do that
- igtl::TimeStamp::Pointer timestamp = igtl::TimeStamp::New();
- timestamp->SetTime(nd->GetTimeStamp().GetMTime() / 1000, nd->GetTimeStamp().GetMTime() % 1000);
- MITK_INFO << timestamp;
+ MITK_INFO << ConvertToIGTLTimeStamp(nd->GetIGTTimeStamp());
}
qtdMsg->Pack();
//add the igtl message to the mitk::IGTLMessage
output->SetMessage(qtdMsg.GetPointer());
}
void mitk::NavigationDataToIGTLMessageFilter::GenerateDataModeSendTDataMsg()
{
bool isValidData = true;
mitk::IGTLMessage* output = this->GetOutput();
assert(output);
//create a output igtl message
igtl::TrackingDataMessage::Pointer tdMsg = igtl::TrackingDataMessage::New();
mitk::AffineTransform3D::Pointer transform;
Vector3D position;
igtl::Matrix4x4 igtlTransform;
vnl_matrix_fixed<ScalarType, 3, 3> rotationMatrix;
vnl_matrix_fixed<ScalarType, 3, 3> rotationMatrixTransposed;
for (unsigned int index = 0; index < this->GetNumberOfIndexedInputs(); index++)
{
const mitk::NavigationData* nd = GetInput(index);
assert(nd);
//create a new tracking element
igtl::TrackingDataElement::Pointer tde = igtl::TrackingDataElement::New();
//get the navigation data components
transform = nd->GetAffineTransform3D();
position = transform->GetOffset();
//check the rotation matrix
rotationMatrix = transform->GetMatrix().GetVnlMatrix();
rotationMatrixTransposed = rotationMatrix.transpose();
// a quadratic matrix is a rotation matrix exactly when determinant is 1
// and transposed is inverse
if (!Equal(1.0, vnl_det(rotationMatrix), 0.1)
|| !((rotationMatrix*rotationMatrixTransposed).is_identity(0.1)))
{
//the rotation matrix is not valid! => invalidate the current element
isValidData = false;
}
//convert the transform into a igtl type
ConvertAffineTransformationIntoIGTLMatrix(transform, igtlTransform);
//fill the tracking element with life
tde->SetMatrix(igtlTransform);
tde->SetPosition(position[0], position[1], position[2]);
std::stringstream name;
name << nd->GetName();
if (name.rdbuf()->in_avail() == 0)
{
name << "TrackingTool" << index;
}
tde->SetName(name.str().c_str());
//insert this element into the tracking data message
tdMsg->AddTrackingDataElement(tde);
//copy the time stamp
- //todo find a better way to do that
- igtl::TimeStamp::Pointer timestamp = igtl::TimeStamp::New();
- timestamp->SetTime(nd->GetTimeStamp().GetMTime() / 1000, nd->GetTimeStamp().GetMTime() % 1000);
- tdMsg->SetTimeStamp(timestamp);
-
+ tdMsg->SetTimeStamp(ConvertToIGTLTimeStamp(nd->GetIGTTimeStamp()));
}
tdMsg->Pack();
//add the igtl message to the mitk::IGTLMessage
output->SetMessage(tdMsg.GetPointer());
output->SetDataValid(isValidData);
}
void mitk::NavigationDataToIGTLMessageFilter::SetOperationMode(OperationMode mode)
{
m_OperationMode = mode;
this->Modified();
this->CreateOutputsForAllInputs();
}
void mitk::NavigationDataToIGTLMessageFilter::ConnectTo(
mitk::NavigationDataSource* UpstreamFilter)
{
for (DataObjectPointerArraySizeType i = 0;
i < UpstreamFilter->GetNumberOfOutputs(); i++)
{
this->SetInput(i, UpstreamFilter->GetOutput(i));
}
}
diff --git a/Modules/IGT/Algorithms/mitkNavigationDataToIGTLMessageFilter.h b/Modules/IGT/Algorithms/mitkNavigationDataToIGTLMessageFilter.h
index 799900d30d..a92dd036b5 100644
--- a/Modules/IGT/Algorithms/mitkNavigationDataToIGTLMessageFilter.h
+++ b/Modules/IGT/Algorithms/mitkNavigationDataToIGTLMessageFilter.h
@@ -1,170 +1,173 @@
/*===================================================================
The Medical Imaging Interaction Toolkit (MITK)
Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.
This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.
See LICENSE.txt or http://www.mitk.org for details.
===================================================================*/
#ifndef _MITKNAVIGATIONDATATOIGTLMessageFILTER_H__
#define _MITKNAVIGATIONDATATOIGTLMessageFILTER_H__
#include "mitkCommon.h"
#include "mitkPointSet.h"
#include "mitkIGTLMessageSource.h"
#include "mitkNavigationData.h"
#include "mitkNavigationDataSource.h"
namespace mitk {
/**Documentation
*
* \brief This filter creates IGTL messages from mitk::NavigaitionData objects
*
*
* \ingroup IGT
*
*/
class MITKIGT_EXPORT NavigationDataToIGTLMessageFilter : public IGTLMessageSource
{
public:
mitkClassMacro(NavigationDataToIGTLMessageFilter, IGTLMessageSource);
itkFactorylessNewMacro(Self)
itkCloneMacro(Self)
/**Documentation
* \brief There are four different operation modes.
*
* - ModeSendQTransMsg: every input NavigationData is processed into one
* output message that contains a position and a orientation (quaternion).
* - ModeSendTransMsg: every input NavigationData is processed into one
* output message that contains a 4x4 transformation.
* - ModeSendQTDataMsg:all input NavigationData is processed into one single
* output message that contains a position and orientation (quaternion) for
* each navigation data.
* - ModeSendTDataMsg:all input NavigationData is processed into one single
* output message that contains a 4x4 transformation for
* each navigation data.
*/
enum OperationMode
{
ModeSendQTransMsg,
ModeSendTransMsg,
ModeSendQTDataMsg,
ModeSendTDataMsg
};
/**
* \brief filter execute method
*/
virtual void GenerateData() override;
using Superclass::SetInput;
/**
* \brief Sets one input NavigationData
*/
virtual void SetInput(const mitk::NavigationData *NavigationData);
/**
* \brief Sets the input NavigationData at a specific index
*/
virtual void SetInput(unsigned int idx, const NavigationData* nd);
/**
* \brief Returns the input of this filter
*/
const mitk::NavigationData* GetInput();
/**
* \brief Returns the input number idx of this filter
*/
const mitk::NavigationData* GetInput(unsigned int idx);
/**
* \brief Sets the mode of this filter.
*
* See OperationMode for the behavior in the different modes
* \warn A call to this method will change the number of outputs of the filter.
* After calling this method, all previously acquired pointers to outputs are invalid
* Always set the operation mode first, then get the outputs with GetOutput()
*/
virtual void SetOperationMode(OperationMode mode);
/**
* \brief returns the mode of this filter.
*
* See OperationMode for the behavior in the different modes
*/
itkGetConstMacro(OperationMode, OperationMode);
/**
* empty implementation to prevent calling of the superclass method that
* would try to copy information from the input NavigationData to the output
* PointSet, which makes no sense!
*/
void GenerateOutputInformation() override {};
/**
*\brief Connects the input of this filter to the outputs of the given
* NavigationDataSource
*
* This method does not support smartpointer. use FilterX.GetPointer() to
* retrieve a dumbpointer.
*/
virtual void ConnectTo(mitk::NavigationDataSource * UpstreamFilter);
protected:
NavigationDataToIGTLMessageFilter();
virtual ~NavigationDataToIGTLMessageFilter();
/**
* \brief Generates the output
*
*/
// virtual void GenerateData();
/**
* \brief Generates the output for ModeSendQTDataMsg
*
*/
virtual void GenerateDataModeSendQTDataMsg();
/**
* \brief Generates the output for ModeSendTDataMsg
*/
virtual void GenerateDataModeSendTDataMsg();
/**
* \brief Generates the output for ModeSendQTransMsg
*
*/
virtual void GenerateDataModeSendQTransMsg();
/**
* \brief Generates the output for ModeSendTransMsg
*/
virtual void GenerateDataModeSendTransMsg();
/**
* \brief create output objects according to OperationMode for all inputs
*/
virtual void CreateOutputsForAllInputs();
OperationMode m_OperationMode; ///< Stores the mode. See enum OperationMode
// unsigned int m_RingBufferSize; ///< Stores the ringbuffer size
unsigned int m_CurrentTimeStep; ///< Indicates the current timestamp
// unsigned int m_NumberForMean; ///< Number of Navigation Data, which should be averaged
+ /** Converts a mitk::IGTTimestamp (double, milliseconds) to an OpenIGTLink timestamp */
+ igtl::TimeStamp::Pointer ConvertToIGTLTimeStamp(double IGTTimeStamp);
+
/** Measurement class to calculate latency and frame count */
};
} // namespace mitk
#endif // _MITKNAVIGATIONDATATOIGTLMessageFILTER_H__
diff --git a/Modules/IGTBase/include/mitkNavigationData.h b/Modules/IGTBase/include/mitkNavigationData.h
index 34044b3d9e..047f4d3b6a 100644
--- a/Modules/IGTBase/include/mitkNavigationData.h
+++ b/Modules/IGTBase/include/mitkNavigationData.h
@@ -1,297 +1,297 @@
/*===================================================================
The Medical Imaging Interaction Toolkit (MITK)
Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.
This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.
See LICENSE.txt or http://www.mitk.org for details.
===================================================================*/
#ifndef MITKNAVIGATIONDATA_H_HEADER_INCLUDED_
#define MITKNAVIGATIONDATA_H_HEADER_INCLUDED_
#include <itkDataObject.h>
#include <MitkIGTBaseExports.h>
#include <mitkCommon.h>
#include <mitkNumericTypes.h>
namespace mitk {
/**Documentation
* \brief Navigation Data
*
* This class represents the data object that is passed through the MITK-IGT navigation filter
* pipeline. It encapsulates position and orientation of a tracked tool/sensor. Additionally,
* it contains a data structure that contains error/plausibility information
*
* It provides methods to work with the affine transformation represented by its orientation and position.
* Additionally, it provides a constructor to construct a NavigationData object from an AffineTransform3D and
* a getter to create an AffineTransform3D from a NavigationData object.
*
* \ingroup IGT
*/
class MITKIGTBASE_EXPORT NavigationData : public itk::DataObject
{
public:
mitkClassMacroItkParent(NavigationData, itk::DataObject);
itkFactorylessNewMacro(Self);
itkCloneMacro(Self);
mitkNewMacro2Param(Self, mitk::AffineTransform3D::Pointer, const bool);
mitkNewMacro1Param(Self, mitk::AffineTransform3D::Pointer);
/**
* \brief Type that holds the position part of the tracking data
*/
typedef mitk::Point3D PositionType;
/**
* \brief Type that holds the orientation part of the tracking data
*/
typedef mitk::Quaternion OrientationType;
/**
* \brief type that holds the error characterization of the position and orientation measurements
*/
typedef itk::Matrix<mitk::ScalarType,6,6> CovarianceMatrixType;
/**
- * \brief type that holds the time at which the data was recorded
+ * \brief type that holds the time at which the data was recorded in milliseconds
*/
typedef double TimeStampType;
/**
* \brief sets the position of the NavigationData object
*/
itkSetMacro(Position, PositionType);
/**
* \brief returns position of the NavigationData object
*/
itkGetConstMacro(Position, PositionType);
/**
* \brief sets the orientation of the NavigationData object
*/
itkSetMacro(Orientation, OrientationType);
/**
* \brief returns the orientation of the NavigationData object
*/
itkGetConstMacro(Orientation, OrientationType);
/**
* \brief returns true if the object contains valid data
*/
virtual bool IsDataValid() const;
/**
* \brief sets the dataValid flag of the NavigationData object indicating if the object contains valid data
*/
itkSetMacro(DataValid, bool);
/**
- * \brief sets the IGT timestamp of the NavigationData object
+ * \brief sets the IGT timestamp of the NavigationData object in milliseconds
*/
itkSetMacro(IGTTimeStamp, TimeStampType);
/**
- * \brief gets the IGT timestamp of the NavigationData object
+ * \brief gets the IGT timestamp of the NavigationData object in milliseconds
*/
itkGetConstMacro(IGTTimeStamp, TimeStampType);
/**
* \brief sets the HasPosition flag of the NavigationData object
*/
itkSetMacro(HasPosition, bool);
/**
* \brief gets the HasPosition flag of the NavigationData object
*/
itkGetConstMacro(HasPosition, bool);
/**
* \brief sets the HasOrientation flag of the NavigationData object
*/
itkSetMacro(HasOrientation, bool);
/**
* \brief gets the HasOrientation flag of the NavigationData object
*/
itkGetConstMacro(HasOrientation, bool);
/**
* \brief sets the 6x6 Error Covariance Matrix of the NavigationData object
*/
itkSetMacro(CovErrorMatrix, CovarianceMatrixType);
/**
* \brief gets the 6x6 Error Covariance Matrix of the NavigationData object
*/
itkGetConstMacro(CovErrorMatrix, CovarianceMatrixType);
/**
* \brief set the name of the NavigationData object
*/
itkSetStringMacro(Name);
/**
* \brief returns the name of the NavigationData object
*/
itkGetStringMacro(Name);
/**
* \brief Graft the data and information from one NavigationData to another.
*
* Copies the content of data into this object.
* This is a convenience method to setup a second NavigationData object with all the meta
* information of another NavigationData object.
* Note that this method is different than just using two
* SmartPointers to the same NavigationData object since separate DataObjects are
* still maintained.
*/
virtual void Graft(const DataObject *data) override;
/**
* \brief copy meta data of a NavigationData object
*
* copies all meta data from NavigationData data to this object
*/
virtual void CopyInformation(const DataObject* data) override;
/**
* \brief Prints the object information to the given stream os.
* \param os The stream which is used to print the output.
* \param indent Defines the indentation of the output.
*/
void PrintSelf(std::ostream& os, itk::Indent indent) const override;
/**
* Set the position part of m_CovErrorMatrix to I*error^2
* This means that all position variables are assumed to be independent
*/
void SetPositionAccuracy(mitk::ScalarType error);
/**
* Set the orientation part of m_CovErrorMatrix to I*error^2
* This means that all orientation variables are assumed to be independent
*/
void SetOrientationAccuracy(mitk::ScalarType error);
/**
* \brief Calculate AffineTransform3D from the transformation held by this NavigationData.
* TODO: should throw an error if transformation is invalid.
*/
mitk::AffineTransform3D::Pointer GetAffineTransform3D() const;
/**
* \brief Calculate the RotationMatrix of this transformation.
*/
mitk::Matrix3D GetRotationMatrix() const;
/**
* \brief Transform by an affine transformation
*
* This method applies the affine transform given by self to a
* given point, returning the transformed point.
*/
mitk::Point3D TransformPoint(const mitk::Point3D point) const;
/**
* Get inverse of the Transformation represented by this NavigationData.
* @throws mitk::Exception in case the transformation is invalid (only case: quaternion is zero)
*/
mitk::NavigationData::Pointer GetInverse() const;
/** Compose with another NavigationData
*
* This method composes self with another NavigationData of the
* same dimension, modifying self to be the composition of self
* and other. If the argument pre is true, then other is
* precomposed with self; that is, the resulting transformation
* consists of first applying other to the source, followed by
* self. If pre is false or omitted, then other is post-composed
* with self; that is the resulting transformation consists of
* first applying self to the source, followed by other. */
void Compose(const mitk::NavigationData::Pointer n, const bool pre = false);
protected:
mitkCloneMacro(Self);
NavigationData();
/*
* Copy constructor internally used.
*/
NavigationData(const mitk::NavigationData& toCopy);
/**
* Creates a NavigationData object from an affineTransform3D.
* Caution: NavigationData doesn't support spacing, only translation and rotation. If the affine
* transform includes spacing it cannot be converted to a NavigationData and an exception is thrown.
* @param checkForRotationMatrix if this is true, the rotation matrix coming from the affineTransform is checked
* for being a rotation matrix. If it isn't, an exception is thrown. Disable this check by
* setting checkForRotationMatrix to false.
*
* @throws mitkException if checkForRotationMatrix is true and a non rotation matrix was introduced by
* AffineTransform.
*/
NavigationData(mitk::AffineTransform3D::Pointer affineTransform3D, const bool checkForRotationMatrix = true);
virtual ~NavigationData();
/**
* \brief holds the position part of the tracking data
*/
PositionType m_Position;
/**
* \brief holds the orientation part of the tracking data
*/
OrientationType m_Orientation;
/**
* \brief A 6x6 covariance matrix parameterizing the Gaussian error
* distribution of the measured position and orientation.
*
* The hasPosition/hasOrientation fields define which entries
* are valid.
*/
CovarianceMatrixType m_CovErrorMatrix; ///< holds the error characterization of the position and orientation
/**
* \brief defines if position part of m_CovErrorMatrix is valid
*/
bool m_HasPosition;
/**
* \brief defines if orientation part of m_CovErrorMatrix is valid
*/
bool m_HasOrientation;
/**
* \brief defines if the object contains valid values
*/
bool m_DataValid;
/**
* \brief contains the time at which the tracking data was recorded
*/
TimeStampType m_IGTTimeStamp;
/**
* \brief name of the navigation data
*/
std::string m_Name;
private:
void ResetCovarianceValidity();
// pre = false
static mitk::NavigationData::Pointer getComposition(const mitk::NavigationData::Pointer nd1, const mitk::NavigationData::Pointer nd2);
};
/**
* @brief Equal A function comparing two navigation data objects for beeing equal in meta- and imagedata
*
* @ingroup MITKTestingAPI
*
* Following aspects are tested for equality:
* - position
* - orientation
* - other members and flags of the class
*
* @param rightHandSide An NavigationData to be compared
* @param leftHandSide An NavigationData to be compared
* @param eps Tolarence for comparison. You can use mitk::eps in most cases.
* @param verbose Flag indicating if the user wants detailed console output or not.
* @return true, if all subsequent comparisons are true, false otherwise
*/
MITKIGTBASE_EXPORT bool Equal( const mitk::NavigationData& leftHandSide, const mitk::NavigationData& rightHandSide, ScalarType eps = mitk::eps, bool verbose = false );
} // namespace mitk
#endif /* MITKNAVIGATIONDATA_H_HEADER_INCLUDED_ */
diff --git a/Modules/OpenIGTLink/mitkIGTLMessage.cpp b/Modules/OpenIGTLink/mitkIGTLMessage.cpp
index 703f0d8ae1..a5e844c7b9 100644
--- a/Modules/OpenIGTLink/mitkIGTLMessage.cpp
+++ b/Modules/OpenIGTLink/mitkIGTLMessage.cpp
@@ -1,180 +1,181 @@
/*===================================================================
The Medical Imaging Interaction Toolkit (MITK)
Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.
This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.
See LICENSE.txt or http://www.mitk.org for details.
===================================================================*/
#include "mitkIGTLMessage.h"
#include "mitkException.h"
#include "mitkIGTLMessageCommon.h"
mitk::IGTLMessage::IGTLMessage() : itk::DataObject(),
- m_DataValid(false), m_IGTTimeStamp(0.0), m_Name()
+ m_DataValid(false), m_IGTTimeStamp(0), m_Name()
{
m_Message = igtl::MessageBase::New();
}
mitk::IGTLMessage::IGTLMessage(const mitk::IGTLMessage& toCopy) :
itk::DataObject()
{
// TODO SW: Graft does the same, remove code duplications, set Graft to
// deprecated, remove duplication in tescode
this->Graft(&toCopy);
}
mitk::IGTLMessage::~IGTLMessage()
{
}
mitk::IGTLMessage::IGTLMessage(igtl::MessageBase::Pointer message,
std::string name)
{
this->SetMessage(message);
this->SetName(name);
}
void mitk::IGTLMessage::Graft( const DataObject *data )
{
// Attempt to cast data to an IGTLMessage
const Self* msg;
try
{
msg = dynamic_cast<const Self *>(data);
}
catch( ... )
{
itkExceptionMacro( << "mitk::IGTLMessage::Graft cannot cast "
<< typeid(data).name() << " to "
<< typeid(const Self *).name() );
return;
}
if (!msg)
{
// pointer could not be cast back down
itkExceptionMacro( << "mitk::IGTLMessage::Graft cannot cast "
<< typeid(data).name() << " to "
<< typeid(const Self *).name() );
return;
}
// Now copy anything that is needed
this->SetMessage(msg->GetMessage());
this->SetDataValid(msg->IsDataValid());
this->SetIGTTimeStamp(msg->GetIGTTimeStamp());
this->SetName(msg->GetName());
}
void mitk::IGTLMessage::SetMessage(igtl::MessageBase::Pointer msg)
{
m_Message = msg;
unsigned int ts = 0;
unsigned int frac = 0;
- m_Message->GetTimeStamp(&ts, &frac);
- this->SetIGTTimeStamp((double)ts + (double)frac/1000.0);
+ m_Message->GetTimeStamp(&ts, &frac); //ts = seconds / frac = nanoseconds
+ double timestamp = ts * 1000.0 + frac;
+ this->SetIGTTimeStamp(timestamp);
this->SetDataValid(true);
}
bool mitk::IGTLMessage::IsDataValid() const
{
return m_DataValid;
}
void mitk::IGTLMessage::PrintSelf(std::ostream& os, itk::Indent indent) const
{
this->Superclass::PrintSelf(os, indent);
os << indent << "name: " << this->GetName() << std::endl;
os << indent << "data valid: " << this->IsDataValid() << std::endl;
os << indent << "TimeStamp: " << this->GetIGTTimeStamp() << std::endl;
os << indent << "OpenIGTLinkMessage: " << std::endl;
m_Message->Print(os);
}
std::string mitk::IGTLMessage::ToString() const
{
std::stringstream output;
output << "name: " << this->GetName() << std::endl <<
"MessageType: " << this->GetIGTLMessageType() << std::endl <<
"TimeStamp: " << this->GetIGTTimeStamp() << std::endl <<
"OpenIGTLinkMessage: " << std::endl;
return output.str();
}
void mitk::IGTLMessage::CopyInformation( const DataObject* data )
{
this->Superclass::CopyInformation( data );
const Self * nd = nullptr;
try
{
nd = dynamic_cast<const Self*>(data);
}
catch( ... )
{
// data could not be cast back down
itkExceptionMacro(<< "mitk::IGTLMessage::CopyInformation() cannot cast "
<< typeid(data).name() << " to "
<< typeid(Self*).name() );
}
if ( !nd )
{
// pointer could not be cast back down
itkExceptionMacro(<< "mitk::IGTLMessage::CopyInformation() cannot cast "
<< typeid(data).name() << " to "
<< typeid(Self*).name() );
}
/* copy all meta data */
}
bool mitk::Equal(const mitk::IGTLMessage& leftHandSide,
const mitk::IGTLMessage& rightHandSide,
ScalarType /*eps*/, bool verbose)
{
bool returnValue = true;
if( std::string(rightHandSide.GetName()) != std::string(leftHandSide.GetName()) )
{
if(verbose)
{
MITK_INFO << "[( IGTLMessage )] Name differs.";
MITK_INFO << "leftHandSide is " << leftHandSide.GetName()
<< "rightHandSide is " << rightHandSide.GetName();
}
returnValue = false;
}
if( rightHandSide.GetIGTTimeStamp() != leftHandSide.GetIGTTimeStamp() )
{
if(verbose)
{
MITK_INFO << "[( IGTLMessage )] IGTTimeStamp differs.";
MITK_INFO << "leftHandSide is " << leftHandSide.GetIGTTimeStamp()
<< "rightHandSide is " << rightHandSide.GetIGTTimeStamp();
}
returnValue = false;
}
return returnValue;
}
const char* mitk::IGTLMessage::GetIGTLMessageType() const
{
return this->m_Message->GetDeviceType();
}
template < typename IGTLMessageType >
IGTLMessageType* mitk::IGTLMessage::GetMessage() const
{
return dynamic_cast<IGTLMessageType*>(this->m_Message);
}
diff --git a/Modules/OpenIGTLink/mitkIGTLMessage.h b/Modules/OpenIGTLink/mitkIGTLMessage.h
index d3e0d9ddcd..5e0596605e 100644
--- a/Modules/OpenIGTLink/mitkIGTLMessage.h
+++ b/Modules/OpenIGTLink/mitkIGTLMessage.h
@@ -1,199 +1,199 @@
/*===================================================================
The Medical Imaging Interaction Toolkit (MITK)
Copyright (c) German Cancer Research Center,
Division of Medical and Biological Informatics.
All rights reserved.
This software is distributed WITHOUT ANY WARRANTY; without
even the implied warranty of MERCHANTABILITY or FITNESS FOR
A PARTICULAR PURPOSE.
See LICENSE.txt or http://www.mitk.org for details.
===================================================================*/
#ifndef MITKIGTLMESSAGEH_HEADER_INCLUDED_
#define MITKIGTLMESSAGEH_HEADER_INCLUDED_
#include <itkDataObject.h>
#include "MitkOpenIGTLinkExports.h"
#include <mitkNumericTypes.h>
#include <mitkCommon.h>
#include "igtlMessageBase.h"
namespace mitk {
/**Documentation
* \brief A wrapper for the OpenIGTLink message type
*
* This class represents the data object that is passed through the
* MITK-OpenIGTLink filter pipeline. It wraps the OpenIGTLink message type.
* Additionally, it contains a data structure that contains error/plausibility
* information.
*
*/
class MITKOPENIGTLINK_EXPORT IGTLMessage : public itk::DataObject
{
public:
mitkClassMacroItkParent(IGTLMessage, itk::DataObject);
itkFactorylessNewMacro(Self);
itkCloneMacro(Self);
mitkNewMacro2Param(Self, igtl::MessageBase::Pointer,std::string);
/**
- * \brief type that holds the time at which the data was recorded
+ * \brief type that holds the time at which the data was recorded in milliseconds
*/
typedef double TimeStampType;
/**
* \brief Sets the OpenIGTLink message
*/
void SetMessage(igtl::MessageBase::Pointer msg);
/**
* \brief returns the OpenIGTLink message
*/
itkGetConstMacro(Message, igtl::MessageBase::Pointer);
/**
* \brief returns true if the object contains valid data
*/
virtual bool IsDataValid() const;
/**
* \brief sets the dataValid flag of the IGTLMessage object indicating if
* the object contains valid data
*/
itkSetMacro(DataValid, bool);
/**
* \brief gets the IGT timestamp of the IGTLMessage object
*/
itkGetConstMacro(IGTTimeStamp, TimeStampType);
/**
* \brief set the name of the IGTLMessage object
*/
itkSetStringMacro(Name);
/**
* \brief returns the name of the IGTLMessage object
*/
itkGetStringMacro(Name);
/**
* \brief Graft the data and information from one IGTLMessage to another.
*
* Copies the content of data into this object.
* This is a convenience method to setup a second IGTLMessage object with
* all the meta information of another IGTLMessage object.
* Note that this method is different than just using two
* SmartPointers to the same IGTLMessage object since separate DataObjects
* are still maintained.
*/
virtual void Graft(const DataObject *data) override;
/**
* \brief copy meta data of a IGTLMessage object
*
* copies all meta data from IGTLMessage data to this object
*/
virtual void CopyInformation(const DataObject* data) override;
/**
* \brief Prints the object information to the given stream os.
* \param os The stream which is used to print the output.
* \param indent Defines the indentation of the output.
*/
void PrintSelf(std::ostream& os, itk::Indent indent) const override;
std::string ToString() const;
/** Compose with another IGTLMessage
*
* This method composes self with another IGTLMessage of the
* same dimension, modifying self to be the composition of self
* and other. If the argument pre is true, then other is
* precomposed with self; that is, the resulting transformation
* consists of first applying other to the source, followed by
* self. If pre is false or omitted, then other is post-composed
* with self; that is the resulting transformation consists of
* first applying self to the source, followed by other. */
void Compose(const mitk::IGTLMessage::Pointer n, const bool pre = false);
/** Returns the OpenIGTL Message type
**/
const char* GetIGTLMessageType() const;
template < typename IGTLMessageType > IGTLMessageType* GetMessage() const;
protected:
mitkCloneMacro(Self);
IGTLMessage();
/**
* Copy constructor internally used.
*/
IGTLMessage(const mitk::IGTLMessage& toCopy);
/**
* Creates a IGTLMessage object from an igtl::MessageBase and a given name.
*/
IGTLMessage(igtl::MessageBase::Pointer message, std::string name = "");
virtual ~IGTLMessage();
/**
* \brief holds the actual OpenIGTLink message
*/
igtl::MessageBase::Pointer m_Message;
/**
* \brief defines if the object contains valid values
*/
bool m_DataValid;
/**
* \brief contains the time at which the tracking data was recorded
*/
TimeStampType m_IGTTimeStamp;
/**
* \brief name of the navigation data
*/
std::string m_Name;
private:
// pre = false
static mitk::IGTLMessage::Pointer getComposition(
const mitk::IGTLMessage::Pointer nd1,
const mitk::IGTLMessage::Pointer nd2);
/**
* \brief sets the IGT timestamp of the IGTLMessage object
*/
itkSetMacro(IGTTimeStamp, TimeStampType);
};
/**
* @brief Equal A function comparing two OpenIGTLink message objects for
* being equal in meta- and imagedata
*
* @ingroup MITKTestingAPI
*
* Following aspects are tested for equality:
* - TBD
*
* @param rightHandSide An IGTLMessage to be compared
* @param leftHandSide An IGTLMessage to be compared
* @param eps Tolarence for comparison. You can use mitk::eps in most cases.
* @param verbose Flag indicating if the user wants detailed console output
* or not.
* @return true, if all subsequent comparisons are true, false otherwise
*/
MITKOPENIGTLINK_EXPORT bool Equal( const mitk::IGTLMessage& leftHandSide,
const mitk::IGTLMessage& rightHandSide,
ScalarType eps = mitk::eps,
bool verbose = false );
} // namespace mitk
#endif /* MITKIGTLMESSAGEH_HEADER_INCLUDED_ */