Part I · 第 5 章刚体运动与李群 · 5.7Part I · Ch. 5 Rigid-Body Motion & Lie Groups · 5.7

扰动与流形上的求导Perturbations & Derivatives on Manifolds

旋转矩阵有 9 个数，却只有 3 个自由度，既不能对它直接求导，也不能直接给它加增量。办法是：站在当前姿态上，沿切空间迈一小步 δ，再用 Exp 贴回流形。这一节讲清左扰动与右扰动、SO(3) 与 SE(3) 上的导数怎么算，以及优化器为什么永远在求一个小增量。 A rotation matrix has nine numbers but only three degrees of freedom, so you can neither differentiate with respect to it nor simply add an increment to it. The fix: stand at the current orientation, take a small step δ in the tangent space, and map it back with Exp. This lesson explains left vs. right perturbations, how to differentiate on SO(3) and SE(3), and why optimizers always solve for a small increment.

约 22 分钟~22 min 前置：3.3 雅可比 · 5.5 李代数 · 5.6 指数映射Prereqs: 3.3 Jacobian · 5.5 Lie algebras · 5.6 Exp & Log 4 个交互4 interactives

1为什么机器人需要它Why robots need this

机器人里很多问题最后都是"找一个姿态 $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，用的都是这套"扰动 + 回缩"的写法。

Many robotics problems end up as "find an orientation $R$ that makes the residual small": estimating the body's attitude from a gyro, accelerometer and magnetometer, aligning two lidar scans, calibrating the hand-eye transform between a camera and an arm. Whether you solve them by gradient descent or by Gauss-Newton (Lesson 6.6), you need the answer to one question: if $R$ changes a little, how much does the residual change?

Differentiating directly with respect to $R$ causes two problems. First, $R$ has nine entries but only three degrees of freedom, so nine partial derivatives give a redundant "gradient" whose directions mostly lead off the manifold. Second, even if you walk along it, $R + \Delta R$ is no longer a rotation matrix (Lesson 5.4). Euler angles do use only three numbers, but their Jacobian degenerates at gimbal lock (Lesson 5.2).

The manifold way is: do not differentiate on $R$; differentiate in the tangent space at $R$. Stand at the current $R$ and apply a small perturbation $R\,\Exp(\boldsymbol\delta)$. Here $\boldsymbol\delta\in\R^3$ is an ordinary vector, so differentiating with respect to it is the Jacobian of Lesson 3.3. Once you have the increment, map it back with $\Exp$, and every step stays exactly on $\SO$. Error-state EKFs, bundle adjustment in visual SLAM, IMU preintegration and ICP are all written in this "perturb and retract" style.

2直觉：站在 R 上，往哪边推？Intuition: standing at R, which way do you push?

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

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

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

There are two ways to add a small extra rotation to the current orientation $R$; they differ only in which side $\Exp(\boldsymbol\delta)$ is multiplied on:

Right perturbation (local) $R\,\Exp(\boldsymbol\delta)$: $\boldsymbol\delta$ is read in the body frame. Picture yourself sitting inside the robot saying "turn a bit more about my own x-axis".
Left perturbation (global) $\Exp(\boldsymbol\delta)\,R$: $\boldsymbol\delta$ is read in the world frame. Picture standing outside saying "turn it a bit more about the map's x-axis".

The same three numbers $\boldsymbol\delta$ are two different physical rotations, unless $R = I$. Yet any small rotation can be written either way, only with different numbers. The lab below puts the two side by side: right perturbation on the left, left perturbation on the right; the amber (right) and purple (left) arrows are their rotation axes.

FIG 1右扰动 R·Exp(δ) 与左扰动 Exp(δ)·RRight perturbation R·Exp(δ) vs. left perturbation Exp(δ)·R

右扰动：R Exp(δ)　转轴 = Rδ̂RIGHT · R Exp(δ) · axis = Rδ̂

