A Micro Lie Theory 论文理解

找到一篇 2018 年的论文 [1]，是 Quaternion kinematics for the error-state Kalman filter [2] 的作者 Joan Solà 写的。

论文的名字是 A micro Lie theory for state estimation in robotics。这篇论文介绍 Lie Theory，先中规中矩地介绍了 Lie Group, Tangent space, Lie Algebra。后面详细介绍了与 Lie Algebra 有关的导数计算。导数计算是我关注的重点。

以下正文中的公式引用使用两种形式：本文的公式，如(1)；原文 [1] 的公式，如(1*)。

本文有可能概念不清，仅作为我对这些概念的自我理解。

1. 概念介绍

进行简单的概念介绍。配合 [3] III.48 Lie Theory 理解这部分内容更好。

Group 是 set 与 operation 的组合，operation 作用于 set，并且满足 4 条性质，见公式 (1-4*)。

Lie Group 是 Group，并且是连续的。对于”连续“的理解可以参考 [3] Page 230。

The group SO(2) is not just a continuous group, but also a Lie group. Roughly speaking, this means that it is a group in which one can meaningfully define the concept of a smooth curve (that is, a curve that is not just continuous but differentiable as well). Given any two elements \(R_{\theta}\) and \(R_{\varphi}\) of SO(2), one can easily define a smooth path from \(R_{\theta}\) to \(R_{\varphi}\) by smoothly modifying \(\theta\) until it becomes \(\varphi\). (The most obvious such path would be given in parametric form by \(R_{(1-t)\theta + t\varphi}\), as t goes from 0 to 1.)

注意：

1. Group 的本质是集合，并且在这个集合上定义了一种运算，并且要求这个集合与运算一起满足一些条件。Group 并不是 Matrix，只是在大多数应用中，使用 Matrix 表达 Group，方便运算。
2. Group 可以是不连续，如在 SO(2) 中选择有限的 element，组成 Subgroup。subgroup 对应的集合是 {旋转 0，旋转 \(\pi/2\)，旋转 \(\pi\)，旋转 \(3\pi/2\)}，集合上的操作就是 rotation composition。（Subgroup 一定包含原 Group 中的 identity element。）

”连续“的概念中出现了”smooth curve“，为了应对这个概念，引出了 Smooth Manifold 的概念。[1] 中对 Smooth Manifold 的说明如下。也可以参考 [1] Figure 2。

On one hand, a differentiable or smooth manifold is a topological space that locally resembles linear space.

topological space 的定义见 [4] Page 20。简单说就是一个 set 与一个 collection。collection 是这个 set 的 subsets 组成的 set。并且要求这个 collection 满足3 个条件：空集与原 set 是这个 collection 的元素；collection 中任意有限个元素的交集在 collection 中；collection 中任意个元素的并集在 collection 中。

Smooth Manifold 在任意一点处可以建立一个小的 vector space，Smooth Manifold 任意一点对 curve 参数方程的参数 t 求导的结果在这点的 vector space 上。这个 vector space 被称作这一点处的 Tangent Space。

Lie Algebra 是 Lie Group 的 Smooth Manifold 在 Lie Group 的 Identity element 处的 tangent space，是一个 matrix space。如 SO(3) 对 t 的导数是一个 skew-symmetric matrix，这个 skew-symmetric matrix 就是 lie algebra。而角速度 \(\omega \in \mathbb{R}^3\) 并不是 lie algebra，skew-symmetric matrix 与 \(\mathbb{R}^3\) 同构（Homomorphism）。

即 Smooth Manifold 存在两个 space：一个 space 是 Lie Group 中元素组成的 space 称作 Manifold；一个 space 是 Tangent Space，是 vector space（对 scalar product 与 plus 闭合），用于在局部近似 Manifold

2. 导数

在本科的高等数学微积分中学到的导数定义如下。

\[\begin{align} f^{\prime}(x) = \lim_{\Delta x \to 0} {f(x + \Delta x) - f(x) \over \Delta x} \label{eq:R_derivative} \end{align}\]

\(f: \mathbb{R} \to \mathbb{R}\)，是一个定义域与值域都是 \(\mathbb{R}\) 的函数。\(\Delta x\) 的取值空间也是 \(\mathbb{R}\)。

但是如果 \(f\) 的定义域是 Lie Group，即 \(x\) 应当写作 \(\mathcal{X}\)，\(\mathcal{X}\) 是 Manifold 中的元素，对应的 \(\Delta x\) 写作 \(\bm{\tau}\) 是 Tangent Space 中的元素。也不能用 \(\mathbb{R}\) 中的 + 号，需要重新定义。Manifold 中的元素有 Group 定义中的操作 \(\circ\) 可以替换 + 号。但是，现在是 Manifold 与 Tangent Space 中元素的加号需要定义。

