Registration of 3D shapes is a key step in both 3D model creation (from scanners or computer vision systems) and shape analysis. For rigid-body alignment based purely on geometry (as opposed to RGB-D), the most common methods are based on variants of the Iterative Closest Point (ICP) algorithm [Besl and McKay 1992] . In this method, points are repeatedly selected from one model, their nearest points on the other model (given the current best-estimate rigidbody alignment) are selected as correspondences, and an incremental transformation is found that minimizes distances between point pairs. The algorithm eventually converges to a local minimum of surface-to-surface distance.

Because ICP-like algorithms can be made efficient and reliable, they have become widely adopted. As a result, researchers have focused on both addressing the shortcomings of ICP and extending it to new settings such as color-based registration and non-rigid alignment. One particular class of improvements has focused on the loss function that is optimized to obtain an incremental transformation. For example, as compared to the original work of Besl and McKay, which minimized point-to-point distance, the method of [Chen and Medioni 1992] minimized the distance between a point on one mesh and a plane containing the matching point and perpendicular to its normal. This point-to-plane objective generally results in faster convergence to the correct alignment and greater ultimate accuracy, though it does not necessarily increase the basin of convergence. Work by [Fitzgibbon 2003], [Mitra et al. 2004], and [Pottmann et al. 2006] showed that both point-to-point and point-to-plane minimization may be thought of as approximations to minimizing the squared Euclidean distance function of the surface, and they presented algorithms that achieved greater convergence speed and stability, albeit at the cost of greater computational complexity and/or auxiliary data structures.

This paper proposes a symmetrized version of the point-to-plane objective for use in ICP, incorporating two key ideas. First, the plane in which the error is minimized is based on the surface normals of both points in the corresponding pair. Second, the optimization is performed in a “stationary” coordinate system, while both meshes are moved in opposite directions. These changes require a relatively small modification to the optimization problem being performed, and almost no increased computation per iteration, but result in improved convergence of ICP.

The reason for this improvement is that the symmetric objective is minimized whenever the pair of points lies on a second-order (constant-curvature) patch of surface, rather than being minimized only if the points are on a plane. Thus, we gain some of the same benefits as second-order distance function minimization methods, but without explicit computation of second-order surface properties, or the need for volumetric data structures to store an approximation to the squared Euclidean distance function.

In addition to the primary contribution of the new symmetric objective, we also introduce an alternative approach to linearization of rotations that allows us to reduce the optimization to a linear least-squares problem, while still solving for the exact transformation when correspondences are exact. We conduct experiments that demonstrate both greater per-iteration error reduction and an increase in the convergence basin for our proposed method.

2 RELATED WORK

Since the original ICP algorithms by [Besl and McKay 1992] and [Chen and Medioni 1992], there have been significant efforts to improve convergence and stability. For a comprehensive overview of many variants, see the surveys by [Rusinkiewicz and Levoy 2001], [Diez et al. 2015], and [Pomerleau et al. 2015]. Much of this work focuses on finding better correspondences (e.g., by matching local surface properties or descriptors), performing outlier-tolerant optimization, or generalizing to non-rigid deformation. Here we focus specifically on methods that modify the objective function and/or the strategy for minimizing it.

[Segal et al. 2009] generalize ICP to associate a probabilistic model (in practice, a covariance matrix) with each point. This allows for a “soft plane-to-plane” minimization that improves the matching of planar surfaces. [Halber and Funkhouser 2017] have explored incorporating additional constraints between planes, such as parallelism or orthogonality, into registration.