左扰动：Exp(δ) R　转轴 = δ̂LEFT · Exp(δ) R · axis = δ̂

淡色坐标系是世界系，中等亮度的是当前姿态 $R$，最亮的是扰动之后的结果。细弧线是三个轴尖从 $R$ 走向结果的路径。右边的 $\boldsymbol\delta$ 默认和左边是同样的三个数；切换到"等价数值"后，右边改用 $R\boldsymbol\delta$，两个结果就重合了。两边共用同一个相机，可以拖动旋转视角。 The faint frame is the world, the medium one is the current orientation $R$, and the brightest is the result after the perturbation. The thin arcs are the paths of the three axis tips from $R$ to the result. By default the right panel uses the same three numbers $\boldsymbol\delta$ as the left; switch to "equivalent numbers" and the right panel uses $R\boldsymbol\delta$ instead, and the two results coincide. Both panels share one camera: drag to orbit.

试一试Try this

让 $R$ 明显不是单位阵（转角 > 10°），$\|\boldsymbol\delta\| > 0.2$，而且保持"相同数值"，同时让左右两个结果几乎重合（相差 < 0.5°）。提示：什么时候左乘和右乘无所谓？Make $R$ clearly not the identity (angle > 10°), $\|\boldsymbol\delta\| > 0.2$, keep "same numbers", and still make the two results nearly coincide (within 0.5°). Hint: when does it not matter on which side you multiply?
切换到"等价数值"。读一读右侧面板的转轴，它和左侧面板的转轴是什么关系？Switch to "equivalent numbers". Compare the axis in the right panel with the one in the left panel: how are they related?

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

The answer to the second task is the star of Lesson 5.8: a right perturbation's axis reads $R\boldsymbol\delta$ in world coordinates, so $R\,\Exp(\boldsymbol\delta) = \Exp(R\boldsymbol\delta)\,R$. The axis travels with the object, so a factor $R$ sits between the two conventions. On $\SE$ that $R$ becomes a 6×6 "adjoint matrix".

3动手玩：一阶近似与雅可比Play: first-order approximation and the Jacobian

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

