座標系の表現（座標変換）

三次元直交座標系は原点の位置と姿勢で定めることが出来る．

原点の位置に関しては3Dベクトルで説明した位置ベクトルを使うのが分かりやすい．

姿勢に関しては座標系の姿勢（回転）の表現で説明したように多数の表現方法がある．

近年のオブジェクト指向的な発想からすれば，数学的な表現の優劣は別にして， 「座標系クラス」がどのような機能を提供するかが重要であり，実のところプログラムで実現する場合にはその内部表現／実装は問題にはならない．

ここでは，まず座標系の姿勢を表す直交行列と原点位置を表す位置ベクトルとで表現し，座標変換でそれたがどのように変換されるか 個々に分かり易く議論する．

その上で数学的表現と演算が一致している座標系の姿勢と位置を一つに合わせた4×4同次変換行列を導入して， 分かりやすい表現の上でその機能・役割を定式化する．

図1 ベクトル，マトリックスによる座標系の表現

図1は単純に位置ベクトル$^0\boldsymbol{d}_1$と直行行列$^0A_1$で座標系を表現したものである． それらの成分表示は参照座標系により変わるのでそれを明示しいる． これを座標の変換としてとらえる場合は参照座標系の明示は不要である．

この表現はOpenCVで良く用いられる[R|t]の表現と同等である．OpenCVのこの表現は3×4の行列を 表している．

図2　ベクトルの座標変換

図2はベクトルの座標変換を示している．ベクトルの場合は座標系の原点位置によらないので 直交行列とベクトルの積で表現出来る． 図2には$^0\boldsymbol{d}_1$などの座標系原点への位置ベクトルが描かれているが これは不要であり，式を改めて式を書くと， $$ ^0\boldsymbol{v} = {^0A_1} {^1\boldsymbol{v}}　\tag{1} $$ $$ ^1\boldsymbol{v} = {^1A_2} {^2\boldsymbol{v}} \tag{2} $$ となり，式(1),(2)を連鎖させて $$ ^0\boldsymbol{v} = {^0A_1}{^1A_2}{^2\boldsymbol{v}}　\tag{3} $$ などの変換が可能である．

図3　位置ベクトルの座標変換

図3は点Pに対する位置ベクトルの座標変換を示している．位置ベクトルの場合は座標系の原点位置に依存するので 計算がもう少し複雑になる．座標系$\Sigma_0$から見た点Pの位置ベクトル$^0\boldsymbol{p}$はベクトルとしては 座標系$\Sigma_0$の原点から座標系$\Sigma_1$の原点へのベクトル$^0\boldsymbol{d}_1$と， 座標系$\Sigma_1$の原点から点Pへのベクトル$^1\boldsymbol{p}$を加えたものになる． ただし$^1\boldsymbol{p}$は座標系$\Sigma_1$での成分表示になっているため 座標系$\Sigma_0$への成分表示に変換して加える必要がある．したがって，

$$ ^0\boldsymbol{p} = {^0A_1} {^1\boldsymbol{p}}+^0\boldsymbol{d}_1　\tag{4} $$ となる．

図が煩雑になるので座標系$\Sigma_2$は描かれていないが同様に，

$$ ^1\boldsymbol{p} = {^1A_2} {^2\boldsymbol{p}}+^1\boldsymbol{d}_2　\tag{5} $$

これを連鎖させると $$ ^0\boldsymbol{p} = {^0A_1} \left( {{^1A_2} {^2\boldsymbol{p}}+^1\boldsymbol{d}_2}\right)+^0\boldsymbol{d}_1 \tag{6} $$

この式は一見複雑に見えるが，変換を順次適用すると考えればそれほど難しいものではない． また後で導入する4×4同次変換行列を用いると表現上も簡単に見せることが可能となる．

図2に戻って見ると，座標系$\Sigma_0$から見た座標系$\Sigma_2$の原点位置$^0\boldsymbol{d}_2$は，座標系$\Sigma_2$からみた自身の原点位置は$0$なので式(6)で$^2\boldsymbol{p}=\boldsymbol{0}$として計算できる．

$$ ^0\boldsymbol{d}_2 = {^0A_1} {^1\boldsymbol{d}_2} + ^0\boldsymbol{d}_1　\tag{7} $$ また姿勢を表す直行行列は， $$ ^0A_2 = {^0A_1}{^1A_2} \tag{8} $$ これらが座標系の変換を連鎖させた式になる．

図3で座標系$\Sigma_1$から見た$\Sigma_0$を考える．

姿勢については， $$ ^1A_0 = {^0A_1^{-1}} = {^0A_1^\mathrm{T}} \tag{9} $$ となる．

原点位置について，ベクトルとしては， $$ -{^0\boldsymbol{d}_1} $$ であるが，このベクトルは座標系$\Sigma_0$で表現されたものであるので$\Sigma_1$から見たものに変換する必要がある． すなわち $$ ^1\boldsymbol{d}_0 = -{^1A_0}{^0\boldsymbol{d}_1} = -({^0A_1^\mathrm{T}}){^0\boldsymbol{d}_1} \tag{10} $$ となる．

ここで座標系の姿勢と位置を以下のように一つの4×4の行列で表した同時変換行列を導入する ¹⁾．

$$ T = \left( \begin{array}{c} \begin{array}{c} A \\ \begin{array}{c} 0 & 0 & 0 \end{array} \end{array} & \begin{array}{c} \boldsymbol{d} \\ 1 \end{array} \end{array} \right) $$