[Fitzgibbon 2003] proposes to directly minimize the distance between samples on one shape (referred to as “data”) and the second shape itself (the “model”). This is done by computing the squared distance transform of the model, evaluating it at data locations, applying a robustifying kernel, and minimizing the result using Levenberg-Marquardt. [Mitra et al. [Mitra et al. 2004] propose two methods for using the distance field of a shape for optimization: one based on local quadratic approximation at closest corresponding points, and the other based on a global hierarchical $d^2$-Tree data structure [Leopoldseder et al. 2003] that stores a bounded-error approximation to the global squared distance field. [Pottmann et al. 2006] analyze the theoretical properties of distance-function minimization, and demonstrate its improved convergence.

The variants described above all perform local minimization, requiring an initial guess. This may be based on exhaustive search, matching of descriptors (such as spin images [Huber and Hebert 2003] or integral invariants [Gelfand et al. 2005]), or finding constrained point arrangements [Aiger et al. 2008]. In contrast, [Yang et al. 2015] combine local registration with a branch-and-bound algorithm that yields a provably globally-optimal solution. The loss function, however, is still based on point-to-point, which is exploited for derivation of the error bounds for global search.

In this paper, we derive an objective that is closest in spirit to simple point-to-plane minimization, but locally converges to zero for quadratic, rather than just planar, patches. This is done by considering the normals of both points in a pair, though we do so in a way unrelated to [Segal et al. 2009].

3 METHOD

3.1 Background and Motivation

Consider the problem of aligning surfaces $P$ and $Q$. This involves finding a rigid-body transformation $( {\prosedeflabel{ICP}{{R}}} | {\prosedeflabel{ICP}{{t}}} )$ such that applying the transformation to $P$ causes it to lie on top of $Q$ . The original ICP algorithm of [Besl and McKay 1992] may be thought of as an instance of Expectation Maximization: the problem is solved by alternately computing pairs of corresponding points $( {\prosedeflabel{ICP}{{p}}} _i, {\prosedeflabel{ICP}{{q}}} _i)$, where ${\prosedeflabel{ICP}{{q}}} _i$ is the closest point to ${\prosedeflabel{ICP}{{p}}} _i$ given the current transformation , and finding the transformation minimizing the point-to-point objective :

Because this iteration converges slowly, authors including [Fitzgibbon 2003], [Mitra et al. 2004], and [Pottmann et al. 2006] have re-cast alignment as iterative minimization of the squared Euclidean distance function of $Q$, sampled at points ${\proselabel{ICP}{{p}}} _i$. The most accurate way to accomplish this is to pre-compute a data structure that stores at each point in space (an approximation to) the squared distance field, then use it at run-time in an optimization based on Levenberg-Marquardt [Fitzgibbon 2003] or Newton’s method [Mitra et al. 2004]. This leads to fast convergence and a wide convergence basin, though at significant computational and storage cost. A simpler approach is to approximate the distance function based on the local surface at each corresponding point ${\proselabel{ICP}{{q}}} _i$. The “on-demand” method of [Mitra et al. 2004] approximates the surface as locally quadratic, which requires evaluation of second-order surface properties (i.e., curvatures). Even more straightforward is to approximate the surface around ${\proselabel{ICP}{{q}}} _i$ as planar, which only requires evaluation of surface normals ${ {\prosedeflabel{ICP}{{n_q}}} }_{,i}$ . Indeed, this approach dates back to the work of [Chen and Medioni 1992], who minimized what has come to be called the point-to-plane objective :

It can be shown that minimizing this objective is equivalent to Gauss-Newton minimization of squared Euclidean distance.

The latter does indeed improve convergence rate relative to the point-to-point objective, and point-to-plane minimization has become the workhorse of most modern ICP-like implementations. However, point-to-plane ICP has been observed to have a narrower convergence basin than point-to-point [Mitra et al. 2004]. In addition, the residual at optimal alignment is zero only when the surface is locally flat, if the correspondences are not perfect (which is necessarily the case if point sets {${\proselabel{ICP}{{p}}} _i$} and {${\proselabel{ICP}{{q}}} _i$} differ in their sampling of the surface, as with 3D scans). This is important because the zero-set of the objective function defines what transformations are “free,” in the sense that the surface is allowed to slide along itself to permit geometric features elsewhere to lock down the transformation. This is exploited by work such as the normalspace sampling of [Rusinkiewicz and Levoy 2001], the covariance-eigenvector-directed sampling of [Gelfand et al. 2003], and the stable sampling of [Brown and Rusinkiewicz 2007], all of which bias the sampling to regions that are most necessary to constrain the aligning transformation.

We therefore seek to develop a new objective function whose zero-set allows a greater class of surfaces to “slide along” themselves at zero penalty. Thinking within the framework of Expectation Maximization, this makes the method as robust as possible to mis-estimation of point correspondences in the Expectation step.

3.2 A Symmetric Objective Function

Because the surfaces $P$ and $Q$ should be, up to noise, the same, we consider what residual the objective function will attain if we were to sample a pair of nearby points $({\proselabel{ICP}{{p}}}, {\proselabel{ICP}{{q}}})$ on that surface. In the point-to-plane case, the error is
$$( {\proselabel{ICP}{{p}}} - {\proselabel{ICP}{{q}}} )\cdot {\proselabel{ICP}{{n_q}}} \tag{3}\label{3}$$
If we consider the possibility of sampling $( {\proselabel{ICP}{{p}}} , {\proselabel{ICP}{{q}}} )$ anywhere within some small region of a surface, this will be zero only if the surface is perfectly flat. However, consider the more symmetric function

$$( {\proselabel{ICP}{{p}}} - {\proselabel{ICP}{{q}}} )\cdot ( {\proselabel{ICP}{{n_p}}} + {\proselabel{ICP}{{n_q}}} )\tag{4}\label{4}$$

Examining the behavior of this function in 2D (see Figure 1), we see that it is zero whenever ${\proselabel{ICP}{{p}}}$ and ${\proselabel{ICP}{{q}}}$ are sampled from a circle, since ${\proselabel{ICP}{{n_p}}}$ and ${\proselabel{ICP}{{n_q}}}$ have opposite projections onto ${\proselabel{ICP}{{p}}} − {\proselabel{ICP}{{q}}}$. As rigid-body transformations are applied to $P$, this expression will continue to evaluate to zero as long as ${\proselabel{ICP}{{p}}}$ and ${\proselabel{ICP}{{q}}}$ end up in a relative position consistent with their placement on some circle (Figure 2, top). A similar property is true in 3D: Equation $\ref{4}$ evaluates to zero as long as ${\proselabel{ICP}{{p}}}$ and ${\proselabel{ICP}{{q}}}$ and their normals are consistent with some cylinder. Because it is difficult to describe, and especially to visualize, the set of $( {\proselabel{ICP}{{p}}} , {\proselabel{ICP}{{n_p}}} )$ that lie on arbitrary cylinders containing a fixed $( {\proselabel{ICP}{{n_q}}} , {\proselabel{ICP}{{n_q}}} )$ - it is a 4D space - Section 4.1 investigates a different property: Equation $\ref{4}$ also holds whenever ${\proselabel{ICP}{{p}}}$ and ${\proselabel{ICP}{{q}}}$ are consistent with a locally-second-order surface centered between them. While this constraint still provides a great deal of freedom for $( {\proselabel{ICP}{{p}}} , {\proselabel{ICP}{{n_p}}} )$ to move relative to $( {\proselabel{ICP}{{q}}} , {\proselabel{ICP}{{n_q}}} )$, it is a “more useful” form of freedom than provided by the point-to-plane metric. In particular, it constrains $( {\proselabel{ICP}{{p}}} , {\proselabel{ICP}{{n_p}}} )$ to be consistent with a plausible extension of $( {\proselabel{ICP}{{n_q}}} , {\proselabel{ICP}{{n_q}}} )$, unlike point-to-plane (Figure 2, bottom). Note that achieving this property does not require the evaluation of any higher-order information (i.e., curvature), which is a major benefit for computational efficiency and noise resistance.

To formulate an objective function, we consider minimizing Equation $\ref{4}$ with respect to transformations applied to the surfaces $P$ and/or $Q$. Although most previous work applies a rigid-body transformation to only one of the surfaces (e.g., the transformation in ${\proselabel{ICP}{{\varepsilon_{plane}}}}$ is applied only to $P$), we consider a symmetric split of the transformation: we imagine evaluating the metric in a fixed, “neutral” coordinate system and applying opposite transformations to $P$ and $Q$. Thus, we can formulate a symmetric objective as:

This splitting of rotations has a number of advantages when we consider linearizing the objective (Section 3.3):

• It reduces linearization error by optimizing for half of the rotation angle.
• It further reduces error, because the error in linearizing ${\proselabel{ICP}{{R}}} {\proselabel{ICP}{{p}}} − {\proselabel{ICP}{{q}}}$ is proportional to ${\proselabel{ICP}{{p}}} · {\proselabel{ICP}{{a}}}$, where a is the rotation axis, while the error in linearizing ${\proselabel{ICP}{{R}}} {\proselabel{ICP}{{p}}} − {\proselabel{ICP}{{R}}} ^{-1} {\proselabel{ICP}{{q}}}$ is proportional to $( {\proselabel{ICP}{{p}}} − {\proselabel{ICP}{{q}}} ) · {\proselabel{ICP}{{a}}}$. Except for extreme misalignment, ${\proselabel{ICP}{{p}}} − {\proselabel{ICP}{{q}}}$ is usually smaller than ${\proselabel{ICP}{{p}}}$ (i.e., the distance from ${\proselabel{ICP}{{p}}}$ to the origin).
• It enables reduction of linearization error to zero when correspondences are exact (Section 4.2).

We have also explored a simpler version of this objective in which the normals are not rotated. That is, the direction of minimization per point pair remains fixed, while the points themselves are rotated in opposite directions:
$${\proselabel{ICP}{{\varepsilon_{symm}}}} = \sum_i {\left[ \left( {\proselabel{ICP}{{R}}} {\proselabel{ICP}{{p}}} _i + {\proselabel{ICP}{{R}}} ^{-1} {\proselabel{ICP}{{q}}} _i + {\proselabel{ICP}{{t}}} \right) \cdot \left({ {\proselabel{ICP}{{n_p}}} }_i + { {\proselabel{ICP}{{n_q}}} }_i \right) \right]}^{2} \tag{6}\label{6}$$

Why might this be a reasonable simplification to make? Consider the sum of two unit-length vectors in 2D. Applying opposite rotations to the vectors preserves the direction of their sum, so that the contribution of each point pair to the two variants of the objective would be the same up to a scale. In 3D, this is not true for all rotation axes, but approaches true as np approaches nq . The experiments in Section 4.3 show that the two objectives lead to similar convergence, but ${\proselabel{ICP}{{\varepsilon_{symm}}}}$ leads to simpler derivations and implementation. Therefore, the remainder of this paper adopts ${\prosedeflabel{ICP}{{\varepsilon_{symm}}}}$ as the symmetric objective .

3.3 Linear Approximation

The traditional method for converting an objective function involving rotations into an easily-optimized linear least-squares system is to make the approximations $\cos {\proselabel{ICP}{{θ}}} ∼ 1$, $\sin {\proselabel{ICP}{{θ}}} ∼ {\proselabel{ICP}{{θ}}}$, for small incremental rotations ${\proselabel{ICP}{{θ}}}$. This converts the rotation matrix ${\proselabel{ICP}{{R}}}$ into a linear form, which then yields a linear least-squares system.

We instead pursue a linearization that starts with the Rodrigues rotation formula for the effect of a rotation ${\proselabel{ICP}{{R}}}$ on a vector $v$:

$${\proselabel{ICP}{{R}}} v = v \cos {\proselabel{ICP}{{θ}}} + ( {\proselabel{ICP}{{a}}} \times v )\sin {\proselabel{ICP}{{θ}}} + {\proselabel{ICP}{{a}}} ( {\proselabel{ICP}{{a}}} \cdot v )(1-\cos {\proselabel{ICP}{{θ}}} )\tag{7}\label{7}$$

where ${\prosedeflabel{ICP}{{a}}}$ and ${\prosedeflabel{ICP}{{θ}}}$ are the axis and angle of rotation . We observe that the last term in (7) is quadratic in the incremental rotation angle ${\proselabel{ICP}{{θ}}}$, so we drop it to linearize:

$$\begin{align*} {\proselabel{ICP}{{R}}} v & \approx v \cos {\proselabel{ICP}{{θ}}} + ( {\proselabel{ICP}{{a}}} \times v)\sin {\proselabel{ICP}{{θ}}} \\ & = \cos {\proselabel{ICP}{{θ}}} (v + ( {\proselabel{ICP}{{ã}}} \times v))\end{align*}\tag{8}\label{8}$$

where $\DeclareMathOperator*{\argmax}{arg\,max} \DeclareMathOperator*{\argmin}{arg\,min} \begin{align*} \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 'ã', 'ICP', 'def', false, '')", "id":"ICP-ã", "sym":"ã", "func":"ICP", "localFunc":"", "type":"def", "case":"equation"} }{ {\mathit{ã}} } & = \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 'a', 'ICP', 'use', false, '')", "id":"ICP-a", "sym":"a", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\mathit{a}} }tan\left( \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 'θ', 'ICP', 'use', false, '')", "id":"ICP-θ", "sym":"θ", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\mathit{θ}} } \right)\\\eqlabel{ {"onclick":"event.stopPropagation(); onClickEq(this, 'ICP', ['θ', 'a', 'ã'], false, [], [], 'w6MgPSBhIHRhbijOuCk=');"} }{} \end{align*} $ . Substituting into (6),
$${\proselabel{ICP}{{\varepsilon_{symm}}}} \approx \sum_i \left( \cos {\proselabel{ICP}{{θ}}} ( {\proselabel{ICP}{{p}}} _i - {\proselabel{ICP}{{q}}} _i)\cdot {\proselabel{ICP}{{n}}} _i + \cos {\proselabel{ICP}{{θ}}} ( {\proselabel{ICP}{{ã}}} \times ( {\proselabel{ICP}{{p}}} _i+ {\proselabel{ICP}{{q}}} _i))\cdot {\proselabel{ICP}{{n}}} _i + {\proselabel{ICP}{{t}}} \cdot {\proselabel{ICP}{{n}}} _i \right)\notag$$

