Medical Imaging Interaction Toolkit  2018.4.99-1bab67a2
Medical Imaging Interaction Toolkit
mitkIGTLMessageToNavigationDataFilter.cpp
Go to the documentation of this file.
1 /*============================================================================
2 
3 The Medical Imaging Interaction Toolkit (MITK)
4 
5 Copyright (c) German Cancer Research Center (DKFZ)
6 All rights reserved.
7 
8 Use of this source code is governed by a 3-clause BSD license that can be
9 found in the LICENSE file.
10 
11 ============================================================================*/
12 
14 #include "igtlTrackingDataMessage.h"
15 #include "igtlQuaternionTrackingDataMessage.h"
16 #include "igtlTransformMessage.h"
17 #include "mitkQuaternion.h"
18 #include <vnl/vnl_det.h>
19 
22 {
23  mitk::NavigationData::Pointer output = mitk::NavigationData::New();
24  this->SetNumberOfRequiredOutputs(1);
25  this->SetNthOutput(0, output.GetPointer());
26 }
27 
29 {
30 }
31 
33 {
34  this->SetInput(0, msg);
35 }
36 
38 {
39  if (msg == nullptr) // if an input is set to nullptr, remove it
40  {
41  this->RemoveInput(idx);
42  }
43  else
44  {
45  // ProcessObject is not const-correct so a const_cast is required here
46  this->ProcessObject::SetNthInput(idx, const_cast<IGTLMessage*>(msg));
47  }
49 }
50 
51 const mitk::IGTLMessage*
53 {
54  if (this->GetNumberOfInputs() < 1)
55  return nullptr;
56 
57  return static_cast<const IGTLMessage*>(this->ProcessObject::GetInput(0));
58 }
59 
60 const mitk::IGTLMessage*
62 {
63  if (this->GetNumberOfInputs() < 1)
64  return nullptr;
65 
66  return static_cast<const IGTLMessage*>(this->ProcessObject::GetInput(idx));
67 }
68 
69 const mitk::IGTLMessage*
71 {
72  const DataObjectPointerArray& inputs = const_cast<Self*>(this)->GetInputs();
73  for (DataObjectPointerArray::const_iterator it = inputs.begin();
74  it != inputs.end(); ++it)
75  {
76  if (std::string(messageName) ==
77  (static_cast<IGTLMessage*>(it->GetPointer()))->GetName())
78  {
79  return static_cast<IGTLMessage*>(it->GetPointer());
80  }
81  }
82  return nullptr;
83 }
84 
85 itk::ProcessObject::DataObjectPointerArraySizeType
87 {
88  DataObjectPointerArray outputs = this->GetInputs();
89  for (DataObjectPointerArray::size_type i = 0; i < outputs.size(); ++i)
90  {
91  if (messageName ==
92  (static_cast<IGTLMessage*>(outputs.at(i).GetPointer()))->GetName())
93  {
94  return i;
95  }
96  }
97  throw std::invalid_argument("output name does not exist");
98 }
99 
101  mitk::IGTLMessageSource* UpstreamFilter)
102 {
103  for (DataObjectPointerArraySizeType i = 0;
104  i < UpstreamFilter->GetNumberOfOutputs(); i++)
105  {
106  this->SetInput(i, UpstreamFilter->GetOutput(i));
107  }
108 }
109 
111  unsigned int numOutputs)
112 {
113  this->SetNumberOfIndexedOutputs(numOutputs);
115 }
116 
118 {
119  // create outputs for all inputs
120  // this->SetNumberOfIndexedOutputs(this->GetNumberOfIndexedInputs());
121  bool isModified = false;
122  for (unsigned int idx = 0; idx < this->GetNumberOfIndexedOutputs(); ++idx)
123  {
124  if (this->GetOutput(idx) == nullptr)
125  {
126  mitk::NavigationData::Pointer newOutput = mitk::NavigationData::New();
127  this->SetNthOutput(idx, newOutput);
128  isModified = true;
129  }
130  }
131 
132  if (isModified)
133  this->Modified();
134 }
135 
137 {
138  const mitk::IGTLMessage* input = this->GetInput(0);
139  assert(input);
140 
141  //cast the input message into the proper type
142  igtl::TransformMessage* tMsg =
143  (igtl::TransformMessage*)(input->GetMessage().GetPointer());
144 
145  //check if cast was successful
146  if (!tMsg)
147  {
148  mitkThrow() << "Cast from igtl::MessageBase to igtl::TransformMessage "
149  << "failed! Please check the message.";
150  }
151 
152  /* update outputs with tracking data from tools */
153  for (unsigned int i = 0; i < this->GetNumberOfOutputs(); ++i)
154  {
155  mitk::NavigationData* output = this->GetOutput(i);
156  assert(output);
157 
158  if (input->IsDataValid() == false)
159  {
160  output->SetDataValid(false);
161  continue;
162  }
163 
164  //get the transformation matrix and convert it into an affinetransformation
165  igtl::Matrix4x4 transformation_;
166  tMsg->GetMatrix(transformation_);
167  mitk::AffineTransform3D::Pointer affineTransformation =
168  mitk::AffineTransform3D::New();
169  mitk::Matrix3D transformation;
171  for (unsigned int r = 0; r < 3; r++)
172  {
173  for (unsigned int c = 0; c < 3; c++)
174  {
175  transformation.GetVnlMatrix().set(r, c, transformation_[r][c]);
176  }
177  offset.SetElement(r, transformation_[r][3]);
178  }
179  //convert the igtl matrix here and set it in the affine transformation
180  affineTransformation->SetMatrix(transformation);
181  affineTransformation->SetOffset(offset);
182 
183  //create a new navigation data here, there is a neat constructor for
184  //affine transformations that sets the orientation, position according to
185  //the affine transformation. The other values are initialized with standard
186  //values
187  mitk::NavigationData::Pointer nd =
188  mitk::NavigationData::New(affineTransformation, true);
189  //set the time stamp
190  nd->SetIGTTimeStamp(input->GetTimeStamp());
191  //set the name
192  nd->SetName(input->GetName());
193 
194  output->Graft(nd);
195  }
196 }
197 
199 {
200  const mitk::IGTLMessage* input = this->GetInput(0);
201  assert(input);
202 
203  //cast the input message into the proper type
204  igtl::TrackingDataMessage* tdMsg =
205  (igtl::TrackingDataMessage*)(input->GetMessage().GetPointer());
206 
207  //check if cast was successful
208  if (!tdMsg)
209  {
210  mitkThrow() << "Cast from igtl::MessageBase to igtl::TrackingDataMessage "
211  << "failed! Please check the message.";
212  }
213 
214  //get the number of tracking data elements
215  unsigned int numTrackingDataElements =
216  tdMsg->GetNumberOfTrackingDataElements();
217 
218  if (!numTrackingDataElements)
219  {
220  MITK_ERROR("IGTLMsgToNavDataFilter") << "There are no tracking data "
221  "elements in this message";
222  }
223 
224  /* update outputs with tracking data from tools */
225  for (unsigned int i = 0; i < this->GetNumberOfOutputs(); ++i)
226  {
227  mitk::NavigationData* output = this->GetOutput(i);
228  assert(output);
229 
230  //invalidate the output
231  output->SetDataValid(false);
232 
233  //check if the current index, all outputs that have no corresponding input
234  //tracking element stay invalidated, the others are validated according to
235  //the tracking element
236  if (input->IsDataValid() == false) { continue; }
237  output->SetDataValid(true);
238 
239  //get the tracking data element which holds all the data
240  igtl::TrackingDataElement::Pointer td;
241  tdMsg->GetTrackingDataElement(i, td);
242 
243  //get the transformation matrix and convert it into an affinetransformation
244  igtl::Matrix4x4 transformation_;
245  td->GetMatrix(transformation_);
246  mitk::AffineTransform3D::Pointer affineTransformation =
247  mitk::AffineTransform3D::New();
248  mitk::Matrix3D transformation;
250  for (unsigned int r = 0; r < 3; r++)
251  {
252  for (unsigned int c = 0; c < 3; c++)
253  {
254  transformation.GetVnlMatrix().set(r, c, transformation_[r][c]);
255  }
256  offset.SetElement(r, transformation_[r][3]);
257  }
258  //convert the igtl matrix here and set it in the affine transformation
259  affineTransformation->SetMatrix(transformation);
260  affineTransformation->SetOffset(offset);
261 
262  mitk::NavigationData::Pointer nd;
263 
264  //check the rotation matrix
265  vnl_matrix_fixed<ScalarType, 3, 3> rotationMatrix =
266  affineTransformation->GetMatrix().GetVnlMatrix();
267  vnl_matrix_fixed<ScalarType, 3, 3> rotationMatrixTransposed =
268  rotationMatrix.transpose();
269  // a quadratic matrix is a rotation matrix exactly when determinant is 1
270  // and transposed is inverse
271  if (!Equal(1.0, vnl_det(rotationMatrix), 0.1)
272  || !((rotationMatrix*rotationMatrixTransposed).is_identity(0.1)))
273  {
274  MITK_ERROR("IGTLMsgToNavDataFilter") << "tried to initialize NavData "
275  << "with non-rotation matrix :" << rotationMatrix << " (Does your "
276  "AffineTransform3D object include spacing? This is not "
277  "supported by NavigationData objects!)";
279  }
280  else
281  {
282  //create a new navigation data here, there is a neat constructor for
283  //affine transformations that sets the orientation, position according to
284  //the affine transformation. The other values are initialized with standard
285  //values
286  nd = mitk::NavigationData::New(affineTransformation, true);
287  }
288  //set the time stamp
289  nd->SetIGTTimeStamp(input->GetIGTTimeStamp());
290  //set the name
291  nd->SetName(td->GetName());
292  output->Graft(nd);
293  }
294 }
295 
296 void
298 {
299  const mitk::IGTLMessage* input = this->GetInput(0);
300  assert(input);
301 
302  //cast the input message into the proper type
303  igtl::QuaternionTrackingDataMessage* tdMsg =
304  (igtl::QuaternionTrackingDataMessage*)(input->GetMessage().GetPointer());
305 
306  //check if cast was successful
307  if (!tdMsg)
308  {
309  mitkThrow() << "Cast from igtl::MessageBase to igtl::TrackingDataMessage "
310  << "failed! Please check the message.";
311  }
312 
313  //get the number of tracking data elements
314  unsigned int numTrackingDataElements =
315  tdMsg->GetNumberOfQuaternionTrackingDataElements();
316 
317  if (!numTrackingDataElements)
318  {
319  MITK_ERROR("IGTLMsgToNavDataFilter") << "There are no tracking data "
320  "elements in this message";
321  }
322 
323  /* update outputs with tracking data from tools */
324  for (unsigned int i = 0; i < this->GetNumberOfOutputs(); ++i)
325  {
326  mitk::NavigationData* output = this->GetOutput(i);
327  assert(output);
328 
329  //invalidate the output
330  output->SetDataValid(false);
331 
332  //check if the current index, all outputs that have no corresponding input
333  //tracking element stay invalidated, the others are validated according to
334  //the tracking element
335  if (input->IsDataValid() == false || i >= numTrackingDataElements)
336  {
337  continue;
338  }
339  output->SetDataValid(true);
340 
341  //get the tracking data element which holds all the data
342  igtl::QuaternionTrackingDataElement::Pointer td;
343  tdMsg->GetQuaternionTrackingDataElement(i, td);
344 
345  //get the quaternion and set it
346  float quaternion_[4]; //igtl quat type
347  td->GetQuaternion(quaternion_);
348  mitk::Quaternion quaternion;
349  quaternion.put(0, quaternion_[0]);
350  quaternion.put(1, quaternion_[1]);
351  quaternion.put(2, quaternion_[2]);
352  quaternion.put(3, quaternion_[3]);
353  output->SetOrientation(quaternion);
354  output->SetHasOrientation(true);
355 
356  //get the position and set it
357  float position_[3]; //igtl position type
358  td->GetPosition(position_);
359  mitk::NavigationData::PositionType position; //mitk position type
360  position.SetElement(0, position_[0]);
361  position.SetElement(1, position_[1]);
362  position.SetElement(2, position_[2]);
363  output->SetPosition(position);
364  output->SetHasPosition(true);
365  //set the time stamp
366  output->SetIGTTimeStamp(input->GetTimeStamp());
367  //set the name
368  output->SetName(td->GetName());
369 
370  //there is no explicit covarience matrix
372  }
373 }
374 
376 {
377  //get the IGTLMessage from the previous filter
378  const mitk::IGTLMessage* input = this->GetInput(0);
379  assert(input);
380 
381  //check if the message is valid, if it is not valid we do not generate new
382  //outputs
383  if (!input->IsDataValid())
384  {
385  MITK_DEBUG("IGTLMessageToNavigationDataFilter") << "Input data is invalid.";
386  return;
387  }
388 
389  //get the message type
390  const char* msgType = input->GetIGTLMessageType();
391 
392  //check if the IGTL message has the proper type
393  if (strcmp(msgType, "TRANSFORM") == 0)
394  {
395  this->GenerateTransformData();
396  }
397  else if (strcmp(msgType, "TDATA") == 0)
398  {
399  this->GenerateTrackingDataData();
400  }
401  else if (strcmp(msgType, "QTDATA") == 0)
402  {
404  }
405  else
406  {
407  //the message has another type
408  //ignore
409  MITK_INFO("IGTLMessageToNavigationDataFilter") << "The input has a unknown "
410  << "message type: "
411  << msgType;
412  }
413 }
414 
416 {
417  // Superclass::GenerateOutputInformation();
418 
419  // mitk::NavigationData* output = this->GetOutput(0);
420  // assert(output);
421  // const mitk::IGTLMessage* input = this->GetInput(0);
422  // assert(input);
423 
424  itkDebugMacro(<< "GenerateOutputInformation()");
425 
426  // output->Initialize(input->GetPixelType(), input->GetDimension(), input->GetDimensions());
427 
428  // // initialize geometry
429  // output->SetPropertyList(input->GetPropertyList()->Clone());
430  // mitk::TimeGeometry::Pointer clonGeometry = input->GetTimeGeometry()->Clone();
431  // output->SetTimeGeometry(clonGeometry.GetPointer());
432 }
DataObjectPointerArraySizeType GetInputIndex(std::string messageName)
return the index of the input with name messageName, throw std::invalid_argument exception if that na...
static Pointer New()
virtual void ConnectTo(mitk::IGTLMessageSource *UpstreamFilter)
Connects the input of this filter to the outputs of the given IGTLMessageSource.
NavigationData * GetOutput(void)
return the output (output with id 0) of the filter
#define MITK_INFO
Definition: mitkLogMacros.h:18
itk::Matrix< mitk::ScalarType, 6, 6 > CovarianceMatrixType
type that holds the error characterization of the position and orientation measurements ...
virtual void SetCovErrorMatrix(CovarianceMatrixType _arg)
sets the 6x6 Error Covariance Matrix of the NavigationData object
#define MITK_ERROR
Definition: mitkLogMacros.h:20
Navigation Data.
virtual void SetName(const char *_arg)
set the name of the NavigationData object
#define MITK_DEBUG
Definition: mitkLogMacros.h:22
DataCollection - Class to facilitate loading/accessing structured data.
virtual void SetInput(const IGTLMessage *msg)
Set the input of this filter.
virtual void SetDataValid(bool _arg)
sets the dataValid flag of the NavigationData object indicating if the object contains valid data ...
virtual void SetIGTTimeStamp(TimeStampType _arg)
sets the IGT timestamp of the NavigationData object in milliseconds
void GenerateOutputInformation() override
Defines how the input will be copied into the output.
virtual bool IsDataValid() const
returns true if the object contains valid data
virtual void SetHasPosition(bool _arg)
sets the HasPosition flag of the NavigationData object
void CreateOutputsForAllInputs()
Create an output for each input.
static Vector3D offset
IGTLMessage * GetOutput(void)
return the output (output with id 0) of the filter
virtual void SetHasOrientation(bool _arg)
sets the HasOrientation flag of the NavigationData object
const IGTLMessage * GetInput(void) const
Get the input of this filter.
A wrapper for the OpenIGTLink message type.
virtual void SetOrientation(OrientationType _arg)
sets the orientation of the NavigationData object
void SetNumberOfExpectedOutputs(unsigned int numOutputs)
Sets the number of expected outputs.
#define mitkThrow()
const char * GetIGTLMessageType() const
OpenIGTLink message source.
vnl_quaternion< ScalarType > Quaternion
virtual igtl::MessageBase::Pointer GetMessage() const
returns the OpenIGTLink message
virtual void SetPosition(PositionType _arg)
sets the position of the NavigationData object
void Graft(const DataObject *data) override
Graft the data and information from one NavigationData to another.
MITKNEWMODULE_EXPORT bool Equal(mitk::ExampleDataStructure *leftHandSide, mitk::ExampleDataStructure *rightHandSide, mitk::ScalarType eps, bool verbose)
Returns true if the example data structures are considered equal.
virtual const char * GetName() const
returns the name of the IGTLMessage object
virtual TimeStampType GetIGTTimeStamp() const
gets the IGT timestamp of the IGTLMessage object