KinectFusion

KinectFusion (Newcombe et al., 2011) reconstructs a room in real time from a handheld depth camera alone. Every depth map is fused into one volume of truncated signed distances (TSDF fusion), and the camera is tracked by iterative closest point against a surface ray cast from that volume rather than against the previous frame. Running both on a GPU at 30 Hz, it brought dense SLAM of rooms to a consumer sensor.

Problem

A Kinect delivers noisy 640 × 480 depth maps full of holes at 30 Hz (§2.1). Monocular SLAM of the time built sparse point maps (§2.2), and TSDF fusion needed known camera poses. KinectFusion estimates every frame’s 6-DoF pose and fuses it into a dense surface, in constant time per frame for a given volume.

Inputs and Outputs

Input: a stream of depth maps and the depth camera’s intrinsics $K$ ; colour is not used, so the system also works in the dark (§1).
Output: a camera pose per frame and a TSDF volume, from which surfaces are ray cast for display or meshed with marching cubes.

Intuition

Chaining frame-to-frame alignments, as visual odometry does, accumulates error at every step. A view rendered from the fused model is a less noisy, more complete reference than the last frame, and anchors each pose to the whole model.

Algorithm

For each new raw depth map $Z_k$ (the paper’s $R_k$ ; here $R_k$ is a rotation), as in Fig. 3:

Surface measurement. Smooth the depth with a bilateral filter, compute vertex and normal maps, and build a three-level pyramid (§3.2).
Pose estimation. From the previous pose, align the live maps with the maps predicted from the model by projective, point-to-plane ICP, coarse to fine (§3.5).
Surface reconstruction update. Fuse the raw, unfiltered depth into the TSDF at the new pose (§3.3).
Surface prediction. Ray cast the TSDF from the new pose to obtain the vertex and normal maps the next frame is tracked against (§3.4).

Mathematical Formulation

Conventions. Poses here are world-to-camera, $X_c = R_k X_w + \mathbf{t}_k$ . The paper’s $T_{g,k}$ (Eq. 1) is the inverse, camera-to-world, with rotation $R_k^\top$ and translation $-R_k^\top \mathbf{t}_k$ , the camera centre; below, $T_k = T_{g,k}$ .

Surface measurement. The bilateral filter (Eq. 2) weights neighbours $\mathbf{q}$ of pixel $\mathbf{u}$ by $\mathcal{N}_{\sigma_s}(\lVert \mathbf{u} - \mathbf{q} \rVert)\,\mathcal{N}_{\sigma_r}(\lvert Z_k(\mathbf{u}) - Z_k(\mathbf{q}) \rvert)$ , with $\mathcal{N}_\sigma(t) = \exp(-t^2\sigma^{-2})$ . The filtered depth $D_k$ gives vertices and normals (Eqs. 3–4):

V_k(\mathbf{u}) = D_k(\mathbf{u})\, K^{-1} \dot{\mathbf{u}}, \qquad N_k(\mathbf{u}) = \nu\big[(V_k(u{+}1, v) - V_k(u, v)) \times (V_k(u, v{+}1) - V_k(u, v))\big],

where $\dot{\mathbf{u}}$ is the pixel in homogeneous coordinates and $\nu[\mathbf{x}] = \mathbf{x}/\lVert \mathbf{x} \rVert$ . Each coarser pyramid level block-averages the depth at half resolution, using only values within $3\sigma_r$ of the central pixel so that edges are not blurred.

Projective data association. Each live vertex, transformed by the current estimate $\tilde{T}_k$ , is projected into the previous camera and paired with the predicted world-frame vertex $\hat{V}_{k-1}$ and normal $\hat{N}_{k-1}$ at that pixel:

\hat{\mathbf{u}} = \pi\big(K\, T_{k-1}^{-1} \tilde{T}_k \dot{V}_k(\mathbf{u})\big).

A pair is kept if the live vertex is valid, the vertices are within $\varepsilon_d$ and the normals agree within $\varepsilon_\theta$ (Eq. 17); the paper gives no threshold values. Eq. 17 is printed as an inner product of the normals at most $\varepsilon_\theta$ ; since it is meant to reject incompatible normals, we read it as an angle of at most $\varepsilon_\theta$ .

