视觉 / 惯性里程计Visual & Inertial Odometry

一架无人机在室内飞，没有 GPS；一副 AR 眼镜要把虚拟物体钉在桌面上，头一转，画面必须在十几毫秒内跟上；一台火星直升机要在一到三分钟的飞行里知道自己飞了多远。它们都需要里程计：不依赖外部基础设施，只靠自带的传感器，连续地估计“我相对起点移动了多少、现在朝向哪里”。

两种最便宜、最常见的传感器各能解决一半。IMU（9.4 节）每秒 200 到 1000 次报告角速度和“比力”（加速度减去重力），响应快、不受光照影响，可把它积分两次求位置，零偏会让误差无界地增长，几十秒后就没法用。相机（第 10、11 章）看着世界里的固定特征，用 PnP、三角化和光束法平差就能算出自己的运动，可单目相机测不出绝对尺度，快速运动时图像会模糊，看向白墙时什么也算不出来。

两者的缺点恰好互补：IMU 提供公制尺度、重力方向和高频的运动预测，相机提供不漂的几何约束，并把 IMU 的零偏“校准”出来。把它们放进同一个估计问题，就是视觉惯性里程计（VIO）。本节先分别看两个“半传感器”怎样失败（第 2 节），再跑一个完整的紧耦合滑动窗口 VIO（第 3 节），然后拆开它的三块关键零件：IMU 预积分、初始化和可观性（第 4 节）。

为了能在浏览器里实时跑、并且看得清楚，本节的世界是一个竖直的平面：\(x\) 向右，\(z\) 向上，重力 \(\mathbf g=(0,-9.81)\) 在平面内。机器人（一架“二维无人机”）的状态是位置 \(\mathbf p\)、速度 \(\mathbf v\) 和俯仰角 \(\theta\)；IMU 有一个陀螺和两个加速度计；相机朝下，是一台“一维相机”：每个地面特征只给出一个像素坐标 \(u\)。所有公式在第 5 节都会写回三维。

A drone flies indoors without GPS; a pair of AR glasses must pin a virtual object to a table, and when the head turns the picture must follow within a dozen milliseconds; a Mars helicopter must know how far it has flown during a flight of one to three minutes. They all need odometry: a continuous estimate of "how far have I moved from the start, and which way am I facing", using only on-board sensors and no external infrastructure.

The two cheapest and most common sensors each solve half of it. An IMU (Lesson 9.4) reports angular rate and "specific force" (acceleration minus gravity) 200 to 1000 times a second; it reacts fast and does not care about lighting, but when integrated twice to get position its bias makes the error grow without bound, and after a few dozen seconds it is useless. A camera (Chapters 10 and 11) watches fixed features in the world and recovers its own motion with PnP, triangulation and bundle adjustment, but a single camera cannot measure absolute scale, its images blur under fast motion, and facing a blank wall it computes nothing.

Their weaknesses are complementary: the IMU provides metric scale, the direction of gravity and a high-rate motion prediction; the camera provides geometric constraints that do not drift, and "calibrates out" the IMU's bias. Putting both into one estimation problem is visual-inertial odometry (VIO). This lesson first shows how each "half-sensor" fails on its own (Section 2), then runs a complete tightly coupled sliding-window VIO (Section 3), and then takes apart its three key components: IMU preintegration, initialization and observability (Section 4).

To run in real time in the browser and stay easy to see, the world of this lesson is a vertical plane: \(x\) to the right, \(z\) up, gravity \(\mathbf g=(0,-9.81)\) inside the plane. The robot (a "2D drone") has position \(\mathbf p\), velocity \(\mathbf v\) and pitch angle \(\theta\); its IMU has one gyro and two accelerometers; its camera looks down and is a "1D camera": each ground feature gives a single pixel coordinate \(u\). Section 5 writes every formula back in 3D.

“捷联”（strapdown）的意思是 IMU 直接固定在机体上，跟着机体一起转。要从它的读数得到位置，每个采样周期做三件事：用陀螺的角速度更新姿态 \(\theta\)；用姿态把加速度计测得的比力 \(\mathbf f\)（机体系）转到世界系，再加回重力，得到真正的加速度 \(\mathbf a=R(\theta)\mathbf f+\mathbf g\)；把加速度积分一次得速度，再积分一次得位置。比力这个名字很关键：静止的加速度计读到的不是 0，而是向上的 \(9.81\) m/s²（支撑力），自由落体时它才读 0。

