VTK/Examples/IterativeClosestPointsTransform

#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();
 
}