VTK修炼之道58：图形基本操作进阶_点云配准技术（迭代最近点ICP算法）

1.Iterative Closest Points算法

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

1.1 基本原理

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

1.2 算法步骤（四元数配准）

假设给两个三维点集 X1 和 X2，ICP方法的配准步骤如下：

第一步，计算X2中的每一个点在X1 点集中的对应近点；

第二步，求得使上述对应点对平均距离最小的刚体变换，求得平移参数和旋转参数；

第三步，对X2使用上一步求得的平移和旋转参数，得到新的变换点集；

第四步， 如果新的变换点集与参考点集满足两点集的平均距离小于某一给定阈值，则停止迭代计算，否则新的变换点集作为新的X2继续迭代，直到达到目标函数的要求。

1.3 最近点对查找

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

2.VTK中实现ICP算法实验

vtk中已经封装了最基本的ICP算法，由类vtkIterativeClosestPointTransform负责。
具体的示例代码如下所示：

#include <vtkAutoInit.h>
VTK_MODULE_INIT(vtkRenderingOpenGL);
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("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<vtkVertexGlyphFilter>  sourceGlyph =
vtkSmartPointer<vtkVertexGlyphFilter>::New();
sourceGlyph->SetInputData(source);
sourceGlyph->Update();

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

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

//进行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();

StartByMatchingCentroidsOn()该函数就是去偏移（中心归一/重心归一）。通过分别计算重心，然后平移，使得两点集中心重合。
配准后的结果如下图：



黄色点集是浮动点集；绿色点集是金标准；红色点集是经过配准矩阵调整后的点集。

2.vtkOrientationMarkerWidget类设置坐标系心得

//谨记：顺序不可以颠倒！！！
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();//开启鼠标交互

3.参看资料

1.《C++ primer》

2.《The VTK User’s Guide – 11thEdition》

3.  张晓东, 罗火灵. VTK图形图像开发进阶[M]. 机械工业出版社, 2015.