01 从状态估计到因子图：VIO 为什么是一张图

1. 我们到底想估计什么

视觉惯性里程计，通常记作 VIO，即 Visual-Inertial Odometry。它的目标是在没有全局定位传感器的情况下，仅依靠相机和 IMU，估计载体在时间上的运动轨迹。

假设系统在离散时刻 k=0,1,...,Nk=0,1,\dots,Nk=0,1,...,N 维护一组状态。这里 kkk 表示第 kkk 个关键时刻，通常可以理解为第 kkk 个图像帧或关键帧。一个典型 VIO 状态可以写成：

xk= $pk, qk, vk, ba,k, bg,k$ \mathbf{x}_k = \left $\\mathbf{p}_k,\\, \\mathbf{q}_k,\\, \\mathbf{v}_k,\\, \\mathbf{b}_{a,k},\\, \\mathbf{b}_{g,k} \\right$ xk= $pk,qk,vk,ba,k,bg,k$

其中：

xk\mathbf{x}_kxk 表示第 kkk 个时刻的完整状态；
pk∈R3\mathbf{p}_k \in \mathbb{R}^3pk∈R3 表示 IMU 坐标系原点在世界坐标系下的位置；
qk∈SO(3)\mathbf{q}_k \in SO(3)qk∈SO(3) 或单位四元数表示 IMU 坐标系相对于世界坐标系的姿态；
vk∈R3\mathbf{v}_k \in \mathbb{R}^3vk∈R3 表示 IMU 在世界坐标系下的速度；
ba,k∈R3\mathbf{b}_{a,k} \in \mathbb{R}^3ba,k∈R3 表示加速度计零偏；
bg,k∈R3\mathbf{b}_{g,k} \in \mathbb{R}^3bg,k∈R3 表示陀螺仪零偏。

除了每一帧的运动状态，VIO 还可能估计相机和 IMU 之间的外参：

Tic= $pic, qic$ \mathbf{T}_{ic} = \left $\\mathbf{p}_{ic},\\, \\mathbf{q}_{ic} \\right$ Tic= $pic,qic$

其中：

Tic\mathbf{T}_{ic}Tic 表示从相机坐标系到 IMU 坐标系的刚体变换；
pic∈R3\mathbf{p}_{ic} \in \mathbb{R}^3pic∈R3 表示相机原点在 IMU 坐标系下的位置；
qic\mathbf{q}_{ic}qic 表示相机坐标系相对于 IMU 坐标系的姿态。

单目视觉还需要估计特征点的深度。常见做法不是直接估计深度 dld_ldl，而是估计逆深度：

λl=1dl \lambda_l = \frac{1}{d_l} λl=dl1

其中：

lll 表示第 lll 个特征点；
dld_ldl 表示该特征点在其首次观测相机坐标系下的深度；
λl\lambda_lλl 表示该特征点的逆深度。

使用逆深度有两个重要好处。第一，远处点的深度趋向无穷大时，逆深度趋向 000，数值上更稳定。第二，单目系统在初始化阶段深度不确定性很大，逆深度通常比直接深度更适合优化。

于是，一个滑动窗口中的整体未知量可以抽象为：

X={x0,x1,...,xN, λ1,λ2,...,λM, Tic} \mathcal{X} = \left\{ \mathbf{x}_0,\mathbf{x}_1,\dots,\mathbf{x}N,\, \lambda_1,\lambda_2,\dots,\lambda_M,\, \mathbf{T}{ic} \right\} X={x0,x1,...,xN,λ1,λ2,...,λM,Tic}

其中：

X\mathcal{X}X 表示当前优化问题中的全部未知变量；
NNN 表示窗口内状态的最后一个时刻索引；
MMM 表示当前窗口内参与优化的特征点数量。

VIO 的核心问题就是：给定 IMU 测量和图像特征观测，估计最可能的 X\mathcal{X}X。

2. 为什么估计问题可以写成最大后验

传感器测量有噪声，所以我们不能要求状态完全满足所有观测。更合理的说法是：寻找一组状态，使它在概率意义下最能解释已经发生的测量。

记所有测量为 Z\mathcal{Z}Z，包括 IMU 测量和视觉观测。我们想求：

X∗=arg⁡max⁡Xp(X∣Z) \mathcal{X}^{*} = \arg\max_{\mathcal{X}} p(\mathcal{X}\mid \mathcal{Z}) X∗=argXmaxp(X∣Z)

其中：

