目标：

• 李群、李代数概念
• SO(3)，SE(3)与对应李代数表示方式
• BCH近似的意义
• 李代数上的扰动模型
• 运用Spphus对李代数运算

基础

引入

旋转矩阵作为优化变量所引入的额外约束带来求解困难

通过李群-李代数转换关系，把位姿估计变成无约束的优化问题，简化求解

定义
$$
\begin{gathered}
S O(3)=\left{\boldsymbol{R} \in \mathbb{R}^{3 \times 3} \mid \boldsymbol{R} \boldsymbol{R}^{T}=\boldsymbol{I}, \operatorname{det}(\boldsymbol{R})=1\right} \
S E(3)=\left{\boldsymbol{T}=\left[\begin{array}{cc}
\boldsymbol{R} & \boldsymbol{t} \
\mathbf{0}^{T} & 1
\end{array}\right] \in \mathbb{R}^{4 \times 4} \mid \boldsymbol{R} \in S O(3), \boldsymbol{t} \in \mathbb{R}^{3}\right}
\end{gathered}
$$
三维旋转矩阵 $\rightarrow$ 特殊正交群 $S O(3)$
变换矩阵 $\rightarrow$ 特殊欧式群 $S E(3)$
特点
$$
\begin{gathered}
\boldsymbol{R}{1}+\boldsymbol{R}{2} \notin S O(3) \
\boldsymbol{R}{1} \boldsymbol{R}{2} \in S O(3), \quad \boldsymbol{T}{1} \boldsymbol{T}{2} \in S E(3)
\end{gathered}
$$
对加法不封闭
对乘法封闭 $\rightarrow$ 旋转/变换的复合

群

定义

群是一种集合加上一种运算的代数结构, $G=(A, \cdot)$

条件
封闭性: $\forall a_{1}, a_{2} \in A, \quad a_{1} \cdot a_{2} \in A$.
结合律: $\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)$.
么元: $\exists a_{0} \in A, \quad$ s.t. $\quad \forall a \in A, \quad a_{0} \cdot a=a \cdot a_{0}=a$.
逆: $\quad \forall a \in A, \quad \exists a^{-1} \in A, \quad$ s.t. $\quad a \cdot a^{-1}=a_{0} .$

• 封闭性→变换的复合
• 结合律→变换复合的顺序固定下, 计算先后无关
• 么元→单位元(Identity Element)
• 逆→群中的矩阵元素为可逆

矩阵中常见群

• 一般线性群 $G L(n)$ 指 $n \times n$ 的可逆矩阵, 它们对矩阵乘法成群
• 特殊正交群 $S O(n)$ 也就是所谓的秩转矩阵群, 其中 $S O(2)$ 和 $S O(3)$ 最为常见
• 特殊欧氏群 $S E(n) \quad$ 也就是前面提到的 $n$ 维欧氏变换, 如 $S E(2)$ 和 $S E(3)$
  李群
  定义：具有连续(光滑)性质的群
  反例：整数群(离散)矩阵中常见群