2.1. \(\oplus, \ominus\) 的定义

[1] 的 II. E (25-28*) 定义了在 Manifold 上的加法与减法，因为 Group 的定义中没有保证 Commutative Law（交换律）。所以加法与减法的定义分为左右两种（在 Group 定义的基础上满足交换律的 Group 叫做 Abelian Group）。

定义中的基本思想是使用 exponential map 将 tangent space 中的元素转换为 lie group 中的元素，即将 tangent space 中的元素（delta）转换为 Manifold 中的元素。使用 Group 定义中的 operation 将 delta apply 到 Manifold 的元素中去。

需要注意的是 \(\ominus\) 的定义，这个定义的核心是保证 tangent space 的 exponential map 在最左侧或者最右侧，不与 manifold 元素糅合在一起（这一点在 VIO 的 orientation 定义中非常重要）。

注意，在 (25-28*) 的定义中只有 \(T_{\epsilon}\mathcal{M}\) 是 lie algebra，见 lie algebra 的定义 (8*)。lie algebra 是特殊的 tangent space。

返回去看 \((\ref{eq:R_derivative})\) 的定义，可以将 \((\mathbb{R}, +)\) 看做是一个 group，这个 group 的 identity element 是 0，元素的逆就是其相反数，加法是有结合律的。\(\Delta x\) 是在 tangent space 上的元素，tangent space 与 manifold 都是 \(\mathbb{R}\)，所以可以用 Group 中的 operation + 法将 tangent space 中的元素 apply 到 manifold 中的元素。

之后的文章默认使用右 \(\oplus, \ominus\)。

2.2. Adjoint 定义

Adjiont 的定义是为了方便，方便将 manifold 中的元素 apply 到 tangent space 中的元素，将这个过程用 adjoint 表示。

区分好 adjoint 与 adjoint matrix。

在我之前的博客中，我说了 SO(3) 的 adjoint matrix 是它自身。即参考 [5] 的公式 (10)。

\[\begin{align} \mathbf{R}\bm{\phi}^{\wedge}\mathbf{R}^T = (\mathbf{R}\bm{\phi})^{\wedge} \end{align}\]

上式中等号右侧的 \(\mathbf{R}\) 应当写作 \(\mathbf{Ad}_\mathbf{R}\)。即 \(\mathbf{Ad}_\mathbf{R} = \mathbf{R}\)。

2.3. 导数定义

[1] II. G. 2) 与 3) 的公式 (41*)(44*) 定义了两个 Jacobian，分别是使用左右 \(\oplus, \ominus\) 定义的。这种 Jacobian 的使用方式见 (43*) 与 (45*)。

紧接着作者在 [1] II. G. 4) 提到了在某些应用中存在将 \(\oplus, \ominus\) 混用的情况。如，使用左 \(\oplus\) 与右 \(\ominus\)。

公式 (47*) 与 (48*) 第一次出现了分数线下的 \(D\) 左上角标。这个需要搞清楚，之后的公式 (59*), (60*) 也涉及到分数线下的 \(D\) 左上角标。

可以从 (49*) (50*) 推测分数线上下的左上角标的含义，但是这只是从导数的应用上推测。并没有对这个导数的含义又说明。分数线上的 \(D\) 左上角标表示使用的 \(\ominus\) 方式的左右方向，\(\epsilon\) 表示 left minus，\(\mathcal{X}, \mathcal{Y}\) 表示 right minus（\(\mathcal{X}, \mathcal{Y}\) 应当理解成 \(f(\mathcal{X})\)）。分数线下的 \(D\) 左上角标表示使用的 \(\oplus\) 的左右方向，\(\epsilon\) 表示 left plus，\(\mathcal{X}\) 表示 right plus（此处只有 \(\mathcal{X}\) 是变量）。所以可以知道公式 (41*) 的 \({{}^\mathcal{X} D f(\mathcal{X})\over D \mathcal{X}}\) 是 \({{}^\mathcal{X} D f(\mathcal{X})\over {}^\mathcal{X} D \mathcal{X}}\) 的简写，同理公式 (44)。

举个例子，\({{}^{\epsilon}D\mathcal{Y} \over {}^\mathcal{X}D\mathcal{X}}\) 的定义。

