\[
% to cope with definition mismatch between MathJax and LaTeX
\newcommand{\coloneq}{\mathrel{:=}}
\newcommand{\eqcolon}{\mathrel{=:}}
% general purpose
\newcommand{\ctext}[1]{\raise0.2ex\hbox{\textcircled{\scriptsize{#1}}}}
% mathematics
% general purpose
\DeclarePairedDelimiterX{\parens}[1]{\lparen}{\rparen}{#1}
\DeclarePairedDelimiterX{\braces}[1]{\lbrace}{\rbrace}{#1}
\DeclarePairedDelimiterX{\bracks}[1]{\lbrack}{\rbrack}{#1}
\DeclarePairedDelimiterX{\verts}[1]{|}{|}{#1}
\DeclarePairedDelimiterX{\Verts}[1]{\|}{\|}{#1}
\DeclarePairedDelimiterX{\setComprehension}[2]{\lbrace}{\rbrace}{#1\,\delimsize\vert\,#2}
\newcommand{\as}{{\quad\textrm{as}\quad}}
\newcommand{\st}{{\textrm{ s.t. }}}
\newcommand{\naturalNumbers}{\mathbb{N}}
\newcommand{\integers}{\mathbb{Z}}
\newcommand{\rationalNumbers}{\mathbb{Q}}
\newcommand{\realNumbers}{\mathbb{R}}
\newcommand{\nonNegRealNumbers}{\mathbb{R}_{\geq 0}}
\newcommand{\posRealNumbers}{\mathbb{R}_{> 0}}
\newcommand{\complexNumbers}{\mathbb{C}}
\newcommand{\field}{\mathbb{F}}
\newcommand{\EuclideanSpace}{\mathbb{E}}
\newcommand{\func}[2]{{#1}\parens*{#2}}
\newcommand{\argmax}{\operatorname*{arg~max}}
\newcommand{\argmin}{\operatorname*{arg~min}}
% set theory
\newcommand{\range}[2]{\braces*{#1,\dotsc,#2}}
\renewcommand{\complement}{\mathrm{c}}
\newcommand{\ind}[2]{\mathbbm{1}_{#1}\parens*{#2}}
\newcommand{\indII}[1]{\mathbbm{1}\braces*{#1}}
% number theory
\newcommand{\abs}[1]{\verts*{#1}}
\newcommand{\combi}[2]{{_{#1}\mathrm{C}_{#2}}}
\newcommand{\perm}[2]{{_{#1}\mathrm{P}_{#2}}}
\newcommand{\GaloisField}[1]{\mathrm{GF}\parens*{#1}}
% real analysis
\newcommand{\NapierE}{\mathrm{e}}
\newcommand{\sgn}{\operatorname{sgn}} % sign function
\newcommand{\rect}{\operatorname{rect}} % rectangular function
\newcommand{\cl}[1]{\operatorname{cl}#1} % closure of a set
\newcommand{\img}[1]{\operatorname{im}\parens*{#1}} % image of a function
\newcommand{\dom}[1]{\operatorname{dom}\parens*{#1}} % domain of a function
\newcommand{\LittleO}[2]{\underset{#1}{o}\parens*{#2}} % little-o notation
\newcommand{\norm}[1]{\Verts*{#1}} % norm of a vector or function
\newcommand{\floor}[1]{\left\lfloor #1\right\rfloor}
\newcommand{\ceil}[1]{\left\lceil#1\right\rceil}
\newcommand{\sinc}{\operatorname{sinc}}
\newcommand{\nrmSinc}{\operatorname{nsinc}} % normalized sinc function
\newcommand{\GammaFunc}[1]{\Gamma\parens*{#1}}
\newcommand{\erf}{\operatorname{erf}}
% inverse trigonometric functions
\newcommand{\asin}{\operatorname{Sin}^{-1}}
\newcommand{\acos}{\operatorname{Cos}^{-1}}
\newcommand{\atan}{\operatorname{Tan}^{-1}}
\newcommand{\atanEx}[2]{\atan{\parens*{#1,#2}}}
% derivative
\newcommand{\deriv}[3]{\frac{\mathrm{d}^{#3}#1}{\mathrm{d}{#2}^{#3}}}
\newcommand{\derivLong}[3]{\frac{\mathrm{d}^{#3}}{\mathrm{d}{#2}^{#3}}#1}
\newcommand{\partDeriv}[3]{\frac{\mathrm{\partial}^{#3}#1}{\mathrm{\partial}{#2}^{#3}}}
\newcommand{\partDerivLong}[3]{\frac{\mathrm{\partial}^{#3}}{\mathrm{\partial}{#2}^{#3}}#1}
\newcommand{\partDerivIIHetero}[3]{\frac{\mathrm{\partial}^2#1}{\partial#2\mathrm{\partial}#3}}
\newcommand{\partDerivIIHeteroLong}[3]{{\frac{\mathrm{\partial}^2}{\partial#2\mathrm{\partial}#3}#1}}
% integral
\newcommand{\pv}{{\textrm{ p.v. }}}
\newcommand{\integrate}[5]{\int_{#1}^{#2}{#3}{\;\mathrm{d}^{#4}}#5}
\newcommand{\LebInteg}[4]{\int_{#1} {#2} {#3}\parens*{\;\mathrm{d}#4}}
% complex analysis
\newcommand{\conj}[1]{\overline{#1}}
\renewcommand{\Re}[1]{{\operatorname{Re}{\parens*{#1}}}}
\renewcommand{\Im}[1]{{\operatorname{Im}{\parens*{#1}}}}
\newcommand{\Arg}{\operatorname{Arg}}
\newcommand{\Log}{\operatorname{Log}}
% Laplace transform
\newcommand{\LPLC}[1]{\operatorname{\mathcal{L}}\parens*{#1}}
\newcommand{\ILPLC}[1]{\operatorname{\mathcal{L}}^{-1}\parens*{#1}}
% discrete fourier transform
\newcommand{\DFT}[1]{\mathrm{DFT}\parens*{#1}}
\newcommand{\IDFT}[1]{\mathrm{IDFT}\parens*{#1}}
% Z-transform
\newcommand{\ZTrans}[1]{\operatorname{\mathcal{Z}}\parens*{#1}}
\newcommand{\IZTrans}[1]{\operatorname{\mathcal{Z}}^{-1}\parens*{#1}}
\newcommand{\SSZTrans}[1]{\underset{\text{s.s.}}{\mathcal{Z}}\parens*{#1}} % single-sided Z-transform
% linear algebra
\newcommand{\bm}[1]{{\boldsymbol{#1}}}
\newcommand{\vecEntry}[2]{\bm{#1}\bracks*{#2}}
\newcommand{\matEntry}[3]{#1\bracks*{#2}\bracks*{#3}}
\newcommand{\matPart}[5]{\matEntry{#1}{#2:#3}{#4:#5}}
\newcommand{\diag}[1]{\operatorname{diag}\parens*{#1}}
\newcommand{\transpose}[1]{{#1}^\top}
\newcommand{\HerConj}[1]{{#1}^*}
\newcommand{\tr}[1]{\operatorname{tr}{\parens*{#1}}}
\newcommand{\inProd}[2]{\left\langle#1,#2\right\rangle}
\newcommand{\dotProd}[2]{#1 \cdot #2}
\newcommand{\HadamardProd}{\odot}
\newcommand{\HadamardDiv}{\oslash}
\newcommand{\Span}[1]{\operatorname{span}\bracks*{#1}}
\newcommand{\Ker}[1]{\operatorname{Ker}\parens*{#1}}
\newcommand{\rank}[1]{\operatorname{rank}\parens*{#1}}
% vector
% unit vector
\newcommand{\vix}{\bm{i}_x}
\newcommand{\viy}{\bm{i}_y}
\newcommand{\viz}{\bm{i}_z}
% graph theory
\newcommand{\neighborhood}{\mathcal{N}}
% probability theory
\newcommand{\PDF}[2]{\operatorname{PDF}\bracks*{#1,#2}}
\newcommand{\Ber}[1]{\operatorname{Ber}\parens*{#1}}
\newcommand{\Beta}[2]{\operatorname{Beta}\parens*{#1,#2}}
\newcommand{\ExpDist}[1]{\operatorname{ExpDist}\parens*{#1}}
\newcommand{\ErlangDist}[2]{\operatorname{ErlangDist}\parens*{#1,#2}}
\newcommand{\PoissonDist}[1]{\operatorname{PoissonDist}\parens*{#1}}
\newcommand{\GammaDist}[2]{\operatorname{Gamma}\parens*{#1,#2}}
\newcommand{\cind}[2]{\ind{#1\left| #2\right.}} % conditional indicator function
\renewcommand{\Pr}[1]{\operatorname{Pr}\parens*{#1}}
\DeclarePairedDelimiterX{\cPrParens}[2]{(}{)}{#1\,\delimsize\vert\,#2}
\newcommand{\cPr}[2]{\operatorname{Pr}\cPrParens{#1}{#2}}
\newcommand{\Ev}[2]{\operatorname{E}_{#1}\bracks*{#2}} % expected value
\newcommand{\cE}[3]{\Ev{#1}{\left.#2\right|#3}}
\newcommand{\Var}[2]{\operatorname{Var}_{#1}\bracks*{#2}}
\newcommand{\Cov}[2]{\operatorname{Cov}\bracks*{#1,#2}}
\newcommand{\CovMat}[1]{\operatorname{Cov}\bracks*{#1}}
% physics
% unit of measurement
\newcommand{\second}{\text{s}}
\newcommand{\hertz}{\text{Hz}}
\newcommand{\decibel}{\text{dB}}
% signal processing
% Discrete Time Fourier Transform
\newcommand{\DTFT}[1]{\mathrm{DTFT}\parens*{#1}}
\newcommand{\IDTFT}[1]{\mathrm{IDTFT}\parens*{#1}}
% computer science
% fixed-point arithmetic
\newcommand{\IntPartBW}[1]{\underbracket[0.140ex]{#1}_\mathrm{i}} % 0.140ex is half of the default thickness. See: [How to make underbracket thinner](https://tex.stackexchange.com/questions/559078/how-to-make-underbracket-thinner)
\newcommand{\DecPartBW}[1]{\underbracket[0.140ex]{#1}_\mathrm{d}}
\newcommand{\TotalBW}[1]{\underbracket[0.140ex]{#1}}
\newcommand{\Rat}[1]{\operatorname{Rat}\parens*{#1}} % Maps a fixed-point number to a corresponding rational number.
% programming
\newcommand{\plpl}{\mathrel{++}}
\newcommand{\pleq}{\mathrel{+}=}
\newcommand{\asteq}{\mathrel{*}=}
\]
回転行列$R$が与えられたときに、対応する回転ベクトル$\bm{r}$を求める方法を以下に述べる。回転軸の向きと回転方向に任意性があるが、回転ベクトルの向きに時計回りに回転させるものと見做す(つまり$\theta \geq 0$)。
まず、回転ベクトル$\bm{r}$から回転行列$R$への次の変換則(Rodriguesの公式)を既知とする(比較的簡単に導出できる)。
$$ \begin{aligned} \theta &= \|\bm{r}\|_2 \\ \tilde{\bm{r}} &= \begin{cases} \bm{r} / \theta & (\theta > 0) \\ \bm{0} & (\theta = 0) \end{cases} \\ R &= (\cos\theta)I + (1-\cos\theta)\tilde{\bm{r}}\tilde{\bm{r}}^\top + (\sin \theta) \begin{bmatrix} 0 & -\tilde{r}_z & \tilde{r}_y \\ \tilde{r}_z & 0 & -\tilde{r}_x \\ -\tilde{r}_y & \tilde{r}_x & 0 \end{bmatrix} \end{aligned} $$$\mathop{\mathrm{trace}}(R) = 1 + 2\cos\theta$が成り立つので$\cos\theta$が次式で求まる。
$$ \cos\theta = \frac{\mathop{\mathrm{trace}}(R) – 1}{2} $$後で解るが、$\sin\theta = 0$となる場合が厄介なので先に片付けておく。
$\cos\theta = 1$のときは無回転と同じなので$\bm{r} = [0,0,0]^\top$とすればよい。
$\cos\theta = -1$のときは回転ベクトルの向きに任意性があるが、$\pi$回転と見做す。回転軸を求めるには、適当なベクトル$\bm{v}_1$を$R$で回転させて$\bm{v}_2 = R\bm{v}_1$とし、$\tilde{\bm{r}} = (\bm{v}_1 + \bm{v}_2)/\|\bm{v}_1 + \bm{v}_2\|_2$とする。しかし$\bm{v}_1$として回転軸に垂直なものを選んでしまうと$\bm{v}_1 + \bm{v}_2 = \bm{0}$となり都合が悪い。そこで、$\bm{v}_1$として$x,y,z$各軸方向の単位ベクトル$\bm{i}_x, \bm{i}_y, \bm{i}_z$を試す。これらのうち少なくとも1つは回転軸と垂直でない(垂直度合が低いほど内積の絶対値が大きい。$\langle\bm{r},\bm{i}_x\rangle = r_x, \langle\bm{r},\bm{i}_y\rangle = r_y, \langle\bm{r},\bm{i}_z\rangle = r_z$であり、$\|\bm{r}\|=1$の下で$\max\{r_x^2,r_y^2,r_z^2\}$を最小化する問題の解の目的関数値は$1/3$なので、必ず都合の良いベクトルが見つかる)から、$\|\bm{v}_1 + \bm{v}_2\|_2$が最大となるものを$\bm{v}_1$として採用する。そして$\bm{r} = \pi\tilde{\bm{r}}$とする。
$\cos\theta \neq \pm 1$のとき$\sin\theta \neq 0$である。次式が成り立つ。
$$ (\sin \theta) \begin{bmatrix} 0 & -\tilde{r}_z & \tilde{r}_y \\ \tilde{r}_z & 0 & -\tilde{r}_x \\ -\tilde{r}_y & \tilde{r}_x & 0 \end{bmatrix} = \frac{R – R^\top}{2} $$これより次のベクトルを計算できる。
$$ \tilde{\bm{r}} = \frac{(\sin\theta)[\tilde{r}_x, \tilde{r}_y, \tilde{r}_z]^\top}{\|(\sin\theta)[\tilde{r}_x, \tilde{r}_y, \tilde{r}_z]^\top\|_2} = \mathop{\mathrm{sgn}}(\sin\theta) [\tilde{r}_x, \tilde{r}_y, \tilde{r}_z]^\top $$$\tilde{\bm{r}}$と平行でない適当なベクトルから$\tilde{\bm{r}}$方向の成分を除去して規格化して$\tilde{\bm{r}}$と垂直な大きさ1のベクトル$\bm{v}_1$を作る。具体的には、前述の$\bm{i}_x, \bm{i}_y, \bm{i}_z$を試して、$\tilde{\bm{r}}$との内積の絶対値が最小のものを用いて$\bm{v}_1$を作る。$\bm{v}_1$を$R$で回転させて$\bm{v}_2 \coloneqq R\bm{v}_1$とする。$\sin \theta$の符号で場合分けすると解るが、$\bm{v}_1\times\bm{v}_2$は$\tilde{\bm{r}}$と同じ向きになり、両者の内積が$|\sin\theta|$となる。$\sin\theta$の符号は確定できないが、$\psi \coloneqq \mathop{\mathrm{atan2}}(|\sin\theta|,\cos\theta)$として$\bm{r} = \psi\tilde{\bm{r}}$とすれば、$R$と同じ回転結果を導く、回転角度が0超過$\pi$未満であるような回転ベクトルを求めたことになる。
Python3による実装
import math
import numpy as np
def rotVec2rotMat(r: np.ndarray) -> np.ndarray:
assert r.shape == (3,1)
dataType = r.dtype
theta = np.linalg.norm(r)
if theta < np.deg2rad(0.001):
return np.identity(3, dtype=dataType)
r_ = r/theta
rx, ry, rz = r_.T[0]
term3 = np.array(
[
[0, -rz, ry],
[rz, 0, -rx],
[-ry, rx, 0]
],
dtype=dataType
)
return math.cos(theta)*np.identity(3) + (1-math.cos(theta))*np.inner(r_, r_) + math.sin(theta)*term3
def rotMat2rotVec(R: np.ndarray) -> np.ndarray:
assert R.shape == (3,3)
dataType = R.dtype
cos_theta = 0.5*(R.diagonal().sum()-1.0)
# case: nearly 0 rotation
if abs(1.0 - cos_theta) < np.deg2rad(0.001):
return np.array([[0,0,0]], dtype=dataType).T
I = np.identity(3, dtype=dataType)
n_1 = I[:,0]
n_2 = I[:,1]
n_3 = I[:,2]
# case: nearly pi rotation
if abs(1.0 + cos_theta) < np.deg2rad(0.001):
m_1 = R[:,0]
m_2 = R[:,1]
m_3 = R[:,2]
v_1 = n_1 + m_1
v_2 = n_2 + m_2
v_3 = n_3 + m_3
norm1 = np.linalg.norm(v_1)
norm2 = np.linalg.norm(v_2)
norm3 = np.linalg.norm(v_3)
v_and_norms = ((v_1, norm1), (v_2, norm2), (v_3, norm3))
v_and_norms_sorted = sorted(v_and_norms, key=lambda x: -x[1])
r = math.pi * v_and_norms_sorted[0][0] / v_and_norms_sorted[0][1]
return np.array([r], dtype=dataType).T
# case: theta is not multiple of pi
R2 = R - R.T
r_tilde2 = np.array([R2[2,1], R2[0,2], R2[1,0]], dtype=dataType)
r_tilde = r_tilde2 / np.linalg.norm(r_tilde2)
inprod_1 = np.inner(r_tilde, n_1)
inprod_2 = np.inner(r_tilde, n_2)
inprod_3 = np.inner(r_tilde, n_3)
n_and_inprods = ((n_1, inprod_1), (n_2, inprod_2), (n_3, inprod_3))
n_and_inprods_sorted = sorted(n_and_inprods, key=lambda x: x[1]**2)
n_good, inprod_good = n_and_inprods_sorted[0]
v_1_temp = n_good - inprod_good*r_tilde
v_1 = v_1_temp / np.linalg.norm(v_1_temp)
v_2 = np.matmul(R, v_1).astype(dataType)
v_3 = np.cross(v_1, v_2)
sin_theta = np.inner(r_tilde, v_3)
theta = math.atan2(sin_theta, cos_theta)
return np.array([theta*r_tilde], dtype=dataType).T
def test_rotMat2rotVec():
r_temp = np.array([[1],[2],[3]], np.double)
r_temp2 = r_temp / np.linalg.norm(r_temp)
theta = -math.pi/2
r = theta*r_temp2
R = rotVec2rotMat(r)
print('R:\n', R)
r_hat = rotMat2rotVec(R)
print('r_hat:\n', r_hat)
R_hat = rotVec2rotMat(r_hat)
print('R_hat:\n', R_hat)
test_rotMat2rotVec()
# R:
# [[ 0.07142857 0.94464087 -0.32023677]
# [-0.65892658 0.28571429 0.69583267]
# [ 0.7488082 0.16131019 0.64285714]]
# r_hat:
# [[-0.41981298]
# [-0.83962595]
# [-1.25943893]]
# R_hat:
# [[ 0.07142857 0.94464087 -0.32023677]
# [-0.65892658 0.28571429 0.69583267]
# [ 0.7488082 0.16131019 0.64285714]]
コメントを残す
コメントを投稿するにはログインしてください。