VTK：图形基本操作进阶——点云配准技术（迭代最近点ICP算法）

1.Interative Closest Points算法

点云数据配准最经典的方法是迭代最近点算法。ICP算法是一个跌代的过程，每次迭代中对于源数据点P找到目标点集Q中的最近点，然后给予最小乘原理求解当前的变换矩阵T。通过不断迭代直至收敛，即完成了点集的配准。

1. List item

基本原理
ICP算法是大多数点云配准的心，它是一个点对刚性算法。基本思想是：假设两个点集P和X近似对齐，对P上的每个点，假设X上的最近点与之对齐。采用最近点搜索，在X上找出P上各点对应的最近点，构成集合Y，然后计算一个新的P到Y刚体变换。重复上述过程直到配准收敛。

1. 算法步骤（四元数配准）
   假设给两个三维点集x1和x2,ICP方法的配准步骤如下：
   ①，计算x2中的每一个点在x1点集中的对应点；
   ②，求得使上述对应点对平均距离最小的刚体变换，求得平移参数和旋转参数；
   ③，对x2使用上一步求得的平移和旋转参数，得到新的变换点集。
   ④，如果新的变换点集与参考点集满足两点集的平均距离小于给定的某一个阈值，则停止得带计算，否则新的变换点集作为新的x2继续迭代，知道达到目标函数的要求。

3.最近点对查找
对应点的计算是整个配准过程中消耗时间最长的步骤，查找最近点，利用k-d tree 提高查找速度K-d tree法建立点的拓扑关系是基于二叉树的坐标轴分割，构造k-d tree的过程就是按照二叉树法则生成，首先X轴寻找分割线，即计算所有点的X值的平均值，一最接近这个平均值的点的x值将空间分成两部分，然后在分成的子空间中按Y轴寻找分割线，将其各分成两部分，分割好的子空间在按x轴分割…依照类推，最后知道分割的区域内只有一个点。这样的分割过程对于一个二叉树，二叉树的分节点就对应一条分割线，而二叉树的每个叶子节点就对应一个点。这样的拓扑关系就建立了。

2.VTK中实现ICP算法

vtk中已经封装了最基本的ICP算法，由类vtkInterativeClosePointTransform负责。

#include <vtkAutoInit.h>
VTK_MODULE_INIT(vtkRenderingOpenGL2);
VTK_MODULE_INIT(vtkRenderingFreeType);
VTK_MODULE_INIT(vtkInteractionStyle);

