Difference between revisions of "VTK/Examples/Cxx/Filtering/IterativeClosestPointsTransform"

From KitwarePublic
< VTK‎ | Examples‎ | Cxx
Jump to navigationJump to search
(major cleanup)
Line 1: Line 1:
 
== ICP.cxx ==
 
== ICP.cxx ==
 
<source lang="cpp">
 
<source lang="cpp">
 +
#include <vtkSmartPointer.h>
 +
#include <vtkVertexGlyphFilter.h>
 
#include <vtkPoints.h>
 
#include <vtkPoints.h>
 
#include <vtkPolyData.h>
 
#include <vtkPolyData.h>
Line 11: Line 13:
 
#include <vtkXMLPolyDataWriter.h>
 
#include <vtkXMLPolyDataWriter.h>
  
vtkPolyData* CreatePolyData();
+
vtkSmartPointer<vtkPolyData> CreatePolyData();
vtkPolyData* PerturbPolyData(vtkPolyData* polydata);
+
vtkSmartPointer<vtkPolyData> PerturbPolyData(vtkSmartPointer<vtkPolyData> polydata);
vtkPolyData* TranslatePolyData(vtkPolyData* polydata);
+
vtkSmartPointer<vtkPolyData> TranslatePolyData(vtkSmartPointer<vtkPolyData> polydata);
void WritePolyData(vtkPolyData* polydata, const std::string &Filename);
+
void WritePolyData(vtkSmartPointer<vtkPolyData> polydata, const std::string &Filename);
  
 
int main(int argc, char *argv[])
 
int main(int argc, char *argv[])
Line 21: Line 23:
 
   This demo produces target points which are at the origin and unit length along each axis. It then perturbs the points and shifts each of them .3 in +y direction - the resulting points are the "source" points. It then attempts to move the source points as close as possible to the target points. The noise is added to make the example more realistic. Also, the noise ensures nothing was done wrong (i.e. accidentally using the target points as the result and claiming it worked perfectly when in fact nothing happened!)
 
   This demo produces target points which are at the origin and unit length along each axis. It then perturbs the points and shifts each of them .3 in +y direction - the resulting points are the "source" points. It then attempts to move the source points as close as possible to the target points. The noise is added to make the example more realistic. Also, the noise ensures nothing was done wrong (i.e. accidentally using the target points as the result and claiming it worked perfectly when in fact nothing happened!)
 
   */
 
   */
+
 
 
   //this is the "target" or "correct" point set
 
   //this is the "target" or "correct" point set
   vtkPolyData* targetPolydata = CreatePolyData();
+
   vtkSmartPointer<vtkPolyData> targetPolydata = CreatePolyData();
   WritePolyData(targetPolydata, vtkstd::string("target.vtp"));
+
   WritePolyData(targetPolydata, std::string("target.vtp"));
+
 
 
   //The points are perturbed  
 
   //The points are perturbed  
   vtkPolyData* perturbedPolydata = PerturbPolyData(TargetPolydata);
+
   vtkSmartPointer<vtkPolyData> perturbedPolydata = PerturbPolyData(targetPolydata);
   WritePolyData(perturbedPolydata, vtkstd::string("target_perturbed.vtp"));
+
   WritePolyData(perturbedPolydata, std::string("target_perturbed.vtp"));
+
 
 
   //this is the "source" point set - the one that we wish to align as closely as possible with the "target" point set
 
   //this is the "source" point set - the one that we wish to align as closely as possible with the "target" point set
   vtkPolyData* sourcePolydata = TranslatePolyData(PerturbedPolydata);
+
   vtkSmartPointer<vtkPolyData> sourcePolydata = TranslatePolyData(perturbedPolydata);
   WritePolyData(sourcePolydata, vtkstd::string("source.vtp"));
+
   WritePolyData(sourcePolydata, std::string("source.vtp"));
+
 
 
   //setup ICP transform
 
   //setup ICP transform
   vtkIterativeClosestPointTransform* icp = vtkIterativeClosestPointTransform::New();
+
   vtkSmartPointer<vtkIterativeClosestPointTransform> icp =  
 +
      vtkSmartPointer<vtkIterativeClosestPointTransform>::New();
 
   icp->SetSource(sourcePolydata);
 
   icp->SetSource(sourcePolydata);
 
   icp->SetTarget(targetPolydata);
 
   icp->SetTarget(targetPolydata);