9.4 节已经看到，陀螺零偏 \(b_g\) 积分一次变成线性增长的角度误差。到了位置，事情更糟。加速度计零偏 \(b_a\) 被积分两次：位置误差 \(\tfrac12b_at^2\)。陀螺零偏的危害更隐蔽：姿态错了 \(b_gt\)，你就把重力的一部分（约 \(g\,b_gt\)）当成了水平加速度，再积分两次，位置误差 \(\tfrac16g\,b_gt^3\)。下图让一架二维无人机沿 8 字形飞 60 秒，IMU 以 200 Hz 采样，实时做捷联积分。

"Strapdown" means the IMU is bolted to the body and turns with it. To get position from its readings, every sample period does three things: update the attitude \(\theta\) with the gyro rate; use the attitude to rotate the accelerometer's specific force \(\mathbf f\) (body frame) into the world frame and add gravity back to get the true acceleration \(\mathbf a=R(\theta)\mathbf f+\mathbf g\); integrate the acceleration once for velocity and again for position. The name specific force matters: an accelerometer at rest does not read 0 but \(9.81\) m/s² upward (the supporting force); only in free fall does it read 0.

Lesson 9.4 showed that a gyro bias \(b_g\), integrated once, becomes a linearly growing angle error. For position things get worse. An accelerometer bias \(b_a\) is integrated twice: position error \(\tfrac12b_at^2\). A gyro bias does its damage more quietly: with the attitude wrong by \(b_gt\), you mistake part of gravity (about \(g\,b_gt\)) for horizontal acceleration, and after two more integrations the position error is \(\tfrac16g\,b_gt^3\). Below, a 2D drone flies a figure of eight for 60 s; the IMU is sampled at 200 Hz and the strapdown integration runs live.

视觉里程计（VO）把第 11 章的工具串成一条流水线：初始化用最初几帧求出相机运动和一批三维点（11.1 节的对极几何、11.3 节的三角化）；之后每来一帧，先用恒速模型预测位姿，再对已有的地图点做 PnP（11.4 节）；视差足够的新特征被三角化加入地图；最后对最近若干帧做一次局部 BA（11.6 节）把误差压下去。下图就是这样一条流水线，在下方的“一维图像”里你能看到每帧跟踪到的特征。

有一个细节是二维世界特有的：一维相机看一个点只得到一个数，两帧图像对相机运动没有任何约束（三维里 11.1 节的五点法 / 八点法可以做到），所以这里的初始化是对最初 1 秒的 11 帧做一次小的 BA（从一个粗略的猜测出发）。初始化时必须人为规定一个尺度，这里规定“第 0 帧到第 10 帧的基线长 1 个单位”。从此以后，VO 的整条轨迹和地图都以这个单位计量：你不知道它对应多少米。拖动“显示尺度”滑块，把整个 VO 结果放大，可以让它和真实轨迹重合；可是重投影误差一点也不变，因为图像本来就不含尺度（11.6 节的规范自由度）。更糟的是，每一次三角化和 PnP 都会带进一点尺度误差，它们一帧一帧地累积：尺度会漂。要说明的是，本图的一维相机每帧只有很少、很弱的约束，所以尺度漂得比真实的单目 VO 快得多：真实系统在二维图像上跟踪几百个特征，同样会漂，但慢得多。漂移的存在是真实的，它的速度被这个玩具世界夸大了。

Visual odometry (VO) chains the tools of Chapter 11 into a pipeline: initialization recovers the camera motion and a first set of 3D points from the first few frames (epipolar geometry, Lesson 11.1; triangulation, Lesson 11.3); then for every new frame a constant-velocity model predicts the pose, PnP against the existing map points refines it (Lesson 11.4); new features with enough parallax are triangulated into the map; finally a local BA over the most recent frames squeezes the error down (Lesson 11.6). The figure below is such a pipeline; the "1D image" at the bottom shows the features tracked in each frame.

One detail is peculiar to the 2D world: a 1D camera gets a single number per point, so two frames put no constraint at all on the camera motion (in 3D the five-point or eight-point method of Lesson 11.1 does it). Initialization here is therefore a small BA over the 11 frames of the first second (started from a rough guess). Initialization must declare a scale; here, "the baseline from frame 0 to frame 10 is 1 unit". From then on the whole VO trajectory and map are measured in this unit, and you do not know how many metres it is. Drag the "display scale" slider to blow the VO result up until it lies on the true path; the reprojection error does not change at all, because images contain no scale (the gauge freedom of Lesson 11.6). Worse, every triangulation and every PnP brings in a little scale error, and these accumulate frame by frame: the scale drifts. Note that the 1D camera of this figure gets only a few weak constraints per frame, so its scale drifts far faster than in a real monocular VO, which tracks hundreds of features in a 2D image: real systems drift too, but much more slowly. The existence of the drift is real; its speed is exaggerated by this toy world.