\[\begin{align} {{}^{\epsilon}D\mathcal{Y} \over {}^\mathcal{X}D\mathcal{X}} &= \lim_{{}^\mathcal{X}\bm{\tau} \to \bm{0}} {f(\mathcal{X} \oplus {}^\mathcal{X}\bm{\tau}) \ominus f(\mathcal{X})\over {}^\mathcal{X}\bm{\tau}} \\ &= \lim_{{}^\mathcal{X}\bm{\tau} \to \bm{0}} {\text{Log}[f(\mathcal{X}\circ\text{Exp}({}^\mathcal{X}\bm{\tau}))\circ f(\mathcal{X})^{-1}] \over {}^\mathcal{X}\bm{\tau}} \end{align}\]

证明 (47*) (48*)。按照 (49*) (50*) 写出公式中出现的 6 个 Jacobians 的”含义“，再使用 \(f(\mathcal{X} \oplus {}^\mathcal{X}\bm{\tau}) = f({}^\epsilon\bm{\tau} \oplus\mathcal{X})\) 的方式去证明。令 \(\mathcal{Y} = f(\mathcal{X})\)。

\[\begin{align} f(\mathcal{X} \oplus {}^\mathcal{X}\bm{\tau}) &\longrightarrow_{{}^\mathcal{X}\bm{\tau}\to\bm{0}} {{}^{\epsilon}D\mathcal{Y} \over {}^\mathcal{X}D\mathcal{X}} {}^\mathcal{X}\bm{\tau} \oplus f(\mathcal{X}) \\ f({}^\epsilon\bm{\tau} \oplus\mathcal{X}) &\longrightarrow_{{}^\epsilon\bm{\tau}\to\bm{0}} {{}^{\epsilon}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} {}^\epsilon\bm{\tau} \oplus f(\mathcal{X}) \\ f(\mathcal{X} \oplus {}^\mathcal{X}\bm{\tau}) &\longrightarrow_{{}^\mathcal{X}\bm{\tau}\to\bm{0}} f(\mathcal{X}) \oplus {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\mathcal{X}D\mathcal{X}} {}^\mathcal{X}\bm{\tau} \\ f({}^\epsilon\bm{\tau} \oplus\mathcal{X}) &\longrightarrow_{{}^\mathcal{X}\bm{\tau}\to\bm{0}} f(\mathcal{X}) \oplus {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} {}^\epsilon\bm{\tau} \\ f(\mathcal{X} \oplus {}^\mathcal{X}\bm{\tau}) &\longrightarrow_{{}^\mathcal{X}\bm{\tau}\to\bm{0}} f(\mathcal{X}) \oplus {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\mathcal{X} D\mathcal{X}} {}^\mathcal{X}\bm{\tau} \end{align}\]

(47*) 第 1 个等号。

\[\begin{align} {{}^{\epsilon}D\mathcal{Y} \over {}^\mathcal{X}D\mathcal{X}} {}^\mathcal{X}\bm{\tau} \oplus f(\mathcal{X}) &= {{}^{\epsilon}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} {}^\epsilon\bm{\tau} \oplus f(\mathcal{X}) \\ {{}^{\epsilon}D\mathcal{Y} \over {}^\mathcal{X}D\mathcal{X}} {}^\mathcal{X}\bm{\tau} &= {{}^{\epsilon}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} {}^\epsilon\bm{\tau} \\ {{}^{\epsilon}D\mathcal{Y} \over {}^\mathcal{X}D\mathcal{X}} {}^\mathcal{X}\bm{\tau} &=^{(30^*)} {{}^{\epsilon}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} \mathbf{Ad}_{\mathcal{X}} {}^\mathcal{X}\bm{\tau} \\ {{}^{\epsilon}D\mathcal{Y} \over {}^\mathcal{X}D\mathcal{X}} &= {{}^{\epsilon}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} \mathbf{Ad}_{\mathcal{X}} \\ \end{align}\]

(47*) 第 2 个等号。

\[\begin{align} {{}^{\epsilon}D\mathcal{Y} \over {}^\mathcal{X}D\mathcal{X}} {}^\mathcal{X}\bm{\tau} \oplus f(\mathcal{X}) &= f(\mathcal{X}) \oplus {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\mathcal{X}D\mathcal{X}} {}^\mathcal{X}\bm{\tau} \\ f(\mathcal{X}) \oplus {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\mathcal{X}D\mathcal{X}} {}^\mathcal{X}\bm{\tau} &= {{}^{\epsilon}D\mathcal{Y} \over {}^\mathcal{X}D\mathcal{X}} {}^\mathcal{X}\bm{\tau} \oplus f(\mathcal{X}) \\ {{}^{\epsilon}D\mathcal{Y} \over {}^\mathcal{X}D\mathcal{X}} {}^\mathcal{X}\bm{\tau} &=^{(32^*)} \mathbf{Ad}_{f(\mathcal{X})} {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\mathcal{X}D\mathcal{X}} {}^\mathcal{X}\bm{\tau} \\ {{}^{\epsilon}D\mathcal{Y} \over {}^\mathcal{X}D\mathcal{X}} &= \mathbf{Ad}_{f(\mathcal{X})} {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\mathcal{X}D\mathcal{X}} \\ &= \mathbf{Ad}_{\mathcal{Y}} {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\mathcal{X}D\mathcal{X}} \end{align}\]