Time to differentiate. Suppose a point on the object (say a gripper fingertip) sits at a fixed position $\mathbf p$ in the body frame; its world position is $f(R) = R\mathbf p$. Apply a right perturbation $R \to R\,\Exp(\boldsymbol\delta)$ and use $\Exp(\boldsymbol\delta)\approx I + \skew{\boldsymbol\delta}$ (first-order expansion, Lesson 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\|$，纵轴是预测误差。

The last step used $\boldsymbol\delta\times\mathbf p = -\mathbf p\times\boldsymbol\delta$ (Lesson 1.8). So the Jacobian of $f$ with respect to a right perturbation is $J = -R\skew{\mathbf p}$, an ordinary 3×3 matrix. Note its null space: $J\boldsymbol\delta = 0$ when $\boldsymbol\delta\parallel\mathbf p$, since spinning about $\mathbf p$ itself leaves the point where it is.

"First-order approximation" means more than "approximation": the error is much smaller than $\boldsymbol\delta$ itself. Shrink $\boldsymbol\delta$ tenfold and the error shrinks about a hundredfold. The lab shows it. On the left is a 3D view: the amber arrow is $R\mathbf p$, the green dot the true result $R\,\Exp(\boldsymbol\delta)\mathbf p$, the purple dot the linear prediction $R\mathbf p + J\boldsymbol\delta$, and the faint ring is every place $\mathbf p$ can reach by spinning about that axis. On the right is a log-log plot: horizontal axis $\varepsilon = \|\boldsymbol\delta\|$, vertical axis the prediction error.

FIG 2f(R) = Rp 的一阶近似：误差随 ε 按平方缩小First-order model of f(R) = Rp: the error shrinks like ε²

拖动画面旋转视角drag to orbit

$\mathbf p = (0.8,\,0.5,\,0.6)$ 固定在机体系里。$\boldsymbol\delta = \varepsilon\,\hat{\boldsymbol\delta}$ 的方向用方位角和仰角设定（在机体系里），大小 $\varepsilon$（对数刻度）用滑块设定。右图黄线是当前方向下的误差曲线，两条虚线是斜率 1 和斜率 2 的参考线，圆点是当前的 $\varepsilon$。曲线和斜率 2 平行，说明误差 $\propto\varepsilon^2$。 $\mathbf p = (0.8,\,0.5,\,0.6)$ is fixed in the body frame. The direction of $\boldsymbol\delta = \varepsilon\,\hat{\boldsymbol\delta}$ is set by azimuth and elevation (in the body frame) and its size $\varepsilon$ (log scale) by the slider. In the plot the amber line is the error curve for the current direction, the two dashed lines are reference slopes 1 and 2, and the dot marks the current $\varepsilon$. The curve runs parallel to slope 2: the error is $\propto\varepsilon^2$.

试一试Try this

把 $\varepsilon$ 缩小到预测误差低于 $10^{-4}$。$\varepsilon$ 缩小 10 倍，误差缩小多少倍？看读数里的"误差 / ε²"。Shrink $\varepsilon$ until the prediction error is below $10^{-4}$. When $\varepsilon$ shrinks tenfold, by what factor does the error shrink? Watch the "error / ε²" readout.
把 $\hat{\boldsymbol\delta}$ 转到和 $\mathbf p$ 平行（$\varepsilon \ge 0.05$）。$J\boldsymbol\delta$ 变成了什么？为什么误差也消失了？Turn $\hat{\boldsymbol\delta}$ until it is parallel to $\mathbf p$ (with $\varepsilon \ge 0.05$). What does $J\boldsymbol\delta$ become? Why does the error vanish too?

Exp 本身的导数：右雅可比 $J_r$The derivative of Exp itself: the right Jacobian $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)$：

Another common situation: the state stores a rotation vector $\boldsymbol\phi$ (a coordinate in the Lie algebra) and you want to know how the rotation changes when $\boldsymbol\phi$ gets a small increment $\boldsymbol\delta$. Intuition says $\Exp(\boldsymbol\phi + \boldsymbol\delta) = \Exp(\boldsymbol\phi)\Exp(\boldsymbol\delta)$, and it is wrong. The space of rotation vectors is a "curved map": the same $\boldsymbol\delta$ means a different actual turn at different $\boldsymbol\phi$. The "exchange rate" is the right Jacobian $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$ 变大以后，红色明显偏离，绿色仍然贴着真值。

The lab below compares three methods: the truth $\Exp(\boldsymbol\phi+\boldsymbol\delta)$ (solid frame), the naive $\hl{naive}{\Exp(\boldsymbol\phi)\Exp(\boldsymbol\delta)}$ (red dashed) and the corrected $\Exp(\boldsymbol\phi)\Exp(\hl{Jr}{J_r}\boldsymbol\delta)$ (green dashed). When $\theta$ is small, $J_r\approx I$ and the naive version is fine; as $\theta$ grows, red drifts away while green stays glued to the truth.

FIG 3Exp(φ+δ)：天真做法 vs. 右雅可比修正Exp(φ+δ): naive vs. corrected by the right Jacobian

拖动画面旋转视角drag to orbit

紫色虚线是 $\boldsymbol\phi$ 的转轴，最淡的坐标系是 $\Exp(\boldsymbol\phi)$。实心坐标系是真值 $\Exp(\boldsymbol\phi+\boldsymbol\delta)$，红点和绿点分别是天真做法和修正做法的轴尖。读数里的角度误差是它们和真值之间的最小转角。 The dashed purple line is the axis of $\boldsymbol\phi$ and the faintest frame is $\Exp(\boldsymbol\phi)$. The solid frame is the truth $\Exp(\boldsymbol\phi+\boldsymbol\delta)$; the red and green dots are the axis tips of the naive and corrected methods. The angle errors in the readout are the smallest rotation between each method and the truth.

