Programming for Beginners Programming for everyone!

Point to Plane ICP

問題の定式化

Point to Plane ICPはPoint to Point ICPとは異なり、目的位置を表す点群に法線方向が与えられていることを前提とする。ここで言うPlaneというのは目的位置の点を通り、法線に直交するような平面のことを指す。

従って、Point to Plane ICPでは最適化問題が各頂点xiの最近傍点piとその最近傍点の法線niを用いて以下のように定義される。

(1)minR,ti=1N((Rxi+tpi)ni)2

この定式化は一見Point to Point ICPのそれに近く、同じように解けそうなのであるが、それはtに対してだけで、Rに対しての最適化問題は Rが行列式が1の直交行列であるという制約から解析的な解をもたない。そのため、この最適化問題は数値的に解く必要がある。

例えば回転行列を四元数を用いて表すと、四元数の要素4次元と並進を表すベクトル3次元の合計7次元のベクトルを未知数とするような非線形最適化問題が定義できるので、それを最急降下法などの一般的な解法によって解く方法もある。

今回は、実装をより簡単にするため、回転行列の回転角度が十分に小さいとみなせると仮定し、その際に得られる一般的な行列表示に対して最適化問題を解く。上記の最適化はRが直交行列であるが故に解析解が得られなかったので、このように制約を緩和すれば解析的に解を得ることができる。

回転行列の線形化

回転行列Rは回転軸をw、回転角度をθとする時、ロドリゲスの回転公式によって以下のように与えられる。

R(w,θ)=I+sinθK(w)+(1cosθ)K(w)2

ここで行列K(w)wと外積を取る操作に対応する作用素で以下のように表せる。

K(w)=(0wzwywz0wxwywx0)

この回転行列の定義においてθが十分に小さいと仮定すると、cosθ1, sinθθとなることから、

R(w,θ)I+θK(w)=I+K(θw)=I+(0θwzθwyθwz0θwxθwyθwx0)

という関係式が得られる。ここで改めてa=θwと置き直すと、回転行列を表すのに必要な未知数は3次元ベクトルaの各要素のみとなる。

最適化問題の変形

線形化された回転行列I+K(a)を用いると、Kが外積を表す作用素だったことに注意して、剛体変換の式は以下のように書き直せる。

Rx+t=(I+K(a))x+t=x+a×x+t

これをさらに法線方向の距離を表す式に代入すると、途中のスカラー三重積に注意して

(x+a×x+tp)n=(xn)+(a×x)n+(tn)(pn)=a(x×n)+(tn)+(xp)n

と書ける。ここで、atを縦方向に連結した6次元ベクトルを考えると、この式はさらに以下のように変形できる。

(x×nn)(at)(px)n

以上から、atを連結したベクトルをuR6とすることで、式(1)の最適化問題は、s, rを適当なベクトルとして、以下のように書き直せる。

minusur2

これをuについて微分したものが0であると仮定して項を整理すれば、一般的な線形の連立一次方程式の形

Au=b

が得られ、この時、A, bはそれぞれ

A=i=1N[(x×nn)(x×nn)T]R6×6b=i=1N[(x×nn)(pixi)ni]R6

と表せる。

剛体変換の復元

上記の連立一次方程式を解くことで線形化された回転行列の要素を表すベクトルaが得られる。このベクトルがθwであったことを思い出すと、

θ=a,w=aθ

である。あとは、これらの値をロドリゲスの回転公式に代入することで回転行列Rが得られる。

並進の要素についてもuと同時に与えられていることから、これを以ってPoint to Plane ICPにおいても剛体変形を求めることができた。

なお、上記の計算では回転行列の回転角度が十分に小さいと仮定したが、ICPのステップの繰り返しにより回転成分も積み上がっていくので、1ステップ分の回転角度が小さくとも、十分に一般的な剛体変形を考慮することが可能である。