Work on Anisotropic Mode for ICP

Cleaned up some of the code, it works for isotropic scaling but not
anisotropic, I think the mahalanobis metric isn't working quite
properly yet.

Common/DataModel/vtkIterativeClosestPointTransform.cxx

@@ -15,12 +15,14 @@
 #include "vtkIterativeClosestPointTransform.h"
 
 #include "vtkCellLocator.h"
+#include "vtkKdTree.h"
 #include "vtkDataSet.h"
 #include "vtkLandmarkTransform.h"
 #include "vtkMath.h"
 #include "vtkObjectFactory.h"
 #include "vtkPoints.h"
 #include "vtkTransform.h"
+#include "vtkMatrix3x3.h"
 
 vtkStandardNewMacro(vtkIterativeClosestPointTransform);
 
@@ -264,7 +266,6 @@ void vtkIterativeClosestPointTransform::InternalDeepCopy(vtkAbstractTransform *t
 void vtkIterativeClosestPointTransform::InternalUpdate()
 {
   // Check source, target
-
   if (this->Source == nullptr || !this->Source->GetNumberOfPoints())
   {
     vtkErrorMacro(<<"Can't execute with nullptr or empty input");
@@ -278,14 +279,17 @@ void vtkIterativeClosestPointTransform::InternalUpdate()
   }
 
   // Create locator
-
-  this->CreateDefaultLocator();
-  this->Locator->SetDataSet(this->Target);
-  this->Locator->SetNumberOfCellsPerBucket(1);
-  this->Locator->BuildLocator();
+  if(this->AnisotropicScaling)
+  {
+    this->KdTree = vtkKdTree::New();
+  } else {
+    this->CreateDefaultLocator();
+    this->Locator->SetDataSet(this->Target);
+    this->Locator->SetNumberOfCellsPerBucket(1);
+    this->Locator->BuildLocator();
+  }
 
   // Create two sets of points to handle iteration
-
   int step = 1;
   if (this->Source->GetNumberOfPoints() > this->MaximumNumberOfLandmarks)
   {
@@ -312,8 +316,10 @@ void vtkIterativeClosestPointTransform::InternalUpdate()
   vtkPoints *points2 = vtkPoints::New();
   points2->SetNumberOfPoints(nb_points);
 
-  // Fill with initial positions (sample dataset using step)
+  vtkPoints *targetp = vtkPoints::New();
+  targetp->SetNumberOfPoints(this->Target->GetNumberOfPoints());
 
+  // Fill with initial positions (sample dataset using step)
   vtkTransform *accumulate = vtkTransform::New();
   accumulate->PostMultiply();
 
@@ -352,6 +358,7 @@ void vtkIterativeClosestPointTransform::InternalUpdate()
                           target_centroid[2] - source_centroid[2]);
     accumulate->Update();
 
+    //translate source points
     for (i = 0, j = 0; i < nb_points; i++, j += step)
     {
       double outPoint[3];
@@ -368,7 +375,7 @@ void vtkIterativeClosestPointTransform::InternalUpdate()
     }
   }
 
-  // Go
+  //Go
 
   vtkIdType cell_id;
   int sub_id;
@@ -377,30 +384,75 @@ void vtkIterativeClosestPointTransform::InternalUpdate()
 
   vtkPoints *temp, *a = points1, *b = points2;
 
-  this->NumberOfIterations = 0;
+  //anisotropic variables
+  vtkMatrix3x3 *scales = vtkMatrix3x3::New();
+  scales->Identity();
 
+  this->NumberOfIterations = 0;
   do
   {
-    // Fill points with the closest points to each vertex in input
+    //Fill points with the closest points to each vertex in input
+    if(this->AnisotropicScaling) {
+      //get scales from last iteration, identities on first iteration
+      if(this->NumberOfIterations > 0) {
+        scales = this->LandmarkTransform->GetScalingMatrix();
+      }
 
-    for(i = 0; i < nb_points; i++)
-    {
-      this->Locator->FindClosestPoint(a->GetPoint(i),
-                                      outPoint,
-                                      cell_id,
-                                      sub_id,
-                                      dist2);
-      closestp->SetPoint(i, outPoint);
+      //scale target points
+      for (i=0; i < targetp->GetNumberOfPoints(); i++)
+      {
+        double p[3];
+        this->Target->GetPoint(i, p);
+        p[0] *= sqrt(scales->GetElement(0, 0));
+        p[1] *= sqrt(scales->GetElement(1, 1));
+        p[2] *= sqrt(scales->GetElement(2, 2));
+        targetp->SetPoint(i, p);
+      }
+      this->KdTree->BuildLocatorFromPoints(targetp);
+      //scale source points and get closest points
+      for(i = 0; i < nb_points; i++)
+      {
+        double p[3];
+        a->GetPoint(i, p);
+        p[0] *= sqrt(scales->GetElement(0, 0));
+        p[1] *= sqrt(scales->GetElement(1, 1));
+        p[2] *= sqrt(scales->GetElement(2, 2));
+        cell_id = this->KdTree->FindClosestPoint(p,
+                                                 dist2);
+        this->Target->GetPoint(cell_id, p);
+        std::cout << cell_id << " , " << dist2 << std::endl;
+        closestp->SetPoint(i, p);
+      }
+    } else {
+      for(i = 0; i < nb_points; i++)
+      {
+        this->Locator->FindClosestPoint(a->GetPoint(i),
+                                        outPoint,
+                                        cell_id,
+                                        sub_id,
+                                        dist2);
+        closestp->SetPoint(i, outPoint);
+      }
     }
 
-    // Build the landmark transform
-
+    double z[3];
+    //--------------------------------------------
+    std::cout << "Transformation" << std::endl;
+    accumulate->GetMatrix()->Print(std::cout);
+    std::cout << "Landmark Transform" << std::endl;
+    this->LandmarkTransform->GetMatrix()->Print(std::cout);
+    std::cout << "Current Source" << std::endl;
+    for(int i=0; i < nb_points; i++) {a->GetPoint(i, z);std::cout << z[0] << "," << z[1] << ","  << z[2] << std::endl;}
+    std::cout << "Current Target" << std::endl;
+    for(int i=0; i < closestp->GetNumberOfPoints(); i++) {closestp->GetPoint(i, z);std::cout << z[0] << "," << z[1] << "," << z[2] << std::endl;}
+    //--------------------------------------------
+
+    //Build the landmark transform
     this->LandmarkTransform->SetSourceLandmarks(a);
     this->LandmarkTransform->SetTargetLandmarks(closestp);
     this->LandmarkTransform->Update();
 
-    // Concatenate (can't use this->Concatenate directly)
-
+    //Concatenate (can't use this->Concatenate directly)
     accumulate->Concatenate(this->LandmarkTransform->GetMatrix());
 
     this->NumberOfIterations++;
@@ -411,12 +463,12 @@ void vtkIterativeClosestPointTransform::InternalUpdate()
     }
 
     // Move mesh and compute mean distance if needed
-
     if (this->CheckMeanDistance)
     {
       totaldist = 0.0;
     }
 
+    //calculate difference between points for this iteration and previous
     for(i = 0; i < nb_points; i++)
     {
       a->GetPoint(i, p1);
@@ -433,6 +485,7 @@ void vtkIterativeClosestPointTransform::InternalUpdate()
       }
     }
 
+    //check if distance between points is small enough to end
     if (this->CheckMeanDistance)
     {
       if (this->MeanDistanceMode == VTK_ICP_MODE_RMS)
@@ -448,6 +501,8 @@ void vtkIterativeClosestPointTransform::InternalUpdate()
       }
     }
 