把转角 $\theta$ 拉到 150° 以上，$\|\boldsymbol\delta\| > 0.2$。比较两种做法的误差，修正后的误差应该小得多。再把 $\theta$ 推向 350°，看 $\det J_r$ 变成什么。Push $\theta$ beyond 150° with $\|\boldsymbol\delta\| > 0.2$ and compare the two errors: the corrected one should be much smaller. Then push $\theta$ toward 350° and watch what $\det J_r$ does.

4再看数学The math

提示：把鼠标悬停在公式中带底色的符号上，FIG 1 或 FIG 3 中对应的元素会被高亮（§3 正文里的 $J$ 和两种做法也对应 FIG 2、FIG 3）。Tip: hover the highlighted symbols in the formulas; the matching element in FIG 1 or FIG 3 lights up (the $J$ and the two methods in the §3 text light up FIG 2 and FIG 3 too).

⊕ 与回缩 (retraction)⊕ and retraction

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

Applying an increment $\boldsymbol\delta\in\R^3$ from the tangent space to a group element yields a new group element. This operation is a retraction, and the two conventions are both written $\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à 等的论文系统地讲了这套记号）。

The last equality uses $R\,\Exp(\boldsymbol\delta)R\T = \Exp(R\boldsymbol\delta)$ (rotation conjugation, Lesson 5.8). For small $\boldsymbol\delta$, the "difference" between $R\oplus\boldsymbol\delta$ and $R$ is read back with $\ominus$: $R_2\ominus R_1 = \Log(R_1\T R_2)$ (right). The pair $\oplus,\ominus$ makes operations on a manifold look like vector arithmetic (the paper by Solà et al. develops this notation systematically).

流形上的导数Derivatives on a manifold

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

Let $f:\SO\to\R^m$. The derivative of $f$ at $R$ with respect to a right perturbation is the ordinary partial derivative in the vector $\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$ 是常向量）：

The same for a left perturbation. The most common results ($\mathbf p$ is a constant vector):

函数Function	右扰动 $R\,\Exp(\boldsymbol\delta)$Right $R\,\Exp(\boldsymbol\delta)$	左扰动 $\Exp(\boldsymbol\delta)\,R$Left $\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}$。