Line 42: Line 45:
 
   icp->SetMaximumNumberOfIterations(20);
 
   icp->SetMaximumNumberOfIterations(20);
 
   icp->StartByMatchingCentroidsOn();
 
   icp->StartByMatchingCentroidsOn();
 +
  icp->Modified();
 
   icp->Update();
 
   icp->Update();
 
+
 
 
   //get the resulting transformation matrix (this matrix takes the source points to the target points)
 
   //get the resulting transformation matrix (this matrix takes the source points to the target points)
   vtkMatrix4x4* m = icp->GetMatrix();
+
   vtkSmartPointer<vtkMatrix4x4> M = icp->GetMatrix();
   vtkstd::cout << "The resulting matrix is: " << *m << vtkstd::cout;
+
   cout << "The resulting matrix is: " << *M << cout;
+
 
 
   //transform the source points by the ICP solution
 
   //transform the source points by the ICP solution
   vtkTransformPolyDataFilter* iCPTransFilter = vtkTransformPolyDataFilter::New();
+
   vtkSmartPointer<vtkTransformPolyDataFilter> iCPTransFilter =  
 +
      vtkSmartPointer<vtkTransformPolyDataFilter>::New();
 
   iCPTransFilter->SetInput(sourcePolydata);
 
   iCPTransFilter->SetInput(sourcePolydata);
 
   iCPTransFilter->SetTransform(icp);
 
   iCPTransFilter->SetTransform(icp);
 
   iCPTransFilter->Update();
 
   iCPTransFilter->Update();
+
 
   vtkPolyData* resultPolydata = iCPTransFilter->GetOutput();
+
   vtkSmartPointer<vtkPolyData> resultPolydata = iCPTransFilter->GetOutput();
   WritePolyData(resultPolydata, vtkstd::string("result.vtp"));
+
   WritePolyData(resultPolydata, std::string("result.vtp"));
+
 
 
   //if you need to take the target points to the source points, the matrix is:
 
   //if you need to take the target points to the source points, the matrix is:
 
   icp->Inverse();
 
   icp->Inverse();
   vtkMatrix4x4* minv = icp->GetMatrix();
+
   vtkSmartPointer<vtkMatrix4x4> minv = icp->GetMatrix();
   vtkstd::cout << "The resulting inverse matrix is: " << *minv << vtkstd::cout;
+
   cout << "The resulting inverse matrix is: " << *minv << cout;
 
    
 
    
   return 0;
+
   return EXIT_SUCCESS;
 
}
 
}
  
  
vtkPolyData* CreatePolyData()
+
vtkSmartPointer<vtkPolyData> CreatePolyData()
 
{
 
{
 
   //This function creates a set of 4 points (the origin and a point unit distance along each axis)
 
   //This function creates a set of 4 points (the origin and a point unit distance along each axis)
 
+
 
   vtkSmartPointer<vtkPoints> sourcePoints = vtkSmartPointer<vtkPoints>::New();
+
   vtkSmartPointer<vtkPoints> sourcePoints =  
   vtkSmartPointer<vtkCellArray> sourceVertices = vtkSmartPointer<vtkCellArray>::New();
+
      vtkSmartPointer<vtkPoints>::New();
+
    
   //create three points and create vertices out of them
+
   //create three points
  vtkIdType pid[1]; //this is needed to store the resulting point ids when adding them to the vtkPoints
 
 
 
   double origin[3] = {0.0, 0.0, 0.0};
 
   double origin[3] = {0.0, 0.0, 0.0};
   pid[0] = sourcePoints->InsertNextPoint(origin);
+
   sourcePoints->InsertNextPoint(origin);
  sourceVertices->InsertNextCell(1,pid);
 
 
 
   double sourcePoint1[3] = {1.0, 0.0, 0.0};
 
   double sourcePoint1[3] = {1.0, 0.0, 0.0};
   pid[0] = sourcePoints->InsertNextPoint(sourcePoint1);
+
   sourcePoints->InsertNextPoint(sourcePoint1);
  sourceVertices->InsertNextCell(1,pid);
 
 
 
   double sourcePoint2[3] = {0.0, 1.0, 0.0};
 
   double sourcePoint2[3] = {0.0, 1.0, 0.0};
   pid[0] = sourcePoints->InsertNextPoint(sourcePoint2);
+
   sourcePoints->InsertNextPoint(sourcePoint2);
  sourceVertices->InsertNextCell(1,pid);
 
 
 
   double sourcePoint3[3] = {0.0, 0.0, 1.0};
 
   double sourcePoint3[3] = {0.0, 0.0, 1.0};
   pid[0] = sourcePoints->InsertNextPoint(sourcePoint3);
+
   sourcePoints->InsertNextPoint(sourcePoint3);
  sourceVertices->InsertNextCell(1,pid);
+
   
+
   vtkSmartPointer<vtkPolyData> polydata =  
  //combine everything into a polydata
+
      vtkSmartPointer<vtkPolyData>::New();
   vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
 
 
   polydata->SetPoints(sourcePoints);
 
   polydata->SetPoints(sourcePoints);
   polydata->SetVerts(sourceVertices);
+
    
+
  vtkSmartPointer<vtkVertexGlyphFilter> vertexFilter =
   return polydata;
+
      vtkSmartPointer<vtkVertexGlyphFilter>::New();
 +
  vertexFilter->SetInputConnection(polydata->GetProducerPort());
 +
  vertexFilter->Update();
 +
 
 +
   return vertexFilter->GetOutput();
 
}
 
}
  
