学视觉 SLAM 时，很多人能分别说出旋转矩阵、四元数、李群李代数和高斯—牛顿，却始终不明白：这些内容为什么会同时出现在位姿优化里？

这篇文章只解决一条主线：

机器人如何表示“我在哪、朝哪看”，又如何根据观测误差，一点一点修正自己的位姿？

读完后，你应该能够把下面这条链完整讲通：

$$R,t,T⟶SO(3),SE(3)⟶\Delta \xi ⟶J⟶\text{GN/LM}⟶T\leftarrow \exp (\Delta {\xi }^{\wedge })T$$

适合读者： 正在学习《视觉 SLAM 十四讲》第 3、4、6 章，或者已经见过这些名词、却还没有形成整体联系的读者。
阅读目标： 建立“姿态表示 → 位姿微调 → 非线性优化”的整体观，而不是孤立地背公式。
系列说明： 本文是「SLAM 系列」第一篇，后续将通过代码和具体问题继续深入。如有错误，欢迎在评论区指正。

文章摘要： 本文从坐标变换和姿态表示出发，依次串联旋转矩阵、欧拉角、旋转向量、四元数、$SO(3)$、$SE(3)$、李代数扰动、非线性最小二乘、高斯—牛顿法与 LM，最后通过重投影误差说明一次完整的 SLAM 位姿优化是如何发生的。

关键词：视觉SLAM 位姿优化 李群李代数 四元数 高斯牛顿 非线性优化

目录

• 0. 一篇讲清的三层主线
• 第一部分：位姿与姿态表示
  • 1. 位姿 = 位置 + 姿态
  • 2. 坐标变换的约定：最容易翻车的点
  • 3. 旋转矩阵：三列就是三个新坐标轴
  • 4. 欧拉角：最像人话，但必须说明规则
  • 5. 轴角与旋转向量
  • 6. 四元数：工程中常用的旋转表示
  • 7. 变换矩阵与齐次坐标
  • 8. 四种姿态表示对比
• 第二部分：位姿微调——李群与李代数
  • 9. 为什么不能直接给旋转矩阵"加小量"
  • 10. 群、李群、李代数
  • 11. so(3) 为什么是反对称矩阵
  • 12. 指数映射 / 对数映射
  • 13. 左扰动与右扰动
• 第三部分：非线性优化
  • 14. 从观测残差到最小二乘
  • 15. 高斯牛顿法：手算一次你就懂了
  • 16. LM：给高斯牛顿加刹车
  • 17. 四种优化方法对比
  • 18. 后端闭环：从一个像素误差到位姿修正
• 第四部分：12 条高频误区与公式自检法
• 总结：最小记忆卡片

---

0. 一篇讲清的三层主线

先不要把下面的内容看成三座互不相干的公式山。它们围绕同一个问题逐层展开：

机器人怎样表示自己在哪里、朝哪里；观测有误差时，又怎样一点一点修正这个位姿？

第一步：姿态表示
  "位姿是什么，怎样表示？"
    ├── 位置：平移向量 t
    └── 姿态：旋转矩阵 R / 欧拉角 / 轴角 / 四元数
          │
          ▼
第二步：位姿微调（李群与李代数）
  "旋转矩阵有约束，不方便直接加减；怎样给位姿做一个小修改？"
    ├── 位姿所在空间：SO(3)、SE(3)（李群）
    └── 小增量所在空间：so(3)、se(3)（李代数）
          │
          ▼
第三步：非线性优化
  "到底应该修改多少，才能让预测和观测更一致？"
    ├── 定义残差与代价函数
    ├── 用雅可比做局部线性近似
    ├── 高斯—牛顿 / LM 求增量 Δξ
    └── 用 exp(Δξ^) 更新位姿，继续迭代

SLAM 后端优化的总纲，可以压缩成一个框：

$$\boxed{\text{位姿 }T\stackrel{\text{观测产生残差}}{\to }\underset{\xi }{\min }\frac{1}{2}\sum _i\Vert e_i(\xi ){\Vert }^2\stackrel{\text{求增量}}{\to }T\leftarrow \exp (\Delta {\xi }^{\wedge })T}$$

后面所有内容，都是在解释这个框里的每一个符号。

---

第一部分：位姿与姿态表示

1. 位姿 = 位置 + 姿态

