简介
对比另外两个比较流行的非线性优化库，Ceres 作为对非线性最小二乘的通用优化库，除了例子中有一些关于 SLAM 的简单应用，并没有对 SLAM 问题有额外的定义的节点和边。而 G2O 虽然对 SLAM 问题中大量可能涉及到的节点和边有预先的定义，但是由于其本身文档的缺乏，导致可能对用户使用不太友好。而 GTSAM 除了其本身对 iSAM 的使用外，对 SLAM 问题（包括其他诸如 Navigation 和 SFM 等问题）都有预先定义好的节点，而且还带有对其内部数学原理说明的文档，因此阅读其文档不仅对 GTSAM 本身的源码结构能够更加清晰地了解，对非线性优化过程中涉及到的数学逻辑也能够有更加清晰地认知。
在阅读源码时，我发现我对刚体运动中旋转的理解还是有点模棱两可，思路不够清晰，在求解雅可比时还是一种解方程的思路来进行并没有对其背后的几何意义又很清晰的认识。因此，这篇博客主要对 GTSAM 文档中李群李代数以及李群中涉及的微分概念进行整理，集中在二维和三维的部分，原文档见：LieGroup.pdf
李群引入
以一个 2 维下运动的小粒子来对李群进行说明。假设一个机器人在平面上位姿为 $(x, y, \theta)$，$t_0$ 时刻位姿为 $(x_0, y_0, \theta_0)$，当给它一个小的向前的速度 $v_x$ 时，它的位置变化为：
$$
\begin{aligned}
\dot{x} &= v_x\\
x_t &= x_0 + v_xt
\end{aligned}
$$
同理，如果同时给 $x$ 和 $y$ 方向提供速度时，位姿变化为：
$$
(x_t, y_t, \theta_t) = (x_0 + v_xt, y_0 + v_y t, \theta_0)
$$
相对地，如果我们不施加平移速度，而是施加旋转角速度速度 $\omega$，位姿变化为：
$$
(x_t, y_t, \theta_t) = (x_0, y_0, \theta_0 + \omega t)
$$
然而，当我们将旋转角速度和平移线速度同时施加时，结果却不是简单的相加，即：
$$
(x_t, y_t, \theta_t) \neq (x_0 + v_xt, y_0 + v_y t, \theta_0 + \omega t)
$$
按照上式右侧表达，机器人会沿直线运动，并绕自身旋转。而我们知道当施加旋转速度的同时加上平移速度时，运动轨迹显然会受机器人自身旋转的影响。在一段很短的时间内，我们可以将机器人按照这个方式进行运动（一阶近似）：
$$
(x_\delta, y_\delta, \theta_\delta) = (x_0 + v_x \delta, y_0 + v_y \delta, \theta_0 + \omega \delta)
$$
然而在下一时刻，由于机器人姿态发生了变化，而我们施加的平移速度是在机器人坐标系下的（我们让机器人向 “前” 或者向 “左” 移动），因此平移时也应该在新的姿态下进行。最后呈现出来的轨迹会是一条弧线，如下图所示：
这里有一个结论，当我们需要对物体进行一个旋转和一个平移时，先进行旋转再进行平移。我们定义两个位姿来描述机器人具体是怎么运动的：
$$
\begin{aligned}
T_1T_2 &= (x_1, y_1, \theta_1)(x_2, y_2, \theta_2)\\
&= (x_1 + \cos{\theta_1}x_2, -\sin{\theta_1}y_2, , y_1 + \sin{\theta_1}x_2 + \cos{\theta_1}y_2, \theta_1 + \theta_2)
\end{aligned}
$$
将其转换成矩阵形式便得到常用的位姿变换矩阵的形式了：
$$
T_1T_2 =
\begin{bmatrix}
R_1 & t_1\\
0 & 1
\end{bmatrix}
\begin{bmatrix}
R_2 & t_2\\
0 & 1
\end{bmatrix}=
\begin{bmatrix}
R_1R_2 & R_2t_2 + t_1\\
0 & 1
\end{bmatrix}
$$
二维情况旋转矩阵为：
$$
R =
\begin{bmatrix}
\cos{\theta} & -\sin{\theta}\\
\sin{\theta} & \cos{\theta}
\end{bmatrix}
$$
而机器人在微小时间内的运动为：
$$
\begin{aligned}
T(\delta) &= \begin{bmatrix}
\cos{\omega\delta} & -\sin{\omega\delta} & v_x\delta\\
\sin{\omega\delta} & \cos{\omega\delta} & v_y\delta\\
0 & 0 & 1
\end{bmatrix}\\
&= \begin{bmatrix}
1 & -\omega\delta & v_x\delta\\
\omega\delta & 1 & v_y\delta\\
0 & 0 & 1
\end{bmatrix}\\
&= I + \delta
\begin{bmatrix}
0 & -\omega & v_x\\
\omega & 0 & v_y\\
0 & 0 & 0
\end{bmatrix}
\end{aligned}
$$
将上述后半部分的矩阵定义为一个 2D Twist （速度）向量：$\xi = (v, \omega)$，并将其矩阵形式表示为：
$$
\hat{\xi} \triangleq
\begin{bmatrix}
0 & -\omega & v_x\\
\omega & 0 & v_y\\
0 & 0 & 0
\end{bmatrix}
$$
当 $t$ 很大时，我们需要将 $t$ 分解成很多部分，并通过以上形式组合起来：
$$
T(t) \approx (I + \frac{t}{n}\hat{\xi})...(I + \frac{t}{n}\hat{\xi}) = (I + \frac{t}{n}\hat{\xi})^n
$$
令 $n$ 趋向于无穷大，我们就可以得到一个方阵的矩阵映射，这个映射为 2D Twist 向量到 2D 刚体变换矩阵的指数映射：
$$
\begin{aligned}
T(t) &= \lim_{n\rightarrow\infty}(I + \frac{t}{n}\hat{\xi})^n &= \exp{(t\hat{\xi})}\\
\end{aligned}
$$
上述的推导过程实际上包括李群理论的所有元素。将 2D 刚体变换矩阵的空间以及其组合 (Compose) 的操作，称为特殊欧式群 SE(2)（Special Euclidean Group）。由于它同时是一个拓扑群以及流形，因此也是一个李群（Lie Group），即对它其中的元素的相乘和求逆都是平滑的。而上述中 2D Twist 向量组成的空间，以及一个特殊的二元操作，称为李代数 (Lie Algebra) $\mathfrak{se}(2)$
李群基本内容
指数映射和对数映射
这一部分主要介绍常用的李群和对应的李代数。
一个李群 G 同时是一个群以及一个流形。而与之对应的李代数 $\mathfrak{g}$ ，以不严谨地说法来看可以认为在其原点的正切空间，定义了该李群在原点下的行为。从李代数 $\mathfrak{g}$ 到李群 G 的映射称为指数映射：$\exp: \mathfrak{g} \rightarrow G$，通常是多对一的映射。从上面的例子可以看出指数映射将李代数映射为李群，而反向的映射可以在局部原点定义，称为对数映射：$\log: G \rightarrow \mathfrak{g}$。李群的类型有很多种，这里只关注矩阵李群。
李代数 $\mathfrak{g}$ 被称为代数的原因为它包含了一个二元操作，称为李括号（Lie bracket）：$[X, Y]$，对于矩阵李群而言，李括号的定义为：$[A, B] \triangleq AB - BA$。李括号和其对于李群的操作如下：对于可交换群中，李代数 $\mathfrak{g}$ 元素的相加可以用来作为群操作。即：我们令李代数 $\mathfrak{g}$ 中的两个元素相加 $Z = X + Y$，将其映射为李群时，其效果等同于将两个李代数元素分别进行映射然后进行群操作：
$$
\exp{(Z)} = \exp{(X + Y)} = \exp{(X)}\exp{(Y)}
$$
而反过来却不行：
$$
Z = \log{(\exp{(X)\exp{(Y)}})} \neq X + Y
$$
而是需要使用 BCH 公式进行计算，由于这里关注的李群都是非可交换群（SO(3), SE(3)）等，因此我们需要用 BCH 公式来进行近似：
$$
Z = X + Y + [X, Y]/2 + [X - Y, [X, Y]]/12 - [Y, [X, [X,Y]]]/24 + ...
$$
指数坐标
对于 n 维的矩阵李群，定义在向量空间下的李代数 $\mathfrak{g}$ 和 n-维实数空间 $\mathbb{R}^n$ 是同构的，因此我们可以定义一个操作，将 $\mathbb{R}^n$ 下的元素 （n 维向量）映射至李代数空间 $\mathfrak{g}$ 下的元素。而对于矩阵李群，其李代数也是 n 维的方阵，对于不同的李群，$\hat{}$ 的具体操作不同，会在后续进行进行整理。
$$
\hat{}:x \in \mathbb{R}^n \rightarrow \hat{x} \in \mathfrak{g}
$$
Action 和 Adjoint
在李群中，除了 Compose 以外，另外两个常见的内容为：Action 和 Adjoint，下面分别进行介绍。
Action 为一个李群作用在其对应流形上的一个的行为。例如，在对一个 2D 点进行 2D 旋转 （SO(2)），或者对一个 3D 点进行 3D 旋转 (SO(3))。SE(2) 或者 SE(3) 的 Action 也是对点（向量）进行刚体变换，但是需要将点的形式改为齐次形式，具体会在后面部分说明。
对于李群 G 而言，其作用在流形 M 上的 Left action 定义为：$\boldsymbol{\Phi}: G \times M \rightarrow M$，且满足以下性质：
- 对于单位李群 e，其作用不会产生效果，即：
$\boldsymbol{\Phi}(e, p) = p$ - 连续两个 Action 可以合并为一个：
$\boldsymbol{\Phi}(g, \boldsymbol{\Phi}(h, p)) = \boldsymbol{\Phi}(gh, p)$
对于 n 维矩阵群 G 而言，action 的形式通常为矩阵-向量相乘，即：$q = Ap\qquad p,q\in \mathbb{R}^n$
有时候，我们想要一个点 p 施加 Action，然而我们只知道该点在另一个坐标系下的的坐标 $p'$，即：$p' = Tp$，T 为全局坐标系到局部坐标系的转换矩阵，因此在施加 Action 前，需要应用 $T^{-1}$，然后在全局坐标系下施加 Action $A$，最后用 $T$ 转换为局部坐标系，即：
$$
q' = TAT^{-1}p'
$$
上式中，我们称 $TAT^{-1}$ 和 A 伴随。通常来收，伴随映射 $Ad_{g}$ 将李群中的元素 $a$ 转换为它的伴随 $gag^{-1}$。这个描述了李群 G 中的伴随关系，但是对应李代数 $\mathfrak{g}$ 也有等效的表示：
$$
\boldsymbol{Ad}_ge^{\hat{x}} = g\exp{(\hat{x})}g^{-1} = \exp{(Ad_g\hat{x})}
$$
上式中，$Ad_g: \mathfrak{g}\rightarrow\mathfrak{g}$ 是利用李群参数 $g$ 对李代数的伴随映射，也被称作伴随表示（adjoint representation）。在上述这个例子中，关于这个映射的直观解释为定义在原点（全局坐标系下）的一个变换 $\exp{(\hat{x})})$，通过李群元素 $g$ 来转换为局部坐标系下的变化，可以写在一步中，即李代数 $\hat{x}$ 的伴随表示 $Ad_g\hat{x}$。
在特殊的矩阵李群下，伴随可以定义为：
$$
Ad_T\hat{x} \triangleq T\hat{x}T^{-1}
$$
根据上面的等式，有：
$$
T\exp{(\hat{x})}T^{-1} = \exp{(T\hat{x}T^{-1})}
$$
这个性质在求李群微分时经常会被用到。接下来主要会对 2D 和 3D 情况下的李群和李代数，以及对应的 Action 和 Adjoint 进行整理。
2D 旋转
2D 旋转下的李群和李代数
在 2D 旋转下，李群 SO(2) 是一系列 $2\times2$ 矩阵的集合。其对应李代数为 $2\times2$ 反对称矩阵的集合，由于 SO(2) 为一维流形，因此 $\mathfrak{so}(2)$ 和 $\mathbb{R}$ 同构，定义其对应的映射如下：
$$
\begin{aligned}
\hat{}&: \mathbb{R} \rightarrow\mathfrak{so}(2)\\
\hat{}&: \omega \rightarrow \hat{\omega} = [\omega]_{+}\\
[\omega]_{+} &=
\begin{bmatrix}
0 & -\omega\\
\omega & 0
\end{bmatrix}
\end{aligned}
$$
李代数到李群的指数映射为：
$$
\exp{([\omega]_{+})} = \begin{bmatrix}
\cos{\omega} & -\sin{\omega}\\
\sin{\omega} & \cos{\omega}
\end{bmatrix}
$$
上式中的 $\omega$ 单位为角度单位，如 rad 或者 deg。
2D 旋转下的 Action, Adjoint
$\mathfrak{so}(2)$ 的 Action 的几何意义为对一个 2D 点进行 2D 旋转，即：$q = Rp$，这个很直观，下面来看一下在一个旋转添加一个增量产生的旋转结果，即：
$$
q(\omega) = R\exp{([\omega]_{+})}p
$$
当 $\omega$ 很小（扰动）时，有：$\exp{([\omega]_{+})}\approx I + [\omega]_{+} = I + [1]_{+}$。此时，$[1]_{+}$ 类似于对点进行平面上的叉乘(旋转 90°)，即：
$$
\begin{aligned}
[1]_{+}\begin{bmatrix}x \\ y\end{bmatrix}= R_{\pi/2}\begin{bmatrix}x \\ y\end{bmatrix} = \begin{bmatrix}-y \\ x\end{bmatrix}
\end{aligned}
$$
因此，对 $\omega$ 的导数为：
$$
\begin{aligned}
\frac{\partial q(\omega)}{\partial\omega} &= R\frac{\partial}{\partial\omega}(\exp{([\omega]_{+})}p) = R\frac{\partial}{\partial\omega}(w[1]_{+}p) = RH_p\\
H_p &\triangleq [1]_{+}p = \begin{bmatrix}
-p_y\\
p_x
\end{bmatrix}
\end{aligned}
$$
这个导数对旋转 $R$ 施加局部坐标系下的增量（扰动） $\hat{\omega}$ 时，表示 $Rp$ 的变化趋势。
$\mathfrak{so}(2)$ 的伴随表示和它本身一样，如下所示：
$$
\begin{aligned}
Ad_R\hat{\omega} &=
\begin{bmatrix}
\cos{\omega} & -\sin{\omega}\\
\sin{\omega} & \cos{\omega}
\end{bmatrix}
\begin{bmatrix}
0 & -\omega\\
\omega & 0
\end{bmatrix}
\begin{bmatrix}
\cos{\omega} & -\sin{\omega}\\
\sin{\omega} & \cos{\omega}
\end{bmatrix}^T\\
&= \begin{bmatrix}
0 & -\omega\\
\omega & 0
\end{bmatrix}\\
Ad_R\hat{\omega} &= \hat{\omega}
\end{aligned}
$$
这个性质的几何意义为：在 2D 情况下，我们将定义同一个旋转矩阵作用在不同坐标系下对一个点进行旋转结果都是一致的。例如，将机器人坐标系下的一个向量 $p$ 旋转 30° 得到旋转后的局部向量坐标 $p'$，或者将这个向量的全局坐标 $q$ 旋转 30° 得到的转换后的全局向量坐标 $q'$，其对应的局部坐标为 $p'$。这个结论应该挺直观的，因为在 2D 情况下旋转轴不变，对一个向量进行旋转应该和它在哪个坐标系下的表示没有关系。不存在在 A 坐标系下旋转了 30°，却在 B 坐标系旋转了 60°。
2D 刚体变换
2D 刚体变换下的李群和李代数
SE(2) 的元素可以表示为以下形式：
$$
T \triangleq
\begin{bmatrix}
R & t\\
0 & 1
\end{bmatrix}
$$
其中，$R\in SO(2), t \in \mathbb{R}^2$，分别为旋转分量和平移分量。其对应的李代数 $\mathfrak{se}(2)$ 为定义在向量空间下的通过 twist 3维坐标 $\xi$ 参数化的 $3\times3$ 矩阵，其对应的映射为：
$$
\begin{aligned}
\xi &\triangleq \begin{bmatrix}
v\\
w
\end{bmatrix}\\
\hat{\xi} &\triangleq
\begin{bmatrix}
[\omega]_{+} & v\\
0 & 0
\end{bmatrix}
\end{aligned}
$$
李代数的指数坐标 $v, \omega$ 的范围分别为长度单位和角度单位
2D 刚体变换下的 Compose, Action, Adjoint
下面来看一下 2D 刚体变换下的三种常见情况：Compose，Action 和 Adjoint。
Compose 为连续进行多次刚体变换，通过变换矩阵连乘即可：
$$
T_1T_2 =
\begin{bmatrix}
R_1 & t_1 \\
0 & 1
\end{bmatrix}
\begin{bmatrix}
R_2 & t_2 \\
0 & 1
\end{bmatrix} =
\begin{bmatrix}
R_1R_2 & R_1t_2 + t_1 \\
0 & 1
\end{bmatrix}
$$
Action 为对一个 2D 点进行旋转和平移，点本身可以视作是起点在原点的一个向量。在进行变换时需要将坐标转为齐次形式，如下所示：
$$
\hat{q} = \begin{bmatrix}
q \\
1
\end{bmatrix} =
\begin{bmatrix}
R & t\\
0 & 1
\end{bmatrix}
\begin{bmatrix}
p \\
1
\end{bmatrix} = T\hat{p}
$$
我们可以将其对应的李代数 $\hat{\xi}$ 作用在点上，得到局部坐标系下的速度向量 $\hat{\xi}\hat{p}$：
$$
\hat{\xi}\hat{p} =
\begin{bmatrix}
[\omega]_{+} & v\\
0 & 0
\end{bmatrix}
\begin{bmatrix}
p \\ 1
\end{bmatrix} =
\begin{bmatrix}
[\omega]_{+}p + v\\
0
\end{bmatrix}
$$
考虑前两行，可以将其视为 $\mathbb{R}^2$ 下的速度，作为一个 $2\times3$ 矩阵 $H_p$ 直接作用在指数坐标 $\xi$ 上，这个表示了这个点沿弧线的切向线速度：
$$
[\omega]_{+}p + v = v + R_{\pi/2}p\omega =
\begin{bmatrix}
I_2 & R_{\pi/2}p
\end{bmatrix}
\begin{bmatrix}
v \\ w
\end{bmatrix} = H_p\xi
$$
$\mathfrak{se}(2)$ 的伴随表示 （Adjoint） 如下，其几何意义为将某个坐标系下的李代数 $\hat{\xi}$ 通过 $T$ 转换为另一个坐标系下的坐标变换：
$$
\begin{aligned}
Ad_T\hat{\xi} &= T\hat{\xi}T^{-1}\\
&=
\begin{bmatrix}
R & t\\
0 & 1
\end{bmatrix}
\begin{bmatrix}
[\omega]_{+} & v\\
0 & 0
\end{bmatrix}
\begin{bmatrix}
R^T & -R^Tt\\
0 & 1
\end{bmatrix}\\
&=
\begin{bmatrix}
[\omega]_{+} & -[\omega]_{+}t + Rv\\
0 & 0
\end{bmatrix}\\
&=
\begin{bmatrix}
[\omega]_{+} & Rv - t^{\perp}\omega\\
0 & 0
\end{bmatrix}
\end{aligned}
$$
通过整理，我们可以得到一个平面 Twist 坐标到另一个坐标系下的转换关系：
$$
\begin{bmatrix}
v'\\
\omega'
\end{bmatrix} =
\begin{bmatrix}
R & -t^\perp\\
0 & 1
\end{bmatrix}
\begin{bmatrix}
v\\
\omega
\end{bmatrix}
$$
3D 旋转
3D 旋转下的李群和李代数
3D 旋转下，李群 SO(3) 是一系列 $3\times3$ 可逆矩阵的集合，其对应的李代数 $\mathfrak{so}(3)$ 为 $3\times3$ 反对称矩阵的集合。由于 SO(3) 是三维流形，因此$\mathfrak{so}(3)$ 和 $\mathbb{R}^3$ 同构，我们定义从三维向量 $\mathbb{R}^3$ 到 $\mathfrak{so}(3)$ 的映射如下。其李括号操作对应的 $\mathbb{R}^3$ 中的叉乘。
$$
\begin{aligned}
\hat{} &: \mathbb{R}^3 \rightarrow \mathfrak{so}(3)\\
\hat{} &: \omega \rightarrow \hat{\omega} = [\omega]_{\times}\\
[\omega]_{\times} &=
\begin{bmatrix}
0 & -\omega_x & \omega_y\\
\omega_z & 0 & -\omega_x\\
-\omega_y & \omega_x & 0
\end{bmatrix}
\end{aligned}
$$
对任意一个三维向量 $\omega$ 可以通过指数映射得到对应的旋转矩阵：$R = \exp{([\omega]_{\times})}$，可以称 $\omega$ 为对应李群的指数坐标系。它和对应旋转的轴角表示等效，其大小 $\theta$ 表示旋转角度，方向表示旋转轴。除了用通用方法计算指数映射外，还可以用以下公式进行计算：
$$
e^{\hat{\omega}} = I + \frac{\sin{\theta}}{\theta}\hat{\omega} + \frac{1-\cos{\theta}}{\theta^2}\hat{\omega}^2
$$
其中，$\hat{\omega}^2 = \omega\omega^T - I$，而 $\omega\omega^T$ 为 $\omega$ 的外积，因此，公式也可以改写为：
$$
e^{\hat{\omega}} = (\cos{\theta})I + \frac{\sin{\theta}}{\theta}\hat{\omega} + \frac{1-\cos{\theta}}{\theta^2}\omega\omega^T
$$
3D 旋转下的 Compose, Action, Adjoint
李群 SO(3) 的 Compose 为连续进行右乘，表示在局部坐标系连续进行旋转：
$$
R = R_1R_2
$$
李群 SO(3) 的 Action 为对一个点进行 3D 旋转：$q = Rp$，同样，在 $R$ 附近利用进行增量旋转的形式为：
$$
q(\omega) = R\exp{([\omega]_{\times})}p
$$
对 $\omega$ 求导可以得到该旋转在收到局部坐标系内的扰动时的变化趋势：
$$
\begin{aligned}
\frac{\partial q(\omega)}{\partial\omega} &= R\frac{\partial}{\partial\omega}(e^{[\omega]_{\times}}p) = R\frac{\partial}{\partial\omega}([\omega]_{\times}p)\\
&= R\frac{\partial}{\partial\omega}(-[p]_{\times}\omega) = -R[p]_{\times}
\end{aligned}
$$
李群 SO(3) 的 Adjoint 如下所示：
$$
\begin{aligned}
R[\omega]_{\times}R^T &= [R\omega]_{\times}\\
Ad_R[\omega]_{\times} &= R[\omega]_{\times}R^T = [R\omega]_{\times}
\end{aligned}
$$
这个映射反映了对于指数坐标，只需要将其旋转从 $\omega$ 旋转至 $R\omega$ 即可完成不同坐标系下旋转的转换。例如，我们想要对局部坐标系 R 的点施加轴角为 $\omega$ 的旋转，我们可以：
- 一种方法是：将点 p 转为世界坐标系，然后施加
$\omega$旋转，最后转换回局部坐标系中：$q = R\exp{[\omega]_\times}R^Tp$ - 另一种方法是，直接对点施加旋转
$\omega$关于$R$的伴随表示(即转换后的轴角)：$q = \exp{([R\omega]_\times)}p$
3D 刚体变换
3D 刚体变换下的李群和李代数
3D 刚体变换下的李群 SE(3) 为一系列 $4\times4$ 的可逆矩阵的几何，其形式如下，其中 $R \in SO(3), t \in \mathbb{R}^3$ 分别为变换中的旋转分量和平移分量
$$
T \triangleq
\begin{bmatrix}
R & t\\
0 & 1
\end{bmatrix}
$$
其对应的 Twist 坐标 $\xi \in \mathbb{R}^3$ 以及其到对应李代数 $\hat{\xi}\in\mathfrak{so}(3)$ 的映射为：
$$
\begin{aligned}
\xi &\triangleq
\begin{bmatrix}
\omega\\
v
\end{bmatrix}\\
\hat{\xi} &\triangleq
\begin{bmatrix}
[\omega]_{\times} & v\\
0 & 0
\end{bmatrix}
\end{aligned}
$$
从李代数到李群的指数映射为 $T = \exp{\hat{\xi}}$
3D 刚体变换下的 Compose, Action, Adjoint
3D 刚体变换下的 Compose 为变换矩阵连续右乘：$T = T_1T_2$，表示在局部坐标系连续进行变换。
3D 刚体变换下的 Action 为对一个 3D 点进行旋转和平移，需要将 3D 的坐标转为齐次形式：
$$
\hat{q} =
\begin{bmatrix}
q\\1
\end{bmatrix} =
\begin{bmatrix}
Rp + t \\1
\end{bmatrix} =
\begin{bmatrix}
R & t\\
0 & 1
\end{bmatrix}
\begin{bmatrix}
p \\ 1
\end{bmatrix} =
T\hat{p}
$$
同样如果在变换所在局部坐标系附近施加一个关于 $\xi$ 的位姿变换如下所示：
$$
\hat{q}(\xi) = T\exp{(\hat{\xi})}p
$$
对 $\omega$ 可以得到该旋转在受到局部坐标系下扰动的变化趋势：
$$
\begin{aligned}
\frac{\partial\hat{q}(\xi)}{\partial\xi} &= T\frac{\partial}{\partial\xi}(\hat{\xi}\hat{p})\\
\hat{\xi}\hat{p} &=
\begin{bmatrix}
[\omega]_{\times} & v\\
0 & 0
\end{bmatrix}
\begin{bmatrix}
p \\ 1
\end{bmatrix} =
\begin{bmatrix}
\omega\times p + v\\
0
\end{bmatrix}
\end{aligned}
$$
上式中，$\omega\times p + v$ 为 $\mathbb{R}^3$ 下的一个速度向量，可以重新将其改写成矩阵相乘形式，表示对该旋转对应的指数坐标 $\xi$ 使用 $H_p$ 进行转换：
$$
\omega\times p + v = -p \times \omega + v =
\begin{bmatrix}
-[p]_{\times} & I_3
\end{bmatrix}
\begin{bmatrix}
\omega \\
v
\end{bmatrix}
$$
因此，上式求导结果为：
$$
\frac{\partial\hat{q}(\xi)}{\partial\xi} = T\frac{\partial}{\partial\xi}(\hat{\xi}\hat{p}) = T
\begin{bmatrix}
-[p]_{\times} & I_3\\
0 & 0
\end{bmatrix}
$$
而对点的逆变换为：
$$
\hat{q} =
\begin{bmatrix}
q\\1
\end{bmatrix} =
\begin{bmatrix}
R^T(p - t) \\1
\end{bmatrix} =
\begin{bmatrix}
R^T & -R^Tt\\
0 & 1
\end{bmatrix}
\begin{bmatrix}
p \\ 1
\end{bmatrix} =
T^{-1}\hat{p}
$$
3D 刚体变换下的伴随映射为：
$$
\begin{aligned}
Ad_T\hat{\xi} &= T\hat{\xi}T^{-1}\\
&=
\begin{bmatrix}
R & t\\
0 & 1
\end{bmatrix}
\begin{bmatrix}
[\omega]_{\times} & v\\
0 & 0
\end{bmatrix}
\begin{bmatrix}
R^T & -R^Tt\\
0 & 1
\end{bmatrix}\\
&=\begin{bmatrix}
[R\omega]_{\times} & -[R\omega]_{\times}t + Rv\\
0 & 0
\end{bmatrix}\\
&=\begin{bmatrix}
[R\omega]_{\times} & t\times R\omega + Rv\\
0 & 0
\end{bmatrix}
\end{aligned}
$$
通过转换，我们也可以讲伴随映射用 twist 坐标的的变换来表达：
$$
\begin{bmatrix}
\omega'\\
v'
\end{bmatrix} =
\begin{bmatrix}
R & 0\\
[t]_{\times}R & R
\end{bmatrix}
\begin{bmatrix}
\omega\\
v
\end{bmatrix}
$$
这个转换表示如何将 twist 坐标从一个坐标系转换到另一个坐标系。