The second row comes from $(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$. A good habit: after deriving, check that the two sides convert into each other through $\boldsymbol\delta_l = R\boldsymbol\delta_r$. For the first row, $-R\skew{\mathbf p}\,R\T = -\skew{R\mathbf p}$.

左右雅可比与 BCHLeft and right Jacobians, and 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$。

Used backwards, this gives the first-order BCH formula for composing two rotations: for small $\boldsymbol\delta$, $\Log\big(\Exp(\boldsymbol\phi)\Exp(\boldsymbol\delta)\big)\approx\boldsymbol\phi + J_r^{-1}(\boldsymbol\phi)\boldsymbol\delta$. The BCH expansion $\mathbf a+\mathbf b+\tfrac12\mathbf a\times\mathbf b+\cdots$ from Pitfall 3 of Lesson 5.6, taken to first order in the small $\mathbf b$, is exactly the $\tfrac12\skew{\boldsymbol\phi}$ term of $J_r^{-1}$ (the sign depends on left vs. right). The matrix $V$ in the translation block of $\Exp(\boldsymbol\xi)$ in Lesson 5.6 is $J_l$; the course code R101.M.V3 implements $J_l$. At $\theta = 2\pi$ the Jacobian is singular: $\det J = 2(1-\cos\theta)/\theta^2$ hits 0 there, so keep rotation vectors away from norm $2\pi$.

SE(3) 上同理The same on 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$ 上：

For a pose $T = (R,\mathbf t)$ the twist ordering is still $(\boldsymbol\omega,\mathbf v)$ (Lesson 5.6). A right perturbation $T\,\Exp(\boldsymbol\delta)$ with $\boldsymbol\delta = (\boldsymbol\delta_\omega,\boldsymbol\delta_v)$ acts on a body-frame point $\mathbf p$ as

$$ 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）。

The $3\times6$ matrix on the left is the first factor of the "reprojection error with respect to pose" Jacobian in visual SLAM: differentiate with respect to the world point $T\mathbf p$ first, then multiply by the camera projection Jacobian (Part III).

5机器人中的它：Gauss-Newton 估计姿态In robotics: Gauss-Newton attitude estimation

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

A drone has several direction measurements: the gravity direction from the accelerometer, magnetic north from the magnetometer, and bearings to a few known landmarks from a camera. Each is read in the body frame as $\mathbf b_i$, while its true direction in the world, $\mathbf r_i$, is known. We want the attitude $R$ such that $R\mathbf b_i$ is as close as possible to $\mathbf r_i$ (this is Wahba's problem). The residual of each measurement and its Jacobian with respect to a right perturbation are

$$ \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}$。两者从同一个初值出发，用同样的数据和同样的雅可比。

That is the whole Gauss-Newton iteration: solve a 3×3 linear system in the tangent space, then retract. FIG 4 runs two updates side by side: the retraction $R\leftarrow R\,\Exp(\boldsymbol\delta)$ and the naive addition to the matrix $R \leftarrow R + R\skew{\boldsymbol\delta}$. Both start from the same initial guess, with the same data and the same Jacobian.

FIG 4Wahba 问题：Gauss-Newton + 回缩Wahba's problem: Gauss-Newton + retraction

黄 = 世界方向 r，青 = R·b　拖动旋转视角amber = world r · cyan = R·b · drag to orbit

四条观测 $\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$ 有多远。 Four measurements $\mathbf b_i = R_\star\T\mathbf r_i + $noise, where $R_\star$ is the true attitude (not shown). In the 3D view the amber arrows are the world directions $\mathbf r_i$, the cyan arrows are $R\mathbf b_i$ for the current estimate, and the red segments are the residuals. The plot on the right shows the cost $\tfrac12\sum\|\mathbf e_i\|^2$ of both updates (log scale). The "orthogonality error" $\|R\T R-I\|_F$ in the readout measures how far the iterate has drifted off $\SO$.

试一试Try this

从误差约 90° 的初值出发，反复点"迭代一步"，直到回缩那一路的增量 $\|\boldsymbol\delta\| < 10^{-6}$。需要几步？Start from an initial guess about 90° off and keep pressing "Step" until the retraction run has $\|\boldsymbol\delta\| < 10^{-6}$. How many steps does it take?
迭代几步，让"天真加法"那一路的正交性误差超过 0.05。它的 $R$ 还是旋转矩阵吗？Iterate until the "naive addition" run's orthogonality error exceeds 0.05. Is its $R$ still a rotation matrix?

真实系统里In real systems

误差状态 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 等人）用右雅可比把陀螺零偏的修正量传播到预积分的旋转里。

An error-state EKF (ESKF) writes the small attitude error as a right (or left) perturbation $R = \hat R\,\Exp(\boldsymbol\delta)$, runs the Kalman filter only on $\boldsymbol\delta\in\R^3$, then injects it, $\hat R\leftarrow\hat R\,\Exp(\hat{\boldsymbol\delta})$, and resets $\boldsymbol\delta$ to zero: exactly this lesson's "solve for an increment, then retract". In the back end of visual SLAM (g2o, GTSAM, Ceres) every pose node carries its own $\oplus$, and the factor-graph Jacobians are blocks like $-R\skew{\mathbf p}$ above (Lesson 6.9). IMU preintegration (Forster et al.) uses the right Jacobian to propagate a correction of the gyro bias into the preintegrated rotation.

6常见误区Common pitfalls

误区 1：左右扰动的公式混着抄Pitfall 1: mixing left- and right-perturbation formulas

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

The Jacobian of $R\mathbf p$ is $-R\skew{\mathbf p}$ for a right perturbation and $-\skew{R\mathbf p}$ for a left one. When copying formulas from a paper or another library, check which side it uses. Covariances too: whether they describe a left or a right error must match the Jacobians (Lesson 5.9). Mixing them rarely crashes; the solver just converges slowly, or the filter quietly becomes inconsistent.

误区 2：对矩阵元素或欧拉角求导Pitfall 2: differentiating with respect to matrix entries or Euler angles

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

Differentiating with respect to the nine entries gives a $3\times9$ Jacobian with six redundant degrees of freedom, and stepping along it leaves $\SO$. Differentiating with respect to Euler angles makes the Jacobian lose rank near pitch $\pm90^\circ$, and the optimizer thrashes. The perturbation $\boldsymbol\delta$ is always three numbers, and at $\boldsymbol\delta = 0$ it is a local coordinate with no singularity: its main advantage.

误区 3：把 Exp(φ+δ) 当成 Exp(φ)Exp(δ)Pitfall 3: treating Exp(φ+δ) as Exp(φ)Exp(δ)

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

It holds only when $\boldsymbol\phi$ is small or $\boldsymbol\delta\parallel\boldsymbol\phi$. Either correct with $J_r$, or avoid storing the state as a rotation vector in the first place: keep $R$ (or a quaternion) and let $\boldsymbol\delta$ be only a local increment. The latter is what FIG 4 does, and what most estimators do.

误区 4：用"加完再归一化"代替回缩Pitfall 4: "add, then renormalize" instead of retracting

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

Doing an SVD projection after $R + R\skew{\boldsymbol\delta}$ does give a valid rotation matrix, but only an inexact approximation of $R\,\Exp(\boldsymbol\delta)$, visibly off when $\boldsymbol\delta$ is large. Retracting with $R\,\Exp(\boldsymbol\delta)$ costs just one Rodrigues evaluation and stays exactly on the group.

误区 5：SE(3) 雅可比里 twist 分量顺序对不上Pitfall 5: twist component order in SE(3) Jacobians

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

With this lesson's $\boldsymbol\delta = (\boldsymbol\delta_\omega,\boldsymbol\delta_v)$ the right-perturbation Jacobian is $[-R\skew{\mathbf p}\;\;R]$. Libraries that put translation first (see Lesson 5.6) need $[R\;\;-R\skew{\mathbf p}]$. With the columns in the wrong order, the solved pose increment mixes rotation and translation.

7小练习Check yourself

1. 机体系中的固定点 $\mathbf p$，世界位置 $R\mathbf p$。它对右扰动 $R\,\Exp(\boldsymbol\delta)$ 的雅可比是？1. A fixed body-frame point $\mathbf p$ has world position $R\mathbf p$. What is its Jacobian with respect to a right perturbation $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}$ 一般不相等。$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$. Option C is the left-perturbation Jacobian. Note $-\skew{R\mathbf p} = -R\skew{\mathbf p}R\T$: the two differ by one $R$, which is $\boldsymbol\delta_l = R\boldsymbol\delta_r$. In option D, $\skew{\mathbf p}R$ and $R\skew{\mathbf p}$ are generally different matrices.