+    //swap current set of points and previous
+    //previous will be written over by next iteration
     temp = a;
     a = b;
     b = temp;

Common/DataModel/vtkIterativeClosestPointTransform.h

@@ -45,6 +45,7 @@
 #define VTK_ICP_MODE_RMS 0
 #define VTK_ICP_MODE_AV 1
 
+class vtkKdTree;
 class vtkCellLocator;
 class vtkLandmarkTransform;
 class vtkDataSet;
@@ -155,6 +156,16 @@ public:
   vtkBooleanMacro(StartByMatchingCentroids, vtkTypeBool);
   //@}
 
+  //@{
+  /**
+   * Calculate Anisotropic scaling factors in the transformation.
+   * The default is Off.
+   */
+  vtkSetMacro(AnisotropicScaling, vtkTypeBool);
+  vtkGetMacro(AnisotropicScaling, vtkTypeBool);
+  vtkBooleanMacro(AnisotropicScaling, vtkTypeBool);
+  //@}
+
   //@{
   /**
    * Get the internal landmark transform. Use it to constrain the number of
@@ -212,12 +223,14 @@ protected:
   vtkDataSet* Source;
   vtkDataSet* Target;
   vtkCellLocator *Locator;
+  vtkKdTree *KdTree;
   int MaximumNumberOfIterations;
   vtkTypeBool CheckMeanDistance;
   int MeanDistanceMode;
   double MaximumMeanDistance;
   int MaximumNumberOfLandmarks;
   vtkTypeBool StartByMatchingCentroids;
+  vtkTypeBool AnisotropicScaling;
 
   int NumberOfIterations;
   double MeanDistance;

Common/Transforms/vtkLandmarkTransform.cxx

@@ -30,6 +30,12 @@ vtkLandmarkTransform::vtkLandmarkTransform()
   this->Mode = VTK_LANDMARK_SIMILARITY;
   this->SourceLandmarks=nullptr;
   this->TargetLandmarks=nullptr;
+  this->Matrix->Identity();
+  this->RotationMatrix = vtkMatrix3x3::New();
+  this->ScalingMatrix = vtkMatrix3x3::New();
+  this->RotationMatrix->Identity();
+  this->ScalingMatrix->Identity();
+  this->Threshold = 1e-9;
 }
 
 //----------------------------------------------------------------------------
@@ -43,6 +49,12 @@ vtkLandmarkTransform::~vtkLandmarkTransform()
   {
     this->TargetLandmarks->Delete();
   }
+  if(this->RotationMatrix) {
+    this->RotationMatrix->Delete();
+  }
+  if(this->ScalingMatrix) {
+    this->RotationMatrix->Delete();
+  }
 }
 
 //----------------------------------------------------------------------------
@@ -213,6 +225,20 @@ void vtkLandmarkTransform::InternalUpdate()
     //calculate final translation
     Eigen::Vector3d t =  Y_centroid - (Q * A * X_centroid);
 
+    //set scaling and rotation matrices sperate to regular
+    //transformation matrix output
+    this->RotationMatrix->Identity();
+    this->ScalingMatrix->Identity();
+    for(int i=0; i < 3; i ++)
+    {
+      this->RotationMatrix->SetElement(i, 0, Q(i, 0));
+      this->RotationMatrix->SetElement(i, 1, Q(i, 1));
+      this->RotationMatrix->SetElement(i, 2 ,Q(i, 2));
+    }
+    this->ScalingMatrix->SetElement(0, 0, A(0, 0));
+    this->ScalingMatrix->SetElement(1, 1, A(1, 1));
+    this->ScalingMatrix->SetElement(2, 2, A(2, 2));
+
     Q = Q * A;
 
     //fill rotation into the matrix

Common/Transforms/vtkLandmarkTransform.h

@@ -34,6 +34,7 @@
 
 #include "vtkCommonTransformsModule.h" // For export macro
 #include "vtkLinearTransform.h"
+#include "vtkMatrix3x3.h"
 
 #define VTK_LANDMARK_RIGIDBODY 6
 #define VTK_LANDMARK_SIMILARITY 7
@@ -97,6 +98,20 @@ public:
   const char *GetModeAsString();
   //@}
 
+  //@{
+  /**
+   * Get the rotation matrix independent of scaling.
+   */
+  vtkGetObjectMacro(RotationMatrix, vtkMatrix3x3);
+  //@}
+
+  //@{
+  /**
+   * Get the scaling matrix independent of rotation.
+   */
+  vtkGetObjectMacro(ScalingMatrix, vtkMatrix3x3);
+  //@}
+
   /**
    * Invert the transformation.  This is done by switching the
    * source and target landmarks.
@@ -128,6 +143,9 @@ protected:
   vtkPoints* SourceLandmarks;
   vtkPoints* TargetLandmarks;
 
+  vtkMatrix3x3 *ScalingMatrix;
+  vtkMatrix3x3 *RotationMatrix;
+
   int Mode;
 
   double Threshold;