Part I · 第 6 章优化 · 6.10Part I · Ch. 6 Optimization · 6.10

流形上的优化Optimization on Manifolds

旋转矩阵和位姿不是向量：把梯度步长直接加上去，就会离开 $\SO$ 和 $\SE$。流形上的优化只有一个想法：在当前点的切空间里（那里是普通向量空间）迈一步，再用指数映射把结果"放回"流形。它让位姿图、旋转平均、扫描配准在不用欧拉角、不用约束的情况下，都能用上 Gauss-Newton 和梯度下降。 Rotation matrices and poses are not vectors: add a gradient step to one and you fall off $\SO$ or $\SE$. Optimization on manifolds has one idea: take the step in the tangent space of the current point, where ordinary vector arithmetic works, then use the exponential map to put the result back on the manifold. It lets pose graphs, rotation averaging and scan alignment use Gauss-Newton and gradient descent without Euler angles and without constraints.

约 30 分钟~30 min 前置：5.6 指数映射与对数映射 · 5.7 扰动与流形上的求导 · 6.3 梯度下降 · 6.6 Gauss-NewtonPrereqs: 5.6 Exp & Log maps · 5.7 Perturbations · 6.3 Gradient descent · 6.6 Gauss-Newton 4 个交互4 interactives

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

机器人估计的东西，一大半不住在 $\R^n$ 里：相机姿态是 $\SO$，刚体位姿是 $\SE$，四元数是单位球面 $S^3$，法向量是 $S^2$。6.3、6.6 节的优化器都假定 $\x\leftarrow\x+\boldsymbol\delta$ 有意义。对旋转矩阵 $R$，$R+\Delta$ 通常不再是旋转矩阵（5.4 节）；对单位向量，$\x+\boldsymbol\delta$ 的长度不再是 1。

常见的"土办法"有三种：用欧拉角当变量（遇到万向节锁就失效）；把 $R$ 的 9 个元素当自由变量再加正交约束（变量多、约束难）；每步之后用 SVD 把矩阵"拉回"最近的旋转（能用，但丢掉了几何结构，也不知道该怎么求导）。流形优化给出第四种、也是最干净的办法：只在切空间里线性化和更新，再用 $\Exp$ 回到流形。三个自由度的旋转就用三个数的增量，没有冗余，没有奇异，也没有约束。

这一招在机器人里无处不在：SLAM 里每个位姿的更新、视觉惯性里程计（VIO）里每个姿态状态的更新、扫描匹配 ICP 里每次迭代的刚体变换、旋转平均与外参标定，背后都是它。

More than half of what a robot estimates does not live in $\R^n$: camera attitude is in $\SO$, a rigid pose in $\SE$, a quaternion on the unit sphere $S^3$, a surface normal on $S^2$. The optimizers of Lessons 6.3 and 6.6 assume $\x\leftarrow\x+\boldsymbol\delta$ makes sense. For a rotation matrix $R$, $R+\Delta$ is usually not a rotation (Lesson 5.4); for a unit vector, $\x+\boldsymbol\delta$ no longer has length 1.

There are three common hacks: use Euler angles as variables (which fail at gimbal lock); treat the 9 entries of $R$ as free variables plus orthogonality constraints (many variables, hard constraints); or snap the matrix back to the nearest rotation with an SVD after each step (it works, but throws away the geometry and does not tell you how to differentiate). Manifold optimization offers a fourth, cleanest way: linearize and update only in the tangent space, then return to the manifold with $\Exp$. A 3-degree-of-freedom rotation gets a 3-number increment: no redundancy, no singularity, no constraints.

The trick is everywhere in robotics: every pose update in SLAM, every attitude-state update in visual-inertial odometry (VIO), every rigid-transform iteration in ICP scan matching, rotation averaging and extrinsic calibration are built on it.

2直觉：在圆上取平均Intuition: averaging on a circle

最小的"弯曲空间"是圆 $S^1$，也就是平面旋转 $\mathrm{SO}(2)$。设圆上有几个角度 $\theta_1,\dots,\theta_n$，求它们的"平均"。不能直接用算术平均（想想 $359^\circ$ 与 $1^\circ$），要把"平均"理解成最小化到各点的距离平方和 $\sum_i\tfrac12 d(\theta,\theta_i)^2$，这里 $d$ 是沿圆弧的测地距离。

流形优化的一次迭代分三步：①在当前估计 $\theta$ 处画出切线（切空间）；②用对数映射把每个数据点"展开"到切线上，得到一个普通的实数 $s_i=\operatorname{Log}_\theta(\theta_i)$（沿圆弧走过的有向弧长）；③在切线上做普通平均 $\bar s=\tfrac1n\sum s_i$，再用指数映射沿圆弧走 $\bar s$，回到圆上。它就是梯度下降（$-\nabla f=\sum s_i$），只是"加法"换成了"沿测地线走"。

对比另一种做法：先把各点当作平面向量求平均（圆内的质心），再把结果投影回圆上。两者回答的是两个不同的问题（第二个最小化弦长，而不是弧长），一般给出不同的答案。