2. $R$ 是绕 z 轴转 90°。一个右扰动 $\boldsymbol\delta_r = (\varepsilon,0,0)$（绕机体 x 轴），等价于哪个左扰动 $\boldsymbol\delta_l$？2. $R$ is a 90° rotation about z. A right perturbation $\boldsymbol\delta_r = (\varepsilon,0,0)$ (about the body x-axis) is equivalent to which left perturbation $\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 轴转"。$\boldsymbol\delta_l = R\boldsymbol\delta_r$. A 90° turn about z sends $\hat x$ to $\hat y$, so $\boldsymbol\delta_l = (0,\varepsilon,0)$. Intuitively, the body's x-axis now points along the world's y-axis, so "turn about the body x" is "turn about the world y".

被忽略的是二阶项，$\propto\|\boldsymbol\delta\|^2$。FIG 2 的双对数图里误差曲线的斜率就是 2。What is dropped is the second-order term, $\propto\|\boldsymbol\delta\|^2$. The error curve in FIG 2's log-log plot has slope 2.

4. 优化器求出切空间增量 $\boldsymbol\delta$ 以后，为什么要用 $R\leftarrow R\,\Exp(\boldsymbol\delta)$ 更新，而不是 $R\leftarrow R+R\skew{\boldsymbol\delta}$？4. After the optimizer solves for a tangent-space increment $\boldsymbol\delta$, why update with $R\leftarrow R\,\Exp(\boldsymbol\delta)$ rather than $R\leftarrow R+R\skew{\boldsymbol\delta}$?

