おべんきょうwiki

三次元点群の姿勢あわせ

最終更新:

yahirohumpty

- view
管理者のみ編集可

三次元点群の姿勢あわせ

対応が取れている三次元点群同士の姿勢あわせを一発で解く方法

定義

文献中のクォータニオン表現は虚部,実部の順になっている.

q = (V \ w) ^T = (x \ y \ z \ w) ^T

以下この表現に合わせる.

クォータニオンを行列に変換する式を定義する.


</p>
<pre>Q(q) =
\begin{bmatrix}
wI + \lambda (V) & V \\
-V^T & w
\end{bmatrix} 
</pre>
<p>=
</p>
<pre>\begin{bmatrix}
w & -z & y & x \\
z & w & -x & y \\
-y & x & w & z \\
-x & -y & -z & w
\end{bmatrix} 
</pre>
<p>


</p>
<pre>W(q) =
\begin{bmatrix}
wI - \lambda (V) & V \\
-V^T & w
\end{bmatrix}
</pre>
<p>=
</p>
<pre>\begin{bmatrix}
w & z & -y & x \\
-z & w & x & y \\
y & -x & w & z \\
-x & -y & -z & w
\end{bmatrix} 
</pre>
<p>

ただしここで\lambda (V)は三次元ベクトルの外積行列である.


\lambda (V) =
</p>
<pre>\begin{bmatrix}
0 & -V_z & V_y \\
V_z & 0 & -V_x \\
-V_y & V_x & 0
\end{bmatrix} 
</pre>
<p>

また,位置ベクトルに対応するクォータニオンを以下のように定義する.

 p = 1/2 (p_x \ p_y \ p_z \ 0) ^T

座標変換計算

点対応の取れている二つの点群
^0 p _i , ^1 p _i , i = 1 \ldots k 
が与えられたとき,座標系0から1への変換行列を求める.

まず,以下の行列を計算する.


C_1 = -2 \sum _{i=1} ^k \alpha _i Q( ^1 p _i )^T W( ^0 p _i )


C_2 = \sum _{i=1} ^k \alpha _i I


C_3 = 2 \sum _{i=1} ^k \alpha _i (W( ^0 p _i ) - Q( ^1 p _i ))


A=(1/2) ( C_3^T ( C_2 + C_2^T ) ^{-1} C_3 - C_1 - C_1^T )

ここで\alpha _iは各点に対する重み.

次に,Aを固有値分解し最大固有値に対応する固有ベクトルr = (V_r \ w_r)を求める.

最後に求めたrから回転行列Rと並進ベクトル(のクォータニオン表現であることに注意)tを求める.


R=( w_r^2 - V_r^T V_r ) I + 2 V_r V_r^T + 2 w_r \lambda (V_r)


t=-W(r) ^T ( C_2 + C_2^T )^{-1} C_3 r

なお,

( C_2 + C_2^T )^{-1} = 1 / (2 \sum _{i=1} ^k \alpha _i) I

である.

参考文献

Walker, Shao, Volz; "Estimating 3-D Location Parameters Using Dual Number Quaternions", CVGIP, Vol. 54, No. 3, pp. 358-367, 1991.

タグ:

三次元幾何
目安箱バナー