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 #include 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(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(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(nd)); this->CreateOutputsForAllInputs(); } const mitk::NavigationData* mitk::NavigationDataToIGTLMessageFilter::GetInput(void) { if (this->GetNumberOfInputs() < 1) return NULL; return static_cast(this->ProcessObject::GetInput(0)); } const mitk::NavigationData* mitk::NavigationDataToIGTLMessageFilter::GetInput(unsigned int idx) { if (this->GetNumberOfInputs() < 1) return NULL; return static_cast(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 rotationMatrix; vnl_matrix_fixed 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 #include #include #include 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 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(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(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(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 #include "MitkOpenIGTLinkExports.h" #include #include #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_ */