目的

Scale drift-awareな場合におけるpose graph optimization(こちらなど)で登場するSim3 optimizationにおいて， $\mathrm{Sim(3)}$ と $\mathfrak{sim}\mathrm{(3)}$ の変換式を導出する必要があったため，その変換式(exponential map)を求めます．

exponential map の導出

準備

3次元相似変換行列 ${\bf S}$ を，

\begin{align}
    {\bf S}
    &=
    \begin{bmatrix}
        s \cdot {\bf R} && {\bf t} \\
        {\bf 0}_3^{\mathsf T} && 1
    \end{bmatrix}
\end{align}

とします．

群 $\mathrm{Sim(3)}$ の生成子(generator)は，

\begin{align}
    {\bf G}_1
    &=
    \begin{bmatrix}
        0 && 0 && 0 && 1 \\
        0 && 0 && 0 && 0 \\
        0 && 0 && 0 && 0 \\
        0 && 0 && 0 && 0 \\
    \end{bmatrix}
    , &
    {\bf G}_2
    &=
    \begin{bmatrix}
        0 && 0 && 0 && 0 \\
        0 && 0 && 0 && 1 \\
        0 && 0 && 0 && 0 \\
        0 && 0 && 0 && 0 \\
    \end{bmatrix}
    , &
    {\bf G}_3
    &=
    \begin{bmatrix}
        0 && 0 && 0 && 0 \\
        0 && 0 && 0 && 0 \\
        0 && 0 && 0 && 1 \\
        0 && 0 && 0 && 0 \\
    \end{bmatrix}
    ,
    \\
    {\bf G}_4
    &=
    \begin{bmatrix}
        0 && 0 && 0 && 0 \\
        0 && 0 && -1 && 0 \\
        0 && 1 && 0 && 0 \\
        0 && 0 && 0 && 0 \\
    \end{bmatrix}
    , &
    {\bf G}_5
    &=
    \begin{bmatrix}
        0 && 0 && 1 && 0 \\
        0 && 0 && 0 && 0 \\
        -1 && 0 && 0 && 0 \\
        0 && 0 && 0 && 0 \\
    \end{bmatrix}
    , &
    {\bf G}_6
    &=
    \begin{bmatrix}
        0 && -1 && 0 && 0 \\
        1 && 0 && 0 && 0 \\
        0 && 0 && 0 && 1 \\
        0 && 0 && 0 && 0 \\
    \end{bmatrix}
    ,
    \\
    {\bf G}_7
    &=
    \begin{bmatrix}
        1 && 0 && 0 && 0 \\
        0 && 1 && 0 && 0 \\
        0 && 0 && 1 && 0 \\
        0 && 0 && 0 && 0 \\
    \end{bmatrix}
\end{align}

です．
すなわち，リー代数 $\mathfrak{sim}\mathrm{(3)}$ の任意の元 ${\bf M}$ は，これらの一次結合

{\bf M} = v_1 {\bf G}_1 + v_2 {\bf G}_2 + v_3 {\bf G}_3 + \omega_1 {\bf G}_4 + \omega_2 {\bf G}_5 + \omega_3 {\bf G}_6 + \lambda {\bf G}_7

で表されます．
さらに，${\bf M}$ の exponential map $\mathrm{exp}: \mathfrak{sim}\mathrm{(3)} \rightarrow \mathrm{Sim(3)}$ は，$\mathrm{exp({\bf M})} \in \mathrm{Sim(3)}$ となります．

ここで，${\bf v} = [v_1, v_2, v_3]^{\mathsf T}$，${\bf \omega} = [\omega_1, \omega_2, \omega_3]^{\mathsf T}$と置くと，

\begin{align}
    {\bf M}
    &=
    \begin{bmatrix}
        \lambda \cdot {\bf I}_3 + [{\bf \omega}]_\times && {\bf v} \\
        {\bf 0}_3^{\mathsf T} && 0
    \end{bmatrix}
\end{align}

となります（$[{\bf \omega}]_\times$ は外積行列）．

${\bf M} \in \mathfrak{sim}\mathrm{(3)}$ の $({\bf v}, {\bf \omega}, \lambda)$ から ${\bf S} \in \mathrm{Sim(3)}$ の $({\bf t}, {\bf R}, s)$ を求める式を得ることが最終目標です．

導出

行列指数関数の定義による展開

まずは，$\mathrm{exp({\bf M})}$ を行列指数関数の定義に従って展開します．