下图运行的是一个真实的 VIO 估计器，结构和 VINS-Mono、OKVIS 这类系统相同，只是降到了二维。窗口里保存最近 \(W\) 个关键帧（这里每帧都是关键帧，10 Hz），每个关键帧的状态是 \(\mathbf x_k=(\mathbf p_k,\mathbf v_k,\theta_k)\)，再加上一组共享的 IMU 零偏 \(\mathbf b=(b_{a,x},b_{a,z},b_g)\) 和窗口里看到的地面特征位置 \(\mathbf l_m\)。每来一帧，估计器最小化三类残差平方和（6.9 节的因子图）：

• 视觉残差：每个特征在每个关键帧里的预测像素减去观测像素（权重 1/σ²，σ = 1 px）。直接用像素而不是先算出一个 VO 位姿，这叫紧耦合。
• IMU 残差：相邻两个关键帧之间，IMU 积分出来的位移、速度和转角应当与两帧状态之差一致（权重来自 IMU 噪声的传播，第 4、5 节的“预积分”）。
• 先验残差：窗口滑走的那些旧状态留下的信息。

最后一项是本图的重点。窗口满了，最老的关键帧必须离开，否则问题会越来越大。有两种做法：边缘化，把最老的状态用 Schur 补（11.6 节）消去，它通过各个因子提供的信息变成一个作用在剩余变量上的高斯先验；直接丢弃，把它连同它的观测和 IMU 因子一起扔掉，为了固定规范，再把新的最老关键帧的位置和俯仰角钉在当前估计上。两个估计器吃的是完全相同的数据。

The figure runs a real VIO estimator with the same structure as systems such as VINS-Mono and OKVIS, only reduced to 2D. The window keeps the last \(W\) keyframes (here every frame is a keyframe, at 10 Hz); each keyframe has state \(\mathbf x_k=(\mathbf p_k,\mathbf v_k,\theta_k)\), plus one shared set of IMU biases \(\mathbf b=(b_{a,x},b_{a,z},b_g)\) and the positions \(\mathbf l_m\) of the ground features seen in the window. At every frame the estimator minimizes the sum of squares of three kinds of residuals (the factor graph of Lesson 6.9):

• Visual residuals: for each feature in each keyframe, predicted pixel minus observed pixel (weight 1/σ², σ = 1 px). Using pixels directly, instead of first computing a VO pose, is called tight coupling.
• IMU residuals: between two consecutive keyframes, the displacement, velocity change and rotation integrated from the IMU must agree with the difference of the two states (weights from propagating the IMU noise; "preintegration", Sections 4 and 5).
• Prior residual: the information left behind by old states that slid out of the window.

The last item is the point of this figure. When the window is full, the oldest keyframe has to leave, or the problem would grow forever. There are two ways: marginalize it, eliminating the oldest state with a Schur complement (Lesson 11.6) so that the information its factors carried becomes a Gaussian prior on the remaining variables; or drop it, throwing it away together with its observations and its IMU factor, and, to fix the gauge, nailing the new oldest keyframe's position and pitch at their current estimates. Both estimators eat exactly the same data.

FIG 3 的优化器每一帧要迭代好几次，每次迭代都会改动窗口里每个关键帧的速度和姿态。如果 IMU 残差是“从关键帧 \(i\) 的当前估计出发，把中间 20 个采样（或 1 秒的 200 个）重新积分一遍，看是否到达关键帧 \(j\)”，每次迭代都要把整个窗口的 IMU 数据重新积分。预积分（Lupton 与 Sukkarieh，2012；Forster 等，2017）的观察是：位移可以拆成三部分，

The optimizer in FIG 3 iterates several times per frame, and every iteration changes the velocity and attitude of every keyframe in the window. If the IMU residual were "start from the current estimate of keyframe \(i\), re-integrate the 20 samples in between (or 200 for one second), and see whether you arrive at keyframe \(j\)", every iteration would have to re-integrate the IMU data of the whole window. The observation behind preintegration (Lupton & Sukkarieh, 2012; Forster et al., 2017) is that the displacement splits into three parts,

