Normal distributions transform

The normal distributions transform (NDT) is a point cloud registration algorithm introduced by Peter Biber and Wolfgang Straßer in 2003, while working at University of Tübingen.

The algorithm registers two point clouds by first associating a piecewise normal distribution to the first point cloud, that gives the probability of sampling a point belonging to the cloud at a given spatial coordinate, and then finding a transform that maps the second point cloud to the first by maximising the likelihood of the second point cloud on such distribution as a function of the transform parameters.

Originally introduced for 2D point cloud map matching in simultaneous localization and mapping (SLAM) and relative position tracking, the algorithm was extended to 3D point clouds and has wide applications in computer vision and robotics. NDT is very fast and accurate, making it suitable for application to large scale data, but it is also sensitive to initialisation, requiring a sufficiently accurate initial guess, and for this reason it is typically used in a coarse-to-fine alignment strategy.

Formulation
The NDT function associated to a point cloud is constructed by partitioning the space in regular cells. For each cell, it is possible to define the mean $$\textstyle \mathbf{q} = \frac{1}{n} \sum_i \mathbf{x_i}$$ and covariance $$\textstyle \mathbf{S} = \frac{1}{n} \sum_i \left(\mathbf{x}_i - \mathbf{q}\right) \left(\mathbf{x}_i - \mathbf{q}\right)^\top$$ of the $$n$$ points of the cloud $$\mathbf{x}_1, \dots, \mathbf{x}_n$$ that fall within the cell. The probability density of sampling a point at a given spatial location $$\mathbf{x}$$ within the cell is then given by the normal distribution


 * $$e^{-\frac{1}{2} \left(\mathbf{x} - \mathbf{q}\right)^\top \mathbf{S}^{-1} \left(\mathbf{x} - \mathbf{q}\right)}$$.

Two point clouds can be mapped by a Euclidean transformation $$f$$ with rotation matrix $$\mathbf{R}$$ and translation vector $$\mathbf{t}$$


 * $$f_{\mathbf{R}, \mathbf{t}}(\mathbf{x}) = \mathbf{R} \mathbf{x} + \mathbf{t}$$

that maps from the second cloud to the first, parametrised by the rotation angles and translation components.

The algorithm registers the two point clouds by optimising the parameters of the transformation that maps the second cloud to the first, with respect to a loss function based on the NDT of the first point cloud, solving the following problem


 * $$\arg\min_{\mathbf{R}, \mathbf{t}} \left\{ -\sum_i \operatorname{NDT} \left( f_{\mathbf{R}, \mathbf{t}} \left( \mathbf{x_i} \right) \right) \right\}$$

where the loss function represents the negated likelihood, obtained by applying the transformation to all points in the second cloud and summing the value of the NDT at each transformed point $$f_{\mathbf{R}, \mathbf{t}}(\mathbf{x})$$. The loss is piecewise continuous and differentiable, and can be optimised with gradient-based methods (in the original formulation, the authors use Newton's method).

In order to reduce the effect of cell discretisation, a technique consists of partitioning the space into multiple overlapping grids, shifted by half cell size along the spatial directions, and computing the likelihood at a given location as the sum of the NDTs induced by each grid.