diff --git a/Modules/SurfaceRegistration/Testing/mitkAnisotropicIterativeClosestPointRegistrationTest.cpp b/Modules/SurfaceRegistration/Testing/mitkAnisotropicIterativeClosestPointRegistrationTest.cpp index 5d60acf011..3658463877 100644 --- a/Modules/SurfaceRegistration/Testing/mitkAnisotropicIterativeClosestPointRegistrationTest.cpp +++ b/Modules/SurfaceRegistration/Testing/mitkAnisotropicIterativeClosestPointRegistrationTest.cpp @@ -1,115 +1,112 @@ /*=================================================================== 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 AICP registration achieved for publications. + * Test to verify the results of the A-ICP registration. * The test can be used for both, the standard AICP and the trimmed variant. - - * Results for the standard variant are published in: - Maier-Hein et. al. "Convergent Iterative Closest-Point Algorithm to Accomodate - Anisotropic and Inhomogenous Localization Error". - * Results for the trimmed variant are published in: - Kilgus et. al. "Registration of Partially Overlapping Surfaces for Range Image based - Augmented Reality on Mobile Devices". + * 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"); - - 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(); - 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." ); - - // compute the cov matrices for the fixed surface ( ct surface) - 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); - - // run the algorithm - mitk::AnisotropicIterativeClosestPointRegistration::Pointer aICP = - mitk::AnisotropicIterativeClosestPointRegistration::New(); + MITK_TEST_BEGIN("mitkAnisotropicIterativeClosestPointRegistrationTest"); + + 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(); + 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." ); + + // compute the cov matrices for the fixed surface ( ct surface) + 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); + + // run the algorithm + mitk::AnisotropicIterativeClosestPointRegistration::Pointer aICP = + mitk::AnisotropicIterativeClosestPointRegistration::New(); aICP->SetMovingSurface(movingSurface); aICP->SetFixedSurface(fixedSurface); aICP->SetCovarianceMatricesMovingSurface(covarianceMatricesMoving); aICP->SetCovarianceMatricesFixedSurface(covarianceMatricesFixed); aICP->SetFRENormalizationFactor(FRENormalizationFactor); aICP->SetThreshold(0.0000000010); if(trimmedPart != 0.0) { aICP->SetTrimmFactor(1.0 - trimmedPart); MITK_INFO << "Algorithm is using trimmed variant."; } MITK_INFO << "Before do register"; 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."); double tre = -1; tre = mitk::AnisotropicRegistrationCommon::ComputeTargetRegistrationError( movingTargets.GetPointer(), fixedTargets.GetPointer(), aICP->GetRotation(), aICP->GetTranslation() ); - // 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."); + // 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/mitkAnisotropicRegistrationCommon.cpp b/Modules/SurfaceRegistration/mitkAnisotropicRegistrationCommon.cpp index a8355bed78..b3c297c15f 100644 --- a/Modules/SurfaceRegistration/mitkAnisotropicRegistrationCommon.cpp +++ b/Modules/SurfaceRegistration/mitkAnisotropicRegistrationCommon.cpp @@ -1,109 +1,109 @@ /*=================================================================== 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 mitk::AnisotropicRegistrationCommon::WeightMatrix -mitk::AnisotropicRegistrationCommon::CalculateWeightMatrix( const WeightMatrix &sigma_X, - const WeightMatrix &sigma_Y ) +mitk::AnisotropicRegistrationCommon::CalculateWeightMatrix(const CovarianceMatrix &sigma_X, + const CovarianceMatrix &sigma_Y ) { WeightMatrix returnValue; WeightMatrix sum = sigma_X + sigma_Y; vnl_svd svd(sum.GetVnlMatrix()); WeightMatrix diag; diag.Fill(0.0); diag[0][0] = 1.0 / sqrt(svd.W(0)); diag[1][1] = 1.0 / sqrt(svd.W(1)); diag[2][2] = 1.0 / sqrt(svd.W(2)); WeightMatrix V; //convert vnl matrix to itk matrix... for (unsigned int i = 0; i < 3; ++i) for (unsigned int j = 0; j < 3; ++j) V[i][j] = svd.V()[i][j]; //add weighting matrix for point j1 (corresponding to identity transform) returnValue = V * diag * V.GetTranspose(); return returnValue; } void mitk::AnisotropicRegistrationCommon::TransformPoints( vtkPoints *src, vtkPoints *dst, const Rotation &rotation, const Translation &translation ) { #pragma omp parallel for for ( vtkIdType i = 0; i < src->GetNumberOfPoints(); ++i ) { double p_in[3]; double p_out[3]; src->GetPoint(i,p_in); for ( unsigned int j = 0; j < 3; ++j ) { p_out[j] = p_in[0] * rotation[j][0] + p_in[1] * rotation[j][1] + p_in[2] * rotation[j][2] + translation[j]; } dst->SetPoint(i,p_out); } } void mitk::AnisotropicRegistrationCommon::PropagateMatrices( const MatrixList &src, MatrixList &dst, const Rotation &rotation ) { const vnl_matrix_fixed < double, 3, 3 > rotationT = rotation.GetTranspose(); #pragma omp parallel for for ( size_t i = 0; i < src.size(); ++i ) { dst[i] = rotation * src[i] * rotationT; } } double mitk::AnisotropicRegistrationCommon::ComputeTargetRegistrationError(const mitk::PointSet *movingTargets, const mitk::PointSet *fixedTargets, const Rotation &rotation, const Translation &translation) { double tre = 0.0; for ( int i = 0; i < movingTargets->GetSize(); ++i ) { mitk::Point3D pm = movingTargets->GetPoint(i); mitk::Point3D ps = fixedTargets->GetPoint(i); // transform point pm = rotation * pm + translation; const double dist = (ps[0] - pm[0]) * (ps[0] - pm[0]) + (ps[1] - pm[1]) * (ps[1] - pm[1]) + (ps[2] - pm[2]) * (ps[2] - pm[2]); tre += dist; } tre /= movingTargets->GetSize(); return sqrt(tre); } diff --git a/Modules/SurfaceRegistration/mitkAnisotropicRegistrationCommon.h b/Modules/SurfaceRegistration/mitkAnisotropicRegistrationCommon.h index 346a39e52a..e6ab1d7429 100644 --- a/Modules/SurfaceRegistration/mitkAnisotropicRegistrationCommon.h +++ b/Modules/SurfaceRegistration/mitkAnisotropicRegistrationCommon.h @@ -1,66 +1,134 @@ /*=================================================================== 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; + +/** + * \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: -static WeightMatrix CalculateWeightMatrix(const WeightMatrix &sigma_X, const WeightMatrix &sigma_Y ); - -static void TransformPoints(vtkPoints* src, vtkPoints* dst, - const Rotation& rotation, const Translation& translation); - -static void PropagateMatrices(const MatrixList &src, MatrixList& dst, const Rotation &rotation); - +/** @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. + */ 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 456c38b726..95425e25a2 100644 --- a/Modules/SurfaceRegistration/mitkWeightedPointTransform.cpp +++ b/Modules/SurfaceRegistration/mitkWeightedPointTransform.cpp @@ -1,453 +1,474 @@ /*=================================================================== 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; -static double ComputeWeightedFRE ( vtkPoints* X, - vtkPoints* Y, - const Matrix3x3List &CovarianceMatricesMoving, - const Matrix3x3List &CovarianceMatricesFixed, - double FRENormalizationFactor, - Matrix3x3List& WeightMatrices, - const Matrix3x3& rotation, - const itk::Vector& translation + +// 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); + 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]; - returnValue[index++] = W.at(i)[0][0] * M[0][0] + W.at(i)[0][1] * M[0][1] + W.at(i)[0][2] * M[0][2]; - returnValue[index++] = W.at(i)[1][0] * M[1][0] + W.at(i)[1][1] * M[1][1] + W.at(i)[1][2] * M[1][2]; - returnValue[index] = W.at(i)[2][0] * M[2][0] + W.at(i)[2][1] * M[2][1] + W.at(i)[2][2] * M[2][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]; + } } } bool 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); + 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); - //the old set for the next iteration is the new set of the last + // 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(); return true; } void mitk::WeightedPointTransform::SetMovingPointSet(vtkSmartPointer p) { m_MovingPointSet = p; } void mitk::WeightedPointTransform::SetCovarianceMatricesMoving(const mitk::WeightedPointTransform::CovarianceMatrixList &matrices) { m_CovarianceMatricesMoving = matrices; } void mitk::WeightedPointTransform::SetFixedPointSet(vtkSmartPointer p) { m_FixedPointSet = p; } void mitk::WeightedPointTransform::SetCovarianceMatricesFixed(const mitk::WeightedPointTransform::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); }