X∗\mathcal{X}^{*}X∗ 表示最优状态估计；
p(X∣Z)p(\mathcal{X}\mid \mathcal{Z})p(X∣Z) 表示在观测 Z\mathcal{Z}Z 已知时，状态 X\mathcal{X}X 的后验概率；
arg⁡max⁡\arg\maxargmax 表示寻找使后验概率最大的变量取值。

根据贝叶斯公式：

p(X∣Z)=p(Z∣X)p(X)p(Z) p(\mathcal{X}\mid \mathcal{Z}) = \frac{p(\mathcal{Z}\mid \mathcal{X})p(\mathcal{X})}{p(\mathcal{Z})} p(X∣Z)=p(Z)p(Z∣X)p(X)

其中：

p(Z∣X)p(\mathcal{Z}\mid \mathcal{X})p(Z∣X) 是似然，表示给定状态后产生这些测量的概率；
p(X)p(\mathcal{X})p(X) 是先验，表示没有看到当前测量前对状态的已有认知；
p(Z)p(\mathcal{Z})p(Z) 是观测本身出现的概率，它不依赖待优化变量 X\mathcal{X}X。

因为 p(Z)p(\mathcal{Z})p(Z) 对优化变量是常数，所以最大化后验等价于：

X∗=arg⁡max⁡Xp(Z∣X)p(X) \mathcal{X}^{*} = \arg\max_{\mathcal{X}} p(\mathcal{Z}\mid \mathcal{X})p(\mathcal{X}) X∗=argXmaxp(Z∣X)p(X)

为了把乘法变成加法，通常取负对数：

先取对数。因为 log⁡(⋅)\log(\cdot)log(⋅) 是单调递增函数，所以最大值的位置不会改变：

arg⁡max⁡Xp(Z∣X)p(X)=arg⁡max⁡Xlog⁡(p(Z∣X)p(X)) \arg\max_{\mathcal{X}} p(\mathcal{Z}\mid \mathcal{X})p(\mathcal{X}) = \arg\max_{\mathcal{X}} \log \left( p(\mathcal{Z}\mid \mathcal{X})p(\mathcal{X}) \right) argXmaxp(Z∣X)p(X)=argXmaxlog(p(Z∣X)p(X))

利用对数乘法展开公式：

log⁡(ab)=log⁡a+log⁡b \log(ab)=\log a+\log b log(ab)=loga+logb

可以得到：

arg⁡max⁡Xlog⁡(p(Z∣X)p(X))=arg⁡max⁡X $logp(Z∣X)+logp(X)$ \arg\max_{\mathcal{X}} \log \left( p(\mathcal{Z}\mid \mathcal{X})p(\mathcal{X}) \right) = \arg\max_{\mathcal{X}} \left $\\log p(\\mathcal{Z}\\mid \\mathcal{X}) + \\log p(\\mathcal{X}) \\right$ argXmaxlog(p(Z∣X)p(X))=argXmax $logp(Z∣X)+logp(X)$

最大化一个函数，等价于最小化它的相反数：

arg⁡max⁡Xf(X)=arg⁡min⁡X−f(X) \arg\max_{\mathcal{X}} f(\mathcal{X}) = \arg\min_{\mathcal{X}} -f(\mathcal{X}) argXmaxf(X)=argXmin−f(X)

因此：

X∗=arg⁡min⁡X−log⁡p(Z∣X)−log⁡p(X) \mathcal{X}^{*} = \arg\min_{\mathcal{X}} -\log p(\mathcal{Z}\mid \mathcal{X}) -\log p(\mathcal{X}) X∗=argXmin−logp(Z∣X)−logp(X)

如果测量噪声服从高斯分布：

n∼N(0,Σ) \mathbf{n} \sim \mathcal{N}(\mathbf{0},\mathbf{\Sigma}) n∼N(0,Σ)

其中：

n\mathbf{n}n 表示测量噪声；
N\mathcal{N}N 表示高斯分布；
0\mathbf{0}0 表示均值为零；
Σ\mathbf{\Sigma}Σ 表示协方差矩阵。

那么一个残差 r(X)\mathbf{r}(\mathcal{X})r(X) 对应的负对数似然可以写成：

−log⁡p(r(X))≡12r(X)⊤Σ−1r(X) -\log p(\mathbf{r}(\mathcal{X})) \equiv \frac{1}{2} \mathbf{r}(\mathcal{X})^\top \mathbf{\Sigma}^{-1} \mathbf{r}(\mathcal{X}) −logp(r(X))≡21r(X)⊤Σ−1r(X)

为了看清楚这个式子从哪里来，先写测量模型：

z=h(X)+n \mathbf{z} = h(\mathcal{X})+\mathbf{n} z=h(X)+n

其中：

