视觉SLAM中常用李群来表示一个相机的姿态,而在SLAM的计算中常常要用到李群和李代数中的指数映射和对数映射,本文是学习其指数映射和对数映射推导过程的笔记。

预备知识

\(\vec r = (\alpha, \beta,\gamma) ^T\)\(\vec T = (T_x,T_y,T_z) ^T\)\(\vec \mu = (\vec r,\vec T) ^T\)

矩阵叉积: \[ \vec r \times \vec t = \left[\begin{matrix} \beta t_z & - & \gamma t_y \\ \gamma t_x & - & \alpha t_z \\ \alpha t_y & - & \beta t_x \end{matrix}\right] \\ \vec r \times \vec t = \hat {\vec r} \vec t \] 其中 \(\hat{\vec r}\) 表示向量\(\vec r\) 的反对称矩阵 \[ r = \hat{\vec r} = \left[ \begin{matrix} 0 & -\gamma & \beta \\ \gamma & 0 & -\alpha \\ -\beta & \alpha &0 \end{matrix}\right] \] 反对称矩阵的性质: \[ \hat {\vec r} ^T = - \hat {\vec r} \]


SO(3)

指数映射:由三维向量得到旋转矩阵

这个过程被称为指数映射,通过三维向量的反对称矩阵的的指数映射来的到旋转矩阵。矩阵的指数映射可以使用泰勒公式展开: \[ R = exp(\hat {\vec r}) \]

\[ exp(A) = \sum_{n = 0}^\infty {\frac{1}{n!}A^n} \]

指数映射有向量映射到矩阵,对于旋转矩阵: \[ \begin{split} R = exp(r) &= I + r + \frac{1}{2!} r^2 +\dots +\frac{1}{n!}r^n \\ &= I + \sum_{i = 0}^\infty{\left[ \frac{r^{2i+1}}{(2i+1 !)} +\frac{r^{2i+2}}{(2i+2 !)} \right]} \end{split} \]

\[ \vec r的模长:\theta = |\vec r| \\ r^3 = - |\vec r|r = -\theta r \\ r^{2i+1} = (-1)^i \theta ^{2i}r \\ r^{2i+2} = (-1)^i \theta ^{2i} r^2 \]

最后的推导就是: \[ \begin{split} R = exp(r) &= I + r + \frac{1}{2!} r^2 +\dots +\frac{1}{n!}r^n \\ &= I + \sum_{i = 0}^\infty{\left[ \frac{r^{2i+1}}{(2i+1 )!} +\frac{r^{2i+2}}{(2i+2 )!} \right]} \\ &= I + \left( 1-\frac{\theta ^2}{3!} + \frac{\theta ^4}{5!}- \dots\right) r+ \left( \frac{1}{2!}-\frac{\theta ^2}{4!} + \frac{\theta ^4}{6!}-\dots\right)r^2 \\ &= I +\frac{1}{\theta} \left( 1-\frac{\theta ^3}{3!} + \frac{\theta ^5}{5!}- \dots + (-1)^n\frac{\theta ^{2n+1}}{(2n+1)! }\right) r \\ &+ \frac{1}{\theta ^2}\left( \frac{\theta ^2}{2!}-\frac{\theta ^4}{4!} + \frac{\theta ^6}{6!}-\dots +(-1)^{n+1}\frac{\theta ^{2n}}{(2n)!} \right)r^2 \\ &= I +\frac{1}{\theta} \left( 1-\frac{\theta ^3}{3!} + \frac{\theta ^5}{5!}- \dots + (-1)^n\frac{\theta ^{2n+1}}{(2n+1)! }\right) r \\ & + \frac{1}{\theta ^2}\left[1-\left(1- \frac{\theta ^2}{2!}+\frac{\theta ^4}{4!} - \frac{\theta ^6}{6!}+\dots +(-1)^{n+1}\frac{\theta ^{2n}}{(2n)!} \right)\right]r^2 \\ &= I + \left( \frac{\sin \theta}{\theta} \right) r + \left( \frac{1- \cos \theta}{\theta ^2} \right) r^2 \end{split} \] 由三维的向量通过指数映射得到旋转矩阵的推导介绍啦。

对数映射:由旋转矩阵得到三维向量

这个过程被称为对数映射 \[ \vec r = log(R) \] 已知旋转矩阵R,那么旋转角可以通过R的迹来求得: \[ tr(R) = 1+2\cos \varphi \\ \varphi = \arccos {\frac{1}{2}(tr(R)-1)} \\ r = \frac{\varphi}{2 \sin \varphi} (R-R^T) \]


SE(3)

指数映射:由六维向量到 \(\left[\begin{smallmatrix}R & T\\0&1\end{smallmatrix}\right]\) 矩阵

\(SE(3)\) 不仅包含旋转信息,还包含了位移信息,旋转部分用 \(SO(3)\) 表达,其指数映射的推导过程和 \(SO(3)\) 类似,直接给出结论: \[ exp(\hat {\vec \mu}) =exp\left(\begin{matrix}\vec r \\ \vec T\end{matrix}\right) = \left[\begin{matrix} exp(r)& VT\\ 0&1 \end{matrix}\right] = \left[\begin{matrix} R& t\\ 0&1 \end{matrix}\right] \\ \begin{split} \vec t & = (I+\frac{1-cos\theta}{\theta^2}r + \frac{\theta-\sin\theta}{\theta ^3}r^2)T \\ & = \vec r 'T \end{split} \]

对数映射:由矩阵到六维向量

\[ log\left[\begin{matrix}R&t\\0&1\end{matrix}\right] = log\left[\begin{matrix}exp(\vec r)&t\\0&1\end{matrix}\right] = \left[\begin{matrix}r&A^{-1}t\\0&0\end{matrix}\right] \\ A^{-1} = I-\frac{1}{2}r + \frac{2\sin\theta - \theta(1+\cos\theta)}{2\theta^2\sin\theta}r^2 \]