GTSAM 学习记录（三）：常见李群李代数及其函数的导数和微分

简介

在 GTSAM 学习记录（二）：李群李代数及其几何意义中，我根据 GTSAM 的文档对 SLAM 过程中常见的李群（2D 和 3D 情况下的旋转以及刚体变换）进行了整理，尝试理解其对应的几何意义。这篇博客主要是对 GTSAM 的另一篇文档进行整理。主要内容是对非线性优化中常见的一些函数进行微分和导数的推导。在一些常见的关于旋转的函数对旋转求导过程的推导里面，我也进行了一些常见函数的推导，但主要还是集中在运用一些换算技巧来进行解题的思路，这篇博客主要尝试在推导微分和导数的同时，理解其几何意义。源文档见：Derivatives and Differentials

多元微分基本概念

向量空间的多元微分比较基础，这里简单列举一下其几个基本概念，对于导数的计算方法可以参考：矩阵微积分。

函数的雅可比和导数

如果对函数 $f: \mathbb{R}^n \rightarrow \mathbb{R}^m$ 而言，有一个矩阵 $f'(a)\in \mathbb{R}^{m\times n}$ 满足以下条件则这个函数是可微的

$$
\lim_{\xi\rightarrow 0}\frac{|f(a) + f'(a)\xi - f(a + \xi)|}{|\xi|} = 0
$$

上式中， $|e|$ 表示向量的模。 $f'(a)$ 为函数 $f$ 在 $a$ 点的雅可比矩阵，线性映射： $Df_a: \xi \rightarrow f'(a)\xi$ 称为 $f$ 在 $a$ 的导数，在之后也会用 $F_a \triangleq f'(a)$ 来表示雅可比，用来强调它是一个矩阵。而在使用中，我们常用导数 $Df_a$ 来将一个变量值的增量 $\xi$ 通过雅可比矩阵映射为函数值上的增量： $f'(a)\xi$ ，因此可以用它来对函数 $f$ 进行线性近似，这个也是函数 $f$ 的泰勒一阶展开：

$$
f(a + \xi) \approx f(a) + f'(a)\xi
$$

导数性质

链式法则：如果 $f: \mathbb{R}^n \rightarrow \mathbb{R}^p$ 在 $a$ 可微且 $g: \mathbb{R}^p \rightarrow \mathbb{R}^m$ 在 $f(a)$ 可微，则对于函数 $h = g\circ f$ 在 $a$ 的雅可比矩阵 $H_a \in \mathbb{R}^{m\times n}$ 为： $H_a = G_{f(a)}F_a$
逆：如果 $f: \mathbb{R}^n \rightarrow \mathbb{R}^p$ 可微且有一个可微的逆函数 $g\triangleq f^{-1}$ 则它在 $a$ 的雅可比矩阵 $G_a$ 是 $f$ 在 $g(a)$ 的雅可比矩阵的逆，即 $G_a = [F_{g(a)}]^{-1}$

自变量为李群的多元函数

对于李群而言，由于其没有向量空间上的加法操作，因此为了引入导数的概念，这里用一个映射将局部坐标 $\xi$ 映射到李群元素 a 在李群空间附近的邻域上，用来作为李群上的 “加法” 操作，以方便我们后续通过增量的方式定义导数，如下所示：

$$
a\oplus \xi \triangleq a \exp{(\hat{\xi})}
$$

式中， $\xi\in\mathbb{R}^n$ 是 $a$ 系下的局部坐标， $\hat{\xi}$ 为 $\xi$ 的对应李代数， $\exp{\xi}$ 为李代数到李群的指数映射。以李群 SO(3) 为例，局部坐标可以表示为 $\xi = \omega t$ ，其几何意义为以 $a$ 作为参考系下的一个角度扰动。 $\hat{\xi} = [\omega t]_{\times}$ 为反对称矩阵。

局部坐标和正切向量

在微分几何中，在李群 $a$ 下的正切向量 $v \in T_aG$ 是李代数 $\mathfrak{g}$ 的元素，定义为：

$$
v \triangleq \left.\frac{\partial \gamma(t)}{\partial t}\right|_{t = 0}
$$

其中， $\gamma$ 是在 $t = 0$ 经过李群 $a$ 的曲线，即 $\gamma(0) = a$ 。对任何的局部坐标 $\xi$ ，通过在上一部分中定义的李群增量方法，都可以作为 $\gamma$ 函数，即： $\gamma(t) = a\exp{(\widehat{t\xi})}$ ，其对应的正切向量为：

$$
v = \left.\frac{\partial a\exp{(\widehat{t\xi})}}{\partial t}\right|_{t = 0} = a\left.\frac{\partial (I + t\hat{\xi})}{\partial t}\right|_{t = 0} = a\hat{\xi}
$$

这个等式定义了局部坐标 $\xi\in\mathbb{R}^n$ 和实际正切向量 $a\hat{xi} \in \mathfrak{g}$ 的映射关系。向量 $\xi$ 定义了 a 在其自身坐标系下流形上的运动方向（变化趋势）。举个例子，假设 $R^n_b(t)$ 为一个从导航系 $n$ 到载体系 $b$ 的旋转。通过一个校正后的陀螺仪测得载体在载体系下的角速度 $\omega^b$ ，对应的正切向量为： $\dot{R}^n_b(t) = R^n_b\widehat{\omega^b}$

导数

我们可以类比向量空间下的导数定义来对李群空间下的导数定义，这里关注的函数为： $f: G\rightarrow \mathbb{R}^m$ ，即从李群空间到 m 维实数空间下的映射，对李群 $a$ 采用局部坐标 $\xi$ 添加增量，并利用其函数值的线性近似来定义李群函数的导数：

$$
f(a\exp{(\hat{\xi})}) \approx f(a) + f'(a)\xi
$$

假设对 $f: G\rightarrow \mathbb{R}^m$ 在 $a\in G$ 下存在矩阵 $f'(a) \in \mathbb{R}^{m\times n}$ 满足：

$$
\lim_{\xi\rightarrow 0}\frac{|f(a) + f'(a)\xi - f(a\exp{(\hat{\xi})})|}{|\xi|} = 0
$$

则称 $f$ 在 $a$ 可微，其中， $f'(a)$ 为 $f$ 在 $a$ 下的雅可比矩阵， $Df_a:\xi \rightarrow f'(a)\xi$ 为 $f$ 在 $a$ 下的导数。

常见李群函数的求导

常见李群到向量的函数有：Action（对点进行正变换），Inverse Action (对点进行逆变换)，下面分别来看一下函数导数如何计算。

先看正变换，多元函数依赖两个自变量 $f: G \times \mathbb{R}^n \rightarrow \mathbb{R}^n$ ：

$$
f(T, p) = Tp
$$

结合自变量和因变量的维度，我们知道 $f$ 在 $(T, p)$ 的雅可比矩阵是一个 $n \times (m + n)$ 的矩阵，结合乘法的导数有：

$$
Df_{(T,p)}(\xi, \delta p) = D_1f_{T, p}(\xi) + D_2f_{T, p}(\delta p)
$$

下面分别进行求导：

对 $p$ 求导，需要对 $p$ 施加增量最后利用上面式子对照可以等到对应雅可比矩阵：

$$
\begin{aligned}
f(T, p + \delta p) &= T(p + \delta p) \\
&= Tp + T\delta p = f(T, p) + D_2f(\delta p)\\
\Rightarrow  D_2f(\delta p) &= T\delta p
\end{aligned}
$$

接下来是 $D_1f$ ，这是一个关于第一个参数 $T$ 的变化的雅可比矩阵，我们想要找到一个矩阵能够满足以下等式（导数定义）：

$$
Tp + D_1f(\xi) \approx f(T\exp{(\hat{\xi}}), p)= T\exp{(\hat{\xi})p}
$$

在小量的情况下，可以对指数映射进行泰勒展开得到：

$$
\begin{aligned}
T\exp{(\hat{\xi})p} &\approx T(I+\hat{\xi})p = Tp + T\hat{\xi}p\\
&= Tp + D_1f(\xi)\\
\end{aligned}
$$

可以证明，对不同的李群， $\hat{\xi}p$ 可以转换为一个关于 $p$ 的矩阵 $H_p$ 和 $\xi$ 相乘的形式。因此，我们得到了 $f$ 对 $(T, p)$ 的雅可比矩阵为： $F_{(T, p)} = \begin{bmatrix} TH(p) & T\end{bmatrix} = T\begin{bmatrix} H(p) & I\end{bmatrix}$ ，其中， $H_p$ 为关于 p 的矩阵。举个例子，对 SO(3)，有： $\hat{\xi}p = -[p]_{\times}\xi$ ，则雅可比矩阵为： $F_{(R, p)} = R\begin{bmatrix} -[p]_{\times} & I_3\end{bmatrix}$

接下来看逆变换的求导，逆变换函数： $g: G\times \mathbb{R}^{n} \rightarrow \mathbb{R}^{n}$ 如下所示：

$$
g(T, p) = T^{-1}p
$$

同样，先对 $p$ 求导：

$$
g(T, p + \delta p) = T^{-1}(p + \delta p) = T^{-1}p + T_{-1}\delta p = g(T, p) + D_2g(\delta p)
$$

接下来对 $T$ 求导，利用指数映射的泰勒展开化简：

$$
\begin{aligned}
g(Te^{\hat{\xi}}, p) &= (Te^{\hat{\xi}})^{-1}p = e^{-\hat{\xi}}T^{-1}p\\
&= (I - \hat{\xi})T^{-1}p = T^{-1}p - \hat{\xi}(T^{-1}p) = g(T,p) + D_1g(\xi)
\end{aligned}
$$

同样，对不同的李群，可以将 $- \hat{\xi}(T^{-1}p)$ 转换为一个关于 $T^{-1}p$ 的矩阵和 $\xi$ 相乘，因此 $g$ 对 $(T,p)$ 的雅可比矩阵： $G_{(T,p)} = \begin{bmatrix} -H(T^{-1}p) & T^{-1}\end{bmatrix}$ 。在 SO(3) 下，同理可得： $G_{(R,p)} = \begin{bmatrix}[R^Tp]_{\times} & R^T\end{bmatrix}$

李群之间的映射

以上，我们概括了两类函数，一类是从向量空间到向量空间的函数，一类是从李群到向量空间的函数。接下来关注李群到李群的映射的求导。假设函数为： $f = g\circ \phi$ ，其中， $g: G\rightarrow \mathbb{R}^n$ ，而 $\phi: H \rightarrow G$ 是 n 维李群 H 到 p 维李群的平滑映射。按照复合函数的求导法则，我们可以通过链式法则，获得 $f$ 在李群 $a \in H$ 的导数：

$$
f'(a) = (g\circ \phi)'(a) = G_{\phi(a)}\phi'(a)
$$

其中， $\phi'(a)$ 是一个 $n\times p$ 的矩阵，是对 $\phi: H \rightarrow G$ 的最佳线性近似。对应的线性映射 $D\phi_a$ 称为 $\phi$ 在 $a$ 的微分或前推 (pushforward)。和函数导数类似定义我们可以给出（不严谨）的 $\phi$ 在 $a$ 的前推为一个线性映射 $D\phi_a: \mathbb{R}^n \rightarrow \mathbb{R}^p$ ， $D\phi_a(\xi) \triangleq \phi'(a)\xi$ 且当 $\xi \rightarrow 0$ 时：

$$
\phi(ae^{\hat{\xi}}) \approx\phi(a)\exp{(\widehat{\phi'(a)\xi})}
$$

这里，称 $\phi'(a)$ 为映射 $\phi$ 在 $a$ 的雅可比矩阵。接下来看一下常见李群映射的雅可比矩阵推导。

左乘常值映射

设 G 为 n 维李群。映射 $\phi: G \rightarrow G$ 定义为： $\phi(g) = hg$ ，其中 $h \in G$ 为一个常值。下面进行映射的雅可比矩阵推导：

设 $y = D\phi_a(\xi)$ ，由导数定义有：

$$
\begin{aligned}
    \phi(a)e^{\hat{y}} &= \phi(ae^{\hat{\xi}})\\
    hae^{\hat{y}} &= hae^{\hat{\xi}}\\
    \Rightarrow \hat{y} &= \hat{\xi}\\
    \Rightarrow D\phi_a(\xi) &= \xi\\
    \Rightarrow \phi'(a)\xi &= \xi \Rightarrow \phi'(a) = I
\end{aligned}
$$

因此，对应左乘常值映射 $\phi(g)$ ，其在 $a\in G$ 的雅可比矩阵为 $I$ 。

逆映射

对逆映射的雅可比矩阵推导利用到李群的这个性质：施加不同坐标系的增量的过程也可以用施加该增量的伴随形式来表示，即：

$$
ge^{\hat{\xi}}g^{-1} = \exp{(Ad_g\hat{\xi})}
$$

其中， $Ad_g: \mathfrak{g} \rightarrow \mathfrak{g}$ 为李代数的伴随表示。

设 G 为 n 维李群。映射 $\phi: G \rightarrow G$ 定义为李群的逆： $\phi(g) = g^{-1}$ ，其雅可比矩阵推导如下：

设 $y = D\phi_a(\xi)$ ，由导数定义有：

$$
\begin{aligned}
    \phi(a)e^{\hat{y}} &= \phi(ae^{\hat{\xi}})\\
    a^{-1}e^{\hat{y}} &= (ae^{\hat{\xi}})^{-1}\\
    (a^{-1}e^{\hat{y}})^{-1} &= ae^{\hat{\xi}}\\
    e^{-\hat{{y}}}a &= ae^{\hat{\xi}}\\
    e^{-\hat{{y}}} &= ae^{\hat{\xi}}a^{-1}\\
    -\hat{y} &= Ad_g(\hat{\xi})\\
    \hat{y} &= -Ad_g(\hat{\xi})
\end{aligned}
$$

因此，我们得到结论：逆映射的雅可比矩阵满足以下关系：

$$
\widehat{(\phi'(g))\xi} = -Ad_g(\hat{\xi})
$$

这个结论的直观意义是：对逆映射的线性估计为对 $\xi$ 取反，同时取伴随矩阵保证增量施加在正确的坐标系上。以 SO（3）下的 3D 旋转为例，李代数的伴随表示为：

$$
Ad_g(\hat{\omega}) = R\hat{\omega}R^T = [R\omega]_{\times}
$$

因此，逆映射的微分为： $\widehat{(\phi'(R))\omega} = -[R\omega]_{\times}$ ，雅可比矩阵为： $\phi'(R) = -R$

右乘常值映射

设 G 为 n 维李群。映射 $\phi: G \rightarrow G$ 定义为： $\phi(g) = gh$ ，其中 $h \in G$ 为一个常值。下面进行映射的雅可比矩阵推导：

设 $y = D\phi_a(\xi)$ ，由导数定义有：

$$
\begin{aligned}
\phi(a)e^{\hat{y}} &= \phi(ae^{\hat{\xi}})\\
ahe^{\hat{y}} &= ae^{\hat{\xi}}h\\
e^{\hat{y}} &= h^{-1}e^{\hat{\xi}}h\\
\hat{y} &= Ad_{h^{-1}}\hat{\xi}
\end{aligned}
$$

因此，我们得到结论：逆映射的雅可比矩阵满足以下关系，和 $g$ 本身无关：

$$
\widehat{(\phi'(g))\xi} = Ad_{h^{-1}}(\hat{\xi})
$$

在 SO(3) 下，右乘常值旋转 $R$ 如下： $\phi(A) = AR$ ，且满足：

$$
\begin{aligned}
\widehat{D\phi_A(\omega)} &= Ad_{R^T}[\xi]_{\times}\\
&= [R^T\omega]_{\times}\\
\phi'(A)\omega &= R^T\omega\\
\phi'(A) &= R^T
\end{aligned}
$$

组合映射

组合为两个旋转变量相乘，设 G 为 n 维李群。映射 $\phi: G \times G \rightarrow G$ 定义为： $\phi(g, h) = gh$ 。下面进行映射的雅可比矩阵推导：

首先根据乘法的求导法则，有：

$$
D_{\phi(a, b)}(x, y) = D_1\phi_{(a, b)}x + D_2\phi_{(a,b)}y
$$

先对第一项微分进行推导，此时可以视 $h$ 为常值，根据前面的结论有：

$$
\widehat{D_1\phi_{(a, b)}x} = Ad_{b^{-1}}(\hat{x})
$$

对第二项进行推导，此时视 $g$ 为常值，根据前面的结论有：

$$
D_2\phi_{(a, b)}y = y
$$

同样，以 SO(3) 为例，设映射为: $\phi(A, B) = AB，Ad_{B^T}[\omega]_{\times} = [B^T\omega]_{\times}$ ，雅可比矩阵为：

$$
\phi'(A,B) =
\begin{bmatrix}
    B^T & I_3
\end{bmatrix}
$$

相对映射

相对映射如下：设 G 为 n 维李群。映射 $\phi: G \times G \rightarrow G$ 定义为： $\phi(g, h) = g^{-1}h$ 。下面进行映射的雅可比矩阵推导：

同样分为两部分：

$$
D_{\phi(a, b)}(x, y) = D_1\phi_{(a, b)}x + D_2\phi_{(a,b)}y
$$

先看第二项，可以视 $g^{-1}$ 为常值，因此为常值左乘映射，由之前的结论知道对 $h$ 的微分为： $D_2\phi_{(a, b)}y = y$

再看第一项：

设 $y = D_1\phi_{(a, b)}x$ ，由导数定义有：

$$
\begin{aligned}
    \phi(g, h)e^{\hat{y}} &= \phi(ge^{\hat{x}}, h)\\
    g^{-1}he^{\hat{y}} &= (ge^{\hat{x}})^{-1}h\\
    (e^{\hat{y}})^{-1}h^{-1}g &= h^{-1}ge^{\hat{x}}\\
    (e^{\hat{y}})^{-1} &= h^{-1}ge^{\hat{x}}g^{-1}h\\
    (e^{-\hat{y}}) &= (h^{-1}g)e^{\hat{x}}(h^{-1}g)^{-1}\\
    \hat{y} &= -Ad_{\phi(h, g)}\hat{x}
\end{aligned}
$$

同样，以 SO(3) 为例，设映射为: $\phi(A, B) = AB，Ad_{B^TA}[\omega]_{\times} = [B^TA\omega]_{\times}$ ，雅可比矩阵为：

$$
\phi'(A,B) =
\begin{bmatrix}
    -B^TA & I_3
\end{bmatrix}
$$

数值前推

接下来来看一下数值微分过程，考虑以下映射：

$$
\begin{aligned}
f(g)e^{\hat{y}} &= f(ge^{\hat{x}})\\
    e^{\hat{y}} &= f(g)^{-1}f(ge^{\hat{x}})\\
\end{aligned}
$$

进行李群到李代数的对数映射，找到对应的指数坐标：

$$
y(x) = \log[f(g)^{-1}f(ge^{\hat{x}})]
$$

关注 $x = 0$ ，且施加 $i$ 方向上的扰动，即 $e_i = [0,0,1,0,0]$ ，然后求导：

$$
\frac{\partial y(d)}{\partial d} \triangleq \lim_{d\rightarrow 0}\frac{y(d) - y(0)}{d} = \lim_{d\rightarrow 0}\frac{1}{d}\log[f(g)^{-1}f(ge^{\hat{de_i}})]
$$

指数映射

TODO

结论

以上关于不同函数/映射的推导应该能涵盖大部分 SLAM 中会涉及到的李群李代数的雅可比矩阵求解。注意：对第二部分李群到李群的映射的雅可比求解不能直接作为优化问题中的残差函数。例如：在对相对旋转求雅可比式，通常我们会将相对旋转约束转化为李代数的局部坐标（没有冗余自由度），然后采用函数的方式求导，部分例子可以看：我之前在一些常见的关于旋转的函数对旋转求导过程的推导推导的结果不同，后续需要再看一下。这篇博客里应用的技巧相当于在李群上施加右扰动求雅可比。

GTSAM