z\mathbf{z}z 表示真实传感器测量；
h(X)h(\mathcal{X})h(X) 表示由状态 X\mathcal{X}X 预测出来的测量；
n\mathbf{n}n 表示测量噪声。

定义残差：

r(X)=h(X)−z \mathbf{r}(\mathcal{X}) = h(\mathcal{X})-\mathbf{z} r(X)=h(X)−z

如果噪声是零均值高斯噪声，则残差也可以按同样的协方差建模。设残差维度为 ddd，多元高斯概率密度为：

p(r)=1(2π)d∣Σ∣exp⁡(−12r⊤Σ−1r) p(\mathbf{r}) = \frac{1} {\sqrt{(2\pi)^d|\mathbf{\Sigma}|}} \exp \left( -\frac{1}{2} \mathbf{r}^{\top} \mathbf{\Sigma}^{-1} \mathbf{r} \right) p(r)=(2π)d∣Σ∣ 1exp(−21r⊤Σ−1r)

其中：

ddd 表示残差向量的维度；
∣Σ∣|\mathbf{\Sigma}|∣Σ∣ 表示协方差矩阵 Σ\mathbf{\Sigma}Σ 的行列式；
exp⁡(⋅)\exp(\cdot)exp(⋅) 表示自然指数函数。

对这个概率密度取负对数：

−log⁡p(r)=−log⁡ $1(2π)d∣Σ∣exp(-12r⊤Σ-1r)$ -\log p(\mathbf{r}) = -\log \left $\\frac{1} {\\sqrt{(2\\pi)\^d\|\\mathbf{\\Sigma}\|}} \\exp \\left( -\\frac{1}{2} \\mathbf{r}\^{\\top} \\mathbf{\\Sigma}\^{-1} \\mathbf{r} \\right) \\right$ −logp(r)=−log $(2π)d∣Σ∣ 1exp(-21r⊤Σ-1r)$

展开后得到：

−log⁡p(r)=12r⊤Σ−1r+12log⁡((2π)d∣Σ∣) -\log p(\mathbf{r}) = \frac{1}{2} \mathbf{r}^{\top} \mathbf{\Sigma}^{-1} \mathbf{r} + \frac{1}{2} \log \left( (2\pi)^d|\mathbf{\Sigma}| \right) −logp(r)=21r⊤Σ−1r+21log((2π)d∣Σ∣)

如果协方差 Σ\mathbf{\Sigma}Σ 已知，那么：

12log⁡((2π)d∣Σ∣) \frac{1}{2} \log \left( (2\pi)^d|\mathbf{\Sigma}| \right) 21log((2π)d∣Σ∣)

对优化变量 X\mathcal{X}X 是常数，不影响最优解的位置。因此在优化中可以忽略这个常数项，只保留与 X\mathcal{X}X 有关的二次项：

12r(X)⊤Σ−1r(X) \frac{1}{2} \mathbf{r}(\mathcal{X})^\top \mathbf{\Sigma}^{-1} \mathbf{r}(\mathcal{X}) 21r(X)⊤Σ−1r(X)

其中：

r(X)\mathbf{r}(\mathcal{X})r(X) 表示由状态 X\mathcal{X}X 预测出来的测量与真实测量之间的误差；
Σ−1\mathbf{\Sigma}^{-1}Σ−1 是信息矩阵，协方差越小，信息越大；
上标 ⊤\top⊤ 表示矩阵或向量转置。

因此，VIO 最终变成一个加权最小二乘问题：

X∗=arg⁡min⁡X∑i∥ri(X)∥Σi−12 \mathcal{X}^{*} = \arg\min_{\mathcal{X}} \sum_i \left\| \mathbf{r}i(\mathcal{X}) \right\|^2{\mathbf{\Sigma}_i^{-1}} X∗=argXmini∑∥ri(X)∥Σi−12

其中：

iii 表示第 iii 个约束或第 iii 个因子；
ri(X)\mathbf{r}_i(\mathcal{X})ri(X) 表示第 iii 个约束的残差；
Σi\mathbf{\Sigma}_iΣi 表示第 iii 个约束的噪声协方差；
∥r∥Σ−12\|\mathbf{r}\|^2_{\mathbf{\Sigma}^{-1}}∥r∥Σ−12 表示马氏距离，定义为：

∥r∥Σ−12=r⊤Σ−1r \left\|\mathbf{r}\right\|^2_{\mathbf{\Sigma}^{-1}} = \mathbf{r}^{\top}\mathbf{\Sigma}^{-1}\mathbf{r} ∥r∥Σ−12=r⊤Σ−1r

