Skip to the lesson
FixTheRobot Explore Robotics / Localization & SLAM / M6 Visual and Visual-Inertial SLAM / M6-T7

Localization & SLAM · Theory · M6

M6-T7 — Visual-Inertial SLAM

Filter or smoother, the design questions are the same: what to keep, what to forget, and how to forget it without lying.

  • Lesson 7 of 9
  • Module 6: Visual and Visual-Inertial SLAM
  • Theory
  • About 9 minutes to read

M6-T6 explained why a camera and an IMU belong together. This lesson looks at how production-quality systems are built. They fall into two families: filters, which keep a compact state and a covariance and discard the past after every update (MSCKF, OpenVINS); and smoothers, which optimise over a window of recent keyframes and compress older ones into a prior (OKVIS, VINS-Mono, Kimera-VIO, ORB-SLAM3). Their differences in cost, accuracy and consistency are the main thing to understand.

Learning objectives

After this lesson, you should be able to:

  • Describe the multi-state constraint Kalman filter and how it avoids putting features in the state.
  • Explain sliding-window optimisation and marginalisation with a Schur complement.
  • Say why loop closure in visual-inertial systems is a four-degree-of-freedom pose graph.
  • Compare filters and smoothers for consistency, accuracy, cost and loop closure support.

The problem and the intuition

A VIO system cannot keep everything. After a minute of motion there are thousands of poses and tens of thousands of points; optimising all of them in real time is impossible, and most of them no longer matter for the current position. The question is how to forget. A filter forgets by summarising: it keeps a mean and covariance of the current state (and a few recent camera poses) and throws away everything else. A smoother forgets by marginalising: it keeps a window of recent keyframes in an optimisation and converts the oldest keyframe into a single prior over the remaining ones.

Forgetting is not free. If you forget in the wrong way, for instance by linearising at different points in different places, the estimator invents information that is not there: it becomes inconsistent (over-confident) and drifts in yaw and position which it believes it has observed. Much of the design of a good VIO system is about forgetting honestly.

Filters and smoothers differ in how they forget. Honest forgetting needs consistent linearisation points; relinearising inside a window is what gives smoothers their accuracy.

Filter versus sliding-window optimisationLeft, a filter keeps a compact state with a covariance and discards old poses after each update. Right, a sliding window keeps a few keyframes and recent points and re-optimises them, marginalising the oldest keyframe into a prior.Filter (MSCKF)Sliding window (VINS-Mono, OKVIS) recent camera poses in the state update with a feature tracknullspace projection removes the feature single linearisation point per state (FEJ)cost ~ window size, no relinearisation oldest keyframe marginalised into a prior iterate over the windowrelinearise, robust loss loop closure: 4-DoF pose graphcost ~ window × iterations
A filter and a sliding-window smoother, side by side.

The algorithms: MSCKF, sliding windows, loop closure

Problem. Estimate the trajectory, velocity and IMU biases (and, with loop closure, a consistent map) from camera and IMU data, at real-time rates and bounded cost.

Assumptions. As in M6-T6: calibrated camera and IMU with a known extrinsic and time offset, white-plus-random-walk IMU noise, a static visual scene. Outlier features are rejected before estimation.

MSCKF (Mourikis and Roumeliotis, 2007)

The state holds the current IMU state (orientation, position, velocity, biases) and the poses of the last N camera frames, the “sliding window of clones”. Features are not in the state. When a feature track ends, it is triangulated from its observations, and its reprojection residuals r (2m values for m observations) are linearised as r ≈ Hx δx + Hf δf. Multiplying by a basis N of the left nullspace of Hf removes the feature error: Nᵀ r ≈ Nᵀ Hx δx. This gives 2m − 3 constraints on the camera poses alone, used in a standard EKF update. The cost is linear in the number of features and cubic only in the (small) state size. Modern descendants such as OpenVINS (Geneva et al., 2020) add SLAM features in the state, online calibration and first-estimate Jacobians (FEJ) to keep the observability of yaw and position correct.

Sliding-window optimisation (OKVIS, VINS-Mono)

OKVIS (Leutenegger et al., 2015) and VINS-Mono (Qin et al., 2018) minimise the sum of three kinds of terms over a window of keyframes and the points they see:

min over X, bias, features