李群
定义：具有连续(光滑)性质的群
反例：整数群(离散)
李代数的引出
反对称矩阵的推导
任意旋转矩阵 $\boldsymbol{R}$ 满足:
$$
\boldsymbol{R} \boldsymbol{R}^{T}=\boldsymbol{I}
$$
$\boldsymbol{R}$ 作为某个相机的旋转为时间的函数: $\boldsymbol{R}(t)$ :
$$
\boldsymbol{R}(t) \boldsymbol{R}(t)^{T}=\boldsymbol{I}
$$
对时间求导, 得到:
$$
\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}
$$
楔形符号的运用
前述^^符号: 向量 $a \stackrel{\bigwedge}{\longleftrightarrow}$ 反对称矩阵 $\boldsymbol{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], \quad \boldsymbol{A}^{\vee}=\boldsymbol{a}
$$
二者结合
$$
\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}=\boldsymbol{\phi}(t)^{\wedge}
$$
旋转矩阵求导的性质
$$
\dot{\boldsymbol{R}}(t)=\phi(t)^{\wedge} \boldsymbol{R}(t)=\left[\begin{array}{ccc}
0 & -\phi_{3} & \phi_{2} \
\phi_{3} & 0 & -\phi_{1} \
-\phi_{2} & \phi_{1} & 0
\end{array}\right] \boldsymbol{R}(t)
$$
即: 对旋转矩阵求一次导数, 只需左乘一个 $\phi^{\wedge}(t)$ 矩阵即可
把 $\boldsymbol{R}(t)$ 在O附近一阶泰勒展开
$$
\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}
$$
反映了 $\boldsymbol{R}$ 的导数性质, 故称它在 $S O(3)$ 原点附近的正切空间上, 同时在 $t_{0}$ 附近, 设 $\phi$ 保持为常数 $\phi\left(t_{0}\right)=\phi_{0}$
$$
\dot{\boldsymbol{R}}(t)=\boldsymbol{\phi}\left(t_{0}\right)^{\wedge} \boldsymbol{R}(t)=\boldsymbol{\phi}{0}^{\wedge} \boldsymbol{R}(t)
$$
关于 $\boldsymbol{R}$ 的微分方程, 初始值 $\boldsymbol{R}(0)=\boldsymbol{I}$, 解之
$$
\boldsymbol{R}(t)=\exp \left(\phi{0}^{\wedge} t\right) .
$$
主要结论
旋转矩阵为反对称矩阵
$$
\dot{\boldsymbol{R}}(t) \boldsymbol{R}(t)^{T}=\phi(t)^{\wedge}
$$
$f\left(\boldsymbol{R}, \phi_{0}\right)$ 即旋转矩阵与另一个反对称矩阵的指数关系
$$
\boldsymbol{R}(t)=\exp \left(\boldsymbol{\phi}{0} t\right) .
$$
李代数的定义
概述
李代数描述了李群的局部性质
定义
李代数由一个集合 $\mathbb{V}$, 一个数域 $\mathbb{F}$ 和一个二元运算 $[,$, , 组成。如果它们满足以下几条性 质，称 $\left(\mathbb{V}, \mathbb{F},[,\right.$, , 为一个李代数，记作 $\mathfrak{g}{\text {。 }}$

1. 封闭性 $\forall \boldsymbol{X}, \boldsymbol{Y} \in \mathbb{V},[\boldsymbol{X}, \boldsymbol{Y}] \in \mathbb{V}$.
2. 双线性 $\forall \boldsymbol{X}, \boldsymbol{Y}, \boldsymbol{Z} \in \mathbb{V}, a, b \in \mathbb{F}$ ，有:
   $[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}]$
3. 自反性 $\forall \boldsymbol{X} \in \mathbb{V},[\boldsymbol{X}, \boldsymbol{X}]=\mathbf{0}$.
4. 雅可比等价 $\quad \forall \boldsymbol{X}, \boldsymbol{Y}, \boldsymbol{Z} \in \mathbb{V},[\boldsymbol{X},[\boldsymbol{Y}, \boldsymbol{Z}]]+[\boldsymbol{Z},[\boldsymbol{Y}, \boldsymbol{X}]]+[\boldsymbol{Y},[\boldsymbol{Z}, \boldsymbol{X}]]=\mathbf{0}$.

其中二元运算被称为李括号。
相比于群中的较为简单的二元运算, 李括号表达了两个元素的差异。它不要求结合律, 而要 求元素和自己做李括号之后为零的性质。
eg三维向量 $\mathbb{R}^{3}$ 上定义的叉积 $\times$ 是一种李括号, 因此 $\mathfrak{g}=\left(\mathbb{R}^{3}, \mathbb{R}, \times\right)$ 构成了一个李代数。

李代数 $\mathfrak{s o}(3)$
引入
$S O(3)$ 对应的李代数是定义在 $\mathbb{R}^{3}$ 上的向量 $\phi$, 每个 $\phi$ 都可以生成一个反对称矩阵:
$$
\boldsymbol{\Phi}=\boldsymbol{\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}
$$
在此定义下, 两个向量 $\phi_{1}, \phi_{2}$ 的李括号为:
$$
\left[\boldsymbol{\phi}{1}, \boldsymbol{\phi}{2}\right]=\left(\boldsymbol{\Phi}{1} \boldsymbol{\Phi}{2}-\boldsymbol{\Phi}{2} \boldsymbol{\Phi}{1}\right)^{\vee}
$$
定义: 三维向量的集合
$$
\mathfrak{s o}(3)=\left{\phi \in \mathbb{R}^{3}, \Phi=\phi^{\wedge} \in \mathbb{R}^{3 \times 3}\right} .
$$
$\mathfrak{s o}(3)$ 是一个由三维向量组成的集合, 每个向量对应到一个反对称矩阵, 可以表达旋转矩阵的导数。 它与 $S O(3)$ 的关系由指数映射给定.

李代数 $\mathfrak{s e}(3)$
定义: 六维向量的集合(平移+旋转)
$$
\mathfrak{s e}(3)=\left{\boldsymbol{\xi}=\left[\begin{array}{l}
\rho \
\phi
\end{array}\right] \in \mathbb{R}^{6}, \boldsymbol{\rho} \in \mathbb{R}^{3}, \phi \in \mathfrak{s o}(3), \boldsymbol{\xi}^{\wedge}=\left[\begin{array}{cc}
\boldsymbol{\phi}^{\wedge} & \rho \
\mathbf{0}^{T} & 0
\end{array}\right] \in \mathbb{R}^{4 \times 4}\right}
$$
每个 $\mathfrak{s e}(3)$ 元素记作 $\boldsymbol{\xi}$, 六维向量。前三维为平移, 记作 $\rho$; 后三维为 旋转, 记作 $\phi$, 实质上是 $\mathfrak{s o}(3)$ 元素
楔形符号的含义扩展
同时拓展了 $\wedge$ 符号的含义。在 $\mathfrak{s e}(3)$ 中, 同 样使用 $\wedge$ 符号, 将一个六维向量转换成四维矩阵, 但 这里不再表示反对称：
$$
\boldsymbol{\xi}^{\wedge}=\left[\begin{array}{ll}
\boldsymbol{\phi}^{\wedge} & \boldsymbol{\rho} \
\mathbf{0}^{T} & 0
\end{array}\right] \in \mathbb{R}^{4 \times 4}
$$
我们仍使用 $\wedge$ 和 $\vee$ 符号来指代“从向量到矩阵”和“从矩阵到向量”的关系, 以保持 和 $\mathfrak{s o}(3)$ 上的一致 性。
读者可以简单地把 $\mathfrak{s e}(3)$ 理解成 “由一个平移加上一个 $\mathfrak{s o}(3)$ 元素 构成的向量” (尽管这里的 $\rho$ 还不 直接是平移)。同样, 李代数 $\mathfrak{s e}(3)$ 亦有类似于 $\mathfrak{s o}(3)$ 的 李括号:
$$
\left[\boldsymbol{\xi}{1}, \boldsymbol{\xi}{2}\right]=\left(\boldsymbol{\xi}{1}^{\wedge} \boldsymbol{\xi}{2}^{\wedge}-\boldsymbol{\xi}{2}^{\wedge} \boldsymbol{\xi}{1}^{\wedge}\right)^{\vee}
$$
指数与对数映射
指数映射解决的问题
$\exp \left(\phi^{\wedge}\right)$ 是如何计算的
$S O(3)$ 上的指数映射
主要结论
$\mathfrak{s o}(3)$ 中向量 $\phi \rightarrow S O(3)$ 中旋转矩阵 $\boldsymbol{R}$
$$
\exp \left(\theta \boldsymbol{a}^{\wedge}\right)=\cos \theta \boldsymbol{I}+(1-\cos \theta) \mathbf{a} \boldsymbol{a}^{T}+\sin \theta \boldsymbol{a}^{\wedge} .
$$
解读:

• $\mathfrak{s o}(3)$ 实际上就是由旋转向量组成的空间
• 指数映射即罗德里格斯公式
• $\theta$ 和 $\backslash$ bold $a$ 分别为 $\phi$ 的模长和方向
  $\mathfrak{s o}(3)$ 中向量 $\phi \leftarrow S O(3)$ 中旋转矩阵 $\boldsymbol{R}$
  $$
  \phi=\ln (\boldsymbol{R})^{\vee}=\left(\sum_{n=0}^{\infty} \frac{(-1)^{n}}{n+1}(\boldsymbol{R}-\boldsymbol{I})^{n+1}\right)^{\vee}
  $$
  性质
• 满射：每个 $S O(3)$ 中的元素都可以找到一个 $\mathfrak{s o}(3)$ 元素与之对应; 但是可能存在多个 $\mathfrak{s o}(3)$ 元素 对应到同一个 $S O(3)$
• 旋转角度固定在 $\pm \pi$ 之间, 李群和李代数元素一一对应

$S E(3)$ 上的指数映射
指数映射形式
$$
\begin{aligned}
\exp \left(\boldsymbol{\xi}^{\wedge}\right) &=\left[\begin{array}{lll}
\sum_{n=0}^{\infty} \frac{1}{n !}\left(\boldsymbol{\phi}^{\wedge}\right)^{n} & \sum_{n=0}^{\infty} \frac{1}{(n+1) !}\left(\boldsymbol{\phi}^{\wedge}\right)^{n} \boldsymbol{\rho} & \
\mathbf{0}^{T} & 1
\end{array}\right] \
& \triangleq\left[\begin{array}{cc}
\boldsymbol{R} & \boldsymbol{J} \boldsymbol{\rho} \
\mathbf{0}^{T} & 1
\end{array}\right]=\boldsymbol{T}
\end{aligned}
$$
$\boldsymbol{R}$ 为 $S O(3)$ 中的元素, 与 $\mathfrak{s e}(3)$ 中的旋转部分 $\phi$ 对应
右上角 $J$ :
$$
\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}
$$

小结

李代数求导与扰动模型

BCH公式与近似形式
关键问题
$S O(3)$ 中完成两个矩阵乘法 $\longleftrightarrow \mathfrak{s o}(3)$ 中完成两个加法是否成立?
即研究
$$
\ln (\exp (\boldsymbol{A}) \exp (\boldsymbol{B}))=\boldsymbol{A}+\boldsymbol{B}
$$
是否成立
结果: 不成立
BCH公式(Baker-Campbell-Gausdorff)
$$
\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
$$
其中 ロ为李括号。 $\mathrm{BCH}$ 公式告诉我们, 当处理两个矩阵指数之积时, 它们会产生一些由李括号组成 的余项
BCH公式近似形式
特别地, 考虑 $S O(3)$ 上的李代数 $\ln \left(\exp \left(\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)\right)^{\vee}$, 当 $\phi_{1}$ 或 $\phi_{2}$ 为小量时, 小量二次以上的 项都可以被忽略掉。此时, BCH 拥有线性近似表达:
$$
\ln \left(\exp \left(\phi_{1}^{\wedge}\right) \exp \left(\phi_{2}^{\wedge}\right)\right)^{\vee} \approx \begin{cases}\boldsymbol{J}{l}\left(\boldsymbol{\phi}{2}\right)^{-1} \boldsymbol{\phi}{1}+\boldsymbol{\phi}{2} & \text { if } \boldsymbol{\phi}{1} \text { is small } \ \boldsymbol{J}{r}\left(\boldsymbol{\phi}{1}\right)^{-1} \boldsymbol{\phi}{2}+\boldsymbol{\phi}{1} & \text { if } \boldsymbol{\phi}{2} \text { is small. }\end{cases}
$$
第一种情况(左乘近似):
当对一个旋转矩阵 $\boldsymbol{R}{2}$ (李代数为 $\phi{2}$ ) 左乘一个微小旋转矩阵 $\boldsymbol{R}{1}$ ( 李代数为 $\phi{1}$ ) 时, 可以近 似地看作, 在原有的李代数 $\phi_{2}$ 上, 加上了一项 $\boldsymbol{J}{l}\left(\phi{2}\right)^{-1} \phi_{1}$ 。
第二种情况(右乘近似):
右乘一个微小位移的情况
以左乘为例。左乘 $\mathrm{BCH}$ 近似雅可比 $\boldsymbol{J}{l}$ :
$$
\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}
$$
而右乘雅可比仅需要对自变量取负号即可:
$$
\boldsymbol{J}{r}(\phi)=\boldsymbol{J}{l}(-\boldsymbol{\phi})
$$
这样, 我们就可以谈论李群乘法与李代数加法的关系了
$\mathrm{BCH}$ 近似意义重述
$S O(3)$ 上的微小旋转
$$
\exp \left(\Delta \phi^{\wedge}\right) \exp \left(\phi^{\wedge}\right)=\exp \left(\left(\phi+J{l}^{-1}(\phi) \Delta \phi\right)^{\wedge}\right)
$$
$\mathfrak{s o}(3)$ 上加法
$$
\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)
$$
$S E(3)$ 近似公式
$$
\exp \left(\Delta \boldsymbol{\xi}^{\wedge}\right) \exp \left(\boldsymbol{\xi}^{\wedge}\right) \approx \exp \left(\left(\mathcal{J}_{l}^{-1} \Delta \boldsymbol{\xi}+\boldsymbol{\xi}\right)^{\wedge}\right)
$$
$\mathrm{SO}(3)$ 李代数上的求导
如何关于带有李代数的函数的求导
SLAM中估计一个相机的位置和姿态, 位姿由 $S O(3)$ 上旋转矩阵或 $S E(3)$ 上变换矩阵描述。设某时 刻位姿为 $T$, 观察到一个世界坐标位于 $p$ 的点, 产生了一个观测数据 $z$, 则:
$$
z=T p+w
$$
然而观测噪声 $w$ 的存在, $z$ 往往不可能精确地满足 $z=T p$ 的关系。计算理想的观测与实际数据 的误差:
$$
e=z-T p
$$
假设一共有 $N$ 个这样的路标点和观测, 于是就有 $N$ 个上式。那么, 对小夢卜的位姿估计, 相当于 是寻找一个最优的 $T$, 使得整体误差最小化:
$$
\min {T} J(\boldsymbol{T})=\sum{i=1}^{N}\left|\boldsymbol{z}{i}-\boldsymbol{T} \boldsymbol{p}{i}\right|_{2}^{2}
$$
求解此问题, 需要计算目标函数 $J$ 关于变换矩阵 $\boldsymbol{T}$ 的导数
使用李代数解决求导问题的两种思路