这一步非常关键：因子图不是凭空出现的工程形式，而是最大后验估计在高斯噪声假设下的自然结果。

3. 什么是因子图

因子图是一种表达优化问题稀疏结构的图模型。它包含两类节点：

变量节点：表示未知量，例如位姿、速度、零偏、特征点逆深度、外参；
因子节点：表示约束，例如 IMU 预积分约束、视觉重投影约束、先验约束。

如果一个因子依赖某些变量，就在图中把这个因子和这些变量连起来。

这里要区分两个容易混用的概念：因子图和图优化。

因子图是问题的表达形式，它回答：

有哪些变量？有哪些约束？每个约束连接哪些变量？ \text{有哪些变量？有哪些约束？每个约束连接哪些变量？} 有哪些变量？有哪些约束？每个约束连接哪些变量？

图优化是求解过程，它回答：

给定这些变量和约束，怎样求出最优状态？ \text{给定这些变量和约束，怎样求出最优状态？} 给定这些变量和约束，怎样求出最优状态？

因此，更准确地说：

因子图=图优化问题的建模方式 \text{因子图} = \text{图优化问题的建模方式} 因子图=图优化问题的建模方式

图优化=在因子图上做非线性最小二乘求解 \text{图优化} = \text{在因子图上做非线性最小二乘求解} 图优化=在因子图上做非线性最小二乘求解

在 VIO 和 SLAM 语境里，大家经常把因子图优化、图优化、factor graph optimization 混着说，是因为建图和求解通常连续发生。但严格区分时，factor graph 更偏向模型结构，graph optimization 更偏向求解过程。

还要注意，因子图并不要求一定保留历史帧。只要有变量和约束，就可以构成因子图。比如单帧问题也可以写成：

X={x0,λ1,λ2,...,λM} \mathcal{X} = \left\{ \mathbf{x}_0,\lambda_1,\lambda_2,\dots,\lambda_M \right\} X={x0,λ1,λ2,...,λM}

其中：

x0\mathbf{x}_0x0 表示当前帧状态；
λl\lambda_lλl 表示第 lll 个特征点的深度或逆深度；
MMM 表示当前帧中参与建模的特征点数量。

如果当前帧有先验、深度观测、已知地图点观测或其他传感器约束，就可以形成因子：

fl(x0,λl) f_l(\mathbf{x}_0,\lambda_l) fl(x0,λl)

这仍然是因子图。

不过，是否能构成因子图和是否有足够信息求解，是两个不同问题。以单目视觉为例，单帧观测通常只能提供 bearing，也就是特征点方向。若某个三维点只被当前帧看到，它可以写成：

Pl=dl $ulvl1$ \mathbf{P}_l = d_l \begin{bmatrix} u_l\\ v_l\\ 1 \end{bmatrix} Pl=dl ulvl1

其中：

Pl\mathbf{P}_lPl 表示第 lll 个空间点在当前相机坐标系下的位置；
dld_ldl 表示该点深度；
ulu_lul 和 vlv_lvl 表示归一化平面坐标。

此时深度 dld_ldl 仍然未知，所以单帧单目图虽然可以被写成因子图，但通常约束不足。滑动窗口 VIO 保留多帧历史，不是因为"有历史帧才叫因子图"，而是因为多帧视觉、IMU 预积分和边缘化先验能提供足够的信息来估计运动、尺度、速度和零偏。

可以这样记：

因子图≠必须包含历史帧 \text{因子图} \neq \text{必须包含历史帧} 因子图=必须包含历史帧

滑窗因子图=包含有限历史帧的一类特殊因子图 \text{滑窗因子图} = \text{包含有限历史帧的一类特殊因子图} 滑窗因子图=包含有限历史帧的一类特殊因子图

对于 VIO，一个典型的因子图可以抽象为：

G=(V,F) \mathcal{G} = \left( \mathcal{V}, \mathcal{F} \right) G=(V,F)

其中：

G\mathcal{G}G 表示因子图；
V\mathcal{V}V 表示变量节点集合；
F\mathcal{F}F 表示因子节点集合。

变量集合可以写成：

V={xk,λl,Tic} \mathcal{V} = \left\{ \mathbf{x}k,\lambda_l,\mathbf{T}{ic} \right\} V={xk,λl,Tic}

因子集合可以写成：

F={fimu,fproj,fprior} \mathcal{F} = \left\{ f_{\text{imu}}, f_{\text{proj}}, f_{\text{prior}} \right\} F={fimu,fproj,fprior}

其中：

