前言上一讲结束时我们面对一个尴尬的局面。旋转矩阵 R 有 9 个数但只有 3 个自由度被 6 个约束锁死它不能像普通向量那样加一个小量因为 R ΔR 几乎一定不再是旋转矩阵。可是 SLAM 的后端是一个优化问题优化的基本动作就是沿着梯度方向走一小步——如果连走一小步都定义不了优化就无从谈起。这一讲要解决的就是这件事。它的核心思想可以用一句话说完旋转矩阵本身不能加但每一个旋转矩阵都对应着一个三维向量三维向量可以加我们在向量上做加法再把结果映射回旋转矩阵。这个三维向量所在的空间叫李代数旋转矩阵所在的空间叫李群两者之间的映射叫指数映射和对数映射。如果你已经读过 Solà 的两册笔记会发现本讲的内容你大部分已经见过李群就是光滑的群李代数就是单位元处的切空间指数映射就是把切空间缠到流形上。但这本书的路线和 Solà 不同——Solà 是从抽象的流形出发一步步推到 SO(3) 这个特例这本书反过来从 SO(3) 这个具体对象出发用微积分直接算出李代数是什么然后再给它命名。两条路线互为补充。本册会按书的路线走并在关键处标注它对应 Solà 的哪个概念。本讲另一个与 Solà 不同的地方是扰动的方向。书默认使用左乘扰动Solà 默认使用右乘扰动。两者都对只是选择了不同的坐标系来表示扰动量第三章会专门解释。第一章 李群与李代数基础1.1 群带一种运算的集合上一讲说 SO(3) 和 SE(3) 都是群当时没有解释。现在解释。群是一个集合 A 加上一种运算 ·满足四条性质封闭性任意两个元素运算的结果仍在集合里。对任意 a₁, a₂ ∈ A有 a₁ · a₂ ∈ A。结合律(a₁ · a₂) · a₃ a₁ · (a₂ · a₃)。幺元存在一个元素 a₀使得对任意 a 都有 a₀ · a a · a₀ a。逆对任意 a存在 a⁻¹使得 a · a⁻¹ a₀。四条性质的首字母可以记成封结幺逆。对 SO(3)运算是矩阵乘法两个旋转矩阵相乘还是旋转矩阵封闭矩阵乘法满足结合律单位阵 I 是幺元Rᵀ 是逆。对 SE(3) 同理幺元是 4×4 单位阵逆是上一讲写出的 T⁻¹。注意群不要求交换律。两个旋转先后顺序不同结果不同所以 SO(3) 不是交换群。这件事从一开始就决定了李代数上的加法只能是近似的第三章的 BCH 公式会说清楚这一点。李群是具有连续光滑性质的群。整数加法群是群但它的元素是离散的不是李群。SO(3) 和 SE(3) 的元素可以连续变化——一个刚体可以在空间中连续地转动和移动所以它们是李群。直观地说李群既是群又是流形一个局部看起来像欧氏空间的光滑曲面。1.2 李代数的引出对约束求导这一节是整讲最关键的推导书里用一页篇幅完成。请逐行跟。考虑一个随时间变化的旋转矩阵 R(t)。它在每一时刻都满足正交约束R(t) R(t)ᵀ I两边对时间求导乘积求导法则Ṙ(t) R(t)ᵀ R(t) Ṙ(t)ᵀ 0把第二项移到右边Ṙ(t) R(t)ᵀ − R(t) Ṙ(t)ᵀ − ( Ṙ(t) R(t)ᵀ )ᵀ最后一步用了 (AB)ᵀ BᵀAᵀ。这个式子说矩阵 Ṙ Rᵀ 等于它自己转置的负数——它是一个反对称矩阵。上一讲说过任何 3×3 反对称矩阵都对应一个三维向量。于是存在某个三维向量 φ(t)使得Ṙ(t) R(t)ᵀ φ(t)^两边右乘 R(t)利用 Rᵀ R IṘ(t) φ(t)^ R(t)这个式子的含义是旋转矩阵对时间的导数等于某个反对称矩阵左乘它自己。换句话说想知道 R 怎么变只需要知道一个三维向量 φ。这个 φ 在物理上就是角速度——如果你把 R(t) 看作刚体的姿态φ(t) 就是它在世界系下的瞬时角速度向量。这正是上一讲四元数运动学方程 q̇ ½ q ⊗ [0, ω] 的矩阵版本。现在设 t₀ 0 时 R(0) I并假设在 t₀ 附近 φ 保持为常数 φ₀。那么上式是一个线性常微分方程 Ṙ φ₀^ R初值 R(0) I它的解是R(t) exp(φ₀^ t)这里 exp 是矩阵指数定义方式和标量指数的泰勒级数完全一样exp(A) I A A²/2! A³/3! ⋯。到这里我们得到了两个结论在 R I 附近旋转矩阵的变化完全由一个三维向量 φ 决定给定 φ可以通过矩阵指数 exp(φ^) 得到一个旋转矩阵。这个三维向量 φ 的全体就是 SO(3) 的李代数记作 so(3)。用 Solà 的语言说它就是 SO(3) 在单位元 I 处的切空间——这里的推导正是在算切向量R(t) 在 t 0 处的切向量 Ṙ(0) φ₀^。1.3 李代数的正式定义抽象地说李代数由一个集合 V、一个数域 F 和一个二元运算 [·, ·] 组成这个二元运算叫李括号要满足四条性质封闭性[X, Y] ∈ V。双线性[aX bY, Z] a[X, Z] b[Y, Z]对第二个变量同理。自反性[X, X] 0。雅可比等价[X, [Y, Z]] [Z, [X, Y]] [Y, [Z, X]] 0。这些性质看起来抽象但有一个你已经很熟悉的例子三维向量加上外积运算 (ℝ³, ℝ, ×) 就是一个李代数。外积封闭、双线性、a × a 0、满足雅可比恒等式。所以 so(3) 和带外积的三维向量空间在结构上是同一个东西这不是巧合。李括号的直观含义它度量两个元素有多不可交换。对矩阵来说[A, B] AB − BA当 A、B 可交换时括号为零。李群不可交换所以它的李代数需要一个括号来记录这种不可交换性。1.4 so(3) 与 se(3)so(3)so(3) { Φ φ^ ∈ ℝ³ˣ³ | φ ∈ ℝ³ }它的元素是三维反对称矩阵但由于反对称矩阵和三维向量一一对应我们通常直接把 so(3) 看作三维向量的集合不区分 φ 和 φ^。李括号定义为[φ₁, φ₂] (Φ₁Φ₂ − Φ₂Φ₁)∨可以验证它等于 φ₁ × φ₂——又回到了外积。so(3) 与 SO(3) 的关系由指数映射给出R exp(φ^)se(3)se(3) { ξ [ρ; φ] ∈ ℝ⁶ , ξ^ [ φ^ ρ ; 0ᵀ 0 ] ∈ ℝ⁴ˣ⁴ }每个元素是一个六维向量 ξ前三维 ρ 与平移有关后三维 φ 与旋转有关就是 so(3) 的元素。注意两件事书和 Sophus 都把平移在前、旋转在后。Solà 的 τ [ρ; θ] 同样是平移在前。但 g2o 的某些顶点类型是旋转在前用的时候要查。对六维向量的 ^ 运算得到的是一个 4×4 矩阵它不再是反对称的只是沿用了同一个符号。写成分块就是┌ φ^ ρ ┐ ξ^ │ │ 左上 3×3 反对称右上 3×1底行全零 └ 0ᵀ 0 ┘ρ不是平移向量 t。它们之间差一个矩阵 J第二章会推出来。这是初学者最容易混淆的地方。se(3) 的李括号是 [ξ₁, ξ₂] (ξ₁^ ξ₂^ − ξ₂^ ξ₁^)∨。第二章 指数映射与对数映射2.1 SO(3) 上的指数映射罗德里格斯公式的真实身份上一节说 R exp(φ^)但一个矩阵的指数是无穷级数怎么算把 φ 写成模长乘方向φ θ a其中 θ |φ|a φ / θ 是单位向量。先算 a^ 的幂a^ a^ a aᵀ − I 直接展开 3×3 矩阵乘法可验证 a^ a^ a^ a^ (a aᵀ − I) (a^ a) aᵀ − a^ 0 − a^ −a^第二行用了 a^ a a × a 0。于是 a^ 的三次方等于负的一次方更高次幂都可以降回一次或二次(a^)⁴ −(a^)², (a^)⁵ a^, (a^)⁶ (a^)², ⋯把泰勒级数按奇偶次分组exp(θ a^) I θ a^ (θ²/2!) (a^)² (θ³/3!) (a^)³ (θ⁴/4!) (a^)⁴ ⋯ I [ θ − θ³/3! θ⁵/5! − ⋯ ] a^ [ θ²/2! − θ⁴/4! ⋯ ] (a^)² I sin θ · a^ (1 − cos θ) · (a^)²两个方括号分别是 sin θ 和 1 − cos θ 的泰勒级数。再代入 (a^)² a aᵀ − Iexp(θ a^) I sin θ · a^ (1 − cos θ)(a aᵀ − I) cos θ · I (1 − cos θ) · a aᵀ sin θ · a^这正是上一讲的罗德里格斯公式。所以so(3) 的元素就是旋转向量指数映射就是罗德里格斯公式。上一讲用几何推导得到的东西这一讲用纯代数又推了一遍两条路殊途同归。反过来从 R 求 φ 叫对数映射φ ln(R)∨计算方法不需要真的算矩阵对数的级数用上一讲的办法即可θ arccos( (tr R − 1) / 2 ), R a aa 是特征值 1 的特征向量指数映射是满射但不是单射每个旋转矩阵都能由某个 φ 映过来但 φ 和 φ 2π a 映到同一个 R。如果把 θ 限制在 [0, π]映射就在这个范围内一一对应。Solà 把这个范围内的部分叫大写 Exp/Log直接作用于向量和作用于矩阵的小写 exp/log区分书里没有做这个区分统一用 exp 和 ln读者需要根据上下文判断输入是向量还是矩阵。2.2 SE(3) 上的指数映射ρ 与 t 的关系对 se(3) 做同样的事。写出 exp(ξ^) 的级数并分块计算可以得到┌ Σ (1/n!) (φ^)ⁿ Σ (1/(n1)!) (φ^)ⁿ ρ ┐ ┌ R J ρ ┐ exp(ξ^) │ │ │ │ └ 0ᵀ 1 ┘ └ 0ᵀ 1 ┘左上角的级数就是 SO(3) 的指数映射结果是 R。右上角多了一个级数把它记作 JJ Σ (1/(n1)!) (φ^)ⁿ, n 从 0 到 ∞用和 2.1 节一样的技巧降幂、分奇偶可以把它算成闭式J (sin θ / θ) · I (1 − sin θ / θ) · a aᵀ ((1 − cos θ) / θ) · a^于是变换矩阵的平移部分是t J ρ这就是 ρ 与 t 的关系平移 t 等于 J 乘以 se(3) 里的平移分量 ρJ 由旋转部分 φ 决定。只有当旋转为零θ → 0J → I时才有 t ρ。为什么会多出一个 J直观解释是ξ 描述的是一个匀速螺旋运动——以恒定角速度 φ 转动、同时以恒定线速度 ρ 平移、持续单位时间。因为在平移的过程中坐标系一直在转最终走到的位置不是简单的 ρ而是 ρ 在转动过程中各时刻方向的积分这个积分就是 J ρ。对数映射反过来从 T 的左上角 R 求出 φ算出 J再由 t 解出 ρ J⁻¹ t。这个 J 还会在第三章以左雅可比的身份再次出现到时候你会发现它们是同一个矩阵。2.3 小结两组映射关系so(3) ──exp(φ^)──▶ SO(3) se(3) ──exp(ξ^)──▶ SE(3) φ ∈ ℝ³ ◀──ln(R)∨─── R ξ ∈ ℝ⁶ ◀──ln(T)∨─── T ​ R cosθ I (1−cosθ) a aᵀ sinθ a^ T [ R Jρ ; 0ᵀ 1 ] θ arccos((tr R − 1)/2), R a a J (sinθ/θ) I (1 − sinθ/θ) a aᵀ ((1−cosθ)/θ) a^第三章 李代数求导与扰动模型3.1 问题李代数上的加法对应什么现在我们有了一条路把 R 变成 φ在 φ 上做加法再变回 R。但一个自然的问题是两个 φ 相加对应的两个 R 之间是什么关系换句话说下面这个等式成立吗exp(φ₁^) exp(φ₂^) ? exp((φ₁ φ₂)^)对标量e^a e^b e^(ab) 当然成立。对矩阵不成立——除非 A 和 B 可交换。SO(3) 不可交换所以上式一般不成立。这意味着李代数上的加法不等于李群上的乘法。那它们之间差多少这由BCH 公式Baker-Campbell-Hausdorff给出ln( exp(A) exp(B) ) A B ½[A, B] (1/12)[A, [A, B]] − (1/12)[B, [A, B]] ⋯右边是 A B 再加上一堆李括号的修正项。当 A、B 可交换时所有括号为零退化成 A B。修正项的大小取决于 A、B 有多不可交换、以及它们有多大。3.2 BCH 的线性近似与左右雅可比完整的 BCH 公式用不上SLAM 里只用它的一阶近似当 φ₁ 或 φ₂ 之一是小量时ln( exp(φ₁^) exp(φ₂^) )∨ ≈ J_l(φ₂)⁻¹ φ₁ φ₂ 当 φ₁ 为小量 ln( exp(φ₁^) exp(φ₂^) )∨ ≈ J_r(φ₁)⁻¹ φ₂ φ₁ 当 φ₂ 为小量这里出现的 J_l 和 J_r 分别叫左雅可比和右雅可比。左雅可比的表达式是J_l (sin θ / θ) · I (1 − sin θ / θ) · a aᵀ ((1 − cos θ) / θ) · a^它和 2.2 节的 J 一模一样。它的逆是J_l⁻¹ (θ/2) cot(θ/2) · I (1 − (θ/2) cot(θ/2)) · a aᵀ − (θ/2) · a^右雅可比则是把自变量取负J_r(φ) J_l(−φ)这两条近似的含义要用文字说一遍。第一条说在一个旋转 R₂ exp(φ₂^) 的左边乘上一个小旋转 exp(φ₁^)结果对应的李代数不是 φ₂ φ₁而是 φ₂ J_l⁻¹ φ₁——小量 φ₁ 要先被 J_l⁻¹ 修正一下再加上去。第二条说在右边乘一个小旋转修正矩阵换成 J_r⁻¹。把它们反过来写就得到后面最常用的形式。设 R exp(φ^)给它左乘一个小扰动 ΔR exp(Δφ^)exp(Δφ^) exp(φ^) exp( (φ J_l⁻¹(φ) Δφ)^ )以及如果在李代数上直接加一个小量 Δφ等价于在李群上乘一个经 J 修正过的小旋转exp( (φ Δφ)^ ) exp( (J_l Δφ)^ ) exp(φ^) exp(φ^) exp( (J_r Δφ)^ )这组等式是李代数加法与李群乘法之间的换算规则。记住一个口诀群上左乘小量 ↔ 代数上加 J_l⁻¹ 修正的小量代数上加小量 ↔ 群上左乘 J_l 修正的小量或右乘 J_r 修正的小量。对 SE(3) 有完全平行的结论只是 J 变成 6×6 的矩阵形式复杂得多书里给出了符号但不展开实践中直接调库。与 Solà 的对接Solà 的 J_l、J_r 与书里完全相同同样的闭式表达式。Solà 的 ⊕ 运算 R ⊕ θ R Exp(θ) 是右乘对应这里的第二种写法书默认的左乘扰动对应 Solà 的左 ⊕。Solà 笔记里推的J_r(θ) J_l(−θ)、J_l R J_r这些性质在这里全部成立。3.3 为什么要对位姿求导绕了这么大一圈目的是什么是为了求导。SLAM 后端要最小化一个误差。典型的误差是路标点经位姿变换后再投影到图像上与实际观测到的像素位置之差。这个误差是位姿的函数。要用高斯牛顿或 LM 法最小化它就需要误差对位姿的导数。最简单的情形一个空间点 p 经旋转 R 变成 R p。我们要算 ∂(R p) / ∂R。问题是R 是一个 3×3 矩阵它的变化不是普通的加减上面这个导数到底是什么意思有两条路。3.4 第一条路对李代数求导导数模型既然 R 不能加那就对 φ 求导。R exp(φ^)我们算 ∂(exp(φ^) p) / ∂φ。按导数定义∂(R p)/∂φ lim_{δφ→0} [ exp((φ δφ)^) p − exp(φ^) p ] / δφ对分子第一项用 3.2 节的换算规则 exp((φ δφ)^) exp((J_l δφ)^) exp(φ^)再对小旋转做一阶泰勒 exp((J_l δφ)^) ≈ I (J_l δφ)^exp((φ δφ)^) p ≈ (I (J_l δφ)^) R p R p (J_l δφ)^ R p分子只剩 (J_l δφ)^ (R p)。利用上一讲的反交换性质 a^ b −b^ a(J_l δφ)^ (R p) −(R p)^ (J_l δφ)于是∂(R p)/∂φ −(R p)^ J_l结果里带一个 J_l这个 J_l 的表达式含三角函数、还要计算 θ 和 a每次迭代都要算很麻烦。3.5 第二条路扰动模型左乘换一个思路。不对 φ 求导而是给 R 左乘一个小扰动 ΔR exp(δφ^)然后对这个扰动的李代数 δφ 求导∂(R p)/∂δφ lim_{δφ→0} [ exp(δφ^) R p − R p ] / δφ对小旋转做一阶泰勒 exp(δφ^) ≈ I δφ^exp(δφ^) R p − R p ≈ δφ^ R p −(R p)^ δφ于是∂(R p)/∂δφ −(R p)^J_l 消失了。结果就是一个反对称矩阵由 R p 直接构造不需要三角函数。两条路得到的导数不同是因为它们对R 变了一点的定义不同第一条路说的是φ 变了 δφ第二条路说的是R 被左乘了 exp(δφ^)。两者描述的是同一个几何事实R 发生了一个小转动只是用了不同的坐标。扰动模型之所以简单是因为它把小转动直接放在 R 的旁边而不是放在 φ 上再经过 exp 这一层非线性。SLAM 里一律用扰动模型。优化器的工作流程是当前位姿 R算出扰动模型下的雅可比解出一个增量 δφ然后用 R ← exp(δφ^) R 更新位姿。这个更新天然保证结果还是旋转矩阵不需要归一化、不需要投影回 SO(3)。这就是上一讲末尾那个问题——如何给旋转加一点点——的最终答案不是加是左乘一个小旋转。3.6 SE(3) 上的扰动模型对变换矩阵做同样的事。空间点 p齐次坐标经 T 变成 T p。给 T 左乘小扰动 ΔT exp(δξ^)δξ [δρ; δφ]∂(T p)/∂δξ lim [ exp(δξ^) T p − T p ] / δξ ≈ [ δξ^ T p ] / δξ写出 δξ^ 和 T p 的分块。设 T p [q; 1]其中 q R p t 是变换后的三维点则┌ δφ^ δρ ┐ ┌ q ┐ ┌ δφ^ q δρ ┐ ┌ δρ − q^ δφ ┐ δξ^ T p │ │ │ │ │ │ │ │ └ 0ᵀ 0 ┘ └ 1 ┘ └ 0 ┘ └ 0 ┘最后一步又用了 δφ^ q −q^ δφ。现在对 δξ [δρ; δφ] 求导上面三行对 δρ 的导数是 I3×3对 δφ 的导数是 −q^第四行恒为零。写成 4×6 矩阵┌ I −q^ ┐ ∂(T p)/∂δξ │ │ ≜ (T p)⊙ └ 0ᵀ 0ᵀ ┘书里给这个矩阵起了个符号 ⊙读作圆点。注意列的顺序前三列对应 δρ平移扰动后三列对应 δφ旋转扰动和 ξ [ρ; φ] 的顺序一致。如果你以后看到某个库的雅可比前三列是旋转那是因为它的 ξ 定义是旋转在前两种约定都有差别只是列的置换。这个 4×6 矩阵在第 7 讲的 PnP 和第 9 讲的 BA 里会被反复使用重投影误差对位姿的雅可比就是像素对相机系三维点的雅可比乘以这里的相机系三维点对位姿的雅可比的前三行。3.7 左乘还是右乘书默认左乘扰动Solà 默认右乘。两者的差别在于扰动量在哪个坐标系下表示。左乘 exp(δφ^) R先做 R再在世界系R 的输出端里转一个小量 δφ。δφ 是世界系下的小转动。右乘 R exp(δφ^)先在机体系R 的输入端里转一个小量 δφ再做 R。δφ 是机体系下的小转动。两种扰动都合法求出的雅可比相差一个 R 或 Rᵀ因为同一个小转动在两个坐标系下的表示差一个 R。选哪种只是约定问题但在一个系统里必须从头到尾一致雅可比用左扰动推的更新就必须左乘混用会让优化器往错误的方向走。对右乘∂(R p)/∂δφ −R p^注意 p^ 在 R 后面和左乘的 −(R p)^ 不同。IMU 预积分、ESKF 以及 Solà 的论文通常用右乘因为陀螺仪测的角速度是机体系下的ORB-SLAM、g2o 的 SE3 顶点用左乘。读论文时第一件事就是弄清它用的哪种。与 EMF 项目 ESKF 的对接误差状态卡尔曼滤波里的姿态误差 δθ 就是这里的 δφ它住在 so(3) 里协方差是 3×3 的注入步骤 R ← R exp(δθ^)或左乘版本就是 3.5 节的更新重置步骤把 δθ 清零、同时修正协方差修正用的矩阵就是 J_r 或 J_l 的近似。你在 EMF 里用过的东西其数学身份现在应该清楚了。第四章 实践Sophus4.1 Sophus 是什么Sophus 是基于 Eigen 的李群库由 Hauke Strasdat 维护实现了 SO(2)、SO(3)、SE(2)、SE(3)、Sim(3) 等常用李群及其李代数操作。它和 Eigen 一样是纯头文件库。书使用的是模板化的新版 Sophus类型名是Sophus::SO3d、Sophus::SE3d后缀 d 表示 double。4.2 SO(3) 的基本操作// 构造从旋转矩阵或四元数 Eigen::Matrix3d R Eigen::AngleAxisd(M_PI / 2, Eigen::Vector3d(0, 0, 1)).toRotationMatrix(); Eigen::Quaterniond q(R); Sophus::SO3d SO3_R(R); Sophus::SO3d SO3_q(q); // 两者等价 ​ // 对数映射李群 → 李代数三维向量 Eigen::Vector3d so3 SO3_R.log(); ​ // hat / vee向量 ↔ 反对称矩阵 Eigen::Matrix3d so3_hat Sophus::SO3d::hat(so3); Eigen::Vector3d so3_vee Sophus::SO3d::vee(so3_hat); ​ // 指数映射 左乘扰动更新 Eigen::Vector3d update_so3(1e-4, 0, 0); // 一个很小的扰动 Sophus::SO3d SO3_updated Sophus::SO3d::exp(update_so3) * SO3_R;最后一行就是 3.5 节的 R ← exp(δφ^) R。注意Sophus::SO3d::exp的参数是三维向量对应 Solà 的大写 Exp不是反对称矩阵。.log()也直接返回三维向量。Sophus 替你处理了 ^ 和 ∨。绕 z 轴转 90° 的 SO3 做 log 应得到 (0, 0, π/2)可以作为检查。4.3 SE(3) 的基本操作Eigen::Vector3d t(1, 0, 0); Sophus::SE3d SE3_Rt(R, t); Sophus::SE3d SE3_qt(q, t); ​ typedef Eigen::Matrixdouble, 6, 1 Vector6d; Vector6d se3 SE3_Rt.log(); // 六维向量前三维 ρ后三维 φ ​ Eigen::Matrix4d se3_hat Sophus::SE3d::hat(se3); Vector6d se3_vee Sophus::SE3d::vee(se3_hat); ​ Vector6d update_se3 Vector6d::Zero(); update_se3(0) 1e-4; // 对平移分量 ρ 的扰动 Sophus::SE3d SE3_updated Sophus::SE3d::exp(update_se3) * SE3_Rt;运行这段代码会发现SE3_Rt.log()的前三维不等于(1, 0, 0)。这就是 2.2 节说的 t J ρ——由于有 90° 的旋转J 不是单位阵ρ 和 t 不同。亲手打印一次比看十遍推导更能记住这件事。SE3_Rt.matrix()返回 4×4 的 Eigen 矩阵SE3_Rt.rotationMatrix()和SE3_Rt.translation()分别取出 R 和 t。4.4 例子评估轨迹误差书的 4.4.2 节给了一个有实际用途的例子有一条真实轨迹groundtruth和一条估计轨迹每条轨迹是一串带时间戳的 SE(3) 位姿如何定量评价估计得好不好绝对轨迹误差Absolute Trajectory ErrorATE定义为ATE_all sqrt( (1/N) Σᵢ ‖ ln( T_gt,i⁻¹ T_esti,i )∨ ‖² )对每一时刻 i算真实位姿的逆乘估计位姿得到一个误差位姿把它映射到 se(3) 得到六维误差向量取范数平方对所有时刻取平均再开方。这是一个李代数意义下的均方根误差。如果只关心位置不关心姿态就只取平移部分ATE_trans sqrt( (1/N) Σᵢ ‖ trans( T_gt,i⁻¹ T_esti,i ) ‖² )还有一种相对位姿误差Relative Pose ErrorRPE比较的是相隔 Δt 的两帧之间的相对运动RPE_all sqrt( (1/(N−Δt)) Σᵢ ‖ ln( (T_gt,i⁻¹ T_gt,iΔt)⁻¹ (T_esti,i⁻¹ T_esti,iΔt) )∨ ‖² )ATE 反映全局一致性有没有漂移RPE 反映局部精度里程计每一步准不准。SLAM 论文里报的指标基本都是这两个评估工具 evo 实现的也是它们。代码的核心就一行double error (groundtruth[i].inverse() * estimated[i]).log().norm();.inverse()、*、.log()、.norm()——这一讲的全部内容压缩在一行里。读轨迹文件时记得 TUM 格式四元数实部在后构造Eigen::Quaterniond(qw, qx, qy, qz)时要把顺序对回来。第五章 相似变换群 Sim(3)这一节是书中的选读内容但单目 SLAM 绕不开它所以简要说明。单目相机无法确定尺度把整个场景放大两倍、相机离远两倍拍出的图像完全一样。所以单目 SLAM 估计出的轨迹和真实轨迹之间差一个未知的缩放因子 s而且这个 s 会随时间漂移。回环检测时两次经过同一地点的尺度可能已经不一样了这时用 SE(3) 去对齐是不够的需要多一个自由度来吸收尺度差异。这就是相似变换群 Sim(3)┌ sR t ┐ S │ │ 7 个自由度 └ 0ᵀ 1 ┘它对点的作用是 p′ sR p t。对应的李代数 sim(3) 是七维向量 ζ [ρ; φ; σ]比 se(3) 多一个尺度分量 σ对应 s e^σ。Sim(3) 的扰动模型与 SE(3) 类似结果是一个 4×7 的矩阵设 q sR p t┌ I −q^ q ┐ ∂(S p)/∂ζ │ │ └ 0ᵀ 0ᵀ 0 ┘多出的最后一列 q 对应尺度扰动——把尺度放大一点点点 q 沿自身方向伸长一点点。ORB-SLAM 的回环校正和 Essential Graph 优化就是在 Sim(3) 上做的第 10 讲和 ORB-SLAM 论文会用到。Sophus 提供了Sophus::Sim3d。