$$\sum_i \left[ ({\proselabel{ICP}{{p̃}}}_i - {\proselabel{ICP}{{q̃}}}_i)\cdot {\proselabel{ICP}{{n}}} _i + (({\proselabel{ICP}{{p̃}}}_i + {\proselabel{ICP}{{q̃}}}_i)\times {\proselabel{ICP}{{n}}} _i) \cdot {\proselabel{ICP}{{ã}}} + {\proselabel{ICP}{{n}}} _i \cdot {\proselabel{ICP}{{t̃}}} \right]^2\tag{10}\label{10}$$

where $\DeclareMathOperator*{\argmax}{arg\,max} \DeclareMathOperator*{\argmin}{arg\,min} \begin{align*} \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 'p̃', 'ICP', 'def', false, '')", "id":"ICP-p̃", "sym":"p̃", "func":"ICP", "localFunc":"", "type":"def", "case":"equation"} }{ {\textit{p̃}} }_{ \mathit{i} } & = \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 'p', 'ICP', 'use', false, '')", "id":"ICP-p", "sym":"p", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\mathit{p}} }_{ \mathit{i} } - \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, '$\\\\bar{p}$', 'ICP', 'use', false, '')", "id":"ICP-$\\\\bar{p}$", "sym":"$\\\\bar{p}$", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\bar{p}} }\\\eqlabel{ {"onclick":"event.stopPropagation(); onClickEq(this, 'ICP', ['p', '$\\\\bar{p}$', 'p̃'], false, [], [], 'cMyDX2kgPSBwX2kgLSBgJFxiYXJ7cH0kYA==');"} }{} \end{align*} $ and $\DeclareMathOperator*{\argmax}{arg\,max} \DeclareMathOperator*{\argmin}{arg\,min} \begin{align*} \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 'q̃', 'ICP', 'def', false, '')", "id":"ICP-q̃", "sym":"q̃", "func":"ICP", "localFunc":"", "type":"def", "case":"equation"} }{ {\textit{q̃}} }_{ \mathit{i} } & = \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 'q', 'ICP', 'use', false, '')", "id":"ICP-q", "sym":"q", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\mathit{q}} }_{ \mathit{i} } - \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, '$\\\\bar{q}$', 'ICP', 'use', false, '')", "id":"ICP-$\\\\bar{q}$", "sym":"$\\\\bar{q}$", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\bar{q}} }\\\eqlabel{ {"onclick":"event.stopPropagation(); onClickEq(this, 'ICP', ['$\\\\bar{q}$', 'q', 'q̃'], false, [], [], 'ccyDX2kgPSBxX2kgLSBgJFxiYXJ7cX0kYA==');"} }{} \end{align*} $. This is a least-squares problem in ${\proselabel{ICP}{{a}}}̃$ and ${\proselabel{ICP}{{t̃}}}$, and the final transformation from $P$ to $Q$ is:

where $ {\proselabel{ICP}{{θ}}} = tan^{−1} || {\proselabel{ICP}{{ã}}} ||$

Note that the new linearization results in the same system of equations as would the traditional approach. What changes is how the solved-for variables ${\proselabel{ICP}{{ã}}}$ and ${\proselabel{ICP}{{t̃}}}$ are interpreted. This produces a modest increase in accuracy but, more importantly, is necessary to obtain the property that the linearization is exact for exact correspondences (see Section 4.2). We may interpret (10) as a Gauss-Newton step applied to (6), using the Gibbs representation of rotations.

4 THEORETICAL AND EXPERIMENTAL RESULTS

4.1 The Symmetric Error Is Zero When Corresponding Points Are Consistent With a Quadratic Surface

Assume that points ${\proselabel{ICP}{{p}}}$ and ${\proselabel{ICP}{{q}}}$ are located on a height field $z = h(x,y)$ that may be approximated locally as second-order. We construct a coordinate system centered at their geodesic midpoint m, with the surface tangent to $xy$ (see Figure 3). The height of that surface relative to the tangent plane may be expressed as a quadratic function of $xy$ displacement away from $m$:
$$\Delta z = \frac{1}{2}\begin{pmatrix} \Delta x & \Delta y \\ \end{pmatrix} \begin{pmatrix} e & f \\ f & g \end{pmatrix} \begin{pmatrix} \Delta x \\ \Delta y \end{pmatrix}\tag{12}\label{12}$$
This is an even function, so if ${\proselabel{ICP}{{p}}}$ and ${\proselabel{ICP}{{q}}}$ are displaced by equal amounts in opposite directions from $m$, then their $z$ coordinates are equal, and ${\proselabel{ICP}{{p}}} − {\proselabel{ICP}{{q}}}$ is parallel to the $xy$ plane. Conversely, the perturbation of the surface normal away from $zˆ$ is an odd function:

$$\begin{pmatrix} \Delta {\proselabel{ICP}{{n}}}_x \\ \Delta {\proselabel{ICP}{{n}}}_y \end{pmatrix} = - \begin{pmatrix} e & f \\ f & g \end{pmatrix} \begin{pmatrix} \Delta x \\ \Delta y \end{pmatrix}\tag{13}\label{13}$$

Therefore, ${\proselabel{ICP}{{n_p}}} + {\proselabel{ICP}{{n_q}}}$ is parallel to the $z$ axis, and so it must be perpendicular to ${\proselabel{ICP}{{p}}} − {\proselabel{ICP}{{q}}}$.
Note that the property that the error vanishes near a second-order patch of surface does not hold for point-to-point, point-to-plane, or even the method of [Mitra et al. 2004]. The latter, for example, considers a quadratic approximant to the square of the Euclidean distance function, which can be minimized only at a plane, line, or point. An objective function that vanishes near a curved surface, however, would require a higher-order approximation. This partially explains the faster convergence of ${\proselabel{ICP}{{\varepsilon_{symm}}}}$, as observed in the experiments of Section 4.3.

Note also that, as ${\proselabel{ICP}{{p}}}$ and ${\proselabel{ICP}{{q}}}$ move away from being consistent with a second-order surface, the error in Equation $\ref{4}$ remains well-behaved: it is just linear in positions and normals. This is in contrast to the (squared) Euclidean distance function, whose Hessian diverges at the medial surface.

