视觉SLAM_06_指数与对数映射

一字艺

已于 2022-02-12 18:38:28 修改

阅读量2.1k

点赞数

分类专栏：视觉SLAM 文章标签：计算机视觉人工智能自动驾驶算法

于 2022-02-04 18:30:25 首次发布

本文链接：https://blog.csdn.net/xa996904892/article/details/122784864

版权

视觉SLAM 专栏收录该内容

7 篇文章 1 订阅

订阅专栏

说明：转发请注明作者和出处；以下文章内容为精简版，详细版以pdf格式的幻灯片形式呈现，请见链接：视觉SLAM_06_指数与对数映射.pdf

0.目录

0.目录
1.SO(3)上的指数映射
2.SE(3)上的指数映射
3.参考文献

1.SO(3)上的指数映射

任意矩阵的指数映射可以写成一个泰勒展开，但是只有在收敛的情况下才会有结果，其结果仍是一个矩阵：
$e^{\boldsymbol A} = \sum_{n=0}^{\infty} \frac{1}{n!} \boldsymbol{A} ^ n \tag{1}$ 同样地，对 $\mathfrak{so}(3)$ 中的任意元素 $\boldsymbol \phi$ ，也可以按此方式定义其指数映射：
$e^{\boldsymbol \phi \hat{\,} ^{}} = \sum_{n=0}^{\infty} \frac{1}{n!} (\boldsymbol \phi \hat{\,} ^{} ) ^ n \tag{2}$ 但此定义无法直接计算，下面推导一种计算指数映射的简便方法。

