diff --git a/Modules/SurfaceRegistration/Testing/mitkAnisotropicIterativeClosestPointRegistrationTest.cpp b/Modules/SurfaceRegistration/Testing/mitkAnisotropicIterativeClosestPointRegistrationTest.cpp index 3658463877..969d4ff6e7 100644 --- a/Modules/SurfaceRegistration/Testing/mitkAnisotropicIterativeClosestPointRegistrationTest.cpp +++ b/Modules/SurfaceRegistration/Testing/mitkAnisotropicIterativeClosestPointRegistrationTest.cpp @@ -1,112 +1,123 @@ /*=================================================================== 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 #include #include #include #include "mitkAnisotropicIterativeClosestPointRegistration.h" #include "mitkCovarianceMatrixCalculator.h" #include "mitkAnisotropicRegistrationCommon.h" /** * Test to verify the results of the A-ICP registration. * The test can be used for both, the standard AICP and the trimmed variant. * The test runs the convergence experiments on the public * datasets used in L. Maier-Hein et al." Convergent Iterative Closest-Point * Algorithm to Accomodate Anisotropic and Inhomogenous Localization Error.", * IEEE T Pattern Anal 34 (8), 1520-1532, 2012. to ensure correct results. */ int mitkAnisotropicIterativeClosestPointRegistrationTest( int argc, char* args[]) { MITK_TEST_BEGIN("mitkAnisotropicIterativeClosestPointRegistrationTest"); + typedef itk::Matrix < double, 3, 3 > Matrix3x3; + typedef itk::Vector < double, 3 > Vector3; + typedef std::vector < Matrix3x3 > CovarianceMatrixList; + MITK_TEST_CONDITION_REQUIRED( argc >= 8, "Testing if all arguments are set."); //load input surfaces std::string fixedSurfaceFile = args[1]; std::string movingSurfaceFile = args[2]; std::string fixedTargetsFile = args[3]; std::string movingTargetsFile = args[4]; double trimmedPart = atof(args[5]); double freLiterature = atof(args[6]); double treLiterature = atof(args[7]); mitk::PointSet::Pointer movingTargets = mitk::IOUtil::LoadPointSet(movingTargetsFile); mitk::PointSet::Pointer fixedTargets = mitk::IOUtil::LoadPointSet(fixedTargetsFile); mitk::Surface::Pointer fixedSurface = mitk::IOUtil::LoadSurface(fixedSurfaceFile); mitk::Surface::Pointer movingSurface = mitk::IOUtil::LoadSurface(movingSurfaceFile); // compute cov matrices for the moving (.e.g. ToF Surface) - mitk::CovarianceMatrixCalculator::Pointer matrixCalculator = mitk::CovarianceMatrixCalculator::New(); + mitk::CovarianceMatrixCalculator::Pointer matrixCalculator = + mitk::CovarianceMatrixCalculator::New(); + + // compute the covariance matrices for the moving surface (X) matrixCalculator->SetInputSurface(movingSurface); matrixCalculator->ComputeCovarianceMatrices(); - std::vector< itk::Matrix > covarianceMatricesMoving = matrixCalculator->GetCovarianceMatrices(); - double FRENormalizationsigmaMoving = matrixCalculator->GetMeanVariance(); - MITK_INFO << "1: " << covarianceMatricesMoving.size() << " , 2: " << movingSurface->GetVtkPolyData()->GetNumberOfPoints(); - MITK_TEST_CONDITION_REQUIRED( (int)covarianceMatricesMoving.size() == movingSurface->GetVtkPolyData()->GetNumberOfPoints(), "Testing if a covariance matrix was generated for every point." ); + CovarianceMatrixList sigmas_X = matrixCalculator->GetCovarianceMatrices(); + double meanVarX = matrixCalculator->GetMeanVariance(); - // compute the cov matrices for the fixed surface ( ct surface) + MITK_TEST_CONDITION_REQUIRED( (int)sigmas_X.size() == movingSurface->GetVtkPolyData()->GetNumberOfPoints(), "Testing if a covariance matrix was generated for every point." ); + + // compute the covariance matrices for the fixed surface (Y) matrixCalculator->SetInputSurface(fixedSurface); matrixCalculator->ComputeCovarianceMatrices(); - std::vector< itk::Matrix > covarianceMatricesFixed= matrixCalculator->GetCovarianceMatrices(); - double FRENormalizationsigmaFixed = matrixCalculator->GetMeanVariance(); - MITK_INFO << "1: " << covarianceMatricesFixed.size() << " , 2: " << fixedSurface->GetVtkPolyData()->GetNumberOfPoints(); - MITK_TEST_CONDITION_REQUIRED( (int)covarianceMatricesFixed.size() == fixedSurface->GetVtkPolyData()->GetNumberOfPoints(), "Testing if a covariance matrix was generated for every point." ); - double FRENormalizationFactor = sqrt(FRENormalizationsigmaMoving+FRENormalizationsigmaFixed); + CovarianceMatrixList sigmas_Y = matrixCalculator->GetCovarianceMatrices(); + double meanVarY = matrixCalculator->GetMeanVariance(); + + MITK_TEST_CONDITION_REQUIRED( (int)sigmas_Y.size() == fixedSurface->GetVtkPolyData()->GetNumberOfPoints(), "Testing if a covariance matrix was generated for every point." ); + // the FRE normalization factor + double normalizationFactor = sqrt( meanVarX + meanVarY); // run the algorithm mitk::AnisotropicIterativeClosestPointRegistration::Pointer aICP = mitk::AnisotropicIterativeClosestPointRegistration::New(); + // set up parameters aICP->SetMovingSurface(movingSurface); aICP->SetFixedSurface(fixedSurface); - aICP->SetCovarianceMatricesMovingSurface(covarianceMatricesMoving); - aICP->SetCovarianceMatricesFixedSurface(covarianceMatricesFixed); - aICP->SetFRENormalizationFactor(FRENormalizationFactor); - aICP->SetThreshold(0.0000000010); + aICP->SetCovarianceMatricesMovingSurface(sigmas_X); + aICP->SetCovarianceMatricesFixedSurface(sigmas_Y); + aICP->SetFRENormalizationFactor(normalizationFactor); + // enable trimming if(trimmedPart != 0.0) { aICP->SetTrimmFactor(1.0 - trimmedPart); MITK_INFO << "Algorithm is using trimmed variant."; } - MITK_INFO << "Before do register"; + // run the algorithm aICP->Update(); - MITK_INFO<<"After do register"; MITK_INFO << "FRE:" << aICP->GetFRE() << ", ref: " << freLiterature; // //value 3.15857 obtained from: \e130-projekte\NeedleNavigation\AnisotropicICP\EvaluationTPAMI\Experimente_Maerz2011/Ergebnisse.xlsx MITK_TEST_CONDITION( mitk::Equal(aICP->GetFRE(), freLiterature,0.001), "Testing if FRE equals results from literature."); + Matrix3x3 rotation = aICP->GetRotation(); + Vector3 translation = aICP->GetTranslation(); + double tre = -1; tre = mitk::AnisotropicRegistrationCommon::ComputeTargetRegistrationError( movingTargets.GetPointer(), fixedTargets.GetPointer(), - aICP->GetRotation(), - aICP->GetTranslation() + rotation, + translation ); - // tre = mitk::AnisotropicRegistrationTestUtil::ComputeTRE( movingTargets, fixedTargets, aICP->GetRotation(), aICP->GetTranslation()); + MITK_INFO << "tre: " << tre << ", ref :" << treLiterature; // // //value 0.0784612 obtained from: \e130-projekte\NeedleNavigation\AnisotropicICP\EvaluationTPAMI\Experimente_Maerz2011/Ergebnisse.xlsx MITK_TEST_CONDITION( mitk::Equal(tre, treLiterature), "Testing if TRE equals the results from literature."); MITK_TEST_END(); } diff --git a/Modules/SurfaceRegistration/mitkAnisotropicIterativeClosestPointRegistration.cpp b/Modules/SurfaceRegistration/mitkAnisotropicIterativeClosestPointRegistration.cpp index 8be4877f8d..5da780f793 100644 --- a/Modules/SurfaceRegistration/mitkAnisotropicIterativeClosestPointRegistration.cpp +++ b/Modules/SurfaceRegistration/mitkAnisotropicIterativeClosestPointRegistration.cpp @@ -1,301 +1,302 @@ /*=================================================================== 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. ===================================================================*/ // MITK #include "mitkAnisotropicIterativeClosestPointRegistration.h" #include "mitkWeightedPointTransform.h" #include "mitkAnisotropicRegistrationCommon.h" #include // VTK #include #include #include #include #include // STL pair #include -// comperator to sort correspondences -struct myComperator +/** Comperator implementation used to sort the CorrespondenceList. + */ +struct AICPComperator { typedef std::pair < unsigned int, double > Correspondence; bool operator() (const Correspondence& a, const Correspondence& b) { return (a.second < b.second); } -} myComp; +} AICPComp; mitk::AnisotropicIterativeClosestPointRegistration::AnisotropicIterativeClosestPointRegistration() : m_MaxIterations(1000), m_Threshold(0.000001), m_FRENormalizationFactor(0.0), m_SearchRadius(30.0), m_MaxIterationsInWeightedPointTransform(1000), m_FRE(0.0), m_TrimmFactor(0.0), m_NumberOfIterations(0), m_MovingSurface(NULL), m_FixedSurface(NULL), m_WeightedPointTransform(mitk::WeightedPointTransform::New()) { } mitk::AnisotropicIterativeClosestPointRegistration::~AnisotropicIterativeClosestPointRegistration() { } void mitk::AnisotropicIterativeClosestPointRegistration::ComputeCorrespondences ( vtkPoints* X, vtkPoints* Z, vtkKdTreePointLocator* Y, const CovarianceMatrixList& sigma_X, const CovarianceMatrixList& sigma_Y, CovarianceMatrixList& sigma_Z, CorrespondenceList& correspondences, const double radius ) { typedef itk::Matrix < double, 3, 3 > WeightMatrix; # pragma omp parallel for for ( vtkIdType i = 0; i < X->GetNumberOfPoints(); ++i ) { vtkIdType bestIdx = 0; mitk::Vector3D x; mitk::Vector3D y; double bestDist = std::numeric_limits::max(); vtkIdList* ids = vtkIdList::New(); double r = radius; double p[3]; // get point X->GetPoint(i,p); // fill vector x[0] = p[0]; x[1] = p[1]; x[2] = p[2]; // double the radius till we find at least one point while( ids->GetNumberOfIds() <= 0 ) { Y->FindPointsWithinRadius(r,p,ids); r *= 2.0; } // loop over the points in the sphere and find the point with the // minimal weighted squared distance for ( vtkIdType j = 0; j < ids->GetNumberOfIds(); ++j ) { // get id const vtkIdType id = ids->GetId(j); // compute weightmatrix WeightMatrix m = mitk::AnisotropicRegistrationCommon::CalculateWeightMatrix( sigma_X[i], sigma_Y[id] ); // point of the fixed data set Y->GetDataSet()->GetPoint(id,p); // fill mitk vector y[0] = p[0]; y[1] = p[1]; y[2] = p[2]; const mitk::Vector3D res = m * ( x - y ); const double dist = res[0] * res[0] + res[1] * res[1] + res[2] * res[2]; if ( dist < bestDist ) { bestDist = dist; bestIdx = id; } } // save correspondences of the fixed point set Y->GetDataSet()->GetPoint(bestIdx,p); Z->SetPoint(i,p); sigma_Z[i] = sigma_Y[bestIdx]; Correspondence _pair(i,bestDist); correspondences[i] = _pair; ids->Delete(); } } void mitk::AnisotropicIterativeClosestPointRegistration::Update() { unsigned int k = 0; unsigned int numberOfTrimmedPoints = 0; double diff = 0.0; double FRE_new = std::numeric_limits::max(); // Moving pointset vtkPoints* X = vtkPoints::New(); // Correspondences vtkPoints* Z = vtkPoints::New(); // Covariance matrices of the pointset X CovarianceMatrixList& Sigma_X = m_CovarianceMatricesMovingSurface; // Covariance matrices of the pointset Y CovarianceMatrixList& Sigma_Y = m_CovarianceMatricesFixedSurface; // Covariance matrices of the correspondences CovarianceMatrixList Sigma_Z; // transform of the current iteration Rotation RotationNew; Translation TranslationNew; // corresponding indizes with distance CorrespondenceList distanceList; // sorted datasets used if trimming is enabled vtkPoints* X_sorted = vtkPoints::New(); vtkPoints* Z_sorted = vtkPoints::New(); CovarianceMatrixList Sigma_X_sorted; CovarianceMatrixList Sigma_Z_sorted; // create kdtree for correspondence search vtkKdTreePointLocator* Y = vtkKdTreePointLocator::New(); Y->SetDataSet(m_FixedSurface->GetVtkPolyData()); Y->BuildLocator(); // initialize local variables // copy the moving pointset to prevent to modify it X->DeepCopy(m_MovingSurface->GetVtkPolyData()->GetPoints()); // initialize size of the correspondences Z->SetNumberOfPoints(X->GetNumberOfPoints()); // size of the corresponding matrices Sigma_Z.resize(X->GetNumberOfPoints()); distanceList.resize(X->GetNumberOfPoints()); RotationNew.SetIdentity(); TranslationNew.Fill(0.0); // reset members m_FRE = std::numeric_limits::max(); m_Rotation.SetIdentity(); m_Translation.Fill(0.0); // compute number of correspondences based // on the trimmfactor if ( m_TrimmFactor > 0.0) { numberOfTrimmedPoints = X->GetNumberOfPoints() * m_TrimmFactor; } // initialize the sizes of the sorted datasets // used in the trimmed version of the algorithm Sigma_Z_sorted.resize(numberOfTrimmedPoints); Sigma_X_sorted.resize(numberOfTrimmedPoints); X_sorted->SetNumberOfPoints(numberOfTrimmedPoints); Z_sorted->SetNumberOfPoints(numberOfTrimmedPoints); do { // reset innerloop double currSearchRadius = m_SearchRadius; unsigned int radiusDoubled = 0; k = k + 1; MITK_INFO << "iteration: " << k; do { // search correspondences ComputeCorrespondences( X, Z, Y, Sigma_X, Sigma_Y, Sigma_Z, distanceList, currSearchRadius ); // tmp pointers vtkPoints* X_k = X; vtkPoints* Z_k = Z; CovarianceMatrixList* Sigma_Z_k = &Sigma_Z; CovarianceMatrixList* Sigma_X_k = &Sigma_X; // sort the correspondences depending on their // distance, if trimming is enabled if ( m_TrimmFactor > 0.0 ) { - std::sort ( distanceList.begin(), distanceList.end(), myComp ); + std::sort ( distanceList.begin(), distanceList.end(), AICPComp ); // map correspondences to the data arrays for ( unsigned int i = 0; i < numberOfTrimmedPoints; ++i ) { const int idx = distanceList[i].first; Sigma_Z_sorted[i] = Sigma_Z[idx]; Sigma_X_sorted[i] = Sigma_X[idx]; Z_sorted->SetPoint(i,Z->GetPoint(idx)); X_sorted->SetPoint(i,X->GetPoint(idx)); } // assign pointers X_k = X_sorted; Z_k = Z_sorted; Sigma_X_k = &Sigma_X_sorted; Sigma_Z_k = &Sigma_Z_sorted; } // compute weighted transformation // set parameters m_WeightedPointTransform->SetMovingPointSet(X_k); m_WeightedPointTransform->SetFixedPointSet(Z_k); m_WeightedPointTransform->SetCovarianceMatricesMoving(*Sigma_X_k); m_WeightedPointTransform->SetCovarianceMatricesFixed(*Sigma_Z_k); m_WeightedPointTransform->SetMaxIterations(m_MaxIterationsInWeightedPointTransform); m_WeightedPointTransform->SetFRENormalizationFactor(m_FRENormalizationFactor); // run computation m_WeightedPointTransform->ComputeTransformation(); // retrieve result RotationNew = m_WeightedPointTransform->GetTransformR(); TranslationNew = m_WeightedPointTransform->GetTransformT(); FRE_new = m_WeightedPointTransform->GetFRE(); // double the radius radiusDoubled += 1; currSearchRadius *= 2.0; // sanity check to prevent endless loop if ( radiusDoubled >= 20 ) { mitkThrow() << "Radius doubled 20 times, preventing endless loop, check input and search radius"; } // termination constraint diff = m_FRE - FRE_new; } while ( diff < -1.0e-3 ); // increase radius as long as the FRE grows MITK_INFO << "FRE:" << m_FRE << ", FRE_new: "<< FRE_new; // transform points and propagate matrices mitk::AnisotropicRegistrationCommon::TransformPoints(X,X,RotationNew,TranslationNew); mitk::AnisotropicRegistrationCommon::PropagateMatrices(Sigma_X,Sigma_X,RotationNew); // update global transformation m_Rotation = RotationNew * m_Rotation; m_Translation = RotationNew * m_Translation + TranslationNew; MITK_INFO << "diff:" << diff; // update FRE m_FRE = FRE_new; } while ( diff >= m_Threshold && k < m_MaxIterations ); m_NumberOfIterations = k; // free memory Y->Delete(); Z->Delete(); X->Delete(); X_sorted->Delete(); Z_sorted->Delete(); } diff --git a/Modules/SurfaceRegistration/mitkAnisotropicIterativeClosestPointRegistration.h b/Modules/SurfaceRegistration/mitkAnisotropicIterativeClosestPointRegistration.h index fe629fb49d..d96b7b5605 100644 --- a/Modules/SurfaceRegistration/mitkAnisotropicIterativeClosestPointRegistration.h +++ b/Modules/SurfaceRegistration/mitkAnisotropicIterativeClosestPointRegistration.h @@ -1,309 +1,318 @@ /*=================================================================== 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 __ANISOTROPICITERATIVECLOSESTPOINTREGISTRATION_H__ #define __ANISOTROPICITERATIVECLOSESTPOINTREGISTRATION_H__ //MITK #include #include //EXPORTS #include "MitkSurfaceRegistrationExports.h" // STL #include // ITK #include // forward declarations class vtkPoints; class vtkKdTreePointLocator; namespace mitk { class Surface; class WeightedPointTransform; /** * \ingroup AnisotropicRegistration * * @brief Implementation of the anisotropic iterative closest point (A-ICP) * algoritm. * * This class implements the anisotropic interative closest point (A-ICP) * algorithm presented in L. Maier-Hein et al. in "Convergent Iterative * Closest-Point Algorithm to Accomodate Anisotropic and Inhomogenous * Localization Error.", IEEE T Pattern Anal 34 (8), 1520-1532, 2012. * The algorithm computes the optimal transformation to align two surfaces. * In addition to the surfaces a list of covariance matrices is used as * input for every surface. Each covariance matrix represents the error of * a specific vertex in the Surface. The covariance matrices * for each surface can be defined by the user, or calculated * by the CovarianceMatrixCalculator. In addition a trimmed algorithm version * is provided to compute the registration of partial overlapping surfaces. - * Example: + * The algorithm needs a clean surface non manifold edges and without duplicated + * vertices. In addition vtkCleanPolyData can be used to ensure a correct + * Surface representation. * + * \b Example: * - * \code{.cpp} + * + * \code * typedef itk::Matrix < double, 3, 3 > Matrix3x3; * typedef itk::Vector < double, 3 > Vector3; * typedef std::vector < Matrix3x3 > CovarianceMatrixList; * + * // compute the covariance matrices * mitk::CovarianceMatrixCalculator::Pointer matrixCalculator = * mitk::CovarianceMatrixCalculator::New(); * + * // compute the covariance matrices for the moving surface (X) * matrixCalculator->SetInputSurface(movingSurface); * matrixCalculator->ComputeCovarianceMatrices(); * CovarianceMatrixList sigmas_X = matrixCalculator->GetCovarianceMatrices(); * double meanVarX = matrixCalculator->GetMeanVariance(); * - * // compute the cov matrices for the fixed surface ( ct surface) + * // compute the covariance matrices for the fixed surface (Y) * matrixCalculator->SetInputSurface(fixedSurface); * matrixCalculator->ComputeCovarianceMatrices(); * CovarianceMatrixList sigmas_Y = matrixCalculator->GetCovarianceMatrices(); * double meanVarY = matrixCalculator->GetMeanVariance(); * + * // the FRE normalization factor * double normalizationFactor = sqrt( meanVarX + meanVarY); * * // A-ICP algorithm * mitk::AnisotropicIterativeClosestPointRegistration::Pointer aICP = * mitk::AnisotropicIterativeClosestPointRegistration::New(); * - * // set up parameters + * // set up parameters * aICP->SetMovingSurface(movingSurface); * aICP->SetFixedSurface(fixedSurface); - * aICP->SetCovarianceMatricesMovingSurface(covarianceMatricesMoving); - * aICP->SetCovarianceMatricesFixedSurface(covarianceMatricesFixed); + * aICP->SetCovarianceMatricesMovingSurface(sigmas_X); + * aICP->SetCovarianceMatricesFixedSurface(sigmas_Y); * aICP->SetFRENormalizationFactor(normalizationFactor); * - * // enable trimming. 40 percent of the moving point set - * // will be used for registration + * // Trimming is enabled if a fator > 0.0 is set. + * // 40 percent of the moving point set + * // will be used for registration in this example. + * // To disable trimming set the trim factor back to 0.0 * aICP->SetTrimmFactor(0.4); * * // run the algorithm * aICP->Update(); * * // retrieve the computed transformation * Matrix3x3 rotation = aICP->GetRotation(); * Vector3 translation = aICP->GetTranslation(); * \endcode * */ class MitkSurfaceRegistration_EXPORT AnisotropicIterativeClosestPointRegistration : public itk::Object { protected: /** Definition of a 3x3 covariance matrix.*/ typedef itk::Matrix < double, 3, 3 > CovarianceMatrix; /** Definition of a list of covariance matrices.*/ typedef std::vector< CovarianceMatrix > CovarianceMatrixList; /** Definition of a translation vector.*/ typedef mitk::Vector3D Translation; /** Definition of a 3x3 rotation matrix.*/ typedef CovarianceMatrix Rotation; /** Definition of a correspondeces, index and distance.*/ typedef std::pair < unsigned int, double > Correspondence; /** Definition of a list of correspondences.*/ typedef std::vector < Correspondence > CorrespondenceList; AnisotropicIterativeClosestPointRegistration(); ~AnisotropicIterativeClosestPointRegistration(); /** Max amount of iterations. Default is 1000.*/ unsigned int m_MaxIterations; /** Threshold used for termination. Default is 1.0e-6.*/ double m_Threshold; /** Normalization factor for the feducial registration error. default is 0.0.*/ double m_FRENormalizationFactor; /** Search radius for the correspondence search. Default is 30.*/ double m_SearchRadius; /** The maximum number of iterations in the weighted point based * registration. Default is 1000. */ double m_MaxIterationsInWeightedPointTransform; /** The fiducial registration error (FRE).*/ double m_FRE; /** Trimmfactor for partial overlapping registration. Default is 0.*/ double m_TrimmFactor; /** Amount of iterations used by the algorithm.*/ unsigned int m_NumberOfIterations; /** Moving surface that is transformed on the fixed surface.*/ itk::SmartPointer < Surface > m_MovingSurface; /** The fixed / target surface.*/ itk::SmartPointer < Surface > m_FixedSurface; /** The weighted point based registration algorithm.*/ itk::SmartPointer < WeightedPointTransform > m_WeightedPointTransform; /** The covariance matrices belonging to the moving surface (X).*/ CovarianceMatrixList m_CovarianceMatricesMovingSurface; /** The covariance matrices belonging to the moving surface (Y).*/ CovarianceMatrixList m_CovarianceMatricesFixedSurface; /** The computed 3x1 translation vector.*/ Translation m_Translation; /** The computed 3x3 rotation matrix.*/ Rotation m_Rotation; /** * Method that computes the correspondences between the moving point set X * and the fixed point set Y. The distances between the points * are weighted with weight matrices that are computed from the covariances * along the surfaces axes. This method implements the runtime optimization * presented by L. Maier-Hein et al.. The correspondences are computed with * the help of a kd tree. The correspondences are searched in a given radius * in the euklidian space. Every correspondence found in this radius is * weighted based on the covariance matrices and the best weighting will be * used as a correspondence. * * @param X The moving point set. * @param Z The returned correspondences from the fixed point set. * @param Y The fixed point set saved in a kd tree. * @param sigma_X Covariance matrices belonging to the moving point set. * @param sigma_Y Covariance matrices belonging to the fixed point set. * @param sigma_Z Covariance matrices belonging to the correspondences found. * @param correspondences Saved correspondences, in a pair containing the * their index in Y and distance. * @param radius The search radius used in in kd tree. * */ void ComputeCorrespondences ( vtkPoints* X, vtkPoints* Z, vtkKdTreePointLocator *Y, const CovarianceMatrixList& sigma_X, const CovarianceMatrixList& sigma_Y, CovarianceMatrixList& sigma_Z, CorrespondenceList &correspondences, const double radius ); public: /** Set the maximum amount of iterations used by the algorithm. */ itkSetMacro(MaxIterations, unsigned int) /** Set the threshold used to terminate the algorithm.*/ itkSetMacro(Threshold, double) /** Set the normalization factor for the fiducial registration error (FRE). * The normalization factor is computed with the help of the mean variance * of both CovarianceMatrixList that can be obtained when the covariance * matrices are calculated with the CovarianceMatrixCalculator: * * \code{.cpp} * double FRENormalizationFactor = sqrt ( MeanVarianceX + MeanVarianceY ); * \endcode */ itkSetMacro(FRENormalizationFactor, double) /** Set search radius for the correspondence search.*/ itkSetMacro(SearchRadius, double) /** Set the maximim number of iterations used by the point based registration * algorithm. */ itkSetMacro(MaxIterationsInWeightedPointTransform, double) /** Get the fiducial registration error (FRE).*/ itkGetMacro(FRE,double) /** Get the number of iterations used by the algorithm.*/ itkGetMacro(NumberOfIterations, unsigned int) /** * Factor that trimms the point set in percent.E.g. 0.4 will use 40 precent * of the point set. To enable the trimmed version a * factor > 0 and < 1 must be set. The default value is 0.0. */ itkSetMacro(TrimmFactor,double) /** * Set moving surface that includes the point set (X). */ itkSetMacro(MovingSurface,itk::SmartPointer) /** * Set fixed surface that includes the point set (Y). */ itkSetMacro(FixedSurface,itk::SmartPointer) /** * Returns the 3x1 translation vector computed by the algorithm. */ itkGetConstReferenceMacro(Translation,Translation) /** * Returns the 3x3 rotation matrix computed by the algorithm. */ itkGetConstReferenceMacro(Rotation,Rotation) /** * Set the covariance matrices of the moving surface. The algorithm * need the same amount of covariance and points available in the surface. * The covariance matrix for every vertex in a Surface can be calculated by * the CovarianceMatrixCalculator. It is also possible to define * arbitrary matrices by hand. */ void SetCovarianceMatricesMovingSurface( CovarianceMatrixList& list ) { m_CovarianceMatricesMovingSurface = list; } /** * Set the covariance matrices of thefixed surface. The algorithm * need the same amount of covariance and points available in the surface. * The covariance matrix for every vertex in a Surface can be calculated by * the CovarianceMatrixCalculator. It is also possible to define * arbitrary matrices by hand. */ void SetCovarianceMatricesFixedSurface( CovarianceMatrixList& list ) { m_CovarianceMatricesFixedSurface = list; } mitkClassMacro(AnisotropicIterativeClosestPointRegistration, itk::Object) itkNewMacro(Self) /** * This method executes the algorithm. * * @warning The algorithm is only a simple calculation filter and can not be * used in a mitk filter pipline. * * @throws Exception if the search radius was doubled more than 20 times to * prevent endless loops. Re-run the with a different search radius that * will find the correspondences. */ void Update(); }; } #endif diff --git a/Modules/SurfaceRegistration/mitkCovarianceMatrixCalculator.cpp b/Modules/SurfaceRegistration/mitkCovarianceMatrixCalculator.cpp index 4d0653b884..424a312454 100644 --- a/Modules/SurfaceRegistration/mitkCovarianceMatrixCalculator.cpp +++ b/Modules/SurfaceRegistration/mitkCovarianceMatrixCalculator.cpp @@ -1,414 +1,415 @@ /*=================================================================== 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 "mitkCovarianceMatrixCalculator.h" #include #include #include #include #include #include #include #include // forward declarations of private functions static vtkIdList* GetNeighboursOfPoint( unsigned int index, vtkPolyData* polydata); static vtkIdList* CalculatePCAonPointNeighboursForNormalVector ( int index, double normal[3], itk::Matrix & mat, double curVertex[3], std::vector& pointList, vtkPolyData* polyData ); static itk::Matrix ComputeCovarianceMatrix( itk::Matrix < double,3,3 > & axes, double sigma[3], double normalizationValue ); namespace mitk { +/** \brief Pimpl to hold the prifate data in the CovarianceMatrixCalculator.*/ struct CovarianceMatrixCalculatorData { vtkPolyDataNormals* m_PolyDataNormals; vtkPolyData* m_PolyData; Surface* m_Input; double m_VoronoiScalingFactor; bool m_EnableNormalization; double m_MeanVariance; CovarianceMatrixCalculatorData() : m_PolyDataNormals(vtkPolyDataNormals::New()), m_PolyData(NULL), m_Input(NULL), m_VoronoiScalingFactor(1.0), m_EnableNormalization(false), m_MeanVariance(0.0) { m_PolyDataNormals->SplittingOff(); } ~CovarianceMatrixCalculatorData() { if ( m_PolyDataNormals ) m_PolyDataNormals->Delete(); } }; } mitk::CovarianceMatrixCalculator::CovarianceMatrixCalculator() : d (new CovarianceMatrixCalculatorData()) { } mitk::CovarianceMatrixCalculator::~CovarianceMatrixCalculator() { delete d; } void mitk::CovarianceMatrixCalculator::SetVoronoiScalingFator(const double factor) { d->m_VoronoiScalingFactor = factor; } void mitk::CovarianceMatrixCalculator::EnableNormalization(bool state) { d->m_EnableNormalization = state; } double mitk::CovarianceMatrixCalculator::GetMeanVariance() const { return d->m_MeanVariance; } const mitk::CovarianceMatrixCalculator::CovarianceMatrixList& mitk::CovarianceMatrixCalculator::GetCovarianceMatrices() const { return m_CovarianceMatrixList; } void mitk::CovarianceMatrixCalculator::SetInputSurface(Surface *input) { d->m_Input = input; } void mitk::CovarianceMatrixCalculator::ComputeCovarianceMatrices() { double normalizationValue = -1.0; vtkDataArray* normals = NULL; d->m_MeanVariance = 0.0; if ( !d->m_Input ) mitkThrow() << "No input surface was set in mitk::CovarianceMatrixCalculator"; d->m_PolyData = d->m_Input->GetVtkPolyData(); // compute surface normals d->m_PolyDataNormals->SetInputData(d->m_PolyData); d->m_PolyDataNormals->Update(); normals = d->m_PolyDataNormals->GetOutput()->GetPointData()->GetNormals(); if( d->m_EnableNormalization ) normalizationValue = 1.5; // clear the matrixlist m_CovarianceMatrixList.clear(); // allocate memory if required if ( d->m_PolyData->GetNumberOfPoints() > (vtkIdType)m_CovarianceMatrixList.capacity() ) m_CovarianceMatrixList.reserve(d->m_PolyData->GetNumberOfPoints()); for ( vtkIdType i = 0; i < d->m_PolyData->GetNumberOfPoints(); ++i ) { Vertex normal; Vertex currentVertex; Vertex variances = { 0.0, 0.0, 0.0 }; CovarianceMatrix mat; mat.Fill(0.0); normals->GetTuple(i,normal); d->m_PolyData->GetPoint(i, currentVertex); ComputeOrthonormalCoordinateSystem(i,normal,mat,variances, currentVertex); //use prefactor for sigma along surface variances[0] = (d->m_VoronoiScalingFactor * variances[0]); variances[1] = (d->m_VoronoiScalingFactor * variances[1]); variances[2] = (d->m_VoronoiScalingFactor * variances[2]); d->m_MeanVariance += ( variances[0] + variances[1] + variances[2] ); // compute the covariance matrix and save it CovarianceMatrix covarianceMatrix = ComputeCovarianceMatrix(mat, variances, normalizationValue); m_CovarianceMatrixList.push_back(covarianceMatrix); } if ( d->m_EnableNormalization ) d->m_MeanVariance = normalizationValue / 3.0; else d->m_MeanVariance /= (3.0 * (double) d->m_PolyData->GetNumberOfPoints()); // reset input d->m_PolyData = NULL; d->m_Input = NULL; } // Get a list with the id's of all surrounding conected vertices // to the current vertex at the given index in the polydata vtkIdList* GetNeighboursOfPoint( unsigned int index, vtkPolyData* polydata) { vtkIdList* cellIds = vtkIdList::New(); vtkIdList* result = vtkIdList::New(); polydata->GetPointCells(index,cellIds); for (vtkIdType j=0; j < cellIds->GetNumberOfIds(); j++) { vtkIdList* newPoints = polydata->GetCell(cellIds->GetId(j))->GetPointIds(); for (vtkIdType k=0; k < newPoints->GetNumberOfIds(); k++) { //if point has not yet been inserted add id if (result->IsId(newPoints->GetId(k))==-1) { result->InsertNextId(newPoints->GetId(k)); } } } cellIds->Delete(); return result; } // Computes a primary component analysis of the surounding vertices // of the verex at the current index. vtkIdList* CalculatePCAonPointNeighboursForNormalVector( int index, double normal[3], itk::Matrix & mat, double curVertex[3], std::vector& pointList, vtkPolyData* polyData ) { typedef std::vector VectorType; typedef VectorType::const_iterator ConstPointIterator; typedef double Vertex[3]; Vertex mean = { 0.0, 0.0, 0.0 }; Vertex tmp = { 0.0, 0.0, 0.0 }; vtkIdList* neighbourPoints = GetNeighboursOfPoint(index, polyData); const vtkIdType size = neighbourPoints->GetNumberOfIds(); // reserve memory for all neighbours pointList.reserve(size); // project neighbours on plane given by normal // and compute mean for(vtkIdType i = 0; i < size; ++i) { mitk::Point3D p; Vertex resultPoint; polyData->GetPoint((neighbourPoints->GetId(i)),tmp); vtkPlane::GeneralizedProjectPoint(tmp,curVertex,normal,resultPoint); p[0] = resultPoint[0]; p[1] = resultPoint[1]; p[2] = resultPoint[2]; mean[0] += p[0]; mean[1] += p[1]; mean[2] += p[2]; pointList.push_back(p); } mean[0] /= (double) size; mean[1] /= (double) size; mean[2] /= (double) size; // compute the covariances with matrix multiplication for( ConstPointIterator it = pointList.begin(); it != pointList.end(); ++it ) { tmp[0] = ((*it)[0] - mean[0]); tmp[1] = ((*it)[1] - mean[1]); tmp[2] = ((*it)[2] - mean[2]); // on diagonal elements mat[0][0] += tmp[0] * tmp[0]; mat[1][1] += tmp[1] * tmp[1]; mat[2][2] += tmp[2] * tmp[2]; // of diagonal elements mat[1][0] += tmp[0] * tmp[1]; mat[2][0] += tmp[0] * tmp[2]; mat[2][1] += tmp[1] * tmp[2]; } // copy upper triangle to lower triangle, // we got a symetric matrix mat[0][1] = mat[1][0]; mat[0][2] = mat[2][0]; mat[1][2] = mat[2][1]; // variance mat /= (size - 1); vnl_svd svd(mat.GetVnlMatrix()); for ( int i = 0; i < 3; ++i ) for ( int j = 0; j < 3; ++j ) mat[i][j] = svd.U()[j][i]; return neighbourPoints; } // Computes an orthonormal system for a vertex with it's surrounding neighbours. void mitk::CovarianceMatrixCalculator::ComputeOrthonormalCoordinateSystem( const int index, Vertex normal, CovarianceMatrix& axes, Vertex variances, Vertex curVertex ) { typedef std::vector VectorType; typedef VectorType::const_iterator ConstPointIterator; VectorType projectedPoints; Vertex meanValues = { 0.0, 0.0, 0.0 }; //project neighbours to new coordinate system and get principal axes vtkIdList* neighbourPoints = CalculatePCAonPointNeighboursForNormalVector( index, normal, axes, curVertex, projectedPoints, d->m_PolyData ); // Set the normal as the third principal axis axes[2][0] = normal[0]; axes[2][1] = normal[1]; axes[2][2] = normal[2]; for( vtkIdType i = 0; i < neighbourPoints->GetNumberOfIds(); ++i ) { mitk::Point3D projectedPoint; Vertex curNeighbour; d->m_PolyData->GetPoint(neighbourPoints->GetId(i), curNeighbour); curNeighbour[0] = curNeighbour[0] - curVertex[0]; curNeighbour[1] = curNeighbour[1] - curVertex[1]; curNeighbour[2] = curNeighbour[2] - curVertex[2]; for(int k = 0; k < 3; ++k) { projectedPoint[k] = axes[k][0] * curNeighbour[0] + axes[k][1] * curNeighbour[1] + axes[k][2] * curNeighbour[2]; meanValues[k] += projectedPoint[k]; } // reuse the allocated vector from the PCA on the point neighbours projectedPoints[i] = projectedPoint; } meanValues[0] /= (double) projectedPoints.size(); meanValues[1] /= (double) projectedPoints.size(); meanValues[2] /= (double) projectedPoints.size(); // compute variances along new axes for( ConstPointIterator it = projectedPoints.begin(); it != projectedPoints.end(); ++it ) { const mitk::Point3D& p = *it; variances[0] += (p[0] - meanValues[0]) * (p[0] - meanValues[0]); variances[1] += (p[1] - meanValues[1]) * (p[1] - meanValues[1]); variances[2] += (p[2] - meanValues[2]) * (p[2] - meanValues[2]); } variances[0] /= (double) (projectedPoints.size() - 1); variances[1] /= (double) (projectedPoints.size() - 1); variances[2] /= (double) (projectedPoints.size() - 1); //clean up neighbourPoints->Delete(); } // Sorts the axes of the computed orthonormal system based on // the eigenvalues in a descending order itk::Matrix ComputeCovarianceMatrix( itk::Matrix < double,3,3 > & axes, double sigma[3], double normalizationValue ) { unsigned int idxMax,idxMin,idxBetween; itk::Matrix returnValue; itk::Matrix V; itk::Matrix diagMatrix; diagMatrix.Fill(0.0); if(sigma[0] >= sigma[1] && sigma[0] >= sigma[2]) { idxMax = 0; if(sigma[1] >= sigma[2]) { idxBetween = 1; idxMin = 2; } else { idxBetween = 2; idxMin = 1; } } else if (sigma[1] >= sigma[0] && sigma[1] >= sigma[2]) { idxMax = 1; if(sigma[0] >= sigma[2]) { idxBetween = 0; idxMin = 2; } else { idxBetween = 2; idxMin = 0; } } else // index 2 corresponds to largest sigma { idxMax = 2; if(sigma[0] >= sigma[1]) { idxBetween = 0; idxMin = 1; } else { idxBetween = 1; idxMin = 0; } } V[0][0] = axes[idxMax][0]; V[1][0] = axes[idxMax][1]; V[2][0] = axes[idxMax][2]; V[0][1] = axes[idxBetween][0]; V[1][1] = axes[idxBetween][1]; V[2][1] = axes[idxBetween][2]; V[0][2] = axes[idxMin][0]; V[1][2] = axes[idxMin][1]; V[2][2] = axes[idxMin][2]; diagMatrix[0][0] = sigma[idxMax]; diagMatrix[1][1] = sigma[idxBetween]; diagMatrix[2][2] = sigma[idxMin]; returnValue = V * diagMatrix * V.GetTranspose(); if(normalizationValue > 0.0) { double trace = returnValue[0][0] + returnValue[1][1] + returnValue[2][2]; returnValue *= (normalizationValue / trace); } return returnValue; } diff --git a/Modules/SurfaceRegistration/mitkCovarianceMatrixCalculator.h b/Modules/SurfaceRegistration/mitkCovarianceMatrixCalculator.h index 84472013af..488d95fd55 100644 --- a/Modules/SurfaceRegistration/mitkCovarianceMatrixCalculator.h +++ b/Modules/SurfaceRegistration/mitkCovarianceMatrixCalculator.h @@ -1,114 +1,117 @@ /*=================================================================== 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 __MITK_COVARIANCEMATRIXCALCULATOR_H__ #define __MITK_COVARIANCEMATRIXCALCULATOR_H__ // exports #include #include #include #include #include namespace mitk { - +// forward declarations class Surface; struct CovarianceMatrixCalculatorData; /** * \ingroup AnisotropicRegistration * * @brief Class that computes the covariance matrices for every point in a * {@link Surface} used in the A-ICP algorithm. * * Computes a covariance matrix for every vertex in a given {@link Surface} * based on it's direct neighbours and saves them in a CovarianceMatrixList. * The Class implements the CM_PCA method presented by * L. Maier-Hein et al. in "Convergent Iterative Closest-Point Algorithm * to Accomodate Anisotropic and Inhomogenous Localization Error.", - * IEEE T Pattern Anal 34 (8), 1520-1532, 2012. + * IEEE T Pattern Anal 34 (8), 1520-1532, 2012. The algorithm needs + * a clean Surface with non manifold edges and no duplicated vertices. To + * ensure a clean Surface representation use vtkCleanPolyData. */ class MitkSurfaceRegistration_EXPORT CovarianceMatrixCalculator : public itk::Object { protected: // local typedefs /** Definition of the covariance matrix.*/ typedef itk::Matrix CovarianceMatrix; /** Definition of a list of covariance matrices */ typedef std::vector CovarianceMatrixList; typedef double Vertex[3]; + /** Pimpl to hold private data.*/ CovarianceMatrixCalculatorData* d; /** List that stores the computed covariance matrices. */ CovarianceMatrixList m_CovarianceMatrixList; void ComputeOrthonormalCoordinateSystem( const int index, Vertex normal, CovarianceMatrix &principalComponents, Vertex variances, Vertex curVertex); CovarianceMatrixCalculator(); ~CovarianceMatrixCalculator(); public: mitkClassMacro(CovarianceMatrixCalculator, itk::Object) itkNewMacro (Self) /** Sets the scaling factor for the voronoi area. * @param factor The scaling factor. */ void SetVoronoiScalingFator( const double factor ); /** Enables/disables the covariance matrix normalization. * @param state Enables the covariance matrix normalization. */ void EnableNormalization( bool state ); /** Returns the mean of variance of all computed covariance matrices. * @return The mean variance. */ double GetMeanVariance() const; /** Returns a reference to the CovarianceMatrixList with the computed covariance matrices. * @return A CovarianceMatrixList. */ const CovarianceMatrixList& GetCovarianceMatrices() const; /** Sets the input {@link Surface} for which the covariance matrices will be calculated. * @param input A {@link Surface}. */ void SetInputSurface (Surface *input); /** Method that compites the covariance matrices for the input surface. * @throws std::exception If the input surface is not set. */ void ComputeCovarianceMatrices(); }; } #endif