Medical Imaging Interaction Toolkit  2018.4.99-1640525a
Medical Imaging Interaction Toolkit
mitkNavigationData.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 
13 #include "mitkNavigationData.h"
14 #include "vnl/vnl_det.h"
15 #include "mitkException.h"
16 
18 m_Position(), m_Orientation(0.0, 0.0, 0.0, 1.0), m_CovErrorMatrix(),
19 m_HasPosition(true), m_HasOrientation(true), m_DataValid(false), m_IGTTimeStamp(0.0),
20 m_Name()
21 {
22  m_Position.Fill(0.0);
23  m_CovErrorMatrix.SetIdentity();
24 }
25 
26 
30  m_Name(toCopy.GetName())
31 {/* TODO SW: Graft does the same, remove code duplications, set Graft to deprecated, remove duplication in tescode */}
32 
34 {
35 }
36 
37 
38 void mitk::NavigationData::Graft( const DataObject *data )
39 {
40  // Attempt to cast data to an NavigationData
41  const Self* nd;
42  try
43  {
44  nd = dynamic_cast<const Self *>( data );
45  }
46  catch( ... )
47  {
48  itkExceptionMacro( << "mitk::NavigationData::Graft cannot cast "
49  << typeid(data).name() << " to "
50  << typeid(const Self *).name() );
51  return;
52  }
53  if (!nd)
54  {
55  // pointer could not be cast back down
56  itkExceptionMacro( << "mitk::NavigationData::Graft cannot cast "
57  << typeid(data).name() << " to "
58  << typeid(const Self *).name() );
59  return;
60  }
61  // Now copy anything that is needed
62  this->SetPosition(nd->GetPosition());
63  this->SetOrientation(nd->GetOrientation());
64  this->SetDataValid(nd->IsDataValid());
65  this->SetIGTTimeStamp(nd->GetIGTTimeStamp());
66  this->SetHasPosition(nd->GetHasPosition());
67  this->SetHasOrientation(nd->GetHasOrientation());
68  this->SetCovErrorMatrix(nd->GetCovErrorMatrix());
69  this->SetName(nd->GetName());
70 }
71 
72 
74 {
75  return m_DataValid;
76 }
77 
78 
79 void mitk::NavigationData::PrintSelf(std::ostream& os, itk::Indent indent) const
80 {
81  this->Superclass::PrintSelf(os, indent);
82  os << indent << "data valid: " << this->IsDataValid() << std::endl;
83  os << indent << "Position: " << this->GetPosition() << std::endl;
84  os << indent << "Orientation: " << this->GetOrientation() << std::endl;
85  os << indent << "TimeStamp: " << this->GetIGTTimeStamp() << std::endl;
86  os << indent << "HasPosition: " << this->GetHasPosition() << std::endl;
87  os << indent << "HasOrientation: " << this->GetHasOrientation() << std::endl;
88  os << indent << "CovErrorMatrix: " << this->GetCovErrorMatrix() << std::endl;
89 }
90 
91 
92 void mitk::NavigationData::CopyInformation( const DataObject* data )
93 {
94  this->Superclass::CopyInformation( data );
95 
96  const Self * nd = nullptr;
97  try
98  {
99  nd = dynamic_cast<const Self*>(data);
100  }
101  catch( ... )
102  {
103  // data could not be cast back down
104  itkExceptionMacro(<< "mitk::NavigationData::CopyInformation() cannot cast "
105  << typeid(data).name() << " to "
106  << typeid(Self*).name() );
107  }
108  if ( !nd )
109  {
110  // pointer could not be cast back down
111  itkExceptionMacro(<< "mitk::NavigationData::CopyInformation() cannot cast "
112  << typeid(data).name() << " to "
113  << typeid(Self*).name() );
114  }
115  /* copy all meta data */
116 }
117 
118 
120 {
121  for ( int i = 0; i < 3; i++ )
122  for ( int j = 0; j < 3; j++ )
123  {
124  m_CovErrorMatrix[ i ][ j ] = 0;
125  // assume independence of position and orientation
126  m_CovErrorMatrix[ i + 3 ][ j ] = 0;
127  m_CovErrorMatrix[ i ][ j + 3 ] = 0;
128  }
129  m_CovErrorMatrix[0][0] = m_CovErrorMatrix[1][1] = m_CovErrorMatrix[2][2] = error * error;
130 }
131 
132 
134 {
135  for ( int i = 0; i < 3; i++ )
136  for ( int j = 0; j < 3; j++ ) {
137  m_CovErrorMatrix[ i + 3 ][ j + 3 ] = 0;
138  // assume independence of position and orientation
139  m_CovErrorMatrix[ i + 3 ][ j ] = 0;
140  m_CovErrorMatrix[ i ][ j + 3 ] = 0;
141  }
142  m_CovErrorMatrix[3][3] = m_CovErrorMatrix[4][4] = m_CovErrorMatrix[5][5] = error * error;
143 }
144 
145 void
146 mitk::NavigationData::Compose(const mitk::NavigationData::Pointer n, const bool pre)
147 {
148  NavigationData::Pointer nd3;
149  if (!pre)
150  nd3 = getComposition(this, n);
151  else
152  nd3 = getComposition(n, this);
153 
154  this->Graft(nd3);
155 }
156 
158  mitk::AffineTransform3D::Pointer affineTransform3D,
159  const bool checkForRotationMatrix) : itk::DataObject(),
160  m_Position(),
162  m_Name()
163 {
164  mitk::Vector3D offset = affineTransform3D->GetOffset();
165 
166  m_Position[0] = offset[0];
167  m_Position[1] = offset[1];
168  m_Position[2] = offset[2];
169 
170  vnl_matrix_fixed<ScalarType, 3, 3> rotationMatrix = affineTransform3D->GetMatrix().GetVnlMatrix();
171  vnl_matrix_fixed<ScalarType, 3, 3> rotationMatrixTransposed = rotationMatrix.transpose();
172 
173  if (checkForRotationMatrix)
174  {
175  // a quadratic matrix is a rotation matrix exactly when determinant is 1 and transposed is inverse
176  if (!Equal(1.0, vnl_det(rotationMatrix), 0.1)
177  || !((rotationMatrix*rotationMatrixTransposed).is_identity(0.1)))
178  {
179  mitkThrow() << "tried to initialize NavigationData with non-rotation matrix :" << rotationMatrix << " (Does your AffineTransform3D object include spacing? This is not supported by NavigationData objects!)";
180  }
181 
182  }
183 
184  // the transpose is because vnl_quaterion expects a transposed rotation matrix
185  m_Orientation = Quaternion(rotationMatrixTransposed);
186 }
187 
188 mitk::AffineTransform3D::Pointer
190 {
191  AffineTransform3D::Pointer affineTransform3D = AffineTransform3D::New();
192 
193  // first set rotation
194  affineTransform3D->SetMatrix(this->GetRotationMatrix());
195 
196  // now set offset
197  Vector3D vector3D;
198 
199  for (int i = 0; i < 3; ++i) {
200  vector3D[i] = m_Position[i];
201  }
202  affineTransform3D->SetOffset(vector3D);
203 
204  return affineTransform3D;
205 }
206 
209 {
210  vnl_matrix_fixed<ScalarType,3,3> vnl_rotation = m_Orientation.rotation_matrix_transpose().transpose(); // :-)
211  Matrix3D mitkRotation;
212 
213  for (int i = 0; i < 3; ++i) {
214  for (int j = 0; j < 3; ++j) {
215  mitkRotation[i][j] = vnl_rotation[i][j];
216  }
217  }
218 
219  return mitkRotation;
220 }
221 
224 {
225  vnl_vector_fixed<ScalarType, 3> vnlPoint;
226 
227  for (int i = 0; i < 3; ++i) {
228  vnlPoint[i] = point[i];
229  }
230 
231  Quaternion normalizedQuaternion = this->GetOrientation().normalize();
232  // first get rotated point
233  vnlPoint = normalizedQuaternion.rotate(vnlPoint);
234 
235  Point3D resultingPoint;
236 
237  for (int i = 0; i < 3; ++i) {
238  // now copy it to our format + offset
239  resultingPoint[i] = vnlPoint[i] + this->GetPosition()[i];
240  }
241 
242  return resultingPoint;
243 }
244 
245 mitk::NavigationData::Pointer
247 {
248  // non-zero quaternion does not have inverse: throw exception in this case.
249  Quaternion zeroQuaternion;
250  zeroQuaternion.fill(0);
251  if (Equal(zeroQuaternion, this->GetOrientation()))
252  mitkThrow() << "tried to invert zero quaternion in NavigationData";
253 
254  mitk::NavigationData::Pointer navigationDataInverse = this->Clone();
255  navigationDataInverse->SetOrientation(this->GetOrientation().inverse());
256 
257  // To vnl_vector
258  vnl_vector_fixed<ScalarType, 3> vnlPoint;
259  for (int i = 0; i < 3; ++i) {
260  vnlPoint[i] = this->GetPosition()[i];
261  }
262 
263  // invert position
264  vnlPoint = -(navigationDataInverse->GetOrientation().rotate(vnlPoint));
265 
266  // back to Point3D
267  Point3D invertedPosition = this->GetPosition();
268  for (int i = 0; i < 3; ++i) {
269  invertedPosition[i] = vnlPoint[i];
270  }
271 
272  navigationDataInverse->SetPosition(invertedPosition);
273 
274  // Inversion does not care for covariances for now
275  navigationDataInverse->ResetCovarianceValidity();
276 
277  return navigationDataInverse;
278 }
279 
280 void
281 mitk::NavigationData::ResetCovarianceValidity()
282 {
283  this->SetHasPosition(false);
284  this->SetHasOrientation(false);
285 }
286 
287 mitk::NavigationData::Pointer
288 mitk::NavigationData::getComposition(const mitk::NavigationData::Pointer nd1,
289  const mitk::NavigationData::Pointer nd2)
290 {
291  NavigationData::Pointer nd3 = nd1->Clone();
292 
293  // A2 * A1
294  nd3->SetOrientation(nd2->GetOrientation() * nd1->GetOrientation());
295 
296  // first: b1, b2 vnl vector
297  vnl_vector_fixed<ScalarType,3> b1, b2, b3;
298  for (int i = 0; i < 3; ++i) {
299  b1[i] = nd1->GetPosition()[i];
300  b2[i] = nd2->GetPosition()[i];
301  }
302 
303  // b3 = A2b1 + b2
304  b3 = nd2->GetOrientation().rotate(b1) + b2;
305 
306  // back to mitk::Point3D
307  Point3D point;
308  for (int i = 0; i < 3; ++i) {
309  point[i] = b3[i];
310  }
311 
312  nd3->SetPosition(point);
313 
314  nd3->ResetCovarianceValidity();
315 
316  return nd3;
317 }
318 
319 bool mitk::Equal(const mitk::NavigationData& leftHandSide, const mitk::NavigationData& rightHandSide, ScalarType eps, bool verbose)
320 {
321  bool returnValue = true;
322 
323  // Dimensionality
324  if( !mitk::Equal(rightHandSide.GetPosition(), leftHandSide.GetPosition(), eps) )
325  {
326  if(verbose)
327  {
328  MITK_INFO << "[( NavigationData )] Position differs.";
329  MITK_INFO << "leftHandSide is " << leftHandSide.GetPosition()
330  << "rightHandSide is " << rightHandSide.GetPosition();
331  }
332  returnValue = false;
333  }
334 
335  // Dimensionality
336  if( !mitk::Equal(rightHandSide.GetOrientation(), leftHandSide.GetOrientation(), eps) )
337  {
338  if(verbose)
339  {
340  MITK_INFO << "[( NavigationData )] Orientation differs.";
341  MITK_INFO << "leftHandSide is " << leftHandSide.GetOrientation()
342  << "rightHandSide is " << rightHandSide.GetOrientation();
343  }
344  returnValue = false;
345  }
346 
347  if( rightHandSide.GetCovErrorMatrix() != leftHandSide.GetCovErrorMatrix() )
348  {
349  if(verbose)
350  {
351  MITK_INFO << "[( NavigationData )] CovErrorMatrix differs.";
352  MITK_INFO << "leftHandSide is " << leftHandSide.GetCovErrorMatrix()
353  << "rightHandSide is " << rightHandSide.GetCovErrorMatrix();
354  }
355  returnValue = false;
356  }
357 
358  if( std::string(rightHandSide.GetName()) != std::string(leftHandSide.GetName()) )
359  {
360  if(verbose)
361  {
362  MITK_INFO << "[( NavigationData )] Name differs.";
363  MITK_INFO << "leftHandSide is " << leftHandSide.GetName()
364  << "rightHandSide is " << rightHandSide.GetName();
365  }
366  returnValue = false;
367  }
368 
369  if( rightHandSide.GetIGTTimeStamp() != leftHandSide.GetIGTTimeStamp() )
370  {
371  if(verbose)
372  {
373  MITK_INFO << "[( NavigationData )] IGTTimeStamp differs.";
374  MITK_INFO << "leftHandSide is " << leftHandSide.GetIGTTimeStamp()
375  << "rightHandSide is " << rightHandSide.GetIGTTimeStamp();
376  }
377  returnValue = false;
378  }
379 
380  return returnValue;
381 }
void SetOrientationAccuracy(mitk::ScalarType error)
Pointer Clone() const
void CopyInformation(const DataObject *data) override
copy meta data of a NavigationData object
#define MITK_INFO
Definition: mitkLogMacros.h:18
bool m_HasPosition
defines if position part of m_CovErrorMatrix is valid
bool m_HasOrientation
defines if orientation part of m_CovErrorMatrix is valid
virtual void SetCovErrorMatrix(CovarianceMatrixType _arg)
sets the 6x6 Error Covariance Matrix of the NavigationData object
double ScalarType
Navigation Data.
OrientationType m_Orientation
holds the orientation part of the tracking data
virtual void SetName(const char *_arg)
set the name of the NavigationData object
void PrintSelf(std::ostream &os, itk::Indent indent) const override
Prints the object information to the given stream os.
mitk::AffineTransform3D::Pointer GetAffineTransform3D() const
Calculate AffineTransform3D from the transformation held by this NavigationData. TODO: should throw a...
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
virtual const char * GetName() const
returns the name of the NavigationData object
CovarianceMatrixType m_CovErrorMatrix
A 6x6 covariance matrix parameterizing the Gaussian error distribution of the measured position and o...
virtual PositionType GetPosition() const
returns position of the NavigationData object
virtual void SetHasPosition(bool _arg)
sets the HasPosition flag of the NavigationData object
static Vector3D offset
virtual TimeStampType GetIGTTimeStamp() const
gets the IGT timestamp of the NavigationData object in milliseconds Please note, that there is also t...
virtual void SetHasOrientation(bool _arg)
sets the HasOrientation flag of the NavigationData object
mitk::NavigationData::Pointer GetInverse() const
virtual void SetOrientation(OrientationType _arg)
sets the orientation of the NavigationData object
#define mitkThrow()
bool verbose(false)
std::string m_Name
name of the navigation data
vnl_quaternion< ScalarType > Quaternion
virtual bool GetHasPosition() const
gets the HasPosition flag of the NavigationData object
virtual void SetPosition(PositionType _arg)
sets the position of the NavigationData object
mitk::Point3D TransformPoint(const mitk::Point3D point) const
Transform by an affine transformation.
TimeStampType m_IGTTimeStamp
contains the time at which the tracking data was recorded
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 bool GetHasOrientation() const
gets the HasOrientation flag of the NavigationData object
void Compose(const mitk::NavigationData::Pointer n, const bool pre=false)
mitk::Matrix3D GetRotationMatrix() const
Calculate the RotationMatrix of this transformation.
PositionType m_Position
holds the position part of the tracking data
MITKCORE_EXPORT const ScalarType eps
bool m_DataValid
defines if the object contains valid values
virtual CovarianceMatrixType GetCovErrorMatrix() const
gets the 6x6 Error Covariance Matrix of the NavigationData object
virtual OrientationType GetOrientation() const
returns the orientation of the NavigationData object
virtual bool IsDataValid() const
returns true if the object contains valid data
void SetPositionAccuracy(mitk::ScalarType error)