6.2 Exponential Map

As above, the exponential map from $\mathrm{sim}(3)$ to $\mathrm{Sim}(3)$ is the matrix exponential on a linear combination of the generators:

$$\boldsymbol{\delta}=\left(\begin{array}{cc} \mathbf{u} & \boldsymbol{\omega}\end{array}\lambda\right) \in \mathrm{sim}(3)$$ (145)

$$\exp\left(\boldsymbol{\delta}\right) = \exp\left(\begin{array}{c\vert c} \boldsymbol{\omega}_{\times} & \mathbf{u}\\ \hline \mathbf{0} & -\lambda \end{array}\right)$$ (146)

$$= \mathbf{I}+\left(\begin{array}{c\vert c} \boldsymbol{\omega}_{\ti... ...bda^{2}\mathbf{u}\\ \hline \mathbf{0} & -\lambda^{3} \end{array}\right)+\cdots$$ (147)

The series is similar to that for $\mathrm{se}(3)$, but now rotation, translation and scale are being interleaved infinitesimally. The rotation and scale components of the exponential are immediately clear, but the translation component involves the mixing of the three. We can write out the series for the translation multiplier:

$$\exp\left(\begin{array}{c\vert c} \boldsymbol{\omega}_{\times} & \mathbf{u}\\ \hline \mathbf{0} & 0 \end{array}\right) = \left(\begin{array}{c\vert c} \exp\left(\boldsymbol{\omega}_{\tim... ...}\mathbf{u}\\ \hline \mathbf{0} & \exp\left(-\lambda\right) \end{array}\right)$$ (148)

$$\mathbf{V} = \sum_{n=0}^{\infty}\sum_{k=0}^{n}\dfrac{\boldsymbol{\omega}_{\times}^{n-k}\left(-\lambda\right)^{k}}{\left(n+1\right)!}$$ (149)

$$= \sum_{k=0}^{\infty}\sum_{n=k}^{\infty}\dfrac{\boldsymbol{\omega}_{\times}^{n-k}\left(-\lambda\right)^{k}}{\left(n+1\right)!}$$ (150)

$$= \sum_{k=0}^{\infty}\sum_{j=0}^{\infty}\dfrac{\boldsymbol{\omega}_{\times}^{j}\left(-\lambda\right)^{k}}{\left(j+k+1\right)!}$$ (151)

Again letting $\theta^{2}=\boldsymbol{\omega}^{T}\boldsymbol{\omega}$, and using the identity from Eq. 9, we partition the terms into odd and even powers of $\boldsymbol{\omega}_{\times}$, and factor:

$$\mathbf{V} = \left(\sum_{k=0}^{\infty}\dfrac{\left(-\lambda\right)^{k}}{\left(... ...2i+1}}{(2i+k+2)!}+\dfrac{\boldsymbol{\omega}_{\times}^{2i+2}}{(2i+k+3)!}\right]$$ (152)

$$= \left(\sum_{k=0}^{\infty}\dfrac{\left(-\lambda\right)^{k}}{\left(... ...2i}\left(-\lambda\right)^{k}}{(2i+k+3)!}\right)\boldsymbol{\omega}_{\times}^{2}$$ (153)

The first coefficient is easily identified as a Taylor series, leaving the other two coefficients to be analyzed:

$$\mathbf{V} = A\mathbf{I}+B\boldsymbol{\omega}_{\times}+C\boldsymbol{\omega}_{\times}^{2}$$ (154)

$$A = \dfrac{1-\exp\left(-\lambda\right)}{\lambda}$$ (155)

$$B = \sum_{k=0}^{\infty}\sum_{i=0}^{\infty}\dfrac{(-1)^{i}\theta^{2i}\left(-\lambda\right)^{k}}{(2i+k+2)!}$$ (156)

$$C = \sum_{k=0}^{\infty}\sum_{i=0}^{\infty}\dfrac{(-1)^{i}\theta^{2i}\left(-\lambda\right)^{k}}{(2i+k+3)!}$$ (157)

Consider coefficient $B$:

$$B = \sum_{k=0}^{\infty}\sum_{i=0}^{\infty}\dfrac{(-1)^{i}\theta^{2i}\left(-\lambda\right)^{k}}{(2i+k+2)!}$$ (158)

$$= \sum_{i=0}^{\infty}\left[(-1)^{i}\theta^{2i}\sum_{k=0}^{\infty}\dfrac{\left(-\lambda\right)^{k}}{(2i+k+2)!}\right]$$ (159)

$$= \sum_{i=0}^{\infty}\left[\dfrac{(-1)^{i}\theta^{2i}}{\lambda^{2i}}\sum_{k=0}^{\infty}\dfrac{\left(-\lambda\right)^{2i+k}}{(2i+k+2)!}\right]$$ (160)

$$= \sum_{i=0}^{\infty}\left[\dfrac{(-1)^{i}\theta^{2i}}{\lambda^{2i}}\sum_{m=2i}^{\infty}\dfrac{\left(-\lambda\right)^{m}}{(m+2)!}\right]$$ (161)

$$= \sum_{i=0}^{\infty}\left(\dfrac{(-1)^{i}\theta^{2i}}{\lambda^{2i}... ...m+2)!}-\sum_{m=0}^{2i-1}\dfrac{\left(-\lambda\right)^{m}}{(m+2)!}\right]\right)$$ (162)