これを用いると座標系の変換が行列の積として簡単に表現できる．たとえば， $$ ^0T_1 = \left( \begin{array}{c} \begin{array}{c} ^0A_1 \\ \begin{array}{c} 0 & 0 & 0 \end{array} \end{array} & \begin{array}{c} ^0\boldsymbol{d}_1 \\ 1 \end{array} \end{array} \right) $$ $$ ^1T_2 = \left( \begin{array}{c} \begin{array}{c} ^1A_2 \\ \begin{array}{c} 0 & 0 & 0 \end{array} \end{array} & \begin{array}{c} ^1\boldsymbol{d}_2 \\ 1 \end{array} \end{array} \right) $$ とすると，式(7),(8)より， $$ {^0T_1}{^1T_2} = \left( \begin{array}{c} \begin{array}{c} {^0A_1}{^1A_2} \\ \begin{array}{c} 0 & 0 & 0 \end{array} \end{array} & \begin{array}{c} {^0A_1} {^1\boldsymbol{d}_2} + ^0\boldsymbol{d}_1 \\ 1 \end{array} \end{array} \right) = {^0T_2} $$ となり，すなわち $$ {^0T_2}={^0T_1}{^1T_2}　\tag{11} $$ のように，座標系の姿勢のみを表現した直交行列の変換と類似の分かりやすい記法となる．

式(9),(10)より， $$ {^1T_0} = \left( \begin{array}{c} \begin{array}{c} {^0A_1^\mathrm{T}} \\ \begin{array}{c} 0 & 0 & 0 \end{array} \end{array} & \begin{array}{c} -({^0A_1^\mathrm{T}}){^0\boldsymbol{d}_1} \\ 1 \end{array} \end{array} \right) = {^0T_1^{-1}} \tag{12} $$ これが$^0T_1$の逆行列になっていることは，式(10)の$^1T_2$にこの$^1T_0$を代入して計算するとすぐに分かる．

また位置ベクトルを $$ ^0\boldsymbol{p} = \left( \begin{array}{c} ^0\vec{\boldsymbol{p}} \\ 1 \end{array} \right) $$ $$ ^1\boldsymbol{p} = \left( \begin{array}{c} ^1\vec{\boldsymbol{p}} \\ 1 \end{array} \right) $$ と拡張すれば（少し紛らわしいがここでは$^0\vec{\boldsymbol{p}}$が拡張前の3次元ベクトル） ²⁾， 式(4)より，

$$ ^0\boldsymbol{p} = {^0T_1} {^1\boldsymbol{p}} = \left( \begin{array}{c} \begin{array}{c} ^0A_1 \\ \begin{array}{c} 0 & 0 & 0 \end{array} \end{array} & \begin{array}{c} ^0\boldsymbol{d}_1 \\ 1 \end{array} \end{array} \right) \left( \begin{array}{c} ^1\vec{\boldsymbol{p}} \\ 1 \end{array} \right) $$ $$ =\left( \begin{array}{c} {^0A_1} {^1\vec{\boldsymbol{p}}}+^0\boldsymbol{d}_1 \\ 1 \end{array} \right) \tag{13} $$

として座標変換に伴う位置ベクトルの変換がシンプルににかける．

さらに座標系を連鎖させた場合の位置ベクトルも $$ ^0\boldsymbol{p} = {^0T_1}{^1T_2}{^2\boldsymbol{p}} \tag{14} $$ のように簡単に表記できることは，式(13)ないしは式(11)を用いて計算すると 式(6)と一致することからすぐ分かる．

ベクトルを $$ ^0\boldsymbol{v} = \left( \begin{array}{c} ^0\vec{\boldsymbol{v}} \\ 0 \end{array} \right) $$ $$ ^1\boldsymbol{v} = \left( \begin{array}{c} ^1\vec{\boldsymbol{v}} \\ 0 \end{array} \right) $$ と拡張すれば，

$$ ^0\boldsymbol{v} = {^0T_1} {^1\boldsymbol{v}} = \left( \begin{array}{c} \begin{array}{c} ^0A_1 \\ \begin{array}{c} 0 & 0 & 0 \end{array} \end{array} & \begin{array}{c} ^0\boldsymbol{d}_1 \\ 1 \end{array} \end{array} \right) \left( \begin{array}{c} ^1\vec{\boldsymbol{v}} \\ 0 \end{array} \right) =\left( \begin{array}{c} {^0A_1} {^1\vec{\boldsymbol{v}}} \\ 0 \end{array} \right) $$

となり座標変換に伴うベクトルの変換も同じように書けることが分かる．

同次変換行列で考えるときにベクトルと位置ベクトルとの違いは4行目が0か1かというものになる． これは表現としては面白く，

• 位置ベクトルと位置ベクトルの差はベクトルになる
• 位置ベクトルとベクトルの和は位置ベクトルになる
• 位置ベクトルと位置ベクトルの和は意味をなさない

などの解釈が可能となっている．

ただし，ベクトルの変換の場合には原点位置は影響しないので姿勢を表す3×3の直行行列との積で 表現した方が素直ではある．

座標系の姿勢と位置の表現（ないしはその変換の表現）に同次変換行列を用いることで変換の連鎖が $$ {^0T_2} = {^0T_1}{^1T_2} $$ などの形式で容易に表現できる．

また位置ベクトルも， $$ ^0\boldsymbol{p} = {^0T_1}{^1\boldsymbol{p}} $$ の形で分かりやすく表現出来る．

同次変換行列を，たとえば任意のある座標系のz軸方向10.0[m]など，特定の座標系によらない座標変換に用いる場合は， 左上の参照座標系の表示をしないことが多い．

同次変換行列による表現はとても便利なものであるが，最初に述べたようにプログラムで実装する際は 座標系の連鎖やそれに伴うベクトルの変換などの機能が実現できていれば 内部表現を必ずしも4×4の行列にする必要はない．

座標系演算のプログラム

練習問題