(48*) 第 1 个等号。

\[\begin{align} f(\mathcal{X}) \oplus {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} {}^\epsilon\bm{\tau} &= f(\mathcal{X}) \oplus {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\mathcal{X} D\mathcal{X}} {}^\mathcal{X}\bm{\tau} \\ {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} {}^\epsilon\bm{\tau} &= {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\mathcal{X} D\mathcal{X}} {}^\mathcal{X}\bm{\tau} \\ {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} \mathbf{Ad}_{\mathcal{X}} {}^\mathcal{X}\bm{\tau} &=^{(30^*)} {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\mathcal{X} D\mathcal{X}} {}^\mathcal{X}\bm{\tau} \\ {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} \mathbf{Ad}_{\mathcal{X}} &= {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\mathcal{X} D\mathcal{X}} \\ {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} &= {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\mathcal{X} D\mathcal{X}} \mathbf{Ad}_{\mathcal{X}}^{-1} \end{align}\]

(48*) 第 2 个等号。

\[\begin{align} f(\mathcal{X}) \oplus {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} {}^\epsilon\bm{\tau} &= {{}^{\epsilon}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} {}^\epsilon\bm{\tau} \oplus f(\mathcal{X}) \\ {{}^{\epsilon}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} {}^\epsilon\bm{\tau} &=^{(32^*)} \mathbf{Ad}_{f(\mathcal{X})} {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} {}^\epsilon\bm{\tau} \\ {{}^{\epsilon}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} &= \mathbf{Ad}_{f(\mathcal{X})} {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} \\ {{}^{\epsilon}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} &= \mathbf{Ad}_{\mathcal{Y}} {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} \\ {{}^{\mathcal{Y}}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} &= \mathbf{Ad}_{\mathcal{Y}}^{-1} {{}^{\epsilon}D\mathcal{Y} \over {}^\epsilon D\mathcal{X}} \end{align}\]

2.4. 链式法则

在对 Lie Theory 没有清晰了解之前，我尝试使用自己所认为的 naive 链式法则计算一些导数，发现结果与直接使用左右扰动计算出来的导数不一致。

[1] III. A 的 (59*) (60*) 给出了链式法则，公式下面有证明 (59*) 的第 1 个等号。作为练习，我证明 (59*) 的第 2 个等号与 (60*)。

(59*) 第 1 个等号。

\[\begin{align} g(f({}^\epsilon\bm{\tau} \oplus \mathcal{X})) &\longrightarrow_{{}^\epsilon\bm{\tau}\to\bm{0}} g({{}^\epsilon D \mathcal{Y} \over {}^\epsilon D \mathcal{X}} {}^\epsilon\bm{\tau} \oplus f(\mathcal{X})) \\ &\longrightarrow_{{}^\epsilon\bm{\tau}\to\bm{0}} g(f(\mathcal{X})) \oplus {{}^\mathcal{Z} D \mathcal{Z}\over {}^\epsilon D \mathcal{Y}} ({{}^\epsilon D \mathcal{Y} \over {}^\epsilon D \mathcal{X}} {}^\epsilon\bm{\tau}) \end{align}\]

[1] 的原文在 (59*) 上说了一句 we need to chain also the reference frames。从上面这个公式看，第二行扩展出来的 \({{}^\mathcal{Z} D \mathcal{Z}\over {}^\epsilon D \mathcal{Y}}\) 的分数线下 \(D\) 左上角标 \(\epsilon\) 是固定，不能是 \(\mathcal{Y}\)，因为第一行右侧出现的 \(\oplus\) 是一个左 \(\oplus\)。而第一行右侧的左 \(\oplus\) 是由第一行左侧的左 \(\oplus\) 决定的，反应到 Jacobian 符号上就是 chain rule。这就是这一句话的含义。

(60*) 证明。

