ch10_pose_point

第十章　位姿与点联合估计 §

Chapter 10   Pose-and-Point Estimation §

---

10.1 束调整 / Bundle Adjustment §

10.1.1 问题描述 / Problem Setup §

中文

束调整（Bundle Adjustment, BA） 是计算机视觉与机器人中最重要的非线性最小二乘问题之一：同时估计相机轨迹（$K$ 个位姿）和场景地图（$M$ 个路标点）。

与第 9 章位姿估计不同，这里路标点坐标 ${\mathbf{p}}_j$ 也是未知量，需要与位姿一起优化。

待估状态

$$\mathbf{x}=\{{\mathbf{T}}_1,\dots ,{\mathbf{T}}_K,{\mathbf{p}}_1,\dots ,{\mathbf{p}}_M\},\text{\hspace{1em}(10.1)}$$

其中 ${\mathbf{T}}_k\in SE(3)$ 是第 $k$ 个相机位姿，${\mathbf{p}}_j\in {\mathbb{R}}^4$（齐次坐标）是第 $j$ 个路标点的世界坐标。

可观测性（Gauge Freedom）

仅靠视觉测量，绝对坐标系无法确定（整体平移、旋转、尺度不可观）。标准处理方式：固定第 0 个位姿 ${\mathbf{T}}_0=1$，不参与优化。这消除了规范自由度（gauge freedom）。

测量模型

每次相机 $k$ 观测到路标 $j$ 产生一个测量：

$${\mathbf{y}}_{jk}=\mathbf{g}({\mathbf{T}}_k,{\mathbf{p}}_j)+{\mathbf{n}}_{jk},{\mathbf{n}}_{jk}\sim \mathcal{N}(0,{\mathbf{R}}_{jk}),\text{\hspace{1em}(10.2)}$$

其中 $\mathbf{g}$ 是复合映射：先将路标转换到相机坐标系，再经过相机投影模型：

$$\mathbf{g}({\mathbf{T}}_k,{\mathbf{p}}_j)=\mathbf{s}({\mathbf{T}}_k{\mathbf{p}}_j).\text{\hspace{1em}(10.3)}$$

$\mathbf{s}(\cdot )$ 可以是针孔模型、立体相机模型等任意非线性投影。

---

English

Bundle Adjustment (BA) is a cornerstone algorithm in computer vision and robotics: simultaneously estimate $K$ camera poses and $M$ landmark positions from visual measurements.

State to estimate:

$$\mathbf{x}=\{{\mathbf{T}}_1,\dots ,{\mathbf{T}}_K,{\mathbf{p}}_1,\dots ,{\mathbf{p}}_M\},$$

where ${\mathbf{T}}_k\in SE(3)$ and ${\mathbf{p}}_j\in {\mathbb{R}}^4$ (homogeneous).

Gauge freedom: visual measurements determine only relative geometry. We fix ${\mathbf{T}}_0=1$ (not included in the state) to remove the unobservable rigid-body gauge freedom.

Measurement model: camera $k$ observing landmark $j$:

$${\mathbf{y}}_{jk}=\mathbf{s}({\mathbf{T}}_k{\mathbf{p}}_j)+{\mathbf{n}}_{jk},{\mathbf{n}}_{jk}\sim \mathcal{N}(0,{\mathbf{R}}_{jk}).$$

---

10.1.2 最大似然解 / Maximum Likelihood Solution §

中文

在全部测量 $\mathbf{y}=\{{\mathbf{y}}_{jk}\}$ 已知的情况下，最大似然（ML）问题等价于最小化

$$J(\mathbf{x})=\frac{1}{2}\sum _{j,k}{\mathbf{e}}_{jk}^T{\mathbf{R}}_{jk}^{-1}{\mathbf{e}}_{jk},\text{\hspace{1em}(10.4)}$$

其中测量残差为

$${\mathbf{e}}_{jk}(\mathbf{x})={\mathbf{y}}_{jk}-\mathbf{s}({\mathbf{T}}_k{\mathbf{p}}_j).\text{\hspace{1em}(10.5)}$$

线性化：在工作点 ${\mathbf{x}}_{\mathrm{op}}=\{{\mathbf{T}}_{\mathrm{op},k},{\mathbf{p}}_{\mathrm{op},j}\}$ 处，对位姿施加李代数扰动、对路标施加线性扰动：