fimuf_{\text{imu}}fimu 表示 IMU 预积分因子；
fprojf_{\text{proj}}fproj 表示视觉重投影因子；
fpriorf_{\text{prior}}fprior 表示先验因子，通常来自边缘化。

图优化的目标函数可以写成：

min⁡X(∑k∥rimu,k∥Σimu,k−12+∑l∑(i,j)∈Olρ(∥rproj,li,j∥Σimg−12)+∥rprior∥2) \min_{\mathcal{X}} \left( \sum_k \left\| \mathbf{r}{\text{imu},k} \right\|^2{\mathbf{\Sigma}{\text{imu},k}^{-1}} + \sum{l}\sum_{(i,j)\in\mathcal{O}l} \rho \left( \left\| \mathbf{r}{\text{proj},l}^{i,j} \right\|^2_{\mathbf{\Sigma}{\text{img}}^{-1}} \right) + \left\| \mathbf{r}{\text{prior}} \right\|^2 \right) Xmin k∑∥rimu,k∥Σimu,k−12+l∑(i,j)∈Ol∑ρ( rproj,li,j Σimg−12)+∥rprior∥2

其中：

rimu,k\mathbf{r}_{\text{imu},k}rimu,k 表示第 kkk 和第 k+1k+1k+1 个状态之间的 IMU 残差；
Σimu,k\mathbf{\Sigma}_{\text{imu},k}Σimu,k 表示该 IMU 残差的协方差；
rproj,li,j\mathbf{r}_{\text{proj},l}^{i,j}rproj,li,j 表示第 lll 个特征点在第 iii 帧和第 jjj 帧之间形成的视觉重投影残差；
Ol\mathcal{O}_lOl 表示能够观测到第 lll 个特征点的帧对集合；
Σimg\mathbf{\Sigma}_{\text{img}}Σimg 表示图像观测噪声协方差；
ρ(⋅)\rho(\cdot)ρ(⋅) 表示鲁棒核函数，用于降低外点的影响；
rprior\mathbf{r}_{\text{prior}}rprior 表示历史边缘化留下的先验残差。

4. IMU 因子：把连续运动压缩成相邻状态约束

IMU 的原始测量频率通常远高于相机。假设在第 iii 帧和第 jjj 帧之间有大量 IMU 测量。直接把每个 IMU 测量都放进优化会让问题非常大。因此 VIO 通常使用 IMU 预积分，将一段时间内的 IMU 测量压缩成一个相邻关键帧之间的约束。

IMU 测量模型为：

a^t=Rt⊤(atw−g)+ba,t+na,t \hat{\mathbf{a}}_t = \mathbf{R}t^\top \left( \mathbf{a}t^w-\mathbf{g} \right) + \mathbf{b}{a,t} + \mathbf{n}{a,t} a^t=Rt⊤(atw−g)+ba,t+na,t

ω^t=ωt+bg,t+ng,t \hat{\boldsymbol{\omega}}t = \boldsymbol{\omega}t + \mathbf{b}{g,t} + \mathbf{n}{g,t} ω^t=ωt+bg,t+ng,t

其中：

ttt 表示连续时间；
a^t\hat{\mathbf{a}}_ta^t 表示加速度计测量值；
atw=p¨t\mathbf{a}_t^w=\ddot{\mathbf{p}}_tatw=p¨t 表示载体在世界坐标系下的真实线加速度；
Rt\mathbf{R}_tRt 表示从 IMU 坐标系到世界坐标系的旋转矩阵；
g\mathbf{g}g 表示世界坐标系下的重力加速度向量；
ba,t\mathbf{b}_{a,t}ba,t 表示加速度计零偏；
na,t\mathbf{n}_{a,t}na,t 表示加速度计白噪声；
ω^t\hat{\boldsymbol{\omega}}_tω^t 表示陀螺仪测量值；
ωt\boldsymbol{\omega}_tωt 表示真实角速度；
bg,t\mathbf{b}_{g,t}bg,t 表示陀螺仪零偏；
ng,t\mathbf{n}_{g,t}ng,t 表示陀螺仪白噪声。

这里的符号很容易弄反。加速度计测到的不是世界系线加速度 atw\mathbf{a}_t^watw 本身，而是比力，也就是"去掉重力后的加速度"在 IMU 坐标系下的表达：

ft=Rt⊤(atw−g) \mathbf{f}_t = \mathbf{R}_t^\top \left( \mathbf{a}_t^w-\mathbf{g} \right) ft=Rt⊤(atw−g)

因此静止时 atw=0\mathbf{a}_t^w=\mathbf{0}atw=0。如果此时 IMU 坐标系和世界坐标系重合，即 Rt=I\mathbf{R}_t=\mathbf{I}Rt=I，则：