\[\begin{align} g(f(\mathcal{X} \oplus {}^\mathcal{X}\bm{\tau})) &\longrightarrow_{{}^\mathcal{X}\bm{\tau}\to\bm{0}} {{{}^\epsilon D \mathcal{Z}\over {}^\mathcal{X} D \mathcal{X}}} {}^\mathcal{X}\bm{\tau} \oplus g(f(\mathcal{X})) \\ g(f(\mathcal{X} \oplus {}^\mathcal{X}\bm{\tau})) &\longrightarrow_{{}^\mathcal{X}\bm{\tau}\to\bm{0}} g(f(\mathcal{X}) \oplus {{}^\mathcal{Y} D \mathcal{Y} \over {}^\mathcal{X} D \mathcal{X}} {}^\mathcal{X}\bm{\tau}) \\ &\longrightarrow_{{}^\mathcal{X}\bm{\tau}\to\bm{0}} {{}^\epsilon D \mathcal{Z} \over {}^\mathcal{Y} D \mathcal{Y}} ({{}^\mathcal{Y} D \mathcal{Y} \over {}^\mathcal{X} D \mathcal{X}} {}^\mathcal{X}\bm{\tau}) \oplus g(f(\mathcal{X}) \\ g(f(\mathcal{X} \oplus {}^\mathcal{X}\bm{\tau})) &\longrightarrow_{{}^\mathcal{X}\bm{\tau}\to\bm{0}} g({{}^\epsilon D \mathcal{Y} \over {}^\mathcal{X} D \mathcal{X}} {}^\mathcal{X}\bm{\tau} \oplus f(\mathcal{X})) \\ &\longrightarrow_{{}^\mathcal{X}\bm{\tau}\to\bm{0}} {{}^\epsilon D \mathcal{Z} \over {}^\epsilon D \mathcal{Y}}({{}^\epsilon D \mathcal{Y} \over {}^\mathcal{X} D \mathcal{X}} {}^\mathcal{X}\bm{\tau}) \oplus g(f(\mathcal{X})) \end{align}\]

2.5. 一些常见的雅克比

[1] III. B 作者开始介绍一些常见的雅克比结论。其中的 3) 介绍了左右雅克比，喜闻乐见。这是 State Estimation for Robotics 直接给出结论的东西，见 [6] 公式 (7.75)。

这一部分非常有意思。所以给出所有公式的证明。

2.5.1. Inverse

\[\begin{align} \mathbf{J}^{\mathcal{X}^{-1}}_{\mathcal{X}} &= {{}^{\mathcal{X}} D \mathcal{X}^{-1} \over D \mathcal{X}} = {{}^{\mathcal{X}} D \mathcal{X}^{-1} \over {}^{\mathcal{X}} D \mathcal{X}} \\ &=^{(41b^*)} \lim_{\bm{\tau}\to\bm{0}} {\text{Log}[f(\mathcal{X})^{-1} \circ f(\mathcal{X} \circ \text{Exp}(\tau))] \over \bm{\tau}} \\ &= \lim_{\bm{\tau}\to\bm{0}} {\text{Log}[(\mathcal{X}^{-1})^{-1} \circ (\mathcal{X} \circ \text{Exp}(\tau))^{-1}] \over \bm{\tau}} \\ &= \lim_{\bm{\tau}\to\bm{0}} {\text{Log}[\mathcal{X} \text{Exp}(-\tau) \mathcal{X}^{-1}] \over \bm{\tau}} \\ &= \lim_{\bm{\tau}\to\bm{0}} {\text{Log}[\mathcal{X} \text{exp}((-\tau)^{\wedge}) \mathcal{X}^{-1}] \over \bm{\tau}} \\ &= \lim_{\bm{\tau}\to\bm{0}} {\text{log}[ \text{exp}(\mathcal{X}(-\tau)^{\wedge}\mathcal{X}^{-1})]^{\vee} \over \bm{\tau}} \\ &= \lim_{\bm{\tau}\to\bm{0}} {-[(\mathcal{X}\tau^{\wedge}\mathcal{X}^{-1})]^{\vee} \over \bm{\tau}} \\ &=^{(31^*)} \lim_{\bm{\tau}\to\bm{0}} {-\mathbf{Ad}_{\mathcal{X}} \bm{\tau} \over \bm{\tau}} \\ &= -\mathbf{Ad}_{\mathcal{X}} \end{align}\]

2.5.2. Composition

公式 (65*) (66*) 的 SO(3) 情况作者在 [2] 中有推导过，见 [2] 的 4.3.5 Jacobians of rotation composition，在 [2] 中使用偏导 \(\partial\) 的形式写是不合适是与 [1] 相违背的（见 (41c*)），SO(3) 是不能求对 SO(3) 的导数的，只能求 so(3) 对应的 \(\mathbb{R}^3\) 对 so(3) 对应的 \(\mathbb{R}^3\) 的导数。

