A Gaussian that updates itself: streaming NDT statistics with Welford

A voxel grid over a point cloud has an obvious, wasteful default: for every voxel, keep a list of the points that landed in it. For a scan of any size that list is the whole dataset again, just re-sorted into buckets. The alternative I built into my octree extension keeps nothing but a running mean and a running 3x3 covariance per voxel, updated one point at a time, in constant memory, and never revisited. A voxel with ten thousand points in it costs exactly the same forty bytes as a voxel with ten.

This is the classic Normal Distributions Transform representation from the SLAM and scan-matching literature: instead of a point cloud, a field of little Gaussians, one per cell, dense enough to query “how likely is a new point here” anywhere in the volume. My code does not cite this anywhere, which is worth being upfront about, but it is exactly Biber and Straßer’s construction, reimplemented from scratch on top of a chunked octree of my own.

The part worth a post on its own is not the Gaussian, it is how the running mean and covariance get updated. There is an obvious formula for variance that every stats course teaches first, and it is quietly wrong to use here. Showing that, rather than asserting it, is most of this post.

The formula everyone reaches for first, and why it loses

Given a stream of numbers $x_1, x_2, \dots, x_n$, the textbook shortcut for their variance avoids a second pass over the data:

$$\operatorname{Var}(x) = \mathbb{E}[x^2] - \big(\mathbb{E}[x]\big)^2$$

Track a running sum and a running sum of squares, and at the end this is one subtraction. It is exactly what I did not write into update(). Instead, every Chunk stores its running statistics the way Welford’s online algorithm computes them: a count, a mean, and an accumulator conventionally called $M_2$, updated per point as

$$\delta = x_n - \bar{x}_{n-1}, \qquad \bar{x}_n = \bar{x}_{n-1} + \frac{\delta}{n}, \qquad M_{2,n} = M_{2,n-1} + \delta \cdot (x_n - \bar{x}_n)$$

with the variance recovered at any point as $M_2 / n$ (or $M_2/(n-1)$ for the unbiased estimator, which is what the density query below actually uses). The header comment above the storage fields points straight at the source:

/// For details see "Welford's online algorithm" -- https://en.wikipedia.org/wiki/
/// Algorithms_for_calculating_variance#Welford's_online_algorithm
/// Of shape <N>: Storing the counts of observed points in a chunk
torch::Tensor m_obs_count;
/// Of shape <N, SPATIAL_DIMS+N_fts>: Storing the cumulative mean
torch::Tensor m_obs_mean;
/// Of shape <N, UPPER_TRIANGULAR_SIZE>: Stores the upper triangular m2 of the pos delta
torch::Tensor m_obs_pos_m2;

Every one of those three fields is a plain torch::Tensor, and it’s worth saying why a 2020s neural-network library shows up at all in code with no neural network in it: torch::Tensor here is just a resizable, reference-counted typed array with a one-line Python binding, playing the part NumPy or Eigen would in C++. It was already installed for other work, and a pip install . away from a usable extension, which is a perfectly good reason to reach for a 2 GB dependency to avoid writing an allocator.

Both formulas compute the same number, in exact arithmetic. They stop agreeing the moment the numbers involved are not small: count, mean and m2 above are all float, and so is every point that streams in. A real scan’s world coordinates are routinely in the thousands: metres from a UTM or GPS origin, or ScanNet’s absolute frame, while the spread inside one 20 cm voxel might be a few centimetres. That is precisely the shape of number where the naive formula falls apart: two large numbers, subtracted to recover something tiny.

Watching it fail

Two hundred points, drawn from a distribution with variance almost exactly 1. The offset slider adds a constant to every one of them before either estimator sees the data, the same way a voxel far from the origin sees its points. The true variance cannot move, shifting every point by the same amount changes nothing about their spread, but the arithmetic has opinions.

With JavaScript enabled this becomes a chart of naive and Welford variance against an offset added to every point. The naive line degrades and crosses zero into negative variance well before an offset of 5,000; Welford’s line stays flat at the true value the whole way to 20,000.

At an offset of a few thousand, E[x²] and (E[x])² are both somewhere in the tens of millions, and their true difference is still close to 1. float carries about seven significant decimal digits, so once those two numbers agree to seven digits the difference is noise, and past that point it is worse than noise, it can go negative. Welford’s recurrence never forms either large number: every step works on x - mean, which stays about the size of the actual spread no matter how far from the origin the points are. That’s the entire argument for using it, made visible rather than asserted, and it’s a nice thing to have watched happen once with real numbers instead of taking on faith.

