1.Iterative Closest Points算法

點云數(shù)據(jù)配準最經(jīng)典的方法是迭代最近點算法（Iterative Closest Points,ICP）。ICP算法是一個迭代的過程，每次迭代中對于源數(shù)據(jù)點P找到目標點集Q中的最近點，然后給予最小二乘原理求解當前的變換矩陣T。通過不斷迭代迭代直至收斂，即完成了點集的配準。

1.1 基本原理

ICP算法是大多數(shù)點云配準算法的心， 它是一個點對剛性算法?；舅枷胧?#xff1a; 假設兩個點集 P和 X近似對齊， 對 P上的每個點，假設 X上的最近點與之對齊。采用最近點搜索， 在 X上找出 P上各個點對應的最近點， 構成集合 Y， 然后計算一個新的 P到 Y的剛體變換。重復上述過程直到配準收斂。

1.2 算法步驟（四元數(shù)配準）

假設給兩個三維點集 X1 和 X2，ICP方法的配準步驟如下：
第一步，計算X2中的每一個點在X1 點集中的對應近點；
第二步，求得使上述對應點對平均距離最小的剛體變換，求得平移參數(shù)和旋轉參數(shù)；
第三步，對X2使用上一步求得的平移和旋轉參數(shù)，得到新的變換點集；
第四步， 如果新的變換點集與參考點集滿足兩點集的平均距離小于某一給定閾值，則停止迭代計算，否則新的變換點集作為新的X2繼續(xù)迭代，直到達到目標函數(shù)的要求。

1.3 最近點對查找

對應點的計算是整個配準過程中耗費時間最長的步驟，查找最近點，利用 k－d tree提高查找速度 K－d tree 法建立點的拓撲關系是基于二叉樹的坐標軸分割，構造 k－d tree 的過程就是按照二叉樹法則生成，首先按 X 軸尋找分割線，即計算所有點的x值的平均值，以最接近這個平均值的點的x值將空間分成兩部分，然后在分成的子空間中按 Y 軸尋找分割線，將其各分成兩部分，分割好的子空間在按X軸分割……依此類推，最后直到分割的區(qū)域內只有一個點。這樣的分割過程就對應于一個二叉樹，二叉樹的分節(jié)點就對應一條分割線，而二叉樹的每個葉子節(jié)點就對應一個點。這樣點的拓撲關系就建立了。

2.VTK中實現(xiàn)ICP算法實驗

vtk中已經(jīng)封裝了最基本的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();//構造浮動數(shù)據(jù)點集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();/*********************************************************///源數(shù)據(jù) 與 目標數(shù)據(jù)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();//配準矩陣調整源數(shù)據(jù)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; }
這個例子首先讀取一個人臉模型。為了方便測試效果，這里對原始模型做了一個平移和旋轉變換。 這里面有個細節(jié)應該正視： vtkIterativeClosestPointTransform類中設置源點集和目標點集的函數(shù)為SetSource()和SetTarget()，其輸入數(shù)據(jù)類型為VTKDataSet，所以集合點集必須進過一定的處理！這里使用vtkVertexGlyphFilter將讀入模型和變換后的點集轉換為相應的vtkPolyData數(shù)據(jù)。 vtkSmartPointer<vtkVertexGlyphFilter> sourceGlyph =vtkSmartPointer<vtkVertexGlyphFilter>::New();sourceGlyph->SetInputData(source);sourceGlyph->Update();vtkSmartPointer<vtkVertexGlyphFilter> targetGlyph =vtkSmartPointer<vtkVertexGlyphFilter>::New();targetGlyph->SetInputData(target);targetGlyph->Update();vtkLandmarkTransform類有點不同，輸入數(shù)據(jù)類型就是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()該函數(shù)就是去偏移（中心歸一/重心歸一）。通過分別計算重心，然后平移，使得兩點集中心重合。 配準后的結果如下圖： 黃色點集是浮動點集；綠色點集是金標準；紅色點集是經(jīng)過配準矩陣調整后的點集。

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圖形圖像開發(fā)進階[M]. 機械工業(yè)出版社, 2015.

總結

以上是生活随笔為你收集整理的VTK修炼之道58：图形基本操作进阶_点云配准技术（迭代最近点ICP算法）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內容還不錯，歡迎將生活随笔推薦給好友。