diff --git a/Modules/IGT/IGTFilters/mitkNavigationDataLandmarkTransformFilter.cpp b/Modules/IGT/IGTFilters/mitkNavigationDataLandmarkTransformFilter.cpp index b0f6257888..b7a8d5b6f1 100644 --- a/Modules/IGT/IGTFilters/mitkNavigationDataLandmarkTransformFilter.cpp +++ b/Modules/IGT/IGTFilters/mitkNavigationDataLandmarkTransformFilter.cpp @@ -1,461 +1,460 @@ /*=================================================================== 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 "mitkNavigationDataLandmarkTransformFilter.h" #include "itkIndent.h" #include "itkEuler3DTransform.h" #include "itkVersorRigid3DTransform.h" #include "itkEuclideanDistancePointMetric.h" #include "itkLevenbergMarquardtOptimizer.h" #include "itkPointSet.h" #include "itkPointSetToPointSetRegistrationMethod.h" #include <algorithm> mitk::NavigationDataLandmarkTransformFilter::NavigationDataLandmarkTransformFilter() : mitk::NavigationDataToNavigationDataFilter(), m_ErrorMean(-1.0), m_ErrorStdDev(-1.0), m_ErrorRMS(-1.0), m_ErrorMin(-1.0), m_ErrorMax(-1.0), m_ErrorAbsMax(-1.0), m_SourcePoints(), m_TargetPoints(), m_LandmarkTransformInitializer(NULL), m_LandmarkTransform(NULL), m_QuatLandmarkTransform(NULL), m_QuatTransform(NULL), m_Errors(), m_UseICPInitialization(false) { m_LandmarkTransform = LandmarkTransformType::New(); m_LandmarkTransformInitializer = TransformInitializerType::New(); m_LandmarkTransformInitializer->SetTransform(m_LandmarkTransform); //transform to rotate orientation m_QuatLandmarkTransform = QuaternionTransformType::New(); m_QuatTransform = QuaternionTransformType::New(); } mitk::NavigationDataLandmarkTransformFilter::~NavigationDataLandmarkTransformFilter() { m_LandmarkTransform = NULL; m_LandmarkTransformInitializer = NULL; m_QuatLandmarkTransform = NULL; m_QuatTransform = NULL; } void mitk::NavigationDataLandmarkTransformFilter::InitializeLandmarkTransform(LandmarkPointContainer& sources, const LandmarkPointContainer& targets) { if (m_UseICPInitialization == true) { if (this->FindCorrespondentLandmarks(sources, targets) == false) // determine landmark correspondences with iterative closest point optimization, sort sort landmarks accordingly { itkExceptionMacro("Landmark correspondence finding failed."); } } if(m_SourcePoints.size() != m_TargetPoints.size())// check whether target and source points size are equal itk registration won't work otherways return; this->UpdateLandmarkTransform(sources, targets); // if size of source and target points is equal } void mitk::NavigationDataLandmarkTransformFilter::SetSourceLandmarks(mitk::PointSet::Pointer mitkSourcePointSet) { m_SourcePoints.clear(); mitk::PointSet::PointType mitkSourcePoint; TransformInitializerType::LandmarkPointType lPoint; for (mitk::PointSet::PointsContainer::ConstIterator it = mitkSourcePointSet->GetPointSet()->GetPoints()->Begin(); it != mitkSourcePointSet->GetPointSet()->GetPoints()->End(); ++it) { mitk::FillVector3D(lPoint, it->Value().GetElement(0), it->Value().GetElement(1), it->Value().GetElement(2)); m_SourcePoints.push_back(lPoint); } if (m_SourcePoints.size() < 3) { itkExceptionMacro("SourcePointSet must contain at least 3 points"); } if (this->IsInitialized()) this->InitializeLandmarkTransform(m_SourcePoints, m_TargetPoints); } void mitk::NavigationDataLandmarkTransformFilter::SetTargetLandmarks(mitk::PointSet::Pointer mitkTargetPointSet) { m_TargetPoints.clear(); TransformInitializerType::LandmarkPointType lPoint; for (mitk::PointSet::PointsContainer::ConstIterator it = mitkTargetPointSet->GetPointSet()->GetPoints()->Begin(); it != mitkTargetPointSet->GetPointSet()->GetPoints()->End(); ++it) { mitk::FillVector3D(lPoint, it->Value().GetElement(0), it->Value().GetElement(1), it->Value().GetElement(2)); m_TargetPoints.push_back(lPoint); } if (m_TargetPoints.size() < 3) { itkExceptionMacro("TargetPointSet must contain at least 3 points"); } if (this->IsInitialized()) this->InitializeLandmarkTransform(m_SourcePoints, m_TargetPoints); } mitk::ScalarType mitk::NavigationDataLandmarkTransformFilter::GetFRE() const { return m_ErrorMean; } mitk::ScalarType mitk::NavigationDataLandmarkTransformFilter::GetFREStdDev() const { return m_ErrorStdDev; } mitk::ScalarType mitk::NavigationDataLandmarkTransformFilter::GetRMSError() const { return m_ErrorRMS; } mitk::ScalarType mitk::NavigationDataLandmarkTransformFilter::GetMinError() const { return m_ErrorMin; } mitk::ScalarType mitk::NavigationDataLandmarkTransformFilter::GetMaxError() const { return m_ErrorMax; } mitk::ScalarType mitk::NavigationDataLandmarkTransformFilter::GetAbsMaxError() const { return m_ErrorAbsMax; } void mitk::NavigationDataLandmarkTransformFilter::AccumulateStatistics(std::vector<mitk::ScalarType>& vector) { //mean, min, max m_ErrorMean = 0.0; m_ErrorMin = itk::NumericTraits<mitk::ScalarType>::max(); m_ErrorMax = itk::NumericTraits<mitk::ScalarType>::min(); m_ErrorAbsMax = 0.0; m_ErrorRMS = 0.0; for (std::vector<mitk::ScalarType>::size_type i = 0; i < vector.size(); i++) { m_ErrorMean += vector[i]; // mean m_ErrorRMS += pow(vector[i],2); // RMS if(vector[i] < m_ErrorMin) // min m_ErrorMin = vector[i]; if(vector[i] > m_ErrorMax) // max m_ErrorMax = vector[i]; if(fabs(vector[i]) > fabs(m_ErrorAbsMax)) // abs_max m_ErrorAbsMax = vector[i]; } m_ErrorMean /= vector.size(); m_ErrorRMS = sqrt(m_ErrorRMS/vector.size()); //standard deviation m_ErrorStdDev = 0.0; for (std::vector<mitk::ScalarType>::size_type i = 0; i < vector.size(); i++) m_ErrorStdDev += pow(vector[i] - m_ErrorMean, 2); if(vector.size() > 1) m_ErrorStdDev = sqrt(m_ErrorStdDev / (vector.size() - 1.0)); this->Modified(); } void mitk::NavigationDataLandmarkTransformFilter::GenerateData() { this->CreateOutputsForAllInputs(); // make sure that we have the same number of outputs as inputs TransformInitializerType::LandmarkPointType lPointIn, lPointOut; /* update outputs with tracking data from tools */ for (unsigned int i = 0; i < this->GetNumberOfOutputs() ; ++i) { mitk::NavigationData* output = this->GetOutput(i); assert(output); const mitk::NavigationData* input = this->GetInput(i); assert(input); if (input->IsDataValid() == false) { output->SetDataValid(false); continue; } output->Graft(input); // First, copy all information from input to output if (this->IsInitialized() == false) // as long as there is no valid transformation matrix, only graft the outputs continue; mitk::NavigationData::PositionType tempCoordinate; tempCoordinate = input->GetPosition(); lPointIn[0] = tempCoordinate[0]; // convert navigation data position to transform point lPointIn[1] = tempCoordinate[1]; lPointIn[2] = tempCoordinate[2]; /* transform position */ lPointOut = m_LandmarkTransform->TransformPoint(lPointIn); // transform position tempCoordinate[0] = lPointOut[0]; // convert back into navigation data position tempCoordinate[1] = lPointOut[1]; tempCoordinate[2] = lPointOut[2]; output->SetPosition(tempCoordinate); // update output navigation data with new position /* transform orientation */ NavigationData::OrientationType quatIn = input->GetOrientation(); vnl_quaternion<double> const vnlQuatIn(quatIn.x(), quatIn.y(), quatIn.z(), quatIn.r()); // convert orientation into vnl quaternion m_QuatTransform->SetRotation(vnlQuatIn); // convert orientation into transform - vnl_quaternion<double> const vnlQLTrans = vnl_quaternion<double>(m_LandmarkTransform->GetMatrix().GetVnlMatrix()); + m_QuatLandmarkTransform->SetMatrix(m_LandmarkTransform->GetMatrix()); - m_QuatLandmarkTransform->SetRotation(vnlQLTrans); // set rotation from landmark transform m_QuatLandmarkTransform->Compose(m_QuatTransform, true); // compose navigation data transform and landmark transform vnl_quaternion<double> vnlQuatOut = m_QuatLandmarkTransform->GetRotation(); // convert composed transform back into a quaternion NavigationData::OrientationType quatOut(vnlQuatOut[0], vnlQuatOut[1], vnlQuatOut[2], vnlQuatOut[3]); // convert back into navigation data orientation output->SetOrientation(quatOut); // update output navigation data with new orientation output->SetDataValid(true); // operation was successful, therefore data of output is valid. } } bool mitk::NavigationDataLandmarkTransformFilter::IsInitialized() const { return (m_SourcePoints.size() >= 3) && (m_TargetPoints.size() >= 3); } void mitk::NavigationDataLandmarkTransformFilter::PrintSelf( std::ostream& os, itk::Indent indent ) const { Superclass::PrintSelf(os, indent); os << indent << this->GetNameOfClass() << ":\n"; os << indent << m_SourcePoints.size() << " SourcePoints exist: \n"; itk::Indent nextIndent = indent.GetNextIndent(); unsigned int i = 0; for (LandmarkPointContainer::const_iterator it = m_SourcePoints.begin(); it != m_SourcePoints.end(); ++it) { os << nextIndent << "Point " << i++ << ": ["; os << it->GetElement(0); for (unsigned int i = 1; i < TransformInitializerType::LandmarkPointType::GetPointDimension(); ++i) { os << ", " << it->GetElement(i); } os << "]\n"; } os << indent << m_TargetPoints.size() << " TargetPoints exist: \n"; i = 0; for (LandmarkPointContainer::const_iterator it = m_TargetPoints.begin(); it != m_TargetPoints.end(); ++it) { os << nextIndent << "Point " << i++ << ": ["; os << it->GetElement(0); for (unsigned int i = 1; i < TransformInitializerType::LandmarkPointType::GetPointDimension(); ++i) { os << ", " << it->GetElement(i); } os << "]\n"; } os << indent << "Landmarktransform initialized: " << this->IsInitialized() << "\n"; if (this->IsInitialized() == true) m_LandmarkTransform->Print(os, nextIndent); os << indent << "Registration error statistics:\n"; os << nextIndent << "FRE: " << this->GetFRE() << "\n"; os << nextIndent << "FRE std.dev.: " << this->GetFREStdDev() << "\n"; os << nextIndent << "FRE RMS: " << this->GetRMSError() << "\n"; os << nextIndent << "Minimum registration error: " << this->GetMinError() << "\n"; os << nextIndent << "Maximum registration error: " << this->GetMaxError() << "\n"; os << nextIndent << "Absolute Maximum registration error: " << this->GetAbsMaxError() << "\n"; } const mitk::NavigationDataLandmarkTransformFilter::ErrorVector& mitk::NavigationDataLandmarkTransformFilter::GetErrorVector() const { return m_Errors; } bool mitk::NavigationDataLandmarkTransformFilter::FindCorrespondentLandmarks(LandmarkPointContainer& sources, const LandmarkPointContainer& targets) const { if (sources.size() < 6 || targets.size() < 6) return false; //throw std::invalid_argument("ICP correspondence finding needs at least 6 landmarks"); /* lots of type definitions */ typedef itk::PointSet<mitk::ScalarType, 3> PointSetType; //typedef itk::BoundingBox<PointSetType::PointIdentifier, PointSetType::PointDimension> BoundingBoxType; typedef itk::EuclideanDistancePointMetric< PointSetType, PointSetType> MetricType; //typedef MetricType::TransformType TransformBaseType; //typedef MetricType::TransformType::ParametersType ParametersType; //typedef TransformBaseType::JacobianType JacobianType; //typedef itk::Euler3DTransform< double > TransformType; typedef itk::VersorRigid3DTransform< double > TransformType; typedef TransformType ParametersType; typedef itk::PointSetToPointSetRegistrationMethod< PointSetType, PointSetType > RegistrationType; /* copy landmarks to itk pointsets for registration */ PointSetType::Pointer sourcePointSet = PointSetType::New(); unsigned int i = 0; for (LandmarkPointContainer::const_iterator it = sources.begin(); it != sources.end(); ++it) { PointSetType::PointType doublePoint; mitk::itk2vtk(*it, doublePoint); // copy mitk::ScalarType point into double point as workaround to ITK 3.10 bug sourcePointSet->SetPoint(i++, doublePoint /**it*/); } i = 0; PointSetType::Pointer targetPointSet = PointSetType::New(); for (LandmarkPointContainer::const_iterator it = targets.begin(); it != targets.end(); ++it) { PointSetType::PointType doublePoint; mitk::itk2vtk(*it, doublePoint); // copy mitk::ScalarType point into double point as workaround to ITK 3.10 bug targetPointSet->SetPoint(i++, doublePoint /**it*/); } /* get centroid and extends of our pointsets */ //BoundingBoxType::Pointer sourceBoundingBox = BoundingBoxType::New(); //sourceBoundingBox->SetPoints(sourcePointSet->GetPoints()); //sourceBoundingBox->ComputeBoundingBox(); //BoundingBoxType::Pointer targetBoundingBox = BoundingBoxType::New(); //targetBoundingBox->SetPoints(targetPointSet->GetPoints()); //targetBoundingBox->ComputeBoundingBox(); TransformType::Pointer transform = TransformType::New(); transform->SetIdentity(); //transform->SetTranslation(targetBoundingBox->GetCenter() - sourceBoundingBox->GetCenter()); itk::LevenbergMarquardtOptimizer::Pointer optimizer = itk::LevenbergMarquardtOptimizer::New(); optimizer->SetUseCostFunctionGradient(false); RegistrationType::Pointer registration = RegistrationType::New(); // Scale the translation components of the Transform in the Optimizer itk::LevenbergMarquardtOptimizer::ScalesType scales(transform->GetNumberOfParameters()); const double translationScale = 5000; //sqrtf(targetBoundingBox->GetDiagonalLength2()) * 1000; // dynamic range of translations const double rotationScale = 1.0; // dynamic range of rotations scales[0] = 1.0 / rotationScale; scales[1] = 1.0 / rotationScale; scales[2] = 1.0 / rotationScale; scales[3] = 1.0 / translationScale; scales[4] = 1.0 / translationScale; scales[5] = 1.0 / translationScale; //scales.Fill(0.01); unsigned long numberOfIterations = 80000; double gradientTolerance = 1e-10; // convergence criterion double valueTolerance = 1e-10; // convergence criterion double epsilonFunction = 1e-10; // convergence criterion optimizer->SetScales( scales ); optimizer->SetNumberOfIterations( numberOfIterations ); optimizer->SetValueTolerance( valueTolerance ); optimizer->SetGradientTolerance( gradientTolerance ); optimizer->SetEpsilonFunction( epsilonFunction ); registration->SetInitialTransformParameters( transform->GetParameters() ); //------------------------------------------------------ // Connect all the components required for Registration //------------------------------------------------------ MetricType::Pointer metric = MetricType::New(); registration->SetMetric( metric ); registration->SetOptimizer( optimizer ); registration->SetTransform( transform ); registration->SetFixedPointSet( targetPointSet ); registration->SetMovingPointSet( sourcePointSet ); try { //registration->StartRegistration(); registration->Update(); } catch( itk::ExceptionObject & e ) { MITK_INFO << "Exception caught during ICP optimization: " << e; return false; //throw e; } MITK_INFO << "ICP successful: Solution = " << transform->GetParameters() << std::endl; MITK_INFO << "Metric value: " << metric->GetValue(transform->GetParameters()); /* find point correspondences */ //mitk::PointLocator::Pointer pointLocator = mitk::PointLocator::New(); // <<- use mitk::PointLocator instead of searching manually? //pointLocator->SetPoints() for (LandmarkPointContainer::const_iterator sourcesIt = sources.begin(); sourcesIt != sources.end(); ++sourcesIt) { } //MetricType::MeasureType closestDistances = metric->GetValue(transform->GetParameters()); //unsigned int index = 0; LandmarkPointContainer sortedSources; for (LandmarkPointContainer::const_iterator targetsIt = targets.begin(); targetsIt != targets.end(); ++targetsIt) { double minDistance = itk::NumericTraits<double>::max(); LandmarkPointContainer::iterator minDistanceIterator = sources.end(); for (LandmarkPointContainer::iterator sourcesIt = sources.begin(); sourcesIt != sources.end(); ++sourcesIt) { TransformInitializerType::LandmarkPointType transformedSource = transform->TransformPoint(*sourcesIt); double dist = targetsIt->EuclideanDistanceTo(transformedSource); MITK_INFO << "target: " << *targetsIt << ", source: " << *sourcesIt << ", transformed source: " << transformedSource << ", dist: " << dist; if (dist < minDistance ) { minDistanceIterator = sourcesIt; minDistance = dist; } } if (minDistanceIterator == sources.end()) return false; MITK_INFO << "minimum distance point is: " << *minDistanceIterator << " (dist: " << targetsIt->EuclideanDistanceTo(transform->TransformPoint(*minDistanceIterator)) << ", minDist: " << minDistance << ")"; sortedSources.push_back(*minDistanceIterator); // this point is assigned sources.erase(minDistanceIterator); // erase it from sources to avoid duplicate assigns } //for (LandmarkPointContainer::const_iterator sortedSourcesIt = sortedSources.begin(); targetsIt != sortedSources.end(); ++targetsIt) sources = sortedSources; return true; } void mitk::NavigationDataLandmarkTransformFilter::UpdateLandmarkTransform(const LandmarkPointContainer &sources, const LandmarkPointContainer &targets) { try { /* calculate transform from landmarks */ m_LandmarkTransformInitializer->SetMovingLandmarks(targets); m_LandmarkTransformInitializer->SetFixedLandmarks(sources); // itk registration always maps from fixed object space to moving object space m_LandmarkTransform->SetIdentity(); m_LandmarkTransformInitializer->InitializeTransform(); /* Calculate error statistics for the transform */ TransformInitializerType::LandmarkPointType curData; m_Errors.clear(); for (LandmarkPointContainer::size_type index = 0; index < sources.size(); index++) { curData = m_LandmarkTransform->TransformPoint(sources.at(index)); m_Errors.push_back(curData.EuclideanDistanceTo(targets.at(index))); } this->AccumulateStatistics(m_Errors); this->Modified(); } catch (std::exception& e) { m_Errors.clear(); m_LandmarkTransform->SetIdentity(); itkExceptionMacro("Initializing landmark-transform failed\n. " << e.what()); } } diff --git a/Modules/IGT/IGTFilters/mitkNavigationDataTransformFilter.cpp b/Modules/IGT/IGTFilters/mitkNavigationDataTransformFilter.cpp index ee78c11be7..1208c76c43 100644 --- a/Modules/IGT/IGTFilters/mitkNavigationDataTransformFilter.cpp +++ b/Modules/IGT/IGTFilters/mitkNavigationDataTransformFilter.cpp @@ -1,109 +1,108 @@ /*=================================================================== 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 "mitkNavigationDataTransformFilter.h" mitk::NavigationDataTransformFilter::NavigationDataTransformFilter() : mitk::NavigationDataToNavigationDataFilter() { m_Transform = NULL; //transform to rotate orientation m_QuatOrgRigidTransform = itk::QuaternionRigidTransform<double>::New(); m_QuatTmpTransform = itk::QuaternionRigidTransform<double>::New(); } mitk::NavigationDataTransformFilter::~NavigationDataTransformFilter() { m_Transform = NULL; } void mitk::NavigationDataTransformFilter::SetRigid3DTransform( TransformType::Pointer transform ) { m_Transform = transform; this->Modified(); } void mitk::NavigationDataTransformFilter::GenerateData() { // only update data if m_Transform was set if(m_Transform.IsNull()) { itkExceptionMacro("Invalid parameter: Transform was not set! Use SetRigid3DTransform() before updating the filter."); return; } else { this->CreateOutputsForAllInputs(); // make sure that we have the same number of outputs as inputs /* update outputs with tracking data from tools */ - for (unsigned int i = 0; i < this->GetNumberOfOutputs() ; ++i) + for (unsigned int i = 0; i < this->GetNumberOfIndexedOutputs() ; ++i) { mitk::NavigationData* output = this->GetOutput(i); assert(output); const mitk::NavigationData* input = this->GetInput(i); assert(input); if (input->IsDataValid() == false) { output->SetDataValid(false); continue; } mitk::NavigationData::PositionType tempCoordinateIn, tempCoordinateOut; tempCoordinateIn = input->GetPosition(); itk::Point<float,3> itkPointIn, itkPointOut; itkPointIn[0] = tempCoordinateIn[0]; itkPointIn[1] = tempCoordinateIn[1]; itkPointIn[2] = tempCoordinateIn[2]; //do the transform itkPointOut = m_Transform->TransformPoint( itkPointIn ); tempCoordinateOut[0] = itkPointOut[0]; tempCoordinateOut[1] = itkPointOut[1]; tempCoordinateOut[2] = itkPointOut[2]; output->Graft(input); // First, copy all information from input to output output->SetPosition(tempCoordinateOut);// Then change the member(s): add new position of navigation data after tranformation output->SetDataValid(true); // operation was successful, therefore data of output is valid. //---transform orientation NavigationData::OrientationType quatIn = input->GetOrientation(); vnl_quaternion<double> const vnlQuatIn(quatIn.x(), quatIn.y(), quatIn.z(), quatIn.r()); TransformType::MatrixType rotMatrixD = m_Transform->GetMatrix(); vnl_quaternion<double> vnlQ = vnl_quaternion<double>(rotMatrixD.GetVnlMatrix()); m_QuatOrgRigidTransform->SetRotation(vnlQ); m_QuatTmpTransform->SetRotation(vnlQuatIn); m_QuatTmpTransform->Compose(m_QuatOrgRigidTransform,false); - vnl_quaternion<double> vnlQuatOut = m_QuatTmpTransform->GetRotation(); NavigationData::OrientationType quatOut(vnlQuatOut[0], vnlQuatOut[1], vnlQuatOut[2], vnlQuatOut[3]); output->SetOrientation(quatOut); } } } diff --git a/Modules/IGT/Testing/mitkNavigationDataLandmarkTransformFilterTest.cpp b/Modules/IGT/Testing/mitkNavigationDataLandmarkTransformFilterTest.cpp index e6b03c2fa3..daf411fc96 100644 --- a/Modules/IGT/Testing/mitkNavigationDataLandmarkTransformFilterTest.cpp +++ b/Modules/IGT/Testing/mitkNavigationDataLandmarkTransformFilterTest.cpp @@ -1,488 +1,511 @@ /*=================================================================== 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 "mitkNavigationDataLandmarkTransformFilter.h" #include "mitkNavigationData.h" #include "mitkTestingMacros.h" #include <iostream> class mitkNavigationDataLandmarkTransformFilterTestClass { public: static void TestInstantiation() { // let's create an object of our class mitk::NavigationDataLandmarkTransformFilter::Pointer myFilter = mitk::NavigationDataLandmarkTransformFilter::New(); // first test: did this work? // using MITK_TEST_CONDITION_REQUIRED makes the test stop after failure, since // it makes no sense to continue without an object. MITK_TEST_CONDITION_REQUIRED(myFilter.IsNotNull(),"Testing instantiation"); } static void TestFilter() { // let's create an object of our class mitk::NavigationDataLandmarkTransformFilter::Pointer myFilter = mitk::NavigationDataLandmarkTransformFilter::New(); // first test: did this work? // using MITK_TEST_CONDITION_REQUIRED makes the test stop after failure, since // it makes no sense to continue without an object. MITK_TEST_CONDITION_REQUIRED(myFilter.IsNotNull(),"Testing instantiation"); /*create helper objects: source and target pointSets for landmark transform*/ mitk::Point3D sPoint1, sPoint2, sPoint3, tPoint1, tPoint2, tPoint3; mitk::FillVector3D(sPoint1, 1.1, 1.1, 1.1); mitk::FillVector3D(sPoint2, 2.2, 2.2, 2.2); mitk::FillVector3D(sPoint3, 3.3, 3.3, 3.3); mitk::FillVector3D(tPoint1, 2.1, 2.1, 2.1); mitk::FillVector3D(tPoint2, 3.2, 3.2, 3.2); mitk::FillVector3D(tPoint3, 4.3, 4.3, 4.3); mitk::PointSet::Pointer sourcePoints = mitk::PointSet::New(); mitk::PointSet::Pointer targetPoints = mitk::PointSet::New(); sourcePoints->SetPoint(0,sPoint1); sourcePoints->SetPoint(1,sPoint2); sourcePoints->SetPoint(2,sPoint3); targetPoints->SetPoint(0,tPoint1); targetPoints->SetPoint(1,tPoint2); targetPoints->SetPoint(2,tPoint3); /* create helper objects: navigation data with position as origin, zero quaternion, zero error and data valid */ mitk::NavigationData::PositionType initialPos1,initialPos2, resultPos1,resultPos2; mitk::FillVector3D(initialPos1, 1.1, 1.1, 1.1); mitk::FillVector3D(initialPos2, 22.2,22.2, 22.2); mitk::FillVector3D(resultPos1, 2.1, 2.1, 2.1); mitk::FillVector3D(resultPos2, 23.2, 23.2, 23.2); mitk::NavigationData::OrientationType initialOri(0.1, 0.1, 0.1, 0.1); mitk::ScalarType initialError(0.0); bool initialValid(true); mitk::NavigationData::Pointer nd1 = mitk::NavigationData::New(); nd1->SetPosition(initialPos1); nd1->SetOrientation(initialOri); nd1->SetPositionAccuracy(initialError); nd1->SetDataValid(initialValid); mitk::NavigationData::Pointer nd2 = mitk::NavigationData::New(); nd2->SetPosition(initialPos2); nd2->SetOrientation(initialOri); nd2->SetPositionAccuracy(initialError); nd2->SetDataValid(initialValid); myFilter->SetInput(0,nd1); MITK_TEST_CONDITION(myFilter->GetInput(0) == nd1, "Testing Set-/GetInput() ND1"); mitk::NavigationData* output1 = myFilter->GetOutput(); MITK_TEST_CONDITION_REQUIRED(output1 != NULL, "Testing GetOutput() ND1"); MITK_TEST_CONDITION(myFilter->IsInitialized() == false, "Testing IsInitialized() before setting source points"); myFilter->SetSourceLandmarks(sourcePoints); MITK_TEST_CONDITION(myFilter->IsInitialized() == false, "Testing IsInitialized() after setting source points and before setting target points"); mitk::PointSet::Pointer zeroTargetPoints = mitk::PointSet::New(); MITK_TEST_FOR_EXCEPTION(itk::ExceptionObject, myFilter->SetTargetLandmarks(zeroTargetPoints)); MITK_TEST_CONDITION(myFilter->IsInitialized() == false, "Testing IsInitialized() after setting target pointset with insufficient points"); myFilter->SetTargetLandmarks(targetPoints); MITK_TEST_CONDITION(myFilter->IsInitialized() == true, "Testing IsInitialized() after setting source& target points"); //------------------------landmark transform should be initialized at this point------------------------ output1->Update(); MITK_TEST_CONDITION_REQUIRED( mitk::Equal(output1->GetPosition(), resultPos1), "Testing ND1 position correctly transformed"); //------------------------add another ND------------------------ myFilter->SetInput(1,nd2); MITK_TEST_CONDITION(myFilter->GetInput(1) == nd2, "Testing Set-/GetInput() ND2"); mitk::NavigationData* output2 = myFilter->GetOutput(1); MITK_TEST_CONDITION_REQUIRED(output2 != NULL, "Testing GetOutput() ND2"); //------------------------update output1 but check result2------------------------ output1->Update(); MITK_TEST_CONDITION_REQUIRED( mitk::Equal(output2->GetPosition(), resultPos2), "Testing ND2 position correctly transformed"); //------------------------update ND on slot 1------------------------ mitk::FillVector3D(initialPos2, 222.22, 222.22, 222.22); mitk::FillVector3D(resultPos2, 223.22, 223.22, 223.22); nd2->SetPosition(initialPos2); myFilter->SetInput(1,nd2); MITK_TEST_CONDITION(myFilter->GetInput(1) == nd2, "Testing Set-/GetInput() ND2 after updating value"); output2 = myFilter->GetOutput(1); MITK_TEST_CONDITION_REQUIRED(output2 != NULL, "Testing GetOutput() ND2 after updating value"); //------------------------update output2 and check result2------------------------ output2->Update(); MITK_TEST_CONDITION( mitk::Equal(output2->GetPosition(), resultPos2), "Testing ND2 position correctly transformed after updating value"); //------------------------change target PointSet------------------------ mitk::FillVector3D(tPoint1, 3.1, 3.1, 3.1); mitk::FillVector3D(tPoint2, 4.2, 4.2, 4.2); mitk::FillVector3D(tPoint3, 5.3, 5.3, 5.3); mitk::FillVector3D(resultPos1, 3.1 ,3.1 ,3.1); mitk::FillVector3D(resultPos2, 224.22, 224.22, 224.22); targetPoints->SetPoint(0,tPoint1); targetPoints->SetPoint(1,tPoint2); targetPoints->SetPoint(2,tPoint3); myFilter->SetTargetLandmarks(targetPoints); output1->Update(); MITK_TEST_CONDITION( mitk::Equal(output1->GetPosition(), resultPos1), "Testing ND1 position correctly transformed after targetPointSet changed"); MITK_TEST_CONDITION( mitk::Equal(output2->GetPosition(), resultPos2), "Testing ND2 position correctly transformed after targetPointSet changed"); //------------------------change source PointSet------------------------ mitk::FillVector3D(sPoint1, 0.1, 0.1, 0.1); mitk::FillVector3D(sPoint2, 1.2, 1.2, 1.2); mitk::FillVector3D(sPoint3, 2.3, 2.3, 2.3); mitk::FillVector3D(resultPos1, 4.1 ,4.1 ,4.1); mitk::FillVector3D(resultPos2, 225.22, 225.22, 225.22); sourcePoints->SetPoint(0,sPoint1); sourcePoints->SetPoint(1,sPoint2); sourcePoints->SetPoint(2,sPoint3); myFilter->SetSourceLandmarks(sourcePoints); output1->Update(); MITK_TEST_CONDITION( mitk::Equal(output1->GetPosition(), resultPos1), "Testing ND1 position correctly transformed after sourcePointSet changed"); MITK_TEST_CONDITION( mitk::Equal(output2->GetPosition(), resultPos2), "Testing ND2 position correctly transformed after sourcePointSet changed"); //--------------------- Test ICP initialization -------------------------- { mitk::PointSet::Pointer sourcePoints = mitk::PointSet::New(); mitk::Point3D s1, s2, s3, s4, s5, s6; mitk::FillVector3D(s1, 1.1, 1.1, 1.1); mitk::FillVector3D(s2, 2.2, 2.2, 2.2); mitk::FillVector3D(s3, 3.3, 3.3, 3.3); mitk::FillVector3D(s4, 4.4, 4.4, 4.4); mitk::FillVector3D(s5, 5.5, 5.5, 5.5); mitk::FillVector3D(s6, 6.6, 6.6, 6.6); sourcePoints->SetPoint(1, s4); // use random source point order sourcePoints->SetPoint(2, s6); sourcePoints->SetPoint(3, s3); sourcePoints->SetPoint(4, s1); sourcePoints->SetPoint(5, s2); sourcePoints->SetPoint(6, s5); mitk::PointSet::Pointer targetPoints = mitk::PointSet::New(); mitk::Point3D t1, t2, t3, t4, t5, t6; mitk::FillVector3D(t1, 2.1, 2.1, 102.1); // ==> targets have offset [1, 1, 101] mitk::FillVector3D(t2, 3.2, 3.2, 103.2); mitk::FillVector3D(t3, 4.3, 4.3, 104.3); mitk::FillVector3D(t4, 5.4, 5.4, 105.4); mitk::FillVector3D(t5, 6.5, 6.5, 106.5); mitk::FillVector3D(t6, 7.6, 7.6, 107.6); targetPoints->SetPoint(1, t1); targetPoints->SetPoint(2, t2); targetPoints->SetPoint(3, t3); targetPoints->SetPoint(4, t4); targetPoints->SetPoint(5, t5); targetPoints->SetPoint(6, t6); mitk::NavigationDataLandmarkTransformFilter::Pointer myFilter = mitk::NavigationDataLandmarkTransformFilter::New(); myFilter->UseICPInitializationOn(); myFilter->SetSourceLandmarks(sourcePoints); myFilter->SetTargetLandmarks(targetPoints); // errors would raise exceptions // prepare input mitk::NavigationData::PositionType initialPos1, resultPos1; mitk::FillVector3D(initialPos1, 1.1, 1.1, 1.1); mitk::FillVector3D(resultPos1, 2.1, 2.1, 102.1); mitk::NavigationData::OrientationType initialOri(0.1, 0.1, 0.1, 0.1); mitk::ScalarType initialError(0.0); bool initialValid(true); mitk::NavigationData::Pointer nd1 = mitk::NavigationData::New(); nd1->SetPosition(initialPos1); nd1->SetOrientation(initialOri); nd1->SetPositionAccuracy(initialError); nd1->SetDataValid(initialValid); myFilter->SetInput(0, nd1); mitk::NavigationData::Pointer output = myFilter->GetOutput(); output->Update(); //MITK_TEST_CONDITION(mitk::Equal(output->GetPosition(), resultPos1), "Testing ND1 position correctly transformed after ICP initialization"); } //------------------------catch exception --> source points < 3------------------------ mitk::NavigationDataLandmarkTransformFilter::Pointer myFilter2 = mitk::NavigationDataLandmarkTransformFilter::New(); MITK_TEST_CONDITION_REQUIRED(myFilter2.IsNotNull(),"Testing instantiation for second filter"); mitk::PointSet::Pointer sourcePoints2 = mitk::PointSet::New(); MITK_TEST_FOR_EXCEPTION(std::exception, myFilter2->SetSourceLandmarks(sourcePoints2);); //------------------------catch exception --> target points < 3------------------------ mitk::NavigationDataLandmarkTransformFilter::Pointer myFilter3 = mitk::NavigationDataLandmarkTransformFilter::New(); MITK_TEST_CONDITION_REQUIRED(myFilter3.IsNotNull(),"Testing instantiation for second filter"); mitk::PointSet::Pointer targetPoints2 = mitk::PointSet::New(); MITK_TEST_FOR_EXCEPTION(std::exception, myFilter3->SetTargetLandmarks(targetPoints2);); //------------------------rotate orientation------------------------ myFilter=NULL; myFilter = mitk::NavigationDataLandmarkTransformFilter::New(); mitk::FillVector3D(sPoint1, 1.1, 1.1, 1.1); mitk::FillVector3D(sPoint2, 1.1, -1.1, 1.1); mitk::FillVector3D(sPoint3, -1.1, -1.1, 1.1); mitk::FillVector3D(tPoint1, -1.1, 1.1, 1.1); mitk::FillVector3D(tPoint2, 1.1, 1.1, 1.1); mitk::FillVector3D(tPoint3, 1.1, -1.1, 1.1); sourcePoints->SetPoint(0,sPoint1); sourcePoints->SetPoint(1,sPoint2); sourcePoints->SetPoint(2,sPoint3); targetPoints->SetPoint(0,tPoint1); targetPoints->SetPoint(1,tPoint2); targetPoints->SetPoint(2,tPoint3); myFilter->SetSourceLandmarks(sourcePoints); myFilter->SetTargetLandmarks(targetPoints); //set initial orientation (x y z r) mitk::NavigationData::OrientationType initialQuat(0.0, 0.0, 0.0, 1.0); mitk::NavigationData::OrientationType resultQuat(0.0, 0.0, -0.7071, -0.7071); //set position mitk::FillVector3D(initialPos1, 2.2, 2.2, 2.2); mitk::FillVector3D(resultPos1, -2.2, 2.2, 2.2); nd1->SetOrientation(initialQuat); nd1->SetPosition(initialPos1); myFilter->SetInput(0,nd1); output1 = myFilter->GetOutput(); output1->Update(); MITK_TEST_CONDITION( mitk::Equal(output1->GetPosition(), resultPos1), "Testing ND1 position correctly transformed "); - /* COMMENTED OUT BECAUSE OF BUG 15021 MITK_TEST_CONDITION( mitk::Equal(output1->GetOrientation(), resultQuat), "Testing ND1 orientation correctly transformed "); - */ + MITK_TEST_OUTPUT(<<"Orientation1"); + MITK_TEST_OUTPUT(<<output1->GetOrientation()); + MITK_TEST_OUTPUT(<<"qX:"); + MITK_TEST_OUTPUT(<<output1->GetOrientation().x()); + MITK_TEST_OUTPUT(<<"qY:"); + MITK_TEST_OUTPUT(<<output1->GetOrientation().y()); + MITK_TEST_OUTPUT(<<"qZ:"); + MITK_TEST_OUTPUT(<<output1->GetOrientation().z()); + MITK_TEST_OUTPUT(<<"qR:"); + MITK_TEST_OUTPUT(<<output1->GetOrientation().r()); + MITK_TEST_OUTPUT(<<"angle:"); + //MITK_TEST_OUTPUT(<<output1->angle()); + //TODO: something was modified on vnl_quaternion, check what. DONE + MITK_TEST_OUTPUT(<<"Orientation2"); + MITK_TEST_OUTPUT(<<resultQuat); + MITK_TEST_OUTPUT(<<"qX:"); + MITK_TEST_OUTPUT(<<resultQuat.x()); + MITK_TEST_OUTPUT(<<"qY:"); + MITK_TEST_OUTPUT(<<resultQuat.y()); + MITK_TEST_OUTPUT(<<"qZ:"); + MITK_TEST_OUTPUT(<<resultQuat.z()); + MITK_TEST_OUTPUT(<<"qR:"); + MITK_TEST_OUTPUT(<<resultQuat.r()); + + //------------------------test FRE calculation------------------------ mitk::PointSet::Pointer refSet = mitk::PointSet::New(); mitk::PointSet::Pointer movSet = mitk::PointSet::New(); mitk::Point3D refPoint; mitk::Point3D movPoint; //Point 0 refPoint.Fill(0); refSet->SetPoint(0, refPoint); movPoint.Fill(1); movSet->SetPoint(0, movPoint); //Point 1 refPoint[0]=3; refPoint[1]=0; refPoint[2]=0; refSet->SetPoint(1, refPoint); movPoint[0]=2; movPoint[1]=1; movPoint[2]=1; movSet->SetPoint(1, movPoint); //Point 2 refPoint[0]=0; refPoint[1]=0; refPoint[2]=3; refSet->SetPoint(2, refPoint); movPoint[0]=1; movPoint[1]=1; movPoint[2]=2; movSet->SetPoint(2, movPoint); //Point 3 refPoint[0]=3; refPoint[1]=0; refPoint[2]=3; refSet->SetPoint(3, refPoint); movPoint[0]=2; movPoint[1]=1; movPoint[2]=2; movSet->SetPoint(3, movPoint); //Point 4 refPoint[0]=0; refPoint[1]=3; refPoint[2]=0; refSet->SetPoint(4, refPoint); movPoint[0]=1; movPoint[1]=2; movPoint[2]=1; movSet->SetPoint(4, movPoint); //Point 5 refPoint[0]=3; refPoint[1]=3; refPoint[2]=0; refSet->SetPoint(5, refPoint); movPoint[0]=2; movPoint[1]=2; movPoint[2]=1; movSet->SetPoint(5, movPoint); //Point 6 refPoint[0]=0; refPoint[1]=3; refPoint[2]=3; refSet->SetPoint(6, refPoint); movPoint[0]=1; movPoint[1]=2; movPoint[2]=2; movSet->SetPoint(6, movPoint); //Point 7 refPoint[0]=3; refPoint[1]=3; refPoint[2]=3; refSet->SetPoint(7, refPoint); movPoint[0]=2; movPoint[1]=2; movPoint[2]=2; movSet->SetPoint(7, movPoint); mitk::NavigationDataLandmarkTransformFilter::Pointer myFREFilter = mitk::NavigationDataLandmarkTransformFilter::New(); myFREFilter->SetSourceLandmarks(refSet); myFREFilter->SetTargetLandmarks(movSet); //very simple test case, everything is the same (min = max = mean = RMS = abs max error) //but still ok to see if the methods work without a crash MITK_TEST_CONDITION_REQUIRED(myFREFilter->GetFRE() == (float) sqrt(3.0),"Testing mean error calculation"); MITK_TEST_CONDITION_REQUIRED(myFREFilter->GetMaxError() == (float) sqrt(3.0),"Testing max error calculation"); MITK_TEST_CONDITION_REQUIRED(myFREFilter->GetMinError() == (float) sqrt(3.0),"Testing min error calculation"); MITK_TEST_CONDITION_REQUIRED(myFREFilter->GetRMSError() == (float) sqrt(3.0),"Testing RMS error calculation"); MITK_TEST_CONDITION_REQUIRED(myFREFilter->GetFREStdDev() == (float) 0.0,"Testing SD calculation"); MITK_TEST_CONDITION_REQUIRED(myFREFilter->GetAbsMaxError() == (float) sqrt(3.0),"Testing abs max error calculation"); MITK_TEST_CONDITION_REQUIRED(myFREFilter->GetErrorVector().size() == 8,"Testing method GetErrorVector"); //todo: extend by a more complex test case with different values? } static void TestPrintSelfMethod() { mitk::PointSet::Pointer refSet = mitk::PointSet::New(); mitk::PointSet::Pointer movSet = mitk::PointSet::New(); mitk::Point3D refPoint; mitk::Point3D movPoint; //Point 0 refPoint.Fill(0); refSet->SetPoint(0, refPoint); movPoint.Fill(1); movSet->SetPoint(0, movPoint); //Point 1 refPoint[0]=3; refPoint[1]=0; refPoint[2]=0; refSet->SetPoint(1, refPoint); movPoint[0]=2; movPoint[1]=1; movPoint[2]=1; movSet->SetPoint(1, movPoint); //Point 2 refPoint[0]=0; refPoint[1]=0; refPoint[2]=3; refSet->SetPoint(2, refPoint); movPoint[0]=1; movPoint[1]=1; movPoint[2]=2; movSet->SetPoint(2, movPoint); //Point 3 refPoint[0]=3; refPoint[1]=0; refPoint[2]=3; refSet->SetPoint(3, refPoint); movPoint[0]=2; movPoint[1]=1; movPoint[2]=2; movSet->SetPoint(3, movPoint); //Point 4 refPoint[0]=0; refPoint[1]=3; refPoint[2]=0; refSet->SetPoint(4, refPoint); movPoint[0]=1; movPoint[1]=2; movPoint[2]=1; movSet->SetPoint(4, movPoint); //Point 5 refPoint[0]=3; refPoint[1]=3; refPoint[2]=0; refSet->SetPoint(5, refPoint); movPoint[0]=2; movPoint[1]=2; movPoint[2]=1; movSet->SetPoint(5, movPoint); //Point 6 refPoint[0]=0; refPoint[1]=3; refPoint[2]=3; refSet->SetPoint(6, refPoint); movPoint[0]=1; movPoint[1]=2; movPoint[2]=2; movSet->SetPoint(6, movPoint); //Point 7 refPoint[0]=3; refPoint[1]=3; refPoint[2]=3; refSet->SetPoint(7, refPoint); movPoint[0]=2; movPoint[1]=2; movPoint[2]=2; movSet->SetPoint(7, movPoint); mitk::NavigationDataLandmarkTransformFilter::Pointer myFREFilter = mitk::NavigationDataLandmarkTransformFilter::New(); myFREFilter->SetSourceLandmarks(refSet); myFREFilter->SetTargetLandmarks(movSet); bool success = false; try { MITK_INFO << "Testing printing of object: " << myFREFilter; success = true; } catch(...) { MITK_ERROR << "Error while printing the object"; } MITK_TEST_CONDITION_REQUIRED(success,"Testing printing object to a stream"); } static void TestFilterInvalidCases() { mitk::NavigationDataLandmarkTransformFilter::Pointer myFilter = mitk::NavigationDataLandmarkTransformFilter::New(); mitk::PointSet::Pointer refSet = mitk::PointSet::New(); mitk::PointSet::Pointer movSet = mitk::PointSet::New(); mitk::Point3D refPoint; mitk::Point3D movPoint; //Point 0 refPoint.Fill(0); refSet->SetPoint(0, refPoint); movPoint.Fill(1); movSet->SetPoint(0, movPoint); //Point 1 refPoint[0]=3; refPoint[1]=0; refPoint[2]=0; refSet->SetPoint(1, refPoint); movPoint[0]=2; movPoint[1]=1; movPoint[2]=1; movSet->SetPoint(1, movPoint); //Point 2 refPoint[0]=0; refPoint[1]=0; refPoint[2]=3; refSet->SetPoint(2, refPoint); movPoint[0]=1; movPoint[1]=1; movPoint[2]=2; movSet->SetPoint(2, movPoint); myFilter->SetUseICPInitialization(true); bool exceptionThrown = false; try { //should throw exception because less than 6 points myFilter->SetSourceLandmarks(refSet); myFilter->SetTargetLandmarks(movSet); } catch(itk::ExceptionObject) { exceptionThrown = true; } MITK_TEST_CONDITION_REQUIRED(exceptionThrown,"Testing invalid number of landmarks when using ICP initialization.") } }; /**Documentation * test for the class "NavigationDataLandmarkTransformFilter". */ int mitkNavigationDataLandmarkTransformFilterTest(int /* argc */, char* /*argv*/[]) { MITK_TEST_BEGIN("NavigationDataLandmarkTransformFilter") mitkNavigationDataLandmarkTransformFilterTestClass::TestInstantiation(); mitkNavigationDataLandmarkTransformFilterTestClass::TestFilter(); mitkNavigationDataLandmarkTransformFilterTestClass::TestPrintSelfMethod(); mitkNavigationDataLandmarkTransformFilterTestClass::TestFilterInvalidCases(); // always end with this! MITK_TEST_END(); }