《视觉SLAM十四讲》-- 李群与李代数

03 李群与李代数

旋转矩阵自身是带有约束的，作为优化变量时，会引入额外的约束，使优化变得困难，通过李群-李代数之间的转换，将位姿估计变为无约束的优化问题，简化求解方式。

3.1 李群与李代数基础

3.1.1 群

（1）三维旋转矩阵构成了特殊正交群，三维变换矩阵构成了特殊欧式群。

（2）群是一种集合加上一种运算的代数结构。

（3）当集合 $A$ 满足以下性质时，即称 $(A,\cdot )$ 成群（记运算为$\cdot$）:

• 封闭性

• 结合律

• 幺元

• 逆

显然旋转矩阵集合和变换矩阵集合与矩阵乘法都成群。

3.1.2 李群

李群是具有连续（光滑）性质的群（相机是连续运动的，非离散的）。

3.2 李代数的引出

对任意旋转矩阵 $\mathbf{R}$，满足

$$\mathbf{R}{\mathbf{R}}^{\mathbf{T}}=\mathbf{I}\text{\hspace{1em}(3-1)}$$

矩阵 $\mathbf{R}$ 随时间变化，是时间 $t$ 的函数，因此

$$\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=-(\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T{)}^T\text{\hspace{1em}(3-2)}$$

这是一个反对称矩阵（满足 ${\mathbf{A}}^{\mathbf{T}}=-\mathbf{A}$）。

在此之前，我们约定反对称符号：

a ∧ = A = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] ， A ∨ = a \boldsymbol{a}^{\wedge}=\boldsymbol{A}=\left[\begin{array}{ccc} 0 & -a_{3} & a_{2} \\ a_{3} & 0 & -a_{1} \\ -a_{2} & a_{1} & 0 \end{array}\right]，A^{\vee}=\boldsymbol{a} a∧=A= ​0a3​−a2​​−a3​0a1​​a2​−a1​0​ ​，A∨=a

那么存在 $\varphi (t{)}^{\wedge }$，使得

$$\varphi (t{)}^{\wedge }=\dot{\mathbf{R}}(t)\mathbf{R}(t{)}^T$$

两端同时右乘 $\mathbf{R}(t)$，得

$$\varphi (t{)}^{\wedge }\mathbf{R}(t)=\dot{\mathbf{R}}(t)\text{\hspace{1em}(3-3)}$$

可以看成，对 $\mathbf{R}(t)$ 求导，相当于左乘 $\varphi (t{)}^{\wedge }$。

当 $t_0=0$ 时，假设此时旋转矩阵 $\mathbf{R}(t_0)=\mathbf{I}$，在此处泰勒展开，即

$$\mathbf{R}(t)=\mathbf{R}(t_0)+\dot{\mathbf{R}}(t_0)(t-t_0)+...\approx \mathbf{I}+\varphi (t{)}^{\wedge }\mathbf{R}(t)$$

假设在 $t_0$ 附近，$\varphi$ 不变，则有微分方程

$$\dot{\mathbf{R}}(t)=\varphi (t_0{)}^{\wedge }\mathbf{R}(t)={\varphi }_0^{\wedge }\mathbf{R}(t)$$
且初值 $\mathbf{R}(0)=\mathbf{I}$ 解之得

$$\mathbf{R}(t)=e^{{\varphi }_0^{\wedge }t}\text{\hspace{1em}(3-4)}$$

3.3 李代数的定义

（1）每个李群都有与之对应的李代数。李代数描述了李群单位元附近的正切空间性质。

（2）李代数由一个集合 $\mathbb{V}$，一个数域 $\mathbb{F}$，和一个二元运算 [ , ] （称为李括号）组成。满足以下性质：

• 封闭性

• 双线性

• 自反性

• 雅克比等价

例如，三维向量 ${\mathbb{R}}^3$ 上定义的叉乘 $\times$ 是一种李括号，因此 $\mathfrak{g}=({\mathbb{R}}^3,{\mathbb{R}}^3,\times )$ 构成了李代数。

3.4 李代数 $\mathfrak{so}(3)$

