李群与李代数
群
与视觉slam有关的群
三维旋转矩阵构成了特殊正交群(special orthogonal group)
$$S O(3)=\left\{R \in \mathbb{R}^{3 \times 3} \mid R R^{T}=I, \operatorname{det}(R)=1\right\}$$
三维变换矩阵构成了特殊欧氏群（special euclidean group）
$$SE(3)=\left\{T=\left[\begin{array}{cc}R & t \\0^{T} & 1\end{array}\right] \in \mathbb{R}^{4 \times 4} \mid R \in SO(3), t \in \mathbb{R}^{3}\right\} $$
群的定义
群（group）是一种集合加上一种运算的代数结构
记集合为A，运算为·，那么当运算满足以下性质时，称（A，·）成群
$$ \begin{array}{ll}\text { 1. 封闭性: } \quad \forall a_{1}, a_{2} \in A, \quad a_{1} \cdot a_{2} \in A \text { . }\end{array} \\ \text { 2. 结合律: } \quad \forall a_{1}, a_{2}, a_{3} \in A, \quad\left(a_{1} \cdot a_{2}\right) \cdot a_{3}=a_{1} \cdot\left(a_{2} \cdot a_{3}\right) \text { . } \\ \begin{array}{llll}\text { 3. 幺元: } & \exists a_{0} \in A, & \text { s.t. } \quad \forall a \in A, & a_{0} \cdot a=a \cdot a_{0}=a \text { . }\end{array} \\ \begin{array}{llll}\text { 4. 逆: } & \forall a \in A, & \exists a^{-1} \in A, & \text { s.t. } & a \cdot a^{-1}=a_{0} \text { . }\end{array} $$
如果满足交换律，就是交换群，矩阵很少有交换群
李群与李代数
李群的定义
李群是指具有光滑（连续）性质的群，SO(3)和SE(3)表示的空间的旋转和变换，它一定是连续的，所以是李群。
李代数的引出
对于任意旋转矩阵 $R$
$$\boldsymbol{R} \boldsymbol{R}^{T}=\boldsymbol{I}$$
它是对时间的函数
$$\boldsymbol{R}(t) \boldsymbol{R}(t)^{T}=\boldsymbol{I}$$
等式两边对时间求导
（矩阵对标量求导就是每个entry求导后转置）
$$\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}+\boldsymbol{R}(t) \dot{\boldsymbol{R}}(t)^{T}=0$$
$$\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}=-\left(\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}\right)^{T}$$
引入向量与反对称矩阵的转换标志
$$\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], \quad \boldsymbol{A}^{\vee}=\boldsymbol{a}$$
由于 $\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}$ 是一个反对称矩阵，所以有
$$\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}=\phi(t)^{\wedge}$$
$$\dot{\boldsymbol{R}}(t)=\phi(t)^{\wedge} \boldsymbol{R}(t)$$
可以看出对 $R$ 求导就是左乘一个 $\phi(t)^{\wedge}$
在原点处 $t_0 = 0$ ，假设 $R(0) = I$，在原点处泰勒展开
$$\begin{aligned}\boldsymbol{R}(t) & \approx \boldsymbol{R}\left(t_{0}\right)+\dot{\boldsymbol{R}}\left(t_{0}\right)\left(t-t_{0}\right) \&=\boldsymbol{I}+\boldsymbol{\phi}\left(t_{0}\right)^{\wedge}(t)\end{aligned}$$
可以看出 $\phi$ 反应了 $R$ 导数的性质，故称它为 $SO(3)$ 原点附近的正切空间（tangent space），设 $\phi(t_0) = \phi_0$
$$\dot{\boldsymbol{R}}(t)=\phi\left(t_{0}\right)^{\wedge} \boldsymbol{R}(t)=\boldsymbol{\phi}_{0}^{\wedge} \boldsymbol{R}(t)$$
将它看成一阶线性齐次微分方程，解得
$$\boldsymbol{R}(t)=\exp \left(\boldsymbol{\phi}_{0}^{\wedge} t\right)$$
此处并不是传统意义上的 $e^{\phi_0^{\wedge} t}$
该式说明对于任意 $t$ 都可以找到 $R$ 和 $\phi$ 的对应关系，这称为指数映射（exponential map）
$\phi$ 就是 $SO(3)$ 对应的李代数 $so(3)$
每个李群都有对应的李代数，李代数描述了李群的局部性质，准确地说，是单位元附近的正切空间
李代数的定义
李代数由一个集合 $\mathbb{V}$ ，一个数域 $\mathbb{F}$ ，和一个二元运算 $[,]$ 组成。如果它们满足以下几条性质，称 $(\mathbb{V}, \mathbb{F},[,])$ 为一个李代数 $\mathfrak{g}$
$$\begin{array}{ll}\text { 1. 封闭性 } \quad \forall \boldsymbol{X}, \boldsymbol{Y} \in \mathbb{V},[\boldsymbol{X}, \boldsymbol{Y}] \in \mathbb{V} \text { . }\end{array}$$
$$\text { 2. 叉线性 } \quad \forall \boldsymbol{X}, \boldsymbol{Y}, \boldsymbol{Z} \in \mathbb{V}, a, b \in \mathbb{F}, \text { 有: }$$
$$[a \boldsymbol{X}+b \boldsymbol{Y}, \boldsymbol{Z}]=a[\boldsymbol{X}, \boldsymbol{Z}]+b[\boldsymbol{Y}, \boldsymbol{Z}], \quad[\boldsymbol{Z}, a \boldsymbol{X}+b \boldsymbol{Y}]=a[\boldsymbol{Z}, \boldsymbol{X}]+b[\boldsymbol{Z}, \boldsymbol{Y}]$$
$$\text { 3. 自反性 } \quad \forall \boldsymbol{X} \in \mathbb{V},[\boldsymbol{X}, \boldsymbol{X}]=\mathbf{0} \text { . }$$
$$\text { 4. 雅可比等价 } \quad \forall X, Y, Z \in \mathbb{V},[X,[Y, Z]]+[Z,[Y, X]]+[Y,[Z, X]]=0$$
二元运算被称为李括号（lie bracket），表达了两个元素的差异
李代数的例子，集合是三维向量，数域是实数，二元运算是叉积
$$\mathfrak{g} = (\mathbb{R}^3, \mathbb{R}, ×)$$
李代数so(3)
$$\mathfrak{s} \mathfrak{o}(3)=\left\{\boldsymbol{\phi} \in \mathbb{R}^{3}, \boldsymbol{\Phi}=\boldsymbol{\phi}^{\wedge} \in \mathbb{R}^{3 \times 3}\right\}$$
$$\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] \in \mathbb{R}^{3 \times 3}$$
$$\left[\phi_{1}, \phi_{2}\right]=\left(\Phi_{1} \Phi_{2}-\Phi_{2} \Phi_{1}\right)^{\vee}$$
它们是一个由三维向量组成的集合，每个向量对应一个反对称矩阵，可以用于表达旋转矩阵的导数
李代数se(3)
$$\mathfrak{s e}(3)=\left\{\boldsymbol{\xi}=\left[\begin{array}{c}\rho \\\phi\end{array}\right] \in \mathbb{R}^{6}, \boldsymbol{\rho} \in \mathbb{R}^{3}, \boldsymbol{\phi} \in \mathfrak{s} \mathfrak{o}(3), \boldsymbol{\xi}^{\wedge}=\left[\begin{array}{cc}\phi^{\wedge} & \rho \\0^{T} & 0\end{array}\right] \in \mathbb{R}^{4 \times 4}\right\}$$
前三维为平移，后三维为旋转，就是so(3)中的元素，此处 ^ 不再表示反对称
可以简单地理解成se(3)就是由一个平移加上一个so(3)元素构成的向量，虽然此处并不是直接平移
$$\boldsymbol{\xi}^{\wedge}=\left[\begin{array}{cc}\phi^{\wedge} & \rho \\0^{T} & 0\end{array}\right] \in \mathbb{R}^{4 \times 4}$$
$$\left[\xi_{1}, \xi_{2}\right]=\left(\xi_{1}^{\wedge} \xi_{2}^{\wedge}-\xi_{2}^{\wedge} \xi_{1}^{\wedge}\right)^{\vee}$$
指数映射和对数映射
SO(3)上的指数映射
指数映射
$$R=\exp \left(\phi^{\wedge}\right)$$
有泰勒展式得
$$\exp \left(\phi^{\wedge}\right)=\sum_{n=0}^{\infty} \frac{1}{n !}\left(\phi^{\wedge}\right)^{n}$$
由于幂次太多不好计算，考虑将 $\phi$ 分解成 $\theta a$ ，因为 $\phi$ 是三维向量，可以分解成方向和模长，此处 $||a|| =1$，这样又如下两条性质
$$\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}=\boldsymbol{a} \boldsymbol{a}^{T}-\boldsymbol{I}$$
$$\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}=-\boldsymbol{a}^{\wedge}$$
这样就可以对泰勒展式进行化简
$$ \begin{aligned}\exp \left(\phi^{\wedge}\right) &=\exp \left(\theta \boldsymbol{a}^{\wedge}\right)=\sum_{n=0}^{\infty} \frac{1}{n !}\left(\theta \boldsymbol{a}^{\wedge}\right)^{n} \\&=\boldsymbol{I}+\theta \boldsymbol{a}^{\wedge}+\frac{1}{2 !} \theta^{2} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\frac{1}{3 !} \theta^{3} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\frac{1}{4 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{4}+\ldots \\&=\boldsymbol{a} \boldsymbol{a}^{T}-\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\theta \boldsymbol{a}^{\wedge}+\frac{1}{2 !} \theta^{2} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}-\frac{1}{3 !} \theta^{3} \boldsymbol{a}^{\wedge}-\frac{1}{4 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{2}+\ldots \\&=\boldsymbol{a} \boldsymbol{a}^{T}+\left(\theta-\frac{1}{3 !} \theta^{3}+\frac{1}{5 !} \theta^{5}-\ldots\right) \boldsymbol{a}^{\wedge}-\left(1-\frac{1}{2 !} \theta^{2}+\frac{1}{4 !} \theta^{4}-\ldots\right) \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \\&=\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\boldsymbol{I}+\sin \theta \boldsymbol{a}^{\wedge}-\cos \theta \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \\&=(1-\cos \theta) \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\boldsymbol{I}+\sin \theta \boldsymbol{a}^{\wedge} \\&=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{a} \boldsymbol{a}^{T}+\sin \theta \boldsymbol{a}^{\wedge}\end{aligned} $$
最终结果
$$\exp \left(\theta \boldsymbol{a}^{\wedge}\right)=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{a} \boldsymbol{a}^{T}+\sin \theta \boldsymbol{a}^{\wedge}$$
这与罗德里格斯公式一致，说明so(3)实际上就是由所谓的旋转向量组成的空间。这样so(3)中任意一个向量对应到了SO(3)中的旋转矩阵
so(3)上的对数映射
反过来可以定义对数映射，表示SO(3)到so(3)
$$\phi=\ln (R)^{\vee}=\left(\sum_{n=0}^{\infty} \frac{(-1)^{n}}{n+1}(R-I)^{n+1}\right)^{\vee}$$
但实际不应该这样求，更简单的方法是
$$\theta=\arccos \left(\frac{\operatorname{tr}(\boldsymbol{R})-1}{2}\right)$$
$$\boldsymbol{R} \boldsymbol{n}=\boldsymbol{n}$$
总结
指数映射是满射，不是单射，这意味着可能存在多个so(3)对应同一个SO(3)。但是，把旋转角固定在[-π,+π]之间，就是一一对应的
旋转矩阵的导数可以由旋转向量指定。总结一下就是 $R$ 对时间求导后的 $\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}$ 是个反对称矩阵，那就有个 $\phi$ 它是个三维向量，变成旋转向量，指数映射刚好可以从罗德里格斯公式求出
SE(3)上的指数映射
求解过程与so(3)相似
$$\begin{aligned}\exp \left(\boldsymbol{\xi}^{\wedge}\right) &=\left[\begin{array}{ccc}\sum_{n=0}^{\infty} \frac{1}{n !}\left(\phi^{\wedge}\right)^{n} & \sum_{n=0}^{\infty} \frac{1}{(n+1) !}\left(\phi^{\wedge}\right)^{n} \rho \\& 0^{T} & 1\end{array}\right] \\& \triangleq\left[\begin{array}{cc}R & J \rho \\0^{T} & 1\end{array}\right]=T\end{aligned}$$
$$\begin{aligned}\sum_{n=0}^{\infty} \frac{1}{(n+1) !}\left(\phi^{\wedge}\right)^{n} &=I+\frac{1}{2 !} \theta \boldsymbol{a}^{\wedge}+\frac{1}{3 !} \theta^{2}\left(\boldsymbol{a}^{\wedge}\right)^{2}+\frac{1}{4 !} \theta^{3}\left(\boldsymbol{a}^{\wedge}\right)^{3}+\frac{1}{5 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{4} \ldots \\&=\frac{1}{\theta}\left(\frac{1}{2 !} \theta^{2}-\frac{1}{4 !} \theta^{4}+\cdots\right)\left(\boldsymbol{a}^{\wedge}\right)+\frac{1}{\theta}\left(\frac{1}{3 !} \theta^{3}-\frac{1}{5} \theta^{5}+\cdots\right)\left(\boldsymbol{a}^{\wedge}\right)^{2}+\boldsymbol{I} \\&=\frac{1}{\theta}(1-\cos \theta)\left(\boldsymbol{a}^{\wedge}\right)+\frac{\theta-\sin \theta}{\theta}\left(\boldsymbol{a} \boldsymbol{a}^{\mathrm{T}}-\boldsymbol{I}\right)+\boldsymbol{I} \\&=\frac{\sin \theta}{\theta} \boldsymbol{I}+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a} \boldsymbol{a}^{\mathrm{T}}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge} \stackrel{\text { def }}{=} \boldsymbol{J}\end{aligned}$$
$$\boldsymbol{J}=\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}^{\wedge}$$
se(3)上的对数映射
由以下两条可以求出 $\phi$
$$\theta=\arccos \left(\frac{\operatorname{tr}(\boldsymbol{R})-1}{2}\right)$$
$$\boldsymbol{R} \boldsymbol{n}=\boldsymbol{n}$$
由 $\phi$ 可以求出 $J$，$\rho$ 可以通过解线性方程求得
$$t=J \rho$$
总结
image)李代数求导与扰动模型
BCH公式与近似形式
由于李群上无加法，所以很难对李群求导，有两个思路。利用李代数上加法定义李群元素的导数？利用指数映射和对数映射完成转换
要解决的基本问题：李代数上的加法是否等价于李群上的乘法？即下式是否成立？
$$\exp \left(\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)=\exp \left(\left(\phi_{1}+\phi_{2}\right)^{\wedge}\right) ?$$
$$\ln{(\exp{(A)} \exp{(B)})} = A + B ?$$
答案是不成立，计算公式是BCH公式
$$\ln (\exp (\boldsymbol{A}) \exp (\boldsymbol{B}))=\boldsymbol{A}+\boldsymbol{B}+\frac{1}{2}[\boldsymbol{A}, \boldsymbol{B}]+\frac{1}{12}[\boldsymbol{A},[\boldsymbol{A}, \boldsymbol{B}]]-\frac{1}{12}[\boldsymbol{B},[\boldsymbol{A}, \boldsymbol{B}]]+\cdots$$
当其中一个为小量时，bch拥有线性近似表示（分为左乘模型和右乘模型）
$$\ln \left(\exp \left(\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)\right)^{\vee} \approx\left\{\begin{array}{ll}J_{l}\left(\phi_{2}\right)^{-1} \phi_{1}+\phi_{2} & \text { if } \phi_{1} \text { is small } \\J_{r}\left(\phi_{1}\right)^{-1} \phi_{2}+\phi_{1} & \text { if } \phi_{2} \text { is small }\end{array}\right.$$
$$\boldsymbol{J}_{l}=\boldsymbol{J}=\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}^{\wedge}$$
$$\boldsymbol{J}_{l}^{-1}=\frac{\theta}{2} \cot \frac{\theta}{2} \boldsymbol{I}+\left(1-\frac{\theta}{2} \cot \frac{\theta}{2}\right) \boldsymbol{a} \boldsymbol{a}^{T}-\frac{\theta}{2} \boldsymbol{a}^{\wedge}$$
$$J_{r}(\phi)=J_{l}(-\phi)$$
这样就解决了李群乘法和李代数加法的问题
SO(3)上的BCH近似
$$\exp \left(\Delta \phi^{\wedge}\right) \exp \left(\phi^{\wedge}\right)=\exp \left(\left(\phi+J_{l}^{-1}(\phi) \Delta \phi\right)^{\wedge}\right)$$
$$\exp \left((\phi+\Delta \phi)^{\wedge}\right)=\exp \left(\left(J_{l} \Delta \phi\right)^{\wedge}\right) \exp \left(\phi^{\wedge}\right)=\exp \left(\phi^{\wedge}\right) \exp \left(\left(J_{r} \Delta \phi\right)^{\wedge}\right)$$
SE(3)上的BCH近似
$$\begin{array}{l}\exp \left(\Delta \xi^{\wedge}\right) \exp \left(\xi^{\wedge}\right) \approx \exp \left(\left(\mathcal{J}_{l}^{-1} \Delta \xi+\xi\right)^{\wedge}\right) \\\exp \left(\xi^{\wedge}\right) \exp \left(\Delta \xi^{\wedge}\right) \approx \exp \left(\left(\mathcal{J}_{r}^{-1} \Delta \xi+\xi\right)^{\wedge}\right)\end{array}$$
问题引出
假设机器人的位姿为 $T$ ，它观测到一个世界坐标系位于 $p$ 的点，产生一个观测数据 $z$ ，假设噪声是 $w$
$$z = Tp + w$$
误差
$$e = z - Tp$$
假设一共有 $N$ 个这样的路标点和观测，就有 $N$ 个式子。那么对机器人进行位姿估计相当于寻找一个最优的 $T$ ，使得误差最小化
$$\min _{\boldsymbol{T}} J(\boldsymbol{T})=\sum_{i=1}^{N}\left\|\boldsymbol{z}_{i}-\boldsymbol{T} \boldsymbol{p}_{i}\right\|_{2}^{2}$$
求解类似问题都要用到导数，由于$SO(3)$ $SE(3)$上没有很好地定义加法，所以求导很麻烦
它们的李代数上定义了良好的加法，所以用李代数求导
这样就有两种思路：
对 $R$ 对应的李代数加上小量，求相对于小量的变化率（导数模型）
对 $R$ 左乘或右乘一个小量，求相对于小量的李代数的变化率（扰动模型）
求导模型
考虑 $SO(3)$ 上的情况，假设对一个空间点 $p$ 进行旋转，得到 $Rp$ 现在要计算旋转之后的点的坐标相对于旋转的导数，非正式地记为
$$\frac{\partial(\boldsymbol{R} \boldsymbol{p})}{\partial \boldsymbol{R}}$$
由于它在李群上不好计算，所以转成李代数
设 $R$ 对应的李代数为 $\phi$
$$ \begin{aligned}\frac{\partial\left(\exp \left(\phi^{\wedge}\right) p\right)}{\partial \phi} &=\lim _{\delta \phi \rightarrow 0} \frac{\exp \left((\phi+\delta \phi)^{\wedge}\right) p-\exp \left(\phi^{\wedge}\right) p}{\delta \phi} \\&=\lim _{\delta \phi \rightarrow 0} \frac{\exp \left(\left(J_{l} \delta \phi\right)^{\wedge}\right) \exp \left(\phi^{\wedge}\right) p-\exp \left(\phi^{\wedge}\right) p}{\delta \phi} \\& \approx \lim _{\delta \phi \rightarrow 0} \frac{\left(I+\left(J_{l} \delta \phi\right)^{\wedge}\right) \exp \left(\phi^{\wedge}\right) p-\exp \left(\phi^{\wedge}\right) p}{\delta \phi} \\&=\lim _{\delta \phi \rightarrow 0} \frac{\left(J_{l} \delta \phi\right)^{\wedge} \exp \left(\phi^{\wedge}\right) p}{\delta \phi} \\&=\lim _{\delta \phi \rightarrow 0} \frac{-\left(\exp \left(\phi^{\wedge}\right) p\right)^{\wedge} J_{l} \delta \phi}{\delta \phi}=-(R p)^{\wedge} J_{l}\end{aligned} $$
这样就推导出旋转后的点相对于李代数的导数
$$\frac{\partial(\boldsymbol{R} \boldsymbol{p})}{\partial \boldsymbol{\phi}} = -(\boldsymbol{R} p)^{\wedge} \boldsymbol{J}_{l}$$
扰动模型(左乘）
对 $R$ 左乘一个扰动 $\Delta R$ ，设左扰动对应的李代数为 $\varphi$，对 $\varphi$ 求导
$$\begin{aligned}\frac{\partial(R p)}{\partial \varphi} &=\lim _{\varphi \rightarrow 0} \frac{\exp \left(\varphi^{\wedge}\right) \exp \left(\phi^{\wedge}\right) p-\exp \left(\phi^{\wedge}\right) p}{\varphi} \\& \approx \lim _{\varphi \rightarrow 0} \frac{\left(1+\varphi^{\wedge}\right) \exp \left(\phi^{\wedge}\right) p-\exp \left(\phi^{\wedge}\right) p}{\varphi} \\&=\lim _{\varphi \rightarrow 0} \frac{\varphi^{\wedge} R p}{\varphi}=\lim _{\varphi \rightarrow 0} \frac{-(R p)^{\wedge} \varphi}{\varphi}=-(R p)^{\wedge}\end{aligned}$$
SE(3)上的扰动模型
$$\frac{\partial(T p)}{\partial \delta \xi}=\lim _{\delta \xi \rightarrow 0} \frac{\exp \left(\delta \xi^{\wedge}\right) \exp \left(\xi^{\wedge}\right) p-\exp \left(\xi^{\wedge}\right) p}{\delta \xi} \\ \approx \lim _{\delta \xi \rightarrow 0} \frac{\left(I+\delta \xi^{\wedge}\right) \exp \left(\xi^{\wedge}\right) p-\exp \left(\xi^{\wedge}\right) p}{\delta \xi} \\ =\lim _{\delta \xi \rightarrow 0} \frac{\delta \boldsymbol{\xi}^{\wedge} \exp \left(\boldsymbol{\xi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\xi}} \\ =\lim _{\delta \xi \rightarrow 0} \frac{\left[\begin{array}{cc}\delta \phi^{\wedge} & \delta \rho \\0^{T} & 0\end{array}\right]\left[\begin{array}{c}R p+t \\1\end{array}\right]}{\delta \xi} \\=\lim _{\delta \xi \rightarrow 0} \frac{\left[\begin{array}{c}\delta \phi^{\wedge}(\boldsymbol{R} p+\boldsymbol{t})+\delta \boldsymbol{\rho} \\0\end{array}\right]}{\delta \boldsymbol{\xi}}=\left[\begin{array}{cc}\boldsymbol{I} & -(\boldsymbol{R} \boldsymbol{p}+\boldsymbol{t})^{\wedge} \\\mathbf{0}^{T} & \mathbf{0}^{T}\end{array}\right] \triangleq(\boldsymbol{T} \boldsymbol{p})^{\odot}$$