Point-to-plane error. The pose minimizes (Eq. 16, which prints the norm of each term; Eq. 22 squares it)

E(T_k) = \sum_{\mathbf{u}\,:\,\Omega(\mathbf{u}) \neq \text{null}} \Big( \big(T_k \dot{V}_k(\mathbf{u}) - \hat{V}_{k-1}(\hat{\mathbf{u}})\big)^\top \hat{N}_{k-1}(\hat{\mathbf{u}}) \Big)^2 .

Linearizing an incremental rotation, $\tilde{R} \approx I + [\boldsymbol{\omega}]_\times$ , around the current world-frame vertex $\mathbf{p} = \tilde{T}_k \dot{V}_k(\mathbf{u})$ makes each residual linear in $\mathbf{x} = (\boldsymbol{\omega}, \boldsymbol{\tau})$ . Each compatible pair contributes a row $\mathbf{a}^\top = [(\mathbf{p} \times \hat{\mathbf{n}})^\top,\ \hat{\mathbf{n}}^\top]$ and a target $b = \hat{\mathbf{n}}^\top(\hat{\mathbf{v}} - \mathbf{p})$ to the $6 \times 6$ normal equations

\Big(\sum \mathbf{a}\mathbf{a}^\top\Big)\, \mathbf{x} = \sum \mathbf{a}\, b .

The paper’s Eqs. 18–26 are the same system with its own parameter order $(\beta, \gamma, \alpha, t_x, t_y, t_z)$ and the opposite sign for the rotation parameters. The GPU sums the per-pixel terms by a tree reduction and the CPU solves by Cholesky decomposition; pyramid levels 3, 2, 1, coarsest first, run at most 4, 5 and 10 iterations (§3.5). Frame-to-frame tracking is the same procedure with the measured maps $(V_{k-1}, N_{k-1})$ in place of the predicted ones.

Surface reconstruction update. The raw depth, unfiltered so as to keep fine structure (§3.3), is fused by the weighted running average of Curless and Levoy (1996). For a voxel at world point $\mathbf{p}$ , projected to the nearest pixel $\mathbf{x}$ , with $\lambda = \lVert K^{-1}\dot{\mathbf{x}} \rVert$ and camera-frame point $X_c = R_k \mathbf{p} + \mathbf{t}_k$ , the measurement in the batch’s sign convention (positive in free space, as the paper’s text has it) is

\eta = Z_k(\mathbf{x}) - \lambda^{-1} \lVert X_c \rVert, \qquad F_{Z_k}(\mathbf{p}) = \begin{cases} \min(1, \eta/\mu) & \eta \ge -\mu \\ \text{null} & \text{otherwise.} \end{cases}

Dividing by $\lambda$ turns ray distance into depth (§3.3). Capping the accumulated weight at $W_\eta$ (Eq. 13) makes the average a moving average.

A sign inconsistency in the paper. Eq. 6 as printed (with $R_k$ for $Z_k$ ), $\Psi(\lambda^{-1}\lVert \mathbf{t}_{g,k} - \mathbf{p} \rVert - Z_k(\mathbf{x}))$ , is negative in front of the surface, whereas the text of §3.3 and the ray caster of §3.4 (positive to negative) put positive values in free space. With that sign, the condition $\eta \ge -\mu$ of Eq. 9 would keep all the unobserved space behind surfaces and drop free space more than $\mu$ in front. Eq. 9 as printed, $\min(1, \eta/\mu)\,\mathrm{sgn}(\eta)$ , returns $\lvert\eta\rvert/\mu$ for $-\mu \le \eta < 0$ , so it never changes sign. The equations above follow the text: measured minus voxel depth, and $\Psi(\eta) = \min(1, \eta/\mu)$ . This is our check of the printed equations, not an erratum from the authors.

Surface prediction. Each pixel’s ray is marched from the minimum sensor depth until $F$ changes from positive to negative; a back face or leaving the volume ends it without a surface. While $F$ is truncated the ray steps by less than $\mu$ , which cannot skip a zero crossing and needed about six times fewer steps than voxel-by-voxel marching (Fig. 6). The crossing is refined linearly between trilinearly interpolated values $F_t^+ > 0$ and $F_{t+\Delta t}^+ < 0$ (Eq. 15), and the normal is the normalized gradient of $F$ (Eq. 14):