SO(3) 对应的李代数是定义在 ${\mathbb{R}}^3$ 上的向量，记

Φ = ϕ ∧ = [ 0 − ϕ 3 ϕ 2 ϕ 3 0 − ϕ 1 − ϕ 2 ϕ 1 0 ] (3-5) \boldsymbol{\Phi}=\phi^{\wedge}=\left[\begin{array}{ccc} 0 & -\phi_{3} & \phi_{2} \\ \phi_{3} & 0 & -\phi_{1} \\ -\phi_{2} & \phi_{1} & 0 \end{array}\right] \tag{3-5} Φ=ϕ∧= ​0ϕ3​−ϕ2​​−ϕ3​0ϕ1​​ϕ2​−ϕ1​0​ ​(3-5)

$\mathfrak{so}(3)$ 的元素是一个三维向量或三维反对称矩阵，即

$$\mathfrak{so}(3)=\left\{\varphi \in {\mathbb{R}}^3,\mathbf{\Phi }={\varphi }^{\wedge }\in {\mathbb{R}}^{3\times 3}\right\}\text{\hspace{1em}(3-6)}$$

$\mathfrak{so}(3)$ 和 SO(3) 的关系由指数映射给定，即

$$\mathbf{R}=\exp ({\varphi }^{\wedge })\text{\hspace{1em}(3-7)}$$

3.5 李代数 $\mathfrak{se}(3)$

对于变换矩阵，亦有李代数 $\mathfrak{se}(3)$，即

$$\mathfrak{se}(3)=\left\{\mathbf{\xi }=\left[\begin{array}{l}\mathbf{\rho }\\ \mathbf{\varphi }\end{array}\right]\in {\mathbb{R}}^6,\mathbf{\rho }\in {\mathbb{R}}^3,\varphi \in \mathfrak{so}(3),{\mathbf{\xi }}^{\wedge }=\left[\begin{array}{ll}{\varphi }^{\wedge }& \mathbf{\rho }\\ 0^{\mathrm{T}}& 0\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}\text{\hspace{1em}(3-8)}$$

这里的 $\wedge$ 不表示对称矩阵，仅表示从向量到矩阵的关系。

3.6 指数与对数映射

3.6.1 SO(3) 上的指数映射

（1）将 $\mathfrak{so}(3)$ 中的三维向量映射到 SO(3) 中的旋转矩阵。

将式（3-7）泰勒展开

$$\exp ({\varphi }^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n\text{\hspace{1em}(3-9)}$$

显然，很难直接计算矩阵的无穷次幂，下面推导一种计算指数映射的简便方法。由于 $\varphi$ 是三维向量，定义它的模长和方向，分别记为 $\theta$ 和 $\mathbf{a}$。则有 $\varphi =\theta \mathbf{a}$（$\theta$ 为模长，$\mathbf{a}$ 为单位方向向量）。

可推导出

a ∧ a ∧ = [ − a 2 2 − a 3 2 a 1 a 2 a 1 a 3 a 1 a 2 − a 1 2 − a 3 2 a 2 a 3 a 1 a 3 a 2 a 3 − a 1 2 − a 2 2 ] = a a T − I (3-10) \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}=\left[\begin{array}{ccc} -a_2^2-a_3^2 & a_1a_2 & a_1a_3 \\ a_1a_2 & -a_1^2-a_3^2 & a_2a_3 \\ a_1a_3 & a_2a_3 & -a_1^2-a_2^2 \end{array}\right]=\boldsymbol{a}\boldsymbol{a}^T-\boldsymbol{I} \tag{3-10} a∧a∧= ​−a22​−a32​a1​a2​a1​a3​​a1​a2​−a12​−a32​a2​a3​​a1​a3​a2​a3​−a12​−a22​​ ​=aaT−I(3-10)

以及

$${\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }={\mathbf{a}}^{\wedge }(\mathbf{a}{\mathbf{a}}^T-\mathbf{I})=-{\mathbf{a}}^{\wedge }\text{\hspace{1em}(3-11)}$$

那么，式（3-9）可推导为

$$\begin{array}{rl}\mathbf{R}=\exp ({\varphi }^{\wedge })=& \exp (\theta {\mathbf{a}}^{\wedge })=\sum _{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}}^{\wedge }\end{array}\text{\hspace{1em}(3-12)}$$