vtkPolyData* PerturbPolyData(vtkPolyData* oldPolydata)
+
vtkSmartPointer<vtkPolyData> PerturbPolyData(vtkSmartPointer<vtkPolyData> oldPolydata)
 
{
 
{
   vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
+
   vtkSmartPointer<vtkPolyData> polydata =  
 +
      vtkSmartPointer<vtkPolyData>::New();
 
   polydata->DeepCopy(oldPolydata);
 
   polydata->DeepCopy(oldPolydata);
+
 
   vtkPoints* points = polydata->GetPoints();
+
   vtkSmartPointer<vtkPoints> points = polydata->GetPoints();
+
 
 
   vtkMath::RandomSeed(time(NULL));
 
   vtkMath::RandomSeed(time(NULL));
+
 
 
   for(unsigned int i = 0; i < points->GetNumberOfPoints(); i++)
 
   for(unsigned int i = 0; i < points->GetNumberOfPoints(); i++)
    {
+
  {
    double p[3];
+
  double p[3];
    points->GetPoint(i, p);
+
  points->GetPoint(i, p);
    for(unsigned int j = 0; j < 3; j++)
+
  for(unsigned int j = 0; j < 3; j++)
      {
+
    {
      double pointNoise = vtkMath::Random(-0.1, 0.1);
+
  double pointNoise = vtkMath::Random(-0.1, 0.1);
      p[j] = p[j] + pointNoise;
+
  p[j] = p[j] + pointNoise;
      }
+
    }
      points->SetPoint(i, p);
+
  points->SetPoint(i, p);
    }
+
  }
+
 
 
   return polydata;
 
   return polydata;
 
}
 
}
  
vtkPolyData* TranslatePolyData(vtkPolyData* oldPolydata)
+
vtkSmartPointer<vtkPolyData> TranslatePolyData(vtkSmartPointer<vtkPolyData> oldPolydata)
 
{
 
{
   vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
+
   vtkSmartPointer<vtkPolyData> polydata =  
 +
      vtkSmartPointer<vtkPolyData>::New();
 
   polydata->DeepCopy(oldPolydata);
 
   polydata->DeepCopy(oldPolydata);
+
 
   vtkPoints* points = polydata->GetPoints();
+
   vtkSmartPointer<vtkPoints> points = polydata->GetPoints();
+
 
 
   //shift all of the points in the y direction + 0.3
 
   //shift all of the points in the y direction + 0.3
 
   for(unsigned int i = 0; i < points->GetNumberOfPoints(); i++)
 
   for(unsigned int i = 0; i < points->GetNumberOfPoints(); i++)
    {
+
  {
    double p[3];
+
  double p[3];
    points->GetPoint(i, p);
+
  points->GetPoint(i, p);
    p[1] = p[1] + 0.3;
+
  p[1] = p[1] + 0.3;
    points->SetPoint(i, p);
+
 
    }
+
  points->SetPoint(i, p);
+
  }
 +
 
 
   return polydata;
 
   return polydata;
 
}
 
}
  