a^t≈−g \hat{\mathbf{a}}_t \approx - \mathbf{g} a^t≈−g

这正是"加速度计静止时读到重力反方向"的含义。例如若世界系 zzz 轴向上，重力向量定义为：

g= $00-9.81$ \mathbf{g} = \begin{bmatrix} 0\\0\\-9.81 \end{bmatrix} g= 00−9.81

则静止加速度计理想读数为：

g= $009.81$ \mathbf{g} = \begin{bmatrix} 0\\0\\9.81 \end{bmatrix} g= 009.81

反过来，运动方程写成：

atw=Rt(a^t−ba,t−na,t)+g \mathbf{a}_t^w = \mathbf{R}t \left( \hat{\mathbf{a}}t-\mathbf{b}{a,t}-\mathbf{n}{a,t} \right) + \mathbf{g} atw=Rt(a^t−ba,t−na,t)+g

所以下面的 IMU 残差中会出现 −12gΔtij2-\frac{1}{2}\mathbf{g}\Delta t_{ij}^2−21gΔtij2 和 −gΔtij-\mathbf{g}\Delta t_{ij}−gΔtij：它们是在把

pj=pi+viΔtij+12gΔtij2+RiΔpij \mathbf{p}_j = \mathbf{p}i + \mathbf{v}i\Delta t{ij} + \frac{1}{2}\mathbf{g}\Delta t{ij}^2 + \mathbf{R}i\Delta\mathbf{p}{ij} pj=pi+viΔtij+21gΔtij2+RiΔpij

移项到残差左侧后得到的。

预积分给出从时刻 iii 到时刻 jjj 的相对运动量：

Δpij,Δqij,Δvij \Delta\mathbf{p}{ij},\quad \Delta\mathbf{q}{ij},\quad \Delta\mathbf{v}_{ij} Δpij,Δqij,Δvij

其中：

Δpij\Delta\mathbf{p}_{ij}Δpij 表示从 iii 到 jjj 的预积分相对位移；
Δqij\Delta\mathbf{q}_{ij}Δqij 表示从 iii 到 jjj 的预积分相对旋转；
Δvij\Delta\mathbf{v}_{ij}Δvij 表示从 iii 到 jjj 的预积分相对速度变化。

给定两个状态 xi\mathbf{x}_ixi 和 xj\mathbf{x}_jxj，IMU 残差通常写成：

rimu,ij= $rp,ijrq,ijrv,ijrba,ijrbg,ij$ \mathbf{r}{\text{imu},ij} = \begin{bmatrix} \mathbf{r}{p,ij} \\ \mathbf{r}{q,ij} \\ \mathbf{r}{v,ij} \\ \mathbf{r}{ba,ij} \\ \mathbf{r}{bg,ij} \end{bmatrix} rimu,ij= rp,ijrq,ijrv,ijrba,ijrbg,ij

其中：

rp,ij=Ri⊤(pj−pi−viΔtij−12gΔtij2)−Δpij \mathbf{r}_{p,ij} = \mathbf{R}_i^\top \left( \mathbf{p}j-\mathbf{p}i-\mathbf{v}i\Delta t{ij} -\frac{1}{2}\mathbf{g}\Delta t{ij}^2 \right) - \Delta\mathbf{p}{ij} rp,ij=Ri⊤(pj−pi−viΔtij−21gΔtij2)−Δpij

rq,ij=2 vec(Δqij−1⊗qi−1⊗qj) \mathbf{r}{q,ij} = 2\,\text{vec} \left( \Delta\mathbf{q}{ij}^{-1} \otimes \mathbf{q}_i^{-1} \otimes \mathbf{q}_j \right) rq,ij=2vec(Δqij−1⊗qi−1⊗qj)

rv,ij=Ri⊤(vj−vi−gΔtij)−Δvij \mathbf{r}_{v,ij} = \mathbf{R}_i^\top \left( \mathbf{v}j-\mathbf{v}i-\mathbf{g}\Delta t{ij} \right) - \Delta\mathbf{v}{ij} rv,ij=Ri⊤(vj−vi−gΔtij)−Δvij

rba,ij=ba,j−ba,i \mathbf{r}{ba,ij} = \mathbf{b}{a,j}-\mathbf{b}_{a,i} rba,ij=ba,j−ba,i

rbg,ij=bg,j−bg,i \mathbf{r}{bg,ij} = \mathbf{b}{g,j}-\mathbf{b}_{g,i} rbg,ij=bg,j−bg,i

这里：

