diff --git a/Modules/SurfaceRegistration/mitkAnisotropicIterativeClosestPointRegistration.cpp b/Modules/SurfaceRegistration/mitkAnisotropicIterativeClosestPointRegistration.cpp index 39facae470..8be4877f8d 100644 --- a/Modules/SurfaceRegistration/mitkAnisotropicIterativeClosestPointRegistration.cpp +++ b/Modules/SurfaceRegistration/mitkAnisotropicIterativeClosestPointRegistration.cpp @@ -1,300 +1,301 @@ /*=================================================================== 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 { typedef std::pair < unsigned int, double > Correspondence; bool operator() (const Correspondence& a, const Correspondence& b) { return (a.second < b.second); } } myComp; 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 Y dataset - + // 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 set + // 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 ); // 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 eb046278f5..fe629fb49d 100644 --- a/Modules/SurfaceRegistration/mitkAnisotropicIterativeClosestPointRegistration.h +++ b/Modules/SurfaceRegistration/mitkAnisotropicIterativeClosestPointRegistration.h @@ -1,156 +1,309 @@ /*=================================================================== 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: + * + * + * \code{.cpp} + * typedef itk::Matrix < double, 3, 3 > Matrix3x3; + * typedef itk::Vector < double, 3 > Vector3; + * typedef std::vector < Matrix3x3 > CovarianceMatrixList; + * + * mitk::CovarianceMatrixCalculator::Pointer matrixCalculator = + * mitk::CovarianceMatrixCalculator::New(); + * + * matrixCalculator->SetInputSurface(movingSurface); + * matrixCalculator->ComputeCovarianceMatrices(); + * CovarianceMatrixList sigmas_X = matrixCalculator->GetCovarianceMatrices(); + * double meanVarX = matrixCalculator->GetMeanVariance(); + * + * // compute the cov matrices for the fixed surface ( ct surface) + * matrixCalculator->SetInputSurface(fixedSurface); + * matrixCalculator->ComputeCovarianceMatrices(); + * CovarianceMatrixList sigmas_Y = matrixCalculator->GetCovarianceMatrices(); + * double meanVarY = matrixCalculator->GetMeanVariance(); + * + * double normalizationFactor = sqrt( meanVarX + meanVarY); + * + * // A-ICP 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(normalizationFactor); + * + * // enable trimming. 40 percent of the moving point set + * // will be used for registration + * 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.*/ + /** Max amount of iterations. Default is 1000.*/ unsigned int m_MaxIterations; - /** Threshold used for termination.*/ + /** Threshold used for termination. Default is 1.0e-6.*/ double m_Threshold; - /** Normalization factor for the feducial registration error.*/ + /** Normalization factor for the feducial registration error. default is 0.0.*/ double m_FRENormalizationFactor; - /** Search radius for the correspondence search.*/ + /** Search radius for the correspondence search. Default is 30.*/ double m_SearchRadius; - /** The maximum number of iterations in the weighted point based registration.*/ + /** The maximum number of iterations in the weighted point based + * registration. Default is 1000. + */ double m_MaxIterationsInWeightedPointTransform; - /** The fiducial registration error.*/ + /** The fiducial registration error (FRE).*/ double m_FRE; - /** Trimmfactor for partial overlapping registration.*/ + /** 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/mitkAnisotropicRegistrationCommon.h b/Modules/SurfaceRegistration/mitkAnisotropicRegistrationCommon.h index 11b3e07ec6..63ac711849 100644 --- a/Modules/SurfaceRegistration/mitkAnisotropicRegistrationCommon.h +++ b/Modules/SurfaceRegistration/mitkAnisotropicRegistrationCommon.h @@ -1,142 +1,143 @@ /*=================================================================== 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 __ANISOTROPICREGISTRATIONCOMMON_H__ #define __ANISOTROPICREGISTRATIONCOMMON_H__ #include #include #include #include // forward declarations class vtkPoints; namespace mitk { class PointSet; /** * \ingroup AnisotropicRegistration + * * \brief A Class that provides common static functions used by all classes * and tests in the anisotropic iterative closest point algorithm * (AnisotropicIterativeClosestPointRegistration). * * The class provides common functionality used by the A-ICP algorithm like: * compute weightmatrices (@ref CalculateWeightMatrix()), * transform points (@ref TransformPoints()), propagate 3 x 3 matrices * (@ref PropagateMatrices()) and compute the target registration error (TRE) * (@ref ComputeTargetRegistrationError()). */ class MitkSurfaceRegistration_EXPORT AnisotropicRegistrationCommon { protected: // local typedefs /** Definition of the 3 x 3 weight matrix.*/ typedef itk::Matrix WeightMatrix; /** Definition of a rotation matrix.*/ typedef WeightMatrix Rotation; /** Definition of the covariance matrix.*/ typedef WeightMatrix CovarianceMatrix; /** Definition of the translation vector.*/ typedef mitk::Vector3D Translation; /** Definition of the weight matrix list.*/ typedef std::vector< WeightMatrix > MatrixList; AnisotropicRegistrationCommon(){} ~AnisotropicRegistrationCommon(){} public: /** @brief Method that computes a WeightMatrix with two CovarianceMatrices. * @param sigma_X CovarianceMatrix from the moving point set. * @param sigma_Y CovarianceMatrix from the fixed point set. * @return The computed WeighMatrix. */ static WeightMatrix CalculateWeightMatrix( const CovarianceMatrix &sigma_X, const CovarianceMatrix &sigma_Y ); /** * @brief Transforms a point cloud with a Rotation and Translation. * * The method uses two point sets as input. It transforms every point from the * source point set and saves the result in the destination. The soure is not * modified. If the same point set is used as source and destination. The method * will modify the source and the transformation is done in place. * * @warning No bound check is done. Ensure that source and destination are * allocated and have the same size. * * @param src The source point set. * @param dst The destination point set. * @param rotation The rotation matrix. * @param translation The translation vector. * */ static void TransformPoints( vtkPoints* src, vtkPoints* dst, const Rotation& rotation, const Translation& translation ); /** * @brief Propagate a list of matrices with a rotation matrix. * * Method that propagate the source list and saves the result in the destination. * If the source and destination lists are the same the matrices will be computed * in place. * * @warning No bound check is done. Make sure that both lists are allocated and * have the same size. * * @param src Reference to the source matrices list. * @param dst Reference to the destination list * @param rotation Reference to a rotation matrix. */ static void PropagateMatrices( const MatrixList &src, MatrixList& dst, const Rotation &rotation ); /** * @brief Compute the target registration error between two point sets. * * Method that is used for testing and evaluation. It computes the target * registration error (TRE) between two point sets with a rotation matrix and * a translation vector. * * @param movingTargets The target points of the moving point set. * @param fixedTargets The target points of the fixed point set. * @param rotation A 3x3 rotation matrix. * @param translation A 3x1 translation vector. * * @return The Target Registration Error (TRE). */ static double ComputeTargetRegistrationError( const mitk::PointSet* movingTargets, const mitk::PointSet* fixedTargets, const Rotation& rotation, const Translation& translation ); }; } #endif diff --git a/Modules/SurfaceRegistration/mitkWeightedPointTransform.cpp b/Modules/SurfaceRegistration/mitkWeightedPointTransform.cpp index 342fb880ea..6b96756bb8 100644 --- a/Modules/SurfaceRegistration/mitkWeightedPointTransform.cpp +++ b/Modules/SurfaceRegistration/mitkWeightedPointTransform.cpp @@ -1,475 +1,478 @@ /*=================================================================== 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 "mitkWeightedPointTransform.h" #include "mitkAnisotropicRegistrationCommon.h" #include #include #include typedef itk::Matrix < double,3,3 > Matrix3x3; typedef std::vector < Matrix3x3 > Matrix3x3List; +/////////////////////////////////////////////// // forward declarations of private functions +/////////////////////////////////////////////// + static double ComputeWeightedFRE ( vtkPoints* X, vtkPoints* Y, const Matrix3x3List &CovarianceMatricesMoving, const Matrix3x3List &CovarianceMatricesFixed, double FRENormalizationFactor, Matrix3x3List& WeightMatrices, const Matrix3x3& rotation, const itk::Vector& translation ); static void calculateWeightMatrices( const Matrix3x3List& X, const Matrix3x3List& Y, Matrix3x3List& result, const Matrix3x3& rotation ); static void IsotropicRegistration( vtkPoints* X, vtkPoints* Y, vtkLandmarkTransform* landmarkTransform, Matrix3x3& rotation, itk::Vector& translation ); mitk::WeightedPointTransform::WeightedPointTransform() : m_Threshold(1.0e-4), m_MaxIterations(1000), m_Iterations(-1), m_FRE(-1.0), m_FRENormalizationFactor(1.0), m_LandmarkTransform(vtkSmartPointer::New()) { } mitk::WeightedPointTransform::~WeightedPointTransform() { m_FixedPointSet = NULL; m_MovingPointSet = NULL; m_LandmarkTransform = NULL; } void mitk::WeightedPointTransform::ComputeTransformation() { WeightedPointRegister( m_MovingPointSet, m_FixedPointSet, m_CovarianceMatricesMoving, m_CovarianceMatricesFixed, m_Threshold, m_MaxIterations, m_Rotation, m_Translation, m_FRE, m_Iterations ); } // computes the weightmatrix with 2 covariance matrices // and a given transformation void calculateWeightMatrices( const Matrix3x3List& X, const Matrix3x3List& Y, Matrix3x3List& result, const Matrix3x3& rotation ) { const vnl_matrix_fixed rotation_T = rotation.GetTranspose(); #pragma omp parallel for for ( size_t i = 0; i < X.size(); ++i ) { const Matrix3x3 w = rotation * X[i] * rotation_T; result[i] = mitk::AnisotropicRegistrationCommon::CalculateWeightMatrix(w,Y[i]); } } // computes the weighted fiducial registration error double ComputeWeightedFRE ( vtkPoints* X, vtkPoints* Y, const Matrix3x3List &CovarianceMatricesMoving, const Matrix3x3List &CovarianceMatricesFixed, double FRENormalizationFactor, Matrix3x3List& WeightMatrices, const Matrix3x3& rotation, const itk::Vector& translation ) { double FRE = 0; //compute weighting matrices calculateWeightMatrices( CovarianceMatricesMoving, CovarianceMatricesFixed, WeightMatrices, rotation ); #pragma omp parallel for reduction(+:FRE) for (unsigned int i = 0; i < WeightMatrices.size(); ++i) { //convert to itk data types (nessecary since itk 4 migration) itk::Vector converted_MovingPoint; double point[3]; X->GetPoint(i,point); converted_MovingPoint[0] = point[0]; converted_MovingPoint[1] = point[1]; converted_MovingPoint[2] = point[2]; // transform point itk::Vector p = rotation * converted_MovingPoint + translation; Y->GetPoint(i,point); p[0] -= point[0]; p[1] -= point[1]; p[2] -= point[2]; //do calculation const itk::Vector D = WeightMatrices.at(i) * p; FRE += (D[0] * D[0] + D[1] * D[1] + D[2] * D[2]); } FRE /= WeightMatrices.size(); FRE = FRENormalizationFactor * sqrt(FRE); return FRE; } // registers two pointsets with an isotropic landmark transform void IsotropicRegistration( vtkPoints* X, vtkPoints* Y, vtkLandmarkTransform* landmarkTransform, Matrix3x3& rotation, itk::Vector& translation ) { landmarkTransform->SetSourceLandmarks(X); landmarkTransform->SetTargetLandmarks(Y); landmarkTransform->SetModeToRigidBody(); landmarkTransform->Modified(); landmarkTransform->Update(); vtkMatrix4x4* m = landmarkTransform->GetMatrix(); for ( int i = 0; i < 3; ++i ) for ( int j = 0; j < 3;++j ) rotation[i][j] = m->GetElement(i,j); translation[0] = m->GetElement(0,3); translation[1] = m->GetElement(1,3); translation[2] = m->GetElement(2,3); } void mitk::WeightedPointTransform::C_maker( vtkPoints* X, const WeightMatrixList &W, itk::VariableSizeMatrix< double >& returnValue ) { #pragma omp parallel for for(vtkIdType i = 0; i < X->GetNumberOfPoints(); ++i ) { unsigned int index = 3u * i; double point[3]; X->GetPoint(i,point); for ( int j = 0; j < 3; ++j ) { returnValue[index][0] = -W.at(i)[j][1] * point[2] + W.at(i)[j][2] * point[1]; returnValue[index][1] = W.at(i)[j][0] * point[2] - W.at(i)[j][2] * point[0]; returnValue[index][2] = -W.at(i)[j][0] * point[1] + W.at(i)[j][1] * point[0]; returnValue[index][3] = W.at(i)[j][0]; returnValue[index][4] = W.at(i)[j][1]; returnValue[index][5] = W.at(i)[j][2]; index += 1; } } } void mitk::WeightedPointTransform::E_maker( vtkPoints* X, vtkPoints* Y, const WeightMatrixList &W, vnl_vector< double >& returnValue ) { #pragma omp parallel for for( unsigned int i = 0; i < X->GetNumberOfPoints(); ++i ) { unsigned int index = 3u * i; double pX[3]; double pY[3]; Matrix3x3 M; X->GetPoint(i,pX); Y->GetPoint(i,pY); M[0][0] = pY[0] - pX[0]; M[0][1] = pY[1] - pX[1]; M[0][2] = pY[2] - pX[2]; M[1][0] = M[0][0]; M[1][1] = M[0][1]; M[1][2] = M[0][2]; M[2][0] = M[0][0]; M[2][1] = M[0][1]; M[2][2] = M[0][2]; for ( unsigned int j = 0; j < 3; ++j ) { returnValue[index + j] = W.at(i)[j][0] * M[j][0] + W.at(i)[j][1] * M[j][1] + W.at(i)[j][2] * M[j][2]; } } } void mitk::WeightedPointTransform::WeightedPointRegister( vtkPoints *X, vtkPoints *Y, const CovarianceMatrixList &Sigma_X, const CovarianceMatrixList &Sigma_Y, double Threshold, int MaxIterations, Rotation &TransformationR, Translation &TransformationT, double& FRE, int& n ) { double FRE_identity = 0.0; double FRE_isotropic_weighted = 0.0; double initialFRE = 0.0; //set config_change to infinite (max double) at start double config_change = std::numeric_limits::max(); Rotation initial_TransformationR; initial_TransformationR.SetIdentity(); Translation initial_TransformationT; initial_TransformationT.Fill(0.0); // Weightmatrices Matrix3x3List W; vtkPoints* X_transformed = vtkPoints::New(); vtkPoints* X_transformedNew = vtkPoints::New(); vnl_vector< double > oldq; itk::VariableSizeMatrix< double > iA; vnl_vector< double > iB; // initialize memory W.resize(X->GetNumberOfPoints()); X_transformed->SetNumberOfPoints(X->GetNumberOfPoints()); X_transformedNew->SetNumberOfPoints(X->GetNumberOfPoints()); iA.SetSize(3u * X->GetNumberOfPoints(), 6u); iB.set_size(3u * X->GetNumberOfPoints()); //calculate FRE_0 with identity transform FRE_identity = ComputeWeightedFRE(X,Y,Sigma_X,Sigma_Y,m_FRENormalizationFactor, W,initial_TransformationR, initial_TransformationT); MITK_INFO << "FRE for identity transform: "< myInverse(iA.GetVnlMatrix()); vnl_vector< double > q = myInverse.pinverse(iB.size()) * iB; //''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''' if (n>1) q = (q+oldq)/2; oldq = q; itk::Vector delta_t; delta_t[0] = q[3]; delta_t[1] = q[4]; delta_t[2] = q[5]; Matrix3x3 delta_theta; delta_theta[0][0] = 1; delta_theta[0][1] = -q[2]; delta_theta[0][2] = q[1]; delta_theta[1][0] = q[2]; delta_theta[1][1] = 1; delta_theta[1][2] = -q[0]; delta_theta[2][0] = -q[1]; delta_theta[2][1] = q[0]; delta_theta[2][2] = 1; vnl_svd svd_delta_theta(delta_theta.GetVnlMatrix()); //convert vnl matrices to itk matrices... Matrix3x3 U; Matrix3x3 V; for (int i = 0; i < 3; ++i) { for (int j = 0; j < 3; ++j) { U[i][j] = svd_delta_theta.U()[i][j]; V[i][j] = svd_delta_theta.V()[i][j]; } } Matrix3x3 delta_R = U * V.GetTranspose(); //update rotation TransformationR = delta_R * TransformationR; //update translation TransformationT = delta_R * TransformationT + delta_t; //update moving points mitk::AnisotropicRegistrationCommon::TransformPoints( X, X_transformedNew, TransformationR, TransformationT ); //calculate config change config_change = CalculateConfigChange(X_transformed,X_transformedNew); // swap the pointers the old set for the next iteration is // the new set of the last iteration vtkPoints* tmp = X_transformed; X_transformed = X_transformedNew; X_transformedNew = tmp; } while ( config_change > Threshold && n < MaxIterations ); //calculate FRE with current transform FRE = ComputeWeightedFRE( X,Y,Sigma_X,Sigma_Y,m_FRENormalizationFactor, W,TransformationR,TransformationT ); MITK_INFO <<"FRE after algorithm (prior to check with initial): "<Delete(); X_transformedNew->Delete(); } void mitk::WeightedPointTransform::SetMovingPointSet(vtkSmartPointer p) { m_MovingPointSet = p; } void mitk::WeightedPointTransform::SetCovarianceMatricesMoving(const CovarianceMatrixList &matrices) { m_CovarianceMatricesMoving = matrices; } void mitk::WeightedPointTransform::SetFixedPointSet(vtkSmartPointer p) { m_FixedPointSet = p; } void mitk::WeightedPointTransform::SetCovarianceMatricesFixed(const CovarianceMatrixList &matrices) { m_CovarianceMatricesFixed = matrices; } double mitk::WeightedPointTransform::CalculateConfigChange( vtkPoints* X, vtkPoints* X_new ) { double sum[3] = { 0.0,0.0,0.0 }; double mean[3] = { 0.0,0.0,0.0 }; double pX[3] = { 0.0,0.0,0.0 }; double pX_new[3] = { 0.0,0.0,0.0 }; // compute mean of the old point set and the first sum for ( vtkIdType i = 0; i < X->GetNumberOfPoints(); ++i ) { X->GetPoint(i,pX); X_new->GetPoint(i,pX_new); // first sum sum[0] += ( pX_new[0] - pX[0] ) * ( pX_new[0] - pX[0] ); sum[1] += ( pX_new[1] - pX[1] ) * ( pX_new[1] - pX[1] ); sum[2] += ( pX_new[2] - pX[2] ) * ( pX_new[2] - pX[2] ); // mean mean[0] += pX[0]; mean[1] += pX[1]; mean[2] += pX[2]; } mean[0] /= X->GetNumberOfPoints(); mean[1] /= X->GetNumberOfPoints(); mean[2] /= X->GetNumberOfPoints(); const double us = sum[0] + sum[1] + sum[2]; // reset sum sum[0] = sum[1] = sum[2] = 0.0; for ( vtkIdType i = 0; i < X->GetNumberOfPoints(); ++i ) { X->GetPoint(i,pX); sum[0] += (pX[0] - mean[0]) * (pX[0] - mean[0]); sum[1] += (pX[1] - mean[1]) * (pX[1] - mean[1]); sum[2] += (pX[2] - mean[2]) * (pX[2] - mean[2]); } const double ls = sum[0] + sum[1] + sum[2]; return sqrt(us/ls); } diff --git a/Modules/SurfaceRegistration/mitkWeightedPointTransform.h b/Modules/SurfaceRegistration/mitkWeightedPointTransform.h index 2a46fd74c2..d64f5803c9 100644 --- a/Modules/SurfaceRegistration/mitkWeightedPointTransform.h +++ b/Modules/SurfaceRegistration/mitkWeightedPointTransform.h @@ -1,253 +1,256 @@ /*=================================================================== 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 __WEIGHTEDPOINTTRANSFORM_H__ #define __WEIGHTEDPOINTTRANSFORM_H__ //EXPORTS #include "MitkSurfaceRegistrationExports.h" //ITK #include #include #include #include #include #include // forward declarations class vtkPoints; class vtkLandmarkTransform; namespace mitk { /** * \ingroup AnisotropicRegistration * * @brief This class implements an extension of the * weighted point based registration algorithm * from A. Danilchenko, R. Balachandran and J. M. Fitzpatrick. * * The class implements an extension of the weighted point based registration * from A. Danilchenko et al. * 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. The extension computes, in order * to ensure the convergence of the algorithm, an isotropic estimation * by an unweighted point based registration algorithm as an initial estimate. * The implemantion was orinally ported to C/C++ by A. Franz. * */ class MitkSurfaceRegistration_EXPORT WeightedPointTransform : public itk::Object { /** Definition of a 3x3 matrix.*/ typedef itk::Matrix Matrix3x3; /** Definition of a 3x3 Weighting matrix.*/ typedef Matrix3x3 WeightMatrix; /** Definition of a Rotation matrix.*/ typedef Matrix3x3 Rotation; /** Definition of a translation vector.*/ typedef itk::Vector Translation; /** Definition of a weight matrix list.*/ typedef std::vector WeightMatrixList; /** Definition of a covariance matrix list.*/ typedef std::vector CovarianceMatrixList; public: mitkClassMacro(WeightedPointTransform, itk::Object); itkNewMacro(Self); /** @brief Method which registers both point sets. */ void ComputeTransformation(); /** @brief Sets the threshold of the registration. Default value is 0.0001.*/ itkSetMacro(Threshold,double) /** @brief Sets the maximum number of iterations of the registration. * Default value is 1000. */ itkSetMacro(MaxIterations,double) /** @return Returns the number of iterations of the last run * of the registration algorithm. Returns -1 if there was no * run of the registration yet. */ itkGetMacro(Iterations,int); /** @return Returns the FRE of the last run of the registration algorithm. * Returns -1 if there was no run of the registration yet. */ itkGetMacro(FRE,double); /** @brief Sets the FRE normalization factor. Default value is 1.0. */ itkSetMacro(FRENormalizationFactor,double); /** @return Returns the current FRE normalization factor.*/ itkGetMacro(FRENormalizationFactor,double); /** Sets the moving point set used for the registration. * @param p The input point set. */ void SetMovingPointSet(vtkSmartPointer p); /** * Set the list of 3x3 covariance matrices belonging to the moving point set. * @param matrices List of covariance matrices. */ void SetCovarianceMatricesMoving( const CovarianceMatrixList& matrices); /** Sets the fixed point set used for the registration. * @param p The input point set. */ void SetFixedPointSet(vtkSmartPointer p); /** * Set the list of 3x3 covariance matrices belonging to the fixed point set. * @param matrices List of covariance matrices. */ void SetCovarianceMatricesFixed( const CovarianceMatrixList& matrices); /** * The translation vector computed by the algorithm. * @return 3x1 translation vector. */ const Translation& GetTransformT() const { return m_Translation; } /** * The rotation matrix computed by the algorithm. */ const Rotation& GetTransformR() const { return m_Rotation; } protected: WeightedPointTransform(); ~WeightedPointTransform(); /** Threshold used to terminate the algorithm.*/ double m_Threshold; /** Max allowed iterations used by the algorithm.*/ int m_MaxIterations; /** The amount of iterations needed by the algorithm.*/ int m_Iterations; /** The fiducial registration error (FRE) used in the algorithm.*/ double m_FRE; /** Normalization factor for the FRE.*/ double m_FRENormalizationFactor; /** Isotropic point based registration used for initial estimate.*/ vtkSmartPointer m_LandmarkTransform; /** The fixed point set (Y).*/ vtkSmartPointer m_FixedPointSet; /** Moving point set (X).*/ vtkSmartPointer m_MovingPointSet; /** Covariance matrices of the moving point set (Sigma_X).*/ CovarianceMatrixList m_CovarianceMatricesMoving; /** Covariance matrices of the moving point set (Sigma_Y).*/ CovarianceMatrixList m_CovarianceMatricesFixed; /** 3x1 translation vector.*/ Translation m_Translation; /** 3x3 rotation matrix.*/ Rotation m_Rotation; /** * original matlab-function: * * Constructs the C matrix of the linear version of the registration * problem, Cq = e, where q = [delta_angle(1:3),delta_translation(1:3)] and * e is produced by e_maker(X,Y,W) * * Authors: JM Fitzpatrick and R Balachandran * Creation: February 2009 * * -------------------------------------------- * * converted to C++ by Alfred Franz in March/April 2010 */ - void C_maker( vtkPoints* X, const WeightMatrixList &W, itk::VariableSizeMatrix< double >& returnValue); + void C_maker( vtkPoints* X, const WeightMatrixList &W, + itk::VariableSizeMatrix< double >& returnValue ); /** * original matlab-function: * * Constructs the e vector of the linear version of the registration * problem, Cq = e, where q = [delta_angle(1:3),delta_translation(1:3)] and * C is produced by C_maker(X,W) * * Authors: JM Fitzpatrick and R Balachandran * Creation: February 2009 * * -------------------------------------------- * * converted to C++ by Alfred Franz in March/April 2010 */ - void E_maker( vtkPoints* X, vtkPoints* Y, const WeightMatrixList &W, vnl_vector< double >& returnValue); + void E_maker( vtkPoints* X, vtkPoints* Y, + const WeightMatrixList &W, + vnl_vector< double >& returnValue ); /** * This method computes the change in a root mean squared * sense between the previous and the actual iteration. * The computed value is used as a termination constraint of the algorithm and * compared against the threshold. * * @param X The moving point set in the previous iteration step. * @param X_new The moving point set in the actual step. * * @return The computed change between the two point sets. */ - double CalculateConfigChange(vtkPoints* X, vtkPoints* X_new); + double CalculateConfigChange( vtkPoints* X, vtkPoints* X_new ); /** * @brief This method performs a variant of the weighted point register alogorithm presented by * A. Danilchenko, R. Balachandran and J. M. Fitzpatrick in January 2010. (Modified in January 2011) * converted to C++ by Alfred Franz in March/April 2010 * - * @param X (input) the moving point set - * @param Y (input) the fixed (static) point set - * @param Sigma_X (input) a 3-by-3-by-N array, each page containing the weighting matrix for the Nth pair of points in X (inverted and squared TODO: describe better) - * @param Sigma_Y (input) a 3-by-3-by-N array, each page containing the weighting matrix for the Nth pair of points in Y (inverted and squared TODO: describe better) - * @param Threshold (input) the relative size of the change to the moving set above which the iteration continues - * @param MaxIterations (input) the maximum number of iterations allowed - * @param Threshold (input) the threshold used to terminate the algorithm - * @param TransformationR (output) this variable will hold the computed rotation matrix - * @param TransformationT (output) this variable will hold the computed translation vector - * @param FRE (output) this variable will hold the computed rotation FRE of the transformation - * @param n (output) this variable will hold the number of iterations used by the algorithm + * @param X (input) the moving point set + * @param Y (input) the fixed (static) point set + * @param Sigma_X (input) a 3-by-3-by-N array, each page containing the weighting matrix for the Nth pair of points in X (inverted and squared TODO: describe better) + * @param Sigma_Y (input) a 3-by-3-by-N array, each page containing the weighting matrix for the Nth pair of points in Y (inverted and squared TODO: describe better) + * @param Threshold (input) the relative size of the change to the moving set above which the iteration continues + * @param MaxIterations (input) the maximum number of iterations allowed + * @param Threshold (input) the threshold used to terminate the algorithm + * @param TransformationR (output) this variable will hold the computed rotation matrix + * @param TransformationT (output) this variable will hold the computed translation vector + * @param FRE (output) this variable will hold the computed rotation FRE of the transformation + * @param n (output) this variable will hold the number of iterations used by the algorithm */ void WeightedPointRegister( vtkPoints* X, vtkPoints* Y, const CovarianceMatrixList &Sigma_X, const CovarianceMatrixList &Sigma_Y, double Threshold, int MaxIterations, Rotation& TransformationR, Translation& TransformationT, double& FRE, int& n ); }; } #endif