前两项只用到起点的状态，第三项 \(\Delta\mathbf p_{ij}\) 只用到 IMU 读数和零偏，它是“在关键帧 \(i\) 的机体系里，假装没有重力、初速度为零时，比力把你推了多远”。起点的速度和姿态怎样变，\(\Delta\mathbf p_{ij}\) 都不用重算。下图把这条链画出来：拖动蓝色箭头的端点改变 \(\mathbf v_i\)，拖动青色机体轴的端点改变 \(\theta_i\)，预积分结果与从头重积分的结果始终重合。

唯一的麻烦是零偏：\(\Delta\mathbf p_{ij}\) 依赖零偏，而零偏也是优化变量。解决办法是对零偏做一阶修正：预积分时顺便算出 \(\partial\Delta\mathbf p/\partial\mathbf b\)，零偏估计变化 \(\delta\mathbf b\) 时用 \(\Delta\mathbf p+\tfrac{\partial\Delta\mathbf p}{\partial\mathbf b}\delta\mathbf b\) 代替重积分。拖动零偏滑块看这个近似有多好。

The first two terms use only the starting state; the third, \(\Delta\mathbf p_{ij}\), uses only the IMU readings and the bias. It is "how far the specific force pushed you, measured in keyframe \(i\)'s body frame, pretending there is no gravity and no initial velocity". However the starting velocity and attitude change, \(\Delta\mathbf p_{ij}\) need not be recomputed. The figure draws this chain: drag the tip of the blue arrow to change \(\mathbf v_i\), drag the end of the cyan body axis to change \(\theta_i\); the preintegrated result and a full re-integration from scratch always coincide.

The one complication is the bias: \(\Delta\mathbf p_{ij}\) depends on the bias, and the bias is also being optimized. The fix is a first-order correction: while preintegrating, also compute \(\partial\Delta\mathbf p/\partial\mathbf b\), and when the bias estimate changes by \(\delta\mathbf b\), use \(\Delta\mathbf p+\tfrac{\partial\Delta\mathbf p}{\partial\mathbf b}\delta\mathbf b\) instead of re-integrating. Drag the bias sliders to see how good this approximation is.

FIG 3 的估计器是从真值出发的。真实系统开机时，它既不知道速度，也不知道重力朝哪（俯仰角），更不知道 VO 的一个单位是多少米。常见的办法（VINS-Mono 的“视觉惯性对齐”）是先让纯视觉跑一小段，得到按未知尺度 \(s\) 计量的位置 \(\bar{\mathbf p}_k\) 和姿态，再把上面的预积分关系当作关于速度、重力和尺度的线性方程来解：

The estimator of FIG 3 started from the truth. A real system that has just been switched on knows neither its velocity, nor which way gravity points (its pitch), nor how many metres one VO unit is. A common recipe (the "visual-inertial alignment" of VINS-Mono) first runs vision alone for a short while, giving positions \(\bar{\mathbf p}_k\) in an unknown unit \(s\) and attitudes, and then solves the preintegration relations above as linear equations in the velocities, gravity and scale:

未知数是每个关键帧的速度、重力向量 \(\mathbf g\) 和尺度 \(s\)。有一种情形下它注定失败：如果真实运动是匀速直线，那么把 \(s\) 和所有速度同时放大同一个倍数，两组方程都照样成立，尺度不可观。加速度计只有在你真的加速时，才“知道”一米有多长。

The unknowns are the velocity of each keyframe, the gravity vector \(\mathbf g\) and the scale \(s\). There is one case in which this must fail: if the true motion is a constant velocity in a straight line, scaling \(s\) and all velocities by the same factor still satisfies both sets of equations, and the scale is unobservable. The accelerometer only "knows" how long a metre is when you actually accelerate.

FIG 3 里位置误差慢慢增长，俯仰角误差却有界。这背后是可观性：如果把整个解（所有关键帧、速度、地图点）沿某个方向一起移动，所有残差都不变，那么这个方向上的信息量为零，估计误差只能像随机游走一样累积。用 6.9 节的话说，信息矩阵 \(H=J^\top WJ\) 在这个方向上有零特征值（11.6 节里 BA 的 7 维规范自由度就是这样的方向）。下图在真值处为一个 7 个关键帧的小批量问题算出 \(H\)，画出最小的特征值，并检查四个“整体动作”：沿 \(x\) 平移、沿 \(z\) 平移、整体旋转、整体缩放。