The smallest "curved space" is the circle $S^1$, i.e. planar rotations $\mathrm{SO}(2)$. Given angles $\theta_1,\dots,\theta_n$ on it, find their "mean". The arithmetic mean will not do (think $359^\circ$ and $1^\circ$); we must read "mean" as the minimizer of the sum of squared distances $\sum_i\tfrac12 d(\theta,\theta_i)^2$, where $d$ is the geodesic distance along the arc.

One iteration of manifold optimization has three steps: (1) draw the tangent line (tangent space) at the current estimate $\theta$; (2) use the logarithm map to "unroll" each data point onto that line, giving an ordinary real number $s_i=\operatorname{Log}_\theta(\theta_i)$ (the signed arc length); (3) take an ordinary average $\bar s=\tfrac1n\sum s_i$ on the line, then use the exponential map to walk $\bar s$ along the arc and land back on the circle. This is gradient descent ($-\nabla f=\sum s_i$) with "addition" replaced by "walk along a geodesic".

Compare another recipe: average the points as plane vectors (the centroid inside the circle) and then project the result back onto the circle. The two answer different questions (the second minimizes chord length rather than arc length) and in general give different answers.

FIG 1圆上的平均：切线、对数、指数Averaging on a circle: tangent line, Log, Exp

紫色点是数据 $\theta_i$，橙色点是当前估计 $\theta$，三者都可拖动。青色直线是 $\theta$ 处的切线，紫色小点是各数据点的 $\operatorname{Log}_\theta$（沿圆弧的有向弧长，被"展开"到切线上）。橙色箭头是它们的平均；沿圆弧走过去（Exp）得到橙色实心点，沿切线走再投影（归一化）得到空心点。灰色 $\times$ 是平面质心，绿色是它投影回圆的"弦平均"。按"迭代一步"看估计如何走向测地平均（在圆上 $\operatorname{Log}$ 保持弧长，所以只要没有数据点越过 $\theta$ 的对点，一步就正好落在测地平均上）。注意圆上的测地代价可以有好几个局部极小，走到哪个取决于估计的起点。 Purple dots are the data $\theta_i$ and the amber dot is the current estimate $\theta$; all are draggable. The cyan straight line is the tangent at $\theta$; the small purple dots on it are each datum's $\operatorname{Log}_\theta$ (signed arc length, "unrolled" onto the tangent). The amber arrow is their average; walking along the arc (Exp) gives the filled amber dot, walking along the tangent and projecting (normalizing) gives the hollow one. The grey $\times$ is the planar centroid and the green dot is its projection back to the circle, the "chordal mean". Press "Iterate once" to watch the estimate move to the geodesic mean (on a circle $\operatorname{Log}$ preserves arc length, so unless a datum crosses the point opposite $\theta$, one step lands exactly on the geodesic mean). Note the geodesic cost on a circle can have several local minima; which one you reach depends on where the estimate starts.

按"迭代一步"：一般一步就落到测地平均上（若有数据点越过对点，则要多走几步）；再按一次，确认箭头已缩到零、估计不再移动。Press "Iterate once": usually one step lands on the geodesic mean (a few more if a datum wraps past the opposite point); press again to confirm the arrow has shrunk to zero and the estimate no longer moves.
拖动数据点，让"测地平均"和"弦平均"相差超过 $8^\circ$。数据点越不均匀，差别越大。Drag the data so the geodesic mean and the chordal mean differ by more than $8^\circ$. The more uneven the data, the bigger the gap.
把两个数据点拖到几乎相对的位置（相差接近 $180^\circ$），再拖动估计：测地平均变得"不稳定"，这是流形上平均的固有限制。Drag two data points nearly opposite each other (about $180^\circ$ apart) and move the estimate around: the geodesic mean becomes unstable, an inherent limit of averaging on manifolds.

核心洞见Key insight

切空间是流形在一点附近的"平直近似"，在这里加法、平均、线性方程组都可以放心使用。流形上的优化 = 在切空间里算，用 Exp 回到流形。这个"Exp"叫作回缩 (retraction)：它不必是严格的测地线，只要一阶上与测地线一致、且结果落在流形上即可。归一化（FIG 1 的空心点）也是合法的回缩。

The tangent space is the "flat approximation" of the manifold near a point, where addition, averaging and linear systems are safe to use. Optimization on a manifold = compute in the tangent space, return with Exp. This "Exp" is called a retraction: it need not be an exact geodesic, only agree with one to first order and land on the manifold. Normalization (the hollow dot in FIG 1) is also a valid retraction.

3黎曼梯度：把梯度投到切面上The Riemannian gradient: project the gradient onto the tangent plane

升到二维曲面：单位球面 $S^2=\{\x:\|\x\|=1\}$。在球面上最小化 $f(\x)=\tfrac12\x^\top A\x$（$A$ 是对称正定矩阵）。答案是 $A$ 的最小特征向量（1.11 节），也是 PCA 里"最不重要"的方向。

函数在 $\R^3$ 里的梯度 $\nabla f=A\x$ 一般不在切平面内：它的一部分指向球外或球内，这部分只会让你离开球面，对"沿球面怎样下降最快"毫无贡献。留下来的才是有用的方向：切平面上的投影 $\operatorname{grad}f=(I-\x\x^\top)\nabla f$，叫黎曼梯度。沿 $-\operatorname{grad}f$ 迈一步（切平面内的向量），再用 $\Exp$（沿大圆走）或归一化回到球面。