$${\mathbf{T}}_k=\exp ({\mathbf{ϵ}}_k^{\wedge }){\mathbf{T}}_{\mathrm{op},k},{\mathbf{p}}_j={\mathbf{p}}_{\mathrm{op},j}+\mathbf{D}{\mathbf{\zeta }}_j,\text{\hspace{1em}(10.6)}$$

其中 $\mathbf{D}=[1_{3\times 3};0_{1\times 3}{]}^T$ 将 3D 扰动嵌入齐次坐标。残差线性化为

$${\mathbf{e}}_{jk}(\mathbf{x})\approx {\mathbf{e}}_{jk}({\mathbf{x}}_{\mathrm{op}})-{\mathbf{G}}_{1,jk}{\mathbf{ϵ}}_k-{\mathbf{G}}_{2,jk}{\mathbf{\zeta }}_j,\text{\hspace{1em}(10.7)}$$

其中雅可比矩阵为

$${\mathbf{G}}_{1,jk}={\mathbf{S}}_{jk}({\mathbf{T}}_{\mathrm{op},k}{\mathbf{p}}_{\mathrm{op},j}{)}^{\odot },{\mathbf{G}}_{2,jk}={\mathbf{S}}_{jk}{\mathbf{T}}_{\mathrm{op},k}\mathbf{D},\text{\hspace{1em}(10.8)}$$

${\mathbf{S}}_{jk}=\frac{\partial \mathbf{s}}{\partial (\cdot )}{|}_{{\mathbf{T}}_{\mathrm{op},k}{\mathbf{p}}_{\mathrm{op},j}}$ 是投影函数的雅可比，$(\cdot {)}^{\odot }$ 是 $SE(3)$ 的伴随算子。

将全部扰动向量堆叠：

$$\delta {\mathbf{x}}_1=\left[\begin{array}{c}{\mathbf{ϵ}}_1\\ ⋮\\ {\mathbf{ϵ}}_K\end{array}\right],\delta {\mathbf{x}}_2=\left[\begin{array}{c}{\mathbf{\zeta }}_1\\ ⋮\\ {\mathbf{\zeta }}_M\end{array}\right],\delta \mathbf{x}=\left[\begin{array}{c}\delta {\mathbf{x}}_1\\ \delta {\mathbf{x}}_2\end{array}\right].\text{\hspace{1em}(10.9)}$$

费用函数的二次近似为

$$J(\mathbf{x})\approx J({\mathbf{x}}_{\mathrm{op}})-{\mathbf{b}}^T\delta \mathbf{x}+\frac{1}{2}\delta {\mathbf{x}}^T\mathbf{A}\delta \mathbf{x},\text{\hspace{1em}(10.10)}$$

其中

