ICP算法(Iterative Closest Point)及VTK实现

转自：http://blog.csdn.net/wangjie0377/article/details/7098446

ICP算法最初由Besl和Mckey提出，是一种基于轮廓特征的点配准方法。基准点在CT图像坐标系及世界坐标系下的坐标点集P = {P_i, i = 0,1, 2,…,k}及U = {U_i,i=0,1,2,…,n}。其中，U与P元素间不必存在一一对应关系，元素数目亦不必相同，设k ≥ n。配准过程就是求取 2 个坐标系间的旋转和平移变换矩阵，使得来自U与P的同源点间距离最小。其过程如下：

(1)计算最近点，即对于集合U中的每一个点，在集合P中都找出距该点最近的对应点，设集合P中由这些对应点组成的新点集为Q = {q_i,i = 0,1,2,…,n}。

(2)采用最小均方根法，计算点集 U 与 Q 之间的配准，使得到配准变换矩阵 R，T，其中R是 3×3 的旋转矩阵，T 是 3×1 的平移矩阵。

(3)计算坐标变换，即对于集合U，用配准变换矩阵R，T进行坐标变换，得到新的点集U₁，即U₁ = RU + T

(4)计算U₁与Q之间的均方根误差，如小于预设的极限值ε，则结束，否则，以点集U₁替换U，重复上述步骤。

VTK中有一个类vtkIterativeClosestPointTransform实现了ICP 算法，并将ICP算法保存在一个4×4的齐次矩阵中。那么如何使用这个类的函数内？以下是一个可参考的DEMO，功能是获得两个坐标系内的点之间的对应关系，也就是求这两个坐标系之间的平移和旋转矩阵。

#include <vtkMatrix4x4.h>

#include <vtkPoints.h>

#include <vtkPolyData.h>

#include <vtkLandmarkTransform.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 <iostream>

 

vtkPolyData* CreatePolyData();

vtkPolyData* PerturbPolyData(vtkPolyData* polydata);

 

int _tmain(int argc, _TCHAR* argv[])

{

    vtkPolyData* TargetPolydata = CreatePolyData();//创建目标坐标系内的点集

    //创建源坐标系内的点，实际上是通过给目标坐标系内点集加一个扰动实现的

    vtkPolyData* SourcePolydata = PerturbPolyData(TargetPolydata);

    //开始用vtkIterativeClosestPointTransform类实现 ICP算法

vtkIterativeClosestPointTransform * icp = vtkIterativeClosestPointTransform::New();

    icp->SetSource(SourcePolydata);

    icp->SetTarget(TargetPolydata);

    icp->GetLandmarkTransform()->SetModeToRigidBody();  icp->SetMaximumNumberOfIterations(20);

    icp->StartByMatchingCentroidsOn();

    icp->Modified();

    icp->Update();

 

    vtkMatrix4x4* M = icp->GetMatrix();

    std::cout << "The resulting matrix is: " << *M << std::cout;

//以下是为更方便地显示矩阵，统一了矩阵内数字显示形式，矩阵内数字形如：1.08e-001

    for(int i = 0;i<= 3;i++)

    {

        printf("\n");

        for(int j = 0;j <= 3;j++)

        {

            printf("%e\t",M->Element[i][j]);

        }

    }

    SourcePolydata->Delete();

    TargetPolydata->Delete();

    getchar();

    return 0;

}

 

vtkPolyData* CreatePolyData()

{

//This function creates a set of 5 points (the origin and a point unit distance along each axis)

    vtkPoints* SourcePoints = vtkPoints::New();

    vtkCellArray* SourceVertices = vtkCellArray::New();

    //create three points and create vertices out of them

    vtkIdType pid[1]; //记录下一个要加入的点在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] = {1.0, 1.0, 0.0};//{0.0, 0.0, 1.0};

    pid[0] = SourcePoints->InsertNextPoint(SourcePoint3);

    SourceVertices->InsertNextCell(1,pid);

    double SourcePoint4[3] = {0.5, 0.5, 0.0};//{0.0, 0.0, 1.0};

    pid[0] = SourcePoints->InsertNextPoint(SourcePoint4);

    SourceVertices->InsertNextCell(1,pid);

    vtkPolyData* polydata = vtkPolyData::New();

    polydata->SetPoints(SourcePoints); //把点导入的polydata中去

    polydata->SetVerts(SourceVertices);

    return polydata;

}

 

vtkPolyData* PerturbPolyData(vtkPolyData* OldPolydata)

{/*给每个点加入扰动，我加的扰动是把原来的点绕Y轴顺时针旋转45度（从Z轴负向看）

   其结果是第1，3，4点的x，z坐标改变了，而第0，2点坐标没变化，因为这两个点在Y轴上，绕Y轴旋转对他们没影响*/

    vtkPolyData* polydata = vtkPolyData::New();

    polydata->DeepCopy(OldPolydata);

    vtkPoints* Points = polydata->GetPoints();

    size_t Sum = Points->GetNumberOfPoints();

    double p[3];

    Points->GetPoint(1, p);

    p[0] = sqrt(2.0)/2.0;

    p[2] = sqrt(2.0)/2.0;

    Points->SetPoint(1, p);///

    Points->GetPoint(3, p);

    p[0] = sqrt(2.0)/2.0;

    p[2] = sqrt(2.0)/2.0;

    Points->SetPoint(3, p);//

    Points->GetPoint(4, p);

    p[0] = sqrt(2.0)/4.0;

    p[2] = sqrt(2.0)/4.0;

    Points->SetPoint(4, p);//

    return polydata;

}

不过VTK计算出来的矩阵好像是反的，即

Q  = RU + T （其中Q 是源坐标系，U是目标坐标系，也就是我给每个点加了扰动的后的坐标系）