3D–3D Odometry

Given a sequence of temporal observations of three-dimensional points reconstructed at instants $t$ and $t'$, denoted respectively by $(\mathbf{x}_i,\mathbf{x}'_i)$, it is possible to estimate a rigid transformation $(\mathbf{R}, \mathbf{t})$ that maps the points observed at instant $t$ to those observed at instant $t'$ according to

$$\mathbf{x}'_i = \mathbf{R}\mathbf{x}_i+\mathbf{t}.$$ (10.98)

This approach is general and does not depend on the particular sensor used to obtain the three-dimensional points.

The transformation can be estimated by minimizing the cost function

$$\sum_i \left\Vert \mathbf{x}'_i-\mathbf{R}\mathbf{x}_i-\mathbf{t} \right\Vert^2.$$ (10.99)

The 3D–3D registration problem can be solved directly as a rigid-transformation estimation problem, following the procedure described in Section 1.10. An affine parameterization of the transformation can be used to initialize the problem and estimated using linear techniques from an overdetermined set of data. This solution, however, does not generally satisfy the constraints specific to a rigid transformation; in particular, its linear part is not constrained to belong to the rotation group $SO(3)$.

A more rigorous solution consists in constraining the problem directly to the space of rigid transformations. In this case, a closed-form solution can be obtained, for example, using Horn's algorithm (Hor87), which determines the optimal rotation through a quaternion representation. Similarly, the procedure based on singular value decomposition described in Section 1.10 directly yields the optimal rigid transformation.

Starting from one of these solutions, a nonlinear minimizer, such as Levenberg–Marquardt (Section 4.3.6), can be used to further refine the estimate of the six pose degrees of freedom, namely, three rotation parameters and three translation parameters. This approach is called 3D-to-3D because it estimates motion from correspondences between three-dimensional points.