• 李代数的求导模型
• 用李代数表示姿态，根据李代数加法来求导
• 扰动模型
• 对李群左乘或右乘微小扰动，对扰动求导(左扰动、右扰动模型)

李代数求导
结论
$$
\frac{\partial(\boldsymbol{R} \boldsymbol{p})}{\partial \boldsymbol{\phi}}=(-\boldsymbol{R} \boldsymbol{p})^{\wedge} \boldsymbol{J}_{l}
$$
推导 计算旋转之后点的坐标相对于旋转的导数，我们不严谨地记为:
$$
\frac{\partial(\boldsymbol{R} \boldsymbol{p})}{\partial \boldsymbol{R}} .
$$
由于 $S O(3)$ 没有加法, 所以该导数无法按照导数的定义进行计算。设 $\boldsymbol{R}$ 对应的李代数为 $\phi$ 我们转而计算:
$$
\frac{\partial\left(\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}\right)}{\partial \boldsymbol{\phi}}
$$
按照导数的定义, 有:
$$
\begin{aligned}
\frac{\partial\left(\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}\right)}{\partial \boldsymbol{\phi}} &=\lim _{\delta \phi \rightarrow 0} \frac{\exp \left((\boldsymbol{\phi}+\delta \boldsymbol{\phi})^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\phi}} \
&=\lim {\delta \phi \rightarrow 0} \frac{\exp \left(\left(\boldsymbol{J}{l} \delta \boldsymbol{\phi}\right)^{\wedge}\right) \exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\phi}} \
& \approx \lim {\delta \phi \rightarrow 0} \frac{\left(\boldsymbol{I}+\left(\boldsymbol{J}{l} \delta \boldsymbol{\phi}\right)^{\wedge}\right) \exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\phi}} \
&=\lim {\delta \phi \rightarrow 0} \frac{\left(\boldsymbol{J}{l} \delta \boldsymbol{\phi}\right)^{\wedge} \exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\phi}} \
&=\lim {\delta \phi \rightarrow 0} \frac{-\left(\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}\right)^{\wedge} \boldsymbol{J}{l} \delta \boldsymbol{\phi}}{\delta \phi}=-(\boldsymbol{R} \boldsymbol{p})^{\wedge} \boldsymbol{J}{l}
\end{aligned}李代数求导
结论
\$$
\frac{\partial(\boldsymbol{R} \boldsymbol{p})}{\partial \boldsymbol{\phi}}=(-\boldsymbol{R} \boldsymbol{p})^{\wedge} \boldsymbol{J}{l}
$$
推导 计算旋转之后点的坐标相对于旋转的导数，我们不严谨地记为:
$$
\frac{\partial(\boldsymbol{R} \boldsymbol{p})}{\partial \boldsymbol{R}} .
$$
由于 $S O(3)$ 没有加法, 所以该导数无法按照导数的定义进行计算。设 $\boldsymbol{R}$ 对应的李代数为 $\phi$ 我们转而计算:
$$
\frac{\partial\left(\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}\right)}{\partial \boldsymbol{\phi}}
$$
按照导数的定义, 有:
$$
\begin{aligned}
\frac{\partial\left(\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}\right)}{\partial \boldsymbol{\phi}} &=\lim _{\delta \phi \rightarrow 0} \frac{\exp \left((\boldsymbol{\phi}+\delta \boldsymbol{\phi})^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\phi}} \
&=\lim {\delta \phi \rightarrow 0} \frac{\exp \left(\left(\boldsymbol{J}{l} \delta \boldsymbol{\phi}\right)^{\wedge}\right) \exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\phi}} \
& \approx \lim {\delta \phi \rightarrow 0} \frac{\left(\boldsymbol{I}+\left(\boldsymbol{J}{l} \delta \boldsymbol{\phi}\right)^{\wedge}\right) \exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}-\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\phi}} \
&=\lim {\delta \phi \rightarrow 0} \frac{\left(\boldsymbol{J}{l} \delta \boldsymbol{\phi}\right)^{\wedge} \exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}}{\delta \boldsymbol{\phi}} \
&=\lim {\delta \phi \rightarrow 0} \frac{-\left(\exp \left(\boldsymbol{\phi}^{\wedge}\right) \boldsymbol{p}\right)^{\wedge} \boldsymbol{J}{l} \delta \boldsymbol{\phi}}{\delta \phi}=-(\boldsymbol{R} \boldsymbol{p})^{\wedge} \boldsymbol{J}_{l}
\end{aligned}
$$
$$
扰动模型(左乘)
结论
$$