Ri\mathbf{R}_iRi 是 qi\mathbf{q}_iqi 对应的旋转矩阵；
Δtij\Delta t_{ij}Δtij 表示从时刻 iii 到时刻 jjj 的时间间隔；
⊗\otimes⊗ 表示四元数乘法；
vec(⋅)\text{vec}(\cdot)vec(⋅) 表示取四元数的虚部向量。

IMU 因子的目的，是让相邻状态之间的相对运动符合 IMU 测到的运动。它解决的问题是：相机帧率较低、单目视觉尺度和短时运动约束弱，而 IMU 能提供高频的运动连续性、尺度和重力方向信息。

5. 视觉因子：把同一个空间点的多次观测连起来

视觉测量来自图像中的特征点。假设第 lll 个空间点第一次在第 iii 帧被观测到，其归一化相机坐标为：

uˉi,l= $ui,lvi,l1$ \bar{\mathbf{u}}{i,l} = \begin{bmatrix} u{i,l}\\ v_{i,l}\\ 1 \end{bmatrix} uˉi,l= ui,lvi,l1

其中：

uˉi,l\bar{\mathbf{u}}_{i,l}uˉi,l 表示第 lll 个特征点在第 iii 帧中的齐次归一化坐标；
ui,lu_{i,l}ui,l 和 vi,lv_{i,l}vi,l 表示归一化平面上的横纵坐标。

如果该点的逆深度为 λl\lambda_lλl，则该点在第 iii 帧相机坐标系下的三维坐标为：

Pci,l=1λluˉi,l \mathbf{P}{c_i,l} = \frac{1}{\lambda_l} \bar{\mathbf{u}}{i,l} Pci,l=λl1uˉi,l

其中 Pci,l\mathbf{P}_{c_i,l}Pci,l 表示特征点在第 iii 帧相机坐标系下的三维坐标。

通过外参和位姿，可以把这个点变换到世界坐标系：

Pw,l=Ri(RicPci,l+pic)+pi \mathbf{P}{w,l} = \mathbf{R}i \left( \mathbf{R}{ic}\mathbf{P}{c_i,l} + \mathbf{p}_{ic} \right) + \mathbf{p}_i Pw,l=Ri(RicPci,l+pic)+pi

其中：

Pw,l\mathbf{P}_{w,l}Pw,l 表示第 lll 个特征点在世界坐标系下的位置；
Ric\mathbf{R}_{ic}Ric 表示从相机坐标系到 IMU 坐标系的旋转；
pic\mathbf{p}_{ic}pic 表示相机到 IMU 的平移外参。

再把这个世界点投影到第 jjj 帧相机坐标系：

Pcj,l=Ric⊤(Rj⊤(Pw,l−pj)−pic) \mathbf{P}{c_j,l} = \mathbf{R}{ic}^{\top} \left( \mathbf{R}j^{\top} \left( \mathbf{P}{w,l}-\mathbf{p}j \right) - \mathbf{p}{ic} \right) Pcj,l=Ric⊤(Rj⊤(Pw,l−pj)−pic)

其中 Pcj,l\mathbf{P}_{c_j,l}Pcj,l 表示该点在第 jjj 帧相机坐标系下的坐标。

设：

Pcj,l= $Xj,lYj,lZj,l$ \mathbf{P}{c_j,l} = \begin{bmatrix} X{j,l}\\ Y_{j,l}\\ Z_{j,l} \end{bmatrix} Pcj,l= Xj,lYj,lZj,l

则投影到归一化平面：

π(Pcj,l)= $Xj,l/Zj,lYj,l/Zj,l$ \pi(\mathbf{P}{c_j,l}) = \begin{bmatrix} X{j,l}/Z_{j,l}\\ Y_{j,l}/Z_{j,l} \end{bmatrix} π(Pcj,l)= $Xj,l/Zj,lYj,l/Zj,l$

如果第 jjj 帧真实观测为：

zj,l= $uj,lvj,l$ \mathbf{z}{j,l} = \begin{bmatrix} u{j,l}\\ v_{j,l} \end{bmatrix} zj,l= $uj,lvj,l$

则视觉重投影残差为：

rproj,li,j=π(Pcj,l)−zj,l \mathbf{r}{\text{proj},l}^{i,j} = \pi(\mathbf{P}{c_j,l}) - \mathbf{z}_{j,l} rproj,li,j=π(Pcj,l)−zj,l

视觉因子的目的，是让同一个空间点在不同帧之间的几何关系一致。它解决的问题是：相机能提供丰富的几何约束，帮助估计轨迹形状、姿态变化和空间结构。