\begin{align}
    {\bf M}^k
    &=
    \begin{bmatrix}
        \lambda \cdot {\bf I}_3 + [{\bf \omega}]_\times && {\bf v} \\
        {\bf 0}_3^{\mathsf T} && 0
    \end{bmatrix}^k
    \\
    &=
    \begin{bmatrix}
        (\lambda \cdot {\bf I}_3 + [{\bf \omega}]_\times)^k && (\lambda \cdot {\bf I}_3 + [{\bf \omega}]_\times)^{k - 1} {\bf v} \\
        {\bf 0}_3^{\mathsf T} && 0
    \end{bmatrix}
\end{align}

の関係式を用いると，

\begin{align}
    \mathrm{exp({\bf M})}
    &=
    \sum_{k = 0}^{\infty} \frac{1}{k!} {\bf M}^k
    \\
    &=
    {\bf I}_4 + \sum_{k = 1}^{\infty} \frac{1}{k!} {\bf M}^k
    \\
    &=
    {\bf I}_4 + \sum_{k = 1}^{\infty} \frac{1}{k!}
    \begin{bmatrix}
        (\lambda \cdot {\bf I}_3 + [{\bf \omega}]_\times)^k && (\lambda \cdot {\bf I}_3 + [{\bf \omega}]_\times)^{k - 1} {\bf v} \\
        {\bf 0}_3^{\mathsf T} && 0
    \end{bmatrix}
    \\
    &=
    \begin{bmatrix}
        {\bf I}_3 + \sum_{k = 1}^{\infty} \frac{1}{k!}(\lambda \cdot {\bf I}_3 + [{\bf \omega}]_\times)^k && \sum_{k = 1}^{\infty} \frac{1}{k!}(\lambda \cdot {\bf I}_3 + [{\bf \omega}]_\times)^{k - 1} {\bf v} \\
        {\bf 0}_3^{\mathsf T} && 0
    \end{bmatrix}
    \\
    &=
    \begin{bmatrix}
        \mathrm{exp}(\lambda \cdot {\bf I}_3 + [{\bf \omega}]_\times) && \left[ \sum_{k = 1}^{\infty} \frac{1}{k!}(\lambda \cdot {\bf I}_3 + [{\bf \omega}]_\times)^{k - 1} \right] {\bf v} \\
        {\bf 0}_3^{\mathsf T} && 0
    \end{bmatrix}
\end{align}

ここで，

\begin{align}
    \mathrm{exp}(\lambda \cdot {\bf I}_3 + [{\bf \omega}]_\times)
    &=
    \mathrm{exp}(\lambda \cdot {\bf I}_3) \cdot \mathrm{exp}([{\bf \omega}]_\times)
    \\
    &=
    \mathrm{exp}(\lambda) \cdot {\bf I}_3 \cdot \mathrm{exp}([{\bf \omega}]_\times)
    \\
    &=
    \mathrm{exp}(\lambda) \cdot \mathrm{exp}([{\bf \omega}]_\times)
    \\
    &=
    s \cdot {\bf R}
\end{align}

であるため，$s = \mathrm{exp}(\lambda)$，${\bf R} = \mathrm{exp}([{\bf \omega}]_\times)$ となります．

SO(3) の exponential map

$\mathrm{exp}([{\bf \omega}]_\times)$ を計算します．$\theta^2 = {\bf \omega}^{\mathsf T} {\bf \omega}$ と置くと，

[{\bf \omega}]_\times^{2k + 1} = (-1)^k \theta^{2k} [{\bf \omega}]_\times \\
[{\bf \omega}]_\times^{2k + 2} = (-1)^k \theta^{2k} [{\bf \omega}]_\times^2

の関係があります．これを用いると，

\begin{align}
\mathrm{exp}([{\bf \omega}]_\times)
&=
\sum_{k = 0}^{\infty} \frac{1}{k!} ([{\bf \omega}]_\times)^k
\\
&=
{\bf I}_3 + \sum_{k = 0}^{\infty} \Biggr[  \frac{1}{(2k + 1)!} [{\bf \omega}]_\times^{2k + 1} + \frac{1}{(2k + 2)!} [{\bf \omega}]_\times^{2k + 2} \Biggr]
\\
&=
{\bf I}_3 + \Biggl[ \sum_{k = 0}^{\infty} \frac{1}{(2k + 1)!} (-1)^k \theta^{2k} \Biggr] [{\bf \omega}]_\times + \Biggl[ \sum_{k = 0}^{\infty} \frac{1}{(2k + 2)!} (-1)^k \theta^{2k} \Biggr] [{\bf \omega}]_\times^2
\end{align}

と変形できます．$\theta = 0$ であれば $\mathrm{exp}([{\bf \omega}]_\times) = {\bf I}_3$ です．$\theta \neq 0$ とすると，