void WritePolyData(vtkPolyData* polydata, const vtkstd::string &filename)
+
void WritePolyData(vtkSmartPointer<vtkPolyData> polydata, const std::string &filename)
 
{
 
{
   vtkSmartPointer<vtkXMLPolyDataWriter> writer = vtkSmartPointer<vtkXMLPolyDataWriter>::New();
+
   vtkSmartPointer<vtkXMLPolyDataWriter> writer =  
 +
      vtkSmartPointer<vtkXMLPolyDataWriter>::New();
 
   writer->SetFileName(filename.c_str());
 
   writer->SetFileName(filename.c_str());
 
   writer->SetInput(polydata);
 
   writer->SetInput(polydata);
Line 154: Line 158:
  
 
== CMakeLists.txt ==
 
== CMakeLists.txt ==
 
+
<source lang="cmake">
<source lang="text">
 
 
PROJECT(ICP)
 
PROJECT(ICP)
 
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
 
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)

Revision as of 16:16, 19 February 2010

ICP.cxx

#include <vtkSmartPointer.h>
#include <vtkVertexGlyphFilter.h>
#include <vtkPoints.h>
#include <vtkPolyData.h>
#include <vtkCellArray.h>
#include <vtkIterativeClosestPointTransform.h>
#include <vtkTransformPolyDataFilter.h>
#include <vtkLandmarkTransform.h> //to set type to ridgid body
#include <vtkMath.h>
#include <vtkMatrix4x4.h>
#include <vtkXMLPolyDataWriter.h>

vtkSmartPointer<vtkPolyData> CreatePolyData();
vtkSmartPointer<vtkPolyData> PerturbPolyData(vtkSmartPointer<vtkPolyData> polydata);
vtkSmartPointer<vtkPolyData> TranslatePolyData(vtkSmartPointer<vtkPolyData> polydata);
void WritePolyData(vtkSmartPointer<vtkPolyData> polydata, const std::string &Filename);

int main(int argc, char *argv[])
{
  /*
  This demo produces target points which are at the origin and unit length along each axis. It then perturbs the points and shifts each of them .3 in +y direction - the resulting points are the "source" points. It then attempts to move the source points as close as possible to the target points. The noise is added to make the example more realistic. Also, the noise ensures nothing was done wrong (i.e. accidentally using the target points as the result and claiming it worked perfectly when in fact nothing happened!)
  */
  
  //this is the "target" or "correct" point set
  vtkSmartPointer<vtkPolyData> targetPolydata = CreatePolyData();
  WritePolyData(targetPolydata, std::string("target.vtp"));
  
  //The points are perturbed 
  vtkSmartPointer<vtkPolyData> perturbedPolydata = PerturbPolyData(targetPolydata);
  WritePolyData(perturbedPolydata, std::string("target_perturbed.vtp"));
  
  //this is the "source" point set - the one that we wish to align as closely as possible with the "target" point set
  vtkSmartPointer<vtkPolyData> sourcePolydata = TranslatePolyData(perturbedPolydata);
  WritePolyData(sourcePolydata, std::string("source.vtp"));
	  
  //setup ICP transform
  vtkSmartPointer<vtkIterativeClosestPointTransform> icp = 
      vtkSmartPointer<vtkIterativeClosestPointTransform>::New();
  icp->SetSource(sourcePolydata);
  icp->SetTarget(targetPolydata);
  icp->GetLandmarkTransform()->SetModeToRigidBody();
  //icp->DebugOn();
  icp->SetMaximumNumberOfIterations(20);
  icp->StartByMatchingCentroidsOn();
  icp->Modified();
  icp->Update();
  
  //get the resulting transformation matrix (this matrix takes the source points to the target points)
  vtkSmartPointer<vtkMatrix4x4> M = icp->GetMatrix();
  cout << "The resulting matrix is: " << *M << cout;
  
  //transform the source points by the ICP solution
  vtkSmartPointer<vtkTransformPolyDataFilter> iCPTransFilter = 
      vtkSmartPointer<vtkTransformPolyDataFilter>::New();
  iCPTransFilter->SetInput(sourcePolydata);
  iCPTransFilter->SetTransform(icp);
  iCPTransFilter->Update();
  
  vtkSmartPointer<vtkPolyData> resultPolydata = iCPTransFilter->GetOutput();
  WritePolyData(resultPolydata, std::string("result.vtp"));
  
  //if you need to take the target points to the source points, the matrix is:
  icp->Inverse();
  vtkSmartPointer<vtkMatrix4x4> minv = icp->GetMatrix();
  cout << "The resulting inverse matrix is: " << *minv << cout;
  
  return EXIT_SUCCESS;
}


vtkSmartPointer<vtkPolyData> CreatePolyData()
{
  //This function creates a set of 4 points (the origin and a point unit distance along each axis)
  
  vtkSmartPointer<vtkPoints> sourcePoints = 
      vtkSmartPointer<vtkPoints>::New();
  
  //create three points
  double origin[3] = {0.0, 0.0, 0.0};
  sourcePoints->InsertNextPoint(origin);
  double sourcePoint1[3] = {1.0, 0.0, 0.0};
  sourcePoints->InsertNextPoint(sourcePoint1);
  double sourcePoint2[3] = {0.0, 1.0, 0.0};
  sourcePoints->InsertNextPoint(sourcePoint2);
  double sourcePoint3[3] = {0.0, 0.0, 1.0};
  sourcePoints->InsertNextPoint(sourcePoint3);
    
  vtkSmartPointer<vtkPolyData> polydata = 
      vtkSmartPointer<vtkPolyData>::New();
  polydata->SetPoints(sourcePoints);
  
  vtkSmartPointer<vtkVertexGlyphFilter> vertexFilter = 
      vtkSmartPointer<vtkVertexGlyphFilter>::New();
  vertexFilter->SetInputConnection(polydata->GetProducerPort());
  vertexFilter->Update();
  
  return vertexFilter->GetOutput();
}

vtkSmartPointer<vtkPolyData> PerturbPolyData(vtkSmartPointer<vtkPolyData> oldPolydata)
{
  vtkSmartPointer<vtkPolyData> polydata = 
      vtkSmartPointer<vtkPolyData>::New();
  polydata->DeepCopy(oldPolydata);
  
  vtkSmartPointer<vtkPoints> points = polydata->GetPoints();
  
  vtkMath::RandomSeed(time(NULL));
  
  for(unsigned int i = 0; i < points->GetNumberOfPoints(); i++)
  {
	  double p[3];
	  points->GetPoint(i, p);
	  for(unsigned int j = 0; j < 3; j++)
	    {
		  double pointNoise = vtkMath::Random(-0.1, 0.1);
		  p[j] = p[j] + pointNoise;
	    }
	  points->SetPoint(i, p);
  }
  
  return polydata;
}

vtkSmartPointer<vtkPolyData> TranslatePolyData(vtkSmartPointer<vtkPolyData> oldPolydata)
{
  vtkSmartPointer<vtkPolyData> polydata = 
      vtkSmartPointer<vtkPolyData>::New();
  polydata->DeepCopy(oldPolydata);
  
  vtkSmartPointer<vtkPoints> points = polydata->GetPoints();
  
  //shift all of the points in the y direction + 0.3
  for(unsigned int i = 0; i < points->GetNumberOfPoints(); i++)
	  {
	  double p[3];
	  points->GetPoint(i, p);
	  p[1] = p[1] + 0.3;
  
	  points->SetPoint(i, p);
  }
  
  return polydata;
}

void WritePolyData(vtkSmartPointer<vtkPolyData> polydata, const std::string &filename)
{
  vtkSmartPointer<vtkXMLPolyDataWriter> writer = 
      vtkSmartPointer<vtkXMLPolyDataWriter>::New();
  writer->SetFileName(filename.c_str());
  writer->SetInput(polydata);
  writer->Write();
	
}

CMakeLists.txt

PROJECT(ICP)
CMAKE_MINIMUM_REQUIRED(VERSION 2.6)
 
FIND_PACKAGE(VTK REQUIRED)
INCLUDE(${VTK_USE_FILE})
 
ADD_EXECUTABLE(ICP ICP.cxx)
TARGET_LINK_LIBRARIES(ICP vtkHybrid)