假设桌上有一台小机器人。世界坐标系记为 $W$，机器人自身坐标系记为 $B$。描述机器人状态需要两件事：

1. 机器人原点在世界中的位置——平移 $\mathbf{t}$；
2. 机器人三个自身坐标轴在世界中指向哪里——旋转 $\mathbf{R}$。

合起来称为位姿（pose）：$\text{pose}=\text{position}+\text{orientation}$。

姿态只有 3 个自由度，却有多种表示：旋转矩阵 9 个数、欧拉角 3 个数、轴角 3～4 个数、单位四元数 4 个数。表示所用数字的个数，不等于真正的自由度——这是第一个要破除的直觉。

2. 坐标变换的约定：最容易翻车的点

同一个物理点，在世界系中是 ${\mathbf{p}}_W$，在机器人系中是 ${\mathbf{p}}_B$，两者通过旋转加平移联系：

$${\mathbf{p}}_W={\mathbf{R}}_{WB}{\mathbf{p}}_B+{\mathbf{t}}_{WB}$$

约定本文采用：${\mathbf{T}}_{WB}$ 表示"把 B 坐标系中的坐标变换成 W 坐标系中的坐标"。

读成人话：

1. ${\mathbf{R}}_{WB}{\mathbf{p}}_B$：先把机器人坐标轴下的方向转到世界坐标轴下；
2. 再加 ${\mathbf{t}}_{WB}$：补上机器人原点相对世界原点的位置。

逆变换：

$${\mathbf{p}}_B={\mathbf{R}}_{WB}^{\mathsf{T}}({\mathbf{p}}_W-{\mathbf{t}}_{WB}),{\mathbf{R}}_{BW}={\mathbf{R}}_{WB}^{\mathsf{T}},{\mathbf{t}}_{BW}=-{\mathbf{R}}_{WB}^{\mathsf{T}}{\mathbf{t}}_{WB}$$

**易错点：**有些论文或代码把下标方向定义反了。不要只凭字母猜，先查作者定义，再用一个简单点代入验证。工程上"坐标系对不上"是 SLAM 新手第一大事故源。

3. 旋转矩阵：三列就是三个新坐标轴

二维旋转热身：

$$\mathbf{R}(\theta )=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]$$

三维绕坐标轴的主动旋转（右手定则）：

$${\mathbf{R}}_x(\alpha )=\left[\begin{array}{ccc}1& 0& 0\\ 0& \cos \alpha & -\sin \alpha \\ 0& \sin \alpha & \cos \alpha \end{array}\right],{\mathbf{R}}_y(\beta )=\left[\begin{array}{ccc}\cos \beta & 0& \sin \beta \\ 0& 1& 0\\ -\sin \beta & 0& \cos \beta \end{array}\right],{\mathbf{R}}_z(\gamma )=\left[\begin{array}{ccc}\cos \gamma & -\sin \gamma & 0\\ \sin \gamma & \cos \gamma & 0\\ 0& 0& 1\end{array}\right]$$

最有用的一个观察：把三个单位轴 ${\mathbf{e}}_1,{\mathbf{e}}_2,{\mathbf{e}}_3$ 分别乘进 $\mathbf{R}$，恰好取出 $\mathbf{R}$ 的三列。所以在 ${\mathbf{R}}_{WB}$ 约定下：

旋转矩阵的三列，就是旋转后 B 系三个坐标轴在 W 系中的坐标。

这比死记 9 个元素有用得多。

旋转矩阵的两个约束（来自"旋转不拉伸、不镜像"）：

$${\mathbf{R}}^{\mathsf{T}}\mathbf{R}=\mathbf{I},\det (\mathbf{R})=1$$

由此立即得到 ${\mathbf{R}}^{-1}={\mathbf{R}}^{\mathsf{T}}$。虽然 9 个数，但 6 个约束限制出 3 个自由度。

旋转复合从右往左读：先 ${\mathbf{R}}_1$ 再 ${\mathbf{R}}_2$ 写成 ${\mathbf{R}}_2{\mathbf{R}}_1$。三维旋转不可交换——先点头再转身 ≠ 先转身再点头。

4. 欧拉角：最像人话，但必须说明规则

欧拉角用三次绕轴旋转描述姿态。航空中常见：roll（滚转）、pitch（俯仰）、yaw（偏航）。一种常见约定：