Up to a two-dimensional surface: the unit sphere $S^2=\{\x:\|\x\|=1\}$. Minimize $f(\x)=\tfrac12\x^\top A\x$ on it, with $A$ symmetric positive definite. The answer is the smallest eigenvector of $A$ (Lesson 1.11), the "least important" direction in PCA.

The gradient of the function in $\R^3$, $\nabla f=A\x$, generally does not lie in the tangent plane: part of it points out of or into the sphere, and that part can only make you leave the sphere without telling you how to descend along it. What remains is the useful direction: its projection onto the tangent plane, $\operatorname{grad}f=(I-\x\x^\top)\nabla f$, the Riemannian gradient. Step along $-\operatorname{grad}f$ (a vector in the tangent plane), then return to the sphere with $\Exp$ (walk along a great circle) or by normalizing.

FIG 2球面上的黎曼梯度下降Riemannian gradient descent on a sphere

拖动画面旋转视角drag to orbit

灰色细线是 $A$ 的三条特征轴（特征值 3、1.2、0.3：最小的是极小点，最大的是极大点，中间那个是鞍点）。橙色点是当前 $\x$；橙色箭头是欧氏梯度 $A\x$，紫色箭头是它在切平面上的投影，即黎曼梯度；半透明圆片是切平面。红点是"不管约束，直接 $\x-\eta\nabla f$"落到的位置，它掉出了球面；蓝点是回缩之后的新迭代点。 The thin neutral-grey lines are the three eigen-axes of $A$ (eigenvalues 3, 1.2, 0.3: the smallest is the minimizer, the largest the maximizer, the middle one a saddle). The amber dot is the current $\x$; the amber arrow is the Euclidean gradient $A\x$ and the purple arrow its projection onto the tangent plane, the Riemannian gradient; the translucent disc is the tangent plane. The red dot is where "ignore the constraint and take $\x-\eta\nabla f$" lands, which falls off the sphere; the blue dot is the new iterate after retraction.

按"运行"，看迭代点沿球面滑向最小特征向量。Press "Run" and watch the iterate slide along the sphere toward the smallest eigenvector.
按"从鞍点出发"再运行：黎曼梯度在鞍点处为零，迭代点要先"犹豫"一阵（起点故意偏离鞍点 $10^{-3}$，这点偏差每步放大，直到把它推开），然后才滑向最小点。Press "Start at saddle" and run: the Riemannian gradient vanishes at a saddle, so the iterate hesitates for a while (the start is deliberately offset $10^{-3}$ from the saddle, and that offset grows each step until it pushes the iterate off) before sliding to the minimizer.

4旋转平均：Lie 代数增量对欧拉角Rotation averaging: Lie-algebra increments vs. Euler angles

现在换成机器人真正关心的 $\SO$。旋转平均：对一组带噪声的旋转 $R_1,\dots,R_n$（多个陀螺仪、多次手眼标定、多路姿态估计的输出），求使测地距离平方和最小的 $R$：

Now the $\SO$ that robots really care about. Rotation averaging: given noisy rotations $R_1,\dots,R_n$ (several gyros, repeated hand-eye calibrations, outputs of multiple attitude estimators), find the $R$ minimizing the sum of squared geodesic distances:

$$ f(R)=\tfrac12\sum_{i=1}^n \big\|\Log(R^\top R_i)\big\|^2 $$

$\Log(R^\top R_i)$ 正是"从 $R$ 转到 $R_i$ 的旋转向量"，也就是 FIG 1 里的 $s_i$ 在 3D 里的版本。代价对 $R$ 的黎曼梯度恰好是 $-\sum_i\Log(R^\top R_i)$（$\SO$ 上的度量是双不变的，所以没有额外系数）。于是整个迭代就是：

$\Log(R^\top R_i)$ is "the rotation vector taking $R$ to $R_i$", the 3D version of the $s_i$ in FIG 1. The Riemannian gradient of the cost at $R$ is exactly $-\sum_i\Log(R^\top R_i)$ (the metric on $\SO$ is bi-invariant, so no extra factors). The whole iteration is therefore:

$$ \hl{lie}{\boldsymbol\omega} = \frac{\eta}{n}\sum_i \Log(R_k^\top R_i), \qquad R_{k+1}=R_k\,\hl{exp}{\Exp(\boldsymbol\omega)} $$

对比欧拉角：把 $R=R_z(\psi)R_y(\vartheta)R_x(\phi)$ 的三个角当作变量，对 $f$ 做普通梯度下降。它三个变量、没有约束，看上去很完美，缺点是参数化本身有奇点：当俯仰角 $\vartheta\to\pm90^\circ$（万向节锁），偏航和滚转绕同一根轴转，参数到旋转的映射的雅可比降秩，于是代价对其中一个方向几乎"没有梯度"，下降极慢甚至停滞。下面两栏用同一组数据、同一个起点，左边用欧拉角，右边用 Lie 代数增量。数据的真值俯仰角默认是 $86^\circ$，接近万向节锁。