∥rprior∥2+∑ ∥rIMU(xi, xi+1, bi)∥Σ2+∑ ρ(∥rcam(xk, fl)∥Σ2)\lVert r_{\text{prior}}\rVert ^{2}+\sum \,\lVert r_{\text{IMU}}(x_{i},\,x_{i+1},\,b_{i})\rVert ^{2}_{\Sigma }+\sum \,\rho (\lVert r_{\text{cam}}(x_{k},\,f_{l})\rVert ^{2}_{\Sigma })

The IMU terms are preintegrated factors (M6-T6), the camera terms are reprojection errors with a robust loss ρ, and rprior summarises everything that has been marginalised. When the window is full, the oldest keyframe (or a non-keyframe frame) is marginalised with a Schur complement: if the normal equations are partitioned into the variables to remove (m) and to keep (r), the information on the kept variables is Hrr − Hrm Hmm⁻¹ Hmr. This is exact for the current linearisation but fixes it, so marginalisation also uses FEJ. The resulting prior is dense, which is why windows are kept small (about 10 keyframes). VINS-Mono also provides a robust visual-inertial initialisation, online extrinsic and time-offset estimation, and relocalisation.

Loop closure with four degrees of freedom

Because roll and pitch are observable from gravity (M6-T6), a visual-inertial system only drifts in yaw and position. VINS-Mono therefore builds a 4-DoF pose graph over keyframes (x, y, z, yaw): sequential edges from VIO, loop edges from bag-of-words recognition and geometric verification. Optimising it removes the accumulated yaw and position drift without disturbing the well-determined tilt. The 6-DoF graphs of Module 4 would change roll and pitch unnecessarily.

Complete systems

  • ORB-SLAM3 (Campos et al., 2021): feature-based, tightly coupled visual-inertial with local bundle adjustment, bag-of-words loop closing and the multi-map Atlas (M6-T5).
  • Kimera-VIO (Rosinol et al., 2020): a GTSAM factor-graph sliding-window smoother with IMU preintegration, part of a metric-semantic mapping pipeline.
  • OpenVINS: a research MSCKF with extensive online calibration and FEJ.
N
basis of the left nullspace of the feature Jacobian Hf; projecting onto it eliminates the feature.
FEJ
first-estimate Jacobians: use the first estimate of a variable in all Jacobians that involve it, to preserve the correct unobservable directions.
Schur complement
the operation that eliminates variables from a linear system, giving the prior used in marginalisation.
4-DoF graph
pose graph over position and yaw only, since gravity fixes roll and pitch.

Strengths, limitations, failure modes

ApproachStrengthLimitation / failure mode
Filter (MSCKF, OpenVINS)Low and predictable cost; good for embedded; no features in the stateLinearisation cannot be revised; inconsistency without FEJ; loop closure is awkward
Sliding-window optimisation (OKVIS, VINS-Mono)Relinearises inside the window: typically more accurate; robust losses; natural loop closureMore compute; dense marginalisation prior; window size trades accuracy for speed
Full SLAM (ORB-SLAM3)Local map plus loop closing and multi-map; best accuracy on benchmarksMost complex; heavy on memory; relies on features and the bag-of-words vocabulary

A worked example: nullspace projection, marginalisation and 4-DoF correction

  1. Nullspace projection. A feature is observed in m = 5 clone poses. That gives 2 × 5 = 10 scalar residuals. The unknown feature position has 3 parameters, so the left nullspace of Hf (10 × 3) has dimension 10 − 3 = 7: seven independent constraints on the camera poses remain, with the feature eliminated. A feature seen in only 2 frames gives 4 − 3 = 1 constraint (the epipolar constraint, M6-T3), which is why features need several views to be informative.
  2. State size. An MSCKF with the current IMU state (15 values: orientation 3, position 3, velocity 3, two biases 3 + 3) and 10 clones of 6 values each has 15 + 60 = 75 error-state variables. The covariance is 75 × 75, tiny compared with a map of 10 000 points.
  3. Sliding window state. A window of 10 keyframes with 15 variables each (pose, velocity, two biases) and 100 features parameterised by inverse depth has 150 + 100 = 250 variables. The cost is dominated by the feature block, which is handled with a Schur complement over the features (they are only connected to keyframes).
  4. Marginalisation fill-in. Removing the oldest keyframe couples every variable that shared a factor with it. The prior is a dense block on those variables, here the second keyframe plus all points seen by both. Dense priors are the reason the window cannot grow arbitrarily.
  5. 4-DoF correction. After a 400 m loop the VIO has drifted 3 m in position and 4° in yaw, with roll and pitch drift below 0.1°. A 4-DoF pose graph moves the keyframes’ positions and yaws to close the loop and leaves roll and pitch alone; a 6-DoF graph would also change the roll and pitch to match the (noisy) loop measurement, degrading what was well estimated.