In FIG 3 the position error slowly grows while the pitch error stays bounded. Behind this is observability: if moving the whole solution (all keyframes, velocities and map points) together along some direction leaves every residual unchanged, there is zero information along that direction and the estimation error can only accumulate like a random walk. In the language of Lesson 6.9, the information matrix \(H=J^\top WJ\) has a zero eigenvalue along it (the 7-dimensional gauge freedom of BA in Lesson 11.6 consists of such directions). The figure computes \(H\) at the true state of a small batch problem with 7 keyframes, plots its smallest eigenvalues, and tests four "whole-solution motions": translate along \(x\), translate along \(z\), rotate everything, scale everything.

三维里，机体姿态 \(R={}^{W}R_{B}\in\SO\)，IMU 测得角速度 \(\boldsymbol\omega_m\) 和比力 \(\mathbf f_m\)（都在机体系），模型是 \(\boldsymbol\omega_m=\boldsymbol\omega+\mathbf b_g+\mathbf n_g\)，\(\mathbf f_m=R^\top(\mathbf a-\mathbf g)+\mathbf b_a+\mathbf n_a\)。捷联方程：

In 3D the body attitude is \(R={}^{W}R_{B}\in\SO\); the IMU measures the angular rate \(\boldsymbol\omega_m\) and the specific force \(\mathbf f_m\) (both in the body frame) with the model \(\boldsymbol\omega_m=\boldsymbol\omega+\mathbf b_g+\mathbf n_g\), \(\mathbf f_m=R^\top(\mathbf a-\mathbf g)+\mathbf b_a+\mathbf n_a\). The strapdown equations:

离散实现时姿态用指数映射更新，\(R_{k+1}=R_k\Exp\big((\boldsymbol\omega_k-\mathbf b_g)\delta t\big)\)（5.6 节），不能把三个角速度当欧拉角的导数去积。在悬停附近做小角度分析，零偏造成的误差为：

In discrete time the attitude is updated with the exponential map, \(R_{k+1}=R_k\Exp\big((\boldsymbol\omega_k-\mathbf b_g)\delta t\big)\) (Lesson 5.6); the three rates must not be integrated as if they were derivatives of Euler angles. A small-angle analysis near hover gives the bias-induced errors:

第二行是白噪声的均方根：加速度噪声积分两次是“速度随机游走”的积分，方差 \(\sigma_a^2t^3/3\)；陀螺噪声先变成角度随机游走 \(\sigma_g\sqrt t\)，乘以 \(g\) 再积分两次，方差 \(g^2\sigma_g^2t^5/20\)。FIG 1 里实测与这三条公式的比值在 20 秒后都在 1% 以内（5 mg、0.05 °/s：10 秒后分别是 2.45 m 和 1.44 m）。代价最高的是 \(t^3\) 项：所以惯导系统最贵的部件是陀螺，导航级陀螺的零偏稳定性在 0.01 °/h 量级，而手机里的 MEMS 陀螺是它的几百到几千倍。

The second line is the RMS of white noise: accelerometer noise integrated twice is the integral of a "velocity random walk", variance \(\sigma_a^2t^3/3\); gyro noise first becomes an angle random walk \(\sigma_g\sqrt t\), which is multiplied by \(g\) and integrated twice, variance \(g^2\sigma_g^2t^5/20\). In FIG 1 the measured errors agree with these formulas to within 1% after 20 s (5 mg and 0.05 °/s give 2.45 m and 1.44 m after 10 s). The costliest term is \(t^3\), which is why the most expensive part of an inertial system is its gyro: a navigation-grade gyro has a bias stability of the order of 0.01 °/h, and the MEMS gyro in a phone is hundreds to thousands of times worse.

在关键帧 \(i\) 与 \(j\) 之间，以 \(i\) 的机体系为参照累积 IMU 采样（Forster 等 2017 的记号）：

Between keyframes \(i\) and \(j\), accumulate the IMU samples in the body frame of \(i\) (the notation of Forster et al. 2017):

\(J^{\cdot}_{\cdot}\) 是预积分量对零偏的雅可比，在累加时顺带递推出来（旋转部分用 5.7 节的右雅可比）。残差的协方差 \(\Sigma_{ij}\) 也递推：把误差 \(\boldsymbol\eta=(\delta\boldsymbol\phi,\delta\mathbf v,\delta\mathbf p)\) 写成 \(\boldsymbol\eta_{k+1}=A_k\boldsymbol\eta_k+B_k\mathbf n_k\)，就有 \(\Sigma_{k+1}=A_k\Sigma_kA_k^\top+B_k\Sigma_nB_k^\top\)（4.5 节的不确定性传播，\(\Sigma_n\) 由噪声密度除以 \(\delta t\) 得到）。FIG 3 的 IMU 因子就以 \(\Sigma_{ij}^{-1}\) 为权重。二维里旋转只是一个角度，\(\Delta\theta_{ij}=\sum(\omega_k-b_g)\delta t\) 对 \(b_g\) 是精确线性的；非线性只来自 \(\Delta\mathbf p\) 和 \(\Delta\mathbf v\) 里的 \(R(\Delta\theta)\)，所以 FIG 4 里一阶修正的误差按 \(\delta b_g^2\) 增长：1 °/s 时 0.14 mm，10 °/s 时 14 mm。实际系统在 \(\delta\mathbf b\) 大到一定程度时才重新预积分。

