VIO and SLAM

Why this matters

The drone knowledge base’s unit on VIN, VIO and SLAM explains the roles of the camera, IMU and data fusion before choosing navigation tools. VIO (visual-inertial odometry) estimates continuous motion from a camera and IMU. SLAM builds a map and localises within it at the same time, and can close loops to reduce drift. UAT 366 used SLAM ideas indoors; this module focuses on depth and scale, the two things that most often make visual navigation go wrong.

Stereo depth

Two cameras separated by a baseline $B$ see the same point at different image positions. The difference is the disparity $d$, and depth is $Z=fB/d$, where $f$ is the focal length in pixels. Because $Z$ is inversely proportional to $d$, a small disparity error causes large depth errors at long range, growing with the square of range, following the relation $\delta Z\approx Z^2\delta d/(fB)$ that Keselman et al. (2017) use to describe Intel RealSense stereo depth cameras.

F_PX, BASE, DD = 640, 0.095, 0.25               # pixels, m, pixels
for z in (2, 5, 10, 20):
    d = F_PX * BASE / z
    dz = z * z / (F_PX * BASE) * DD
    print(f"range {z:>2} m: disparity {d:5.2f} px, depth error about {dz:.3f} m ({dz / z:.1%})")

range  2 m: disparity 30.40 px, depth error about 0.016 m (0.8%)
range  5 m: disparity 12.16 px, depth error about 0.103 m (2.1%)
range 10 m: disparity  6.08 px, depth error about 0.411 m (4.1%)
range 20 m: disparity  3.04 px, depth error about 1.645 m (8.2%)

Single-camera scale

A single camera knows the direction of motion but not the scale: a real house far away and a model house nearby can look the same. VIO solves this with an IMU, which measures acceleration in real units. Comparing visual displacement (unitless) with IMU-integrated displacement over short periods gives a scale factor. There must be enough acceleration for the IMU to measure clearly, so long periods at constant speed make scale hard to estimate.

import numpy as np

imu = np.array([0.52, 1.05, 1.48, 2.10, 2.47, 3.03])                 # m
vis = imu / 3.2 + np.random.default_rng(8).normal(0, 0.01, 6)        # unitless
s = (vis @ imu) / (vis @ vis)
resid = imu - s * vis
print("visual displacements:", np.round(vis, 3))
print(f"estimated scale {s:.3f} (true 3.2), residual RMS {np.sqrt(np.mean(resid ** 2)):.3f} m")
print(f"a 10-unit visual path would be {10 * s:.1f} m")

visual displacements: [0.145 0.315 0.449 0.653 0.749 0.945]
estimated scale 3.250 (true 3.2), residual RMS 0.034 m
a 10-unit visual path would be 32.5 m

Choosing perception sensors

• Stereo gives dense depth at short range and works when there is enough texture and light
• LiDAR gives accurate range farther out, independent of light, but is heavier and more expensive
• A single camera with an IMU is light and cheap but needs enough motion to find scale
• All of them struggle with smooth textureless surfaces, reflective surfaces, water and fog

Module lab

Common mistakes

Summary

• VIO estimates motion from camera and IMU; SLAM adds a map and loop closure
• Stereo depth $Z=fB/d$, with error growing as the square of range
• A single camera does not know scale; an IMU can estimate it when there is enough acceleration
• Choose sensors for the job’s range, lighting and surfaces

Check your understanding

1. With $f$ = 700 px, $B$ = 0.1 m and $d$ = 14 px, what is the depth?
2. If range doubles, by what factor does depth error grow?
3. Why does a single camera not know scale?
4. A 3% scale error on a 200 m path gives what error?
5. How does SLAM differ from VIO?