VTK/Examples/Cxx/Filtering/IterativeClosestPointsTransform

From KitwarePublic
Jump to: navigation, search

ICP.cxx

#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>

vtkPolyData* CreatePolyData();
vtkPolyData* PerturbPolyData(vtkPolyData* polydata);
vtkPolyData* TranslatePolyData(vtkPolyData* polydata);
void WritePolyData(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
  vtkPolyData* targetPolydata = CreatePolyData();
  WritePolyData(targetPolydata, vtkstd::string("target.vtp"));
	
  //The points are perturbed 
  vtkPolyData* perturbedPolydata = PerturbPolyData(TargetPolydata);
  WritePolyData(perturbedPolydata, vtkstd::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
  vtkPolyData* sourcePolydata = TranslatePolyData(PerturbedPolydata);
  WritePolyData(sourcePolydata, vtkstd::string("source.vtp"));
		
  //setup ICP transform
  vtkIterativeClosestPointTransform* icp = vtkIterativeClosestPointTransform::New();
  icp->SetSource(sourcePolydata);
  icp->SetTarget(targetPolydata);
  icp->GetLandmarkTransform()->SetModeToRigidBody();
  //icp->DebugOn();
  icp->SetMaximumNumberOfIterations(20);
  icp->StartByMatchingCentroidsOn();
  icp->Update();

  //get the resulting transformation matrix (this matrix takes the source points to the target points)
  vtkMatrix4x4* m = icp->GetMatrix();
  vtkstd::cout << "The resulting matrix is: " << *m << vtkstd::cout;
	
  //transform the source points by the ICP solution
  vtkTransformPolyDataFilter* iCPTransFilter = vtkTransformPolyDataFilter::New();
  iCPTransFilter->SetInput(sourcePolydata);
  iCPTransFilter->SetTransform(icp);
  iCPTransFilter->Update();
	
  vtkPolyData* resultPolydata = iCPTransFilter->GetOutput();
  WritePolyData(resultPolydata, vtkstd::string("result.vtp"));
	
  //if you need to take the target points to the source points, the matrix is:
  icp->Inverse();
  vtkMatrix4x4* minv = icp->GetMatrix();
  vtkstd::cout << "The resulting inverse matrix is: " << *minv << vtkstd::cout;
  
  return 0;
}


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();
  vtkSmartPointer<vtkCellArray> sourceVertices = vtkSmartPointer<vtkCellArray>::New();
	
  //create three points and create vertices out of them
  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};
  pid[0] = sourcePoints->InsertNextPoint(origin);
  sourceVertices->InsertNextCell(1,pid);
	
  double sourcePoint1[3] = {1.0, 0.0, 0.0};
  pid[0] = sourcePoints->InsertNextPoint(sourcePoint1);
  sourceVertices->InsertNextCell(1,pid);
	
  double sourcePoint2[3] = {0.0, 1.0, 0.0};
  pid[0] = sourcePoints->InsertNextPoint(sourcePoint2);
  sourceVertices->InsertNextCell(1,pid);
	
  double sourcePoint3[3] = {0.0, 0.0, 1.0};
  pid[0] = sourcePoints->InsertNextPoint(sourcePoint3);
  sourceVertices->InsertNextCell(1,pid);
		
  //combine everything into a polydata
  vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
  polydata->SetPoints(sourcePoints);
  polydata->SetVerts(sourceVertices);
		
  return polydata;
}

vtkPolyData* PerturbPolyData(vtkPolyData* oldPolydata)
{
  vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
  polydata->DeepCopy(oldPolydata);
	
  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;
}

vtkPolyData* TranslatePolyData(vtkPolyData* oldPolydata)
{
  vtkSmartPointer<vtkPolyData> polydata = vtkSmartPointer<vtkPolyData>::New();
  polydata->DeepCopy(oldPolydata);
	
  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(vtkPolyData* polydata, const vtkstd::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)