$$\mathbf{R}={\mathbf{R}}_z(\psi ){\mathbf{R}}_y(\theta ){\mathbf{R}}_x(\varphi )$$

只说"欧拉角是 (10°, 20°, 30°)"并不完整，还必须说明：轴顺序（ZYX？）、绕固定世界轴（外旋）还是随物体转动的自身轴（内旋）、手性、输出排列。

**万向锁（gimbal lock）**的正确理解：pitch 转到 ±90° 时，两个旋转轴重合，两项角度不再能独立区分。物体依然有 3 个自由度；坏掉的是这套坐标参数化，就像经纬度在北极附近失效，不代表地球表面在那里少了一维。所以欧拉角适合人机交互和姿态展示，但通常不作为通用旋转插值或无奇异优化的首选参数化。

5. 轴角与旋转向量

欧拉旋转定理：任意三维旋转等价于绕单位轴 $\mathbf{n}$ 转角度 $\theta$，记作 $(\mathbf{n},\theta )$。把二者合成一个三维向量：

$$\mathbf{\varphi }=\theta \mathbf{n}$$

叫旋转向量：方向是旋转轴，长度是旋转角。

帽运算（叉乘变矩阵乘法，理解李代数的一把钥匙）：

$${\mathbf{a}}^{\wedge }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right],{\mathbf{a}}^{\wedge }\mathbf{b}=\mathbf{a}\times \mathbf{b}$$

罗德里格斯公式（轴角 → 旋转矩阵）：

$$\mathbf{R}=\exp ({\mathbf{\varphi }}^{\wedge })=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{n}{\mathbf{n}}^{\mathsf{T}}+\sin \theta {\mathbf{n}}^{\wedge }$$

反解（矩阵 → 轴角）：

$$\theta =\arccos \left(\frac{\mathrm{tr}(\mathbf{R})-1}{2}\right)$$

注意轴角不唯一：$(\mathbf{n},\theta )\sim (-\mathbf{n},-\theta )$，且角度加减 $2\pi$ 表示同一旋转。

6. 四元数：工程中常用的旋转表示

$$\mathbf{q}=w+x\mathbf{i}+y\mathbf{j}+z\mathbf{k},i^2=j^2=k^2=ijk=-1$$

表示纯旋转要求单位长度：$w^2+x^2+y^2+z^2=1$。

轴角 → 四元数（最核心的公式）：

$$\mathbf{q}=\left[\begin{array}{c}\cos (\theta /2)\\ \mathbf{n}\sin (\theta /2)\end{array}\right]$$

用四元数旋转向量（$p^{\prime }=qp{q}^{-1}$，单位四元数 $q^{-1}=q^{\ast }=(w,-\mathbf{v})$）：

$${\mathbf{p}}_q^{\prime }=\mathbf{q}{\mathbf{p}}_q{\mathbf{q}}^{-1}$$

双覆盖：$\mathbf{q}$ 和 $-\mathbf{q}$ 表示同一个旋转（代入上式负号抵消）。物理转一圈 360°，四元数在单位球上走 720° 才回到自己。换来的是：无万向锁 + 可平滑插值（SLERP）。

工程中为什么常用四元数：4 个数比矩阵紧凑，不存在欧拉角式的万向锁，复合高效且适合插值。但要注意保持单位范数，且优化时通常用"四元数存状态、李代数增量做更新"。

7. 变换矩阵与齐次坐标

普通线性变换必须把零向量映射到零向量，而平移会移动原点，所以 $p^{\prime }=Rp+t$ 是仿射变换，不是三维线性变换。给点补一个恒为 1 的维度，就能把旋转和平移统一成一次矩阵乘法：

$$\left[\begin{array}{c}{\mathbf{p}}^{\prime }\\ 1\end{array}\right]=\underset{\mathbf{T}}{\underbrace{\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^{\mathsf{T}}& 1\end{array}\right]}}\left[\begin{array}{c}\mathbf{p}\\ 1\end{array}\right]$$

$\mathbf{T}$ 称为齐次变换矩阵。方向向量末位写 0（$[{\mathbf{d}}^{\mathsf{T}},0{]}^{\mathsf{T}}$），平移不会改变方向。

复合：${\mathbf{T}}_{WB}={\mathbf{T}}_{WA}{\mathbf{T}}_{AB}$，下标像链条一样"消掉"中间的 A。

逆：