Git history says this cost real time to get right: three commits over as many weeks read “Issues with large variance and and stuff,” “Fixed inverse bug. Found coordinate bug,” before landing on “Bunny seems to be working.” Three-by-three covariance math by hand is genuinely easy to get subtly wrong, and there is something satisfying about finding, five years later, exactly which formula the earlier debugging session was fighting.

The multivariate version, per voxel

A voxel needs more than one running number: a 3-vector mean and a symmetric 3x3 covariance. The covariance only needs its upper triangle, six numbers instead of nine, which the header derives from the handshake-problem sum:

/// This comes from getting the upper triangle of a square or 3 + 2 + 1
/// eqn  n +   (n-1) + (n-2) + ... +  2  +  1
/// eqn          1   +   2   + ... + n-2 + n-1 + n
/// 2f = n +    n*n -> f = n(n+1)/2
const uint8_t UPPER_TRIANGULAR_SIZE = ((SPATIAL_DIMS+1)*SPATIAL_DIMS) >> 1;

update() runs the scalar recurrence above once per dimension, then a second nested loop folds in the six cross terms, mixing an already-updated delta for one dimension against the about-to-be-updated delta for another, the standard generalisation of Welford’s algorithm to a covariance matrix:

for (uint8_t dim = 0; dim < SPATIAL_DIMS; dim++) {
    float voxCenter = voxIdxPtr[i*SPATIAL_DIMS+dim];
    voxCenter = (voxCenter*m_voxel_size) + (m_voxel_size/2.0);
    float nv = posPtr[(i%numPts)*SPATIAL_DIMS+dim] - voxCenter;
    float delta = nv - obs_mean_p_oct[dim];
    obs_mean_p_oct[dim] += delta / count;
    float delta2 = nv - obs_mean_p_oct[dim];
    posDelta[dim] = delta;

    for (uint8_t dim2 = 0; dim2 <= dim; dim2++) {
        *obs_pos_m2_oct += posDelta[dim2] * delta2;
        obs_pos_m2_oct++;
    }
}

That last detail, voxCenter, is easy to miss and worth calling out: a voxel’s running mean is stored relative to that voxel’s own centre, not in world coordinates. It’s the same idea as the offset slider above, applied automatically: every voxel keeps its own small local origin, so its statistics never see the large absolute coordinates that made the naive formula fail in the first place. A second, near-identical loop right below folds an arbitrary per-point feature vector, colour, a surface normal, whatever the caller passes, into its own Welford accumulator alongside position:

for (auto ftDim = SPATIAL_DIMS; ftDim<SPATIAL_DIMS+m_num_feats; ftDim++){
    float delta = nvFts[ftDim] - obs_mean_p_oct[ftDim];
    obs_mean_p_oct[ftDim] += delta / count;
    float delta2 = nvFts[ftDim] - obs_mean_p_oct[ftDim];
    for (uint8_t dim = 0; dim < SPATIAL_DIMS; dim++) {
        *obs_feat_m2_oct += posDelta[dim] * delta2;
        obs_feat_m2_oct++;
    }
    *obs_feat_m2_oct += delta * delta2;
    obs_feat_m2_oct++;
}

I never got as far as using that feature accumulator for anything in this post, it is the foundation the segmentation step of this series leans on, distinguishing voxels by colour or normal similarity rather than position alone. The scan pipeline this was built for fed exactly six features per point: RGB and a surface normal estimated by cross-producting neighbouring rays in a depth image, at roughly two-centimetre voxels, deliberately chosen so the result “can be compared directly to Minkowski engine,” in the preprocessing script’s own comment.

Softening the edges: one point, eight voxels

Nothing above says a point can only update one voxel. update() computes eight neighbouring voxel indices for every incoming point and folds it into all eight:

const torch::Tensor ADJACENT_OFFSETS = torch::tensor(
  {0,0,0, 0,0,1, 0,1,0, 0,1,1, 1,0,0, 1,0,1, 1,1,0, 1,1,1},
  {torch::kLong}
).view({SPATIAL_COMB, 1, SPATIAL_DIMS});

