VTKによる2つの形状の剛体位置姿勢推定(ICP)
ICPとは
Iterative Closest Point(ICP)とは2つの形状(点群やサーフェス)の位置姿勢変換行列を求める代表的なアルゴリズムである．具体的なアルゴリズムについては，Wikipediaを参照．
とてもシンプルなアルゴリズムで点群の位置合わせでは広く利用されているが，その一方で初期位置やノイズ，外れ値などに弱いといった欠点もあり，多くの改良アルゴリズムが提案されている．
利用例
1. Kinectなどのレンジセンサから経時的に取得された物体形状から運動(並進と回転)を推定する．
2. 計測された断片的な形状データに3Dモデルを位置合わせする．
位置合わせとしては，基本的なアルゴリズムなので，様々なソフトウェアやライブラリで実装されている．ここでは，VTKにおける実装を用いて，その利用方法を公式サンプルに基づいて説明していく．
VTKによるICP
vtkIterativeClosestPointTransformを用いる．
手順は以下の通り．
-
変換元(Source)と変換先(Target)を設定．
- 計算される変換行列はSourceをTargetに変換する行列である．
- 入力は
vtkPolyDataで良いが，サーフェスがない点群データを用いたい場合には，公式サンプルにもある通り，vtkVertexGlyphFilterでvertexを作成しておく必要があるので注意．
-
変換に関するプロパティを設定．
- 繰返し回数 - 正確な位置合わせを行うためには，十分な繰り返し回数が必要．
- 重心による初期位置合わせ - 初期位置合わせとして重心で位置合わせを行うか． 剛体変換なので，形状の外れ値や欠損がなければ，これだけで並進成分は求まる．
-
変換を計算．
-
Modified(),Update()を呼ぶ．(なぜModified()も呼ぶのかはよく知りません．) 点数や繰り返し回数によっては少し時間がかかる場合もあります． - 計算が終わると，
GetMatrix()で推定された変換行列を取得可能．vtkPolyDataを変換したい場合は，vtkTransformPolyDataを用いる．
-
vtkSmartPointer<vtkIterativeClosestPointTransform> icp =vtkSmartPointer<vtkIterativeClosestPointTransform>::New();
//変換元と変換先をセットする．
icp->SetSource(source);
icp->SetTarget(target);
//プロパティ
icp->GetLandmarkTransform()->SetModeToRigidBody();
icp->SetMaximumNumberOfIterations(20);//繰り返し回数を設定．
icp->StartByMatchingCentroidsOn();//重心位置合わせ．
icp->Modified();
icp->Update();
vtkMatrix4x4 *matrix = icp->GetMatrix();
変換を実行
vtkTransformPolyDataを用いて行うことができる．
vtkSmartPointer<vtkTransformPolyDataFilter> icpFilter = vtkSmartPointer<vtkTransformPolyDataFilter>::New();
icpFilter->SetInputData(source);
icpFilter->SetTransform(icp);
icpFilter->Update();
vtkPolyData* output = icpFilter->GetOutput();