diff --git a/Modules/AlgorithmsExt/src/mitkAnisotropicIterativeClosestPointRegistration.cpp b/Modules/AlgorithmsExt/src/mitkAnisotropicIterativeClosestPointRegistration.cpp
index e988543f42..cea9fa2bd1 100644
--- a/Modules/AlgorithmsExt/src/mitkAnisotropicIterativeClosestPointRegistration.cpp
+++ b/Modules/AlgorithmsExt/src/mitkAnisotropicIterativeClosestPointRegistration.cpp
@@ -1,310 +1,311 @@
 /*===================================================================
 
 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 "mitkAnisotropicRegistrationCommon.h"
 #include "mitkWeightedPointTransform.h"
 #include <mitkProgressBar.h>
 #include <mitkSurface.h>
 // VTK
 #include <vtkIdList.h>
 #include <vtkKdTree.h>
 #include <vtkKdTreePointLocator.h>
 #include <vtkPoints.h>
 #include <vtkPolyData.h>
 // STL pair
 #include <utility>
+#include <omp.h>
 
 /** \brief Comperator implementation used to sort the CorrespondenceList in the
   *        trimmed version of the AnisotropicIterativeClosestPointRegistration.
   */
 struct AICPComperator
 {
   typedef std::pair<unsigned int, double> Correspondence;
 
   bool operator()(const Correspondence &a, const Correspondence &b) { return (a.second < b.second); }
 } AICPComp;
 
 mitk::AnisotropicIterativeClosestPointRegistration::AnisotropicIterativeClosestPointRegistration()
   : m_MaxIterations(1000),
     m_Threshold(0.000001),
     m_FRENormalizationFactor(1.0),
     m_SearchRadius(30.0),
     m_MaxIterationsInWeightedPointTransform(1000),
     m_FRE(0.0),
     m_TrimmFactor(0.0),
     m_NumberOfIterations(0),
     m_MovingSurface(nullptr),
     m_FixedSurface(nullptr),
     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)
+  for (int i = 0; i < X->GetNumberOfPoints(); ++i)
   {
     vtkIdType bestIdx = 0;
     mitk::Vector3D x;
     mitk::Vector3D y;
     double bestDist = std::numeric_limits<double>::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<double>::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<double>::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);
 
   // initialize the progress bar
   unsigned int steps = m_MaxIterations;
   unsigned int stepSize = m_MaxIterations / 10;
   mitk::ProgressBar::GetInstance()->AddStepsToDo(steps);
 
   do
   {
     // reset innerloop
     double currSearchRadius = m_SearchRadius;
     unsigned int radiusDoubled = 0;
 
     k = k + 1;
 
     MITK_DEBUG << "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(), 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_DEBUG << "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_DEBUG << "diff:" << diff;
     // update FRE
     m_FRE = FRE_new;
 
     // update the progressbar. Just use the half every 2nd iteration
     // to use a simulated endless progress bar since we don't have
     // a fixed amount of iterations
     stepSize = (k % 2 == 0) ? stepSize / 2 : stepSize;
     stepSize = (stepSize == 0) ? 1 : stepSize;
     mitk::ProgressBar::GetInstance()->Progress(stepSize);
 
   } while (diff > m_Threshold && k < m_MaxIterations);
 
   m_NumberOfIterations = k;
 
   // finish the progress bar if there are more steps
   // left than iterations used
   if (k < steps)
     mitk::ProgressBar::GetInstance()->Progress(steps);
 
   // free memory
   Y->Delete();
   Z->Delete();
   X->Delete();
   X_sorted->Delete();
   Z_sorted->Delete();
 }