\begin{align}
\mathrm{exp}([{\bf \omega}]_\times)
&=
{\bf I}_3 + \Biggl[ \sum_{k = 0}^{\infty} \frac{1}{(2k + 1)!} (-1)^k \theta^{2k} \Biggr] [{\bf \omega}]_\times + \Biggl[ \sum_{k = 0}^{\infty} \frac{1}{(2k + 2)!} (-1)^k \theta^{2k} \Biggr] [{\bf \omega}]_\times^2
\\
&=
{\bf I}_3 + \frac{1}{\theta} \Biggl[ \sum_{k = 0}^{\infty} \frac{1}{(2k + 1)!} (-1)^k \theta^{2k + 1} \Biggr] [{\bf \omega}]_\times + \frac{1}{\theta^2} \Biggl( 1 - \Biggl[ \sum_{k = -1}^{\infty} \frac{1}{(2k + 2)!} (-1)^{k + 1} \theta^{2k + 2} \Biggr] \Biggr) [{\bf \omega}]_\times^2
\\
&=
{\bf I}_3 + \frac{\sin{(\theta)}}{\theta} [{\bf \omega}]_\times + \frac{1 - \cos{(\theta)}}{\theta^2} [{\bf \omega}]_\times^2
\end{align}

となります．つまり，

\begin{align}
{\bf R} = {\bf I}_3 + \frac{\sin{(\theta)}}{\theta} [{\bf \omega}]_\times + \frac{1 - \cos{(\theta)}}{\theta^2} [{\bf \omega}]_\times^2
\end{align}

となります．これは，exponential map $\mathrm{exp}: \mathfrak{so}\mathrm{(3)} \rightarrow \mathrm{SO(3)}$ です．

並進成分の変換

また，

{\bf W}
=
\sum_{k = 1}^{\infty} \frac{1}{k!}(\lambda \cdot {\bf I}_3 + [{\bf \omega}]_\times)^{k - 1}
=
{\bf I}_3 + \sum_{k = 1}^{\infty} \frac{1}{(k + 1)!}(\lambda \cdot {\bf I}_3 + [{\bf \omega}]_\times)^{k}

と置き，

{\bf I}_3 + \sum_{k = 1}^{\infty} \frac{1}{(k + 1)!} {\bf A}^k
=
\int_0^1 \mathrm{exp}(t \cdot {\bf A}) dt

の関係式を適用すると，${\bf W}$ は

{\bf W}
=
\int_0^1 \mathrm{exp} \bigl( t \cdot (\lambda \cdot {\bf I}_3 + [{\bf \omega}]_\times) \bigr) dt

で得られることが分かります．これを展開していきます．

\begin{align}
{\bf W}
&=
\int_0^1 \mathrm{exp} \bigl( t \cdot (\lambda \cdot {\bf I}_3 + [{\bf \omega}]_\times) \bigr) dt
\\
&=
\int_0^1 \mathrm{exp} ( t \cdot \lambda \cdot {\bf I}_3) \cdot \mathrm{exp}(t \cdot [{\bf \omega}]_\times) dt
\\
&=
\int_0^1 \mathrm{exp} ( t \cdot \lambda) \cdot \mathrm{exp}(t \cdot [{\bf \omega}]_\times) dt
\end{align}

$\mathrm{exp}(t \cdot [{\bf \omega}]_\times)$ を計算します．$\theta^2 = {\bf \omega}^{\mathsf T} {\bf \omega}$ と置くと，

[{\bf \omega}]_\times^{2k + 1} = (-1)^k \theta^{2k} [{\bf \omega}]_\times \\
[{\bf \omega}]_\times^{2k + 2} = (-1)^k \theta^{2k} [{\bf \omega}]_\times^2

の関係があります(再掲)．これを用いると，

\begin{align}
\mathrm{exp}(t \cdot [{\bf \omega}]_\times)
&=
\sum_{k = 0}^{\infty} \frac{1}{k!} (t \cdot [{\bf \omega}]_\times)^k
\\
&=
{\bf I}_3 + \sum_{k = 0}^{\infty} \Biggr[  \frac{1}{(2k + 1)!} t^{2k + 1} [{\bf \omega}]_\times^{2k + 1} + \frac{1}{(2k + 2)!} t^{2k + 2} [{\bf \omega}]_\times^{2k + 2} \Biggr]
\\
&=
{\bf I}_3 + t \Biggl[ \sum_{k = 0}^{\infty} \frac{1}{(2k + 1)!} (-1)^k (t \, \theta)^{2k} \Biggr] [{\bf \omega}]_\times + t^2 \Biggl[ \sum_{k = 0}^{\infty} \frac{1}{(2k + 2)!} (-1)^k (t \, \theta)^{2k} \Biggr] [{\bf \omega}]_\times^2
\end{align}