torch::Tensor voxIdx = torch::empty({SPATIAL_COMB, numPts, SPATIAL_DIMS}, at::kLong);
voxIdx[0] = torch::round(pos/m_voxel_size);
for (auto i =1; i<SPATIAL_COMB; i++){
    voxIdx[i] = voxIdx[0] - ADJACENT_OFFSETS[i];
}

Every point contributes evidence to the eight cells sharing its nearest grid corner, not just the one cell it happens to fall in. A point sitting exactly on a voxel boundary would otherwise vanish entirely into whichever side won a coin flip; spread across eight neighbours, it softens that edge into a gradient instead. It costs a constant 8x more work per point, which for a running statistic nobody stores is a fair trade.

The two-dimensional widget further down reproduces this, with a toggle to turn it off and watch the cell boundaries harden. Two dimensions means four cells share a corner rather than eight, and the bookkeeping differs slightly from the C++ as well: the source finds a point’s own voxel by rounding, then subtracts fixed offsets to reach the other seven, while the query side (below) finds a voxel by flooring. On a single flat grid it was simpler to pick one floor-aligned tiling throughout and spread a point to the cells sharing its nearest corner directly, which is the same softening idea with nothing a reader would see differently.

Querying the field: a Gaussian, by hand

Once a voxel has at least three points its covariance is trustworthy enough to query, and getProbabilities answers “how likely is this position” by evaluating the multivariate normal density directly, determinant and inverse included, with no linear-algebra library call anywhere in the function:

if (octIdx >= 0 && octree_count[chunkId][octIdx] >= 3.0f) {
    float count = octree_count[chunkId][octIdx] - 1.0f; // Doing minus 1 here to account for est variance
    // ... deltaPtrStart[dim] -= mean_it[dim]; for each spatial dim ...
    // ... cov[dim] = (*pos_m2_it)/count; for each of the 6 upper-triangular entries ...

    double detVar = std::max(1e-20, cov[0]*cov[2]*cov[5]+2.0*cov[1]*cov[4]*cov[3]
                                    -cov[2]*cov[3]*cov[3]-cov[0]*cov[4]*cov[4]-cov[5]*cov[1]*cov[1]);
    double invDet = 1.0/detVar;
    double probx = (cov[2]*cov[5]-cov[4]*cov[4])*deltaPtrStart[0]
                  -(cov[1]*cov[5]-cov[4]*cov[3])*deltaPtrStart[1]
                  +(cov[1]*cov[4]-cov[2]*cov[3])*deltaPtrStart[2];
    // proby, probz: the same cofactor pattern for the other two rows

    probPtr[i] = exp(-0.5*(invDet*probx*deltaPtrStart[0] + invDet*proby*deltaPtrStart[1]
                          + invDet*probz*deltaPtrStart[2])) / (sqrt(detVar)*SQRT_K_2_PI);
}

detVar is the determinant of the symmetric 3x3 covariance, expanded by hand along its first row. probx, proby and probz are the covariance’s adjugate (its transposed cofactor matrix) applied to the offset from the mean, which is exactly what Cramer’s rule gives you for solving $\Sigma^{-1}\delta$ without ever forming $\Sigma^{-1}$ as its own matrix. SQRT_K_2_PI is $(2\pi)^{3/2}$, the normalising constant for a three-dimensional Gaussian, computed once as a literal rather than three calls to pow. The count - 1.0f is the usual Bessel correction, an unbiased estimator rather than the population one this post’s precision demo uses; and the >= 3.0f gate above it is what stops a one-or-two-point voxel, whose covariance is either undefined or a division by a very small number, from ever being queried at all.

The accumulator storage above is float; this function’s own local variables, detVar, invDet, probx, are all double. Reading a float covariance into a double computation is a small, deliberate piece of the same numerical-stability story: the storage format doesn’t need more than seven digits (a voxel’s covariance is a small number by construction, the whole point of the local-centre trick above), but a determinant and an exponential can lose a few digits of their own on the way, so the arithmetic buys itself some headroom back.

Watching a Gaussian field build itself

