Medical Imaging Interaction Toolkit  2016.11.0
Medical Imaging Interaction Toolkit
All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Macros Modules Pages
mitkWeightedPointTransform.cpp
Go to the documentation of this file.
1 /*===================================================================
2 
3 The Medical Imaging Interaction Toolkit (MITK)
4 
5 Copyright (c) German Cancer Research Center,
6 Division of Medical and Biological Informatics.
7 All rights reserved.
8 
9 This software is distributed WITHOUT ANY WARRANTY; without
10 even the implied warranty of MERCHANTABILITY or FITNESS FOR
11 A PARTICULAR PURPOSE.
12 
13 See LICENSE.txt or http://www.mitk.org for details.
14 
15 ===================================================================*/
16 
17 // MITK
20 #include <vtkLandmarkTransform.h>
21 #include <vtkMatrix4x4.h>
22 #include <vtkPoints.h>
23 
24 typedef itk::Matrix<double, 3, 3> Matrix3x3;
25 typedef std::vector<Matrix3x3> Matrix3x3List;
26 
28 // forward declarations of private functions
30 
31 static double ComputeWeightedFRE(vtkPoints *X,
32  vtkPoints *Y,
33  const Matrix3x3List &CovarianceMatricesMoving,
34  const Matrix3x3List &CovarianceMatricesFixed,
35  double FRENormalizationFactor,
36  Matrix3x3List &WeightMatrices,
37  const Matrix3x3 &rotation,
38  const itk::Vector<double, 3> &translation);
39 
40 static void calculateWeightMatrices(const Matrix3x3List &X,
41  const Matrix3x3List &Y,
42  Matrix3x3List &result,
43  const Matrix3x3 &rotation);
44 
45 static void IsotropicRegistration(vtkPoints *X,
46  vtkPoints *Y,
47  vtkLandmarkTransform *landmarkTransform,
49  itk::Vector<double, 3> &translation);
50 
52  : m_Threshold(1.0e-4),
53  m_MaxIterations(1000),
54  m_Iterations(-1),
55  m_FRE(-1.0),
56  m_FRENormalizationFactor(1.0),
57  m_LandmarkTransform(vtkSmartPointer<vtkLandmarkTransform>::New())
58 {
59 }
60 
62 {
63  m_FixedPointSet = nullptr;
64  m_MovingPointSet = nullptr;
65  m_LandmarkTransform = nullptr;
66 }
67 
69 {
70  WeightedPointRegister(m_MovingPointSet,
71  m_FixedPointSet,
72  m_CovarianceMatricesMoving,
73  m_CovarianceMatricesFixed,
74  m_Threshold,
75  m_MaxIterations,
76  m_Rotation,
77  m_Translation,
78  m_FRE,
79  m_Iterations);
80 }
81 
82 // computes the weightmatrix with 2 covariance matrices
83 // and a given transformation
85  const Matrix3x3List &Y,
86  Matrix3x3List &result,
87  const Matrix3x3 &rotation)
88 {
89  const vnl_matrix_fixed<double, 3, 3> rotation_T = rotation.GetTranspose();
90 
91 #pragma omp parallel for
92  for (int i = 0; i < static_cast<int>(X.size()); ++i)
93  {
94  const Matrix3x3 w = rotation * X[i] * rotation_T;
96  }
97 }
98 
99 // computes the weighted fiducial registration error
100 double ComputeWeightedFRE(vtkPoints *X,
101  vtkPoints *Y,
102  const Matrix3x3List &CovarianceMatricesMoving,
103  const Matrix3x3List &CovarianceMatricesFixed,
104  double FRENormalizationFactor,
105  Matrix3x3List &WeightMatrices,
106  const Matrix3x3 &rotation,
107  const itk::Vector<double, 3> &translation)
108 {
109  double FRE = 0;
110  // compute weighting matrices
111  calculateWeightMatrices(CovarianceMatricesMoving, CovarianceMatricesFixed, WeightMatrices, rotation);
112 
113 #pragma omp parallel for reduction(+ : FRE)
114  for (int i = 0; i < static_cast<int>(WeightMatrices.size()); ++i)
115  {
116  // convert to itk data types (nessecary since itk 4 migration)
117  itk::Vector<double, 3> converted_MovingPoint;
118  double point[3];
119  X->GetPoint(i, point);
120  converted_MovingPoint[0] = point[0];
121  converted_MovingPoint[1] = point[1];
122  converted_MovingPoint[2] = point[2];
123 
124  // transform point
125  itk::Vector<double, 3> p = rotation * converted_MovingPoint + translation;
126 
127  Y->GetPoint(i, point);
128  p[0] -= point[0];
129  p[1] -= point[1];
130  p[2] -= point[2];
131 
132  // do calculation
133  const itk::Vector<double, 3> D = WeightMatrices.at(i) * p;
134  FRE += (D[0] * D[0] + D[1] * D[1] + D[2] * D[2]);
135  }
136 
137  FRE /= WeightMatrices.size();
138  FRE = FRENormalizationFactor * sqrt(FRE);
139 
140  return FRE;
141 }
142 
143 // registers two pointsets with an isotropic landmark transform
144 void IsotropicRegistration(vtkPoints *X,
145  vtkPoints *Y,
146  vtkLandmarkTransform *landmarkTransform,
148  itk::Vector<double, 3> &translation)
149 {
150  landmarkTransform->SetSourceLandmarks(X);
151  landmarkTransform->SetTargetLandmarks(Y);
152  landmarkTransform->SetModeToRigidBody();
153  landmarkTransform->Modified();
154  landmarkTransform->Update();
155 
156  vtkMatrix4x4 *m = landmarkTransform->GetMatrix();
157 
158  for (int i = 0; i < 3; ++i)
159  for (int j = 0; j < 3; ++j)
160  rotation[i][j] = m->GetElement(i, j);
161 
162  translation[0] = m->GetElement(0, 3);
163  translation[1] = m->GetElement(1, 3);
164  translation[2] = m->GetElement(2, 3);
165 }
166 
168  const WeightMatrixList &W,
169  itk::VariableSizeMatrix<double> &returnValue)
170 {
171 #pragma omp parallel for
172  for (vtkIdType i = 0; i < X->GetNumberOfPoints(); ++i)
173  {
174  unsigned int index = 3u * i;
175  double point[3];
176  X->GetPoint(i, point);
177 
178  for (int j = 0; j < 3; ++j)
179  {
180  returnValue[index][0] = -W.at(i)[j][1] * point[2] + W.at(i)[j][2] * point[1];
181  returnValue[index][1] = W.at(i)[j][0] * point[2] - W.at(i)[j][2] * point[0];
182  returnValue[index][2] = -W.at(i)[j][0] * point[1] + W.at(i)[j][1] * point[0];
183  returnValue[index][3] = W.at(i)[j][0];
184  returnValue[index][4] = W.at(i)[j][1];
185  returnValue[index][5] = W.at(i)[j][2];
186  index += 1;
187  }
188  }
189 }
190 
192  vtkPoints *Y,
193  const WeightMatrixList &W,
194  vnl_vector<double> &returnValue)
195 {
196 #pragma omp parallel for
197  for (vtkIdType i = 0; i < X->GetNumberOfPoints(); ++i)
198  {
199  unsigned int index = 3u * i;
200  double pX[3];
201  double pY[3];
202  Matrix3x3 M;
203 
204  X->GetPoint(i, pX);
205  Y->GetPoint(i, pY);
206 
207  M[0][0] = pY[0] - pX[0];
208  M[0][1] = pY[1] - pX[1];
209  M[0][2] = pY[2] - pX[2];
210  M[1][0] = M[0][0];
211  M[1][1] = M[0][1];
212  M[1][2] = M[0][2];
213  M[2][0] = M[0][0];
214  M[2][1] = M[0][1];
215  M[2][2] = M[0][2];
216 
217  for (unsigned int j = 0; j < 3; ++j)
218  {
219  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];
220  }
221  }
222 }
223 
225  vtkPoints *Y,
226  const CovarianceMatrixList &Sigma_X,
227  const CovarianceMatrixList &Sigma_Y,
228  double Threshold,
229  int MaxIterations,
230  Rotation &TransformationR,
231  Translation &TransformationT,
232  double &FRE,
233  int &n)
234 {
235  double FRE_identity = 0.0;
236  double FRE_isotropic_weighted = 0.0;
237  double initialFRE = 0.0;
238  // set config_change to infinite (max double) at start
239  double config_change = std::numeric_limits<double>::max();
240  Rotation initial_TransformationR;
241  initial_TransformationR.SetIdentity();
242  Translation initial_TransformationT;
243  initial_TransformationT.Fill(0.0);
244  // Weightmatrices
245  Matrix3x3List W;
246  vtkPoints *X_transformed = vtkPoints::New();
247  vtkPoints *X_transformedNew = vtkPoints::New();
248  vnl_vector<double> oldq;
249  itk::VariableSizeMatrix<double> iA;
250  vnl_vector<double> iB;
251 
252  // initialize memory
253  W.resize(X->GetNumberOfPoints());
254  X_transformed->SetNumberOfPoints(X->GetNumberOfPoints());
255  X_transformedNew->SetNumberOfPoints(X->GetNumberOfPoints());
256  iA.SetSize(3u * X->GetNumberOfPoints(), 6u);
257  iB.set_size(3u * X->GetNumberOfPoints());
258 
259  // calculate FRE_0 with identity transform
260  FRE_identity = ComputeWeightedFRE(
261  X, Y, Sigma_X, Sigma_Y, m_FRENormalizationFactor, W, initial_TransformationR, initial_TransformationT);
262 
263  MITK_DEBUG << "FRE for identity transform: " << FRE_identity;
264 
265  // compute isotropic transformation as initial estimate
266  IsotropicRegistration(X, Y, m_LandmarkTransform, initial_TransformationR, initial_TransformationT);
267 
268  // result of unweighted registration algorithm
269  TransformationR = initial_TransformationR;
270  TransformationT = initial_TransformationT;
271 
272  // calculate FRE_0 with isotropic transform
273  FRE_isotropic_weighted =
274  ComputeWeightedFRE(X, Y, Sigma_X, Sigma_Y, m_FRENormalizationFactor, W, TransformationR, TransformationT);
275  MITK_DEBUG << "FRE for transform obtained with unweighted registration: " << FRE_isotropic_weighted;
276 
277  // if R,t is worse than the identity, use the identity as initial transform
278  if (FRE_isotropic_weighted < FRE_identity)
279  {
280  initialFRE = FRE_isotropic_weighted;
281  }
282  else
283  {
284  initialFRE = FRE_identity;
285  TransformationR.SetIdentity(); // set rotation to identity element
286  TransformationT.Fill(0.0); // set translation to identity element
287  initial_TransformationR.SetIdentity();
288  initial_TransformationT.Fill(0.0);
289  }
290 
291  // apply transform to moving set:
292  mitk::AnisotropicRegistrationCommon::TransformPoints(X, X_transformed, TransformationR, TransformationT);
293 
294  // start with iteration 0
295  n = 0;
296 
297  do
298  {
299  n++;
300 
301  calculateWeightMatrices(Sigma_X, Sigma_Y, W, TransformationR);
302 
303  //'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
304  // PROBLEM: no square matrix but the backslash operator in matlab does solve the system anyway. How to convert this
305  // to C++?
306  // good descriptons to the "backslash"-operator (in german):
307  // http://www.tm-mathe.de/Themen/html/matlab__zauberstab__backslash-.html
308  // http://www.tm-mathe.de/Themen/html/matlab__matrix-division__vorsi.html#HoheMatrixA
309  //
310  // current method: treat the problem as a minimization problem, because this is what the
311  // "backslash"-operator also does with "high" matrices.
312  // (and we will have those matrices in most cases)
313 
314  C_maker(X_transformed, W, iA);
315  E_maker(X_transformed, Y, W, iB);
316 
317  vnl_matrix_inverse<double> myInverse(iA.GetVnlMatrix());
318  vnl_vector<double> q = myInverse.pinverse(iB.size()) * iB;
319  //'''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''''
320 
321  if (n > 1)
322  q = (q + oldq) / 2;
323  oldq = q;
324 
325  itk::Vector<double, 3> delta_t;
326  delta_t[0] = q[3];
327  delta_t[1] = q[4];
328  delta_t[2] = q[5];
329 
330  Matrix3x3 delta_theta;
331  delta_theta[0][0] = 1;
332  delta_theta[0][1] = -q[2];
333  delta_theta[0][2] = q[1];
334  delta_theta[1][0] = q[2];
335  delta_theta[1][1] = 1;
336  delta_theta[1][2] = -q[0];
337  delta_theta[2][0] = -q[1];
338  delta_theta[2][1] = q[0];
339  delta_theta[2][2] = 1;
340 
341  vnl_svd<double> svd_delta_theta(delta_theta.GetVnlMatrix());
342 
343  // convert vnl matrices to itk matrices...
344  Matrix3x3 U;
345  Matrix3x3 V;
346 
347  for (int i = 0; i < 3; ++i)
348  {
349  for (int j = 0; j < 3; ++j)
350  {
351  U[i][j] = svd_delta_theta.U()[i][j];
352  V[i][j] = svd_delta_theta.V()[i][j];
353  }
354  }
355 
356  Matrix3x3 delta_R = U * V.GetTranspose();
357 
358  // update rotation
359  TransformationR = delta_R * TransformationR;
360 
361  // update translation
362  TransformationT = delta_R * TransformationT + delta_t;
363 
364  // update moving points
365  mitk::AnisotropicRegistrationCommon::TransformPoints(X, X_transformedNew, TransformationR, TransformationT);
366  // calculate config change
367  config_change = CalculateConfigChange(X_transformed, X_transformedNew);
368 
369  // swap the pointers the old set for the next iteration is
370  // the new set of the last iteration
371  vtkPoints *tmp = X_transformed;
372  X_transformed = X_transformedNew;
373  X_transformedNew = tmp;
374 
375  } while (config_change > Threshold && n < MaxIterations);
376 
377  // calculate FRE with current transform
378  FRE = ComputeWeightedFRE(X, Y, Sigma_X, Sigma_Y, m_FRENormalizationFactor, W, TransformationR, TransformationT);
379 
380  MITK_DEBUG << "FRE after algorithm (prior to check with initial): " << FRE;
381 
382  // compare with FRE_initial
383  if (initialFRE < FRE)
384  {
385  MITK_WARN << "FRE did not improve in anisotropic point registration function";
386  TransformationR = initial_TransformationR;
387  TransformationT = initial_TransformationT;
388  FRE = initialFRE;
389  }
390 
391  MITK_DEBUG << "FRE final: " << FRE;
392 
393  X_transformed->Delete();
394  X_transformedNew->Delete();
395 }
396 
397 void mitk::WeightedPointTransform::SetMovingPointSet(vtkSmartPointer<vtkPoints> p)
398 {
399  m_MovingPointSet = p;
400 }
401 
402 void mitk::WeightedPointTransform::SetCovarianceMatricesMoving(const CovarianceMatrixList &matrices)
403 {
404  m_CovarianceMatricesMoving = matrices;
405 }
406 
407 void mitk::WeightedPointTransform::SetFixedPointSet(vtkSmartPointer<vtkPoints> p)
408 {
409  m_FixedPointSet = p;
410 }
411 
412 void mitk::WeightedPointTransform::SetCovarianceMatricesFixed(const CovarianceMatrixList &matrices)
413 {
414  m_CovarianceMatricesFixed = matrices;
415 }
416 
417 double mitk::WeightedPointTransform::CalculateConfigChange(vtkPoints *X, vtkPoints *X_new)
418 {
419  double sum[3] = {0.0, 0.0, 0.0};
420  double mean[3] = {0.0, 0.0, 0.0};
421  double pX[3] = {0.0, 0.0, 0.0};
422  double pX_new[3] = {0.0, 0.0, 0.0};
423 
424  // compute mean of the old point set and the first sum
425  for (vtkIdType i = 0; i < X->GetNumberOfPoints(); ++i)
426  {
427  X->GetPoint(i, pX);
428  X_new->GetPoint(i, pX_new);
429 
430  // first sum
431  sum[0] += (pX_new[0] - pX[0]) * (pX_new[0] - pX[0]);
432  sum[1] += (pX_new[1] - pX[1]) * (pX_new[1] - pX[1]);
433  sum[2] += (pX_new[2] - pX[2]) * (pX_new[2] - pX[2]);
434 
435  // mean
436  mean[0] += pX[0];
437  mean[1] += pX[1];
438  mean[2] += pX[2];
439  }
440 
441  mean[0] /= X->GetNumberOfPoints();
442  mean[1] /= X->GetNumberOfPoints();
443  mean[2] /= X->GetNumberOfPoints();
444 
445  const double us = sum[0] + sum[1] + sum[2];
446 
447  // reset sum
448  sum[0] = sum[1] = sum[2] = 0.0;
449 
450  for (vtkIdType i = 0; i < X->GetNumberOfPoints(); ++i)
451  {
452  X->GetPoint(i, pX);
453 
454  sum[0] += (pX[0] - mean[0]) * (pX[0] - mean[0]);
455  sum[1] += (pX[1] - mean[1]) * (pX[1] - mean[1]);
456  sum[2] += (pX[2] - mean[2]) * (pX[2] - mean[2]);
457  }
458 
459  const double ls = sum[0] + sum[1] + sum[2];
460 
461  return sqrt(us / ls);
462 }
double CalculateConfigChange(vtkPoints *X, vtkPoints *X_new)
void SetCovarianceMatricesMoving(const CovarianceMatrixList &matrices)
void C_maker(vtkPoints *X, const WeightMatrixList &W, itk::VariableSizeMatrix< double > &returnValue)
#define MITK_DEBUG
Definition: mitkLogMacros.h:26
static Matrix3D rotation
std::vector< Matrix3x3 > Matrix3x3List
void ComputeTransformation()
Method which registers both point sets.
#define MITK_WARN
Definition: mitkLogMacros.h:23
static void IsotropicRegistration(vtkPoints *X, vtkPoints *Y, vtkLandmarkTransform *landmarkTransform, Matrix3x3 &rotation, itk::Vector< double, 3 > &translation)
void SetMovingPointSet(vtkSmartPointer< vtkPoints > p)
void E_maker(vtkPoints *X, vtkPoints *Y, const WeightMatrixList &W, vnl_vector< double > &returnValue)
void SetCovarianceMatricesFixed(const CovarianceMatrixList &matrices)
static T max(T x, T y)
Definition: svm.cpp:70
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)
This method performs a variant of the weighted point register algorithm presented by A...
static void TransformPoints(vtkPoints *src, vtkPoints *dst, const Rotation &rotation, const Translation &translation)
Transforms a point cloud with a Rotation and Translation.
itk::Matrix< double, 3, 3 > Matrix3x3
static WeightMatrix CalculateWeightMatrix(const CovarianceMatrix &sigma_X, const CovarianceMatrix &sigma_Y)
Method that computes a WeightMatrix with two CovarianceMatrices.
static void calculateWeightMatrices(const Matrix3x3List &X, const Matrix3x3List &Y, Matrix3x3List &result, const Matrix3x3 &rotation)
void SetFixedPointSet(vtkSmartPointer< vtkPoints > p)
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 itkEventMacro(BoundingShapeInteractionEvent, itk::AnyEvent) class MITKBOUNDINGSHAPE_EXPORT BoundingShapeInteractor Pointer New()
Basic interaction methods for mitk::GeometryData.