$${\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^{\mathsf{T}}& -{\mathbf{R}}^{\mathsf{T}}\mathbf{t}\\ 0^{\mathsf{T}}& 1\end{array}\right]$$

直觉：先撤销平移、再撤销旋转；逆平移最终表现为 $-{\mathbf{R}}^{\mathsf{T}}\mathbf{t}$ 而不是 $-\mathbf{t}$。

8. 四种姿态表示对比

表示	存储数字	自由度	优点	缺点	常见用途
旋转矩阵 R	9	3	变换向量直接、几何意义清楚	6 个约束、存储多	坐标变换、推导
欧拉角	3	3	直观、适合人读	顺序歧义、万向锁、不宜插值	UI、航空角显示
旋转向量 φ	3	3	紧凑、连接李代数	大角度多值/分支	优化增量、指数映射
单位四元数 q	4	3	紧凑稳定、适合复合和插值	单位约束、q 与 -q 等价、不直观	工程存储、插值

没有永远最好的表示。工程实践通常是：矩阵用于推导和作用于点，四元数用于存储与插值，李代数三维增量用于优化。

---

第二部分：位姿微调——李群与李代数

9. 为什么不能直接给旋转矩阵"加小量"

普通向量可以直接更新：${\mathbf{x}}_{new}=\mathbf{x}+\Delta \mathbf{x}$。但旋转矩阵不行：

$${\mathbf{R}}_{new}\stackrel{?}{=}\mathbf{R}+\Delta \mathbf{R}$$

随便相加后通常不再满足 ${\mathbf{R}}^{\mathsf{T}}\mathbf{R}=\mathbf{I}$、$\det (\mathbf{R})=1$，就不再是合法旋转。

我们需要一种方法：

1. 用普通向量表达"很小的旋转修改"；
2. 能对这个向量做求导和线性求解；
3. 修改后仍严格落回合法旋转集合。

这正是李群与李代数在 SLAM 中的核心作用。

地球表面类比：地球表面是弯曲的。不能沿穿地直线走，但在脚下很小的范围内地面近似平面——可以用"向东 2 米、向北 1 米"的二维向量规划一步，再把它落实到球面。弯曲的合法位姿空间 = 李群；当前点附近的平坦小增量空间 = 李代数（切空间）；从小增量走回合法空间 = 指数映射。

10. 群、李群、李代数

群 = 集合 + 二元运算，满足：封闭性、结合律、存在单位元、每个元素有逆。旋转矩阵以矩阵乘法为运算构成群（复合还是旋转、单位阵是不转、转置是逆）。

李群 = 既是群，又是光滑流形（元素可连续变化，局部能做微积分）。$SO(3)$、$SE(3)$ 都是李群。

李代数 = 李群在单位元处的切空间。对 SLAM 最重要的直觉：

李群存放完整的旋转/位姿，李代数存放它们的局部小增量。

S O ( 3 ) = { R ∈ R 3 × 3 ∣ R T R = I ,   det ⁡ ( R ) = 1 } SO(3)=\{\mathbf R\in\mathbb R^{3\times3}\mid \mathbf R^{\mathsf T}\mathbf R=\mathbf I,\ \det(\mathbf R)=1\} SO(3)={R∈R3×3∣RTR=I, det(R)=1}

$$SE(3)=\left\{\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^{\mathsf{T}}& 1\end{array}\right]|\mathbf{R}\in SO(3),\mathbf{t}\in {\mathbb{R}}^3\right\}$$

$SE(3)$ 有 6 个自由度（3 平移 + 3 旋转），它不是 $SO(3)\times {\mathbb{R}}^3$ 的直积，而是半直积——旋转会作用在平移上：

$$({\mathbf{R}}_1,{\mathbf{t}}_1)({\mathbf{R}}_2,{\mathbf{t}}_2)=({\mathbf{R}}_1{\mathbf{R}}_2,{\mathbf{R}}_1{\mathbf{t}}_2+{\mathbf{t}}_1)$$

11. so(3) 为什么是反对称矩阵

设从单位旋转出发的平滑曲线 $\mathbf{R}(t)$，满足 $\mathbf{R}(t{)}^{\mathsf{T}}\mathbf{R}(t)=\mathbf{I}$、$\mathbf{R}(0)=\mathbf{I}$。对 $t$ 求导：