\[\begin{align} \mathbf{J}^{\mathcal{X}\circ\mathcal{Y}}_{\mathcal{X}} &= {{}^{\mathcal{X}} D \mathcal{X} \circ \mathcal{Y} \over D \mathcal{X}} \\ &= \lim_{\bm{\tau}\to\bm{0}} {\text{Log}[(\mathcal{X}\mathcal{Y})^{-1}(\mathcal{X}\text{Exp}(\bm{\tau})\mathcal{Y})] \over \bm{\tau}} \\ &= \lim_{\bm{\tau}\to\bm{0}} {\text{Log}[\mathcal{Y}^{-1}\mathcal{X}^{-1}\mathcal{X}\text{Exp}(\bm{\tau})\mathcal{Y}] \over \bm{\tau}} \\ &= \lim_{\bm{\tau}\to\bm{0}} {\text{Log}[\mathcal{Y}^{-1}\text{Exp}(\bm{\tau})\mathcal{Y}] \over \bm{\tau}} \\ &= \lim_{\bm{\tau}\to\bm{0}} {\text{Log}[\text{Exp}(\mathbf{Ad}_{\mathcal{Y}^{-1}}\bm{\tau})] \over \bm{\tau}} \\ &= \mathbf{Ad}_{\mathcal{Y}^{-1}} = \mathbf{Ad}_{\mathcal{Y}}^{-1} \end{align}\]

\[\begin{align} \mathbf{J}^{\mathcal{X}\circ\mathcal{Y}}_{\mathcal{Y}} &= {{}^{\mathcal{X}} D \mathcal{X} \circ \mathcal{Y} \over D \mathcal{Y}} \\ &= \lim_{\bm{\tau}\to\bm{0}} {\text{Log}[(\mathcal{X}\mathcal{Y})^{-1}(\mathcal{X}\mathcal{Y}\text{Exp}(\bm{\tau}))] \over \bm{\tau}} \\ &= \lim_{\bm{\tau}\to\bm{0}} {\text{Log}[\text{Exp}(\bm{\tau})] \over \bm{\tau}} \\ &= \mathbf{I} \end{align}\]

2.5.3. Jacobians of \(\mathcal{M}\)

定义左右雅克比，(67*) (71*)。

证明 (68*)。

\[\begin{align} \mathbf{J}_r(\bm{\tau}) &= {{}^{\bm{\tau}} D \text{Exp}(\bm{\tau}) \over D \bm{\tau}} \\ \text{Exp}(\bm{\tau} + \delta \bm{\tau})&=^{(43^*)} \text{Exp}(\bm{\tau}) \oplus {{}^{\bm{\tau}} D \text{Exp}(\bm{\tau}) \over D \bm{\tau}} \delta\bm{\tau} \\ &= \text{Exp}(\bm{\tau}) \text{Exp}({{}^{\bm{\tau}} D \text{Exp}(\bm{\tau}) \over D \bm{\tau}} \delta\bm{\tau}) \\ &= \text{Exp}(\bm{\tau}) \text{Exp}(\mathbf{J}_r(\bm{\tau}) \delta\bm{\tau}) \end{align}\]

同理可证 (72*)。

证明(75*)，(68*) (72*) 的左侧相等，分别取右侧，令它们相等。

\[\begin{align} \text{Exp}(\bm{\tau}) \text{Exp}(\mathbf{J}_r(\bm{\tau}) \delta\bm{\tau}) &= \text{Exp}(\mathbf{J}_l(\bm{\tau}) \delta\bm{\tau}) \text{Exp}(\bm{\tau}) \\ \text{Exp}(\bm{\tau}) \oplus (\mathbf{J}_r(\bm{\tau}) \delta\bm{\tau}) &= (\mathbf{J}_l(\bm{\tau}) \delta\bm{\tau}) \oplus \text{Exp}(\bm{\tau}) \\ \mathbf{J}_l(\bm{\tau}) \delta\bm{\tau} &=^{(32^*)} \mathbf{Ad}_{\text{Exp}(\bm{\tau})} \mathbf{J}_r(\bm{\tau}) \delta\bm{\tau} \\ \mathbf{J}_l(\bm{\tau}) &= \mathbf{Ad}_{\text{Exp}(\bm{\tau})} \mathbf{J}_r(\bm{\tau}) \\ \mathbf{Ad}_{\text{Exp}(\bm{\tau})} &= \mathbf{J}_l(\bm{\tau}) \mathbf{J}_r^{-1}(\bm{\tau}) \end{align}\]

(76*) 的过程，(76*) 的第 2 行第 2 个等号是比较难的。