由于 $\boldsymbol \phi$ 是三维向量，定义它的模长和方向分别为 $\theta$ 和 $\boldsymbol a$ ，于是有 $\boldsymbol \phi = \theta \boldsymbol a$ ，其中， $\| \boldsymbol a \| = 1$ 。
对于 $\boldsymbol a \hat{\,} ^ {}$ 有以下两条性质：
$\boldsymbol a \hat{\,} ^ {} \boldsymbol a \hat{\,} ^ {} = \begin{bmatrix} -a^2_2 - a^2_3 & a_1 a_2 & a_1 a_3 \\ a_1 a_2 & -a^2_1 - a^2_3 & a_2 a_3 \\ a_1 a_3 & a_2 a_3 & -a^2_1 - a^2_2 \end{bmatrix} \tag{3}$ $\boldsymbol a \hat{\,} ^ {} \boldsymbol a \hat{\,} ^ {} \boldsymbol a \hat{\,} ^ {} = \boldsymbol a \hat{\,} ^ {} (\boldsymbol a \boldsymbol a ^T - \boldsymbol I) = - \boldsymbol a \hat{\,} ^ {} \tag{4}$ 这两个式子提供了处理 $\boldsymbol a \hat{\,} ^{}$ 高阶项的方法。指数映射可以写成：
$\begin{aligned} e^{\boldsymbol \phi \hat{\,} ^{}} &= e^{\theta \boldsymbol a \hat{\,} ^{}} = \sum_{n=0}^{\infty} \frac{1}{n!} (\theta \boldsymbol a \hat{\,} ^{}) ^ n \\ &= \boldsymbol I + \theta \boldsymbol a \hat{\,} ^ {} + \frac{1}{2!} \theta^2 \boldsymbol a \hat{\,} ^ {} \boldsymbol a \hat{\,} ^ {} + \frac{1}{3!} \theta^3 \boldsymbol a \hat{\,} ^ {} \boldsymbol a \hat{\,} ^ {} \boldsymbol a \hat{\,} ^ {} + \frac{1}{4!} (\boldsymbol a \hat{\,} ^ {})^4 + ... \\ &= \boldsymbol a \boldsymbol a ^T - \boldsymbol a \hat{\,} ^ {} \boldsymbol a \hat{\,} ^ {} + \theta \boldsymbol a \hat{\,} ^ {} + \frac{1}{2!} \theta \boldsymbol a \hat{\,} ^ {} \boldsymbol a \hat{\,} ^ {} - \frac{1}{3!} \theta^3 \boldsymbol a \hat{\,} ^ {} - \frac{1}{4!} \theta^4 (\boldsymbol a \hat{\,} ^ {})^2 + ... \\ &= \boldsymbol a \boldsymbol a ^T + (\theta - \frac{1}{3!} \theta^3 + \frac{1}{5!} \theta^5 - ...) \boldsymbol a \hat{\,} ^ {} - (1 - \frac{1}{2!} \theta^2 + \frac{1}{4!} - ...) \boldsymbol a \hat{\,} ^ {} \boldsymbol a \hat{\,} ^ {} \\ &= \boldsymbol a \hat{\,} ^ {} \boldsymbol a \hat{\,} ^ {} + \boldsymbol I + \sin \theta \boldsymbol a \hat{\,} ^ {} - \cos \theta \boldsymbol a \hat{\,} ^ {} \boldsymbol a \hat{\,} ^ {} \\ &= (1 - \cos \theta) \boldsymbol a \hat{\,} ^ {} \boldsymbol a \hat{\,} ^ {} + \boldsymbol I + \sin \theta \boldsymbol a \hat{\,} ^ {} \\ &= \cos \theta \boldsymbol I + (1 - \cos \theta) \boldsymbol a \boldsymbol a ^T + \sin \theta \boldsymbol a \hat{\,} ^ {} \end{aligned}$ 即： $\begin{aligned} e^{\theta \boldsymbol a \hat{\,} ^{}} = \cos \theta \boldsymbol I + (1 - \cos \theta) \boldsymbol a \boldsymbol a ^T + \sin \theta \boldsymbol a \hat{\,} ^ {} \end{aligned} \tag{5}$ $\mathfrak{so}(3)$ 实际上就是由所谓的旋转向量组成的空间，通过指数映射，可以把 $\mathfrak{so}(3)$ 中任意一个向量对应到了一个位于SO(3)中的旋转矩阵。反之，也能把SO(3)中的元素对应到 $\mathfrak{so}(3)$ 中：
$\boldsymbol \phi = \ln (\boldsymbol R)^{\vee} = \left(\sum_{n=0}^{\infty} \frac{(-1)^n}{n+1} (\boldsymbol R - \boldsymbol I)^{n + 1} \right)^{\vee} \tag{6}$

2.SE(3)上的指数映射

$\mathfrak{se}(3)$ 上的指数映射形式如下：
$\begin{aligned} e^{\boldsymbol \xi \hat{\,} ^ {}} &= \begin{bmatrix} \sum_{n=0}^{\infty} \frac{1}{n!} \left( \boldsymbol \phi \hat{\,}^{} \right)^n & \sum_{n=0}^{\infty} \frac{1}{(n+1)!}(\boldsymbol \phi \hat{\,}^{})^n \boldsymbol \rho \\ \boldsymbol 0^T & 1 \end{bmatrix} \\ &\overset{\Delta}{=} \begin{bmatrix} \boldsymbol R & \boldsymbol {J\rho} \\ \boldsymbol 0^T & 1 \end{bmatrix} = \boldsymbol T \end{aligned} \tag{7}$
令 $\boldsymbol \phi = \theta \boldsymbol a$ ，其中 $\boldsymbol a$ 为单位向量，则：
$\boldsymbol J = \sum_{n=0}^{\infty} \frac{1}{(n+1)!} (\boldsymbol \phi \hat{\,} ^{})^n = \frac{\sin \theta}{\theta} \boldsymbol I + \left(1- \frac{\sin \theta}{\theta}\right) \boldsymbol a \boldsymbol a^T + \frac{1-\cos \theta}{\theta} \boldsymbol a \hat{\,}^{} \tag{8}$