加法的结果 $R(I+\skew{\boldsymbol\delta})$ 的各列长度变成 $\sqrt{1+\|\boldsymbol\delta\|^2-\delta_i^2}$ 之类，不再是旋转。$\Exp$ 稍贵，但每步只多算一次 Rodrigues 公式。The sum $R(I+\skew{\boldsymbol\delta})$ has columns of length about $\sqrt{1+\|\boldsymbol\delta\|^2-\delta_i^2}$, so it is no longer a rotation. $\Exp$ costs slightly more, one Rodrigues evaluation per step.

5. 位姿 $T=(R,\mathbf t)$，twist 顺序 $(\boldsymbol\omega,\mathbf v)$。机体系点 $\mathbf p$ 的世界位置 $T\mathbf p$ 对右扰动 $(\boldsymbol\delta_\omega,\boldsymbol\delta_v)$ 的 $3\times6$ 雅可比是？5. For a pose $T=(R,\mathbf t)$ with twist order $(\boldsymbol\omega,\mathbf v)$, what is the $3\times6$ Jacobian of the world position $T\mathbf p$ of a body-frame point $\mathbf p$ with respect to a right perturbation $(\boldsymbol\delta_\omega,\boldsymbol\delta_v)$?

$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 是把顺序写反了的左扰动。$T\,\Exp(\boldsymbol\delta)\mathbf p\approx R(\mathbf p+\boldsymbol\delta_\omega\times\mathbf p+\boldsymbol\delta_v)+\mathbf t$, so the rotation columns are $-R\skew{\mathbf p}$ and the translation columns are $R$ (the translation increment is read in the body frame, so it needs $R$ to reach the world frame). Option C is the left-perturbation $[-\skew{T\mathbf p}\;\;I]$ with $T\mathbf p$ mistyped as $R\mathbf p$, a common slip; option A is the left Jacobian with the column order reversed.

8延伸阅读Further reading

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

函数Function	右扰动 \(R\,\Exp(\boldsymbol\delta)\)Right \(R\,\Exp(\boldsymbol\delta)\)	左扰动 \(\Exp(\boldsymbol\delta)\,R\)Left \(\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)\)

1为什么机器人需要它Why robots need this

2直觉：站在 R 上，往哪边推？Intuition: standing at R, which way do you push?

3动手玩：一阶近似与雅可比Play: first-order approximation and the Jacobian

Exp 本身的导数：右雅可比 \(J_r\)The derivative of Exp itself: the right Jacobian \(J_r\)

4再看数学The math

⊕ 与回缩 (retraction)⊕ and retraction

流形上的导数Derivatives on a manifold

左右雅可比与 BCHLeft and right Jacobians, and BCH

SE(3) 上同理The same on SE(3)

5机器人中的它：Gauss-Newton 估计姿态In robotics: Gauss-Newton attitude estimation

6常见误区Common pitfalls

7小练习Check yourself

8延伸阅读Further reading