6. 鲁棒核：为什么不能完全相信每个视觉匹配

真实视觉系统中会有错误匹配、动态物体、光照变化、遮挡等问题。如果直接最小化所有重投影误差的平方，少量外点就可能严重拉偏结果。

因此通常引入鲁棒核函数：

ρ(s) \rho(s) ρ(s)

其中：

sss 表示平方误差，例如 s=∥r∥2s=\|\mathbf{r}\|^2s=∥r∥2；
ρ(s)\rho(s)ρ(s) 表示对平方误差的鲁棒化代价。

普通最小二乘相当于：

ρ(s)=s \rho(s)=s ρ(s)=s

鲁棒核的特点是：当 sss 较小时，它近似 sss；当 sss 很大时，它增长变慢，从而降低外点影响。

举个简单例子。假设大多数特征点重投影误差在 111 像素以内，但一个误匹配点的误差达到 505050 像素。普通平方误差会让该点贡献 250025002500 的代价，优化器可能为了照顾这个错误点而损害整体轨迹。鲁棒核会压低这个异常点的权重，使系统更关注多数一致的观测。

7. 非线性最小二乘：为什么要线性化

VIO 的残差包含旋转、投影、四元数乘法，因此是非线性的。为了求解：

min⁡X∑i∥ri(X)∥2 \min_{\mathcal{X}} \sum_i \left\| \mathbf{r}_i(\mathcal{X}) \right\|^2 Xmini∑∥ri(X)∥2

我们通常在当前估计 X0\mathcal{X}_0X0 附近做一阶泰勒展开：

ri(X0⊞δX)≈ri(X0)+JiδX \mathbf{r}_i(\mathcal{X}_0 \boxplus \delta\mathcal{X}) \approx \mathbf{r}_i(\mathcal{X}_0) + \mathbf{J}_i\delta\mathcal{X} ri(X0⊞δX)≈ri(X0)+JiδX

其中：

X0\mathcal{X}_0X0 表示当前线性化点；
δX\delta\mathcal{X}δX 表示状态增量；
⊞\boxplus⊞ 表示流形上的加法操作；
Ji\mathbf{J}_iJi 表示第 iii 个残差对状态增量的雅克比矩阵。

把所有残差堆叠起来：

r≈r0+JδX \mathbf{r} \approx \mathbf{r}_0+\mathbf{J}\delta\mathcal{X} r≈r0+JδX

其中：

r\mathbf{r}r 表示总残差向量；
r0\mathbf{r}_0r0 表示当前线性化点处的残差；
J\mathbf{J}J 表示总雅克比矩阵。

最小化线性化后的平方误差：

min⁡δX∥r0+JδX∥2 \min_{\delta\mathcal{X}} \left\| \mathbf{r}_0+\mathbf{J}\delta\mathcal{X} \right\|^2 δXmin∥r0+JδX∥2

令导数为零，得到正规方程：

HδX=g \mathbf{H}\delta\mathcal{X} = \mathbf{g} HδX=g

其中：

H=J⊤J \mathbf{H} = \mathbf{J}^{\top}\mathbf{J} H=J⊤J

g=−J⊤r0 \mathbf{g} = -\mathbf{J}^{\top}\mathbf{r}_0 g=−J⊤r0

这里：

H\mathbf{H}H 表示 Hessian 的 Gauss-Newton 近似；
g\mathbf{g}g 表示负梯度方向；
δX\delta\mathcal{X}δX 表示要求解的状态增量。

有些实现会把右端写成 b=J⊤r0\mathbf{b}=\mathbf{J}^{\top}\mathbf{r}_0b=J⊤r0，并在更新符号上做相应约定。关键不是正负号的形式，而是它来自同一个二次近似问题。

8. 为什么因子图适合 VIO

因子图适合 VIO，有三个根本原因。

第一，VIO 约束天然是局部的。IMU 因子只连接相邻两个时刻，视觉因子只连接观测到同一个特征点的若干相机位姿和该特征点逆深度。这意味着整体矩阵非常稀疏。

第二，VIO 是多传感器融合。IMU、视觉、先验、外参、时间延迟都可以统一成残差项放进同一个目标函数中。

第三，VIO 需要实时运行。因子图不仅表达问题，也暴露了稀疏结构，使 Schur 补、滑动窗口、边缘化等方法有了施展空间。

到这里，我们已经得到一张完整的 VIO 因子图。但新的问题马上出现：如果系统一直运行，状态和特征点会越来越多，优化问题会无限变大。实时系统不能无限保留历史变量。

这就引出下一篇的主题：滑动窗口和边缘化。