$${\dot{\mathbf{R}}}^{\mathsf{T}}\mathbf{R}+{\mathbf{R}}^{\mathsf{T}}\dot{\mathbf{R}}=0$$

在 $t=0$ 处 $\mathbf{R}=\mathbf{I}$，得到 $\dot{\mathbf{R}}(0{)}^{\mathsf{T}}+\dot{\mathbf{R}}(0)=0$——单位元附近的切向量必须是反对称矩阵。而任意 $3\times 3$ 反对称矩阵都可由一个三维向量的帽运算表示：

s o ( 3 ) = { ϕ ∧ ∣ ϕ ∈ R 3 } so(3)=\{\boldsymbol\phi^\wedge\mid\boldsymbol\phi\in\mathbb R^3\} so(3)={ϕ∧∣ϕ∈R3}

因此 $so(3)$ 实质上只需 3 个数。$se(3)$ 用 6 个数（本文采用平移在前、旋转在后）：

$$\mathbf{\xi }=\left[\begin{array}{c}\mathbf{\rho }\\ \mathbf{\varphi }\end{array}\right]\in {\mathbb{R}}^6,{\mathbf{\xi }}^{\wedge }=\left[\begin{array}{cc}{\mathbf{\varphi }}^{\wedge }& \mathbf{\rho }\\ 0^{\mathsf{T}}& 0\end{array}\right]$$

**注意：**有的库把旋转放前、平移放后（$[\varphi ,\rho ]$）。两者都可以，但雅可比列顺序也会随之改变，绝不能混用。

12. 指数映射 / 对数映射

矩阵指数定义为幂级数：

$$\exp (\mathbf{A})=\mathbf{I}+\mathbf{A}+\frac{1}{2!}{\mathbf{A}}^2+\frac{1}{3!}{\mathbf{A}}^3+\cdots$$

对 $SO(3)$：

$$\mathbf{R}=\exp ({\mathbf{\varphi }}^{\wedge })$$

展开并利用反对称矩阵性质，就得到罗德里格斯公式。所以"轴角转旋转矩阵"和"so(3) 指数映射到 SO(3)"是同一件事的两种说法。小角度一阶近似：

$$\exp ({\mathbf{\varphi }}^{\wedge })\approx \mathbf{I}+{\mathbf{\varphi }}^{\wedge }$$

对数映射（旋转拉回局部向量空间）：

$${\mathbf{\varphi }}^{\wedge }=\log (\mathbf{R}),\theta =\arccos \left(\frac{\mathrm{tr}(\mathbf{R})-1}{2}\right)$$

$SE(3)$ 的指数映射：

$$\exp ({\mathbf{\xi }}^{\wedge })=\left[\begin{array}{cc}\exp ({\mathbf{\varphi }}^{\wedge })& \mathbf{V}\mathbf{\rho }\\ 0^{\mathsf{T}}& 1\end{array}\right],\mathbf{V}=\mathbf{I}+\frac{1-\cos \theta }{{\theta }^2}{\mathbf{\varphi }}^{\wedge }+\frac{\theta -\sin \theta }{{\theta }^3}({\mathbf{\varphi }}^{\wedge }{)}^2$$

注意右上角是 $\mathbf{V}\mathbf{\rho }$ 而不是 $\mathbf{\rho }$——因为刚体在有限时间内一边旋转一边平移，瞬时平移方向也在变化。旋转很小时 $\mathbf{V}\approx \mathbf{I}+\frac{1}{2}{\mathbf{\varphi }}^{\wedge }$，退化为纯平移。

BCH 公式提醒我们：李代数虽像向量空间，但多个有限增量复合时不能直接相加：

$$\log (\exp (\mathbf{A})\exp (\mathbf{B}))=\mathbf{A}+\mathbf{B}+\frac{1}{2}[\mathbf{A},\mathbf{B}]+\cdots$$

13. 左扰动与右扰动

设当前估计为 $\mathbf{T}$，小增量为 $\Delta \mathbf{\xi }$。两种合法更新：

左扰动（增量在世界/输出一侧）：

$${\mathbf{T}}^{\prime }=\exp (\Delta {\mathbf{\xi }}^{\wedge })\mathbf{T}$$

右扰动（增量在机体/输入一侧）：

$${\mathbf{T}}^{\prime }=\mathbf{T}\exp (\Delta {\mathbf{\xi }}^{\wedge })$$

