1.Iterative Closest Points算法

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

1.1 基本原理

ICP算法是大多數(shù)點云配準(zhǔn)算法的心， 它是一個點對剛性算法。基本思想是： 假設(shè)兩個點集 P和 X近似對齊， 對 P上的每個點，假設(shè) X上的最近點與之對齊。采用最近點搜索， 在 X上找出 P上各個點對應(yīng)的最近點， 構(gòu)成集合 Y， 然后計算一個新的 P到 Y的剛體變換。重復(fù)上述過程直到配準(zhǔn)收斂。

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

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

1.3 最近點對查找

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

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

vtk中已經(jīng)封裝了最基本的ICP算法，由類vtkIterativeClosestPointTransform負(fù)責(zé)。 具體的示例代碼如下所示： #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> //坐標(biāo)系交互 int main() {vtkSmartPointer <vtkPolyDataReader> reader =vtkSmartPointer<vtkPolyDataReader>::New();reader->SetFileName("fran_cut.vtk");reader->Update();//構(gòu)造浮動數(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ù) 與 目標(biāo)數(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();//進(jìn)行ICP配準(zhǔn)求變換矩陣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();//配準(zhǔn)矩陣調(diào)整源數(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);//設(shè)置坐標(biāo)系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);/****************************************************************///謹(jǐn)記：順序不可以顛倒！！！vtkSmartPointer<vtkOrientationMarkerWidget> widget =vtkSmartPointer<vtkOrientationMarkerWidget>::New();widget->SetOutlineColor(1, 1, 1);widget->SetOrientationMarker(axes);widget->SetInteractor(rwi); //加入鼠標(biāo)交互 widget->SetViewport(0.0, 0.0, 0.3, 0.3); //設(shè)置顯示位置 widget->SetEnabled(1);widget->InteractiveOn();//開啟鼠標(biāo)交互 /****************************************************************/render->ResetCamera();rw->Render();rwi->Initialize();rwi->Start();return 0; }
這個例子首先讀取一個人臉模型。為了方便測試效果，這里對原始模型做了一個平移和旋轉(zhuǎn)變換。 這里面有個細(xì)節(jié)應(yīng)該正視： vtkIterativeClosestPointTransform類中設(shè)置源點集和目標(biāo)點集的函數(shù)為SetSource()和SetTarget()，其輸入數(shù)據(jù)類型為VTKDataSet，所以集合點集必須進(jìn)過一定的處理！這里使用vtkVertexGlyphFilter將讀入模型和變換后的點集轉(zhuǎn)換為相應(yīng)的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類型。
//進(jìn)行ICP配準(zhǔn)求變換矩陣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ù)就是去偏移（中心歸一/重心歸一）。通過分別計算重心，然后平移，使得兩點集中心重合。 配準(zhǔn)后的結(jié)果如下圖： 黃色點集是浮動點集；綠色點集是金標(biāo)準(zhǔn)；紅色點集是經(jīng)過配準(zhǔn)矩陣調(diào)整后的點集。

2.vtkOrientationMarkerWidget類設(shè)置坐標(biāo)系心得

//謹(jǐn)記：順序不可以顛倒！！！vtkSmartPointer<vtkOrientationMarkerWidget> widget =vtkSmartPointer<vtkOrientationMarkerWidget>::New();widget->SetOutlineColor(1, 1, 1);widget->SetOrientationMarker(axes);widget->SetInteractor(rwi); //加入鼠標(biāo)交互 widget->SetViewport(0.0, 0.0, 0.3, 0.3); //設(shè)置顯示位置 widget->SetEnabled(1);widget->InteractiveOn();//開啟鼠標(biāo)交互

3.參看資料

1.《C++ primer》
2.《The VTK User’s Guide – 11thEdition》
3. ?張曉東, 羅火靈. VTK圖形圖像開發(fā)進(jìn)階[M]. 機(jī)械工業(yè)出版社, 2015.

總結(jié)

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

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