An Extension of the ICP Algorithm for Modeling Nonrigid Objects with Mobile Robots

1 Introduction

In recent years, there has been a flurry of work on acquir­ ing 3D models from range data. The classical setting in­ volves a range sensor (e.g. , a 3D range camera or a stereo vision system) used to acquire range images of the target ob­ ject from multiple vantage points. The problem of integrating multiple range scans into a 3D model is commonly known as scan registration. Most state-of-the-art implementations are based on the popular iterative closest point algorithm [2]. The topic has received significant attention in fields as di­ verse as computer vision [12; 9] and medical imaging [7], large-scale urban modeling [14], and mobile robotics [10; 8; 15].

ICP aligns range scans by alternating a step in which clos­ est points are identified, and a step by which the optimal trans­ lation and rotation of scans relative to each other is computed. In doing so, most of them are making a rigid object assump­ tion: range scans, if aligned correctly, must be spatially con­ sistent with each other.

Many objects are deformable. For example, people change shape, as do trees, pillows, and so on. A natural research goal is therefore to extend ICP to accommodate local object transformations. Following recent work primarily found in the medical imaging literature [5; 16; 6; 3], we proposes an approach suited for scan registration and 3D modeling of non­ rigid objects that can efficiently deal with hundreds of thousands of variables. To accommodate local deformations, our approach transforms scans into Markov random fields, where nearby measurements are linked by a (nonlinear) potential function. All links are soft. They can be bent, but bending them incurs a penalty. Figure 1 illustrates this transforma­ tion: rigid links between the measurement coordinates and the robot sensor are replaced by soft links between adjacent mea­ surement points. The resulting problem of scan registration under these soft constraints becomes a high-dimensional op­ timization problem with orders of magnitude more variables involved than in regular ICP. We show how to solve this prob­ lem via Taylor series expansion (linearization), and then pro­ pose a coarse-to-fine hierarchical optimization technique for carrying out the optimization efficiently.

Our new algorithm is applied to the problem of learning 3D models of non-stationary objects with a mobile robot. We describe an implemented robot system that utilizes a model differencing technique similar to the one described in [1] to segment scans. By acquiring views of the target objects from multiple sides, our approach enables a robot to acquire a 3D model of a non-stationary object.

2 Scan Registration

This section describes a variant of the well-known iterative closest point algorithm (ICP) [2] for rigid objects. The al­ gorithm alternates two phases, one in which nearest points is identified, and one in which the distance between all pairs of nearest points is minimized.

2.1 Scans

The input to the ICP algorithm is a set of 3D scans denoted

Each such scan dk consists of a collection of lD range mea­ surements, arranged as a 3D "range image":

Figure 2c shows a typical scan acquired by a robot. Further below, we will denote the horizontal angle of the k-th range measurement by ak and the vertical one by f3k· In our experi­ mental setup, scans are obtained by a SICK laser range finder mounted on a tilt unit shown in Figure 2a.

The problem of scan registration can be formulated as the problem of recovering the vantage points from which the scans were taken. In our approach, scans are taken from a mobile robot; hence each vantage point is described by three variables: its x-y location in Cartesian coordinates and its ori­ entation "(:

Here Xk denotes the vantage point from which scan dk was acquired. The set of all vantage points will be denoted x. Recovering the vantage points x is equivalent to registering the scans if one vantage point is (arbitrarily) defined to be X1 = (0, 0, Of.

2.2 Measurement Model

Scans are registered in world coordinates. To do so, measure­ ments d ik must be mapped into 3D world coordinates. This is achieved by a projective function 7!', which takes as an ar­ gument a range measurement and a vantage point and returns as its output the corresponding coordinate in 3D world coor­ dinates:

Here ai and f3i are the orientation of the range measurement dik relative to the sensor. The variable z is the generic height of the sensor which in our robot system is fixed, but is easily generalized to variable heights. For brevity, we will some­ times write 11'i k instead of 7r(dik, Xk) ·

We define the quality of a pairwise scan registration by the probability representing the likelihood p( d ik) of a range mea­ surement under a fixed (hypothetical) registration:

This likelihood distinguishes two cases. The first case models the noise in range perception by a Gaussian with a sensor­ specific covariance E (usually a diagonal matrix). The mea­ surement error under this Gaussian is given by the distance between the point 11'i k under consideration, and the point j in scan dt that maximizes this Gaussian. As is easily to be seen, this point 7l'j! minimizes the Mahalanobis distance to 11'ik· Thus, 1l'j t is simply the point "nearest" to 11'ik in the scan dt under E- 1 .

However, finding the nearest point only makes sense if 1l';k falls within the perceptual range of scan dt . If 1l'ik is oc­ cluded, it might be perfectly well-explained by an object not detectable from the vantage point Xt . This is captured by the second case in (5), which applies when 11'ik lies outside the free space of scan l. The free space of scan l is denoted Ft. It is defined as the region between the robot and the detected objects. If 11'ik lies outside this region, the measurement prob­ ability is assumed to be uniform. The value of this uniform depends on the range of occluded space, but it plays no role in the optimization to come; hence we leave it unspecified.

2.3 Registration as Likelihood Maximization

The goal of scan registration is to determine the vantage points x that maximize the joint likelihood of the scans d and x :

The first term of this product is obtained by calculating the product over all individual measurement likelihoods, assum­ ing noise independence in each measurement:

The term p(x) in (6) is the prior on the individual vantage points (and hence on the registration). The prior of the point Xk i~ expressed as a Gaussian with mean Xk and covariance matnx Ill:

In our robot system, this prior is obtained from the robot's 2D localization routines, supplied by a public domain software package [ 11].

The negative logarithm of the joint likelihood (6) is given by the following sum:

Scans are registered by minimizing the sum, which is equiva­ lent to maximizing the likelihood function.

Unfortunately, minimizing G is not possible in closed form for the following three reasons: (1) the exact terms in the sum depend on the occlusion constraint which are a function of the vantage points x; (2) the result of the minimization over j involves a discrete search for a nearest neighbor, whose out­ come is again a function of x; (3) the projection functions 1r are nonlinear in the points x, and the resulting nonlinear optimization problem is hard.

2.4 Optimization Procedure

ICP-style algorithms minimize the negative log-likelihood by calculating a sequence of vantage points (registrations):

The first set of points are obtained from the prior (e.g., the robot odometry): xl 01 = Xk. The (n + 1)-th vantage points are computed from the n-th ones by the following five step algorithm:

Step 1. The set of occluded points is determined for the n-th points xlnl. This step involves the calculation of a rela­ tive orientation of a point 1rik to scan dt via Xt. It furthermore involves a range comparison with the corresponding measure­ ment in scan dt, to determine if 1rik is occluded relative to dt.

Step 2. The minimization in (9) is carried out by determin­ ing the closest measurement j in each scan relative to each point 1rik:

For 2.: = const. ·I, this calculation is equivalent to finding the closest points (hence the name of the algorithm).

Step 3. The projection function 1r is linearized to obtain a quadratic objective function. This is achieved by the Taylor series expansion:

Here J1~ 1 is the Jacobian (gradient) of 1r at xLnJ, which is di­ rectly obtained from the definition of 1r in (4):

With this approximation, G[n] (the negative log-likelihood function G in the n-th iteration) becomes quadratic in Xk and is of the form

Here A(n] is a matrix, cfn] a vector, and x the vector of all vantage points (the unknowns). The various sums in (9) are simply integrated into A(n] and c[nJ, and f: generalizes 2.: to the full space of all points x:

Step 4. The x-y-values of the vantage points xfn+I] are now obtained through a closed-form solution of minimizing Q[nJ. This solution is obtained by setting the first derivative of Q[nJ with respect to x to zero (derivation omitted, see [13]):

In the original ICP algorithm [2], this step is implemented by calculating the the "center of mass" of each scan and shifting the scans accordingly (which is computationally simpler); our approach also accommodates a prior x on x.

Step 5. Because of the linearization, only Cartesian co­ ordinates are actually updated in Step 4; the orientation co­ ordinates remain unmodified. For this reason, the original ICP literature introduced a separate step for calculating ori­ entations. This is achieved by a singular value decomposition (SVD) step, in which the correlation between residual errors in the scan is determined, and each scan is rotated so as to minimize these error correlations. Details on this step can be found in the ICP literature [2) .

All steps are iterated until a convergence criterion is reached. Satisfactory registrations are usually obtained within the first three to four iterations. If the total number of scans is small (e.g., less than 10), the computationally most expensive step is the determination of the closest points in Step 2. This step is usually implemented efficiently by representing scans through kd-trees [ 4].

Figure 3a-b illustrate the result of scan registration in 2D. The initial configuration in Figure 3a is transformed into the one shown in Figure 3b, which is the one that minimizes the squared distance (maximizes the likelihood). Clearly, both scans are incompatible in shape. Pure registration techniques are unable to handle such shape deformations, but the tech­ nique presented in the next section is.

3 Recovering the Surface Configuration of Nonrigid Objects

The key idea for extending ICP to nonrigid objects was al­ ready discussed in the introduction to this paper, and is high­ lighted in Figure 1. Technically, it involves two modifica­ tions: First, the static relationship between points 7r;k and the corresponding vantage points Xk is replaced by nonrigid links between adjacent points. These links can be bent (but at a probabilistic penalty), to accommodate nonrigid surfaces. Second, and as consequence of this, the optimization now in­ volves the determination of the location of all points 7r;k. in addition to the robot poses Xk. This optimization problem is much higher dimensional, and we will discuss a hierarchical optimization technique for tackling it efficiently. A key char­ acteristic of the approach proposed here is that it fits neatly into the ICP methodology above: Again, under appropriate linearization the target function is quadratic, and estimates are obtained just as in (16).

3.1 Links

The definition of links between pairs of adjacent points makes it necessary to augment the notion of a measurement point. In particular, our approach associates an (imaginary) coordinate system with each node. The origin of each coordinate sys­ tem is the familiar coordinate 7r; k. and its orientation is speci­ fied by three Euler angles (an alternative formulation may use quaternions):

The orientation is initialized arbitrarily; e.g. r ;k = (0, 0, Of. (The result of the optimization is invariant with respect to this initialization.) A link is now given by the affine coor­ dinate transformations among the coordinate systems of adja­ cent measurements. Each link possesses six parameters, three for rotation (denoted t:.r;-.J,k) and three for translation (de­ noted !::.n;-. j,k ). They are calculated as follows:

Here the R's are the rotation matrices around the three coor­ dinate axes. Links enable us to recover a node's coordinates from any of its neighbors:

To model nonrigid surfaces, our approach allows for violation of these link constraints. This is obtained by introducing the following Gaussian potentials for each link

Here 8 defines the strength of the link (the resulting structure is a Markov random field [ 17]).

3.2 Target Function

The negative logarithm of these potentials, summed over all links, is given by the following function H (constant omitted):

As in the scan registration problem, all these terms are non­ linear in the node coordinates 7r;k and the orientations r ik· To obtain a closed-form solution for the resulting equation system, the link function is linearized via a Taylor series ex­ pansion:

where n!~l and r!~l specify the coordinate system for the node ik in the n-th iteration of the optimization. The matrix K[knJk ' ·' is a Jacobian matrix of dimension six by six, which is obtained as the derivative of the functions (19).

By the same logic by which G can be approximated by a quadratic function in the scan registration problem, substitut­ ing our approximation back into the definition of H gives us a quadratic function in all variables 7r;k and r ik· This function can be written in the form

where 1r is the vector of all coordinate system origins, r the vector of all Euler angles, and B[n] is a matrix and f[n] avec­ tor. Calculating B[n] and f[n] is involved but mathematically straightforward.

3.3 Optimization Procedure

Our new version of ICP now minimizes the combined target function G + H, which is again quadratic in all parameters (x, 1r, and r) . By doing so, it simultaneously recovers the scan registration and the surface configuration of the object. The solutions for x and 1r are completely analogous to the one in (16):

The global orientation is still optimized by a single global SVD as above. Our new augmented optimization leads to relative adjustments between measurement points, in which the links play the role of soft constraints.

This is illustrated in Figure 3c-f, for different values for 8 . As the various diagrams illustrate, scans are deformed to improve their match. The degree of the deformation depends on the value of 8, which defines the rigidity of the surface. Figure 3c-f illustrates that our approach succeeds in locally rotating and even rescaling the model.

3.4 Efficient Variable Resolution Optimization

The main problem with the approach so far is its enormous complexity. The number of variables involved in the opti­ mization is orders of magnitude larger than in scan registra­ tion. This is because the target function H is a function of all measurement points 1r and orientations r, whereas G has only the vantage points x as its arguments. The matrix B[n] in (23) is, thus, a (sparse) matrix with many thousand dimensions.

To tackle such problems efficiently the optimization is re­ duced to a sequence of nested optimization problems. In a first step, scans are analyzed for connected components (re­ gions without large disparities); links exist only between con­ nected components in each scan; hence H factors naturally into different subproblems for different connected compo­ nents. Next, the resulting scan patches are thinned. Thin­ ning proceeds by identifying a small number of representa­ tive landmark measurements that are approximately equally spaced. This computation is performed by stipulating a grid over the scan (in workspace coordinates), and selecting mea­ surements closest to the center points of each grid cell. An all point shortest path search then associates remaining measure­ ments with landmark measurements. The optimization is first performed for the thinned scan; after the landmark scans are localized (and the corresponding coordinate transformation are computed), the remaining measurements are optimized locally, in groups corresponding to individual landmark mea­ surements. Smoothness is attained by using multiple land­ mark measurements as boundary conditions in this optimiza­ tion. Figure 4 shows an examples of a thinned graph, for which the optimization can be carried out in seconds.

4 Setup and Experimental Results

Our approach was implemented using a mobile robot, in an attempt to acquire 3D models of non-stationary objects. In a technique adopted from [11], we first acquired a map of the environment (see Figure 5). Non-stationary objects were de­ tected through differencing of scans, using the robot's local­ ization routines to get a rough estimate of this pose. Figure 2b illustrates the segmentation process. Red scans are retained while the black scans are assumed to correspond to the back­ ground and are henceforth discarded.

Figure 5: 2D map, object (center) and four different vantage points.

Figure 6 illustrates one iteration of the algorithm in all es­ sential steps, using data acquired by the robot. Results for matching three scans with different postures are shown in Fig­ ure 7(a-b). While the standard registration procedure leads to a model with six arms, our approach correctly deforms the scan to arrive at an improved model, with two arms. A sim­ ilar result is shown in Figure 7(c-g), which shows three raw scans on the left, followed by the result of (rigid) scan reg­ istration and the result of our approach. Another example is the chair in Figure 8(a-d) scanned in different heights. The standard registration will lead to multiple feet, our approach correctly aligns them. Table 1 shows the cumulative distance between points in the nearest neighbor calculation. The value marked as "start" is the result of an initial registration phase, reflecting the remaining distances under the rigid body as­ sumption. All other columns correspond to further iterations of our algorithm as it adjusts the shape of the scans. This re­ sult illustrates numerically the integrity of the result is indeed improved by iterate the process.

5 Discussion

This paper proposed a technique for simultaneous scan reg­ istration and scan deformation for modeling nonrigid objects. The deformation was made possible through the definition of (soft) links between neighboring scan points, whose config­ uration was calculated during registration. To tackle the re­ sulting optimization problem efficiently, we described a hi­ erarchical optimization techniques that operated on thinned graphs. Experimental results obtained using a mobile robot illustrated the viability of this approach.

There are many problems in object modeling that this pa­ per does not address, but whose inclusion shall be the subject of future research. For example, the present segmentation ap­ proach is somewhat simplistic: It will fail if more than one non-stationary object appears in the scene. The approach re­ quires deformations to be small, and the target object may not move very far during acquisition. Objects are not sub­ segmented. This will cause difficulties when components of objects are adjacent to different other components, or missing entirely, which can happen if components are combined and cannot be separated to find the corresponding part. A final direction of future research involves the integration into ad­ vanced techniques for between-object data association when modeling multiple objects in non-stationary environments [1].