#include <vtkSmartPointer.h>
#include <vtkPolyDataReader.h>
#include <vtkPolyData.h>
#include <vtkTransform.h>
#include <vtkTransformPolyDataFilter.h>
#include <vtkVertexGlyphFilter.h>
#include <vtkIterativeClosestPointTransform.h>
#include <vtkLandmarkTransform.h>
#include <vtkTransformPolyDataFilter.h>
#include <vtkPolyDataMapper.h>
#include <vtkActor.h>
#include <vtkProperty.h>
#include <vtkAxesActor.h>
#include <vtkRenderer.h>
#include <vtkRenderWindow.h>
#include <vtkRenderWindowInteractor.h>
#include <vtkOrientationMarkerWidget.h> //坐标系交互
int main()
{
	vtkSmartPointer <vtkPolyDataReader> reader =
		vtkSmartPointer<vtkPolyDataReader>::New();
	reader->SetFileName("data/fran_cut.vtk");
	reader->Update();

	//构造浮动数据点集
	vtkSmartPointer<vtkPolyData> orig = reader->GetOutput();
	vtkSmartPointer<vtkTransform> trans =
		vtkSmartPointer<vtkTransform>::New();
	trans->Translate(0.2, 0.1, 0.1);
	trans->RotateX(10);

	vtkSmartPointer<vtkTransformPolyDataFilter> transformFilter1 =
		vtkSmartPointer<vtkTransformPolyDataFilter>::New();
	transformFilter1->SetInputData(reader->GetOutput());
	transformFilter1->SetTransform(trans);
	transformFilter1->Update();
	/*********************************************************/
	//源数据 与 目标数据
	vtkSmartPointer<vtkPolyData> source =
		vtkSmartPointer<vtkPolyData>::New();
	source->SetPoints(orig->GetPoints());

	vtkSmartPointer<vtkPolyData> target =
		vtkSmartPointer<vtkPolyData>::New();
	target->SetPoints(transformFilter1->GetOutput()->GetPoints());

	vtkSmartPointer<vtkVertexGlyphFilter>  sourceGlyph =
		vtkSmartPointer<vtkVertexGlyphFilter>::New();
	sourceGlyph->SetInputData(source);
	sourceGlyph->Update();

	vtkSmartPointer<vtkVertexGlyphFilter>  targetGlyph =
		vtkSmartPointer<vtkVertexGlyphFilter>::New();
	targetGlyph->SetInputData(target);
	targetGlyph->Update();

	//进行ICP配准求变换矩阵
	vtkSmartPointer<vtkIterativeClosestPointTransform> icptrans =
		vtkSmartPointer<vtkIterativeClosestPointTransform>::New();
	icptrans->SetSource(sourceGlyph->GetOutput());
	icptrans->SetTarget(targetGlyph->GetOutput());
	icptrans->GetLandmarkTransform()->SetModeToRigidBody();
	icptrans->SetMaximumNumberOfIterations(50);
	icptrans->StartByMatchingCentroidsOn();//去偏移（中心归一/重心归一）
	icptrans->Modified();
	icptrans->Update();

	//配准矩阵调整源数据
	vtkSmartPointer<vtkTransformPolyDataFilter> solution =
		vtkSmartPointer<vtkTransformPolyDataFilter>::New();
	solution->SetInputData(sourceGlyph->GetOutput());
	solution->SetTransform(icptrans);
	solution->Update();
	//
	vtkSmartPointer<vtkPolyDataMapper> sourceMapper =
		vtkSmartPointer<vtkPolyDataMapper>::New();
	sourceMapper->SetInputConnection(sourceGlyph->GetOutputPort());
	vtkSmartPointer<vtkActor> sourceActor =
		vtkSmartPointer<vtkActor>::New();
	sourceActor->SetMapper(sourceMapper);
	sourceActor->GetProperty()->SetColor(1, 1, 0);//设置颜色
	sourceActor->GetProperty()->SetPointSize(2);

	vtkSmartPointer<vtkPolyDataMapper> targetMapper =
		vtkSmartPointer<vtkPolyDataMapper>::New();
	targetMapper->SetInputConnection(targetGlyph->GetOutputPort());
	vtkSmartPointer<vtkActor> targetActor =
		vtkSmartPointer<vtkActor>::New();
	targetActor->SetMapper(targetMapper);
	targetActor->GetProperty()->SetColor(0, 1, 0);
	targetActor->GetProperty()->SetPointSize(3);

	vtkSmartPointer<vtkPolyDataMapper> soluMapper =
		vtkSmartPointer<vtkPolyDataMapper>::New();
	soluMapper->SetInputConnection(solution->GetOutputPort());
	vtkSmartPointer<vtkActor> soluActor =
		vtkSmartPointer<vtkActor>::New();
	soluActor->SetMapper(soluMapper);
	soluActor->GetProperty()->SetColor(1, 0, 0);
	soluActor->GetProperty()->SetPointSize(2);
	//设置坐标系
	vtkSmartPointer<vtkAxesActor> axes =
		vtkSmartPointer<vtkAxesActor>::New();
	//
	vtkSmartPointer<vtkRenderer> render =
		vtkSmartPointer<vtkRenderer>::New();
	render->AddActor(sourceActor);
	render->AddActor(targetActor);
	render->AddActor(soluActor);
	render->SetBackground(0, 0, 0);

	vtkSmartPointer<vtkRenderWindow> rw =
		vtkSmartPointer<vtkRenderWindow>::New();
	rw->AddRenderer(render);
	rw->SetSize(480, 320);
	rw->SetWindowName("Regisration by ICP");

	vtkSmartPointer<vtkRenderWindowInteractor> rwi =
		vtkSmartPointer<vtkRenderWindowInteractor>::New();
	rwi->SetRenderWindow(rw);
	/****************************************************************/
	//谨记：顺序不可以颠倒！！！
	vtkSmartPointer<vtkOrientationMarkerWidget> widget =
		vtkSmartPointer<vtkOrientationMarkerWidget>::New();
	widget->SetOutlineColor(1, 1, 1);
	widget->SetOrientationMarker(axes);
	widget->SetInteractor(rwi); //加入鼠标交互  
	widget->SetViewport(0.0, 0.0, 0.3, 0.3);  //设置显示位置  
	widget->SetEnabled(1);
	widget->InteractiveOn();//开启鼠标交互  
	/****************************************************************/
	render->ResetCamera();
	rw->Render();
	rwi->Initialize();
	rwi->Start();

	return 0;
}

首先读取一个人脸模型，为了方便测试效果，这里对原始数据做了平移转换和旋转变换。

vtkIterativeClosestPointTransform类中设置源点集和目标点集的函数为SetSource（）和SetTarget（），其输入数据类型VTKDataSet，所以集合点集必修进过一定的处理！这里使用vtkVertexGlyphFilter将读入模型和变换后的点集转换为相应的vtkPolyData数据。
vtkSmartPointer sourceGlyph =
vtkSmartPointer::New();
sourceGlyph->SetInputData(source);
sourceGlyph->Update();

vtkSmartPointer<vtkVertexGlyphFilter>  targetGlyph =
	vtkSmartPointer<vtkVertexGlyphFilter>::New();
targetGlyph->SetInputData(target);
targetGlyph->Update();

vtkLandmarkTransform类有点不同，输入数据类型就是vtkPoints类型。

//进行ICP配准求变换矩阵
vtkSmartPointer icptrans =
vtkSmartPointer::New();
icptrans->SetSource(sourceGlyph->GetOutput());
icptrans->SetTarget(targetGlyph->GetOutput());
icptrans->GetLandmarkTransform()->SetModeToRigidBody();
icptrans->SetMaximumNumberOfIterations(50);
icptrans->StartByMatchingCentroidsOn();
icptrans->Modified();
icptrans->Update();