目的
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$の場合など)，上記のパターンの導出が一番難しいと思うので，これ以外の場合については皆さんで頑張ってください．