t^* = t - \frac{\Delta t\, F_t^+}{F_{t+\Delta t}^+ - F_t^+}, \qquad \hat{N} = \nu\big[\nabla F(\mathbf{p})\big].

Parameters

Volume resolution and extent. The experiments use $256^3$ voxels unless stated otherwise; timings span $64^3$ to $512^3$ in a volume printed as 3m³ (Fig. 13), and the system works well for rooms printed as ≤7m³ (§6). The paper does not say whether these are cubic metres or cubes 3 m and 7 m on a side.
Truncation and weights. As in TSDF fusion; $\mu$ grows linearly with depth, and a constant weight of 1 gave good results (§3.3).
Rendering range. About 0.4 to 8 m, which bounds ray-casting time per pixel (§3.4).

Complexity

The authors measured constant time per frame for a given voxel resolution (§4.2); Fig. 13’s cumulative plot rises from about 10 ms per frame at $64^3$ to about 24 ms at $512^3$ (read from the plot). Integration is bound by memory, not computation. The authors report over 65 gigavoxels per second, about 2 ms to update a $512^3$ volume, with 16 bits per voxel component; they verified that 6 bits suffice for the distance (§3.3). The paper does not name the GPU it used.

Implementation

The NumPy code renders 61 depth maps (160 × 120, 4 mm Gaussian noise) of a room corner with three boxes, from cameras on a 0.3 m circle about 2.2 m away. The last pose repeats the first, so the gap between their estimates measures loop drift. It implements the four stages: bilateral filter and pyramid, projective point-to-plane ICP with the paper’s iteration counts (our thresholds: 10 cm, 20°), fusion into a 2 cm TSDF ( $\mu$ = 6 cm, weight 1), and ray casting refined by Eq. 15. Drift is the absolute trajectory error (ATE) of the camera centres. It runs in about 30 seconds.

import os
os.environ["OMP_NUM_THREADS"] = "1"        # small arrays: threads only add overhead
import numpy as np

# Scene (metres, z up): a room corner (walls x = 0 and y = 0, floor z = 0) and three boxes.
BOXES = [(np.array([0.50, 0.60, 0.15]), np.array([0.15, 0.20, 0.15])),
         (np.array([1.00, 0.35, 0.25]), np.array([0.12, 0.12, 0.25])),
         (np.array([0.35, 1.10, 0.10]), np.array([0.20, 0.10, 0.10]))]

# Pinhole camera, 160 x 120 pixels; world-to-camera X_c = R X_w + t.
W, H, F = 160, 120, 140.0
CX, CY = (W - 1) / 2, (H - 1) / 2
uu, vv = np.meshgrid(np.arange(W), np.arange(H))
RAYS = np.stack([(uu - CX) / F, (vv - CY) / F, np.ones((H, W))], -1)   # K^-1 [u, v, 1]

def look_at(C, target):
    z = (target - C) / np.linalg.norm(target - C)
    x = np.cross(z, [0, 0, 1.0]); x /= np.linalg.norm(x)
    R = np.stack([x, np.cross(z, x), z])               # rows: camera axes in world
    return R, -R @ C

def render_depth(R, t, scene="room"):
    """Exact depth per pixel by intersecting rays with the walls, floor and boxes."""
    C, D = -R.T @ t, RAYS.reshape(-1, 3) @ R           # centre, world rays with unit depth
    s = np.full(len(D), np.inf)
    with np.errstate(divide="ignore", invalid="ignore"):
        for i in ((0, 1, 2) if scene == "room" else (0,)):   # "plane": the wall x = 0 only
            s = np.minimum(s, np.where(D[:, i] < 0, -C[i] / D[:, i], np.inf))
        for c0, h0 in (BOXES if scene == "room" else []):    # slab test
            t0, t1 = (c0 - h0 - C) / D, (c0 + h0 - C) / D
            near, far = np.minimum(t0, t1).max(1), np.maximum(t0, t1).min(1)
            s = np.minimum(s, np.where((near <= far) & (near > 0), near, np.inf))
    return np.where(s < 6, s, 0.0).reshape(H, W)        # 0 = no measurement