两者都正确，但雅可比形式不同，通过伴随矩阵转换：

$${\mathrm{Ad}}_{\mathbf{T}}=\left[\begin{array}{cc}\mathbf{R}& {\mathbf{t}}^{\wedge }\mathbf{R}\\ 0& \mathbf{R}\end{array}\right]$$

**工程规则：**先确定变换方向、残差正负、李代数排列和左/右更新，再推雅可比。不要从另一份资料只抄半条公式。

为什么优化喜欢李代数：优化器每轮解一个普通线性方程得到 6 维增量 $\Delta \mathbf{\xi }$，更新时 $T\leftarrow \exp (\Delta {\mathbf{\xi }}^{\wedge })T$。好处：① 最小参数化（6 自由度就用 6 个增量）；② 局部可微（普通雅可比 + 线性代数）；③ 更新后合法（指数映射自动产生 $SE(3)$ 元素，不必强行正交化）。

---

第三部分：非线性优化

14. 从观测残差到最小二乘

观测有噪声，不同观测往往不能被某个位姿同时百分百满足。优化不是找"让每个观测零误差"的魔法，而是找一个总体上最合理的折中：

$${\mathbf{x}}^{\ast }=\arg \underset{\mathbf{x}}{\min }F(\mathbf{x})$$

对第 $i$ 个观测，残差（实际测量减模型预测）：

$${\mathbf{e}}_i(\mathbf{x})={\mathbf{z}}_i-h_i(\mathbf{x})$$

最小二乘代价（前面的 $\frac{1}{2}$ 只是为求导抵消平方的 2）：

$$F(\mathbf{x})=\frac{1}{2}\sum _i\Vert {\mathbf{e}}_i(\mathbf{x}){\Vert }^2$$

为什么平方：正负误差不抵消；大误差受更强惩罚；函数光滑可导；高斯噪声假设下对应最大似然估计。

加权最小二乘：不同测量可信度不同，用信息矩阵 ${\mathbf{\Omega }}_i={\mathbf{\Sigma }}_i^{-1}$ 加权：

$$F(\mathbf{x})=\frac{1}{2}\sum _i{\mathbf{e}}_i^{\mathsf{T}}{\mathbf{\Omega }}_i{\mathbf{e}}_i$$

方差越小、越可信，权重越大。鲁棒核（如 Huber）限制异常值破坏力，但不能替代前端剔除外点。

15. 高斯牛顿法：手算一次你就懂了

把全部残差堆成向量 $\mathbf{e}(\mathbf{x})$，在当前估计附近一阶展开：

$$\mathbf{e}(\mathbf{x}+\Delta \mathbf{x})\approx \mathbf{e}+\mathbf{J}\Delta \mathbf{x}$$

代入代价并对 $\Delta \mathbf{x}$ 求导置零，得到高斯—牛顿正规方程：

$$\boxed{{\mathbf{J}}^{\mathsf{T}}\mathbf{J}\Delta \mathbf{x}=-{\mathbf{J}}^{\mathsf{T}}\mathbf{e}}$$

记 ${\mathbf{H}}_{GN}={\mathbf{J}}^{\mathsf{T}}\mathbf{J}$、$\mathbf{g}={\mathbf{J}}^{\mathsf{T}}\mathbf{e}$，更新 $\mathbf{x}\leftarrow \mathbf{x}+\Delta \mathbf{x}$（位姿则用指数映射更新）。

为什么 $J^{\mathsf{T}}J$ 近似 Hessian：精确 Hessian 是 $J^{\mathsf{T}}J+{\sum }_i{e}_i{\nabla }^2{e}_i$，GN 丢掉第二项——残差小或接近线性时近似很好，且省掉二阶导。

数值实现不要显式求逆：$\Delta \mathbf{x}=-(J^{\mathsf{T}}J{)}^{-1}{J}^{\mathsf{T}}\mathbf{e}$ 看着干净，但代码里用 Cholesky / QR / 稀疏分解解方程，更快更稳。

手算三次迭代：求 $\sqrt{2}$。把 $x^2=2$ 写成最小二乘：$e(x)=x^2-2$，$J(x)=2x$。GN 增量：

$$\Delta x=-\frac{e}{J}=-\frac{x^2-2}{2x}$$

从 $x_0=1$ 开始：