Engineering implications and limitations

Engineering implications

  • Pick the family by the budget. A microcontroller or a phone: MSCKF-style filter. A laptop-class CPU or better and a need for loop closure: a sliding-window smoother or ORB-SLAM3.
  • Check consistency. Compute the normalised estimation error squared (M2-T5, M4-T6) against ground truth on a test run; a filter whose NEES is much larger than the state dimension is over-confident and its covariance should not be trusted downstream.
  • Size the window. Too short and drift grows; too long and it cannot run in real time. Measure the time per optimisation and keep a margin.
  • Estimate extrinsics and time offset online if they cannot be calibrated precisely, but start from a good calibration (M6-T8).
  • Provide an independent absolute reference for long runs: GPS fusion or a pre-built map; loop closure only helps if the place is revisited.
  • Version the configuration. VIO behaviour is dominated by parameters (noise densities, feature counts, window size); record them with every dataset.

Limits

All of these systems assume a mostly static scene, a calibrated rig and well-behaved sensor noise. Rolling-shutter distortion, motion blur in the IMU-camera time offset, and dynamic objects all violate the models. Benchmarks such as EuRoC show errors of a few centimetres over a room, but outdoors at scale the unobservable drift and the rare-failure behaviour are what limit deployment (M6-T9).

Common misconceptions

“A filter is just a worse optimiser.”

A well-designed filter is fast and accurate; smoothers win by relinearising inside a window, and filters win on cost and predictability.

“Marginalising a keyframe loses nothing.”

It loses the ability to relinearise it, and creates a dense prior that must be handled with first-estimate Jacobians to stay consistent.

“Loop closure should correct all six degrees of freedom.”

Roll and pitch are observable from gravity; only position and yaw drift, so a 4-DoF graph is the right model.

“If the covariance is small the estimate is good.”

An inconsistent estimator reports a small covariance while drifting. Check NEES on ground-truth data.

Engineering takeaways

  1. Filters (MSCKF) keep a small state and eliminate features by nullspace projection; smoothers optimise a window and marginalise the oldest keyframe.
  2. Marginalisation by Schur complement gives a dense prior; first-estimate Jacobians keep it consistent.
  3. Loop closure for VIO is a four-degree-of-freedom pose graph because gravity fixes roll and pitch.
  4. ORB-SLAM3, VINS-Mono, OKVIS, Kimera-VIO and OpenVINS differ in how they trade accuracy, cost and loop closure.
  5. Check consistency, not just error: an over-confident estimator is dangerous for downstream planning.

References and further reading

Primary papers and authoritative documentation.

Knowledge check

Three conceptual questions. Write an answer, then reveal the explanation. Your answers stay in your browser.

Question 1

A feature is seen in 6 clone poses in an MSCKF. How many independent constraints on the poses does it give after nullspace projection?

Explanation

It gives 2 × 6 = 12 residuals and 3 unknown feature parameters, so 12 − 3 = 9 independent constraints on the camera poses, with the feature itself eliminated.

Question 2

Why is the loop-closure graph in VINS-Mono four-dimensional per node rather than six?

Explanation

Gravity makes roll and pitch observable, so VIO does not drift in them. The drifting quantities are the position (3) and the yaw (1), so the loop correction acts only on those four degrees of freedom and does not corrupt the well-estimated tilt.

Question 3

What goes wrong if a sliding-window marginalisation uses a fresh linearisation point each time instead of the first estimate?

Explanation

The prior then encodes information in directions (global position and yaw) that the data do not actually constrain, so the estimator becomes over-confident and its error grows without the covariance reflecting it. First-estimate Jacobians preserve the correct unobservable directions.

What’s next

These estimators all assume well-calibrated sensors and consistent time stamps. The next lesson covers how those are obtained and what happens when they are wrong.

Practice this lesson

Diagnose Camera–IMU Synchronization Errors

Estimate the camera-IMU time offset from angular-velocity signals and see what an uncorrected offset does.

Next lesson

M6-T8 — Sensor Calibration and Synchronization

Describe intrinsic, extrinsic and temporal calibration and the errors each produces when wrong.

Nothing below is required to finish this lesson.