Rotation and Translation Estimation

Several techniques are available for obtaining an optimal estimate of the rigid rotation and translation transformation between points belonging to the same space. An optimal estimate means a maximum-likelihood estimate, where the observation noise $\xi_i$ is fully additive in the space $\mathbb{R}^{n}$ in which the points lie.

Consider, therefore, two sets of points $\left\{ \prescript{1}{}{\mathbf{x}}_i \right\}$ and $\left\{ \prescript{2}{}{\mathbf{x}}_i \right\}$ related by

$$\prescript{2}{}{\mathbf{x}}_i = \prescript{2}{}{\mathbf{R}}_... ...cript{1}{}{\mathbf{x}}_i + \prescript{2}{}{\mathbf{t}} + \xi_i$$ (1.104)

as in equation (1.100), but with the noise vector $\xi_i$.

To solve the optimal problem $(\hat{\mathbf{R}}, \hat{\mathbf{t}})$ that transforms all points from $\mathbf{x}^{1}$ to $\mathbf{x}^{2}$, a least-squares criterion is required to optimize a cost function of the form

$$S = \sum_i w_i \Vert \left( \prescript{2}{}{\mathbf{R}}_1 \... ...{}{\mathbf{t}} \right) - \prescript{2}{}{\mathbf{x}}_i \Vert^2$$ (1.105)

where $w_i$ are any priors associated with each sample. The nonlinear least-squares solution clearly suffers from local-minimum problems.

A notable result concerning the translation vector is obtained by applying the usual derivatives $0 = \partial S / \partial \mathbf{t}$ to equation (1.105), which vanishes at

$$\hat{\mathbf{t}} = \bar{\prescript{2}{}{\mathbf{x}}} - \hat{\mathbf{R}} \bar{\prescript{1}{}{\mathbf{x}}}$$ (1.106)

and therefore, given $\mathbf{R}$, the optimal translation can be found immediately; conversely, it is sufficient to determine only the rotation $\hat{\mathbf{R}}$ to solve the pose-estimation problem. This result can also be understood intuitively: the centroids of the two point sets after a rigid transformation must satisfy the relation in equation (1.104).

The optimal rotation $\mathbf{R}$ must therefore minimize

$$S = \sum_i w_i \Vert \prescript{2}{}{\mathbf{R}}_1 \prescript{1}{}{\mathbf{p}}_i - \prescript{2}{}{\mathbf{p}}_i \Vert^2$$ (1.107)

where $\mathbf{p}_i = \mathbf{x}_i - \bar{\mathbf{x}}$ has been defined. Thus, any pose-registration problem in $n$ dimensions is always reduced to a problem with $n$ DOF.

One technique for computing the rotation uses principal components (see Section 2.9.1). The principal components extracted from each point set separately form a basis of the space. A rotation that aligns these bases can be determined: once the column eigenvector matrices $\mathbf{R}_1$ and $\mathbf{R}_2$ have been computed, $\mathbf{R} = \mathbf{R}_2 \left( \mathbf{R}_1 \right)^{\top}$ follows directly. However, multiple solutions may exist, each of which should be checked, and noise can make the estimation of the axes through PCA extremely unreliable (for example, if the distribution is circular, any estimate becomes impossible).

The best way to minimize (1.107) is to minimize, or rather maximize,

$$\begin{array}{l} \argmin_{\mathbf{R}} \sum_i w_i \Vert \pre... ...\mathbf{P}_1 \mathbf{W} \mathbf{P}^{\top}_2 \right) \end{array}$$ (1.108)

that is, minimizing the system of equations (1.105) is equivalent to maximizing the trace of matrix $\hat{\mathbf{R}} \mathbf{H}$, where $\mathbf{H}$ is the correlation matrix between the two point clouds, defined as

$$\mathbf{H} = \cov (\prescript{1}{}{\mathbf{x}}, \prescript{... ...athbf{x}}_i - \bar{\prescript{2}{}{\mathbf{x}}} \right)^{\top}$$ (1.109)

It can be shown that the matrix $\hat{\mathbf{R}}$ that maximizes the trace of $\hat{\mathbf{R}} \mathbf{H}$ is

$$\hat{\mathbf{R}} = \mathbf{V} \mathbf{U}^{\top}$$ (1.110)

where $\mathbf{H} = \mathbf{U} \Sigma \mathbf{V}^{\top}$ has been singular-value decomposed. This algorithm was first described in (Kab76), rediscovered in (AHB87), and significantly improved in (Ume91) (in particular, for the case $\det\hat{\mathbf{R}}=-1$); it is commonly known as the Kabsch–Umeyama algorithm.

This solution, which is much more stable than the PCA-based solution and is always valid for $n > 3$, requires special attention only in the two- and three-dimensional cases to handle possible reflections (in that case, the determinant of the resulting matrix may in fact be negative).

The “disadvantage” of the SVD-based technique compared with the PCA-based technique is that the correspondences between points in the two distributions must be correct.

Combining the two techniques with an iterative approach yields the Iterative Closest Point (ICP) algorithm.