很容易看出，它和罗德里格斯公式很相似。实际上， $\mathfrak{so}(3)$ 就是旋转向量组成的空间，而指数映射即罗德里格斯公式。

（2）相反地，定义对数映射，将旋转矩阵 SO(3) 映射到 $\mathfrak{so}(3)$ 中。

$$\varphi =\ln (\mathbf{R}{)}^{\vee }=(\sum _{n=0}^{\infty }\frac{(-1{)}^n}{n+1}(\mathbf{R}-\mathbf{I}{)}^{n+1}{)}^{\vee }\text{\hspace{1em}(3-13)}$$

但实际上，不需要这么麻烦。在旋转向量部分，我们已经得到了矩阵到向量的转换关系，即

$$\begin{gathered} \theta =\arccos (\frac{tr(\mathbf{R})-1}{2}) \\ \mathbf{Rn}=\mathbf{n}\text{\hspace{1em}(3-14)} \end{gathered}$$

3.6.2 SE(3) 上的指数映射

（1）将 $\mathfrak{se}(3)$ 中的向量映射到 SE(3) 中的变换矩阵。

$$\begin{array}{rl}\exp ({\xi }^{\wedge })=& \left[\begin{array}{cc}{\sum }_{n=0}^{\infty }\frac{1}{n!}({\varphi }^{\wedge }{)}^n& {\sum }_{n=0}^{\infty }\frac{1}{(n+1)!}({\varphi }^{\wedge }{)}^n\mathbf{\rho }\\ 0^T& 1\end{array}\right]\\ \stackrel{△}{=}& \left[\begin{array}{cc}\mathbf{R}& \mathbf{J\rho }\\ 0^T& 1\end{array}\right]=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]=\mathbf{T}\end{array}\text{\hspace{1em}(3-15)}$$

其中， $\mathbf{J}$ 为雅克比矩阵，

$$\mathbf{J}=\frac{\sin \theta }{\theta }\mathbf{I}+(1-\frac{\sin \theta }{\theta })\mathbf{a}{\mathbf{a}}^{\mathbf{T}}+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }\text{\hspace{1em}(3-16)}$$

（2）相反地，通过对数映射，将变换矩阵 SE(3) 映射到 $\mathfrak{se}(3)$ 中。

根据对应关系，旋转向量可由矩阵左上角 $\mathbf{R}$ 计算得到，而平移向量 $\mathbf{\rho }$ 可由 $\mathbf{t}=\mathbf{J\rho }$ 解出。

3.6.3 小结

至此，我们得到了李群、李代数的相互转换关系。

3.7 李代数求导与扰动模型

3.7.1 BCH 公式与近似形式

（1）李代数中的加法不等价于李群中的乘法，即

$$\exp (({\varphi }_1+{\varphi }_2{)}^{\wedge })\ne \exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge })$$

也即

$$\ln (\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge }))\ne ({\varphi }_1+{\varphi }_2{)}^{\wedge }$$

（2）BCH 公式

$$\ln (\exp (\mathbf{A})\exp (\mathbf{B}))=\mathbf{A}+\mathbf{B}+\frac{1}{2}[\mathbf{A},\mathbf{B}]+\frac{1}{12}[\mathbf{A},[\mathbf{A},\mathbf{B}]]-\frac{1}{12}[\mathbf{B},[\mathbf{A},\mathbf{B}]]+...\text{\hspace{1em}(3-17)}$$

针对上式，当 ${\varphi }_1$ 或 ${\varphi }_2$ 为小量时，忽略二次以上的项，近似为：