diff --git a/Modules/AlgorithmsExt/src/mitkAnisotropicRegistrationCommon.cpp b/Modules/AlgorithmsExt/src/mitkAnisotropicRegistrationCommon.cpp
index a1ab081783..852f909836 100644
--- a/Modules/AlgorithmsExt/src/mitkAnisotropicRegistrationCommon.cpp
+++ b/Modules/AlgorithmsExt/src/mitkAnisotropicRegistrationCommon.cpp
@@ -1,104 +1,105 @@
 /*===================================================================
 
 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 <mitkAnisotropicRegistrationCommon.h>
 #include <mitkPointSet.h>
 #include <vtkPoints.h>
+#include <omp.h>
 
 mitk::AnisotropicRegistrationCommon::WeightMatrix mitk::AnisotropicRegistrationCommon::CalculateWeightMatrix(
   const CovarianceMatrix &sigma_X, const CovarianceMatrix &sigma_Y)
 {
   WeightMatrix returnValue;
 
   WeightMatrix sum = sigma_X + sigma_Y;
   vnl_svd<double> 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)
+  for (int 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 (int i = 0; i < static_cast<int>(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/AlgorithmsExt/src/mitkWeightedPointTransform.cpp b/Modules/AlgorithmsExt/src/mitkWeightedPointTransform.cpp
index 4f01709f79..377847e430 100644
--- a/Modules/AlgorithmsExt/src/mitkWeightedPointTransform.cpp
+++ b/Modules/AlgorithmsExt/src/mitkWeightedPointTransform.cpp
@@ -1,463 +1,464 @@
 /*===================================================================
 
 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 <vtkLandmarkTransform.h>
 #include <vtkMatrix4x4.h>
 #include <vtkPoints.h>
+#include <omp.h>
 
 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<double, 3> &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<double, 3> &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<vtkLandmarkTransform>::New())
 {
 }
 
 mitk::WeightedPointTransform::~WeightedPointTransform()
 {
   m_FixedPointSet = nullptr;
   m_MovingPointSet = nullptr;
   m_LandmarkTransform = nullptr;
 }
 
 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<double, 3, 3> rotation_T = rotation.GetTranspose();
 
 #pragma omp parallel for
   for (int i = 0; i < static_cast<int>(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<double, 3> &translation)
 {
   double FRE = 0;
   // compute weighting matrices
   calculateWeightMatrices(CovarianceMatricesMoving, CovarianceMatricesFixed, WeightMatrices, rotation);
 
-#pragma omp parallel
+#pragma omp parallel for
   for (int i = 0; i < static_cast<int>(WeightMatrices.size()); ++i)
   {
     // convert to itk data types (nessecary since itk 4 migration)
     itk::Vector<double, 3> 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<double, 3> 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<double, 3> D = WeightMatrices.at(i) * p;
 #pragma omp critical
     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<double, 3> &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 (int 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 (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<double>::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_DEBUG << "FRE for identity transform: " << FRE_identity;
 
   // compute isotropic transformation as initial estimate
   IsotropicRegistration(X, Y, m_LandmarkTransform, initial_TransformationR, initial_TransformationT);
 
   // result of unweighted registration algorithm
   TransformationR = initial_TransformationR;
   TransformationT = initial_TransformationT;
 
   // calculate FRE_0 with isotropic transform
   FRE_isotropic_weighted =
     ComputeWeightedFRE(X, Y, Sigma_X, Sigma_Y, m_FRENormalizationFactor, W, TransformationR, TransformationT);
   MITK_DEBUG << "FRE for transform obtained with unweighted registration: " << FRE_isotropic_weighted;
 
   // if R,t is worse than the identity, use the identity as initial transform
   if (FRE_isotropic_weighted < FRE_identity)
   {
     initialFRE = FRE_isotropic_weighted;
   }
   else
   {
     initialFRE = FRE_identity;
     TransformationR.SetIdentity(); // set rotation to identity element
     TransformationT.Fill(0.0);     // set translation to identity element
     initial_TransformationR.SetIdentity();
     initial_TransformationT.Fill(0.0);
   }
 
   // apply transform to moving set:
   mitk::AnisotropicRegistrationCommon::TransformPoints(X, X_transformed, TransformationR, TransformationT);
 
   // start with iteration 0
   n = 0;
 
   do
   {
     n++;
 
     calculateWeightMatrices(Sigma_X, Sigma_Y, W, TransformationR);
 
     //'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
     // PROBLEM:  no square matrix but the backslash operator in matlab does solve the system anyway. How to convert this
     // to C++?
     //          good descriptons to the "backslash"-operator (in german):
     //          http://www.tm-mathe.de/Themen/html/matlab__zauberstab__backslash-.html
     //                                                                    http://www.tm-mathe.de/Themen/html/matlab__matrix-division__vorsi.html#HoheMatrixA
     //
     //          current method: treat the problem as a minimization problem, because this is what the
     //          "backslash"-operator also does with "high" matrices.
     //                          (and we will have those matrices in most cases)
 
     C_maker(X_transformed, W, iA);
     E_maker(X_transformed, Y, W, iB);
 
     vnl_matrix_inverse<double> myInverse(iA.GetVnlMatrix());
     vnl_vector<double> q = myInverse.pinverse(iB.size()) * iB;
     //'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
 
     if (n > 1)
       q = (q + oldq) / 2;
     oldq = q;
 
     itk::Vector<double, 3> 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<double> 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_DEBUG << "FRE after algorithm (prior to check with initial): " << FRE;
 
   // compare with FRE_initial
   if (initialFRE < FRE)
   {
     MITK_WARN << "FRE did not improve in anisotropic point registration function";
     TransformationR = initial_TransformationR;
     TransformationT = initial_TransformationT;
     FRE = initialFRE;
   }
 
   MITK_DEBUG << "FRE final: " << FRE;
 
   X_transformed->Delete();
   X_transformedNew->Delete();
 }
 
 void mitk::WeightedPointTransform::SetMovingPointSet(vtkSmartPointer<vtkPoints> p)
 {
   m_MovingPointSet = p;
 }
 
 void mitk::WeightedPointTransform::SetCovarianceMatricesMoving(const CovarianceMatrixList &matrices)
 {
   m_CovarianceMatricesMoving = matrices;
 }
 
 void mitk::WeightedPointTransform::SetFixedPointSet(vtkSmartPointer<vtkPoints> 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);
 }