\[\begin{align} \mathbf{J}_r(-\bm{\tau}) &= \mathbf{J}_{-\bm{\tau}}^{\text{Exp}(-\bm{\tau})} \\ &= \mathbf{J}_{\bm{\tau}}^{\text{Exp}(-\bm{\tau})} \mathbf{J}_{-\bm{\tau}}^{\bm{\tau}} \\ &= \mathbf{J}_{\bm{\tau}}^{\text{Exp}(-\bm{\tau})} (-\mathbf{I}) \\ &= -\mathbf{J}_{\text{Exp}(\bm{\tau})}^{\text{Exp}(\bm{\tau})^{-1}} \mathbf{J}_{\bm{\tau}}^{\text{Exp}(\bm{\tau})} \\ &=^{(62^*)} -(-\mathbf{Ad}_{\text{Exp}(\bm{\tau})}) \mathbf{J}_{\bm{\tau}}^{\text{Exp}(\bm{\tau})} \\ &= \mathbf{Ad}_{\text{Exp}(\bm{\tau})} \mathbf{J}_{\bm{\tau}}^{\text{Exp}(\bm{\tau})} \\ &=^{(67^*)} \mathbf{Ad}_{\text{Exp}(\bm{\tau})} \mathbf{J}_r(\bm{\tau}) \\ &=^{(75^*)} \mathbf{J}_l(\bm{\tau}) \end{align}\]

2.5.4. Group action

这就是最常见的 \(\mathbf{R}\mathbf{p}\) 对 \(\mathbf{R}\) 的 Tangent Space 对应的 \(\mathbf{\phi} \in \mathbb{R}^3\) 的导数。

2.6. 使用 chain rule

[1] III. C 部分。

公式 (79*) 的证明，注意 \(\text{Log}(\mathcal{X})\in\mathbb{R}^m\)，所以 \(\ominus\) 不能使用 manifold 上的定义。

\[\begin{align} \mathbf{J}^{\text{Log}(\mathcal{X})}_\mathcal{X} &= {{}^{\mathcal{X}} D \text{Log}(\mathcal{X}) \over D \mathcal{X}} \\ &=^{(41a^*)} \lim_{\delta\bm{\tau}\to\bm{0}} {\text{Log}(\mathcal{X}\oplus\delta\bm{\tau}) \ominus \text{Log}(\mathcal{X}) \over\delta \bm{\tau}} \\ &= \lim_{\delta\bm{\tau}\to\bm{0}} {\text{Log}(\mathcal{X}\text{Exp}(\delta\bm{\tau})) - \text{Log}(\mathcal{X}) \over \delta\bm{\tau}} \\ &= \lim_{\delta\bm{\tau}\to\bm{0}} {\text{Log}(\text{Exp}(\bm{\tau})\text{Exp}(\delta\bm{\tau})) - \text{Log}(\text{Exp}(\bm{\tau})) \over \delta\bm{\tau}} \\ &=^{(70^*)} \lim_{\delta\bm{\tau}\to\bm{0}} {\bm{\tau} + \mathbf{J}_r^{-1}(\bm{\tau})\delta\bm{\tau} - \bm{\tau} \over \delta\bm{\tau}} \\ &= \mathbf{J}_r^{-1}(\bm{\tau}) \end{align}\]

证明公式 (80*)。

\[\begin{align} \mathbf{J}_\mathcal{X}^{\mathcal{X}\oplus\bm{\tau}} &= \mathbf{J}_\mathcal{X}^{\mathcal{X}\circ\text{Exp}(\bm{\tau})} \\ &=^{(65^*)} \mathbf{Ad}_{\text{Exp}(\bm{\tau})}^{-1} \\ \mathbf{J}_\mathcal{X}^{\mathcal{X}\oplus\bm{\tau}} &= {{}^{\mathcal{X}}D \mathcal{X}\oplus\bm{\tau} \over {}^{\mathcal{X}}D\mathcal{X}} \\ &=^{(41a^*)} \lim_{\delta\bm{\tau}\to\bm{0}}{(\mathcal{X}\oplus\delta\bm{\tau}\oplus\bm{\tau}) \ominus (\mathcal{X}\oplus\bm{\tau}) \over \delta \bm{\tau}} \\ &= \lim_{\delta\bm{\tau}\to\bm{0}}{\text{Log}[(\mathcal{X}\oplus\bm{\tau})^{-1}\circ(\mathcal{X}\oplus\delta\bm{\tau}\oplus\bm{\tau})] \over \delta \bm{\tau}} \\ &= \lim_{\delta\bm{\tau}\to\bm{0}}{\text{Log}[(\mathcal{X}\text{Exp}(\bm{\tau}))^{-1}(\mathcal{X}\text{Exp}(\delta\bm{\tau})\text{Exp}(\bm{\tau}))] \over \delta \bm{\tau}} \\ &= \lim_{\delta\bm{\tau}\to\bm{0}}{\text{Log}[\text{Exp}(\bm{\tau})^{-1}\mathcal{X}^{-1}\mathcal{X}\text{Exp}(\delta\bm{\tau})\text{Exp}(\bm{\tau})] \over \delta \bm{\tau}} \\ &= \lim_{\delta\bm{\tau}\to\bm{0}}{\text{Log}[\text{Exp}(\bm{\tau})^{-1}\text{Exp}(\delta\bm{\tau})\text{Exp}(\bm{\tau})] \over \delta \bm{\tau}} \\ &= \lim_{\delta\bm{\tau}\to\bm{0}}{\text{Log}[\text{Exp}(\mathbf{Ad}_{\text{Exp}(\bm{\tau})^{-1}}\delta\bm{\tau})] \over \delta \bm{\tau}} \\ &= \mathbf{Ad}_{\text{Exp}(\bm{\tau})^{-1}} = \mathbf{Ad}_{\text{Exp}(\bm{\tau})}^{-1} \end{align}\]