The \(J^{\cdot}_{\cdot}\) are the Jacobians of the preintegrated quantities with respect to the bias, propagated alongside the sums (the rotation part uses the right Jacobian of Lesson 5.7). The covariance \(\Sigma_{ij}\) of the residual is propagated too: write the error \(\boldsymbol\eta=(\delta\boldsymbol\phi,\delta\mathbf v,\delta\mathbf p)\) as \(\boldsymbol\eta_{k+1}=A_k\boldsymbol\eta_k+B_k\mathbf n_k\), then \(\Sigma_{k+1}=A_k\Sigma_kA_k^\top+B_k\Sigma_nB_k^\top\) (the uncertainty propagation of Lesson 4.5, with \(\Sigma_n\) the noise density divided by \(\delta t\)). The IMU factors of FIG 3 are weighted by \(\Sigma_{ij}^{-1}\). In 2D a rotation is just an angle and \(\Delta\theta_{ij}=\sum(\omega_k-b_g)\delta t\) is exactly linear in \(b_g\); the nonlinearity comes only from \(R(\Delta\theta)\) inside \(\Delta\mathbf p\) and \(\Delta\mathbf v\), so in FIG 4 the first-order error grows as \(\delta b_g^2\): 0.14 mm at 1 °/s, 14 mm at 10 °/s. Real systems re-preintegrate only once \(\delta\mathbf b\) has grown beyond some threshold.

滑动窗口估计器在每一帧求解一个最大后验问题（4.6 节）：

A sliding-window estimator solves one maximum-a-posteriori problem (Lesson 4.6) per frame:

\(\pi\) 是投影（11.4、11.6 节），\(\rho\) 是鲁棒核（11.5 节）。这里视觉残差直接是像素，所以叫紧耦合。松耦合则让一个独立的 VO 先算出位姿，再把这个位姿当作测量交给 IMU 滤波器；它更简单、模块化，但丢掉了两样东西：每个特征各自的不确定度和它们之间的相关性（VO 位姿的协方差往往只是粗略估计），以及 VO 失败时仍然有用的那一两个特征。图像只剩两三个特征时，紧耦合系统还能用它们约束 IMU，松耦合系统已经拿不到位姿了。

边缘化。把要移走的变量记为 \(\mathbf m\)，留下的记为 \(\mathbf r\)。在当前估计处，把所有连到 \(\mathbf m\) 的因子（加上旧先验）线性化成 \(H\boldsymbol\delta=-\mathbf g\)，再像 11.6 节消去 BA 中的点那样做 Schur 补：

\(\pi\) is the projection (Lessons 11.4 and 11.6) and \(\rho\) a robust loss (Lesson 11.5). The visual residuals here are raw pixels, hence tight coupling. Loose coupling instead lets an independent VO compute a pose first and hands that pose to an IMU filter as a measurement. It is simpler and modular, but it throws away two things: the individual uncertainty of every feature and the correlations between them (a VO pose covariance is usually only a rough guess), and the one or two features that remain useful when VO fails. With only two or three features in the image a tightly coupled system still uses them to constrain the IMU; a loosely coupled one no longer has a pose to hand over.

Marginalization. Call the variables to be removed \(\mathbf m\) and those that stay \(\mathbf r\). At the current estimate, linearize every factor attached to \(\mathbf m\) (plus the old prior) into \(H\boldsymbol\delta=-\mathbf g\), then take the Schur complement, exactly as the points were eliminated from BA in Lesson 11.6:

新先验的能量是 \(\mathbf g^{\star\top}\boldsymbol\delta_r+\tfrac12\boldsymbol\delta_r^\top H^\star\boldsymbol\delta_r\)，其中 \(\boldsymbol\delta_r\) 是相对边缘化时线性化点的偏差：先验从此固定在那个线性化点上。这带来两个工程问题。第一，填充：被边缘化的关键帧看到过的所有地图点、下一个关键帧和零偏，在 \(H^\star\) 里两两相连，先验是稠密的；FIG 3 的实现保留这些点，VINS-Mono 等系统还会丢掉一部分观测来保持稀疏。第二，一致性：如果之后同一个变量的雅可比在新的线性化点上计算，先验和新因子“看到”的零空间不一致，估计器会在不可观方向（三维里的偏航）上凭空获得信息，变得过于自信。First-Estimates Jacobian（FEJ）让同一变量的雅可比始终在第一次估计处计算，以此保持零空间（Huang、Mourikis 与 Roumeliotis）。FIG 3 的二维世界里不可观方向只有两个平移，雅可比与平移无关，所以这个问题不会出现。“丢弃”相当于令 \(H^\star=0\)，再人为钉住一个位姿。

滤波：MSCKF。多状态约束卡尔曼滤波（Mourikis 与 Roumeliotis，2007）走另一条路：EKF（4.8 节）的状态是当前 IMU 状态加最近 \(N\) 个相机位姿的“克隆”，地图点不进状态。一条特征轨迹结束时，先用克隆的位姿三角化出它的位置 \(\hat{\mathbf l}\)，把这条轨迹的所有残差堆起来线性化：\(\mathbf r=H_x\delta\mathbf x+H_l\delta\mathbf l+\mathbf n\)；再左乘 \(H_l\) 左零空间的基 \(A^\top\)（\(A^\top H_l=0\)），得到 \(\mathbf r_o=A^\top H_x\delta\mathbf x+A^\top\mathbf n\)，与特征位置无关，直接做标准 EKF 更新。代价随特征数线性增长。它和滑动窗口优化的本质区别是：滤波器每个测量只线性化一次（相当于一次 Gauss-Newton 迭代），优化器则在窗口内反复重新线性化，精度更高、算得也更多（Strasdat、Montiel 与 Davison 2012 在单目 SLAM 中做过这样的比较）。OpenVINS 走滤波路线，VINS-Mono、OKVIS 走优化路线。

The new prior's energy is \(\mathbf g^{\star\top}\boldsymbol\delta_r+\tfrac12\boldsymbol\delta_r^\top H^\star\boldsymbol\delta_r\), with \(\boldsymbol\delta_r\) the deviation from the linearization point at marginalization time: from then on the prior is frozen at that linearization point. This raises two engineering issues. First, fill-in: every map point the marginalized keyframe saw, the next keyframe and the bias become pairwise connected in \(H^\star\), so the prior is dense; the implementation in FIG 3 keeps them all, while systems such as VINS-Mono drop some observations to stay sparse. Second, consistency: if a variable's Jacobians are later evaluated at a new linearization point, the prior and the new factors "see" different null spaces, and the estimator gains spurious information along an unobservable direction (yaw, in 3D) and becomes overconfident. First-Estimates Jacobians (FEJ) evaluate every Jacobian of a variable at its first estimate to keep the null space intact (Huang, Mourikis & Roumeliotis). In FIG 3's 2D world the only unobservable directions are the two translations, and no Jacobian depends on translation, so the problem cannot appear there. "Dropping" amounts to setting \(H^\star=0\) and then nailing one pose by hand.

Filtering: the MSCKF. The multi-state constraint Kalman filter (Mourikis & Roumeliotis, 2007) takes another road: the state of the EKF (Lesson 4.8) is the current IMU state plus "clones" of the last \(N\) camera poses, and map points are not in the state. When a feature track ends, its position \(\hat{\mathbf l}\) is triangulated from the cloned poses, and all residuals of the track are stacked and linearized: \(\mathbf r=H_x\delta\mathbf x+H_l\delta\mathbf l+\mathbf n\). Multiplying by a basis \(A^\top\) of the left null space of \(H_l\) (\(A^\top H_l=0\)) gives \(\mathbf r_o=A^\top H_x\delta\mathbf x+A^\top\mathbf n\), which no longer involves the feature, and a standard EKF update follows. The cost grows linearly with the number of features. The essential difference from sliding-window optimization: a filter linearizes each measurement once (one Gauss-Newton iteration, in effect), while an optimizer relinearizes repeatedly inside the window, gaining accuracy at more computation (Strasdat, Montiel & Davison 2012 made this comparison for monocular SLAM). OpenVINS follows the filtering route; VINS-Mono and OKVIS follow the optimization route.