迭代 $k$	$x_k$	残差 $e=x^2-2$	增量 $\Delta x$
0	1.000000	-1.000000	+0.500000
1	1.500000	+0.250000	-0.083333
2	1.416667	+0.006944	-0.002451
3	1.414216	约 $6\times 10^{-6}$	—

真实值 $\sqrt{2}\approx 1.414214$。这就是非线性优化的核心循环：当前点求局部斜率 → 预测走多少能消除残差 → 更新 → 重新线性化。

16. LM：给高斯牛顿加刹车

GN 假设局部线性模型可信，但离正确答案较远时步子可能过大。LM 在正规方程里加阻尼：

$$({\mathbf{J}}^{\mathsf{T}}\mathbf{J}+\lambda \mathbf{I})\Delta \mathbf{x}=-{\mathbf{J}}^{\mathsf{T}}\mathbf{e}$$

$\lambda$ 的直觉：

• $\lambda$ 很小 → 接近 GN，大步快走；
• $\lambda$ 很大 → 步小，方向接近负梯度，保守。

LM 比较"局部模型预测下降了多少"和"真实代价下降了多少"：新步好就接受并减小 $\lambda$，不好就拒绝并增大 $\lambda$。可理解为在相信二次模型与谨慎试探之间自适应切换（信赖域视角）。

17. 四种优化方法对比

方法	典型方程/方向	使用信息	优点	主要问题
梯度下降	$\Delta x=-\alpha g$	一阶梯度	简单、每步便宜	步长难选，狭长谷中慢
牛顿法	$H\Delta x=-g$	精确二阶 Hessian	近最优点收敛快	二阶导昂贵，H 可能不正定
高斯—牛顿	$J^{T}J\Delta x=-J^{T}e$	一阶雅可比	不需残差二阶导，SLAM 常用	坏初值可能步子过激
LM	$(J^{T}J+\lambda I)\Delta x=-J^{T}e$	雅可比 + 自适应阻尼	通常比 GN 稳健	策略复杂，可能多次试步

18. 后端闭环：从一个像素误差到位姿修正

世界点经位姿变换到相机系，透视投影得到预测像素，与实际观测比较得到残差，对"位姿小增量"求雅可比，GN/LM 解出增量，再用指数映射更新位姿——然后重新投影、重新迭代：

真实世界点 P_W
      │  姿态变换：R·P_W + t
      ▼
P_C = R·P_W + t
      │  相机投影模型
      ▼
预测像素 ẑ = π(P_C)
      │  与实际观测比较（残差）
      ▼
残差 e = z - ẑ
      │  对位姿小增量求雅可比
      ▼
J = ∂e/∂Δξ
      │  高斯—牛顿 / LM
      ▼
(JᵀJ + λI)·Δξ = -Jᵀe
      │  指数映射更新（回到合法位姿）
      ▼
T ← exp(Δξ^)·T
      └──────────── 重新投影、重新迭代 ────────────┐
      ◄───────────────────────────────────────────┘

位姿小扰动下相机点的一阶变化（左扰动）：

$${\mathbf{P}}_C^{\prime }\approx {\mathbf{P}}_C+\Delta \mathbf{\rho }-{\mathbf{P}}_C^{\wedge }\Delta \mathbf{\varphi }\Rightarrow \frac{\partial {\mathbf{P}}_C}{\partial \Delta \mathbf{\xi }}={\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{P}}_C^{\wedge }\end{array}\right]}_{3\times 6}$$

投影对三维点的雅可比：

$$\frac{\partial \pi }{\partial {\mathbf{P}}_C}={\left[\begin{array}{ccc}{f}_x/Z& 0& -f_{x}X/Z^2\\ 0& f_y/Z& -f_{y}Y/Z^2\end{array}\right]}_{2\times 3}$$

残差雅可比（注意残差定义的负号）：

$${\mathbf{J}}_e=-\frac{\partial \pi }{\partial {\mathbf{P}}_C}\left[\begin{array}{cc}\mathbf{I}& -{\mathbf{P}}_C^{\wedge }\end{array}\right]$$

这就是 SLAM 后端最核心的闭环。 实际在 Ceres / g2o / slam_toolbox 这类优化框架里，你定义误差函数和雅可比，求解器帮你迭代。

---

第四部分：12 条高频误区与公式自检法

高频误区

