The most popular algorithm for aligning of 3D point data is the Iterative Closest Point (ICP). This paper proposes a new algorithm for orthogonal registration of point clouds based on the point-to-plane ICP algorithm for affine transformation. At each iterative step of the algorithm, an approximation of the closed-form solution for the orthogonal transformation is derived.
1. Introduction The Iterative Closest Point (ICP) algorithm [1-5] has become the dominant method for aligning three dimensional models based purely on the geometry. For alignment it is necessary to find a geometric transformation that connects two point clouds in ℝ3 by the best way with respect to the 2 norm. The ICP algorithm consists of two main stages: 1. Searching of corresponding points (pairs) in two clouds; 2. Minimizing the error metric (variational subproblem of the ICP).
There are two basic approaches to choosing the error metric for pairs of points. Within the point-topoint approach [1], the distance between the elements of the pair in ℝ3 is used. Within the point-toplane approach [2] the distance between the point of the first cloud and the tangent plane to the corresponding point of the second cloud is used.
The key point [6] of the ICP algorithm is the search of either an orthogonal or affine transformations, best in the sense of a quadratic metric that combines two point clouds with a given correspondence between points (the variational subproblem of the ICP algorithm).
For the point-to-point metric in the case of orthogonal transformations, the solution in a closedform was obtained by Horn [7,8]. The solution [7] is based on the use of quaternions, whereas the solution [8] uses orthogonal matrices. The solutions are linear in time with respect to the number of point pairs. The original ICP algorithm is widely used for the rigid objects registration, but it does not work well for the case of the non-rigid objects. An extension of the ICP algorithm is proposed [9], using scaling in addition to rotation and translation. A generalization of this algorithm to the case of an arbitrary affine transformation was done [10,11]. A closed-form solution to the point-to-point problem was derived [12-14].
The above mentioned approaches for solving the variational subproblem of the ICP algorithm are based on the point-to-point metric. The point-to-plane metric has been shown to perform better than the point-point metric in terms of accuracy and convergence rate [15]. A closed-form solution to the point-to-plane case for orthogonal transformations is an open problem. Instead, iterative methods based on the linear least-squares optimization or closed-form methods for small angles only are often used [12]. Iterative solutions require an initial approximate estimate of the transformation parameters, and the iterations might converge slowly, converge to a local optimum or not converge at all.
In [16,17] a closed-form solution to the point-to-plane problem for an arbitrary affine transformation is proposed. The affine approach works well when the correspondence between point clouds is good. In this case, the affine point-to-plane method precisely reconstructs original geometric transformation for arbitrary affine transformations, in particular for orthogonal transformations [16,17]. When a correspondence between clouds is not sufficiently good, the affine approach cannot reconstructs an original orthogonal transformation.
In this paper, we propose an approximation of a closed-form solution to the point-to-plane problem for orthogonal transformation. The method is based on the closed-form solution for the affine point-toplane problem [16,17], matrix polar decomposition and the Horn’s method for calculating the nearest orthonormal matrix [8]. The proposed method does not require an initial approximate estimate. Computer simulation results are provided to illustrate the performance of the proposed method of solving the minimization problem. 2. Closed-form solution for affine point-to-plane problem Let = { 1, … , } be a source point cloud, and = { 1, … , } be a destination point cloud in ℝ3.
Suppose that the relationship between points in and is given in such a manner that for each point exists a corresponding point . The ICP algorithm is commonly considered as a geometrical transformation for rigid objects mapping to where is a rotation matrix, is a translation vector, = 1, … , .
The group of affine transformations in the dimension of three has 12 generators. It means that the affine transformation in the dimension of three is a function of 12 variables. Let us consider the ICP variational problem for an arbitrary affine transformation in the point-to-plane case. Denote by ( )a surface constructed from the cloud , by ( )denote a tangent plane of ( )at point . Let ( , ) + , be the following function: coordinates:
( )= ∑ =1(< − , > )2, where <∙,∙> denotes the inner product,
is a matrix of an affine transformation in the homogenous (1) (2) (3) (4) (5) (6) (7) (8) is a point from the cloud , is the unitary normal for ( ) = ( = ( 1 ) arg = .
1 2), 3 1 1 2 .
3 ( 0 ) ( ).
The ICP variational problem can be stated as follows:
The solution of the problem (5) is given by the following way [16,17]: is the coefficients matrix 12 × 12 = ( 11 21 31 41 12 22 32 42 13 23 33
43), = 1, … ,3,
= ∑ =1( ) , , = 1, … ,4, = 1, … ,3, ( ) = 1
1 2
1 3
1 ( 1 1
2 2
2 3
2 2 1
3 0 2 3 3 3 0 0 3
0)
= ∑ =1( 1 1
2 1
3 1 ( 1 1 2 3 2 2 2 2 ) , , = 1, … ,4, , = 1, … ,3, 1 2 3 3 3 3 3 0 0 0 0) , = 1, … , , = 1, … ,3, ( ) = , = 1, … , , , = 1, … ,3. (12) 3 + = ( , = 1, … ,3, 11
21 31 41 12 22 32 42 13 23 33 43), is the coefficients column with 12 elements
= ∑ =1
< , >, = 1, … ,3, 3 + = ∑ =1
< , > , , = 1, … ,3.
is the column of variables with 12 elements = ( 11 The reconstructed affine transform is done by the following formula: 31 32 33 34 = 3) . (15) = −1 . 3. Polar decomposition and orthogonal transformations A square matrix M can be decomposed into the product of an orthonormal matrix R and a positive semi-definite matrix S [5]. The matrix S is always uniquely determined. The matrix R is uniquely determined when M is nonsingular. When M is nonsingular, we can actually write directly [8] = ( 11 12 13
… 1 2 1 ),
The matrix
is positive semi-definite and symmetric. The orthogonal matrix in (18) can be computed by the following way [8]: = = , = ( )−2.
1 √ 1 1 0 ( 0 0 1 0 √ 2 0 1 0 , √ 3) transform, computed by the formula (16), to recalculate a translation = ( 1, 2, 3) . the following matrix × 3: where is orthogonal matrix consisting of columns, that are eigenvectors of the matrix .
Numbers , = 1, … ,3, are eigenvalues of the matrix . The formula (18) also defines [8] a nearest orthogonal matrix for the nonsingular matrix . It means that the formula (18) describes the projection from the group (3) to the subgroup (3).
( ) For approximation of the exact solution of the problem (5) we propose the following method. At each step of the ICP algorithm, we project a top-left submatrix ′ (size of 3 × 3) of a matrix of an affine (3)by the formula (19). After that it is necessary to Denote by a result of projection of a top-left submatrix 3 × 3 of a matrix to (3). Denote by (9) (10) (11) (13) (14) (16) (17) (18) (19) (20) denote by the following vector-column × 1:
Then the problem is the least squares problem for the equation Thus we have: =< − , >.
= .
= ( )−1 .
∑ =1(< + − , > )2 = ∑ =1(< , > −< − , > )2 → min (21) (22) (23) (24) . 3. The block-diagram of the proposed algorithm as part of the ICP algorithm is 5.1. We consider two variants of the ICP algorithm here The first is point-to-point ICP based on Horn algorithm. The second is point-to-plane ICP based on the proposed approximation of an exact solution of the variational problem. Other elements of ICP algorithm are same. 5.1.1. Let be the cloud consisting of 34817 points, see figure 1 (blue colour) The cloud (green colour) is obtained from
by the orthogonal transformation 1 is given by = 1 ∗ , where Computed by the proposed method transformation 1 is given as 1 = ( 1 = ( proposed ICP method converges in 10 iterations, processing time 913 milliseconds. 5.1.2. Let be the cloud consisting of 34817 points, see figure 3 (blue colour) The cloud (green colour) is obtained from by the orthogonal transformation 1 is given by
0.91015 −0.36772 0.19081 −0.79646 2 = (−00.2.315728420 −00.8.414655033 00..5832436236 22..4118028339 ).
0.00000 0.00000 0.00000 1.00000 Computed by the proposed method transformation 2 is given as
0.91015 −0.36772 0.19081 −0.79646 2 = (−00.2.315728420 −00.8.414655033 00..5832436236 22..4118028339 ).
0.00000 0.00000 0.00000 1.00000
The reconstructed by the point-to-point ICP geometrical transformation has the same matrix. The point-to-point ICP method converges in 41 iterations, processing time 2458 milliseconds. The proposed ICP method converges in 16 iterations, processing time 1491 milliseconds. = 2 ∗ , where 5.1.3. Let be the cloud consisting of 34817 points, see figure 5 (blue colour) The cloud (green colour) is obtained from by the orthogonal transformation 3 is given by
0.98163 0.00000 −0.19081 −0.64070 3 = ( 00..0138674310 −00.9.189106831 00..9168375390 01..0231256911 ).
0.00000 0.00000 0.00000 1.00000
The reconstructed by the point-to-point ICP geometrical transformation has the same matrix. The point-to-point ICP method converges in 19 iterations, processing time 984 milliseconds. The proposed ICP method converges in 9 iterations, processing time 747 milliseconds. 5.1.4. Let be the cloud consisting of 106289 points, see figure 7 (blue colour) The cloud (green colour) is obtained from by the orthogonal transformation 4 is given by
0.83867 0.54464 −0.00000 1.38331 4 = (−0.45677 0.70337 −0.54464 −0.29804).
−0.29663 0.45677 0.83867 0.99881 0.00000 0.00000 0.00000 1.00000 Computed by the proposed method transformation 4 is given as
0.83867 0.54464 −0.00000 1.38331 4 = (−0.45677 0.70337 −0.54464 −0.29804).
−0.29663 0.45677 0.83867 0.99881 0.00000 0.00000 0.00000 1.00000
The reconstructed by the point-to-point ICP geometrical transformation has the same matrix. The point-to-point ICP method converges in 24 iterations, processing time 6316 milliseconds. The proposed ICP method converges in 16 iterations, processing time 5792 milliseconds. = 4 ∗ , where
In this paper, we revised error minimizing steps of the ICP algorithm. A new algorithm for orthogonal registration of point clouds based on the point-to-plane ICP algorithm for affine transformation is proposed. At each iterative step of the algorithm, an approximation of the closed-form solution for the orthogonal transformation is derived.