三维 VIO 的整体动作：取绕重力方向（世界 \(z\) 轴）的旋转 \(R_z(\psi)\) 和任意平移 \(\mathbf t\)，令 \(R_k'=R_z R_k\)，\(\mathbf p_k'=R_z\mathbf p_k+\mathbf t\)，\(\mathbf v_k'=R_z\mathbf v_k\)，\(\mathbf l_m'=R_z\mathbf l_m+\mathbf t\)。因为 \(R_z\mathbf g=\mathbf g\)，

The whole-solution motions of 3D VIO: take a rotation \(R_z(\psi)\) about the gravity direction (world \(z\)) and any translation \(\mathbf t\), and set \(R_k'=R_zR_k\), \(\mathbf p_k'=R_z\mathbf p_k+\mathbf t\), \(\mathbf v_k'=R_z\mathbf v_k\), \(\mathbf l_m'=R_z\mathbf l_m+\mathbf t\). Because \(R_z\mathbf g=\mathbf g\),

IMU 残差不变，相机残差显然也不变（相机系里的点坐标没变），所以 \(H\) 有一个 4 维零空间 \(N\)：3 维位置加偏航（Jones 与 Soatto 2011；Kelly 与 Sukhatme 2011；Hesch 等 2014）。绕其他轴的旋转会改变重力在机体系里的方向，因而可观；尺度在有加速度时可观，匀速直线时不可观（FIG 5、FIG 6）。另外还有一些退化运动会让更多方向不可观，例如没有转动时加速度计零偏与姿态（重力方向）分不开。FIG 6 的二维结果是它的缩影：重力在平面内时零空间 2 维，重力垂直平面时 3 维（多了“偏航”），匀速时再加上尺度。

初始化就是在这些可观方向上给出一个足够好的初值。FIG 5 的线性方程组未知数为 \(\mathbf y=(\mathbf v_0,\dots,\mathbf v_{K-1},\mathbf g,s)\)，按噪声加权求最小二乘 \(\hat{\mathbf y}=(A^\top WA)^{-1}A^\top W\mathbf b\)，\((A^\top WA)^{-1}\) 的最后一个对角元就是 \(\sigma_s^2\)。匀速时 \(A\) 有零空间 \(\big(\delta s,\ \delta\mathbf v_k=\delta s\,\mathbf v/s,\ \delta\mathbf g=0\big)\)，只有噪声让它不完全奇异。VINS-Mono 还会在得到 \(\mathbf g\) 后把它的模长约束为 9.81 再细化方向，并先用纯旋转关系估计陀螺零偏；ORB-SLAM3 则把初始化写成一个只含惯性量的 MAP 问题（Campos 等，2020）。静止起步是常见的“第 0 步”：静止几秒，加速度计的平均值给出重力方向，陀螺的平均值给出陀螺零偏，然后再运动起来提供尺度。

The IMU residuals are unchanged, and so obviously are the camera residuals (point coordinates in the camera frame do not change), so \(H\) has a 4-dimensional null space \(N\): 3D position plus yaw (Jones & Soatto 2011; Kelly & Sukhatme 2011; Hesch et al. 2014). Rotations about other axes change the direction of gravity in the body frame and are therefore observable; scale is observable under acceleration and unobservable at constant velocity in a straight line (FIG 5, FIG 6). There are also degenerate motions that make more directions unobservable; for example, without any rotation the accelerometer bias cannot be separated from the attitude (the gravity direction). The 2D results of FIG 6 are a miniature of this: with gravity in the plane the null space is 2-dimensional, with gravity perpendicular to the plane it is 3-dimensional ("yaw" joins), and constant velocity adds scale.

Initialization means providing a good enough starting value along the observable directions. The linear system of FIG 5 has unknowns \(\mathbf y=(\mathbf v_0,\dots,\mathbf v_{K-1},\mathbf g,s)\) and is solved by noise-weighted least squares \(\hat{\mathbf y}=(A^\top WA)^{-1}A^\top W\mathbf b\); the last diagonal entry of \((A^\top WA)^{-1}\) is \(\sigma_s^2\). At constant velocity \(A\) has the null vector \(\big(\delta s,\ \delta\mathbf v_k=\delta s\,\mathbf v/s,\ \delta\mathbf g=0\big)\), and only noise keeps it from being exactly singular. VINS-Mono additionally constrains the norm of \(\mathbf g\) to 9.81 and refines its direction, and estimates the gyro bias first from rotation alone; ORB-SLAM3 instead poses initialization as an inertial-only MAP problem (Campos et al., 2020). Starting at rest is a common "step 0": stand still for a few seconds, take the average accelerometer reading as the gravity direction and the average gyro reading as the gyro bias, then move to provide scale.