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

转载而来，自己学的医学图像

，所以算法原理尚可借鉴，这篇原理讲的很不错网上搜了很多始终不明白似乎这次能知道个来龙去脉了。非常感谢该版主~~~

ICP算法最初由Besl和Mckey提出，是一种基于轮廓特征的点配准方法。基准点在CT图像坐标系及世界坐标系下的坐标点集P =

{Pi, i = 0,1, 2,…,k}及U =

{Ui,i=0,1,2,…,n}。其中，U与P元素间不必存在一一对应关系，元素数目亦不必相同，设k ≥ n。配准过程就是求取 2

个坐标系间的旋转和平移变换矩阵，使得来自U与P的同源点间距离最小。其过程如下：

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

= 0,1,2,…,n}。

(2)采用最小均方根法，计算点集 U 与 Q 之间的配准，使得到配准变换矩阵

R，T，其中R是 3×3 的旋转矩阵，T 是 3×1 的平移矩阵。

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

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

VTK中有一个类vtkIterativeClosestPointTransform实现了ICP

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

#include

//to set

type to ridgid body

#include

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

}

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);