Part I · 第 5 章 刚体运动与李群 · 5.7

扰动与流形上的求导

旋转矩阵有 9 个数，却只有 3 个自由度，既不能对它直接求导，也不能直接给它加增量。办法是：站在当前姿态上，沿切空间迈一小步 δ，再用 Exp 贴回流形。这一节讲清左扰动与右扰动、SO(3) 与 SE(3) 上的导数怎么算，以及优化器为什么永远在求一个小增量。

约 22 分钟 前置：3.3 雅可比 · 5.5 李代数 · 5.6 指数映射 4 个交互

    1为什么机器人需要它

    机器人里很多问题最后都是"找一个姿态 \(R\)，让残差尽量小"：用陀螺仪、加速度计和磁力计估计机身朝向，用激光点云对齐两帧位姿，标定相机和机械臂之间的手眼关系。求解它们的办法，无论梯度下降还是 Gauss-Newton（6.6 节），都要回答同一个问题：\(R\) 稍微变一点，残差会变多少？

    直接对 \(R\) 求导有两个麻烦。一是 \(R\) 有 9 个元素，但只有 3 个自由度，对 9 个元素各求一次偏导，得到的是一个冗余的、而且大部分方向根本走不出去的"梯度"。二是即使你沿着这个梯度走了一步，\(R + \Delta R\) 也不再是旋转矩阵了（5.4 节）。换成欧拉角倒是只有 3 个数，可是在万向节锁处雅可比会退化（5.2 节）。

    流形上的做法是：不在 \(R\) 上求导，而在 \(R\) 的切空间里求导。站在当前的 \(R\)，推一个小扰动 \(R\,\Exp(\boldsymbol\delta)\)。这里 \(\boldsymbol\delta\in\R^3\) 是普通向量，对它求导就是 3.3 节熟悉的雅可比。求出增量以后，再用 \(\Exp\) 贴回群上，每一步都严格留在 \(\SO\) 里。EKF 的误差状态、视觉 SLAM 的光束法平差、IMU 预积分、ICP，用的都是这套"扰动 + 回缩"的写法。

    2直觉：站在 R 上，往哪边推？

    在当前姿态 \(R\) 上再加一个小转动，有两种写法，区别只在 \(\Exp(\boldsymbol\delta)\) 乘在哪一侧：

    • 右扰动（局部） \(R\,\Exp(\boldsymbol\delta)\)：\(\boldsymbol\delta\) 是在机体系里读的。想象你坐在机器人里，说"绕我自己的 x 轴再转一点"。
    • 左扰动（全局） \(\Exp(\boldsymbol\delta)\,R\)：\(\boldsymbol\delta\) 是在世界系里读的。想象你站在外面，说"把它绕地图的 x 轴再转一点"。

    同样的三个数 \(\boldsymbol\delta\)，物理上是两个不同的转动，除非 \(R = I\)。可是任何一个小转动，总能写成其中任意一种形式，只是三个数不一样。下面的实验把两种写法并排放着：左边是右扰动，右边是左扰动，黄色（右）和紫色（左）的箭头是各自的转轴。

    FIG 1右扰动 R·Exp(δ) 与左扰动 Exp(δ)·R
    右扰动：R Exp(δ)　转轴 = Rδ̂
    左扰动：Exp(δ) R　转轴 = δ̂
    淡色坐标系是世界系，中等亮度的是当前姿态 \(R\)，最亮的是扰动之后的结果。细弧线是三个轴尖从 \(R\) 走向结果的路径。右边的 \(\boldsymbol\delta\) 默认和左边是同样的三个数；切换到"等价数值"后，右边改用 \(R\boldsymbol\delta\)，两个结果就重合了。两边共用同一个相机，可以拖动旋转视角。
    试一试
    • 让 \(R\) 明显不是单位阵（转角 > 10°），\(\|\boldsymbol\delta\| > 0.2\)，而且保持"相同数值"，同时让左右两个结果几乎重合（相差 < 0.5°）。提示：什么时候左乘和右乘无所谓？
    • 切换到"等价数值"。读一读右侧面板的转轴，它和左侧面板的转轴是什么关系？

    第二个任务的答案，是 5.8 节的主角：右扰动的转轴在世界系里读作 \(R\boldsymbol\delta\)，所以 \(R\,\Exp(\boldsymbol\delta) = \Exp(R\boldsymbol\delta)\,R\)。转轴随物体一起转了，两种写法之间隔着一个 \(R\)。对 \(\SE\) 来说，这个 \(R\) 会变成 6×6 的"伴随矩阵"。

    3动手玩：一阶近似与雅可比

    现在来求导。设物体上有一个点（比如夹爪指尖）固定在机体系里的位置 \(\mathbf p\)，它在世界系里的位置是 \(f(R) = R\mathbf p\)。让姿态做右扰动 \(R \to R\,\Exp(\boldsymbol\delta)\)，用 \(\Exp(\boldsymbol\delta)\approx I + \skew{\boldsymbol\delta}\)（一阶展开，5.6 节）：

    $$ f\big(R\,\Exp(\boldsymbol\delta)\big) = R\,\Exp(\boldsymbol\delta)\,\mathbf p \;\approx\; R\mathbf p + R\,\skew{\boldsymbol\delta}\,\mathbf p \;=\; R\mathbf p \;\underbrace{-\,R\,\skew{\mathbf p}}_{\hl{J}{J}}\,\boldsymbol\delta, $$

    最后一步用了 \(\boldsymbol\delta\times\mathbf p = -\mathbf p\times\boldsymbol\delta\)（1.8 节）。于是 \(f\) 对右扰动的雅可比就是 \(J = -R\skew{\mathbf p}\)，一个普通的 3×3 矩阵。注意它的零空间：\(\boldsymbol\delta\parallel\mathbf p\) 时 \(J\boldsymbol\delta = 0\)，因为绕着 \(\mathbf p\) 自己转，\(\mathbf p\) 这个点不动。

    "一阶近似"的意思，不只是"近似"，而是误差比 \(\boldsymbol\delta\) 小得多：\(\boldsymbol\delta\) 缩小 10 倍，误差缩小约 100 倍。下面用实验看这件事。左边是三维视图：黄色箭头是 \(R\mathbf p\)，绿点是真实结果 \(R\,\Exp(\boldsymbol\delta)\mathbf p\)，紫点是线性预测 \(R\mathbf p + J\boldsymbol\delta\)，淡色圆环是 \(\mathbf p\) 绕转轴能走到的所有位置。右边是双对数图：横轴 \(\varepsilon = \|\boldsymbol\delta\|\)，纵轴是预测误差。

    FIG 2f(R) = Rp 的一阶近似：误差随 ε 按平方缩小
    拖动画面旋转视角
    \(\mathbf p = (0.8,\,0.5,\,0.6)\) 固定在机体系里。\(\boldsymbol\delta = \varepsilon\,\hat{\boldsymbol\delta}\) 的方向用方位角和仰角设定（在机体系里），大小 \(\varepsilon\)（对数刻度）用滑块设定。右图黄线是当前方向下的误差曲线，两条虚线是斜率 1 和斜率 2 的参考线，圆点是当前的 \(\varepsilon\)。曲线和斜率 2 平行，说明误差 \(\propto\varepsilon^2\)。
    试一试
    • 把 \(\varepsilon\) 缩小到预测误差低于 \(10^{-4}\)。\(\varepsilon\) 缩小 10 倍，误差缩小多少倍？看读数里的"误差 / ε²"。
    • 把 \(\hat{\boldsymbol\delta}\) 转到和 \(\mathbf p\) 平行（\(\varepsilon \ge 0.05\)）。\(J\boldsymbol\delta\) 变成了什么？为什么误差也消失了？

    Exp 本身的导数：右雅可比 \(J_r\)

    还有一种常见的场合：状态存的是旋转向量 \(\boldsymbol\phi\)（李代数里的坐标），想知道它加一个小增量 \(\boldsymbol\delta\) 以后，旋转变了多少。直觉告诉你 \(\Exp(\boldsymbol\phi + \boldsymbol\delta) = \Exp(\boldsymbol\phi)\Exp(\boldsymbol\delta)\)，可惜这不对。旋转向量空间是一张"弯曲的地图"：同样的 \(\boldsymbol\delta\)，在 \(\boldsymbol\phi\) 不同的地方，对应的实际转动不同。把这种"汇率"算出来，就是右雅可比 \(J_r(\boldsymbol\phi)\)：

    $$ \Exp(\boldsymbol\phi + \boldsymbol\delta) \;\approx\; \Exp(\boldsymbol\phi)\,\Exp\big(J_r(\boldsymbol\phi)\,\boldsymbol\delta\big), \qquad J_r(\boldsymbol\phi) = I - \frac{1-\cos\theta}{\theta^2}\skew{\boldsymbol\phi} + \frac{\theta-\sin\theta}{\theta^3}\skew{\boldsymbol\phi}^2,\quad \theta = \|\boldsymbol\phi\|. $$

    下面的实验把三种做法比一比：真值 \(\Exp(\boldsymbol\phi+\boldsymbol\delta)\)（实心坐标系），天真的 \(\hl{naive}{\Exp(\boldsymbol\phi)\Exp(\boldsymbol\delta)}\)（红色虚线），修正后的 \(\Exp(\boldsymbol\phi)\Exp(\hl{Jr}{J_r}\boldsymbol\delta)\)（绿色虚线）。\(\theta\) 很小时 \(J_r\approx I\)，天真的做法也还行；\(\theta\) 变大以后，红色明显偏离，绿色仍然贴着真值。

    FIG 3Exp(φ+δ)：天真做法 vs. 右雅可比修正
    拖动画面旋转视角
    紫色虚线是 \(\boldsymbol\phi\) 的转轴，最淡的坐标系是 \(\Exp(\boldsymbol\phi)\)。实心坐标系是真值 \(\Exp(\boldsymbol\phi+\boldsymbol\delta)\)，红点和绿点分别是天真做法和修正做法的轴尖。读数里的角度误差是它们和真值之间的最小转角。
    • 把转角 \(\theta\) 拉到 150° 以上，\(\|\boldsymbol\delta\| > 0.2\)。比较两种做法的误差，修正后的误差应该小得多。再把 \(\theta\) 推向 350°，看 \(\det J_r\) 变成什么。

    4再看数学

    提示：把鼠标悬停在公式中带底色的符号上，FIG 1 或 FIG 3 中对应的元素会被高亮（§3 正文里的 \(J\) 和两种做法也对应 FIG 2、FIG 3）。

    ⊕ 与回缩 (retraction)

    把切空间里的增量 \(\boldsymbol\delta\in\R^3\) 作用到群元素上，得到新的群元素。这个动作叫回缩，两种写法分别记作 \(\oplus\)：

    $$ R \oplus_{\text{right}} \boldsymbol\delta = R\,\Exp(\boldsymbol\delta), \qquad R \oplus_{\text{left}} \boldsymbol\delta = \Exp(\boldsymbol\delta)\,R, \qquad R\,\Exp(\hl{right}{\boldsymbol\delta_r}) = \Exp(\hl{left}{\boldsymbol\delta_l})\,R \;\Longleftrightarrow\; \boldsymbol\delta_l = R\,\boldsymbol\delta_r. $$

    最后一个等式用到 \(R\,\Exp(\boldsymbol\delta)R\T = \Exp(R\boldsymbol\delta)\)（旋转共轭，5.8 节）。\(\boldsymbol\delta\) 很小时，\(R\oplus\boldsymbol\delta\) 与 \(R\) 的"差"可以用 \(\ominus\) 读回来：\(R_2\ominus R_1 = \Log(R_1\T R_2)\)（右）。这对 \(\oplus,\ominus\) 让流形上的运算看起来像向量运算（Solà 等的论文系统地讲了这套记号）。

    流形上的导数

    设 \(f:\SO\to\R^m\)。\(f\) 在 \(R\) 处对右扰动的导数，定义为把 \(\boldsymbol\delta\) 当作普通向量求偏导：

    $$ \frac{\partial f}{\partial\boldsymbol\delta}\bigg|_R = \lim_{\boldsymbol\delta\to 0}\frac{f\big(R\,\Exp(\boldsymbol\delta)\big) - f(R)}{\boldsymbol\delta}\in\R^{m\times 3}, \qquad f\big(R\,\Exp(\boldsymbol\delta)\big) = f(R) + \frac{\partial f}{\partial\boldsymbol\delta}\boldsymbol\delta + O(\|\boldsymbol\delta\|^2). $$

    对左扰动同理。几个最常用的结果（\(\mathbf p\) 是常向量）：

    函数右扰动 \(R\,\Exp(\boldsymbol\delta)\)左扰动 \(\Exp(\boldsymbol\delta)\,R\)
    \(R\mathbf p\)\(-R\skew{\mathbf p}\)\(-\skew{R\mathbf p}\)
    \(R\T\mathbf p\)\(\skew{R\T\mathbf p}\)\(R\T\skew{\mathbf p}\)
    \(\Log(R)\)\(J_r^{-1}(\Log R)\)\(J_l^{-1}(\Log R)\)

    第二行是 \((R\,\Exp\boldsymbol\delta)\T\mathbf p = \Exp(-\boldsymbol\delta)R\T\mathbf p \approx R\T\mathbf p - \boldsymbol\delta\times R\T\mathbf p = R\T\mathbf p + \skew{R\T\mathbf p}\boldsymbol\delta\)。一个好习惯：算完以后检查两侧的结果能否通过 \(\boldsymbol\delta_l = R\boldsymbol\delta_r\) 互相换算。比如第一行，\(-R\skew{\mathbf p}\,R\T = -\skew{R\mathbf p}\)。

    左右雅可比与 BCH

    $$ \Exp(\boldsymbol\phi+\boldsymbol\delta)\approx \Exp(\boldsymbol\phi)\,\Exp\big(\hl{Jr}{J_r}(\boldsymbol\phi)\boldsymbol\delta\big)\approx \Exp\big(J_l(\boldsymbol\phi)\boldsymbol\delta\big)\,\Exp(\boldsymbol\phi), \qquad J_l(\boldsymbol\phi) = I + \frac{1-\cos\theta}{\theta^2}\skew{\boldsymbol\phi} + \frac{\theta-\sin\theta}{\theta^3}\skew{\boldsymbol\phi}^2, $$ $$ J_r(\boldsymbol\phi) = J_l(-\boldsymbol\phi) = J_l(\boldsymbol\phi)\T, \qquad J_l(\boldsymbol\phi) = \Exp(\boldsymbol\phi)\,J_r(\boldsymbol\phi), \qquad J_{r}^{-1}(\boldsymbol\phi) = I + \tfrac12\skew{\boldsymbol\phi} + \Big(\tfrac{1}{\theta^2} - \tfrac{1+\cos\theta}{2\theta\sin\theta}\Big)\skew{\boldsymbol\phi}^2. $$

    把它倒过来用，就得到两个旋转相乘时的一阶 BCH 公式：当 \(\boldsymbol\delta\) 很小时，\(\Log\big(\Exp(\boldsymbol\phi)\Exp(\boldsymbol\delta)\big)\approx\boldsymbol\phi + J_r^{-1}(\boldsymbol\phi)\boldsymbol\delta\)。5.6 节误区 3 里的 BCH 展开 \(\mathbf a+\mathbf b+\tfrac12\mathbf a\times\mathbf b+\cdots\) 取小量 \(\mathbf b\) 的一阶，正是 \(J_r^{-1}\) 里的 \(\tfrac12\skew{\boldsymbol\phi}\) 项（符号取决于左右）。\(J_l\) 的 \(V\) 矩阵就是 5.6 节 \(\Exp(\boldsymbol\xi)\) 平移部分里的那个 \(V\)，课程代码 R101.M.V3 实现的是 \(J_l\)。\(\theta = 2\pi\) 时 \(J\) 奇异：\(\det J = 2(1-\cos\theta)/\theta^2\) 在那里变成 0，所以不要让旋转向量的模长接近 \(2\pi\)。

    SE(3) 上同理

    位姿 \(T = (R,\mathbf t)\)，twist 的顺序仍是 \((\boldsymbol\omega,\mathbf v)\)（5.6 节）。右扰动 \(T\,\Exp(\boldsymbol\delta)\) 的小量 \(\boldsymbol\delta = (\boldsymbol\delta_\omega,\boldsymbol\delta_v)\) 作用在机体系里的点 \(\mathbf p\) 上：

    $$ T\,\Exp(\boldsymbol\delta)\,\mathbf p \approx R\big(\mathbf p + \boldsymbol\delta_\omega\times\mathbf p + \boldsymbol\delta_v\big) + \mathbf t \;\Longrightarrow\; \frac{\partial (T\mathbf p)}{\partial\boldsymbol\delta_r} = \begin{bmatrix} -R\skew{\mathbf p} & R \end{bmatrix}, \qquad \frac{\partial (T\mathbf p)}{\partial\boldsymbol\delta_l} = \begin{bmatrix} -\skew{T\mathbf p} & I \end{bmatrix}. $$

    左边的 \(3\times6\) 矩阵，是视觉 SLAM 里"重投影误差对位姿的雅可比"的第一块：先对世界点 \(T\mathbf p\) 求导，再乘上相机投影的雅可比（Part III）。

    5机器人中的它：Gauss-Newton 估计姿态

    无人机上有若干条方向观测：加速度计测到的重力方向、磁力计测到的磁北、相机看到的几处已知地标的方向。每一条在机体系里读作 \(\mathbf b_i\)，在世界系里的真实方向是已知的 \(\mathbf r_i\)。要估计机体姿态 \(R\)，使得 \(R\mathbf b_i\) 尽量接近 \(\mathbf r_i\)（这就是 Wahba 问题）。每条观测的残差和它对右扰动的雅可比分别是

    $$ \mathbf e_i(R) = R\mathbf b_i - \mathbf r_i, \qquad J_i = -R\skew{\mathbf b_i}, $$ $$ \Big(\sum_i J_i\T J_i\Big)\boldsymbol\delta = -\sum_i J_i\T\mathbf e_i, \qquad R \leftarrow R\,\Exp(\boldsymbol\delta). $$

    这就是 Gauss-Newton 一轮的全部内容：在切空间里解一个 3×3 线性方程，然后回缩。FIG 4 同时跑两种更新：回缩 \(R\leftarrow R\,\Exp(\boldsymbol\delta)\)，和天真地把增量加到矩阵上 \(R \leftarrow R + R\skew{\boldsymbol\delta}\)。两者从同一个初值出发，用同样的数据和同样的雅可比。

    FIG 4Wahba 问题：Gauss-Newton + 回缩
    黄 = 世界方向 r，青 = R·b　拖动旋转视角
    四条观测 \(\mathbf b_i = R_\star\T\mathbf r_i + \)噪声，\(R_\star\) 是真实姿态（不会显示）。三维视图里，黄色箭头是世界方向 \(\mathbf r_i\)，青色箭头是当前估计旋转后的 \(R\mathbf b_i\)，红线是残差。右图是两种更新的代价 \(\tfrac12\sum\|\mathbf e_i\|^2\)（对数坐标）。读数里的"正交性误差" \(\|R\T R-I\|_F\) 衡量迭代点离开 \(\SO\) 有多远。
    试一试
    • 从误差约 90° 的初值出发，反复点"迭代一步"，直到回缩那一路的增量 \(\|\boldsymbol\delta\| < 10^{-6}\)。需要几步？
    • 迭代几步，让"天真加法"那一路的正交性误差超过 0.05。它的 \(R\) 还是旋转矩阵吗？
    真实系统里

    误差状态 EKF（ESKF）把姿态的小误差写成右（或左）扰动 \(R = \hat R\,\Exp(\boldsymbol\delta)\)，卡尔曼滤波只在 \(\boldsymbol\delta\in\R^3\) 里做，更新后把 \(\boldsymbol\delta\) 注入 \(\hat R\leftarrow\hat R\,\Exp(\hat{\boldsymbol\delta})\) 并清零，就是这一节的"求增量 + 回缩"。视觉 SLAM 的后端（g2o、GTSAM、Ceres）里，每个位姿节点都带着自己的 \(\oplus\)；因子图的雅可比就是上面这些 \(-R\skew{\mathbf p}\) 之类的块（6.9 节）。IMU 预积分（Forster 等人）用右雅可比把陀螺零偏的修正量传播到预积分的旋转里。

    6常见误区

    误区 1：左右扰动的公式混着抄

    \(R\mathbf p\) 对右扰动的雅可比是 \(-R\skew{\mathbf p}\)，对左扰动是 \(-\skew{R\mathbf p}\)。从论文或别的库里抄公式时，先看清它用的是哪一侧。协方差也一样：它描述的误差是左还是右，必须和雅可比一致（5.9 节）。混用时通常不会崩溃，只是收敛变慢，或者滤波器悄悄地变得不一致。

    误区 2：对矩阵元素或欧拉角求导

    对 9 个元素求导，得到的 \(3\times9\) 雅可比有 6 个多余的自由度，而且沿它走会离开 \(\SO\)。对欧拉角求导，在俯仰角 \(\pm90^\circ\) 附近雅可比的秩下降，优化器会剧烈抖动。扰动 \(\boldsymbol\delta\) 始终是 3 个数，而且在 \(\boldsymbol\delta = 0\) 处是一个没有奇异性的局部坐标，这是它的最大优点。

    误区 3：把 Exp(φ+δ) 当成 Exp(φ)Exp(δ)

    只在 \(\boldsymbol\phi\) 很小，或者 \(\boldsymbol\delta\parallel\boldsymbol\phi\) 时成立。要么先用 \(J_r\) 修正，要么干脆别把状态存成旋转向量，而是存 \(R\)（或四元数），只让 \(\boldsymbol\delta\) 当局部增量。后者就是 FIG 4 的做法，也是绝大多数估计器的做法。

    误区 4：用"加完再归一化"代替回缩

    \(R + R\skew{\boldsymbol\delta}\) 之后再做 SVD 投影，确实得到一个合法的旋转矩阵，但它是对 \(R\,\Exp(\boldsymbol\delta)\) 的一个不精确的近似，\(\boldsymbol\delta\) 较大时偏差明显。回缩 \(R\,\Exp(\boldsymbol\delta)\) 的代价只多一次 Rodrigues 公式，而且精确留在群上。

    误区 5：SE(3) 雅可比里 twist 分量顺序对不上

    本课的 \(\boldsymbol\delta = (\boldsymbol\delta_\omega,\boldsymbol\delta_v)\)，右扰动的雅可比是 \([-R\skew{\mathbf p}\;\;R]\)。把平移放在前面的库（见 5.6 节）要写成 \([R\;\;-R\skew{\mathbf p}]\)。列的顺序错了，求出的位姿增量会把转动和平移互相串了。

    7小练习

    1. 机体系中的固定点 \(\mathbf p\)，世界位置 \(R\mathbf p\)。它对右扰动 \(R\,\Exp(\boldsymbol\delta)\) 的雅可比是？
    \(R\,\Exp(\boldsymbol\delta)\mathbf p\approx R\mathbf p + R(\boldsymbol\delta\times\mathbf p) = R\mathbf p - R\skew{\mathbf p}\boldsymbol\delta\)。选项 C 是左扰动的雅可比。注意 \(-\skew{R\mathbf p} = -R\skew{\mathbf p}R\T\)，两者只差一个 \(R\)，这正是 \(\boldsymbol\delta_l = R\boldsymbol\delta_r\)。选项 D 的 \(\skew{\mathbf p}R\) 和 \(R\skew{\mathbf p}\) 一般不相等。
    2. \(R\) 是绕 z 轴转 90°。一个右扰动 \(\boldsymbol\delta_r = (\varepsilon,0,0)\)（绕机体 x 轴），等价于哪个左扰动 \(\boldsymbol\delta_l\)？
    \(\boldsymbol\delta_l = R\boldsymbol\delta_r\)。绕 z 转 90° 把 \(\hat x\) 送到 \(\hat y\)，所以 \(\boldsymbol\delta_l = (0,\varepsilon,0)\)。直观地说：机体的 x 轴此刻指向世界的 y 方向，"绕机体 x 轴转"就是"绕世界 y 轴转"。
    3. 一阶近似 \(f(R\,\Exp\boldsymbol\delta)\approx f(R)+J\boldsymbol\delta\) 的误差通常和 \(\|\boldsymbol\delta\|\) 是什么关系？把 \(\|\boldsymbol\delta\|\) 减半，误差大约变成？
    被忽略的是二阶项，\(\propto\|\boldsymbol\delta\|^2\)。FIG 2 的双对数图里误差曲线的斜率就是 2。
    4. 优化器求出切空间增量 \(\boldsymbol\delta\) 以后，为什么要用 \(R\leftarrow R\,\Exp(\boldsymbol\delta)\) 更新，而不是 \(R\leftarrow R+R\skew{\boldsymbol\delta}\)？
    加法的结果 \(R(I+\skew{\boldsymbol\delta})\) 的各列长度变成 \(\sqrt{1+\|\boldsymbol\delta\|^2-\delta_i^2}\) 之类，不再是旋转。\(\Exp\) 稍贵，但每步只多算一次 Rodrigues 公式。
    5. 位姿 \(T=(R,\mathbf t)\)，twist 顺序 \((\boldsymbol\omega,\mathbf v)\)。机体系点 \(\mathbf p\) 的世界位置 \(T\mathbf p\) 对右扰动 \((\boldsymbol\delta_\omega,\boldsymbol\delta_v)\) 的 \(3\times6\) 雅可比是？
    \(T\,\Exp(\boldsymbol\delta)\mathbf p\approx R(\mathbf p+\boldsymbol\delta_\omega\times\mathbf p+\boldsymbol\delta_v)+\mathbf t\)，所以转动列是 \(-R\skew{\mathbf p}\)，平移列是 \(R\)（平移增量在机体系里读，要乘 \(R\) 才到世界系）。选项 C 是左扰动 \([-\skew{T\mathbf p}\;\;I]\)（把 \(T\mathbf p\) 写成 \(R\mathbf p\) 就是一个常见的错误），选项 A 是把顺序写反了的左扰动。

    8延伸阅读

    • J. Solà, J. Deray, D. Atchuthan, "A micro Lie theory for state estimation in robotics", arXiv:1812.01537 — 左右扰动、⊕/⊖、左右雅可比的系统整理，附 SO(3)/SE(3) 的全部闭式（平移在前）
    • T. Barfoot, State Estimation for Robotics, Ch. 7 — 矩阵李群上的扰动、雅可比以及不确定性
    • C. Forster, L. Carlone, F. Dellaert, D. Scaramuzza, "On-Manifold Preintegration for Real-Time Visual-Inertial Odometry", IEEE Trans. Robotics, 2017 — 右雅可比在 IMU 预积分中的实际用法
    • P.-A. Absil, R. Mahony, R. Sepulchre, Optimization Algorithms on Matrix Manifolds, Princeton University Press, 2008 — 回缩（retraction）与流形上的优化，6.10 节会继续用到