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負責。具體的示例代碼如下所示：1 #include <vtkAutoInit.h> 2 VTK_MODULE_INIT(vtkRenderingOpenGL); 3 VTK_MODULE_INIT(vtkRenderingFreeType); 4 VTK_MODULE_INIT(vtkInteractionStyle); 5 6 #include <vtkSmartPointer.h> 7 #include <vtkPolyDataReader.h> 8 #include <vtkPolyData.h> 9 #include <vtkTransform.h> 10 #include <vtkTransformPolyDataFilter.h> 11 #include <vtkVertexGlyphFilter.h> 12 #include <vtkIterativeClosestPointTransform.h> 13 #include <vtkLandmarkTransform.h> 14 #include <vtkTransformPolyDataFilter.h> 15 #include <vtkPolyDataMapper.h> 16 #include <vtkActor.h> 17 #include <vtkProperty.h> 18 #include <vtkAxesActor.h> 19 #include <vtkRenderer.h> 20 #include <vtkRenderWindow.h> 21 #include <vtkRenderWindowInteractor.h> 22 #include <vtkOrientationMarkerWidget.h> //坐標系交互 23 int main() 24 { 25 vtkSmartPointer <vtkPolyDataReader> reader = 26 vtkSmartPointer<vtkPolyDataReader>::New(); 27 reader->SetFileName("fran_cut.vtk"); 28 reader->Update(); 29 30 //構造浮動數據點集 31 vtkSmartPointer<vtkPolyData> orig = reader->GetOutput(); 32 vtkSmartPointer<vtkTransform> trans = 33 vtkSmartPointer<vtkTransform>::New(); 34 trans->Translate(0.2, 0.1, 0.1); 35 trans->RotateX(10); 36 37 vtkSmartPointer<vtkTransformPolyDataFilter> transformFilter1 = 38 vtkSmartPointer<vtkTransformPolyDataFilter>::New(); 39 transformFilter1->SetInputData(reader->GetOutput()); 40 transformFilter1->SetTransform(trans); 41 transformFilter1->Update(); 42 /*********************************************************/ 43 //源數據 與 目標數據 44 vtkSmartPointer<vtkPolyData> source = 45 vtkSmartPointer<vtkPolyData>::New(); 46 source->SetPoints(orig->GetPoints()); 47 48 vtkSmartPointer<vtkPolyData> target = 49 vtkSmartPointer<vtkPolyData>::New(); 50 target->SetPoints(transformFilter1->GetOutput()->GetPoints()); 51 52 vtkSmartPointer<vtkVertexGlyphFilter> sourceGlyph = 53 vtkSmartPointer<vtkVertexGlyphFilter>::New(); 54 sourceGlyph->SetInputData(source); 55 sourceGlyph->Update(); 56 57 vtkSmartPointer<vtkVertexGlyphFilter> targetGlyph = 58 vtkSmartPointer<vtkVertexGlyphFilter>::New(); 59 targetGlyph->SetInputData(target); 60 targetGlyph->Update(); 61 62 //進行ICP配准求變換矩陣 63 vtkSmartPointer<vtkIterativeClosestPointTransform> icptrans = 64 vtkSmartPointer<vtkIterativeClosestPointTransform>::New(); 65 icptrans->SetSource(sourceGlyph->GetOutput()); 66 icptrans->SetTarget(targetGlyph->GetOutput()); 67 icptrans->GetLandmarkTransform()->SetModeToRigidBody(); 68 icptrans->SetMaximumNumberOfIterations(50); 69 icptrans->StartByMatchingCentroidsOn(); 70 icptrans->Modified(); 71 icptrans->Update(); 72 73 //配准矩陣調整源數據 74 vtkSmartPointer<vtkTransformPolyDataFilter> solution = 75 vtkSmartPointer<vtkTransformPolyDataFilter>::New(); 76 solution->SetInputData(sourceGlyph->GetOutput()); 77 solution->SetTransform(icptrans); 78 solution->Update(); 79 / 80 vtkSmartPointer<vtkPolyDataMapper> sourceMapper = 81 vtkSmartPointer<vtkPolyDataMapper>::New(); 82 sourceMapper->SetInputConnection(sourceGlyph->GetOutputPort()); 83 vtkSmartPointer<vtkActor> sourceActor = 84 vtkSmartPointer<vtkActor>::New(); 85 sourceActor->SetMapper(sourceMapper); 86 sourceActor->GetProperty()->SetColor(1, 1, 0); 87 sourceActor->GetProperty()->SetPointSize(2); 88 89 vtkSmartPointer<vtkPolyDataMapper> targetMapper = 90 vtkSmartPointer<vtkPolyDataMapper>::New(); 91 targetMapper->SetInputConnection(targetGlyph->GetOutputPort()); 92 vtkSmartPointer<vtkActor> targetActor = 93 vtkSmartPointer<vtkActor>::New(); 94 targetActor->SetMapper(targetMapper); 95 targetActor->GetProperty()->SetColor(0, 1, 0); 96 targetActor->GetProperty()->SetPointSize(3); 97 98 vtkSmartPointer<vtkPolyDataMapper> soluMapper = 99 vtkSmartPointer<vtkPolyDataMapper>::New(); 100 soluMapper->SetInputConnection(solution->GetOutputPort()); 101 vtkSmartPointer<vtkActor> soluActor = 102 vtkSmartPointer<vtkActor>::New(); 103 soluActor->SetMapper(soluMapper); 104 soluActor->GetProperty()->SetColor(1, 0, 0); 105 soluActor->GetProperty()->SetPointSize(2); 106 //設置坐標系 107 vtkSmartPointer<vtkAxesActor> axes = 108 vtkSmartPointer<vtkAxesActor>::New(); 109 /// 110 vtkSmartPointer<vtkRenderer> render = 111 vtkSmartPointer<vtkRenderer>::New(); 112 render->AddActor(sourceActor); 113 render->AddActor(targetActor); 114 render->AddActor(soluActor); 115 render->SetBackground(0, 0, 0); 116 117 vtkSmartPointer<vtkRenderWindow> rw = 118 vtkSmartPointer<vtkRenderWindow>::New(); 119 rw->AddRenderer(render); 120 rw->SetSize(480, 320); 121 rw->SetWindowName("Regisration by ICP"); 122 123 vtkSmartPointer<vtkRenderWindowInteractor> rwi = 124 vtkSmartPointer<vtkRenderWindowInteractor>::New(); 125 rwi->SetRenderWindow(rw); 126 /****************************************************************/ 127 //謹記：順序不可以顛倒！！！ 128 vtkSmartPointer<vtkOrientationMarkerWidget> widget = 129 vtkSmartPointer<vtkOrientationMarkerWidget>::New(); 130 widget->SetOutlineColor(1, 1, 1); 131 widget->SetOrientationMarker(axes); 132 widget->SetInteractor(rwi); //加入鼠標交互 133 widget->SetViewport(0.0, 0.0, 0.3, 0.3); //設置顯示位置 134 widget->SetEnabled(1); 135 widget->InteractiveOn();//開啟鼠標交互 136 /****************************************************************/ 137 render->ResetCamera(); 138 rw->Render(); 139 rwi->Initialize(); 140 rwi->Start(); 141 142 return 0; 143 }這個例子首先讀取一個人臉模型。為了方便測試效果，這里對原始模型做了一個平移和旋轉變換。這里面有個細節應該正視：vtkIterativeClosestPointTransform類中設置源點集和目標點集的函數為SetSource()和SetTarget()，其輸入數據類型為VTKDataSet，所以集合點集必須進過一定的處理！這里使用vtkVertexGlyphFilter將讀入模型和變換后的點集轉換為相應的vtkPolyData數據。1 vtkSmartPointer<vtkVertexGlyphFilter> sourceGlyph = 2 vtkSmartPointer<vtkVertexGlyphFilter>::New(); 3 sourceGlyph->SetInputData(source); 4 sourceGlyph->Update(); 5 6 vtkSmartPointer<vtkVertexGlyphFilter> targetGlyph = 7 vtkSmartPointer<vtkVertexGlyphFilter>::New(); 8 targetGlyph->SetInputData(target); 9 targetGlyph->Update();vtkLandmarkTransform類有點不同，輸入數據類型就是vtkPoints類型。
1 //進行ICP配准求變換矩陣 2 vtkSmartPointer<vtkIterativeClosestPointTransform> icptrans = 3 vtkSmartPointer<vtkIterativeClosestPointTransform>::New(); 4 icptrans->SetSource(sourceGlyph->GetOutput()); 5 icptrans->SetTarget(targetGlyph->GetOutput()); 6 icptrans->GetLandmarkTransform()->SetModeToRigidBody(); 7 icptrans->SetMaximumNumberOfIterations(50); 8 icptrans->StartByMatchingCentroidsOn(); 9 icptrans->Modified(); 10 icptrans->Update();StartByMatchingCentroidsOn()該函數就是去偏移（中心歸一/重心歸一）。通過分別計算重心，然后平移，使得兩點集中心重合。配准后的結果如下圖：黃色點集是浮動點集；綠色點集是金標准；紅色點集是經過配准矩陣調整后的點集。
2.vtkOrientationMarkerWidget類設置坐標系心得
1 //謹記：順序不可以顛倒！！！
2 vtkSmartPointer<vtkOrientationMarkerWidget> widget =
3 vtkSmartPointer<vtkOrientationMarkerWidget>::New(); 4 widget->SetOutlineColor(1, 1, 1); 5 widget->SetOrientationMarker(axes); 6 widget->SetInteractor(rwi); //加入鼠標交互
7 widget->SetViewport(0.0, 0.0, 0.3, 0.3); //設置顯示位置
8 widget->SetEnabled(1); 9 widget->InteractiveOn();//開啟鼠標交互