1. “旋转矩阵有 9 个自由度”——错。9 个数，正交 + 行列式约束后只有 3 自由度。
2. “欧拉角就是唯一的三个角”——错。必须连同顺序、内旋/外旋、轴定义一起说明。
3. “万向锁说明物体少了一个自由度”——错。少的是参数化在奇异点的独立性，不是物理空间。
4. “四元数有 4 个自由度”——错。单位范数约束后是 3；且 q 与 -q 同一旋转。
5. “李代数只能表示小角度”——不准确。指数映射可表示有限旋转；优化强调"小"是一阶线性化只在局部可靠。
6. “exp(ξ₁)exp(ξ₂)=exp(ξ₁+ξ₂)”——一般错误，BCH 还有交换子项。
7. “左扰动右扰动雅可比可以照搬”——错，需用伴随关系转换。
8. “残差必须是观测减预测”——没有唯一规定，关键是雅可比和代码全程一致。
9. “高斯—牛顿每轮一定让代价下降”——错。线性模型不可靠，LM/线搜索/信赖域用于增强稳健性。
10. “Hessian 不可逆就是程序 bug”——未必。可能是观测不足、几何退化或未固定规范自由度。
11. “最小二乘能自动处理错误匹配”——错。普通平方损失会放大外点影响，需 RANSAC、卡方门限、鲁棒核。
12. “把 H⁻¹ 算出来再乘是标准实现”——不推荐。用矩阵分解解 $H\Delta x=-g$。

公式出错五步自检法

1. 查维度：投影残差 2 维、位姿增量 6 维 → 雅可比必须 $2\times 6$。
2. 代单位变换：$\mathbf{R}=\mathbf{I},\mathbf{t}=0$ 时变换应保持点不变。
3. 代纯平移/纯旋转：只开一个变量，判断方向是否符合直觉。
4. 检查下标链：${\mathbf{T}}_{WA}{\mathbf{T}}_{AB}={\mathbf{T}}_{WB}$，中间下标接不上则方向有误。
5. 用有限差分验雅可比：比较 $\mathbf{J}\Delta \mathbf{x}$ 与 $\mathbf{e}(\mathbf{x}\oplus \Delta \mathbf{x})-\mathbf{e}(\mathbf{x})$，步长足够小时应接近。

---

总结：最小记忆卡片

姿态表示

$${\mathbf{p}}_A={\mathbf{R}}_{AB}{\mathbf{p}}_B+{\mathbf{t}}_{AB},{\mathbf{R}}^{\mathsf{T}}\mathbf{R}=\mathbf{I},\det \mathbf{R}=1,{\mathbf{R}}^{-1}={\mathbf{R}}^{\mathsf{T}}$$

$$\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0& 1\end{array}\right],{\mathbf{T}}^{-1}=\left[\begin{array}{cc}{\mathbf{R}}^{\mathsf{T}}& -{\mathbf{R}}^{\mathsf{T}}\mathbf{t}\\ 0& 1\end{array}\right]$$

位姿微调

$${\mathbf{a}}^{\wedge }\mathbf{b}=\mathbf{a}\times \mathbf{b},\mathbf{R}=\exp ({\mathbf{\varphi }}^{\wedge }),\mathbf{T}=\exp ({\mathbf{\xi }}^{\wedge })$$

$$\mathbf{T}\leftarrow \exp (\Delta {\mathbf{\xi }}^{\wedge })\mathbf{T}$$

非线性优化

$$F(\mathbf{x})=\frac{1}{2}\sum _i\Vert {\mathbf{e}}_i(\mathbf{x}){\Vert }^2,\mathbf{e}(\mathbf{x}+\Delta \mathbf{x})\approx \mathbf{e}+\mathbf{J}\Delta \mathbf{x}$$

$$\underset{\mathbf{H}}{\underbrace{{\mathbf{J}}^{\mathsf{T}}\mathbf{J}}}\Delta \mathbf{x}=-\underset{\mathbf{g}}{\underbrace{{\mathbf{J}}^{\mathsf{T}}\mathbf{e}}},(\mathbf{H}+\lambda \mathbf{I})\Delta \mathbf{x}=-\mathbf{g}\text{（LM）}$$

一句话总结：

姿态表示告诉你"机器人位姿是什么"；李群与李代数告诉你"怎样合法地微调位姿"；非线性优化告诉你"根据观测误差，这次究竟应该微调多少"。

---