diff --git a/Modules/IGT/DataManagement/mitkNavigationData.cpp b/Modules/IGT/DataManagement/mitkNavigationData.cpp index c1199b8d9f..36ccdb74ae 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(); + mitk::NavigationData::Pointer navigationDataInverse = this->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 728c01783c..0c995ab154 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_ */ diff --git a/Modules/IGT/Testing/mitkNavigationDataTest.cpp b/Modules/IGT/Testing/mitkNavigationDataTest.cpp index 5ee59e216e..8edf576c8a 100644 --- a/Modules/IGT/Testing/mitkNavigationDataTest.cpp +++ b/Modules/IGT/Testing/mitkNavigationDataTest.cpp @@ -1,579 +1,624 @@ /*=================================================================== 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 "mitkTestingMacros.h" #include "mitkNavigationData.h" #include "mitkVector.h" #include #include using namespace mitk; /** * helper function which checks if the non position / orientation / covariance related members are all set to certain values */ static bool AreBasicNavigationMembersEqual(const NavigationData::Pointer nd, const bool dataValid, const NavigationData::TimeStampType timeStamp, const std::string name) { bool result = true; result = result && (nd->IsDataValid() == dataValid); result = result && (mitk::Equal(nd->GetIGTTimeStamp(), timeStamp)); result = result && (0 == name.compare(nd->GetName())); return result; } /** * helper function which checks if the basic members, meaning the members not related to position / orientation / covariance * are the same for two NavigationData objects. */ static bool AreBasicNavigationMembersEqual(const NavigationData::Pointer nd1, const NavigationData::Pointer nd2) { return AreBasicNavigationMembersEqual(nd1, nd2->IsDataValid(), nd2->GetIGTTimeStamp(), nd2->GetName()); } /** * Test if the NavigationData object nd has the correct settings for covariance matrix specific members. * * @param nd * @param hasPosition the desired value for the hasPosition member * @param hasOrientation the desired value for the hasOrientation member * @param covMatrix the covariance matrix nd should have * @return */ static bool AreCovarianceNavigationMembersEqual(const NavigationData::Pointer nd, bool hasPosition, bool hasOrientation, NavigationData::CovarianceMatrixType covMatrix) { bool result = true; result = result && (nd->GetHasPosition() == hasPosition); result = result && (nd->GetHasOrientation() == hasOrientation); result = result && (mitk::MatrixEqualElementWise(nd->GetCovErrorMatrix(), covMatrix)); return result; } /** * Checks if the covariance related members of the two NavigationData objects are the same * * @see AreCovarianceNavigationMembersEqual * * @param nd1 * @param nd2 * @return */ static bool AreCovarianceNavigationMembersEqual(const NavigationData::Pointer nd1, const NavigationData::Pointer nd2) { return AreCovarianceNavigationMembersEqual(nd1, nd2->GetHasPosition(), nd2->GetHasOrientation(), nd2->GetCovErrorMatrix()); } /** * Creates a fully set NavigationData object for testing purposes. * * @return a smartpointer to the object */ static mitk::NavigationData::Pointer GetTestData() { mitk::NavigationData::Pointer nd = mitk::NavigationData::New(); mitk::NavigationData::PositionType p; mitk::FillVector3D(p, 44.4, 55.5, 66.66); nd->SetPosition(p); mitk::NavigationData::OrientationType o(1.0, 2.0, 3.0, 4.0); nd->SetOrientation(o); nd->SetDataValid(true); nd->SetIGTTimeStamp(100.111); nd->SetHasPosition(false); nd->SetHasOrientation(false); mitk::NavigationData::CovarianceMatrixType m; m.Fill(17.17); m(2, 2) = 1000.01; nd->SetCovErrorMatrix(m); nd->SetName("my NavigationData"); nd->SetPositionAccuracy(100.0); nd->SetOrientationAccuracy(10.0); return nd; } static void TestInstatiation() { // Test instantiation of NavigationData mitk::NavigationData::Pointer nd = mitk::NavigationData::New(); MITK_TEST_CONDITION(nd.IsNotNull(),"Test instatiation"); } static void TestGetterSetter() { mitk::NavigationData::Pointer nd = mitk::NavigationData::New(); mitk::NavigationData::PositionType p; mitk::FillVector3D(p, 44.4, 55.5, 66.66); nd->SetPosition(p); MITK_TEST_CONDITION(nd->GetPosition() == p, "Set-/GetPosition()"); mitk::NavigationData::OrientationType o(1.0, 2.0, 3.0, 4.0); nd->SetOrientation(o); MITK_TEST_CONDITION(nd->GetOrientation() == o, "Set-/GetOrientation()"); nd->SetDataValid(true); MITK_TEST_CONDITION(nd->IsDataValid() == true, "Set-/IsDataValid()"); nd->SetIGTTimeStamp(100.111); MITK_TEST_CONDITION(mitk::Equal(nd->GetIGTTimeStamp(), 100.111), "Set-/GetIGTTimeStamp()"); nd->SetHasPosition(false); MITK_TEST_CONDITION(nd->GetHasPosition() == false, "Set-/GetHasPosition()"); nd->SetHasOrientation(false); MITK_TEST_CONDITION(nd->GetHasOrientation() == false, "Set-/GetHasOrientation()"); mitk::NavigationData::CovarianceMatrixType m; m.Fill(17.17); m(2, 2) = 1000.01; nd->SetCovErrorMatrix(m); MITK_TEST_CONDITION(nd->GetCovErrorMatrix() == m, "Set-/GetCovErrorMatrix()"); nd->SetName("my NavigationData"); MITK_TEST_CONDITION(std::string(nd->GetName()) == "my NavigationData", "Set-/GetName()"); nd->SetPositionAccuracy(100.0); mitk::NavigationData::CovarianceMatrixType result = nd->GetCovErrorMatrix(); MITK_TEST_CONDITION(mitk::Equal(result(0, 0), 10000.0) && mitk::Equal(result(1, 1), 10000.0) && mitk::Equal(result(2, 2), 10000.0), "SetPositionAccuracy()"); nd->SetOrientationAccuracy(10.0); mitk::NavigationData::CovarianceMatrixType result2 = nd->GetCovErrorMatrix(); MITK_TEST_CONDITION(mitk::Equal(result2(3, 3), 100.0) && mitk::Equal(result2(4, 4), 100.0) && mitk::Equal(result2(5, 5), 100.0), "SetOrientationAccuracy()"); } static void TestGraft() { //create test data mitk::NavigationData::Pointer nd = GetTestData(); mitk::NavigationData::Pointer graftedCopy = mitk::NavigationData::New(); graftedCopy->Graft(nd); bool graftIsEqual = (nd->GetPosition() == graftedCopy->GetPosition()) && (nd->GetOrientation() == graftedCopy->GetOrientation()) && AreCovarianceNavigationMembersEqual(nd, graftedCopy) && AreBasicNavigationMembersEqual(nd, graftedCopy); MITK_TEST_CONDITION(graftIsEqual, "Graft() produces equal NavigationData object"); } static void TestPrintSelf() { mitk::NavigationData::Pointer nd = GetTestData(); itk::Indent myIndent = itk::Indent(); MITK_TEST_OUTPUT(<<"Testing method PrintSelf(), method output will follow:"); bool success = true; try { nd->PrintSelf(std::cout,myIndent); } catch(...) { success = false; } MITK_TEST_CONDITION(success, "Testing method PrintSelf()."); } static void TestWrongInputs() { mitk::NavigationData::Pointer nd = GetTestData(); // Test CopyInformation bool success = false; try { nd->CopyInformation(NULL); } catch(itk::ExceptionObject e) { success = true; } MITK_TEST_CONDITION(success, "Testing wrong input for method CopyInformation."); // Test Graft success = false; try { nd->Graft(NULL); } catch(itk::ExceptionObject e) { success = true; } MITK_TEST_CONDITION(success, "Testing wrong input for method Graft."); } static mitk::Quaternion quaternion; +static mitk::Quaternion quaternion_realistic; static mitk::Vector3D offsetVector; static mitk::Point3D offsetPoint; static mitk::Matrix3D rotation; static mitk::Quaternion quaternion2; static mitk::Vector3D offsetVector2; static mitk::Point3D offsetPoint2; static mitk::Matrix3D rotation2; static mitk::Point3D point; /** * Helper method setting up data required for tests (like points which shall be transformed by the NavigationData). */ static void SetupNaviDataTests() { // set rotation matrix to /* * 0 -1 0 * 1 0 0 * 0 0 1 */ rotation.Fill(0); rotation[0][1] = -1; rotation[1][0] = 1; rotation[2][2] = 1; // set quaternion to quaternion equivalent // values calculated with javascript at // http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/ quaternion = mitk::Quaternion(0, 0, 0.7071067811865475, 0.7071067811865476); + // a more realistic quaternion from real tracking data + quaternion_realistic = mitk::Quaternion(-0.57747,0.225593,0.366371,0.693933); + // set offset to some value. Some tests need vectors, offers points. double offsetArray[3] = {1.0,2.0,3.123456}; offsetVector = offsetArray; offsetPoint = offsetArray; /***** Second set of data for compose tests ****/ // set rotation2 matrix to /* * 1 0 0 * 0 0 -1 * 0 1 0 */ rotation2.Fill(0); rotation2[0][0] = 1; rotation2[1][2] = -1; rotation2[2][1] = 1; quaternion2 = mitk::Quaternion(0.7071067811865475, 0, 0, 0.7071067811865476); mitk::ScalarType offsetArray2[3] = {1, 0, 0}; offsetVector2 = offsetArray2; offsetPoint2 = offsetArray2; /***** Create a point to be transformed *****/ mitk::ScalarType pointArray[] = {1.0, 3.0, 5.0}; point = pointArray; } /** * Helper method, which creates a NavigationData object using the data created in SetupNaviDataTests() */ static mitk::NavigationData::Pointer CreateNavidata(mitk::Quaternion quaternion, mitk::Point3D offset) { mitk::NavigationData::Pointer navigationData = mitk::NavigationData::New(); navigationData->SetOrientation(quaternion); navigationData->SetPosition(offset); return navigationData; } /** * Helper method which creates an affine transformation only composed by a rotation and a translation (no scalings and the likes). * * @param rotationMatrix * @param offset * @return */ static mitk::AffineTransform3D::Pointer CreateAffineTransform(mitk::Matrix3D rotationMatrix, mitk::Vector3D offset) { mitk::AffineTransform3D::Pointer affineTransform3D = mitk::AffineTransform3D::New(); affineTransform3D->SetOffset(offset); affineTransform3D->SetMatrix(rotationMatrix); return affineTransform3D; } /** * Test if NavigationData::GetInverse returns the correct inverse */ static void TestInverse() { SetupNaviDataTests(); mitk::NavigationData::Pointer nd = CreateNavidata(quaternion, offsetPoint); mitk::NavigationData::Pointer ndInverse = nd->GetInverse(); // calculate expected inverted position vector: b2 = -A2b1 // for -A2b1 we need vnl_vectors. vnl_vector_fixed b1; for (int i = 0; i < 3; ++i) { b1[i] = nd->GetPosition()[i]; } vnl_vector_fixed b2; b2 = -(ndInverse->GetOrientation().rotate(b1)); // now copy result back into our mitk::Point3D mitk::Point3D invertedPosition; for (int i = 0; i < 3; ++i) { invertedPosition[i] = b2[i]; } MITK_TEST_CONDITION(mitk::Equal(nd->GetOrientation().inverse(), ndInverse->GetOrientation()),"Testing GetInverse: orientation inverted"); MITK_TEST_CONDITION(mitk::Equal(invertedPosition, ndInverse->GetPosition()), "Testing GetInverse: position inverted"); bool otherFlagsOk = AreBasicNavigationMembersEqual(nd, ndInverse) && AreCovarianceNavigationMembersEqual(ndInverse, false, false, nd->GetCovErrorMatrix()); // covariance update mechanism not yet implemented, thus validities are set to false. MITK_TEST_CONDITION(otherFlagsOk, "Testing GetInverse: other flags are same"); + + //######################################################################################## + //################### Second test with more realistic quaternion ######################### + //######################################################################################## + + //just copy data to be real sure that it is not overwritten during the test + mitk::Quaternion referenceQuaternion; + referenceQuaternion[0] = quaternion_realistic[0]; + referenceQuaternion[1] = quaternion_realistic[1]; + referenceQuaternion[2] = quaternion_realistic[2]; + referenceQuaternion[3] = quaternion_realistic[3]; + + mitk::Point3D referencePoint; + referencePoint[0] = offsetPoint[0]; + referencePoint[1] = offsetPoint[1]; + referencePoint[2] = offsetPoint[2]; + referencePoint[3] = offsetPoint[3]; + + mitk::NavigationData::Pointer nd2 = CreateNavidata(quaternion_realistic, offsetPoint); + + mitk::NavigationData::Pointer ndInverse2 = nd2->GetInverse(); + MITK_TEST_CONDITION(mitk::Equal(nd2->GetOrientation(),referenceQuaternion),"Testing if the method GetInverse() modifies the data which should never happen!"); + MITK_TEST_CONDITION(mitk::Equal(ndInverse2->GetOrientation(),referenceQuaternion.inverse()),"Testing if the Qrientation was inverted correctly with the realistic quaternion"); } /** * Test if the original orientation and position are retained after inverting two times. */ static void TestDoubleInverse() { SetupNaviDataTests(); mitk::NavigationData::Pointer nd = CreateNavidata(quaternion, offsetPoint); mitk::NavigationData::Pointer ndDoubleInverse = nd->GetInverse()->GetInverse(); MITK_TEST_CONDITION(mitk::Equal(nd->GetOrientation(), ndDoubleInverse->GetOrientation()),"Testing GetInverse double application: orientation preserved"); MITK_TEST_CONDITION(mitk::Equal(nd->GetPosition(), ndDoubleInverse->GetPosition()), "Testing GetInverse double application: position preserved"); } /** * Tests if NavigationData::GetInverse throws an error if the NavigationData has no inverse * (e.g. after it is initialized, no rotation is stored -> the transformation cannot be inverted). */ static void TestInverseError() { // initialize empty NavigationData (quaternion is zeroed) mitk::NavigationData::Pointer nd = mitk::NavigationData::New(); mitk::Quaternion quaternion; quaternion.fill(0); nd->SetOrientation(quaternion); MITK_TEST_FOR_EXCEPTION(mitk::Exception&, nd->GetInverse()); } /** * Tests if NavigationData::TransformPoint works. */ static void TestTransform() { SetupNaviDataTests(); mitk::NavigationData::Pointer navigationData = CreateNavidata(quaternion, offsetPoint); point = navigationData->TransformPoint(point); mitk::ScalarType resultingPointArray[] = {-2, 3, 8.123456}; mitk::Point3D resultingPoint = resultingPointArray; MITK_TEST_CONDITION(mitk::Equal(resultingPoint, point), "Testing point transformation"); } /** * Tests if the construction of a NavigationData object from an * affine transformation which is only composed of a rotation and translation works. */ static void TestAffineConstructor() { SetupNaviDataTests(); mitk::AffineTransform3D::Pointer affineTransform3D = CreateAffineTransform(rotation, offsetVector); mitk::NavigationData::Pointer navigationData = mitk::NavigationData::New(affineTransform3D); MITK_TEST_CONDITION(AreBasicNavigationMembersEqual(navigationData, true, 0.0, ""), "Testing affine constructor: dataValid, timeStamp and name have been initialized to default values"); NavigationData::CovarianceMatrixType covMatrix; // empty covariance matrix MITK_TEST_CONDITION(AreCovarianceNavigationMembersEqual(navigationData, true, true, covMatrix), "Testing affine constructor: covariance matrix values have been correctly initialized"); // TODO: discuss with Alfred // why this is the desired initialization of the covariance information. MITK_TEST_CONDITION(mitk::Equal(navigationData->GetPosition(), offsetPoint), "Testing affine constructor: offset"); MITK_TEST_CONDITION(mitk::Equal(navigationData->GetOrientation(), quaternion), "Testing affine constructor: quaternion"); } /** * Tests if the construction of a NavigationData object throws an error when constructing from an * affine transformation which is orthogonal. */ static void TestAffineConstructorErrorTransposedNotInverse() { SetupNaviDataTests(); rotation.SetIdentity(); rotation[1][0] = 2; // this matrix has determinant = 1 (triangular matrix with ones in diagonal) but transposed != inverse mitk::AffineTransform3D::Pointer affineTransform3D = CreateAffineTransform(rotation, offsetVector); MITK_TEST_FOR_EXCEPTION(mitk::Exception&, mitk::NavigationData::New(affineTransform3D)); } /** * Tests if the construction of a NavigationData object throws an error when constructing from an * affine transformation which is not normal. */ static void TestAffineConstructorErrorDeterminantNonEqualOne() { SetupNaviDataTests(); rotation.SetIdentity(); rotation[0][0] = 2; // determinant for diagonal matrices is product of diagonal elements => det = 2 mitk::AffineTransform3D::Pointer affineTransform3D = CreateAffineTransform(rotation, offsetVector); MITK_TEST_FOR_EXCEPTION(mitk::Exception&, mitk::NavigationData::New(affineTransform3D)); } /** * Tests if the error suppression mechanism works if a NavigationData object is created from a * invalid affine transformation (one that is not only rotation and translation). */ static void TestAffineConstructorErrorCheckingFalse() { SetupNaviDataTests(); rotation.SetIdentity(); rotation[0][0] = 2; // determinant for diagonal matrices is product of diagonal elements => det = 2 mitk::AffineTransform3D::Pointer affineTransform3D = CreateAffineTransform(rotation, offsetVector); bool exceptionSuppressed = true; try { mitk::NavigationData::New(affineTransform3D, false); } catch (mitk::Exception&) { exceptionSuppressed = false; } MITK_TEST_CONDITION(exceptionSuppressed, "Test affine constructor: exception can be suppressed.") } /** * Test if NavigationData::GetAffineTransform3D() returns the correct affine transformation. */ static void TestAffineGetter() { SetupNaviDataTests(); mitk::NavigationData::Pointer navigationData = CreateNavidata(quaternion, offsetPoint); mitk::AffineTransform3D::Pointer affineTransform = navigationData->GetAffineTransform3D(); MITK_TEST_CONDITION(mitk::Equal(affineTransform->GetOffset(), offsetVector), "Testing AffineTransform3D getter: offset"); MITK_TEST_CONDITION(mitk::MatrixEqualElementWise(affineTransform->GetMatrix(), rotation), "Testing AffineTransform3D getter: rotation"); } /** * This test tests the complete chain from affineTransform -> NavigationData -> affineTransform */ static void TestAffineToNaviDataToAffine() { SetupNaviDataTests(); mitk::AffineTransform3D::Pointer affineTransform3D = CreateAffineTransform(rotation, offsetVector); // there and back again: affineTransform -> NavigationData -> affineTransform mitk::NavigationData::Pointer navigationData = mitk::NavigationData::New(affineTransform3D); mitk::AffineTransform3D::Pointer affineTransform3D_2; affineTransform3D_2 = navigationData->GetAffineTransform3D(); MITK_TEST_CONDITION(mitk::Equal(affineTransform3D->GetOffset(), affineTransform3D_2->GetOffset()), "Testing affine -> navidata -> affine chain: offset"); MITK_TEST_CONDITION(mitk::MatrixEqualElementWise(affineTransform3D->GetMatrix(), affineTransform3D_2->GetMatrix()), "Testing affine -> navidata -> affine chain: rotation"); } static void TestCompose(bool pre = false) { SetupNaviDataTests(); mitk::NavigationData::Pointer nd = CreateNavidata(quaternion, offsetPoint); mitk::AffineTransform3D::Pointer at = CreateAffineTransform(rotation, offsetVector); // second transform for composition mitk::NavigationData::Pointer nd2 = CreateNavidata(quaternion2, offsetPoint2); mitk::AffineTransform3D::Pointer at2 = CreateAffineTransform(rotation2, offsetVector2); // save point for affinetransform mitk::Point3D point2 = point; nd->Compose(nd2, pre); point = nd->TransformPoint(point); at->Compose(at2, pre); point2 = at->TransformPoint(point2); MITK_TEST_CONDITION(mitk::Equal(point, point2), "Compose pre = " << pre << ": composition works "); bool covarianceValidityReset = !nd->GetHasOrientation() && !nd->GetHasPosition(); MITK_TEST_CONDITION(covarianceValidityReset, "Compose pre = " << pre << ": covariance validities reset because not implemented yet."); } static void TestReverseCompose() { TestCompose(true); } +/** + * Tests the clone method. + */ +static void TestClone() +{ + SetupNaviDataTests(); + mitk::NavigationData::Pointer nd = CreateNavidata(quaternion, offsetPoint); + mitk::NavigationData::Pointer myClone = nd->Clone(); + MITK_TEST_CONDITION(mitk::Equal(*nd,*myClone,mitk::eps,true), "Test if clone is equal to original object."); + + //change clone, original object should not change + mitk::Point3D myPoint; + mitk::FillVector3D(myPoint,121,32132,433); + myClone->SetPosition(myPoint); + MITK_TEST_CONDITION(!mitk::Equal(*nd,*myClone), "Test if clone could be modified without changing the original object."); +} + /** * This function is testing the Class mitk::NavigationData. For most tests we would need the MicronTracker hardware, so only a few * simple tests, which can run without the hardware are implemented yet (2009, January, 23rd). As soon as there is a working * concept to test the tracking classes which are very close to the hardware on all systems more tests are needed here. */ int mitkNavigationDataTest(int /* argc */, char* /*argv*/[]) { MITK_TEST_BEGIN("NavigationData"); TestInstatiation(); TestGetterSetter(); TestGraft(); TestPrintSelf(); TestWrongInputs(); TestAffineConstructor(); TestAffineConstructorErrorDeterminantNonEqualOne(); TestAffineConstructorErrorTransposedNotInverse(); TestAffineConstructorErrorCheckingFalse(); TestAffineGetter(); TestAffineToNaviDataToAffine(); TestTransform(); TestInverse(); TestDoubleInverse(); TestInverseError(); TestCompose(); TestReverseCompose(); + TestClone(); MITK_TEST_END(); }