Compare Euler angles: take the three angles of $R=R_z(\psi)R_y(\vartheta)R_x(\phi)$ as variables and run plain gradient descent on $f$. Three variables, no constraints, looks perfect. The flaw is the parametrization itself has singularities: when pitch $\vartheta\to\pm90^\circ$ (gimbal lock), yaw and roll rotate about the same axis, the Jacobian from parameters to rotation loses rank, and the cost has almost "no gradient" along one direction, so descent is extremely slow or stalls. The two panels below use the same data and the same start: Euler angles on the left, Lie-algebra increments on the right. The data's true pitch is $86^\circ$ by default, close to gimbal lock.

FIG 3旋转平均：欧拉角 vs. 流形Rotation averaging: Euler angles vs. manifold

欧拉角 + 梯度下降Euler angles + gradient descent

流形：R ← R·Exp(ω)manifold: R ← R·Exp(ω)

单位球面上的小点是 5 个数据旋转的三个坐标轴 $R_i\hat{\imath},R_i\hat{\jmath},R_i\hat{k}$ 的端点（红、绿、蓝）；粗箭头是当前估计的坐标系；金色轨迹是 $\hat k$ 轴端点走过的路。下图：两种方法的代价与最优代价之差（对数坐标）。欧拉角一侧用中心差分求梯度、固定步长 $\gamma$；流形一侧用 $\eta=1$，即直接取对数的平均。 The small dots on the unit sphere are the tips of the three axes $R_i\hat{\imath},R_i\hat{\jmath},R_i\hat{k}$ of the five data rotations (red, green, blue); the bold arrows are the current estimate's frame; the golden trail is the path of the $\hat k$ tip. Below: each method's cost minus the optimal cost (log scale). The Euler side uses central-difference gradients with a fixed step $\gamma$; the manifold side uses $\eta=1$, i.e. directly the average of the logarithms.

保持默认的真值俯仰角 $86^\circ$，按"运行"：欧拉角一侧停在一个平台上，流形一侧几步就收敛。Keep the default true pitch $86^\circ$ and press "Run": the Euler side stalls on a plateau while the manifold side converges in a few steps.
把真值俯仰角拖到 $0^\circ$（远离万向节锁）再运行：欧拉角一侧也能收敛，只是更慢。奇点才是问题所在。Drag the true pitch to $0^\circ$ (far from gimbal lock) and run again: the Euler side converges too, only slower. The singularity is the culprit.

5再看数学The math

