高翔视觉slam十四讲学习（3）

最新推荐文章于 2023-10-03 14:44:36 发布

哥斯拉加

最新推荐文章于 2023-10-03 14:44:36 发布

阅读量190

点赞数

分类专栏：学习笔记文章标签：计算机视觉

本文链接：https://blog.csdn.net/weixin_44824496/article/details/116569882

版权

学习笔记专栏收录该内容

3 篇文章 0 订阅

订阅专栏

第三章李群与李代数（第四讲）

1. 李群与李代数的概念

1.1 群（Group）：是一种集合加上一种运算的代数结构。把集合记作A，运算记作.，则群可记作G=(A,.)。

群满足以下几个条件：

群性质

矩阵中常见的群有：

常见群

李群：是指具有连续（光滑）性质的群。

李代数：每个李群都有与之对应的李代数。李代数描述了李群的局部性质（导数）。

李代数定义与性质

其中二元运算称为李括号。

1.2 李代数

1.2.1 李代数 $\mathfrak{so}(3)$ :定义在 $\mathbb{R}^{3}$ 上的向量，记作 $\phi$ ，每个 $\phi$ 都可以生成一个反对称矩阵

$\Phi =\phi^{\wedge }=\begin{bmatrix} 0 & -\phi_{3} &\phi_{2} \\ \phi_{3}& 0 &-\phi_{1} \\ -\phi_{2}&\phi_{1} &0 \end{bmatrix}$

李代数 $\mathfrak{so}(3)$ 的李括号为：

$[\phi_{1},\phi_{2}]=(\Phi_{1}\Phi_{2}-\Phi_{2}\Phi_{1})^{\vee }$

由于 $\phi$ 与反对称矩阵关系很紧密,在不引起歧义的情况下,就说 $\mathfrak{so}(3)$ 的元素是 3 维向量或者 3 维反对称矩阵,不加区别:

$\mathfrak{so}(3)=\begin{Bmatrix} {\phi \in \mathbb{R}^{3},\Phi=\phi^{\wedge}\in \mathbb{R}^{3\times 3}} \end{Bmatrix}$

$\mathfrak{so}(3)$ 是一个由三维向量组成的集合,每个向量对应到一个反对称矩阵,可以表达旋转矩阵的导数，它与 $SO(3)$ （旋转矩阵）的映射关系为：

$R=\exp(\phi^{\wedge})$

1.2.2 李代数 $\mathfrak{se}(3)$ ：对应于 $SE(3)$ （变换矩阵T）的李代数

$\mathfrak{se}(3)=\left \{ \xi =\begin{bmatrix} \rho \\ \phi \end{bmatrix} \in \mathbb{R}^{6},\rho \in \mathbb{R}^{3},\phi \in so(3).\xi ^{\wedge}=\begin{bmatrix} \phi ^{\wedge} &\rho \\ 0^{T}&0 \end{bmatrix}\in \mathbb{R}^{4\times 4} \right \}$

把 $\mathfrak{se}(3)$ 元素记作 $\xi$ ,它是一个六维向量。前三维为平移,记作 $\rho$ ，后三维为旋转,记作 $\phi$ （实质上是 $\mathfrak{so}(3)$ 元素）

注意这里的 $\wedge$ 符号表示六维向量向四维矩阵的转换（此四维矩阵不为反对称矩阵，是变换矩阵吗？）

李代数 $\mathfrak{se}(3)$ 的李括号为

$[\xi _{1},\xi_{2}]=(\xi_{1}^{\wedge}\xi_{2}^{\wedge}-\xi_{2}^{\wedge}\xi_{1}^{\wedge})^{\vee}$

2. 指数与对数映射（李群与李代数之间的转换）

2.1

$SO(3)$ 上的指数映射（罗德里格斯公式）（基于泰勒展开）

$\exp(\phi^{\wedge})= \sum_{n=0}^{\infty}\frac{1}{n!}A^{n}\\ \exp(\phi^{\wedge})=\exp(\theta a^{\wedge})^{n}=\sum_{n=0}^{\infty}\frac{1}{n!}\(\theta a^{\wedge})^{n}=\cos I+(1-\cos \theta)aa^{T}+\sin \theta a^{\wedge}$

$SO(3)$ 上的对数映射

$\begin{align} \nonumber &\theta =\arccos \frac{tr(R)-1}{2}\\\nonumber &Ra=a \end{align}$

2.2

$SE(3)$ 上的指数映射

$\begin{align} \exp(\xi^{\wedge})&=\begin{bmatrix} \sum\limits_{n=0}^{\infty}\frac{1}{n!} (\phi^{\wedge})^{n} &\sum\limits_{n=0}^{\infty}\frac{1}{(n+1)!} (\phi^{\wedge})^{n}\rho \\\nonumber 0^{T} &1 \end{bmatrix}\\\nonumber &\overset{\bigtriangleup }{=} \begin{bmatrix} R &J\rho \\ 0^{T} &1 \end{bmatrix} =T \end{align}$

