Point to Plane ICP
問題の定式化
Point to Plane ICPはPoint to Point ICPとは異なり、目的位置を表す点群に法線方向が与えられていることを前提とする。ここで言うPlaneというのは目的位置の点を通り、法線に直交するような平面のことを指す。
従って、Point to Plane ICPでは最適化問題が各頂点
この定式化は一見Point to Point ICPのそれに近く、同じように解けそうなのであるが、それは
例えば回転行列を四元数を用いて表すと、四元数の要素4次元と並進を表すベクトル3次元の合計7次元のベクトルを未知数とするような非線形最適化問題が定義できるので、それを最急降下法などの一般的な解法によって解く方法もある。
今回は、実装をより簡単にするため、回転行列の回転角度が十分に小さいとみなせると仮定し、その際に得られる一般的な行列表示に対して最適化問題を解く。上記の最適化は
回転行列の線形化
回転行列
ここで行列
この回転行列の定義において
という関係式が得られる。ここで改めて
最適化問題の変形
線形化された回転行列
これをさらに法線方向の距離を表す式に代入すると、途中のスカラー三重積に注意して
と書ける。ここで、
以上から、
これを
が得られ、この時、
と表せる。
剛体変換の復元
上記の連立一次方程式を解くことで線形化された回転行列の要素を表すベクトル
である。あとは、これらの値をロドリゲスの回転公式に代入することで回転行列
並進の要素についても
なお、上記の計算では回転行列の回転角度が十分に小さいと仮定したが、ICPのステップの繰り返しにより回転成分も積み上がっていくので、1ステップ分の回転角度が小さくとも、十分に一般的な剛体変形を考慮することが可能である。