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

#include

#include

#include

#include

#include

#include

#include

#include

#include

//to set

type to ridgid body

#include

#include

#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

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)

{

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是目标坐标系,也就是我给每个点加了扰动的后的坐标系)

  • 0
    点赞
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值