Complete Implementation of Anisotropic Landmark Transforms

Added all the necessary code for anisotropic transformations
to be performed. Added a new member to the class that the
user would have to set in order to perform anisotropic
transformations as the algorithm is iterative and needs
a stopping point.

Common/Transforms/vtkLandmarkTransform.cxx

@@ -201,7 +201,7 @@ void vtkLandmarkTransform::InternalUpdate()
       Y_transpose[i][2] = Y_translate[2][i];
     }
     double B[3][3];
-    //multiply Y_transpose with X_translate
+    //B = Y_translate' * X_translate
     for(int i=0; i < 3; i++) {
       for(int j=0; j < 3; j++) {
         B[i][j] = 0;
@@ -214,30 +214,194 @@ void vtkLandmarkTransform::InternalUpdate()
     double U[3][3];
     double V[3][3];
     double dd[3][3];
+    double Q[3][3];
     for(int i=0; i < 3; i++) {
       U[i][0] = V[i][0] = dd[i][0] = 0.0f;
       U[i][1] = V[i][1] = dd[i][1] = 0.0f;
       U[i][2] = V[i][2] = dd[i][2] = 0.0f;
     }
+
+    //perform SVD on B and S to find U and V
     vtkMath::SingularValueDecomposition3x3(B, U, S, V);
 
     double UV[3][3];
     vtkMath::Multiply3x3(U, V, UV);
     dd[2][2] = vtkMath::Determinant3x3(UV);
+    //Q = U * dd * V'
+    double V_transpose[3][3];
+    vtkMath::Transpose3x3(V, V_transpose);
+    double ddV_transpose[3][3];
+    vtkMath::Multiply3x3(dd, V_transpose, ddV_transpose);
+    vtkMath::Multiply3x3(U, ddV_transpose, Q);
+
+    //compute residual
+    //FRE_vector = X_transpose * Q' - Y_transpose
+    double Q_transpose[3][3];
+    vtkMath::transpose3x3(Q, Q_transpose);
+    double FRE_vector[3][N_PTS];
+
+    //calculate the FRE value
+    for(int i=0; i < ; i++) {
+      for(int j=0; j < ; j++) {
+        FRE_vector[i][j] = 0;
+        for(int k=0; k < ; k++) {
+          FRE_vector[i][j] += X_translate[i][k] * Q_transpose[k][i];
+        }
+      }
+    }
+    for(int i=0; i < N_PTS; i++)
+    {
+      FRE_vector[i][0] -= Y_tranlate;
+      FRE_vector[i][1] -= Y_tranlate;
+      FRE_vector[i][2] -= Y_tranlate;
+    }
+    double FRE = 0.0f;
+    for(int i=0; i < N_PTS; i++) {
+      FRE += ((FRE_vector[i][0] * FRE_vector[i][0]) +
+              (FRE_vector[i][1] * FRE_vector[i][1]) +
+              (FRE_vector[i][2] * FRE_vector[i][2]));
+    }
+    FRE = sqrt(FRE/(double)N_PTS);
 
+    FRE_orig = 2.0 * (FRE + this->threshold);
 
-    //compute first rotation
+    double QB[3][3];
+    double I[3][3];
+    double BI[3][3];
+    I[0][0] = I[0][1] = I[0][2] = 0.0f;
+    I[1][0] = I[1][1] = I[1][2] = 0.0f;
+    I[2][0] = I[2][1] = I[2][2] = 0.0f;
 
+    //iterate until the FRE value is below the threshold
+    while(fabs(FRE_orig - FRE) > threshold) {
+      //QB= Q'B
+      vtkMath::Multiply(vktMath::Transpose3x3(Q), B, QB);
+      //I = diagonal(QB)
+      I[0][0] = QB[0][0];
+      I[1][1] = QB[1][1];
+      I[2][2] = QB[2][2];
+
+      //BI = B * I
+      vktMath::Multiply3x3(B, I, BI)
+
+      vtkMath::SingleValueDecomposition(BI, U, S, V);
+      vtkMath::Multiply3x3(U, V, UV);
+      dd[2][2] = vtkMath::Determinant3x3(UV);
+
+      //Q = U * dd * V'
+      vtkMath::Multiply3x3(U, vtkMath::Multiply3x3(dd, vtkMath::Transpose3x3(V)), Q);
+
+      //compute residual
+      //FRE_vector = X_transpose * Q' - Y_transpose
+      Q_transpose[3][3];
+      vtkMath::transpose3x3(Q, Q_transpose);
+      FRE_vector[3][N_PTS];
+      FRE_orig = FRE;
+      FRE = 0.0f;
+      //calculate the FRE value
+      for(int i=0; i < ; i++) {
+        for(int j=0; j < ; j++) {
+          FRE_vector[i][j] = 0;
+          for(int k=0; k < ; k++) {
+            FRE_vector[i][j] += X_translate[i][k] * Q_transpose[k][i];
+          }
+        }
+      }
+      for(int i=0; i < N_PTS; i++)
+      {
+        FRE_vector[i][0] -= Y_tranlate;
+        FRE_vector[i][1] -= Y_tranlate;
+        FRE_vector[i][2] -= Y_tranlate;
+      }
 
-    //compute the residual
+      for(int i=0; i < N_PTS; i++) {
+        FRE += ((FRE_vector[i][0] * FRE_vector[i][0]) +
+                (FRE_vector[i][1] * FRE_vector[i][1]) +
+                (FRE_vector[i][2] * FRE_vector[i][2]));
+      }
+      FRE = sqrt(FRE/(double)N_PTS);
+    }
 
+    //recompute X_transpose
+    for(int i=0; i < N_PTS; i++)
+    {
+      X_translate[0][i] = X[0][i] - source_centroid[0];
+      X_translate[1][i] = X[1][i] - source_centroid[1];
+      X_translate[2][i] = X[2][i] - source_centroid[2];
+    }
 
-    //calculate the FRE value
+    //recompute B
+    // Y_transpose = Y_translate'
+    //B = Y_transpose * X_translate
+    for(int i=0; i < N_PTS; i++)
+    {
+      Y_transpose[i][0] = Y_translate[0][i];
+      Y_transpose[i][1] = Y_translate[1][i];
+      Y_transpose[i][2] = Y_translate[2][i];
+    }
+    for(int i=0; i < 3; i++) {
+      for(int j=0; j < 3; j++) {
+        B[i][j] = 0;
+        for(int k=0; k < N_PTS; k++) {
+          B[i][j] += Y_transpose[i][k] * X_translate[k][i];
+        }
+      }
+    }
 
+    //compute A
+    //A = (B'Q) / (X_translate' * X_translate)
+    double B_transpose[3][3];
+    vtkMath::Transpose3x3(B, B_transpose);
+    double B_transposeQ[3][3];
+    vktMath::Multiply3x3(B_transpose, Q, B_transposeQ);
+
+    double X_transposeX[3][3];
+    vtkMath::Transpose3x3(X_translate, X_transpose);
+    vtkMath::Multiply3x3(t_translate, X_Transpose, X_transposeX);
+    double A[3][3];
+    //only the diagonal
+    for(int i=0; i < 3; i++)
+    {
+      A[i][i] = B_transposeQ[i][i] / X_transposeX[i][i];
+    }
 
-    //iterate until the FRE value is below the threshold
+    //calculate the translation
+    double t[3];
+    t[0] = t[1] = t[2] = 0.0f;
+    for(int i=0; i < 3; i++)
+    {
+      t[0] += B_transposeQ[0][i];
+      t[1] += B_transposeQ[1][i];
+      t[2] += B_transposeQ[2][i];
+    }
+
+    t[0] = / 3;
+    t[1] = / 3;
+    t[2] = / 3;
 
+    //generate transformation matrix using Q, A, and t
+    //compute QA
+    double QA[3][3];
+    vtkMath::Multiply3x3(Q, A, QA);
 
+    //fill rotation into the matrix
+    for(int i=0; i < 3; i ++)
+    {
+      this->Matrix->Element[i][0] = QA[i][0];
+      this->Matrix->Element[i][1] = QA[i][1];
+      this->Matrix->Element[i][2] = QA[i][2];
+    }
+
+    //fill translation into the matrix
+    this->Matrix->Element[0][3] = t[0];
+    this->Matrix->Element[1][3] = t[1];
+    this->Matrix->Element[2][3] = t[2];
+
+    // fill the bottom row of the 4x4 matrix
+    this->Matrix->Element[3][0] = 0.0;
+    this->Matrix->Element[3][1] = 0.0;
+    this->Matrix->Element[3][2] = 0.0;
+    this->Matrix->Element[3][3] = 1.0;
   }
   else
   {
@@ -552,6 +716,11 @@ void vtkLandmarkTransform::SetTargetLandmarks(vtkPoints *target)
   this->Modified();
 }
 
+void vtkLandmarkTransform::SetAnisotropicThreshold(double threshold)
+{
+  this->Threshold = threshold;
+}
+
 //----------------------------------------------------------------------------
 void vtkLandmarkTransform::Inverse()
 {

Common/Transforms/vtkLandmarkTransform.h

@@ -60,6 +60,16 @@ public:
   vtkGetObjectMacro(TargetLandmarks, vtkPoints);
   //@}
 
+  //@{
+  /**
+   * Set the error threshold to end the iterative anisotropic mode of
+   * landmark registration. Root Mean Square Error is used to determine
+   * the error between the source and target points. The algorithm will
+   * halt when the error value is less than the threshold.
+   */
+  void SetAnisotropicThreshold(double threshold);
+  //@}
+
   //@{
   /**
    * Set the number of degrees of freedom to constrain the solution to.
@@ -119,6 +129,8 @@ protected:
   vtkPoints* TargetLandmarks;
 
   int Mode;
+
+  double Threshold;
 private:
   vtkLandmarkTransform(const vtkLandmarkTransform&) = delete;
   void operator=(const vtkLandmarkTransform&) = delete;