diff --git a/Modules/IGT/Algorithms/mitkIGTLMessageToNavigationDataFilter.cpp b/Modules/IGT/Algorithms/mitkIGTLMessageToNavigationDataFilter.cpp index 5d10d09ae0..7d83c6345e 100644 --- a/Modules/IGT/Algorithms/mitkIGTLMessageToNavigationDataFilter.cpp +++ b/Modules/IGT/Algorithms/mitkIGTLMessageToNavigationDataFilter.cpp @@ -1,439 +1,439 @@ /*=================================================================== 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 "mitkIGTLMessageToNavigationDataFilter.h" #include "igtlTrackingDataMessage.h" #include "igtlQuaternionTrackingDataMessage.h" #include "igtlTransformMessage.h" #include "mitkQuaternion.h" #include mitk::IGTLMessageToNavigationDataFilter::IGTLMessageToNavigationDataFilter() : mitk::NavigationDataSource() { mitk::NavigationData::Pointer output = mitk::NavigationData::New(); this->SetNumberOfRequiredOutputs(1); this->SetNthOutput(0, output.GetPointer()); } mitk::IGTLMessageToNavigationDataFilter::~IGTLMessageToNavigationDataFilter() { } void mitk::IGTLMessageToNavigationDataFilter::SetInput(const IGTLMessage* msg) { this->SetInput(0, msg); } void mitk::IGTLMessageToNavigationDataFilter::SetInput(unsigned int idx, const IGTLMessage* msg) { if (msg == NULL) // if an input is set to NULL, remove it { this->RemoveInput(idx); } else { // ProcessObject is not const-correct so a const_cast is required here this->ProcessObject::SetNthInput(idx, const_cast(msg)); } this->CreateOutputsForAllInputs(); } const mitk::IGTLMessage* mitk::IGTLMessageToNavigationDataFilter::GetInput(void) const { if (this->GetNumberOfInputs() < 1) return NULL; return static_cast(this->ProcessObject::GetInput(0)); } const mitk::IGTLMessage* mitk::IGTLMessageToNavigationDataFilter::GetInput(unsigned int idx) const { if (this->GetNumberOfInputs() < 1) return NULL; return static_cast(this->ProcessObject::GetInput(idx)); } const mitk::IGTLMessage* mitk::IGTLMessageToNavigationDataFilter::GetInput(std::string messageName) const { const DataObjectPointerArray& inputs = const_cast(this)->GetInputs(); for (DataObjectPointerArray::const_iterator it = inputs.begin(); it != inputs.end(); ++it) { if (std::string(messageName) == (static_cast(it->GetPointer()))->GetName()) { return static_cast(it->GetPointer()); } } return NULL; } itk::ProcessObject::DataObjectPointerArraySizeType mitk::IGTLMessageToNavigationDataFilter::GetInputIndex(std::string messageName) { DataObjectPointerArray outputs = this->GetInputs(); for (DataObjectPointerArray::size_type i = 0; i < outputs.size(); ++i) { if (messageName == (static_cast(outputs.at(i).GetPointer()))->GetName()) { return i; } } throw std::invalid_argument("output name does not exist"); } void mitk::IGTLMessageToNavigationDataFilter::ConnectTo( mitk::IGTLMessageSource* UpstreamFilter) { for (DataObjectPointerArraySizeType i = 0; i < UpstreamFilter->GetNumberOfOutputs(); i++) { this->SetInput(i, UpstreamFilter->GetOutput(i)); } } void mitk::IGTLMessageToNavigationDataFilter::SetNumberOfExpectedOutputs( unsigned int numOutputs) { this->SetNumberOfIndexedOutputs(numOutputs); this->CreateOutputsForAllInputs(); } void mitk::IGTLMessageToNavigationDataFilter::CreateOutputsForAllInputs() { // create outputs for all inputs // this->SetNumberOfIndexedOutputs(this->GetNumberOfIndexedInputs()); bool isModified = false; for (unsigned int idx = 0; idx < this->GetNumberOfIndexedOutputs(); ++idx) { if (this->GetOutput(idx) == NULL) { mitk::NavigationData::Pointer newOutput = mitk::NavigationData::New(); this->SetNthOutput(idx, newOutput); isModified = true; } } if (isModified) this->Modified(); } void mitk::IGTLMessageToNavigationDataFilter::GenerateTransformData() { const mitk::IGTLMessage* input = this->GetInput(0); assert(input); //cast the input message into the proper type igtl::TransformMessage* tMsg = (igtl::TransformMessage*)(input->GetMessage().GetPointer()); //check if cast was successful if (!tMsg) { mitkThrow() << "Cast from igtl::MessageBase to igtl::TransformMessage " << "failed! Please check the message."; } /* update outputs with tracking data from tools */ for (unsigned int i = 0; i < this->GetNumberOfOutputs(); ++i) { mitk::NavigationData* output = this->GetOutput(i); assert(output); if (input->IsDataValid() == false) { output->SetDataValid(false); continue; } //get the transformation matrix and convert it into an affinetransformation igtl::Matrix4x4 transformation_; tMsg->GetMatrix(transformation_); mitk::AffineTransform3D::Pointer affineTransformation = mitk::AffineTransform3D::New(); mitk::Matrix3D transformation; mitk::Vector3D offset; for (unsigned int r = 0; r < 3; r++) { for (unsigned int c = 0; c < 3; c++) { transformation.GetVnlMatrix().set(r, c, transformation_[r][c]); } offset.SetElement(r, transformation_[r][3]); } //convert the igtl matrix here and set it in the affine transformation affineTransformation->SetMatrix(transformation); affineTransformation->SetOffset(offset); //create a new navigation data here, there is a neat constructor for //affine transformations that sets the orientation, position according to //the affine transformation. The other values are initialized with standard //values mitk::NavigationData::Pointer nd = mitk::NavigationData::New(affineTransformation, true); //set the time stamp nd->SetIGTTimeStamp(input->GetTimeStamp()); //set the name nd->SetName(input->GetName()); output->Graft(nd); } } void mitk::IGTLMessageToNavigationDataFilter::GenerateTrackingDataData() { const mitk::IGTLMessage* input = this->GetInput(0); assert(input); //cast the input message into the proper type igtl::TrackingDataMessage* tdMsg = (igtl::TrackingDataMessage*)(input->GetMessage().GetPointer()); //check if cast was successful if (!tdMsg) { mitkThrow() << "Cast from igtl::MessageBase to igtl::TrackingDataMessage " << "failed! Please check the message."; } //get the number of tracking data elements unsigned int numTrackingDataElements = tdMsg->GetNumberOfTrackingDataElements(); if (!numTrackingDataElements) { MITK_ERROR("IGTLMsgToNavDataFilter") << "There are no tracking data " "elements in this message"; } /* update outputs with tracking data from tools */ for (unsigned int i = 0; i < this->GetNumberOfOutputs(); ++i) { mitk::NavigationData* output = this->GetOutput(i); assert(output); //invalidate the output output->SetDataValid(false); //check if the current index, all outputs that have no corresponding input //tracking element stay invalidated, the others are validated according to //the tracking element if (input->IsDataValid() == false || i >= numTrackingDataElements) { continue; } output->SetDataValid(true); //get the tracking data element which holds all the data igtl::TrackingDataElement::Pointer td; tdMsg->GetTrackingDataElement(i, td); //get the transformation matrix and convert it into an affinetransformation igtl::Matrix4x4 transformation_; td->GetMatrix(transformation_); mitk::AffineTransform3D::Pointer affineTransformation = mitk::AffineTransform3D::New(); mitk::Matrix3D transformation; mitk::Vector3D offset; for (unsigned int r = 0; r < 3; r++) { for (unsigned int c = 0; c < 3; c++) { transformation.GetVnlMatrix().set(r, c, transformation_[r][c]); } offset.SetElement(r, transformation_[r][3]); } //convert the igtl matrix here and set it in the affine transformation affineTransformation->SetMatrix(transformation); affineTransformation->SetOffset(offset); mitk::NavigationData::Pointer nd; //check the rotation matrix vnl_matrix_fixed rotationMatrix = affineTransformation->GetMatrix().GetVnlMatrix(); vnl_matrix_fixed 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))) { MITK_ERROR("IGTLMsgToNavDataFilter") << "tried to initialize NavData " << "with non-rotation matrix :" << rotationMatrix << " (Does your " "AffineTransform3D object include spacing? This is not " "supported by NavigationData objects!)"; nd = mitk::NavigationData::New(); } else { //create a new navigation data here, there is a neat constructor for //affine transformations that sets the orientation, position according to //the affine transformation. The other values are initialized with standard //values nd = mitk::NavigationData::New(affineTransformation, true); } //set the time stamp nd->SetIGTTimeStamp(input->GetIGTTimeStamp()); //set the name - nd->SetName(td->GetName()); + nd->SetName(input->GetName()); output->Graft(nd); } } void mitk::IGTLMessageToNavigationDataFilter::GenerateQuaternionTrackingDataData() { const mitk::IGTLMessage* input = this->GetInput(0); assert(input); //cast the input message into the proper type igtl::QuaternionTrackingDataMessage* tdMsg = (igtl::QuaternionTrackingDataMessage*)(input->GetMessage().GetPointer()); //check if cast was successful if (!tdMsg) { mitkThrow() << "Cast from igtl::MessageBase to igtl::TrackingDataMessage " << "failed! Please check the message."; } //get the number of tracking data elements unsigned int numTrackingDataElements = tdMsg->GetNumberOfQuaternionTrackingDataElements(); if (!numTrackingDataElements) { MITK_ERROR("IGTLMsgToNavDataFilter") << "There are no tracking data " "elements in this message"; } /* update outputs with tracking data from tools */ for (unsigned int i = 0; i < this->GetNumberOfOutputs(); ++i) { mitk::NavigationData* output = this->GetOutput(i); assert(output); //invalidate the output output->SetDataValid(false); //check if the current index, all outputs that have no corresponding input //tracking element stay invalidated, the others are validated according to //the tracking element if (input->IsDataValid() == false || i >= numTrackingDataElements) { continue; } output->SetDataValid(true); //get the tracking data element which holds all the data igtl::QuaternionTrackingDataElement::Pointer td; tdMsg->GetQuaternionTrackingDataElement(i, td); //get the quaternion and set it float quaternion_[4]; //igtl quat type td->GetQuaternion(quaternion_); mitk::Quaternion quaternion; quaternion.put(0, quaternion_[0]); quaternion.put(1, quaternion_[1]); quaternion.put(2, quaternion_[2]); quaternion.put(3, quaternion_[3]); output->SetOrientation(quaternion); output->SetHasOrientation(true); //get the position and set it float position_[3]; //igtl position type td->GetPosition(position_); mitk::NavigationData::PositionType position; //mitk position type position.SetElement(0, position_[0]); position.SetElement(1, position_[1]); position.SetElement(2, position_[2]); output->SetPosition(position); output->SetHasPosition(true); //set the time stamp output->SetIGTTimeStamp(input->GetTimeStamp()); //set the name output->SetName(td->GetName()); //there is no explicit covarience matrix output->SetCovErrorMatrix(mitk::NavigationData::CovarianceMatrixType()); } } void mitk::IGTLMessageToNavigationDataFilter::GenerateData() { //get the IGTLMessage from the previous filter const mitk::IGTLMessage* input = this->GetInput(0); assert(input); //check if the message is valid, if it is not valid we do not generate new //outputs if (!input->IsDataValid()) { MITK_DEBUG("IGTLMessageToNavigationDataFilter") << "Input data is invalid."; return; } //get the message type const char* msgType = input->GetIGTLMessageType(); //check if the IGTL message has the proper type if (strcmp(msgType, "TRANSFORM") == 0) { this->GenerateTransformData(); } else if (strcmp(msgType, "TDATA") == 0) { this->GenerateTrackingDataData(); } else if (strcmp(msgType, "QTDATA") == 0) { this->GenerateQuaternionTrackingDataData(); } else { //the message has another type //ignore MITK_INFO("IGTLMessageToNavigationDataFilter") << "The input has a unknown " << "message type: " << msgType; } } void mitk::IGTLMessageToNavigationDataFilter::GenerateOutputInformation() { // Superclass::GenerateOutputInformation(); // mitk::NavigationData* output = this->GetOutput(0); // assert(output); // const mitk::IGTLMessage* input = this->GetInput(0); // assert(input); itkDebugMacro(<< "GenerateOutputInformation()"); // output->Initialize(input->GetPixelType(), input->GetDimension(), input->GetDimensions()); // // initialize geometry // output->SetPropertyList(input->GetPropertyList()->Clone()); // mitk::TimeGeometry::Pointer clonGeometry = input->GetTimeGeometry()->Clone(); // output->SetTimeGeometry(clonGeometry.GetPointer()); -} \ No newline at end of file +}