Everything quoted so far is three-dimensional. SPATIAL_DIMS is 3, a mean is a 3-vector, a covariance has six independent entries, and SQRT_K_2_PI is the normalising constant for a Gaussian in three dimensions. The first widget below is two-dimensional, and that is a swap I want to be explicit about rather than let you assume. The update loop is written over SPATIAL_DIMS, so instantiating it at two dimensions is the same recurrence with one fewer trip round the loop; what it buys is that a 2x2 covariance can be drawn honestly, as an ellipse. A 3x3 one on a flat page has to be flattened into a marginal first, and a marginal is exactly where the interesting part of a covariance goes to hide. The 2x2 density query is my own reduction of the 3x3 Cramer’s-rule expansion above, at n = 2, where the adjugate of a symmetric matrix collapses to one line per row. The source never had a two-dimensional path. There is a three-dimensional island straight after this one that puts the full 3x3 back, ground truth beside reconstruction.

With JavaScript enabled this becomes a canvas you can draw a shape on. Points are then sampled along whatever you drew, streamed one at a time into a voxel grid, and every cell keeps a running mean and 2x2 covariance drawn as an ellipse, with a voxel size slider, a playback speed, and a click-to-query density readout.

Start by drawing. Drag a line across the grid, or take one of the four presets if you are on a keyboard or would rather not, and points appear along the shape, scattered across it by whatever the sensor noise slider says. Then turn the layers on one at a time, because the whole argument of this post is in the difference between them. Points alone are just points. Cell occupancy is the honest voxel grid, a count per cell and nothing more: it tells you where the shape is and nothing about which way it runs. The ellipses are the NDT, and they say the thing a count cannot. On a straight stretch of stroke a cell’s points spread over the whole cell in one direction and only over the sensor noise in the other, so the Gaussian comes out long along the stroke and thin across it.

Those are numbers you can read off rather than take from me. On the default corner shape, at 0.2 m cells and 1.6 cm of noise, a cell in the middle of a wall reports a sigma of about 0.050 m along the wall against 0.010 m across it: an eigenvalue ratio of 22.7 for the cell the widget happens to highlight on load, and ratios in the tens for every other wall cell I clicked. Push the noise slider up and the ellipses fatten across the stroke and the ratio falls, exactly as it should. That anisotropy is the entire reason a scan matcher prefers a field of Gaussians to a field of occupancy counts: a point that is off along the wall costs it almost nothing, and a point that is off through the wall costs it a lot.

The voxel size slider re-tessellates the points without resampling them, which makes the tradeoff visible in one drag. At 0.10 m the corner shape fills 76 cells and only 62 of them clear the count >= 3 gate, so fourteen are drawn as bare grey dots with no ellipse at all, the same thing getProbabilities does when it declines to answer. At 0.45 m the same points fill 11 cells, all of them confident, but a cell that size is now describing a whole corner rather than a patch of wall, and its ellipse is a fat blob that no longer means “surface”. Somewhere in between is the voxel size for your sensor, and there is no way to pick it except by looking.

Then the streaming, which is what the post is really about. Set the speed to one point a second, or press “+1 point” and take it a single point at a time, and watch one cell get outlined as its point lands. Underneath, that cell’s running count, its two sigmas and its eigenvalue ratio update in place. That is Welford’s recurrence happening in front of you: the widget never keeps the points that cell has seen, only the count, the mean and the three numbers of the upper triangle, and a new point walks them forward with the same three lines of arithmetic the C++ runs. The ellipse does not shrink as evidence arrives, because it is already a fair estimate of how much this patch of surface actually varies; what changes is how much the widget trusts it, which is what the opacity is showing.

The compression readout is worth watching rather than trusting. In two dimensions a point costs 8 bytes and a cell’s statistics cost 24, so a cell breaks even at exactly three points, which is the gate. Below that you are paying to summarise less than you stored. On the corner preset at 0.2 m cells, 400 points against 26 occupied cells is 3,200 B of points against 624 B of statistics, 5.1x; 902 points against 27 cells is 11.1x, because the cell count has stopped growing and every further point is free. In three dimensions the same sum is 12 bytes a point against 40 a cell, break-even a shade over three points again. Turning on “spread each point to its 4 corner cells” costs you most of that: on the same preset it takes the grid from 26 occupied cells to 68 and the ratio from 5.1x down to 2.0x, because each point now votes four times. A voxel grid’s saving is not a constant, it is a curve, and it only pays once each cell has seen enough of the scan to be worth summarising.

The same field in three dimensions

The island above is my reduction, chosen so the covariance is visible. This one is the algorithm at the dimensionality it was actually written for: voxelPush running over three dimensions, six entries in the upper triangle, and an ellipsoid that is the whole covariance rather than a shadow of it.