证明公式 (81*)。

\[\begin{align} \mathbf{J}_\bm{\tau}^{\mathcal{X}\oplus\bm{\tau}} &= \mathbf{J}_\bm{\tau}^{\mathcal{X}\circ\text{Exp}(\bm{\tau})} \\ &= \mathbf{J}_{\text{Exp}(\bm{\tau})}^{\mathcal{X}\circ\text{Exp}(\bm{\tau})} \mathbf{J}_\bm{\tau}^{\text{Exp}(\bm{\tau})} \\ &=^{(66^*)} \mathbf{I} \mathbf{J}_\bm{\tau}^{\text{Exp}(\bm{\tau})} \\ &=^{(67^*)} \mathbf{J}_r(\bm{\tau}) \\ \mathbf{J}_\bm{\tau}^{\mathcal{X}\oplus\bm{\tau}} &= {{}^{\bm{\tau}}D \mathcal{X}\oplus\bm{\tau} \over {}^{\bm{\tau}}D\bm{\tau}} \\ &=^{(41a^*)} \lim_{\delta\bm{\tau}\to\bm{0}}{(\mathcal{X}\oplus(\bm{\tau} + \delta\bm{\tau})) \ominus (\mathcal{X}\oplus\bm{\tau}) \over \delta \bm{\tau}} \\ &= \lim_{\delta\bm{\tau}\to\bm{0}}{\text{Log}[(\mathcal{X}\oplus\bm{\tau})^{-1}\circ(\mathcal{X}\oplus(\bm{\tau} + \delta\bm{\tau}))] \over \delta \bm{\tau}} \\ &= \lim_{\delta\bm{\tau}\to\bm{0}}{\text{Log}[(\mathcal{X}\text{Exp}(\bm{\tau}))^{-1}(\mathcal{X}\text{Exp}(\bm{\tau} + \delta\bm{\tau}))] \over \delta \bm{\tau}} \\ &= \lim_{\delta\bm{\tau}\to\bm{0}}{\text{Log}[\text{Exp}(\bm{\tau})^{-1}\text{Exp}(\bm{\tau} + \delta\bm{\tau})] \over \delta \bm{\tau}} \\ &=^{(68^*)} \lim_{\delta\bm{\tau}\to\bm{0}}{\text{Log}[\text{Exp}(\mathbf{J}_r(\bm{\tau})\delta\bm{\tau})] \over \delta \bm{\tau}} \\ &= \mathbf{J}_r(\bm{\tau}) \end{align}\]

Reference

[1] Sola, Joan, Jeremie Deray, and Dinesh Atchuthan. "A micro Lie theory for state estimation in robotics." arXiv preprint arXiv:1812.01537 (2018).

[2] Sola, Joan. "Quaternion kinematics for the error-state Kalman filter." arXiv preprint arXiv:1711.02508 (2017).

[3] Gowers, Timothy, June Barrow-Green, and Imre Leader, eds. The Princeton companion to mathematics. Princeton University Press, 2008.

[4] Lee, John. Introduction to topological manifolds. Vol. 202. Springer Science & Business Media, 2010.

[5] Forster, Christian, et al. "IMU preintegration on manifold for efficient visual-inertial maximum-a-posteriori estimation." Georgia Institute of Technology, 2015.

[6] Barfoot, Timothy D. State estimation for robotics. Cambridge University Press, 2017.