diff --git a/Modules/SurfaceRegistration/mitkAnisotropicRegistrationCommon.h b/Modules/SurfaceRegistration/mitkAnisotropicRegistrationCommon.h index e6ab1d7429..20e7774c18 100644 --- a/Modules/SurfaceRegistration/mitkAnisotropicRegistrationCommon.h +++ b/Modules/SurfaceRegistration/mitkAnisotropicRegistrationCommon.h @@ -1,134 +1,135 @@ /*=================================================================== 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. */ static double ComputeTargetRegistrationError( const mitk::PointSet* movingTargets, const mitk::PointSet* fixedTargets, const Rotation& rotation, const Translation& translation ); }; } #endif diff --git a/Modules/SurfaceRegistration/mitkCovarianceMatrixCalculator.cpp b/Modules/SurfaceRegistration/mitkCovarianceMatrixCalculator.cpp index d608d37eb4..9f88b6ee30 100644 --- a/Modules/SurfaceRegistration/mitkCovarianceMatrixCalculator.cpp +++ b/Modules/SurfaceRegistration/mitkCovarianceMatrixCalculator.cpp @@ -1,406 +1,407 @@ /*=================================================================== 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 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 { 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 +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; } 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; } 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; } 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(); } 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 a75d6d53c7..6caf5af2a9 100644 --- a/Modules/SurfaceRegistration/mitkCovarianceMatrixCalculator.h +++ b/Modules/SurfaceRegistration/mitkCovarianceMatrixCalculator.h @@ -1,110 +1,113 @@ /*=================================================================== 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 { class Surface; struct CovarianceMatrixCalculatorData; /** * \ingroup AnisotropicRegistration * - * Computes covariance matrices for every vertex in a given {@link Surface} based on it's - * direct neighbours. Three different computation are available: - *
    - *
  • CM_PCA computes a covariance matrix based a principal component analysis - * of the adjacent vertices. - *
  • CM_VORONOI gets a covariance matrix based on voronoi regions. - *
  • CM_TOF a covariance matrix that represents the error in the viewing - * of a time of flight camera. - *
+ * @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. + * The Class implements the CM_PCA method presented and developped 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. */ 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]; CovarianceMatrixCalculatorData* d; 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>. + * @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); /** Calculates covariance matrices. * @throws std::exception If the input surface is not set. */ void ComputeCovarianceMatrices(); }; } #endif