提示：把鼠标悬停在公式中带底色的符号上，图中对应的元素会被高亮（增量 $\boldsymbol\omega$：FIG 1 的"平均 Log"箭头；指数回缩：FIG 3 的估计坐标系；欧氏梯度与黎曼梯度：FIG 2 的两支箭头）。Tip: hover the highlighted symbols in the formulas; the matching elements light up (the increment $\boldsymbol\omega$: the "mean Log" arrow in FIG 1; the exponential retraction: the estimate's frame in FIG 3; the Euclidean and Riemannian gradients: the two arrows in FIG 2).

流形、切空间与回缩Manifolds, tangent spaces and retractions

光滑流形 $\mathcal M$ 在每点 $x$ 有一个切空间 $T_x\mathcal M$，它是与 $\mathcal M$ 的维数相同的向量空间。$\SO$ 的维数是 3，$S^2$ 是 2，$\SE$ 是 6。回缩 $\mathcal R_x:T_x\mathcal M\to\mathcal M$ 满足 $\mathcal R_x(\mathbf 0)=x$，并且在 0 处的导数是恒等映射（一阶上与"沿切向走"一致）。李群上最自然的回缩是指数映射（5.6 节）：

A smooth manifold $\mathcal M$ has at each point $x$ a tangent space $T_x\mathcal M$, a vector space of the same dimension as $\mathcal M$: 3 for $\SO$, 2 for $S^2$, 6 for $\SE$. A retraction $\mathcal R_x:T_x\mathcal M\to\mathcal M$ satisfies $\mathcal R_x(\mathbf 0)=x$, and its derivative at 0 is the identity (to first order it agrees with "move along the tangent"). On a Lie group the most natural retraction is the exponential map (Lesson 5.6):

$$ R\ \leftarrow\ R\,\Exp(\boldsymbol\omega)\ \ (\text{right/body perturbation}),\qquad T\leftarrow \Exp(\boldsymbol\xi)\,T\ \ (\text{left/world perturbation}),\qquad \x\leftarrow\frac{\x+\mathbf v}{\|\x+\mathbf v\|}\ \ (S^2) $$

用哪一侧的扰动是自由的，但求导和更新必须用同一侧（5.7 节）。FIG 3 用右扰动，FIG 4 用左扰动。

Which side to perturb on is a free choice, but differentiation and update must use the same side (Lesson 5.7). FIG 3 uses a right perturbation, FIG 4 a left one.

黎曼梯度The Riemannian gradient

对嵌在 $\R^m$ 中的流形（如球面），黎曼梯度是欧氏梯度在切空间上的正交投影；对李群，则把"扰动后的函数"在 0 处求导：

For a manifold embedded in $\R^m$ (like the sphere), the Riemannian gradient is the orthogonal projection of the Euclidean gradient onto the tangent space; for a Lie group, differentiate the "perturbed function" at 0:

$$ \hl{riem}{\operatorname{grad}f(\x)}=\big(I-\x\x^\top\big)\hl{euc}{\nabla f(\x)}\ \ (S^2),\qquad \operatorname{grad}f(R)=\frac{\partial f\big(R\,\Exp(\boldsymbol\omega)\big)}{\partial\boldsymbol\omega}\Big|_{\boldsymbol\omega=\mathbf 0}\in\R^3\ \ (\SO) $$

于是梯度下降变成 $R_{k+1}=R_k\Exp(-\eta\operatorname{grad}f)$。Gauss-Newton 也一样：残差对扰动线性化 $\mathbf r(R\Exp(\boldsymbol\omega))\approx\mathbf r+J\boldsymbol\omega$，解 $J^\top J\boldsymbol\omega=-J^\top\mathbf r$ 得到 3 维（$\SE$ 则是 6 维）增量，再回缩。法方程的大小等于流形的维数，而不是矩阵元素的个数：100 个 $\SE$ 位姿的位姿图，固定第一个之后，正好是 $6\times99$ 个未知数。

Gradient descent becomes $R_{k+1}=R_k\Exp(-\eta\operatorname{grad}f)$. Gauss-Newton is the same: linearize the residual in the perturbation, $\mathbf r(R\Exp(\boldsymbol\omega))\approx\mathbf r+J\boldsymbol\omega$, solve $J^\top J\boldsymbol\omega=-J^\top\mathbf r$ for a 3-vector (6-vector on $\SE$) increment, then retract. The normal equations are as large as the manifold's dimension, not the number of matrix entries: a pose graph of 100 $\SE$ poses, after fixing the first, has exactly $6\times99$ unknowns.

旋转平均的推导Deriving the rotation average

取 $R$ 附近的一点 $R\Exp(\boldsymbol\omega)$，则 $\Log\big((R\Exp(\boldsymbol\omega))^\top R_i\big)=\Log\big(\Exp(-\boldsymbol\omega)R^\top R_i\big)$。把 $\mathbf e_i=\Log(R^\top R_i)$ 记作误差，一阶近似为 $\mathbf e_i-J_l^{-1}(\mathbf e_i)\boldsymbol\omega$（$J_l$ 是 5.7 节的左雅可比），而 $J_l^{-1}(\mathbf e)^\top\mathbf e=\mathbf e$（因为 $J_l(\mathbf e)\mathbf e=\mathbf e$），所以代价对 $\boldsymbol\omega$ 的梯度是 $-\sum_i\mathbf e_i$，不必显式算雅可比。令它为零，得 Karcher 平均的条件 $\sum_i\Log(R^\top R_i)=\mathbf 0$。

Take a point $R\Exp(\boldsymbol\omega)$ near $R$, so $\Log\big((R\Exp(\boldsymbol\omega))^\top R_i\big)=\Log\big(\Exp(-\boldsymbol\omega)R^\top R_i\big)$. Writing the error $\mathbf e_i=\Log(R^\top R_i)$, the first-order approximation is $\mathbf e_i-J_l^{-1}(\mathbf e_i)\boldsymbol\omega$ ($J_l$ is the left Jacobian of Lesson 5.7), and $J_l^{-1}(\mathbf e)^\top\mathbf e=\mathbf e$ (because $J_l(\mathbf e)\mathbf e=\mathbf e$), so the gradient of the cost with respect to $\boldsymbol\omega$ is $-\sum_i\mathbf e_i$ with no explicit Jacobian needed. Setting it to zero gives the Karcher-mean condition $\sum_i\Log(R^\top R_i)=\mathbf 0$.

SE(2) 上的刚体对齐Rigid alignment on SE(2)

设位姿 $T=(R(\theta),\mathbf t)$，点 $\mathbf p_i$ 变换后 $\mathbf a_i=T\mathbf p_i$，对应目标点 $\mathbf q_i$，残差 $\mathbf r_i=\mathbf a_i-\mathbf q_i$。对左扰动 $\boldsymbol\delta=(\boldsymbol\rho,\varphi)$：$\Exp(\boldsymbol\delta)\,\mathbf a\approx\mathbf a+\boldsymbol\rho+\varphi\,\mathbf a^{\perp}$，其中 $\mathbf a^\perp=(-a_y,a_x)$。所以每个点的雅可比是 $J_i=[\,I_2\ \ \mathbf a_i^\perp\,]$（$2\times3$），解 $\sum_iJ_i^\top J_i\,\boldsymbol\delta=-\sum_iJ_i^\top\mathbf r_i$，再更新 $T\leftarrow\Exp(\boldsymbol\delta)T$，其中

Let the pose be $T=(R(\theta),\mathbf t)$, a point $\mathbf p_i$ maps to $\mathbf a_i=T\mathbf p_i$, its target is $\mathbf q_i$, and the residual is $\mathbf r_i=\mathbf a_i-\mathbf q_i$. For a left perturbation $\boldsymbol\delta=(\boldsymbol\rho,\varphi)$: $\Exp(\boldsymbol\delta)\,\mathbf a\approx\mathbf a+\boldsymbol\rho+\varphi\,\mathbf a^{\perp}$, where $\mathbf a^\perp=(-a_y,a_x)$. So each point's Jacobian is $J_i=[\,I_2\ \ \mathbf a_i^\perp\,]$ ($2\times3$); solve $\sum_iJ_i^\top J_i\,\boldsymbol\delta=-\sum_iJ_i^\top\mathbf r_i$, then update $T\leftarrow\Exp(\boldsymbol\delta)T$, where

$$ \Exp(\boldsymbol\rho,\varphi)=\Big(R(\varphi),\ V(\varphi)\boldsymbol\rho\Big),\qquad V(\varphi)=\frac1\varphi\begin{bmatrix}\sin\varphi & -(1-\cos\varphi)\\ 1-\cos\varphi & \sin\varphi\end{bmatrix} $$

这就是 FIG 4 里每一步迭代做的事。点云配准（ICP）还要在每步重新找最近邻对应，这让问题只在局部是"最小二乘"，所以对初值敏感。

That is exactly what each iteration of FIG 4 does. Point-cloud registration (ICP) also re-finds nearest-neighbour correspondences every step, which makes the problem a least-squares problem only locally, hence the sensitivity to the initial guess.

6机器人中的它：激光扫描匹配In robotics: lidar scan matching

移动机器人每扫一帧激光，都要把新扫描 $\{\mathbf p_i\}$（在机器人坐标系里）对齐到已有的地图 $\{\mathbf q_j\}$，求出机器人在地图里的位姿。这就是 ICP：重复"找最近点对应 → 解一次 $\SE$ 上的 Gauss-Newton → 用 $\Exp$ 更新位姿"。下图的地图是一个带墙角的房间，扫描是同一个房间换了位姿（旋转 $17^\circ$、平移约 1 m）。拖动橙色手柄设定初始位姿猜测（圆点是平移，小圈是朝向）。

Each time a mobile robot takes a lidar scan, it must align the new scan $\{\mathbf p_i\}$ (in the robot frame) with the existing map $\{\mathbf q_j\}$ to get the robot's pose in the map. This is ICP: repeat "find nearest-point correspondences, solve one Gauss-Newton step on $\SE$, update the pose with $\Exp$". The map below is a room with corners; the scan is the same room seen from another pose (rotated $17^\circ$, shifted about 1 m). Drag the amber handles to set the initial pose guess (dot = translation, small ring = heading).

FIG 4SE(2) 上的扫描匹配 (ICP)Scan matching (ICP) on SE(2)

蓝点是地图，橙点是用当前位姿变换后的扫描，淡线是当前对应。"最近邻"是真正的 ICP；"已知对应"假定我们知道每个扫描点对应哪个地图点（一个更容易的、近似凸的问题）。"位姿误差"是用当前位姿与真实位姿变换同一批点后的均方根距离。每一步的更新都是 $T\leftarrow\Exp(\boldsymbol\delta)T$。 Blue dots are the map, amber dots the scan transformed by the current pose, and faint lines the current correspondences. "Nearest neighbour" is real ICP; "Known" assumes we know which map point each scan point matches (an easier, nearly convex problem). "Pose error" is the root-mean-square distance between the same points transformed by the current and the true pose. Every update is $T\leftarrow\Exp(\boldsymbol\delta)T$.

用默认的"最近邻"，从离真值不远的起点出发，按"运行"让扫描贴合地图（位姿误差小于 0.05）。With the default "nearest neighbour" and a start not too far from the truth, press "Run" until the scan sits on the map (pose error below 0.05).
把初始朝向转到偏离真值 $80^\circ$ 以上再运行：ICP 最后停在一个错误的位姿（陷入局部极小，或越漂越远）。Rotate the initial heading more than $80^\circ$ away from the truth and run: ICP ends at a wrong pose (stuck in a local minimum or drifting away).
在同一个糟糕的起点上切到"已知对应"：几步就收敛到正确位姿。问题难在对应，而不是 $\SE$ 本身。From the same bad start, switch to "Known": it converges to the right pose in a few steps. The difficulty is the correspondences, not $\SE$ itself.

真实系统里In real systems

3D 里同样的流程在 $\SE$ 上进行（6 维增量）。VIO 的 IMU 预积分、位姿图里的每个位姿、相机外参标定，也是"在切空间里解法方程，用 $\Exp$ 更新"。成熟库已经把这套封装好了：GTSAM、Sophus + Ceres（用 local parameterization / manifold）、Pinocchio 等都提供流形上的变量类型，让优化器本身不必知道变量是矩阵、四元数还是位姿。

In 3D the same procedure runs on $\SE$ (6-dimensional increments). IMU preintegration in VIO, every pose in a pose graph, and camera extrinsic calibration all follow "solve the normal equations in the tangent space, update with $\Exp$". Mature libraries package this: GTSAM, Sophus with Ceres (local parameterizations / manifolds), Pinocchio and others provide manifold variable types, so the optimizer itself never needs to know whether a variable is a matrix, a quaternion or a pose.

7常见误区Common pitfalls

误区 1：求导用左扰动，更新却用右乘Pitfall 1: differentiating with a left perturbation but updating on the right

$R\Exp(\boldsymbol\omega)$ 与 $\Exp(\boldsymbol\omega)R$ 相差一个伴随 $\mathrm{Ad}_R$（5.8 节）。雅可比按左扰动推导、更新却写成 $R\leftarrow R\Exp(\boldsymbol\omega)$，程序不会报错，只是迭代变慢、不收敛，或收敛到不是最小值的点。推导时把"扰动在哪一侧"写在纸上，代码里也用同一侧；用数值微分 $\big(f(R\Exp(h\mathbf e_k))-f(R)\big)/h$ 检查解析雅可比。

$R\Exp(\boldsymbol\omega)$ and $\Exp(\boldsymbol\omega)R$ differ by an adjoint $\mathrm{Ad}_R$ (Lesson 5.8). Derive the Jacobian with a left perturbation but update with $R\leftarrow R\Exp(\boldsymbol\omega)$ and the program will not complain: it just slows down, fails to converge, or converges to a point that is not a minimum. Write down on which side the perturbation lives and use the same side in code; check analytic Jacobians against numerical differences $\big(f(R\Exp(h\mathbf e_k))-f(R)\big)/h$.

误区 2：把旋转向量的增量直接"加"到旋转向量上Pitfall 2: adding increments directly to a rotation vector

以 $\boldsymbol\phi=\Log(R)$ 为变量，更新 $\boldsymbol\phi\leftarrow\boldsymbol\phi+\boldsymbol\delta$，看起来无约束且无冗余。问题有两个：一是 $\|\boldsymbol\phi\|$ 在 $\pi$ 处有奇点和卷绕，二是 $\Exp(\boldsymbol\phi+\boldsymbol\delta)\ne\Exp(\boldsymbol\phi)\Exp(\boldsymbol\delta)$，增量的物理意义随当前 $\boldsymbol\phi$ 而变（雅可比 $J_l(\boldsymbol\phi)$ 介入）。流形优化的做法是每次迭代都在当前点重新建一个以 0 为中心的局部坐标，所以增量永远很小，永远远离奇点。

Take $\boldsymbol\phi=\Log(R)$ as the variable and update $\boldsymbol\phi\leftarrow\boldsymbol\phi+\boldsymbol\delta$: unconstrained and non-redundant, it seems. Two problems: $\|\boldsymbol\phi\|$ has a singularity and wrapping at $\pi$, and $\Exp(\boldsymbol\phi+\boldsymbol\delta)\ne\Exp(\boldsymbol\phi)\Exp(\boldsymbol\delta)$, so the meaning of an increment changes with the current $\boldsymbol\phi$ (the Jacobian $J_l(\boldsymbol\phi)$ intervenes). Manifold optimization rebuilds a local coordinate chart centered at 0 around the current point on every iteration, so increments are always small and always far from singularities.

误区 3：忽视流形上的"最远点"和多个极小Pitfall 3: ignoring antipodal points and multiple minima on manifolds

FIG 1 里，取两个靠得很近的数据点（如 $170^\circ$ 与 $-170^\circ$），把估计放在它们中点的对面（$0^\circ$）：两边的 $\operatorname{Log}$ 恰好抵消，梯度为零，但那里是最大值而不是最小值（见第 3 题）。若两个数据点正好相对，测地平均则不唯一（两个极小点，各距两点 $90^\circ$）。$\SO$ 上若数据散布超过大约 $90^\circ$，Karcher 平均可能不唯一、迭代依赖初值。旋转平均实践中先求一个好的初值（沿生成树链式相乘，或为了鲁棒用谱方法 / 弦距松弛），再做局部流形优化。

In FIG 1, take two data points close together (e.g. $170^\circ$ and $-170^\circ$) and put the estimate opposite their midpoint ($0^\circ$): the two $\operatorname{Log}$s cancel exactly, giving zero gradient, yet it is a maximum rather than a minimum (see Question 3). If the two data points are exactly antipodal, the geodesic mean is not unique (two minima, each $90^\circ$ from both points). On $\SO$, if the data spread beyond about $90^\circ$ the Karcher mean may be non-unique and the iteration depends on the initial value. In practice rotation averaging starts from a good initial guess (chaining along a spanning tree, or spectral/chordal relaxation for robustness) and only then runs local manifold optimization.

误区 4：忘了固定规范，或让 $R$ 慢慢漂离正交Pitfall 4: forgetting gauge, or letting $R$ drift away from orthogonality

只含相对测量的问题有整体刚体变换的规范自由度（6.9 节）：要么固定一个变量，要么给法方程加一点阻尼。另外，若把 $R$ 存成 3×3 矩阵并反复相乘，浮点误差会让它慢慢偏离正交。每隔若干步做一次重新正交化（SVD 或四元数归一化），否则 $\Log$ 会返回错误的角度。

Problems with only relative measurements have the global rigid-motion gauge freedom (Lesson 6.9): fix a variable or add a little damping to the normal equations. Also, if $R$ is stored as a 3×3 matrix and multiplied repeatedly, floating-point error slowly drives it off orthogonality. Re-orthonormalize every so often (SVD or quaternion normalization), or $\Log$ will return wrong angles.

8小练习Check yourself

1. 在单位球面上，当前点 $\x=(0,0,1)$，函数的欧氏梯度 $\nabla f=(1,0,2)$。黎曼梯度是？1. On the unit sphere, the current point is $\x=(0,0,1)$ and the function's Euclidean gradient is $\nabla f=(1,0,2)$. What is the Riemannian gradient?

$(I-\x\x^\top)\nabla f=\nabla f-(\x^\top\nabla f)\x=(1,0,2)-2\cdot(0,0,1)=(1,0,0)$。去掉的 $(0,0,2)$ 是沿球面法向的分量，对沿球面下降没有作用。$(I-\x\x^\top)\nabla f=\nabla f-(\x^\top\nabla f)\x=(1,0,2)-2\cdot(0,0,1)=(1,0,0)$. The removed $(0,0,2)$ is the component along the sphere's normal and does nothing for descent along the sphere.

2. 用梯度下降优化旋转矩阵 $R$，为什么不能写 $R\leftarrow R-\eta\nabla f$，而要写 $R\leftarrow R\,\Exp(-\eta\,\operatorname{grad}f)$？2. When optimizing a rotation matrix $R$ by gradient descent, why write $R\leftarrow R\,\Exp(-\eta\,\operatorname{grad}f)$ rather than $R\leftarrow R-\eta\nabla f$?

$R-\eta\nabla f$ 一般不满足 $R^\top R=I$，离开了流形；$R\Exp(\boldsymbol\omega)$ 永远是旋转，而且 $\boldsymbol\omega$ 只有 3 个分量，没有冗余。$R-\eta\nabla f$ generally violates $R^\top R=I$ and falls off the manifold; $R\Exp(\boldsymbol\omega)$ is always a rotation, and $\boldsymbol\omega$ has only 3 components with no redundancy.

3. 圆上有两个数据角 $170^\circ$ 和 $-170^\circ$。估计 $\theta=0^\circ$ 处两个 $\operatorname{Log}$ 的平均是 0，梯度为零。这个点是？3. A circle has two data angles, $170^\circ$ and $-170^\circ$. At the estimate $\theta=0^\circ$ the average of the two $\operatorname{Log}$s is 0, so the gradient is zero. What kind of point is it?

在 $0^\circ$，代价是 $\tfrac12(170^2+170^2)$（度²），在 $180^\circ$ 则只有 $\tfrac12(10^2+10^2)$。$0^\circ$ 是平稳点但是最大值，所以"梯度为零"并不保证是最小值，流形上的初值很重要。At $0^\circ$ the cost is $\tfrac12(170^2+170^2)$ deg$^2$; at $180^\circ$ it is only $\tfrac12(10^2+10^2)$. $0^\circ$ is stationary but a maximum, so "zero gradient" does not imply a minimum, and initialization matters on manifolds.

4. 用欧拉角做变量的梯度下降，在俯仰角接近 $90^\circ$ 时为什么变慢甚至停滞？4. Why does gradient descent on Euler angles slow down or stall when pitch approaches $90^\circ$?

这是参数化的奇点（万向节锁），不是旋转本身的问题：在 $\SO$ 上用 $R\Exp(\boldsymbol\omega)$ 的局部坐标，每次都以当前点为原点，永远不会碰到奇点。This is a singularity of the parametrization (gimbal lock), not of the rotations themselves: with the local coordinates $R\Exp(\boldsymbol\omega)$ the origin is always the current point, so a singularity is never hit.

5. 一个含 100 个 $\SE$ 位姿的位姿图，固定第 0 号位姿后用 Gauss-Newton 优化。每次迭代的法方程有多少个未知数？5. A pose graph has 100 $\SE$ poses; pose 0 is fixed and Gauss-Newton is used. How many unknowns do the normal equations have per iteration?

$\SE$ 是 6 维流形，每个位姿的切空间增量有 6 个分量，所以是 $6\times99=594$。若把 $4\times4$ 矩阵的 12（或 16）个元素当变量，就会多出大量冗余变量和约束。$\SE$ is a 6-dimensional manifold, so each pose's tangent increment has 6 components: $6\times99=594$. Treating the 12 (or 16) entries of the $4\times4$ matrix as variables would add lots of redundant variables and constraints.

9延伸阅读Further reading

N. Boumal, An Introduction to Optimization on Smooth Manifolds, Cambridge University Press, 2023 — 流形优化的现代入门，免费在线the modern introduction to manifold optimization; free online
P.-A. Absil, R. Mahony & R. Sepulchre, Optimization Algorithms on Matrix Manifolds, Princeton University Press, 2008 — 回缩、黎曼梯度与 Newton 法的经典专著the classic monograph on retractions, Riemannian gradients and Newton methods
J. Solà, J. Deray & D. Atchuthan, A micro Lie theory for state estimation in robotics, arXiv:1812.01537, 2018 — 机器人学里 $\oplus,\ominus$、左右扰动与雅可比的实用手册a practical handbook of $\oplus,\ominus$, left/right perturbations and Jacobians for robotics
T. D. Barfoot, State Estimation for Robotics, Cambridge University Press, 2017, Ch. 7–8 — 李群上的扰动与批量非线性估计perturbations on Lie groups and batch nonlinear estimation
R. Hartley, J. Trumpf, Y. Dai & H. Li, Rotation Averaging, International Journal of Computer Vision, 2013 — 旋转平均的算法与理论综述survey of rotation-averaging algorithms and theory
P. J. Besl & N. D. McKay, A Method for Registration of 3-D Shapes, IEEE TPAMI, 1992 — ICP 原始论文the original ICP paper