$$= \sum_{i=0}^{\infty}\left(\dfrac{(-1)^{i}\theta^{2i}}{\lambda^{2i}... ...{\lambda^{2i}}\sum_{m=0}^{2i-1}\dfrac{\left(-\lambda\right)^{m}}{(m+2)!}\right)$$ (163)

Manipulating the second term yields a more useful form:

$$L \equiv \sum_{i=0}^{\infty}\left(\dfrac{(-1)^{i}\theta^{2i}}{\lambda^{2i}}\left[\sum_{m=0}^{\infty}\dfrac{\left(-\lambda\right)^{m}}{(m+2)!}\right]\right)$$ (164)

$$B = L-\sum_{i=0}^{\infty}\left(\dfrac{(-1)^{i}\theta^{2i}}{\lambda^{2i}}\sum_{m=0}^{2i-1}\dfrac{\left(-\lambda\right)^{m}}{(m+2)!}\right)$$ (165)

$$= L-\sum_{i=0}^{\infty}\left(\dfrac{(-1)^{i}\theta^{2i}}{\lambda^{2... ...ght)^{2p}}{(2p+2)!}+\dfrac{\left(-\lambda\right)^{2p+1}}{(2p+3)!}\right]\right)$$ (166)

$$= L-\sum_{i=0}^{\infty}\left(\dfrac{(-1)^{i}\theta^{2i}}{\lambda^{2... ...mbda\right)^{2p}\left[\dfrac{1}{(2p+2)!}-\dfrac{\lambda}{(2p+3)!}\right]\right)$$ (167)

$$= L-{\displaystyle \sum_{p<i}^{\infty}}\left(\dfrac{(-1)^{i}\theta^... ...mbda\right)^{2p}\left[\dfrac{1}{(2p+2)!}-\dfrac{\lambda}{(2p+3)!}\right]\right)$$ (168)

$$= L-{\displaystyle \sum_{p<i}^{\infty}}\left(\dfrac{(-1)^{i}\theta^... ...lambda^{2(i-p)}}\left[\dfrac{1}{(2p+2)!}-\dfrac{\lambda}{(2p+3)!}\right]\right)$$ (169)

$$= L-{\displaystyle \sum_{p=0}^{\infty}\sum_{q=1}^{\infty}}\left(\df... ...(2p+2)!}-\lambda\left(\dfrac{(-1)^{p}\theta^{2p}}{(2p+3)!}\right)\right]\right)$$ (170)

$$= L-\left(\sum_{q=1}^{\infty}\dfrac{(-1)^{q}\theta^{2q}}{\lambda^{2... ...(2p+2)!}-\lambda\left(\dfrac{(-1)^{p}\theta^{2p}}{(2p+3)!}\right)\right]\right)$$ (171)

$$= \begin{array}[t]{l} \left(\sum_{i=0}^{\infty}\dfrac{(-1)^{i}\... ...{(-1)^{p}\theta^{2p}}{(2p+3)!}\right)\right]\right) \end{array}$$ (172)

Relabel the factors:

$$B = \alpha\cdot\beta-\left(\alpha-1\right)\cdot\gamma$$ (173)

$$= \alpha\cdot\left(\beta-\gamma\right)+\gamma$$ (174)