# ---- Surface measurement (Sec. 3.2): bilateral filter, depth pyramid, vertex and normal maps.
def bilateral(Dm, sigma_s=1.5, sigma_r=0.03, r=2):
    num, den = np.zeros_like(Dm), np.zeros_like(Dm)
    P = np.pad(Dm, r)
    for dy in range(-r, r + 1):
        for dx in range(-r, r + 1):
            q = P[r + dy:r + dy + H, r + dx:r + dx + W]
            w = np.exp(-(dx * dx + dy * dy) / sigma_s**2 - (Dm - q) ** 2 / sigma_r**2) * (q > 0)
            num += w * q; den += w
    return np.where(Dm > 0, num / np.maximum(den, 1e-12), 0.0)

def half(Dm, sigma_r=0.03):
    """2x2 block average of depths within 3 sigma_r of the block's first pixel, then subsample."""
    h, w = Dm.shape[0] // 2, Dm.shape[1] // 2
    b = Dm.reshape(h, 2, w, 2).transpose(0, 2, 1, 3).reshape(h, w, 4)
    ok = (b > 0) & (np.abs(b - b[..., :1]) <= 3 * sigma_r) & (b[..., :1] > 0)
    return np.where(ok.any(-1), (b * ok).sum(-1) / np.maximum(ok.sum(-1), 1), 0.0)

