3D–3D Odometry

Given a series 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 transforms the points observed at instant $t$ into those observed at instant $t'$ according to

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

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.102)

The 3D–3D registration problem can be solved directly as a rigid-transformation estimation problem, according to 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 overdetermined data. However, this solution generally does not satisfy the constraints specific to a rigid transformation and, in particular, its linear part is not constrained to belong to the rotation group $SO(3)$.

A more rigorous solution consists in directly constraining the problem 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 degrees of freedom of the pose, namely three rotation parameters and three translation parameters. This approach is referred to as 3D-to-3D because it estimates motion from correspondences between three-dimensional points.