转载而来 ,自己学的医学图像 ,所以算法原理尚可借鉴,这篇原理讲的很不错 网上搜了很多 始终不明白 似乎这次能知道个来龙去脉了。非常感谢该版主~~~
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 <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("%et",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是目标坐标系,也就是我给每个点加了扰动的后的坐标系)
我是照着http://www.vtk.org/Wiki/Iterative_Closest_Points_(ICP)_Transform内的源程序改写出以上代码的,去掉了原来的随机扰动等部分,取的点很简单很容易验证算出来的旋转和平移矩阵是否正确。