$$\alpha \equiv \sum_{i=0}^{\infty}\dfrac{(-1)^{i}\theta^{2i}}{\lambda^{2i}}$$ (175)

$$\beta \equiv \sum_{m=0}^{\infty}\dfrac{\left(-\lambda\right)^{m}}{(m+2)!}$$ (176)

$$\gamma \equiv \sum_{p=0}^{\infty}\left[\dfrac{(-1)^{p}\theta^{2p}}{(2p+2)!}-\lambda\left(\dfrac{(-1)^{p}\theta^{2p}}{(2p+3)!}\right)\right]$$ (177)

The factors are then recognised as Taylor series:

$$\alpha = \dfrac{\lambda^{2}}{\lambda^{2}+\theta^{2}}$$ (178)

$$\beta = \dfrac{\exp\left(-\lambda\right)-1+\lambda}{\lambda^{2}}$$ (179)

$$\gamma = \dfrac{1-\cos\theta}{\theta^{2}}-\lambda\left(\dfrac{\theta-\sin\theta}{\theta^{3}}\right)$$ (180)

By similar algebra, a formula for coefficient $C$ can be derived from Eq. 157:

$$C = \alpha\cdot\left(\mu-\nu\right)+\nu$$ (181)

$$\mu = \dfrac{1-\lambda+\frac{1}{2}\lambda^{2}-\exp\left(-\lambda\right)}{\lambda^{2}}$$ (182)

$$\nu = \dfrac{\theta-\sin\theta}{\theta^{3}}-\lambda\left(\dfrac{\cos\theta-1+\frac{\theta^{2}}{2}}{\theta^{4}}\right)$$ (183)

Combining these results gives a closed-form exponential map for $\mathrm{sim(3)}$:

$$\left(\begin{array}{ccc} \mathbf{u} & \boldsymbol{\omega} & \lambda\end{array}\right){}^{T} \in \mathrm{sim(3)}$$ (184)

$$\theta^{2} = \boldsymbol{\omega}^{T}\boldsymbol{\omega}$$ (185)

$$X = \dfrac{\sin\theta}{\theta}$$ (186)

$$Y = \dfrac{1-\cos\theta}{\theta^{2}}$$ (187)

$$Z = \dfrac{1-X}{\theta^{2}}$$ (188)

$$W = \dfrac{\frac{1}{2}-Y}{\theta^{2}}$$ (189)

$$\alpha = \dfrac{\lambda^{2}}{\lambda^{2}+\theta^{2}}$$ (190)

$$\beta = \dfrac{\exp\left(-\lambda\right)-1+\lambda}{\lambda^{2}}$$ (191)

$$\gamma = Y-\lambda Z$$ (192)

$$\mu = \dfrac{1-\lambda+\frac{1}{2}\lambda^{2}-\exp\left(-\lambda\right)}{\lambda^{3}}$$ (193)

$$\nu = Z-\lambda W$$ (194)

$$A = \dfrac{1-\exp\left(-\lambda\right)}{\lambda}$$ (195)

$$B = \alpha\cdot\left(\beta-\gamma\right)+\gamma$$ (196)

$$C = \alpha\cdot\left(\mu-\nu\right)+\nu$$ (197)

$$\mathbf{R} = \mathbf{I}+a\boldsymbol{\omega}_{\times}+b\boldsymbol{\omega}_{\times}^{2}$$ (198)

$$\mathbf{V} = A\mathbf{I}+B\boldsymbol{\omega}_{\times}+C\boldsymbol{\omega}_{\times}^{2}$$ (199)

$$\exp\left(\begin{array}{c} \mathbf{u}\\ \boldsymbol{\omega}\\ \lambda \end{array}\right) = \left(\begin{array}{c\vert c} \mathbf{R} & \mathbf{V}\mathbf{u}\\ \hline \mathbf{0} & \exp\left(-\lambda\right) \end{array}\right)$$ (200)

Again, Taylor expansions should be used when $\lambda^{2}$ or $\theta^{2}$ is small. The $\ln()$ function can be implemented by first recovering $\boldsymbol{\omega}$ and $\lambda$, constructing $\mathbf{V}$, and then solving for $\mathbf{u}$ (as in the $\mathrm{SE}(3)$ case).