def vertex_normal(Dm, level):
    s = 2**level
    x, y = np.meshgrid((np.arange(W // s) - (W / s - 1) / 2) / (F / s),
                       (np.arange(H // s) - (H / s - 1) / 2) / (F / s))
    rays = np.stack([x, y, np.ones_like(x)], -1)
    V = Dm[..., None] * rays                             # Eq. 3, camera frame
    dx, dy = V[:-1, 1:] - V[:-1, :-1], V[1:, :-1] - V[:-1, :-1]
    n = np.cross(dy, dx)                                 # Eq. 4, ordered to face the camera
    N = np.zeros_like(V)
    N[:-1, :-1] = n / np.maximum(np.linalg.norm(n, axis=-1, keepdims=True), 1e-12)
    M = (Dm > 0) & (np.linalg.norm(N, axis=-1) > 0.5)
    M[:-1, :-1] &= (Dm[:-1, 1:] > 0) & (Dm[1:, :-1] > 0)
    return V, N, M

def measure(depth):
    Dl = [bilateral(depth)]
    for _ in range(2):
        Dl.append(half(Dl[-1]))
    return [vertex_normal(Dm, l) for l, Dm in enumerate(Dl)]

# ---- Pose estimation (Sec. 3.5): projective association, point-to-plane, coarse to fine.
EPS_D, EPS_THETA = 0.10, np.radians(20)                 # our values; the paper prints none
ITERS = {2: 4, 1: 5, 0: 10}                             # z_max = [4, 5, 10] for levels [3, 2, 1]

def rotvec(w):
    th = np.linalg.norm(w)
    Kx = np.array([[0, -w[2], w[1]], [w[2], 0, -w[0]], [-w[1], w[0], 0]])
    if th < 1e-12:
        return np.eye(3) + Kx
    return np.eye(3) + np.sin(th) / th * Kx + (1 - np.cos(th)) / th**2 * Kx @ Kx

def normal_system(V, N, M, Rwc, C, ref, Rr, tr, s):
    """6x6 system A^T A x = A^T b over compatible pairs (Eqs. 17, 24-26)."""
    Vr, Nr, Mr = ref
    Vg, Ng = V[M] @ Rwc.T + C, N[M] @ Rwc.T              # live vertices in the world
    Xc = Vg @ Rr.T + tr                                  # into the reference camera
    with np.errstate(divide="ignore", invalid="ignore"):
        u = np.rint((F * Xc[:, 0] / Xc[:, 2] + CX) / s).astype(int)
        v = np.rint((F * Xc[:, 1] / Xc[:, 2] + CY) / s).astype(int)
    ok = (Xc[:, 2] > 0) & (u >= 0) & (u < Mr.shape[1]) & (v >= 0) & (v < Mr.shape[0])
    Vg, Ng, u, v = Vg[ok], Ng[ok], u[ok], v[ok]
    q, n = Vr[v, u], Nr[v, u]
    ok = Mr[v, u] & (np.linalg.norm(Vg - q, axis=1) <= EPS_D) & ((Ng * n).sum(1) >= np.cos(EPS_THETA))
    Vg, q, n = Vg[ok], q[ok], n[ok]
    A = np.hstack([np.cross(Vg, n), n])                  # rows [(V x n)^T, n^T]
    b = ((q - Vg) * n).sum(1)
    return A.T @ A, A.T @ b

def track(meas, ref_full, Rr, tr, Rwc, C):
    """Pose of the live frame (camera-to-world Rwc, C), starting from the previous estimate."""
    for level in (2, 1, 0):
        s = 2**level
        ref = tuple(a[::s, ::s] for a in ref_full)
        for _ in range(ITERS[level]):
            AtA, Atb = normal_system(*meas[level], Rwc, C, ref, Rr, tr, s)
            x = np.linalg.solve(AtA, Atb)                # the paper uses a Cholesky solve
            dR = rotvec(x[:3])
            Rwc, C = dR @ Rwc, dR @ C + x[3:]
    return Rwc, C

# ---- Surface reconstruction update (Sec. 3.3) and surface prediction (Sec. 3.4).
VOX, MU = 0.02, 0.06
ORIGIN = np.array([-0.1, -0.1, -0.1])
DIMS = (116, 116, 81)
GRID = ORIGIN + VOX * np.stack(np.meshgrid(*[np.arange(n) for n in DIMS], indexing="ij"), -1).reshape(-1, 3)

def integrate(Fv, Wv, depth, R, t):
    """Projective TSDF in the shared sign convention (+ in front), F in [-1, 1], w = 1."""
    Xc = GRID @ R.T + t
    with np.errstate(divide="ignore", invalid="ignore"):
        u = np.rint(F * Xc[:, 0] / Xc[:, 2] + CX).astype(int)
        v = np.rint(F * Xc[:, 1] / Xc[:, 2] + CY).astype(int)
    idx = np.flatnonzero((Xc[:, 2] > 0) & (u >= 0) & (u < W) & (v >= 0) & (v < H))
    z = depth[v[idx], u[idx]]
    lam = np.linalg.norm(RAYS[v[idx], u[idx]], axis=1)
    eta = z - np.linalg.norm(Xc[idx], axis=1) / lam      # measured minus voxel depth
    keep = (z > 0) & (eta >= -MU)
    i, f = idx[keep], np.minimum(1.0, eta[keep] / MU)
    Fv[i] = (Wv[i] * Fv[i] + f) / (Wv[i] + 1)
    Wv[i] += 1

def trilinear(Fg, p):
    g = (p - ORIGIN) / VOX
    i0 = np.floor(g).astype(int)
    out = np.ones(len(p))                                # outside the volume: free space
    ok = np.all((i0 >= 0) & (i0 < np.array(DIMS) - 1), axis=1)
    i0, fr = i0[ok], g[ok] - i0[ok]
    acc = 0.0
    for c in range(8):
        o = np.array([(c >> 2) & 1, (c >> 1) & 1, c & 1])
        w = np.prod(np.where(o, fr, 1 - fr), axis=1)
        acc = acc + w * Fg[i0[:, 0] + o[0], i0[:, 1] + o[1], i0[:, 2] + o[2]]
    out[ok] = acc
    return out

def nearest(Fg, p):
    i = np.rint((p - ORIGIN) / VOX).astype(int)
    ok = np.all((i >= 0) & (i < np.array(DIMS)), axis=1)
    out = np.ones(len(p))                                # outside the volume: free space
    out[ok] = Fg[i[ok, 0], i[ok, 1], i[ok, 2]]
    return out

def raycast(Fv, R, t, near=0.4, far=5.0):
    """March each pixel ray (nearest-voxel lookups, long steps while F is truncated);
    refine the + to - crossing with trilinear values (Eq. 15)."""
    Fg = Fv.reshape(DIMS)
    C = -R.T @ t
    d = RAYS.reshape(-1, 3) @ R
    d /= np.linalg.norm(d, axis=1, keepdims=True)
    tt = np.full(len(d), near)
    f_prev = nearest(Fg, C + tt[:, None] * d)
    hit = np.full(len(d), np.nan)
    active = f_prev > 0
    while active.any():
        a = np.flatnonzero(active)
        step = np.where(f_prev[a] >= 0.999, 0.8 * MU, 0.5 * VOX)
        t_new = tt[a] + step
        f_new = nearest(Fg, C + t_new[:, None] * d[a])
        cross = f_new < 0                                # + to - : a visible surface
        c, t0 = a[cross], tt[a[cross]]
        f0 = trilinear(Fg, C + t0[:, None] * d[c])
        f1 = trilinear(Fg, C + t_new[cross][:, None] * d[c])
        good = (f0 > 0) & (f1 < 0)
        hit[c[good]] = t0[good] - step[cross][good] * f0[good] / (f1[good] - f0[good])
        tt[a], f_prev[a] = t_new, f_new
        active[a[cross | (t_new > far)]] = False
    ok = ~np.isnan(hit)
    Vg = np.zeros((H * W, 3)); Ng = np.zeros((H * W, 3))
    Vg[ok] = C + hit[ok, None] * d[ok]
    grad = np.stack([trilinear(Fg, Vg[ok] + VOX * e) - trilinear(Fg, Vg[ok] - VOX * e) for e in np.eye(3)], 1)
    Ng[ok] = grad / np.maximum(np.linalg.norm(grad, axis=1, keepdims=True), 1e-12)   # Eq. 14
    return Vg.reshape(H, W, 3), Ng.reshape(H, W, 3), ok.reshape(H, W)

# ---- The loop, frame-to-frame and frame-to-model, on a closed circular trajectory.
def ate_and_gap(est, gt):
    P, Q = np.array([c for _, c in est]), np.array([c for _, c in gt])
    mp, mq = P.mean(0), Q.mean(0)
    U, _, Vt = np.linalg.svd((Q - mq).T @ (P - mp))
    Ra = U @ np.diag([1, 1, np.sign(np.linalg.det(U @ Vt))]) @ Vt     # rigid SE(3) alignment
    ate = np.sqrt(np.mean(np.sum(((P - mp) @ Ra.T + mq - Q) ** 2, axis=1)))
    return ate, np.linalg.norm(P[-1] - P[0])             # last frame repeats the first pose

def run(seed, sigma=0.004, modes=("frame-to-frame", "frame-to-model"), n=61):
    rng = np.random.default_rng(seed)
    a = np.linspace(0, 2 * np.pi, n)                     # a[-1] = 2 pi: back at the start
    e1, e2 = np.array([0.707, -0.707, 0.0]), np.array([-0.3, -0.3, 0.9])
    gt_wc = [look_at(np.array([1.9, 1.9, 1.2]) + 0.3 * (np.cos(b) * e1 + np.sin(b) * e2),
                     np.array([0.5, 0.5, 0.3])) for b in a]
    depths = []
    for R, t in gt_wc:
        Z = render_depth(R, t)
        depths.append(np.where(Z > 0, Z + rng.normal(0, sigma, Z.shape), 0.0))
    gt = [(R.T, -R.T @ t) for R, t in gt_wc]             # camera-to-world (R_wc, centre)
    out = {}
    for mode in modes:
        est = [gt[0]]                                    # frame 0 defines the world
        Fv, Wv = np.ones(len(GRID)), np.zeros(len(GRID))
        integrate(Fv, Wv, depths[0], *gt_wc[0])
        prev = measure(depths[0])[0]
        for k in range(1, n):
            Rwc, C = est[-1]
            Rr, tr = Rwc.T, -Rwc.T @ C                   # reference camera = previous estimate
            if mode == "frame-to-frame":                 # previous measured maps, in the world
                ref = (prev[0] @ Rwc.T + C, prev[1] @ Rwc.T, prev[2])
            else:                                        # model prediction by ray casting
                ref = raycast(Fv, Rr, tr)
            meas = measure(depths[k])
            est.append(track(meas, ref, Rr, tr, Rwc, C))
            prev = meas[0]
            if mode == "frame-to-model":                 # fuse the raw depth at the new pose
                integrate(Fv, Wv, depths[k], est[-1][0].T, -est[-1][0].T @ est[-1][1])
        out[mode] = 1000 * np.array(ate_and_gap(est, gt))
    return out

print("seed  frame-to-frame: ATE, loop gap (mm)  frame-to-model: ATE, loop gap (mm)")
res = []
for seed in range(3):
    r = run(seed)
    res.append([*r["frame-to-frame"], *r["frame-to-model"]])
    print(f"{seed:4d}  {res[-1][0]:22.1f} {res[-1][1]:9.1f}  {res[-1][2]:22.1f} {res[-1][3]:9.1f}")
res = np.array(res)
print(f"seeds 0-2, ATE range (mm): frame-to-frame {res[:, 0].min():.1f}-{res[:, 0].max():.1f}, "
      f"frame-to-model {res[:, 2].min():.1f}-{res[:, 2].max():.1f}")
r = run(0, sigma=0.0, modes=("frame-to-frame",))["frame-to-frame"]
print(f"noise-free depth, frame-to-frame: ATE {r[0]:.1f} mm, loop gap {r[1]:.1f} mm")

# ---- A single plane leaves three directions of the 6x6 system unconstrained.
rng = np.random.default_rng(0)
ROOM = look_at(np.array([1.9, 1.9, 1.2]), np.array([0.5, 0.5, 0.3]))
WALL = look_at(np.array([1.5, 1.0, 1.0]), np.array([0.0, 1.0, 0.8]))     # facing the wall x = 0
for scene, (R, t), sigma in (("room", ROOM, 0.004), ("plane", WALL, 0.0), ("plane", WALL, 0.004)):
    Z = render_depth(R, t, scene)
    V, N, M = measure(np.where(Z > 0, Z + rng.normal(0, sigma, Z.shape), 0.0))[0]
    Rwc, C = R.T, -R.T @ t
    AtA, _ = normal_system(V, N, M, Rwc, C, (V @ Rwc.T + C, N @ Rwc.T, M), R, t, 1)
    ev = np.linalg.eigvalsh(AtA / M.sum())               # per correspondence
    print(f"{scene:5s} sigma {1000 * sigma:.0f} mm, eigenvalues: " + " ".join(f"{e:8.1e}" for e in ev))

# Track a 5 cm slide along the wall (camera and target both move by 5 cm in y).
SLID = look_at(np.array([1.5, 1.05, 1.0]), np.array([0.0, 1.05, 0.8]))
for sigma in (0.0, 0.004):
    ZA, ZB = render_depth(*WALL, "plane"), render_depth(*SLID, "plane")
    ZA, ZB = [np.where(Z > 0, Z + rng.normal(0, sigma, Z.shape), 0.0) for Z in (ZA, ZB)]
    V, N, M = measure(ZA)[0]
    Rwc, C = WALL[0].T, -WALL[0].T @ WALL[1]
    try:
        _, Ce = track(measure(ZB), (V @ Rwc.T + C, N @ Rwc.T, M), *WALL, Rwc, C)
        print(f"slide, sigma {1000 * sigma:.0f} mm: true motion (0, 50, 0) mm, "
              f"estimated ({', '.join(f'{1000 * x:.1f}' for x in Ce - C)}) mm")
    except np.linalg.LinAlgError as e:
        print(f"slide, sigma {1000 * sigma:.0f} mm: {e}")

Output:

seed  frame-to-frame: ATE, loop gap (mm)  frame-to-model: ATE, loop gap (mm)
   0                    20.6      56.1                     0.6       1.0
   1                    20.5      55.7                     0.6       0.9
   2                    20.4      55.8                     0.6       0.7
seeds 0-2, ATE range (mm): frame-to-frame 20.4-20.6, frame-to-model 0.6-0.6
noise-free depth, frame-to-frame: ATE 4.3 mm, loop gap 4.0 mm
room  sigma 4 mm, eigenvalues:  2.2e-02  4.0e-02  1.3e-01  3.6e-01  9.3e-01  9.9e-01
plane sigma 0 mm, eigenvalues:  6.8e-18  6.0e-06  1.1e-04  6.7e-02  1.9e-01  2.7e+00
plane sigma 4 mm, eigenvalues:  7.5e-04  4.5e-03  1.9e-02  6.6e-02  1.9e-01  2.7e+00
slide, sigma 0 mm: Singular matrix
slide, sigma 4 mm: true motion (0, 50, 0) mm, estimated (0.0, 4.7, -4.7) mm

Drift. Over seeds 0–2, frame-to-frame tracking reaches an ATE of 20.4 to 20.6 mm and places the last frame about 56 mm from the identical first one; frame-to-model tracking stays at 0.6 mm with a gap of at most 1 mm. The seeds agree and noise-free depth cuts the frame-to-frame ATE to 4.3 mm, so in our reading depth noise biases every frame-to-frame alignment alike, and fusion removes most of that noise.

A plane. For the room, the smallest per-correspondence eigenvalue is 0.022. For a noise-free wall, the three smallest are about $10^{-4}$ or less: the two translations along the wall and the rotation about its normal change no point-to-plane residual. Tracking a 5 cm slide along the wall, the noise-free system is singular. With 4 mm noise it is solvable and its third-smallest eigenvalue, 0.019, is close to the room’s smallest, yet the estimate recovers 4.7 of the 50 mm: noisy normals, not geometry, constrain those directions. A null-space check therefore needs a noise-aware threshold, not a test for zeros.

Properties and Behavior

The turntable experiment (§4.1). A fixed Kinect watched a tabletop on a turntable through one rotation (about 19 s, $N$ = 560 frames, $256^3$ voxels); with the volume spanning only the rotating scene, this equals a camera on a precise circle. Frame-to-frame tracking gave a non-circular trajectory and a poor reconstruction (Fig. 8a). With frame-to-model tracking, the last frame, a duplicate of the first, was estimated almost on top of it, with small artefacts where the loop closed (Fig. 8c); feeding the loop through $M$ = 4 times registered the two even more closely (Figs. 8d and 10), without joint optimization. With every 8th frame, frame-to-frame drift dropped sharply, but frame-to-model tracking was, in the authors’ words, drift-free (Fig. 11). A $64^3$ volume, 1/64 of the memory, with every 6th frame degraded gracefully (Fig. 12). The paper reports all this in figures, with no numerical trajectory error.

Failure modes (§4.3). The main failure in ordinary rooms is a large plane filling the view, which leaves three degrees of freedom in the null space of the linear system. The system checks that null space and the size of the increment; if either check fails, the user re-aligns the live view with the last known pose (§3.5).

Limitations

A fixed volume. Memory grows with the cube of the extent: a room, not a building (§6). Moving-volume extensions such as Whelan et al.’s Kintinuous stream voxels off the GPU as a mesh that cannot be streamed back; voxel hashing allocates only blocks near surfaces (Nießner et al., 2013, §2, which describes both).
No loop closure. The authors expect very large sequences to drift, visible as misalignments when loops close (§6). In our reading a fused pose error stays in the volume, since nothing removes a frame’s contribution.
Static scenes. Moving objects appear as ICP outliers (Fig. 7); the weight cap of Eq. 13 lets the model follow changes at the cost of less averaging.

Variants

ElasticFusion (Whelan et al., 2015) keeps frame-to-model tracking but maps with surfels, closing loops by deforming the map through a deformation graph instead of a pose graph.
BundleFusion (Dai et al., 2017) optimizes all camera poses globally in real time, sparse-to-dense and local-to-global, on a voxel-hashing TSDF. When a pose changes, it de-integrates the frame from its old pose and re-integrates it at the new one (arXiv v3, §1 and §5).
DynamicFusion (Newcombe, Fox and Seitz, 2015) drops the static-scene assumption, fusing depth into a canonical model that a dense volumetric warp field maps into each live frame.