diff --git a/Modules/IGT/DataManagement/mitkNavigationData.cpp b/Modules/IGT/DataManagement/mitkNavigationData.cpp index c1199b8d9f..1d7227ce3c 100644 --- a/Modules/IGT/DataManagement/mitkNavigationData.cpp +++ b/Modules/IGT/DataManagement/mitkNavigationData.cpp @@ -1,321 +1,385 @@ /*=================================================================== 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 "mitkNavigationData.h" #include "vnl/vnl_det.h" #include "mitkException.h" mitk::NavigationData::NavigationData() : itk::DataObject(), m_Position(), m_Orientation(0.0, 0.0, 0.0, 1.0), m_CovErrorMatrix(), m_HasPosition(true), m_HasOrientation(true), m_DataValid(false), m_IGTTimeStamp(0.0), m_Name() { m_Position.Fill(0.0); m_CovErrorMatrix.SetIdentity(); } mitk::NavigationData::NavigationData(const mitk::NavigationData& toCopy) : itk::DataObject(), m_Position(toCopy.GetPosition()), m_Orientation(toCopy.GetOrientation()), m_CovErrorMatrix(toCopy.GetCovErrorMatrix()), m_HasPosition(toCopy.GetHasPosition()), m_HasOrientation(toCopy.GetHasOrientation()), m_DataValid(toCopy.IsDataValid()), m_IGTTimeStamp(toCopy.GetIGTTimeStamp()), m_Name(toCopy.GetName()) -{/* TODO SW: This constructor is not tested! TODO SW: Graft does the same, remove code duplications, set Graft to deprecated, remove duplication in tescode */} +{/* TODO SW: Graft does the same, remove code duplications, set Graft to deprecated, remove duplication in tescode */} mitk::NavigationData::~NavigationData() { } void mitk::NavigationData::Graft( const DataObject *data ) { // Attempt to cast data to an NavigationData const Self* nd; try { nd = dynamic_cast( data ); } catch( ... ) { itkExceptionMacro( << "mitk::NavigationData::Graft cannot cast " << typeid(data).name() << " to " << typeid(const Self *).name() ); return; } if (!nd) { // pointer could not be cast back down itkExceptionMacro( << "mitk::NavigationData::Graft cannot cast " << typeid(data).name() << " to " << typeid(const Self *).name() ); return; } // Now copy anything that is needed this->SetPosition(nd->GetPosition()); this->SetOrientation(nd->GetOrientation()); this->SetDataValid(nd->IsDataValid()); this->SetIGTTimeStamp(nd->GetIGTTimeStamp()); this->SetHasPosition(nd->GetHasPosition()); this->SetHasOrientation(nd->GetHasOrientation()); this->SetCovErrorMatrix(nd->GetCovErrorMatrix()); this->SetName(nd->GetName()); } bool mitk::NavigationData::IsDataValid() const { return m_DataValid; } void mitk::NavigationData::PrintSelf(std::ostream& os, itk::Indent indent) const { this->Superclass::PrintSelf(os, indent); os << indent << "data valid: " << this->IsDataValid() << std::endl; os << indent << "Position: " << this->GetPosition() << std::endl; os << indent << "Orientation: " << this->GetOrientation() << std::endl; os << indent << "TimeStamp: " << this->GetIGTTimeStamp() << std::endl; os << indent << "HasPosition: " << this->GetHasPosition() << std::endl; os << indent << "HasOrientation: " << this->GetHasOrientation() << std::endl; os << indent << "CovErrorMatrix: " << this->GetCovErrorMatrix() << std::endl; } void mitk::NavigationData::CopyInformation( const DataObject* data ) { this->Superclass::CopyInformation( data ); const Self * nd = NULL; try { nd = dynamic_cast(data); } catch( ... ) { // data could not be cast back down itkExceptionMacro(<< "mitk::NavigationData::CopyInformation() cannot cast " << typeid(data).name() << " to " << typeid(Self*).name() ); } if ( !nd ) { // pointer could not be cast back down itkExceptionMacro(<< "mitk::NavigationData::CopyInformation() cannot cast " << typeid(data).name() << " to " << typeid(Self*).name() ); } /* copy all meta data */ } void mitk::NavigationData::SetPositionAccuracy(mitk::ScalarType error) { for ( int i = 0; i < 3; i++ ) for ( int j = 0; j < 3; j++ ) { m_CovErrorMatrix[ i ][ j ] = 0; // assume independence of position and orientation m_CovErrorMatrix[ i + 3 ][ j ] = 0; m_CovErrorMatrix[ i ][ j + 3 ] = 0; } m_CovErrorMatrix[0][0] = m_CovErrorMatrix[1][1] = m_CovErrorMatrix[2][2] = error * error; } void mitk::NavigationData::SetOrientationAccuracy(mitk::ScalarType error) { for ( int i = 0; i < 3; i++ ) for ( int j = 0; j < 3; j++ ) { m_CovErrorMatrix[ i + 3 ][ j + 3 ] = 0; // assume independence of position and orientation m_CovErrorMatrix[ i + 3 ][ j ] = 0; m_CovErrorMatrix[ i ][ j + 3 ] = 0; } m_CovErrorMatrix[3][3] = m_CovErrorMatrix[4][4] = m_CovErrorMatrix[5][5] = error * error; } void mitk::NavigationData::Compose(const mitk::NavigationData::Pointer n, const bool pre) { NavigationData::Pointer nd3; if (!pre) nd3 = getComposition(this, n); else nd3 = getComposition(n, this); this->Graft(nd3); } mitk::NavigationData::NavigationData( mitk::AffineTransform3D::Pointer affineTransform3D, const bool checkForRotationMatrix) : itk::DataObject(), m_Position(), m_CovErrorMatrix(), m_HasPosition(true), m_HasOrientation(true), m_DataValid(true), m_IGTTimeStamp(0.0), m_Name() { mitk::Vector3D offset = affineTransform3D->GetOffset(); m_Position[0] = offset[0]; m_Position[1] = offset[1]; m_Position[2] = offset[2]; vnl_matrix_fixed rotationMatrix = affineTransform3D->GetMatrix().GetVnlMatrix(); vnl_matrix_fixed rotationMatrixTransposed = rotationMatrix.transpose(); if (checkForRotationMatrix) { // 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))) { mitkThrow() << "tried to initialize NavigationData with non-rotation matrix :" << rotationMatrix << " (Does your AffineTransform3D object include spacing? This is not supported by NavigationData objects!)"; } } // the transpose is because vnl_quaterion expects a transposed rotation matrix m_Orientation = Quaternion(rotationMatrixTransposed); } mitk::AffineTransform3D::Pointer mitk::NavigationData::GetAffineTransform3D() const { AffineTransform3D::Pointer affineTransform3D = AffineTransform3D::New(); // first set rotation affineTransform3D->SetMatrix(this->GetRotationMatrix()); // now set offset Vector3D vector3D; for (int i = 0; i < 3; ++i) { vector3D[i] = m_Position[i]; } affineTransform3D->SetOffset(vector3D); return affineTransform3D; } mitk::Matrix3D mitk::NavigationData::GetRotationMatrix() const { vnl_matrix_fixed vnl_rotation = m_Orientation.rotation_matrix_transpose().transpose(); // :-) Matrix3D mitkRotation; for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { mitkRotation[i][j] = vnl_rotation[i][j]; } } return mitkRotation; } mitk::Point3D mitk::NavigationData::TransformPoint(const mitk::Point3D point) const { vnl_vector_fixed vnlPoint; for (int i = 0; i < 3; ++i) { vnlPoint[i] = point[i]; } Quaternion normalizedQuaternion = this->GetOrientation().normalize(); // first get rotated point vnlPoint = normalizedQuaternion.rotate(vnlPoint); Point3D resultingPoint; for (int i = 0; i < 3; ++i) { // now copy it to our format + offset resultingPoint[i] = vnlPoint[i] + this->GetPosition()[i]; } return resultingPoint; } mitk::NavigationData::Pointer mitk::NavigationData::GetInverse() const { // non-zero quaternion does not have inverse: throw exception in this case. Quaternion zeroQuaternion; zeroQuaternion.fill(0); if (Equal(zeroQuaternion, this->GetOrientation())) mitkThrow() << "tried to invert zero quaternion in NavigationData"; mitk::NavigationData::Pointer navigationDataInverse = NavigationData::Clone(); navigationDataInverse->SetOrientation(this->GetOrientation().inverse()); // To vnl_vector vnl_vector_fixed vnlPoint; for (int i = 0; i < 3; ++i) { vnlPoint[i] = this->GetPosition()[i]; } // invert position vnlPoint = -(navigationDataInverse->GetOrientation().rotate(vnlPoint)); // back to Point3D Point3D invertedPosition = this->GetPosition(); for (int i = 0; i < 3; ++i) { invertedPosition[i] = vnlPoint[i]; } navigationDataInverse->SetPosition(invertedPosition); // Inversion does not care for covariances for now navigationDataInverse->ResetCovarianceValidity(); return navigationDataInverse; } void mitk::NavigationData::ResetCovarianceValidity() { this->SetHasPosition(false); this->SetHasOrientation(false); } mitk::NavigationData::Pointer mitk::NavigationData::getComposition(const mitk::NavigationData::Pointer nd1, const mitk::NavigationData::Pointer nd2) { NavigationData::Pointer nd3 = nd1->Clone(); // A2 * A1 nd3->SetOrientation(nd2->GetOrientation() * nd1->GetOrientation()); // first: b1, b2 vnl vector vnl_vector_fixed b1, b2, b3; for (int i = 0; i < 3; ++i) { b1[i] = nd1->GetPosition()[i]; b2[i] = nd2->GetPosition()[i]; } // b3 = A2b1 + b2 b3 = nd2->GetOrientation().rotate(b1) + b2; // back to mitk::Point3D Point3D point; for (int i = 0; i < 3; ++i) { point[i] = b3[i]; } nd3->SetPosition(point); nd3->ResetCovarianceValidity(); return nd3; } + +bool mitk::Equal(const mitk::NavigationData& leftHandSide, const mitk::NavigationData& rightHandSide, ScalarType eps, bool verbose) +{ + bool returnValue = true; + + // Dimensionality + if( !mitk::Equal(rightHandSide.GetPosition(),leftHandSide.GetPosition()) ) + { + if(verbose) + { + MITK_INFO << "[( NavigationData )] Position differs."; + MITK_INFO << "leftHandSide is " << leftHandSide.GetPosition() + << "rightHandSide is " << rightHandSide.GetPosition(); + } + returnValue = false; + } + + // Dimensionality + if( !mitk::Equal(rightHandSide.GetOrientation(),leftHandSide.GetOrientation()) ) + { + if(verbose) + { + MITK_INFO << "[( NavigationData )] Orientation differs."; + MITK_INFO << "leftHandSide is " << leftHandSide.GetOrientation() + << "rightHandSide is " << rightHandSide.GetOrientation(); + } + returnValue = false; + } + + if( rightHandSide.GetCovErrorMatrix() != leftHandSide.GetCovErrorMatrix() ) + { + if(verbose) + { + MITK_INFO << "[( NavigationData )] CovErrorMatrix differs."; + MITK_INFO << "leftHandSide is " << leftHandSide.GetCovErrorMatrix() + << "rightHandSide is " << rightHandSide.GetCovErrorMatrix(); + } + returnValue = false; + } + + if( std::string(rightHandSide.GetName()) != std::string(leftHandSide.GetName()) ) + { + if(verbose) + { + MITK_INFO << "[( NavigationData )] Name differs."; + MITK_INFO << "leftHandSide is " << leftHandSide.GetName() + << "rightHandSide is " << rightHandSide.GetName(); + } + returnValue = false; + } + + if( rightHandSide.GetIGTTimeStamp() != leftHandSide.GetIGTTimeStamp() ) + { + if(verbose) + { + MITK_INFO << "[( NavigationData )] IGTTimeStamp differs."; + MITK_INFO << "leftHandSide is " << leftHandSide.GetIGTTimeStamp() + << "rightHandSide is " << rightHandSide.GetIGTTimeStamp(); + } + returnValue = false; + } + + return returnValue; +} diff --git a/Modules/IGT/DataManagement/mitkNavigationData.h b/Modules/IGT/DataManagement/mitkNavigationData.h index b7966ef3e4..dc54f172a5 100644 --- a/Modules/IGT/DataManagement/mitkNavigationData.h +++ b/Modules/IGT/DataManagement/mitkNavigationData.h @@ -1,278 +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 MitkIGT_EXPORT NavigationData : public itk::DataObject { public: mitkClassMacro(NavigationData, itk::DataObject); - itkFactorylessNewMacro(Self) - itkCloneMacro(Self) + 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 */ 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 */ itkSetMacro(IGTTimeStamp, TimeStampType); /** * \brief gets the IGT timestamp of the NavigationData object */ 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); /** * \brief copy meta data of a NavigationData object * * copies all meta data from NavigationData data to this object */ virtual void CopyInformation(const DataObject* data); /** * \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; /** * 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 + */ + MitkIGT_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_ */