と変形できます．$\theta \neq 0$ とすると，

\begin{align}
\mathrm{exp}(t \cdot [{\bf \omega}]_\times)
&=
{\bf I}_3 + t \Biggl[ \sum_{k = 0}^{\infty} \frac{1}{(2k + 1)!} (-1)^k (t \, \theta)^{2k} \Biggr] [{\bf \omega}]_\times + t^2 \Biggl[ \sum_{k = 0}^{\infty} \frac{1}{(2k + 2)!} (-1)^k (t \, \theta)^{2k} \Biggr] [{\bf \omega}]_\times^2
\\
&=
{\bf I}_3 + \frac{t}{t \, \theta} \Biggl[ \sum_{k = 0}^{\infty} \frac{1}{(2k + 1)!} (-1)^k (t \, \theta)^{2k + 1} \Biggr] [{\bf \omega}]_\times + \frac{t^2}{t^2 \, \theta^2} \Biggl( 1 - \Biggl[ \sum_{k = -1}^{\infty} \frac{1}{(2k + 2)!} (-1)^{k + 1} (t \, \theta)^{2k + 2} \Biggr] \Biggr) [{\bf \omega}]_\times^2
\\
&=
{\bf I}_3 + \frac{\sin{(t \, \theta)}}{\theta} [{\bf \omega}]_\times + \frac{1 - \cos{(t \, \theta)}}{\theta^2} [{\bf \omega}]_\times^2
\end{align}

となります．$\theta = 0$ であれば，$\mathrm{exp}(t \cdot [{\bf \omega}]_\times) = {\bf I}_3$ となります．

$\theta \neq 0$ のときの $\mathrm{exp}(t \cdot [{\bf \omega}]_\times)$ を ${\bf W}$ に代入すると，

\begin{align}
{\bf W}
&=
\int_0^1 \mathrm{exp} ( t \, \lambda) \, \mathrm{exp}(t \, [{\bf \omega}]_\times) \, dt
\\
&=
\int_0^1 e^{t \lambda} \, \biggl( {\bf I}_3 + \frac{\sin{(t \, \theta)}}{\theta} [{\bf \omega}]_\times + \frac{1 - \cos{(t \, \theta)}}{\theta^2} [{\bf \omega}]_\times^2 \biggr) \, dt
\\
&=
\int_0^1 \biggl( e^{t \lambda} \, {\bf I}_3 + e^{t \lambda} \sin{(t \, \theta)} \frac{[{\bf \omega}]_\times}{\theta} + e^{t \lambda} \frac{[{\bf \omega}]_\times^2}{\theta^2} - e^{t \lambda} \cos{(t \, \theta)} \frac{[{\bf \omega}]_\times^2}{\theta^2} \biggr) \, dt
\end{align}

$\lambda \neq 0$ として積分すると，

\begin{align}
{\bf W}
&=
\frac{e^\lambda - 1}{\lambda} \, {\bf I}_3
\\
&\quad +
\frac{\lambda e^\lambda \sin{(\theta)} + \theta (1 - e^\lambda \cos{(\theta)})}{\lambda^2 + \theta^2} \frac{[{\bf \omega}]_\times}{\theta}
\\
&\quad +
\biggl(
\frac{e^\lambda - 1}{\lambda}
-
\frac{\theta e^\lambda \sin{(\theta)} + \lambda (e^\lambda \cos{(\theta)} - 1)}{\lambda^2 + \theta^2}
\biggr)
\frac{[{\bf \omega}]_\times^2}{\theta^2}
\end{align}

となります．

まとめ

以上より、リー代数 $\mathfrak{sim}\mathrm{(3)}$ の任意の元 ${\bf M}$ の exponential map $\mathrm{exp}: \mathfrak{sim}\mathrm{(3)} \rightarrow \mathrm{Sim(3)}$ は

\begin{align}
    \mathrm{exp({\bf M})}
    =
    \begin{bmatrix}
        \mathrm{exp}(\lambda) \, \mathrm{exp}([{\bf \omega}]_\times) && {\bf W} {\bf v} \\
        {\bf 0}_3^{\mathsf T} && 0
    \end{bmatrix}
\end{align}

となります．

いくつか場合分けを省略していますが($\theta = 0$の場合など)，上記のパターンの導出が一番難しいと思うので，これ以外の場合については皆さんで頑張ってください．