4.2 The Linearization is Exact for Exact Correspondences

Unlike the traditional linearization of rotations, we observe that the linear least-squares problem in (10) produces an exact result when correspondences $( {\proselabel{ICP}{{p}}} _i, {\proselabel{ICP}{{q}}} _i)$ are correct. This is because the approximation of $1/(\cos {\proselabel{ICP}{{θ}}} )^2$ as 1 involves a multiplicative factor that may be interpreted as a weight, and so the only additive term that is actually dropped contains a factor of $( {\proselabel{ICP}{{p̃}}} _i − {\proselabel{ICP}{{q̃}}} _i ) · {\proselabel{ICP}{{a}}}$. However, if correspondences are correct and the points are center-of-mass normalized, then ${\proselabel{ICP}{{p̃}}} _i − {\proselabel{ICP}{{q̃}}} _i$ is guaranteed to be perpendicular to the rotation axis, and hence zero error is introduced by the linearization. We have verified experimentally that this is the case, up to roundoff error.

This an unexpected result, because previous techniques that solve for exact rotations have tended to involve nonlinear optimization, and hence require multiple iterations. There do exist several closed-form solutions for the minimization of ${\proselabel{ICP}{{\varepsilon_{point}}}}$, but all are based on SVD or eigenvector problems [Eggert et al. 1997]. To the best of our knowledge, there are no published techniques that exactly optimize ${\proselabel{ICP}{{\varepsilon_{plane}}}}$ or any related metric with a single linear solve, even for exact correspondences.

4.3 ${\proselabel{ICP}{{\varepsilon_{symm}}}}$ Accelerates Per-Iteration Convergence

In an ICP implementation, of course, correspondences will not be exact. Nevertheless, we observe that ${\proselabel{ICP}{{\varepsilon_{symm}}}}$ produces faster convergence than ${\proselabel{ICP}{{\varepsilon_{point}}}}$ and ${\proselabel{ICP}{{\varepsilon_{plane}}}}$. We conduct an experiment in which we start with two copies of a mesh, then move one copy away from its ground-truth position and orientation. We execute a single iteration of ICP to align the shifted copy back towards the original. We measure error, both before and after that ICP iteration, as the root-mean-square distance between actual vertex positions and their ground-truth locations, where the mesh is scaled such that the root-mean-squared vertex distance from the center of mass is 1. No outlier rejection is performed.

We test a total of six objective functions, of which four are ${\proselabel{ICP}{{\varepsilon_{point}}}}$, ${\proselabel{ICP}{{\varepsilon_{plane}}}}$, ${\proselabel{ICP}{{\varepsilon_{symm-RN}}}}$, and ${\proselabel{ICP}{{\varepsilon_{symm}}}}$. The two additional objectives are:

• Quadratic: the method of [Mitra et al. 2004] that minimizes a locally-quadratic approximant to the squared Euclidean distance function. The implementation uses the “on demand” method described in that paper, in which the approximation uses curvature information at the closest point.

