1.Interative Closest Points算法
配置点云数据最经典的方法是迭代最近点算法。ICP算法是一个下降过程。在每次迭代中，源数据点P找到目标点集Q中的最近点，然后给出最小乘原理来解决当前的转换矩阵T。点集的配准是通过不断迭代直到收敛完成的。
- List item
基本原理 ICP算法是大多数点云匹配的心，是点对刚性算法。基本思想是假设两点集P和X近似对齐，假设X上的最近点对齐P上的每个点。使用最近点搜索，在X上找出P上每个点对应的最近点，形成集合Y，然后计算一个新的P到Y刚体变换。重复上述过程，直到准确收敛。 
- 算法步骤(四元数配准) 假设给两个三维点集x1和x2,ICP方法的准确步骤如下： ①，计算x2中的每一点都在x1点集中对应点； ②，平移参数和旋转参数转换，平移参数和旋转参数； ③，对x2使用上一步获得的平移和旋转参数获得新的变换点集。 ④，如果新的变换点集与参考点集满足两点集的平均距离小于给定的某一个阈值，则停止得带计算，否则新的变换点集作为新的x继续迭代，了解目标函数的要求。
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();