$\xi$ 的指数映射左上角的 $R$ 是 $SO(3)$ 中的元素，与 $\mathfrak{se}(3)$ 中的旋转部分 $\phi$ 对应，设 $\phi =\theta a$ ， $J$ 可整理为：

$J=\frac{\sin\theta}{\theta}I+(1-\frac{\sin\theta}{\theta})aa^{T}+\frac{1-\cos\theta}{\theta}a^{\wedge}$

我们看到,平移部分经过指数映射之后,发生了一次以 $J$ 为系数矩阵的线性变换。

$SE(3)$ 上的指数映射

$\begin{align} \nonumber &\theta =\arccos \frac{tr(R)-1}{2}\\\nonumber &Ra=a \\\nonumber &t=J\rho \end{align}$

总结

对应关系

3. 李代数求导与扰动模型

3.1 左乘、右乘近似

BCH公式（Baker-Campbell-Hausdorff公式）与近似形式

BCH公式：

$\ln(\exp(A)\exp(B))=A+B+\frac{1}{2}[A,B]+\frac{1}{12}[A,[A,B]]-\frac{1}{12}[B,[A,B]]+...$

BCH 公式告诉我们,当处理两个矩阵指数之积时,它们会产生一些由李括号组成的余项。特别地,考虑 $SO(3)$ 上的李代数 $\ln(\exp(\phi_{1}^{\wedge})\exp(\phi_{2}^{\wedge}))^{\vee}$ ，当 $\phi_{1}$ 或 $\phi_{2 }$ 为小量时,小量二次以上的项都可以被忽略掉。此时,BCH 拥有线性近似表达:

$ln(\exp(\phi_{1}^{\wedge})\exp(\phi_{2}^{\wedge}))^{\vee}\approx \left\{\begin{matrix} J_{l}(\phi_{2})^{-1}\phi_{1}+\phi_{2}\ \ if\ \phi_{1}\ is\ small \\ J_{r}(\phi_{1})^{-1}\phi_{2}+\phi_{1} \ \ if\ \phi_{2}\ is\ small \end{matrix}\right.$

以第一个近似为例。该式告诉我们,当对一个旋转矩阵 $R_{2}$ (李代数为 $\phi_{2}$ )左乘一个微小旋转矩阵 $R_{1}$ (李代数为 $\phi_{1}$ )时,可以近似地看作,在原有的李代数 $\phi_{2}$ 上,加上了一项 $J_{l}(\phi_{2})^{-1}\phi_{1}$ 。同理,第二个近似描述了右乘一个微小位移的情况。于是,李代数在 BCH近似下,分成了左乘近似和右乘近似两种。以左乘为例，其中

$\begin{align}\nonumber &J_{l}=J=\frac{\sin \theta}{\theta}I+(1-\frac{\sin \theta}{\theta})aa^{T}+\frac{1-\cos \theta}{\theta}a^{\wedge}\\\nonumber &J_{l}^{-1}=\frac{\theta}{2} \cot \frac{\theta}{2}I+(1-\frac{\theta}{2} \cot \frac{\theta}{2})aa^{T}-\frac{\theta}{2}a^{\wedge} \\\nonumber &J_{r}(\phi)=J_{l}(-\phi) \end{align}$

3.2 扰动模型求导

$SO(3)$ 李代数上的求导

我们经常会构建与位姿有关的函数,然后讨论该函数关于位姿的导数,以调整当前的估计值。

扰动模型（左乘）

$\begin{align}\nonumber \frac{\partial (Rp)}{\partial (\varphi)}&= \lim_{\varphi\rightarrow 0}\frac{\exp (\varphi ^{\wedge})\exp (\phi^{\wedge})p-\exp (\phi^{\wedge})p}{\varphi} \\\nonumber &\approx \lim_{\varphi\rightarrow 0}\frac{(1+\varphi^{\wedge})\exp (\phi^{\wedge})p-\exp (\phi^{\wedge})p}{\varphi} \\\nonumber &=\lim_{\varphi \rightarrow 0} \frac{\varphi^{\wedge}Rp}{\varphi}\\\nonumber &=\lim_{\varphi \rightarrow 0} \frac{-(Rp)^{\wedge}\varphi}{\varphi}\\\nonumber &=-(Rp)^{\wedge} \end{align}$

$SE(3)$ 李代数上的求导

扰动模型（左乘）

$\begin{align} \nonumber \frac{\partial(Tp)}{\partial \delta \xi }&= \begin{bmatrix} I & -(Rp+t)^{\wedge}\\ 0^{T}& 0^{T} \end{bmatrix}\\\nonumber &\overset{\bigtriangleup }=(Tp)^{\bigodot } \end{align}$