• Two-plane: minimizing the sum of squared distances to planes defined by both ${\proselabel{ICP}{{n_p}}}$ and ${\proselabel{ICP}{{n_q}}}$ :
  
  $$\DeclareMathOperator*{\argmax}{arg\,max} \DeclareMathOperator*{\argmin}{arg\,min} \begin{align*} \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, '$\\\\varepsilon_{two-plane}$', 'ICP', 'def', false, '')", "id":"ICP-$\\\\varepsilon_{two-plane}$", "sym":"$\\\\varepsilon_{two-plane}$", "func":"ICP", "localFunc":"", "type":"def", "case":"equation"} }{ {\varepsilon_{two-plane}} } & = \sum_{\mathit{i}} \left( {\left( \left( \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 'R', 'ICP', 'use', false, '')", "id":"ICP-R", "sym":"R", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\mathit{R}} }\idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 'p', 'ICP', 'use', false, '')", "id":"ICP-p", "sym":"p", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\mathit{p}} }_{ \mathit{i} } + \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 'R', 'ICP', 'use', false, '')", "id":"ICP-R", "sym":"R", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\mathit{R}} }^{-1}\idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 'q', 'ICP', 'use', false, '')", "id":"ICP-q", "sym":"q", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\mathit{q}} }_{ \mathit{i} } + \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 't', 'ICP', 'use', false, '')", "id":"ICP-t", "sym":"t", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\mathit{t}} } \right) \cdot \left( \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 'R', 'ICP', 'use', false, '')", "id":"ICP-R", "sym":"R", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\mathit{R}} }\idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, '$n_p$', 'ICP', 'use', false, '')", "id":"ICP-$n_p$", "sym":"$n_p$", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {n_p} }_{ \mathit{i} } \right) \right)}^{2} + {\left( \left( \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 'R', 'ICP', 'use', false, '')", "id":"ICP-R", "sym":"R", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\mathit{R}} }\idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 'p', 'ICP', 'use', false, '')", "id":"ICP-p", "sym":"p", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\mathit{p}} }_{ \mathit{i} } + \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 'R', 'ICP', 'use', false, '')", "id":"ICP-R", "sym":"R", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\mathit{R}} }^{-1}\idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 'q', 'ICP', 'use', false, '')", "id":"ICP-q", "sym":"q", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\mathit{q}} }_{ \mathit{i} } + \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 't', 'ICP', 'use', false, '')", "id":"ICP-t", "sym":"t", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\mathit{t}} } \right) \cdot \left( \idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, 'R', 'ICP', 'use', false, '')", "id":"ICP-R", "sym":"R", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {\mathit{R}} }^{-1}\idlabel{ {"onclick":"event.stopPropagation(); onClickSymbol(this, '$n_q$', 'ICP', 'use', false, '')", "id":"ICP-$n_q$", "sym":"$n_q$", "func":"ICP", "localFunc":"", "type":"use", "case":"equation"} }{ {n_q} }_{ \mathit{i} } \right) \right)}^{2} \right)\\\eqlabel{ {"onclick":"event.stopPropagation(); onClickEq(this, 'ICP', ['q', 'R', '$n_q$', 'p', 't', '$n_p$', '$\\\\varepsilon_{two-plane}$'], false, [], [], 'YCRcdmFyZXBzaWxvbl97dHdvLXBsYW5lfSRgID0g4oiRX2koKChSIHBfaSArIFLigbvCuSBxX2kgKyB0KSDii4UgKFIgYCRuX3AkYF9pKSleMiArICgoUiBwX2kgKyBS4oG7wrkgcV9pICsgdCkg4ouFIChS4oG7wrlgJG5fcSRgX2kpKV4yKQ==');"} }{} \end{align*} \tag{14}\label{14}$$
  
  This approach is a symmetrized version (with split rotations) of methods previously used by [Tagliasacchi et al. 2015] and [Luong et al. 2016], which in turn were related to the Hausdorff metric by [Tkach et al. 2016].

Figure 4, left, shows the result of this experiment on the dragon model [Curless and Levoy 1996]. The graph compares the error after an iteration of alignment (plotted along the y axis) to error before alignment (x axis), for each method. Each datapoint represents the average over 1000 trials, each having a different starting transformation (that produces the same initial error).

We see that the behaviors of the different methods fall into three general categories. The point-to-point objective is the slowest: it reliably exhibits linear convergence. The point-to-plane, two-plane, and quadratic methods exhibit superlinear convergence, while the two symmetric objectives (whose curves lie essentially on top of each other) are even faster: they have a higher convergence order (greater slope of the curve) for large misalignment, and a similar convergence order but more favorable constant once alignment improves. Note that the graph is on a log-log scale, so the vertical distance between the curves can represent improvement by as much as an order of magnitude in per-iteration convergence.