There are two views and one camera. The left is the ground truth: an actual triangle mesh, solid or as a hidden-line wireframe, with every point the sensor has returned so far lying on it. The right is the reconstruction, which knows nothing except the running statistics: a wireframe 2-sigma ellipsoid per cell, and the cell boxes if you turn them on. Orbit either one and the other turns with it, because comparing a surface against its summary only means anything from a fixed shared viewpoint, and hunting for the same angle twice by hand is what usually makes a side-by-side useless. The points that feed the statistics are sampled off the surface of that same mesh, weighted by triangle area so they land evenly, and pushed along the surface normal by the sensor noise. It is the same chain as the 2D island: a shape, points off it, cells, Gaussians, and nothing quietly generated by a tidier process behind the scenes.

Four of the five shapes are analytic, built in the widget from a few dozen lines of geometry. The fifth is the Stanford Bunny, which is here because it is the model my own test_visualise_ndt_bunny.py loads: that script pushes bun_zipper.ply straight through update() and then asks getProbabilities for the density on a regular grid, so it is the original test of exactly the code this post is about. It is also why a commit message in that history, quoted further up, is literally “Bunny seems to be working.” Watching the same bunny come back as a field of ellipsoids five years later was the most fun I had building this.

With JavaScript and WebGL enabled this becomes two linked 3D views: a surface mesh with the sensor’s returns lying on it, and the per-voxel Gaussians reconstructed from those returns alone, drawn as 2-sigma ellipsoids. One camera drives both, from either side.

The thing to look for is the shape of the ellipsoids. A cell sitting on a flat patch comes out as a pancake: two long axes lying in the plane of the surface, one very short one across it. On the default room corner, at 0.4 m cells and 2 cm of sensor noise, a cell holding 21 points reports sigmas of 0.146, 0.101 and 0.019 m. The first two are the cell filling up in the plane. The third is an estimate of the sensor noise itself, which nobody ever told the voxel, falling out of a count, a three-vector and six numbers. A cell straddling the corner, where two walls meet inside one box, comes out much rounder, because its points genuinely do fill a volume and its smallest sigma stops being the noise. That difference is the whole content of the representation, and it is why a scan matcher can push a misaligned scan back onto a wall without any notion of what a wall is.

The voxel size slider makes the same tradeoff as in two dimensions, with the real 3D byte counts attached. At 0.4 m the corner scene fills 80 cells, 79 of them past the gate: 14,400 B of points against 3,200 B of statistics, 4.5x, and by 3,201 points it is 12.0x because the cell count has stopped moving. Drop to 0.2 m and it is 376 cells of which only 202 have three points yet, 15,040 B of statistics against 14,412 B of points, a ratio of 0.96x. That is the honest crossover: a fine grid over a sparse scan is a worse deal than keeping the points, and it stays a worse deal until enough of the scan has arrived. Push to 0.6 m and it is 37 cells at 9.7x, but a cell that size has now swallowed the corner: the one I landed on holds 61 points and reports 0.157, 0.146 and 0.065 m, a ratio of 6 rather than 61, which is the arithmetic saying “this is not a plane” in the only way it can. The bunny is the interesting case, because a bunny has no flat parts to speak of: at 0.16 m and 3,000 points it fills 418 cells, 329 of them confident, for 2.15x.

Streaming works the same way here: one point a second, or “+1 point” for exactly one, and the cell it landed in lights up in both views at once while its count, its mean offset from the cell centre and its three sigmas update underneath. The first three points in a fresh cell are worth watching for their own reason. Three points in three dimensions always lie exactly on a plane, so the covariance is singular and the smallest eigenvalue is zero, which is what the readout means when it calls the cell degenerate. That is not a bug in the widget, it is the reason getProbabilities clamps its determinant with std::max(1e-20, ...) before dividing by it, and a good part of the reason the gate sits at three points rather than one.

The naive-versus-Welford comparison at the top and the two grids below it are the same algorithm looked at from two directions: one shows why the incremental update is computed the way it is, the others show what it buys once you trust it enough to build a data structure on top of it. A voxel that streams a Gaussian instead of storing points is a genuinely small piece of code, a few dozen lines of arithmetic, doing something a naive implementation of the “obvious” formula quietly cannot: staying correct regardless of where in the world it happens to sit.