$$\ln (\exp ({\varphi }_1^{\wedge })\exp ({\varphi }_2^{\wedge }){)}^{\vee }\approx \{\begin{array}{c}{\mathbf{J}}_l({\varphi }_2{)}^{-1}{\varphi }_1+{\varphi }_2\text{ if ϕ1 is small }\\ {\mathbf{J}}_r({\varphi }_1{)}^{-1}{\varphi }_2+{\varphi }_1\text{ if ϕ2 is small }\end{array}\text{\hspace{1em}(3-18)}$$

其中

$${\mathbf{J}}_l=\mathbf{J}=\frac{\sin \theta }{\theta }\mathbf{I}+(1-\frac{\sin \theta }{\theta })\mathbf{a}{\mathbf{a}}^{\mathbf{T}}+\frac{1-\cos \theta }{\theta }{\mathbf{a}}^{\wedge }$$
$${\mathbf{J}}_l^{-1}=\frac{\theta }{2}\cot \frac{\theta }{2}\mathbf{I}+(1-\frac{\theta }{2}\cot \frac{\theta }{2})\mathbf{a}{\mathbf{a}}^{\mathbf{T}}-\frac{\theta }{2}{\mathbf{a}}^{\wedge }\text{\hspace{1em}(3-19)}$$

且

$${\mathbf{J}}_r(\varphi )={\mathbf{J}}_l(-\varphi )\text{\hspace{1em}(3-20)}$$

（3）假设某个旋转矩阵 $\mathbf{R}$ 对应李代数 $\varphi$ ，将其左乘一个微小旋转矩阵 $\Delta \mathbf{R}$，对应的李代数为 $\Delta \varphi$，那么，根据式（3-18），有

$$\Delta \mathbf{RR}=\exp (\Delta {\varphi }^{\wedge })\exp ({\varphi }^{\wedge })=\exp (({\mathbf{J}}_l^{-1}(\varphi )\Delta \varphi +\varphi {)}^{\wedge })\text{\hspace{1em}(3-21)}$$

反之，在李代数上进行微小量加法时，相当于在李群上左乘或右乘一个带左（右）雅克比的量，即

$$\exp ((\varphi +\Delta \varphi {)}^{\wedge })=\exp (({\mathbf{J}}_l\Delta \varphi {)}^{\wedge })\exp ({\varphi }^{\wedge })=\exp ({\varphi }^{\wedge })\exp (({\mathbf{J}}_r\Delta \varphi {)}^{\wedge })\text{\hspace{1em}(3-22)}$$

（4）同样的，对于 SE(3)，也有

$$\exp (\Delta {\mathbf{\xi }}^{\wedge })exp({\xi }^{\wedge })\approx \exp (({\mathcal{J}}_l^{-1}\Delta \mathbf{\xi }+\mathbf{\xi }{)}^{\wedge })$$

$$\exp ({\xi }^{\wedge })exp(\Delta {\mathbf{\xi }}^{\wedge })\approx \exp (({\mathcal{J}}_r^{-1}\Delta \mathbf{\xi }+\mathbf{\xi }{)}^{\wedge })\text{\hspace{1em}(3-23)}$$

其中，${\mathcal{J}}_l$ 是一个 $6\times 6$ 的矩阵。

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

（1）假设我们对空间中某点 $\mathbf{p}$ 进行了旋转，得到了 $\mathbf{Rp}$。现在要计算旋转后的点的坐标相对于旋转矩阵的导数，将其非正式地记为

$$\frac{\partial \mathbf{Rp}}{\partial \mathbf{R}}$$

由于SO(3) 没有加法，故无法按照导数的定义进行计算。假设旋转矩阵 $\mathbf{R}$ 对应的李代数为 $\varphi$，我们转而计算

$$\frac{\partial (\exp ({\varphi }^{\wedge })\mathbf{p})}{\partial \varphi }$$

按照导数定义，有

$$\begin{array}{rl}\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{\varphi }}=\frac{\partial \left(\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}\right)}{\partial \mathbf{\varphi }}& =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{\exp \left((\mathbf{\varphi }+\delta \mathbf{\varphi }{)}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{\exp \left({\left({\mathbf{J}}_l\delta \mathbf{\varphi }\right)}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{\left(\mathbf{I}+{\left({\mathbf{J}}_l\delta \mathbf{\varphi }\right)}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{{\left({\mathbf{J}}_l\delta \mathbf{\varphi }\right)}^{\wedge }\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{-{\left(\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}\right)}^{\wedge }{\mathbf{J}}_l\delta \mathbf{\varphi }}{\delta \mathbf{\varphi }}=-(\mathbf{R}\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l\end{array}\text{\hspace{1em}(3-24)}$$

第 2 行为 BCH 近似；第 3 行为泰勒展开去掉高阶项的结果；第 4 行和第 5 行将反对称符号看做叉积，交换后变号。

但 ${\mathbf{J}}_l$ 形式复杂，计算困难。

3.7.3 扰动模型（左乘）

另一种求导方式是对 $\mathbf{R}$ 进行一次扰动，看结果相对于扰动的变化率。假设微小扰动 $\Delta \mathbf{R}$ 对应的李代数为 $\phi$，对其求导，即

$$\begin{array}{rl}\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{\phi }}& =\frac{\Delta \mathbf{R}\mathbf{Rp}-\mathbf{Rp}}{\partial \mathbf{\phi }}\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\exp \left({\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }}\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\left(\mathbf{I}+{\mathbf{\phi }}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\phi }}\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{{\mathbf{\phi }}^{\wedge }\mathbf{R}\mathbf{p}}{\mathbf{\phi }}=\underset{\mathbf{\phi }\to 0}{\lim }\frac{-(\mathbf{R}\mathbf{p}{)}^{\wedge }\mathbf{\phi }}{\mathbf{\phi }}=-(\mathbf{R}\mathbf{p}{)}^{\wedge }\end{array}\text{\hspace{1em}(3-25)}$$

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

假设空间中某点 $\mathbf{p}$ 经过一次变换 $\mathbf{T}$（对应的李代数为 $\mathbf{\xi }$），得到 $\mathbf{Tp}$，对其左乘一个扰动 $\Delta \mathbf{T}=\exp (\delta {\mathbf{\xi }}^{\wedge })$，对应的李代数为 $\delta \mathbf{\xi }=[\delta \mathbf{\rho },\delta \mathbf{\varphi }{]}^T$，则

$$\begin{array}{rl}\frac{\partial (\mathbf{T}\mathbf{p})}{\partial \delta \mathbf{\xi }}& =\underset{\mathbf{\delta }\mathbf{\xi }\to 0}{\lim }\frac{\Delta \mathbf{T}\mathbf{Tp}-\mathbf{Tp}}{\delta \mathbf{\xi }}\\ & =\underset{\mathbf{\delta }\mathbf{\xi }\to 0}{\lim }\frac{\exp \left(\delta {\mathbf{\xi }}^{\wedge }\right)\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\mathbf{\delta }\mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{(\mathbf{I}+\delta \mathbf{\xi }\wedge )\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\delta {\mathbf{\xi }}^{\wedge }\exp \left({\mathbf{\xi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{cc}\delta {\mathbf{\varphi }}^{\wedge }& \delta \mathbf{\rho }\\ 0^{\mathrm{T}}& 0\end{array}\right]\left[\begin{array}{c}\mathbf{R}\mathbf{p}+\mathbf{t}\\ 1\end{array}\right]}{\delta \mathbf{\xi }}\\ & =\underset{\delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{c}\delta {\mathbf{\varphi }}^{\wedge }(\mathbf{R}\mathbf{p}+\mathbf{t})+\delta \mathbf{\rho }\\ 0^{\mathrm{T}}\end{array}\right]}{[\delta \mathbf{\rho },\delta \mathbf{\varphi }{]}^{\mathrm{T}}}=\left[\begin{array}{cc}\mathbf{I}& -(\mathbf{R}\mathbf{p}+\mathbf{t}{)}^{\wedge }\\ 0^{\mathrm{T}}& 0^{\mathrm{T}}\end{array}\right]\stackrel{\text{ def }}{=}(\mathbf{T}\mathbf{p}{)}^{\odot }\end{array}\text{\hspace{1em}(3-26)}$$

以上两种方法的区别在于，直接法是对旋转矩阵对应的李代数求导，即 $\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \mathbf{\varphi }}$，而扰动模型是对扰动矩阵对应的李代数求导，即 $\frac{\partial (\mathbf{T}\mathbf{p})}{\partial \delta \mathbf{\xi }}$。

3.7.3 相似变换群与李代数

对于空间中某点 $\mathbf{p}$，经过相似变换，得

$${\mathbf{p}}^{\prime }=\left[\begin{array}{ll}s\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\mathbf{p}=s\mathbf{Rp}+\mathbf{t}\text{\hspace{1em}(3-27)}$$

与 SO(3)、SE(3) 类似，相似变换也对矩阵乘法构成群，称为相似变换群 Sim(3)：

$$\mathrm{Sim}(3)=\left\{\mathbf{S}=\left[\begin{array}{ll}s\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\right\}\text{\hspace{1em}(3-28)}$$

对应的李代数为 $\mathfrak{sim}(3)$，其前六维与 $\mathfrak{se}(3)$ 相同，最后多了一项 $\sigma$，即

s i m ( 3 ) = { ζ = [ ρ ϕ σ ] ∈ R 7 , ζ ∧ = [ σ I + ϕ ∧ ρ 0 T 0 ] ∈ R 4 × 4 } (3-29) \mathfrak{sim}(3)=\left\{\boldsymbol{\zeta}=\left[\begin{array}{ll} \boldsymbol{\rho} \\ \boldsymbol{\phi} \\ \sigma \end{array}\right] \in \mathbb{R}^{7} , \boldsymbol{\zeta}^{\wedge}=\left[\begin{array}{ll} \sigma\boldsymbol{I}+\phi^{\wedge} & \boldsymbol{\rho} \\ \mathbf{0}^{\mathrm{T}} & 0 \end{array}\right] \in \mathbb{R}^{4\times4} \right\} \tag{3-29} sim(3)=⎩ ⎨ ⎧​ζ= ​ρϕσ​ ​∈R7,ζ∧=[σI+ϕ∧0T​ρ0​]∈R4×4⎭ ⎬ ⎫​(3-29)

指数映射为

$$\exp ({\zeta }^{\wedge })=\left[\begin{array}{cc}{e}^{\sigma }\exp ({\varphi }^{\wedge })& {\mathbf{J}}_s\mathbf{\rho }\\ 0^T& 1\end{array}\right]\text{\hspace{1em}(3-30)}$$

它和李群的对应的关系为

$$s=e^{\sigma },\mathbf{R}=\exp ({\varphi }^{\wedge }),\mathbf{t}={\mathbf{J}}_s\mathbf{\rho }$$

其中 ${\mathbf{J}}_s$ 为

$$\begin{array}{rl}{\mathbf{J}}_s=& \frac{{\mathrm{e}}^{\sigma }-1}{\sigma }\mathbf{I}+\frac{\sigma {\mathrm{e}}^{\sigma }\sin \theta +\left(1-{\mathrm{e}}^{\sigma }\cos \theta \right)\theta }{{\sigma }^2+{\theta }^2}{\mathbf{a}}^{\wedge }\\ & +\left(\frac{{\mathrm{e}}^{\sigma }-1}{\sigma }-\frac{\left({\mathrm{e}}^{\sigma }\cos \theta -1\right)\sigma +\left({\mathrm{e}}^{\sigma }\sin \theta \right)\theta }{{\sigma }^2+{\theta }^2}\right){\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }\end{array}\text{\hspace{1em}(3-31)}$$

类似地，给予 $\mathbf{Sp}$ 左侧一个扰动量 $\Delta \mathbf{S}$（对应的李代数为 $\mathbf{\zeta }$），并求它对该扰动量的导数，即

$$\frac{\partial \mathbf{S}\mathbf{p}}{\partial \mathbf{\zeta }}=\left[\begin{array}{ccc}\mathbf{I}& -{\mathbf{q}}^{\wedge }& \mathbf{q}\\ 0^{\mathrm{T}}& 0^{\mathrm{T}}& 0\end{array}\right]\text{\hspace{1em}(3-32)}$$

需要注意的是，$\mathbf{Sp}$ 是四维的齐次坐标，$\mathbf{\zeta }$ 是七维向量，所以该导数是 $4\times 7$ 维向量，这里记 $\mathbf{Sp}$ 前三维组成的向量为 $\mathbf{q}$。