Figure 4, center, shows a similar experiment, but for incomplete and partially-overlapping range scans (the bun000 and bun090 scans from the bunny model [Turk and Levoy 1994]). In this case, some form of robust estimation is necessary to account for partial overlap, and this may be done using techniques including explicit outlier rejection [Turk and Levoy 1994]; [Pulli 1999]; [Chetverikov et al. 2005]], consistency conditions [Pajdla and Van 1995]; [Dorai et al. 1998], M-estimation [Gelfand et al. 2003], or sparsity-inducing norms [Bouaziz et al. 2013]. Because it is not our intention to exhaustively explore options for robust estimation, we adopt a relatively simple outlier-rejection strategy that excludes matches having normals with negative dot product, and pairs with (point-to-point) distance greater than 2.5 times the standard deviation of distances at each iteration [Masuda et al. 1996]. (The standard deviation is estimated robustly as 1.4826 times the median distance.) The relative ordering of the ICP variants is similar to the whole-model case, but the variants are closer to each other, due to noise and imperfect outlier rejection Ð these meshes only have an overlap (measured as intersection over union, or IOU) of approximately 23%.

Finally, Figure 4, right, shows the experimental convergence for a pair of scans of an indoor office environment, specifically timestamps 1305031104.030279 and 1305031108.503548 of the freiburg1_xyz sequence from the TUM RGB-D dataset [Sturm et al. 2012][Sturm et al. 2012]. This dataset is qualitatively different from the bunny, since it contains many planar surfaces (which might be expected to boost the performance of Eplane and related variants), but also has some warp and scanning noise from the Kinect sensor (which leads to a nontrivial residual, decreasing the difference between variants). Even in this scenario, which might be expected to provide the least benefit to ${\proselabel{ICP}{{\varepsilon_{symm}}}}$, the symmetric objective outperforms the others.

4.4 ICP with ${\proselabel{ICP}{{\varepsilon_{symm}}}}$ Has a Wide Convergence Basin

To measure how reliably the different ICP objectives reach the correct alignment, we repeatedly misalign two meshes by a given angle (about a random axis) and a given translation magnitude (in a random direction), then run ICP for a fixed number of iterations. For consistency, all variants sample points from both meshes, find closest points (according to Euclidean distance) on the other mesh, and use the outlier rejection strategies described above. The success of ICP is determined by whether points are within a threshold (1% of mesh size) of their ground-truth locations after alignment.

Figure 5 shows the convergence basin for the six ICP variants from the previous experiment, plus two variants that apply the Levenberg-Marquardt algorithm, either to ${\proselabel{ICP}{{\varepsilon_{plane}}}}$ as in the work of [Fitzgibbon 2003], or to the new ${\proselabel{ICP}{{\varepsilon_{symm}}}}$. Each small square shows the percentage of successful trials, averaged over all pairs of bunny scans having IOU overlap greater than 20%, and over 1000 initial transformations that have the given rotation angle and translation magnitude.

With 500 iterations, ${\proselabel{ICP}{{\varepsilon_{point}}}}$ and ${\proselabel{ICP}{{\varepsilon_{plane}}}}$ have regimes in which one is slightly better (${\proselabel{ICP}{{\varepsilon_{point}}}}$ for large translation and small rotation, and vice versa for ${\proselabel{ICP}{{\varepsilon_{plane}}}}$). The quadratic method of [Mitra et al. 2004] and Fitzgibbon’s LM-ICP (applied to Eplane) modestly improve convergence, with Esymm comparable to those two and Etwo-plane improving reliability even more. Levenberg-Marquardt applied to Esymm is both fast and reliable, achieving the widest convergence basin at 20 iterations and coming within a few percent of ${\proselabel{ICP}{{\varepsilon_{two-plane}}}}$ at 500 iterations. The availability of a fast and reliable ICP variant is vital for real-time applications such as interactive 3D scanning [Izadi et al. 2011], which have heretofore used point-to-plane ICP.

5 DISCUSSION AND FUTURE WORK

The symmetric objective represents a simple improvement to traditional point-to-plane ICP. At minimal additional implementation cost, it produces faster and more reliable convergence. Future work in this area consists of exploring the combination of the symmetric objective with modern approaches for denoising, surface descriptor matching, robust lp minimization, and step size control.

A further topic for future investigation is relating the symmetric objective to distance function minimization. Just as Equation $\ref{3}$ can be considered a linearization of the signed distance to Q evaluated at points on P, Equation $\ref{4}$ can be considered the linearization of the sum of that function, plus the signed distance to P evaluated at points on Q. While it might be possible to simplify this description (perhaps by considering samples at the midpoint between the two surfaces), even that does not readily lead to an explanation of the properties in Sections 3 and 4.1. Future analysis of the sum of distance transforms could lead to additional insights on Esymm.

6 REFERENCE