「三次元点群の姿勢あわせ」の編集履歴（バックアップ）一覧はこちら

「三次元点群の姿勢あわせ」(2014/08/06 (水) 20:17:46) の最新版変更点

追加された行は緑色になります。

削除された行は赤色になります。

= 三次元点群の姿勢あわせ =

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

== 定義 ==

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

<math>q = (V \ w) ^T = (x \ y \ z \ w) ^T </math>

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

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

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

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

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

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

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

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

== 座標変換計算 ==

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

まず，以下の行列を計算する．

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

<math>
C_2 = \sum _{i=1} ^k \alpha _i I
</math>

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

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

ここで<math>\alpha _i</math>は各点に対する重み．

次に，<math>A</math>を固有値分解し最大固有値に対応する固有ベクトル<math>r = (V_r \ w_r)</math>を求める．

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

<math>
R=( w_r^2 - V_r^T V_r ) I + 2 V_r V_r^T + 2 w_r \lambda (V_r)
</math>

<math>
t=-W(r) ^T ( C_2 + C_2^T )^{-1} C_3 r
</math>

なお，

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

である．

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

= 三次元点群の姿勢あわせ =

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

== 定義 ==

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

<math>q = (V \ w) ^T = (x \ y \ z \ w) ^T </math>

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

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

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

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

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

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

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

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

== 座標変換計算 ==

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

まず，以下の行列を計算する．

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

<math>
C_2 = \sum _{i=1} ^k \alpha _i I
</math>

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

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

ここで<math>\alpha _i</math>は各点に対する重み．

次に，<math>A</math>を固有値分解し最大固有値に対応する固有ベクトル<math>r = (V_r \ w_r)</math>を求める．

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

<math>
R=( w_r^2 - V_r^T V_r ) I + 2 V_r V_r^T + 2 w_r \lambda (V_r)
</math>

<math>
t=-W(r) ^T ( C_2 + C_2^T )^{-1} C_3 r
</math>

なお，

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

である．

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