在通常的 SLAM 中，我们估计的无非就是在极短的时间内物体的一个相对位姿运动，然后进行累加，即可得到物体的当前位置，即 SLAM 中的定位问题，但是往往该运动在较短的时间内其变化量是极小的，李群对于 SLAM 常用的作用就是将矩阵乘法在扰动模型中转换为加法，本文记录相关内容。
为什么会有李群和李代数的引出。在通常的 SLAM 中，我们估计的无非就是在极短的时间内物体的一个相对位姿运动，然后进行累加，即可得到物体的当前位置，即 SLAM 中的定位问题，但是往往该运动在较短的时间内其变化量是极小的。
通常其运动变化我们可以使用旋转加平移进行表示，即 $(𝑅,𝑡)=𝑇$ 。但是在实际中，我们时常想要知道这一微小的变化量，不能一直将每一次变化都记录下来，这时候就联想到了导数，表示的正是在某一点的一个变化率。可是对于平移 𝑡 来说，其是具有加法运算的，但是对于 𝑅 来说，两个旋转矩阵相加，是没有意义的，而且相加之后是破坏了旋转矩阵 $𝑅𝑅^𝑇=𝐼$ 的性质的。
怎么解决呢，这就要引出李群和李代数来解决。
𝑅 和 𝑇 都是李群的一种，虽然其不具有加法运算，但是当把其映射到李代数空间上时，就具有了加法的性质，在李代数上进行加法计算后，或者说是导数运算后，再将其映射到对应的李群即可。
前面我们介绍了若干种描述三维世界刚体运动的方式。除了用它们来描述相机的位姿之外，还要对它们进行估计和优化。这个问题可以描述为什么样的相机位姿最符合当前观测数据。而典型的做法就是将其构建成一个优化问题来求解。
假如我们有一个坐标系，我们假设它的单位正交基底（两两垂直，长度为单位长度1，想象下直角坐标系的三个坐标轴）为：
$$
[e_1,e_2,e_3]
$$
那么我们就可以用这三个基底乘以某个系数，比如a1、a2，a3，来表示空间中的任意一点（或者说从原点指向这个点的向量），比如这样：
$$ [e_1,e_2,e_3]\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix} $$
$a1、a2，a3$ 就是这个点在这个坐标系下的坐标值，比如我们最熟悉的直角坐标系。
但问题是：我们的坐标系并不一定是唯一的，比如SLAM中就会有一个表示空间中物体绝对位置的世界坐标系（比如你的学校在解放路与西安路交汇处），也会有一个对于世界坐标系来说，不停在运动的以机器人自身为原点，表示相对位置的机器人坐标系（比如说学校在你前方100米），它的基底和之前坐标系是不同的，假如它是：
$$
[e_1’,e_2’,e_3’]
$$
那么自然的，上面那一点，我们也可以用新的基底，乘以一个系数来表示：
$$ [e_1',e_2',e_3']\begin{bmatrix}a_1'\\a_2'\\a_3'\end{bmatrix} $$
因为我们表示的是同一个点（向量），它们是相等的，为了方便描述，我们先假设两个坐标系原点是相同的，即新旧坐标系之间只有旋转关系，没有发生平移：
$$ [e_1,e_2,e_3]\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}=[e_1^{'},e_2^{'},e_3^{'}]\begin{bmatrix}a_1^{'}\\a_2^{'}\\a_3^{'}\end{bmatrix} $$
为了能更好的表示这个点，在新旧坐标系下的坐标值的关系，我们整理一下，两边同时左乘一个 $[𝑒_1^𝑇,𝑒_2^𝑇,𝑒_3^𝑇]^𝑇$ ，于是等式变成了：
$$ \begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}=\begin{bmatrix}e_1^Te_1'&e_1^Te_2'&e_1^Te_3'\\e_2^Te_1'&e_2^Te_2'&e_2^Te_3'\\e_3^Te_1'&e_3^Te_2'&e_3^Te_3'\end{bmatrix}\begin{bmatrix}a_1'\\a_2'\\a_3'\end{bmatrix} $$
我们用一个 $R$ 来表示(Rotation)，它描述了旋转前后，同一点坐标值的变换关系，我们叫它旋转矩阵：
$$
a=Ra’
$$
这个旋转矩阵，有一些特殊的性质：它是一个行列式为1的正交矩阵。它的逆就是它自身的转置 $𝑅^{−1}=𝑅^𝑇$ ，表示与其自身相反的旋转（即转过来和转回去的关系）。反之，行列式为1的正交矩阵，也可以作为一个旋转矩阵，用数学定义表示，就是这样一个集合：
$$ \left\{\begin{matrix}R\in\mathbb{R}^{n\times n}|R\times R^T=I|det(R)=1\end{matrix}\right\} $$
这个群的名字是特殊正交群(Special Orthogonal Group)，当然我们要研究的旋转是发生在三维空间的，其中的n = 3，就简称SO(3)。
$$ SO(3)=\left\{\begin{matrix}R\in\mathbb{R}^{3\times3}|R\times R^T=I|det(R)=1\end{matrix}\right\} $$
不过，上面我们只考虑到旋转，还没有说平移。但是也很简单，在得到的a的基础上直接加上一个向量t来表示平移就好了：
$$ \begin{array}{c} \begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}=\begin{bmatrix}e_1^Te_1^{'}&e_1^Te_2^{'}&e_1^Te_3^{'}\\e_2^Te_1^{'}&e_2^Te_2^{'}&e_2^Te_3^{'}\\e_3^Te_1^{'}&e_3^Te_2^{'}&e_3^Te_3^{'}\end{bmatrix}\begin{bmatrix}a_1^{'}\\a_2^{'}\\a_3^{'}\end{bmatrix}+\begin{bmatrix}t_1\\t_2\\t_3\end{bmatrix}\\a=Ra^{'}+t \end{array} $$ 这样在多次变换之后，就会形成“套娃”，越来越复杂，于是我们引入齐次坐标： $$ \begin{bmatrix}a_1\\a_2\\a_3\\1\end{bmatrix}=\begin{bmatrix}e_1^Te_1^{'}&e_1^Te_2^{'}&e_1^Te_3^{'}&t_1\\e_2^Te_1^{'}&e_2^Te_2^{'}&e_2^Te_3^{'}&t_2\\e_3^Te_1^{'}&e_3^Te_2^{'}&e_3^Te_3^{'}&t_3\\0&0&0&1\end{bmatrix}\begin{bmatrix}a_1^{'}\\a_2^{'}\\a_3^{'}\\1\end{bmatrix} $$ 可以很容易地表示变换了： $$ \begin{array}{l}b=T_1a\\c=T_2b\\c=T_2T_1a\end{array} $$
我们把这个T称为变换矩阵（Transform Matrix），它也有一些特殊的性质，左上角是旋转矩阵R，右侧是平移向量t，左下角为0块，右下角为1。因为我们上面说到的旋转平移都是欧式变换，我们给这类玩意取个同样炫酷的名字：特殊欧式群（Special Euclidean Group），简称SE(3)，用数学来描述就是这样一个集合：
$$ SE(3)=\left\{T=\left[\begin{array}{cc}R&t\\0^T&1\end{array}\right]\in\mathbb{R}^{4\times4}|R\in SO(3),t\in\mathbb{R}^3\right\} $$ 它的逆，与表示旋转的SO(3)一样，表示与它相反的变换（变过去，和变回来）： $$ T^{-1}=\begin{bmatrix}R^T&-R^Tt\\0^T&1\end{bmatrix} $$
通过旋转矩阵和变换矩阵，我们可以把一个坐标（向量），从一个坐标系转换到另一个坐标系。而我们的机器人坐标系（相机坐标系）是以机器人（相机）自身为原点的，也就是说，变换矩阵就表示了当前机器人（相机)的位置（坐标原点的移动）和姿态（两个坐标系之间的旋转）。
SLAM的核心问题之一就是位姿的估计与优化，所以说这部分是怎么都躲不过去的。然而现在位姿是能表示了，用矩阵表示的时候，还是有些问题没有解决。
首先的一个问题就是：对于“旋转”，它只有3个自由度，而为了表示它，我们用了3*3 = 9个量，显然用旋转矩阵来表达旋转是“冗余”的。理论上我们可以用3个量来表示一次旋转。
比如我们可以用一个向量来表示旋转，它的方向是旋转的轴心，而它的长度，是旋转的角度，这种向量我们称为旋转向量（Axis-Angle）。
它的转轴比较好理解，在旋转前后，转轴的方向不应该发生改变，我们假设转轴向量为n即：
$$
Rn=n
$$
所以很容易理解，转轴向量n，应该是旋转矩阵R特征值1对应的特征向量（不明白的去看特征值特征向量的定义）。而旋转角则不太好求，由罗德里格斯公式（Rodrigues’s Formula）求出：
$$ \begin{aligned} &R=cos\theta I+(1-cos\theta)nn^T+sin\theta n^\wedge \\ &tr(R)=cos\theta tr(I)+(1-cos\theta)tr(nn^T)+sin\theta tr(n^\wedge) \\ &=1+2cos\theta \\ &\theta=arccos(\frac{tr(R)-1}{2}) \end{aligned} $$
先不用管里面的
n^是啥，只要明白我们有更紧凑的方式来描述“旋转”，并且它能从旋转矩阵变换得来就好了。因为后面这个问题才是引入李群和李代数的原因。
先来看一下最简单的最小二乘问题，熟悉的可以跳过：
$$
min\frac{1}{2}\left|f(x)\right|_2^2
$$
简单来说，我们要找到一个 𝑥 ，使函数达到极值，换成SLAM的情景，就是我们要找到一个位姿，让它最“合理”，至于什么叫“合理”，怎么评价，后面再说。
还是先从简单的例子开始考虑，如果函数是像 $𝑓(𝑥)=𝑥^2$ 这种很简单的形式，那么很容易通过求导数为0点，通过解析形式求出结果。然而一旦函数变得复杂，无法以解析形式求解，就需要用迭代的方式进行求解了：
问题就出在第二步上，求取函数的下降方向就需要对函数求导，回想一下导数的定义，再看看第二步做了什么，怎么都绕不开一件事 $𝑥_𝑖+Δ𝑥$ ，然而问题是，连续变换时，旋转矩阵R更新的方式是不断左乘新的旋转矩阵，而不是加。
回顾一下旋转矩阵R的性质：它是一个行列式为1的正交矩阵正交矩阵。即它的逆就是它自身的转置 $𝑅{−1}=𝑅𝑇$ 。一个旋转矩阵加另一个旋转矩阵，得到的那一坨东西并不一定还能满足旋转矩阵（特殊正交群SO(3)）的性质，我们管这个叫：
特殊正交群特殊正交群对加法不封闭。
然而，特殊正交群是李群的一种，因此我们可以将其映射为李代数来化乘为加，解决这个问题。
群（Group），是一种集合，加上一种运算组成的结构。假如我们把集合记作G，运算记作**·**，它要求集合和运算满足下面几个条件：
看起来挺复杂，举个例子就很容易整明白了，整数（集合）和加法（运算），就可以构成一个群。
所以说，群这玩意也不那么神秘，任何满足这些条件的集合和运算，都能构成一个群。
上面提到的特殊正交群和特殊欧式群就是一种群，只不过他们对应的运算并不是加法，而是矩阵乘法，所以说它们对加法不封闭。
我们说三维旋转矩阵构成了特殊正交群 𝑆𝑂(3)，变换矩阵构成了特殊欧式群 𝑆𝐸(3)。我们知道，矩阵对于乘法是封闭的，即假设相机进行了两次连续的旋转，旋转矩阵分别为𝑅1 和𝑅2, 这两个矩阵相乘后得到的也是个旋转矩阵，表示了总的旋转。也即：
$$
\forall \mathbf{R}_1,\mathbf{R}_2 \in SO(3),\ \mathbf{R}_1\mathbf{R}_2 \in SO(3)
$$
对变换矩阵也是如此。但这二者对于加法是不封闭的。两个变换矩阵相加后得到的矩阵并不是一个变换矩阵。
定义：李群就是具有**连续（光滑）**性质的群。
前面举的整数的加法的例子显然不是连续的，因而它不是李群。但 𝑆𝑂(3) 和 𝑆𝐸(3) 在实数空间上是连续的（机器人在三维空间中显然是连续地运动，而不会进行“瞬移”）。
介绍完李群，在引入李代数之前，我们来回顾下开头我们提到的问题：为什么要用到李群和李代数？避免一直是数学上的推导。我们用一个比较实际的例子。假设某个时刻我们预测机器人的位姿为𝑇（待定值）, 它观测到了一个惯性坐标系下的点𝑝 而产生了一个观测数据 𝑧，它是该点在相机坐标系下的坐标，则可得
$$
\mathbf{z}=\mathbf{T}\mathbf{p}+\mathbf{\omega}
$$
其中，$𝜔$ 是观测噪声。由于观测噪声的存在，$𝑧$ 无法严格满足式 $𝑧=𝑇𝑝$。因此而产生的误差 $𝑒$ 为
$$
\mathbf{e}=\mathbf{z}-\mathbf{Tp}
$$
若共有 $N$ 个观测值，那么就有 $N$ 个这样的式子。机器人的位姿估计就转变成寻找一个最优的 $𝑇$ 使得整体的误差最小化：
$$ \mathop{\min}_{T} J(\mathbf{T})=\sum\limits^N_{i=1}||\mathbf{z}_i-\mathbf{T}\mathbf{p}_i||^2_2 $$
通常，直接求解上式得出最优的𝑇 是很困难的（或计算量很大）。我们常常先给定一个猜测值（初始值）𝑇0，然后不断地对它进行迭代更新。而这个过程需要用到导数（可以想想梯度下降法）。回顾导数的定义，$𝑓'(𝑥)=limΔ_{𝑥→0} \frac{𝑓(𝑥+Δ𝑥)-𝑓(𝑥)}{Δ𝑥}$。显然计算导数和进行更新时都要用到加法。但SO(3) 和SE(3) 上对矩阵加法的运算并不封闭。如果要继续采取这个迭代更新的策略势必要再想想办法，使得导数“可行”。而这就可以通过李群及其对应的李代数来实现。
现在我们考虑任意的旋转矩阵𝑅:
$$
\mathbf{R}\mathbf{R}^T=\mathbf{I}
$$
因为机器人在不断运动，所以 $𝑅$ 也在随时间不断变换，引入时间 $𝑡$ 就得到 $𝑅(𝑡)$，上式也变成
$$
\mathbf{R}(t)\mathbf{R}(t)^T=\mathbf{I}
$$
等式两边一起对时间求导可得：
$$ \dot{\mathbf{R}}(t)\mathbf{R}(t)^T+\mathbf{R}(t)\dot{\mathbf{R}}(t)^T=0 $$ 亦即 $$ \dot{\mathbf{R}}(t)\mathbf{R}(t)^T=-\mathbf{R}(t)\dot{\mathbf{R}}(t)^T=-(\dot{\mathbf{R}}(t)\mathbf{R}(t)^T)^T $$
一个矩阵等于其自身转置后取负号，可知 $\dot{\mathbf{R}}(t)\mathbf{R}(t)^T$ 是一个反对称矩阵。我们之前就介绍过反对称矩阵和 ^ 符号，可知对应地，对于任意的反对称矩阵，可以找到一个与之对应的向量，这个关系可以用∨ 来表示：
$$ \mathbf{a}^\wedge=\mathbf{A}= \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \\ \end{bmatrix} , \ \ \mathbf{A}^\vee=\mathbf{a} $$
设这个对应的向量为$\phi(t)\in R^3$，就有
$$ \dot{\mathbf{R}}(t)\mathbf{R}(t)^T=\phi(t)^\wedge $$ 等式两边同时右乘 $\mathbf{R}(t)$ 则有 $$ \dot{\mathbf{R}}(t)=\phi(t)^\wedge\mathbf{R}(t) $$ 可以看到，每对旋转矩阵求一次导数，只需要左乘一个 $𝜙(𝑡)^∧$ 就可以了。假设 $𝑡_0=0$ 并设此时$𝑅(0)=𝐼$，进行一阶泰勒展开就有： $$ \mathbf{R}(t) \approx \mathbf{R}(t_0)+\dot{\mathbf{R}}(t_0)(t-t_0)=\mathbf{I}+\phi(t_0)^\wedge \cdot (t) $$
𝜙 反应了𝑅 的导数的性质，故称它在 𝑆𝑂(3) 原点附近的正切空间（𝑇𝑎𝑛𝑔𝑒𝑛𝑡𝑆𝑝𝑎𝑐𝑒）上。
同时，在 $𝑡_0 $附近，设 $𝜙$ 也是一个常数，即 $𝜙(𝑡_0)=𝜙_0$，有
$$ \dot{\mathbf{R}}(t)=\phi(t_0)^\wedge\mathbf{R}(t)=\phi_0^\wedge\mathbf{R}(t) $$ 显然，上式是一个关于𝑅 的微分方程，而且知道初始值 $𝑅(0)=𝐼$，他的解为 $$ \mathbf{R}(t)=\exp (\phi_0^\wedge t) $$
由于我们之前做了一些假设，所以这个式子只在 𝑡 附近有效。
当某个时刻的𝑅 已知时，存在一个向量𝜙，二者满足这个矩阵指数关系 。而这个𝜙，就是对应到 𝑆𝑂(3) 上的李代数 𝑠𝑜(3)。下面我们就介绍一下李代数的定义和如何计算矩阵指数 $exp(𝜙^∧)$.
每个李群都有与之对应的李代数。李代数描述了李群的局部性质。其定义为：李代数由一个集合𝑉, 一个数域𝐹 和一个二元运算[,], 组成。如果它们满足以下几条性质，则称 $(𝑉,𝐹,[,])$ 为一个李代数，记作g。
封闭性：$\forall\mathbf{X},\mathbf{Y}\in V,[\mathbf{X},\mathbf{Y}]\in V.$
双线性$:\forall\mathbf{X},\mathbf{Y},\mathbf{Z}\in V,a,b\in F$,有
$$
[a\mathbf{X}+b\mathbf{Y},\mathbf{Z}]=a[\mathbf{X},\mathbf{Z}]+[\mathbf{Y},\mathbf{Z}],[\mathbf{Z},a\mathbf{X}+b\mathbf{Y}]=a[\mathbf{Z},\mathbf{X}]+b[\mathbf{Z},\mathbf{Y}]
$$
自反性：$\forall\mathbf{X}\in V,[\mathbf{X},\mathbf{X}]=0.$
雅可比等价$:\forall\mathbf{X},\mathbf{Y},\mathbf{Z}\in V,\left[\mathbf{X},[\mathbf{Y},\mathbf{Z}]\right]+\left[\mathbf{Z},[\mathbf{X},\mathbf{Y}]\right]+\left[\mathbf{Y},[\mathbf{Z},\mathbf{X}]\right]=0.$
其中的二元运算 $[,] $ 称为李括号，它表达了两个元素的差异。三维向量 $𝑅^3$ 上定义的叉积就是一种李括号，$𝑔=(𝑅^3,𝑅,×) $ 就构成了李代数。
之前我们提到的 SO(3) 对应的向量 $𝜙$ 就是一种李代数，是定义在 $R^3$ 上的向量。它的反对称矩阵记为 $𝛷=𝜙^∧$. 在定义下面，两个向量 $𝜙_1,𝜙_2$ 的李括号为：
$$
[\phi_1, \phi_2]=(\mathbf{\Phi_1}\mathbf{\Phi_2}-\mathbf{\Phi_2}\mathbf{\Phi_1})^\vee
$$
高翔在书中没有明说，但经过计算可以得到结果为 $𝜙_1,𝜙_2$ 的叉积（又叫外积）。所以这里我猜测so(3) 对应的李括号为三维向量 $R^3$ 上的叉积。
$$ so(3)=\{\phi \in R^3,\ \mathbf{\Phi}=\phi^\wedge \in R^{3\times 3}\} $$
至此，我们已经清楚了𝑠𝑜(3) 的内容：它们是一个由三维向量组成的集合；每个向量对应到一个反对称矩阵；（李括号为三维向量的外积运算）；可以用来表达旋转矩阵的导数，和 𝑆𝑂(3) 的关系由指数映射给定：$𝑅=exp(𝜙^∧)$.
对于$𝑆𝐸(3)$，它也有对应的李代数 $𝑠𝑒(3)$。和 $𝑠𝑜(3) $类似，$𝑠𝑒(3) $ 在 $𝑅^6$ 空间中：
$$ se(3)=\begin{Bmatrix} \mathbf{\xi}=\begin{bmatrix} \mathbf{\rho} \\ \mathbf{\phi} \end{bmatrix} \in R^6,\mathbf{\rho} \in R^3, \mathbf{\phi} \in so(3), \mathbf{\xi}^\wedge=\begin{bmatrix}\phi^\wedge & \mathbf{\rho} \\ \mathbf{0}^T & 0 \end{bmatrix} \in R^{4 \times 4} \end{Bmatrix} $$
把每个 $𝑠𝑒(3)$ 元素记做 $𝜉$，它是一个六维向量：前三维为平移，记作 $𝜌$；后三维是旋转，记作$𝜙$，实质上是 $𝑠0(3)$ 元素。此外，在 $𝑠𝑒(3)$ 中，∧ 符号的含义被拓展了：这里它将一个六维向量转换为四维矩阵，但这里不再表示反对称矩阵。
同样，李代数 $𝑠𝑒(3)$ 也有类似的李括号：
$$ [\xi_1,\ \xi_2]=(\xi_1^\wedge \xi_2^\wedge - \xi_2^\wedge\xi_1^\wedge)^\vee $$
如前所述，李代数到李群是一个指数映射。现在来看看指数映射是如何计算的。
任意矩阵的指数映射都可以写成一个泰勒展开。但要注意的是展开式只有在收敛的情况下才有解，其结果仍然是一个矩阵。所以，对 𝑠𝑜(3) 中的元素𝜙，其指数映射可以写成：
$$ \exp(\phi^\wedge)=\sum\limits_{n=0}^\infty \frac{1}{n!}(\phi^\wedge)^n $$ 将$\phi$ 记为 $\theta\mathbf{a}$, 并利用其性质:(偶次项) $\mathbf{a}^{\wedge}\mathbf{a}^{\wedge}=\mathbf{a}\mathbf{a}^{T}-\mathbf{I}$, (奇次项) $\mathbf{a}^{\wedge}\mathbf{a}^{\wedge}\mathbf{a}^{\wedge}=-\mathbf{a}^{\wedge}$ 可得: $$ \exp{\phi^\wedge}=\exp(\theta \mathbf{a})=\sum\limits_{n=0}^\infty \frac{1}{n!}(\theta \mathbf{a}^\wedge)^n \\ =\cos\theta\mathbf{I}+(1-\cos\theta)\mathbf{a}\mathbf{a}^T+\sin\theta\mathbf{a}^T $$ 𝑠𝑜(3) 实际上就是**由旋转向量组成的空间，而指数映射就是罗德里格斯公式**。也可以定义一个对数映射，将 𝑆𝑂(3) 映射到 𝑠𝑜(3) 上： $$ \phi=\ln(R)^\vee=(\sum\limits_{n=0}^\infty\frac{(-1)^n}{n+1}(\mathbf{R}-\mathbf{I})^{n+1})^\vee $$
但上式太过复杂了。通常我们会用前一讲介绍的旋转向量到旋转矩阵的方式来计算旋转矩阵。
指数映射是一个满射，即对每个SO(3) 中的元素都可以找到一个so(3) 的元素与之对应；但存在多个so(3) 元素对应到同一个SO(3) 上（一个旋转角为𝜃 的旋转向量多旋转一周得到的结果显然是一样的）。如果把旋转角固定在±𝜋 或者[0,2𝜋] 之间，那么李群和李代数中的元素就是一一对应的。
SE(3) 上的指数映射推导原理与SO(3) 类似，但更复杂。这里直接给出结论：
$$ \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 \\ \mathbf{0}^T & 1 \\ \end{bmatrix}= \begin{bmatrix} \mathbf{R} & \mathbf{J}\rho \\ \mathbf{0}^T & 1 \\ \end{bmatrix}=\mathbf{T} $$ $exp(𝜉∧) $ 的左上角是 SO(3) 中的元素 𝑅，是其旋转部分。而矩阵 𝐽 则为： $$ \mathbf{J}=\frac{\sin\theta}{\theta}\mathbf{I}+(1-\frac{\sin\theta}{\theta})\mathbf{a}\mathbf{a}^T+\frac{1-\cos\theta}{\theta}\mathbf{a}^\wedge $$ $𝜉$ 中的平移部分 $𝜌$ 经过指数变换后，发生了一次**以𝐽 为系数的线性变换**。 相反的，$SE(3)$ 到 $se(3)$ 也有对应的对数映射。但一般先利用左上角的旋转矩阵计算出旋转向量；对右上角的平移向量 $𝑡$ 则有： $$ \mathbf{t}=\mathbf{J}\rho $$ 即可得到 $𝜌$。
现在我们有了李代数，which 具有良好的加法运算。因此，我们可以用它来解决求导问题了。具体的思路有两种：
假设空间点 $𝑝$ 进行了一次矩阵表示为 $𝑅$ 的旋转，得到了$𝑅𝑝$。要计算旋转后的点的坐标相对于旋转的导数，可以不严谨的写为：
$$
\frac{\partial(\mathbf{R}\mathbf{p})}{\partial\mathbf{R}}
$$
前面提到，SO(3)没有加法，所以上式无法按照导数的定义进行计算。设𝑅 对应的李代数为𝜙，就可以写成：
$$
\frac{\partial(\exp(\phi^\wedge)\mathbf{p})}{\partial\phi}
$$
按照导数的定义展开，并在推导中利用 𝐵𝐶𝐻 公式和一阶泰勒展开，可以得到下式：
$$ \frac{\partial(\exp(\phi^\wedge)\mathbf{p})}{\partial\phi}=\lim_{\delta\phi\to0}\frac{\exp((\phi+\delta\phi)^\wedge)\mathbf{p}-\exp(\phi^\wedge)\mathbf{p}}{\delta\phi}=-(\mathbf{R}\mathbf{p})^\wedge\mathbf{J}_l $$
这个公式是直接对旋转矩阵 𝑅 进行求导，最后的结果中含有左雅可比矩阵 $𝐽_𝑙$。
另一种方法就是先对旋转矩阵𝑅 进行一次扰动Δ𝑅，然后再对个扰动进行求导。这里以左乘扰动模型为例。设左扰动的李代数为𝜑，直接对𝜑（而不是𝑅）进行求导：
$$ \frac{\partial(\mathbf{R}\mathbf{p})}{\partial\varphi}=\lim_{\varphi\to 0}\frac{\exp(\varphi^\wedge)\exp(\phi^\wedge)\mathbf{p}-\exp(\phi^\wedge)\mathbf{p}}{\varphi}=-(\mathbf{R}\mathbf{p})^\wedge $$
和李代数求导模型进行对比，最明显的就是少了左雅可比矩阵 $𝐽_𝑙$。在扰动模型中，我们把小量Δ𝑅 先直接乘在了李群上，然后再对这个小量的李代数进行求导。而在李代数求导模型中，我们是把这个小量 $𝛿_𝜙$ 加在了李代数上，然后直接对对应的李代数进行求导。因此在所得的结果中就会有一个表示李群和李代数运算间关系（不是转换关系）的左雅可比矩阵$𝐽_𝑙$。
假设某空间点 $𝑝$ 经过了一次变换 𝑇，对应的李代数为 $𝜉$，得到了 $𝑇𝑝$。给 $𝑇$ 左乘一个微小扰动 $Δ𝑇$，扰动项的李代数为$𝛿_𝜉=[𝛿_𝜌,𝛿_𝜙]^𝑇$。则有：
$$ \begin{align} \frac{\partial(\mathbf{T}\mathbf{p})}{\partial\delta\xi} & =\lim_{\delta\xi\to0}\frac{\exp(\delta\xi^\wedge)\exp(\xi^\wedge)\mathbf{p}-\exp(\xi^\wedge)\mathbf{p}}{\delta\xi} \\ & \approx \lim_{\delta \xi \to 0} \frac{(\mathbf{I} + \delta\xi^\wedge) \exp(\xi^\wedge)\mathbf{p}-\exp(\xi^\wedge)\mathbf{p}}{\delta\xi} \\ & = \lim_{\delta \xi \to 0} \frac{\delta \xi^{\wedge} \exp(\xi^{\wedge}) \mathbf{p}}{\delta \xi} \\ & =\lim_{\delta \xi \to 0} \frac{\begin{bmatrix} \delta \phi & \delta \rho \\ \mathbf{0}^T & 0 \end{bmatrix} \begin{bmatrix} \mathbf{R} \mathbf{p} + \mathbf{t} \\ 1 \end{bmatrix} }{\delta \xi} \\ & =\lim_{\delta \xi \to 0} \frac{ \begin{bmatrix} \delta \phi ^{\wedge} (\mathbf{R} \mathbf{p} + \mathbf{t}) + \delta \rho \\ 0 \end{bmatrix}}{\delta \xi} \\ & =\begin{bmatrix} \mathbf{I}_{3\times3} & -(\mathbf{R}\mathbf{p}+\mathbf{t})^\wedge_{3\times3} \\ \mathbf{0}^T_{1\times3} & \mathbf{0}^T_{1\times3}\\ \end{bmatrix} \\ & =(\mathbf{T}\mathbf{p})^\bigodot \end{align} $$
最后的结果被定义为符号⨀ 的一个运算：把一个齐次坐标下的空间点变换成一个4*6 的矩阵。
最后介绍下单目SLAM中会用到的相似变换群Sim(3)。由于单目视觉中存在一个尺度不确定性（简单说，把单目相机两次拍照的两张相片想象成双目相机一次拍的两张照片，这两次拍照中单目相机的位移（对应于双目中的基线）是不知道的，导致了在像素深度估计上的尺度不确定），因此在单目的情况下，需要把尺度因子显式表达出来。就由欧式变换到了相似变换。
设空间点为𝑝，相似变换为：
$$ \mathbf{p}^\prime= \begin{bmatrix} s\mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1\\ \end{bmatrix}=s\mathbf{R}\mathbf{p} + \mathbf{t} $$ 这里，$s$ 就是尺度因子，它相当于对 $p$ 进行了一次缩放。 显然，相似变换也对矩阵乘法构成群，称为相似变换群： $$ Sim(3)= \begin{Bmatrix} \mathbf{S}= \begin{bmatrix} s\mathbf{R} & \mathbf{t} \\ \mathbf{0}^T & 1\\ \end{bmatrix} \in R^{4\times4} \end{Bmatrix} $$ 它对应的李代数为sim(3)，是一个7维向量：前六维就是se(3)，最后一项为尺度因子𝜎。 $$ sim(3)= \begin{Bmatrix} \zeta\ |\ \zeta= \begin{bmatrix} \mathbf{\rho} \\ \mathbf{\phi} \\ \mathbf{\sigma} \end{bmatrix} \in R^7,\ \zeta^\wedge= \begin{bmatrix} \sigma\mathbf{I}+\phi^\wedge & \mathbf{\rho} \\ \mathbf{0}^T & 0\\ \end{bmatrix} \in R^{4\times4} \end{Bmatrix} $$ 二者间仍然是通过指数映射和对数映射相关联。指数映射为： $$ \exp(\zeta^\wedge)= \begin{bmatrix} e^\sigma\exp(\phi^\wedge) & \mathbf{J}_s\mathbf{rho} \\ \mathbf{0}^T & 1 \\ \end{bmatrix} $$ 其中， $$ \mathbf{J}_s=\frac{e^\sigma -1}{\sigma}\mathbf{I}+\frac{\sigma e^\sigma \sin\theta+(1-e^\sigma\cos\theta)\theta}{\sigma^2+\theta^2}\mathbf{a}^\wedge+(\frac{e^\sigma -1}{\sigma}-\frac{(e^\sigma \cos\theta-1)\sigma+(e^\sigma \sin\theta)\theta}{\sigma^2+\theta^2})\mathbf{a}^\wedge\mathbf{a}^\wedge $$ 可以看到，相似变换的雅可比矩阵远比前面的复杂。而对数映射可以写为： $$ s=e^\sigma,\ \mathbf{R}=\exp(\phi^\wedge),\ \mathbf{t}=\mathbf{J}_s\mathbf{\rho} $$ 同样地，𝑆𝑖𝑚(3) 也具有 𝐵𝐶𝐻 近似公式，也有李代数倒数模型和扰动模型。这里我们介绍扰动模型。给𝑆𝑝 左乘一个微小扰动 $exp(𝜁^∧)$ 并对该微小扰动求导，记𝑆𝑝 的前三维坐标为q，有 $$ \frac{\partial(\mathbf{S}\mathbf{p})}{\partial\zeta}= \begin{bmatrix} \mathbf{I}_{3\times3} & -\mathbf{q}^\wedge_{3\times3} & \mathbf{q}_{3\times1} \\ \mathbf{0}^T_{1\times3} & \mathbf{0}^T_{1\times3} & 0\\ \end{bmatrix} $$ 显然，𝑆𝑝 是一个4维齐次坐标，𝜁 是一个7维向量。该导数就是一个4∗7 大小的雅可比矩阵。
此内容由惯性聚合(RSS阅读器)自动聚合整理，仅供阅读参考。 原文来自 — 版权归原作者所有。