$$\mathbf{A}={\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H}=\left[\begin{array}{cc}{\mathbf{A}}_{11}& {\mathbf{A}}_{12}\\ {\mathbf{A}}_{12}^T& {\mathbf{A}}_{22}\end{array}\right],\mathbf{b}={\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{e}({\mathbf{x}}_{\mathrm{op}}),\text{\hspace{1em}(10.11)}$$ $$\mathbf{H}=\left[\begin{array}{cc}{\mathbf{G}}_1& {\mathbf{G}}_2\end{array}\right],\mathbf{W}=\mathbf{R}=\mathrm{diag}({\mathbf{R}}_{10},{\mathbf{R}}_{20},\dots ,{\mathbf{R}}_{MK}).\text{\hspace{1em}(10.12)}$$

最优扰动 $\delta {\mathbf{x}}^{\star }$ 满足线性方程

$$\mathbf{A}\delta {\mathbf{x}}^{\star }=\mathbf{b}.\text{\hspace{1em}(10.13)}$$

然后更新工作点：

$${\mathbf{T}}_{\mathrm{op},k}\leftarrow \exp ({\mathbf{ϵ}}_k^{\star \wedge }){\mathbf{T}}_{\mathrm{op},k},{\mathbf{p}}_{\mathrm{op},j}\leftarrow {\mathbf{p}}_{\mathrm{op},j}+\mathbf{D}{\mathbf{\zeta }}_j^{\star },\text{\hspace{1em}(10.14)}$$

迭代至收敛（高斯-牛顿法 Gauss-Newton）。

GN vs. Newton’s Method

• GN 近似：$\mathbf{A}={\mathbf{G}}_1^T{\mathbf{R}}^{-1}{\mathbf{G}}_1+\cdots$，仅用一阶雅可比，舍去 Hessian 的二阶项。适用于残差较小时（近最优时成立）。
• Newton 法：$\mathbf{A}={\mathbf{G}}^T{\mathbf{R}}^{-1}\mathbf{G}+{\sum }_i{e}_i\frac{{\partial }^2{e}_i}{\partial x^2}$，包含二阶项。收敛域更大，但计算代价高。 BA 问题通常用 GN 法，因为残差（重投影误差）在最优解附近很小。

---

English

The ML objective is

$$J(\mathbf{x})=\frac{1}{2}\sum _{j,k}{\mathbf{e}}_{jk}^T{\mathbf{R}}_{jk}^{-1}{\mathbf{e}}_{jk},{\mathbf{e}}_{jk}={\mathbf{y}}_{jk}-\mathbf{s}({\mathbf{T}}_k{\mathbf{p}}_j).$$

Linearizing about ${\mathbf{x}}_{\mathrm{op}}$ using left perturbations for poses and additive perturbations for landmarks:

$${\mathbf{T}}_k=\exp ({\mathbf{ϵ}}_k^{\wedge }){\mathbf{T}}_{\mathrm{op},k},{\mathbf{p}}_j={\mathbf{p}}_{\mathrm{op},j}+\mathbf{D}{\mathbf{\zeta }}_j.$$

The Jacobians are

$${\mathbf{G}}_{1,jk}={\mathbf{S}}_{jk}({\mathbf{T}}_{\mathrm{op},k}{\mathbf{p}}_{\mathrm{op},j}{)}^{\odot },{\mathbf{G}}_{2,jk}={\mathbf{S}}_{jk}{\mathbf{T}}_{\mathrm{op},k}\mathbf{D}.$$

Stacking all perturbations $\delta \mathbf{x}=[\delta {\mathbf{x}}_1^T,\delta {\mathbf{x}}_2^T{]}^T$, the quadratic approximation is

$$J(\mathbf{x})\approx J({\mathbf{x}}_{\mathrm{op}})-{\mathbf{b}}^T\delta \mathbf{x}+\frac{1}{2}\delta {\mathbf{x}}^T\mathbf{A}\delta \mathbf{x},$$

and the GN step solves $\mathbf{A}\delta {\mathbf{x}}^{\star }=\mathbf{b}$, then updates operating points via the Lie exponential and vector addition.

---

10.1.3 利用稀疏结构 / Exploiting Sparsity §

中文

矩阵 $\mathbf{A}$ 的块结构

$$\mathbf{A}=\left[\begin{array}{cc}{\mathbf{A}}_{11}& {\mathbf{A}}_{12}\\ {\mathbf{A}}_{12}^T& {\mathbf{A}}_{22}\end{array}\right],\text{\hspace{1em}(10.15)}$$

其中

$${\mathbf{A}}_{11}={\mathbf{G}}_1^T{\mathbf{R}}^{-1}{\mathbf{G}}_1,{\mathbf{A}}_{12}={\mathbf{G}}_1^T{\mathbf{R}}^{-1}{\mathbf{G}}_2,{\mathbf{A}}_{22}={\mathbf{G}}_2^T{\mathbf{R}}^{-1}{\mathbf{G}}_2.\text{\hspace{1em}(10.16)}$$

关键观察：

1. ${\mathbf{A}}_{22}$ 是块对角矩阵。 原因：${\mathbf{G}}_2$ 中不同路标的列块之间没有耦合（每个路标的扰动 ${\mathbf{\zeta }}_j$ 只影响自身行），因此

$${\mathbf{A}}_{22}=\mathrm{diag}\left(\sum _k{\mathbf{G}}_{2,1k}^T{\mathbf{R}}_{1k}^{-1}{\mathbf{G}}_{2,1k},\dots ,\sum _k{\mathbf{G}}_{2,Mk}^T{\mathbf{R}}_{Mk}^{-1}{\mathbf{G}}_{2,Mk}\right).\text{\hspace{1em}(10.17)}$$

2. ${\mathbf{A}}_{11}$ 也是块对角矩阵。 原因：每个测量只关联一个位姿，因此 ${\mathbf{G}}_1$ 的每行只对应一个位姿的列块，导致 ${\mathbf{A}}_{11}={\mathbf{G}}_1^T{\mathbf{R}}^{-1}{\mathbf{G}}_1$ 呈块对角形式：

$${\mathbf{A}}_{11}=\mathrm{diag}\left(\sum _{j=1}^M{\mathbf{G}}_{1,j1}^T{\mathbf{R}}_{j1}^{-1}{\mathbf{G}}_{1,j1},\dots ,\sum _{j=1}^M{\mathbf{G}}_{1,jK}^T{\mathbf{R}}_{jK}^{-1}{\mathbf{G}}_{1,jK}\right).$$

整个矩阵 $\mathbf{A}$ 呈**箭头形（arrowhead）**稀疏结构：

$$\mathbf{A}=\left[\begin{array}{cccc}\ast & \ast & \ast & \ast \\ \ast & \ast & & \\ \ast & & \ast & \\ \ast & & & \ast \end{array}\right],\text{\hspace{1em}(10.18)}$$

其中左上角 ${\mathbf{A}}_{11}$（位姿-位姿块）和右下角 ${\mathbf{A}}_{22}$（路标-路标块，块对角）。

Schur 补消去路标

利用块消去（Schur 补）先消去路标变量：

$$\underset{{\tilde{\mathbf{A}}}_{11}}{\underbrace{({\mathbf{A}}_{11}-{\mathbf{A}}_{12}{\mathbf{A}}_{22}^{-1}{\mathbf{A}}_{12}^T)}}\delta {\mathbf{x}}_1^{\star }={\tilde{\mathbf{b}}}_1,\text{\hspace{1em}(10.19)}$$

其中 ${\tilde{\mathbf{b}}}_1={\mathbf{b}}_1-{\mathbf{A}}_{12}{\mathbf{A}}_{22}^{-1}{\mathbf{b}}_2$。

由于 ${\mathbf{A}}_{22}$ 块对角，其逆 ${\mathbf{A}}_{22}^{-1}$ 可逐块求解，计算量仅 $O(M)$（每个 $3\times 3$ 块求逆）。然后求解规模为 $6K\times 6K$ 的缩减位姿系统，再回代求路标扰动：

$$\delta {\mathbf{x}}_2^{\star }={\mathbf{A}}_{22}^{-1}({\mathbf{b}}_2-{\mathbf{A}}_{12}^T\delta {\mathbf{x}}_1^{\star }).\text{\hspace{1em}(10.20)}$$

总体复杂度 $O(K^3+K^{2}M)$，相比直接解 $O((6K+3M{)}^3)$ 有巨大提升。

Cholesky 分解

也可以利用 $\mathbf{A}$ 的稀疏结构做 Cholesky 分解 $\mathbf{A}={\mathbf{U}}^T\mathbf{U}$：

$$\mathbf{U}=\left[\begin{array}{cc}{\mathbf{U}}_{11}& {\mathbf{U}}_{12}\\ 0& {\mathbf{U}}_{22}\end{array}\right],\text{\hspace{1em}(10.21)}$$

其中 ${\mathbf{U}}_{22}$ 块对角（继承自 ${\mathbf{A}}_{22}$），求逆高效。后向回代即可得到 $\delta {\mathbf{x}}^{\star }$ 和不确定性 $\hat{\mathbf{P}}={\mathbf{A}}^{-1}$。

---

English

The matrix $\mathbf{A}$ has arrowhead sparsity: ${\mathbf{A}}_{22}$ (landmark-landmark) is block-diagonal because each landmark’s perturbation only appears in its own rows, while ${\mathbf{A}}_{11}$ (pose-pose) is dense among poses that share landmarks.

Schur complement (eliminate landmarks first):

$$({\mathbf{A}}_{11}-{\mathbf{A}}_{12}{\mathbf{A}}_{22}^{-1}{\mathbf{A}}_{12}^T)\delta {\mathbf{x}}_1^{\star }={\mathbf{b}}_1-{\mathbf{A}}_{12}{\mathbf{A}}_{22}^{-1}{\mathbf{b}}_2.$$

Since ${\mathbf{A}}_{22}^{-1}$ is block-diagonal (each $3\times 3$ block inverted independently), the reduced pose system has size $6K\times 6K$. Back-substitution recovers the landmark corrections. Total cost: $O(K^3+K^{2}M)$.

Cholesky: A block-Cholesky factorization $\mathbf{A}={\mathbf{U}}^T\mathbf{U}$ exploits the same sparsity and allows efficient extraction of posterior covariances.

---

10.1.4 初始化策略 / Initialization §

中文

BA 是非凸优化，GN 需要良好的初值。实践中常用：

1. 旋转平均（rotation averaging）：从成对相对旋转中初始化绝对旋转。
2. 平移求解（translation solver）：固定旋转后线性求解平移。
3. 三角化（triangulation）：固定位姿后，用线性最小二乘初始化路标位置。

经典策略：先做递增式 BA（每添加一帧做一次小型 BA），再做全局 BA 精化。

---

English

BA is non-convex; GN requires good initialization. Common strategy: rotation averaging → translation initialization → triangulation → incremental BA → global BA refinement.

---

10.1.5 插值示例 / Interpolation Example §

中文

欠约束 BA：当某时刻相机没有足够路标观测（欠约束）时，该位姿不可估计。一种正则化方法是引入轨迹插值约束。

假设我们已知位姿 ${\mathbf{T}}_2$，并假定 ${\mathbf{T}}_1$ 与 ${\mathbf{T}}_2$ 之间满足常速运动（匀速插值）：

$${\mathbf{T}}_1={\mathbf{T}}_2^{\alpha },\alpha \in [0,1],\text{\hspace{1em}(10.22)}$$

其中 ${\mathbf{T}}^{\alpha }=\exp (\alpha \ln (\mathbf{T}{)}^{\vee }{}^{\wedge })$。此时 ${\mathbf{T}}_1$ 不是独立变量，可用 ${\mathbf{T}}_2$ 表达。

对 $\alpha$ 处的扰动：${\mathbf{ϵ}}_1=\mathcal{A}(\alpha ,{\mathbf{\xi }}_{\mathrm{op}}){\mathbf{ϵ}}_2$，其中 $\mathcal{A}$ 是插值雅可比矩阵（依赖 $\alpha$ 和工作点）。

插值矩阵：设原扰动状态 $\delta \mathbf{x}=[\delta {\mathbf{x}}_1^T,\delta {\mathbf{x}}_2^T{]}^T$，引入缩减扰动状态 $\delta {\mathbf{x}}^{\prime }={\mathbf{I}}^T\delta \mathbf{x}$：

$${\mathbf{I}}^T=\left[\begin{array}{ccc}\mathcal{A}(\alpha ,{\mathbf{\xi }}_{\mathrm{op}})& 1& 0\\ 0& 0& 1\end{array}\right],\text{\hspace{1em}(10.23)}$$

新的费用函数变为

$${\mathbf{A}}^{\prime }={\mathbf{I}}^T\mathbf{A}\mathbf{I},{\mathbf{b}}^{\prime }={\mathbf{I}}^T\mathbf{b},{\mathbf{A}}^{\prime }\delta {\mathbf{x}}^{\prime \star }={\mathbf{b}}^{\prime }.\text{\hspace{1em}(10.24)}$$

重要结论：插值操作不破坏箭头形稀疏结构——${\mathbf{A}}^{\prime }$ 的路标-路标块 ${\mathbf{A}}_{22}^{\prime }$ 仍保持块对角，Schur 补方法依然可用。

---

English

When a pose is underconstrained (too few landmark observations), a constant-velocity interpolation provides regularization. Assume pose ${\mathbf{T}}_1={\mathbf{T}}_2^{\alpha }$ for some $\alpha \in [0,1]$.

The perturbation at ${\mathbf{T}}_1$ is then ${\mathbf{ϵ}}_1=\mathcal{A}(\alpha ,{\mathbf{\xi }}_{\mathrm{op}}){\mathbf{ϵ}}_2$ (interpolation Jacobian). This reduces the free variables via an interpolation matrix $\mathbf{I}$:

$${\mathbf{A}}^{\prime }={\mathbf{I}}^T\mathbf{AI},{\mathbf{b}}^{\prime }={\mathbf{I}}^T\mathbf{b}.$$

Crucially, the landmark-landmark block ${\mathbf{A}}_{22}^{\prime }$ remains block-diagonal, preserving the arrowhead structure and Schur complement efficiency.

---

10.2 同步定位与建图（SLAM） / Simultaneous Localization and Mapping §

10.2.1 问题描述 / Problem Setup §

中文

直觉：BA vs. SLAM

BA 是纯粹的最大似然问题——仅依赖视觉测量，位姿间没有耦合。SLAM 是最大后验问题——除了视觉测量，还引入了运动先验（来自 IMU、轮速计或常速假设），位姿之间通过运动模型相互关联。

另一个关键区别：BA 需要固定某个位姿（规范自由度），SLAM 有运动先验，因此 ${\mathbf{T}}_0$ 也可以参与估计。

待估状态

$$\mathbf{x}=\{{\mathbf{T}}_0,{\mathbf{T}}_1,\dots ,{\mathbf{T}}_K,{\mathbf{p}}_1,\dots ,{\mathbf{p}}_M\},\text{\hspace{1em}(10.25)}$$

与 BA 相比增加了 ${\mathbf{T}}_0$。

输入（运动信息）

$$\mathbf{v}=\{{\check{\mathbf{T}}}_0,{\mathbf{\varpi }}_1,{\mathbf{\varpi }}_2,\dots ,{\mathbf{\varpi }}_K\},\text{\hspace{1em}(10.26)}$$

其中 ${\check{\mathbf{T}}}_0$ 是初始位姿先验，${\mathbf{\varpi }}_k$ 是第 $k$ 步的广义速度输入。

测量 与 BA 相同：$\mathbf{y}=\{{\mathbf{y}}_{jk}\}$。

---

English

SLAM extends BA by incorporating a motion prior (from odometry, IMU, or a constant-velocity model) linking consecutive poses. This adds the initial pose ${\mathbf{T}}_0$ to the estimated state (no gauge freedom needed since the prior provides an absolute reference):

$$\mathbf{x}=\{{\mathbf{T}}_0,{\mathbf{T}}_1,\dots ,{\mathbf{T}}_K,{\mathbf{p}}_1,\dots ,{\mathbf{p}}_M\}.$$

Inputs: $\mathbf{v}=\{{\check{\mathbf{T}}}_0,{\mathbf{\varpi }}_1,\dots ,{\mathbf{\varpi }}_K\}$.

---

10.2.2 批量最大后验解 / Batch Maximum a Posteriori Solution §

中文

从第 9.2 节的运动先验，定义运动误差：

$${\mathbf{e}}_{v,k}({\mathbf{x}}_{\mathrm{op}})=\{\begin{array}{ll}\ln {\left({\check{\mathbf{T}}}_0{\mathbf{T}}_{\mathrm{op},0}^{-1}\right)}^{\vee }& k=0\\ \ln {\left(\exp \left(\Delta t_k{\mathbf{\varpi }}_k^{\wedge }\right){\mathbf{T}}_{\mathrm{op},k-1}{\mathbf{T}}_{\mathrm{op},k}^{-1}\right)}^{\vee }& k=1\dots K\end{array},\text{\hspace{1em}(10.27)}$$

运动先验代价项：$J_{v,k}=\frac{1}{2}{\mathbf{e}}_{v,k}^T{\mathbf{Q}}_k^{-1}{\mathbf{e}}_{v,k}$。

总代价函数：

$$J(\mathbf{x})=\underset{\text{运动先验}}{\underbrace{\sum _{k=0}^K{J}_{v,k}}}+\underset{\text{视觉测量}}{\underbrace{\sum _{j,k}{J}_{y,jk}}},\text{\hspace{1em}(10.28)}$$

矩阵形式：

$$\mathbf{A}={\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H},\mathbf{H}=\left[\begin{array}{cc}{\mathbf{F}}^{-1}& 0\\ {\mathbf{G}}_1& {\mathbf{G}}_2\end{array}\right],\mathbf{W}=\left[\begin{array}{cc}\mathbf{Q}& 0\\ 0& \mathbf{R}\end{array}\right],\text{\hspace{1em}(10.29)}$$

其中 ${\mathbf{F}}^{-1}$ 是第 9.2 节的运动先验块三对角矩阵，$\mathbf{Q}=\mathrm{diag}({\check{\mathbf{P}}}_0,{\mathbf{Q}}_1,\dots ,{\mathbf{Q}}_K)$。

块矩阵展开：

$$\mathbf{A}=\left[\begin{array}{cc}{\mathbf{A}}_{11}& {\mathbf{A}}_{12}\\ {\mathbf{A}}_{12}^T& {\mathbf{A}}_{22}\end{array}\right]=\left[\begin{array}{cc}{\mathbf{F}}^{-T}{\mathbf{Q}}^{-1}{\mathbf{F}}^{-1}+{\mathbf{G}}_1^T{\mathbf{R}}^{-1}{\mathbf{G}}_1& {\mathbf{G}}_1^T{\mathbf{R}}^{-1}{\mathbf{G}}_2\\ {\mathbf{G}}_2^T{\mathbf{R}}^{-1}{\mathbf{G}}_1& {\mathbf{G}}_2^T{\mathbf{R}}^{-1}{\mathbf{G}}_2\end{array}\right].\text{\hspace{1em}(10.30)}$$

运动先验雅可比（与第 9.2 节一致）：

$${\mathbf{E}}_k=\mathcal{J}(-{\mathbf{e}}_{v,k}{)}^{-1},{\mathbf{F}}_{k-1}=\mathcal{J}(-{\mathbf{e}}_{v,k}{)}^{-1}\mathrm{Ad}({\mathbf{T}}_{\mathrm{op},k}{\mathbf{T}}_{\mathrm{op},k-1}^{-1}).\text{\hspace{1em}(10.31)}$$

求解 $\mathbf{A}\delta {\mathbf{x}}^{\star }=\mathbf{b}$，然后更新工作点并迭代。

---

English

SLAM adds motion prior cost terms to the BA objective. The motion error at step $k$ is (from §9.2):

$${\mathbf{e}}_{v,k}=\ln {\left(\exp (\Delta t_k{\mathbf{\varpi }}_k^{\wedge }){\mathbf{T}}_{\mathrm{op},k-1}{\mathbf{T}}_{\mathrm{op},k}^{-1}\right)}^{\vee },k>0.$$

The full system matrix:

$$\mathbf{A}={\mathbf{H}}^T{\mathbf{W}}^{-1}\mathbf{H},\mathbf{H}=\left[\begin{array}{cc}{\mathbf{F}}^{-1}& 0\\ {\mathbf{G}}_1& {\mathbf{G}}_2\end{array}\right],\mathbf{W}=\mathrm{diag}(\mathbf{Q},\mathbf{R}).$$

The block ${\mathbf{A}}_{11}$ now contains both the motion prior contribution ${\mathbf{F}}^{-T}{\mathbf{Q}}^{-1}{\mathbf{F}}^{-1}$ (block-tridiagonal) and the measurement contribution ${\mathbf{G}}_1^T{\mathbf{R}}^{-1}{\mathbf{G}}_1$. The block ${\mathbf{A}}_{22}$ is unchanged from BA (block-diagonal).

---

10.2.3 利用稀疏结构 / Exploiting Sparsity §

中文

引入运动先验不破坏箭头形稀疏结构：

• ${\mathbf{A}}_{12}$ 和 ${\mathbf{A}}_{22}$：与 BA 完全相同，${\mathbf{A}}_{22}$ 仍块对角。
• ${\mathbf{A}}_{11}$：增加了运动先验项 ${\mathbf{F}}^{-T}{\mathbf{Q}}^{-1}{\mathbf{F}}^{-1}$，但该项是块三对角的，因此 ${\mathbf{A}}_{11}$ 整体仍为块三对角。

两种利用稀疏的策略：

策略	利用哪个块的稀疏	适用场景
Schur 补	${\mathbf{A}}_{22}$ 块对角	路标数 $M$ 远大于位姿数 $K$
Cholesky	${\mathbf{A}}_{11}$ 块三对角	位姿数 $K$ 远大于路标数 $M$

注意：Schur 补需要构建 ${\mathbf{A}}_{11}^{-1}$，而 ${\mathbf{A}}_{11}^{-1}$ 是稠密的，因此当位姿数量很大时，Cholesky 方法更优。

因子图视角

SLAM 的代价函数可以用因子图直观表示：每个测量 $J_{y,jk}$ 和每个运动先验 $J_{v,k}$ 是图中的一个”因子”（黑点），连接对应的变量节点。因子图清晰展示了变量间的耦合关系，是理解稀疏结构的利器。

---

English

Adding the motion prior does not destroy the arrowhead sparsity:

• ${\mathbf{A}}_{22}$ remains block-diagonal (unchanged).
• ${\mathbf{A}}_{11}$ gains the block-tridiagonal prior term ${\mathbf{F}}^{-T}{\mathbf{Q}}^{-1}{\mathbf{F}}^{-1}$.

Two exploitation strategies:

• Schur complement (eliminating landmarks): preferred when $M\gg K$.
• Cholesky (exploiting tridiagonal ${\mathbf{A}}_{11}$): preferred when $K\gg M$ (avoids forming the dense ${\mathbf{A}}_{11}^{-1}$).

---

10.2.4 SLAM 示例 / SLAM Example §

中文

图 10.4 展示了一个简单的 SLAM 问题：3 个路标点、3 个自由位姿（${\mathbf{T}}_0$ 也参与估计）。

代价函数共 9 项：

$$J=\underset{\text{运动先验（3项）}}{\underbrace{J_{v,0}+J_{v,1}+J_{v,2}}}+\underset{\text{视觉测量（6项）}}{\underbrace{J_{y,10}+J_{y,30}+J_{y,11}+J_{y,21}+J_{y,22}+J_{y,32}}}.\text{\hspace{1em}(10.32)}$$

与 BA 不同，运动先验确保 $\mathbf{A}$ 始终良条件——即便没有任何测量，也能给出轨迹的先验解。

---

English

A minimal SLAM example with $K=3$ poses and $M=3$ landmarks has 9 cost terms: 3 motion prior terms (one per step, including the initial prior on ${\mathbf{T}}_0$) and 6 measurement terms. Unlike BA, the motion prior guarantees $\mathbf{A}$ is always well-conditioned.

---

10.3 本章小结 / Chapter Summary §

中文

问题	方法	结构	关键工具
BA	最大似然	箭头形，${\mathbf{A}}_{22}$ 块对角	Schur 补 / Cholesky
SLAM	最大后验	同上 + ${\mathbf{A}}_{11}$ 块三对角	Schur 补 / Cholesky
插值 BA	+ 插值约束	仍保持箭头形	插值矩阵 $\mathbf{I}$

核心技术共性：

1. 所有问题均用 GN 法迭代；位姿通过指数映射更新，路标通过线性加法更新。
2. 稀疏结构由问题物理结构决定（路标各自独立 → ${\mathbf{A}}_{22}$ 块对角；运动先验链 → ${\mathbf{A}}_{11}$ 块三对角）。
3. Schur 补将复杂度从 $O((6K+3M{)}^3)$ 降至 $O(K^3+K^{2}M)$。

---

English

Problem	Criterion	Structure	Key tool
BA	ML	Arrowhead, ${\mathbf{A}}_{22}$ block-diag	Schur/Cholesky
SLAM	MAP	+ ${\mathbf{A}}_{11}$ block-tridiag	Schur/Cholesky
Interpolated BA	ML + constraint	Arrowhead preserved	Interpolation matrix

All problems use Gauss-Newton iteration with left $SE(3)$ perturbations for poses. Sparsity is a consequence of physical structure: independent landmarks give block-diagonal ${\mathbf{A}}_{22}$; motion priors give block-tridiagonal ${\mathbf{A}}_{11}$. The Schur complement reduces complexity from $O((6K+3M{)}^3)$ to $O(K^3+K^{2}M)$.

---

下一章将把连续时间 GP 先验引入三维轨迹估计，得到 STEAM 算法。/ The next chapter introduces continuous-time GP priors for 3D trajectory estimation, leading to the STEAM algorithm.