Point Cloud Registration Based on Multiparameter Functional

: The registration of point clouds in a three-dimensional space is an important task in many areas of computer vision, including robotics and autonomous driving. The purpose of registration is to ﬁnd a rigid geometric transformation to align two point clouds. The registration problem can be affected by noise and partiality (two point clouds only have a partial overlap). The Iterative Closed Point (ICP) algorithm is a common method for solving the registration problem. Recently, artiﬁcial neural networks have begun to be used in the registration of point clouds. The drawback of ICP and other registration algorithms is the possible convergence to a local minimum. Thus, an important characteristic of a registration algorithm is the ability to avoid local minima. In this paper, we propose an ICP-type registration algorithm ( λ -ICP) that uses a multiparameter functional ( λ -functional). The proposed λ -ICP algorithm generalizes the NICP algorithm (normal ICP). The application of the λ -functional requires a consistent choice of the eigenvectors of the covariance matrix of two point clouds. The paper also proposes an algorithm for choosing the directions of eigenvectors. The performance of the proposed λ -ICP algorithm is compared with that of a standard point-to-point ICP and neural network Deep Closest Points (DCP).


Introduction
Point cloud registration is a key element in the reconstruction of a 3D spatial environment for robots and sensors. Point cloud registration means finding an orthogonal transformation in R 3 that maximizes a consistent overlap between two clouds. The iterative closest points algorithm (ICP) [1,2] is the most known algorithm for registering point clouds. The two basic steps of the ICP algorithm alternate with each other, that is, the estimation of the geometric transformation based on a fixed correspondence (variational problem of the ICP algorithm) and updating the correspondences to their closest matches (matching step).
The most famous functionals used in the variational problem are presented in various ICP variants; that is, point-to-point [1], point-to-plane [2], generalized ICP [3] and normal ICP (NICP) [4,5]. In [6], a closed-form solution to the point-to-point affine problem is described. Closed-form solutions to the affine point-to-plane problem are suggested [7,8]. Closed-form solutions to the point-to-point problem in the class of orthogonal transformations are obtained [9][10][11].
More robust ICP algorithms with other functionals have also been proposed, which are called generalized ICP [4] and NICP [5]. The corresponding variational problems are solved using the iterative conjugate gradients and Gauss-Newton methods [4,5].
The variational ICP problem can be solved not only by deterministic, but also by stochastic methods. One of them is the Grey Wolf Optimizer (GWO) algorithm. Recently, GWO has been applied to a rough point clouds alignment [12] and to the realization of the point-to-point ICP algorithm [13].
Usually, the ICP algorithm monotonically reduces functional values, but owing to the problem non-convexity, the algorithm often stops at suboptimal local minima. Thus, the probability of obtaining an acceptable transformation as a result of the ICP algorithm is a comparative criterion for different types of ICP algorithms and other types of registration algorithms.
The paper [4] presents an experimental comparative analysis of the NICP algorithm, generalized ICP, NDT [14] and KinFu [15]. The analysis shows that the NICP yields better results and a higher robustness compared to the state-of-the-art methods [4]. A closed-form solution to the NICP variational problem is proposed [5]. A comparative analysis of the Gauss-Newton iterative method [4] and the closed-form solution to the NICP variational problem are also obtained [5].
Recently, neural network-based approaches have been proposed for point cloud classification, segmentation and shape matching [16][17][18]. Methods for registering point clouds using neural networks have also been recently suggested [18][19][20]. In particular, an efficient neural network approach referred to as Deep Closest Points (DCP) was applied to the problem of registering point clouds [21]. The DCP neural network implements the main steps of the ICP algorithm. The DCP algorithm maps input point clouds with permutations/rigid-invariant embedding that help identify matching pairs of points; the attention-based module then predicts a soft match between the point clouds; finally, the differentiable singular value decomposition layer predicts the rigid transformation [21]. DCP is trained on the ModelNet40 3D point clouds database [22]. In [21], a comparative analysis of the DCP, ICP and PointNetLK neural network [23] algorithms was carried out.
The complexity of the registration task depends on the features of the source and target point clouds. If the point clouds are congruent, then we have a relatively simpler registration problem than in the case of non-congruent clouds.
Note that in real-world applications, a registration algorithm must deal with noncongruent point clouds. An important part of comparing registration algorithms is their evaluation on non-congruent clouds. An easy way to obtain a pair of non-congruent clouds is to add noise.
In this paper, we propose an ICP variational problem based on the functional which we call λ-functional. Accordingly, the version of ICP described here is referred to as λ-ICP. The proposed λ-functional is a generalization of the NICP functional [4,5]. The NICP functional requires the computation of the covariance matrix in the neighborhood of each point in the cloud. The functional includes an information matrix associated with the covariance matrix. The covariance matrix is factorized by the eigenvalue decomposition into the product of orthogonal eigenvector matrices and a diagonal eigenvalue matrix. The information matrix coincides with the covariance matrix or differs from it by the diagonal elements in the eigenvalue matrix.
In [5], it was noted that the selection of diagonal elements for a given point cloud increases the probability of NICP convergence to an acceptable solution. The application of the proposed λ-functional is based on this observation. The following method is proposed to solve the λ-ICP variational problem. First, we find a solution to the variational problem in the GL (3) class. Second, we project the obtained result onto the submanifold SO(3) in R 9 . Finally, the translation vector is calculated. These three steps provide a closed-form solution to the variational problem of the λ-ICP algorithm that approximates the exact solution. A similar approximation method is used for the point-to-plane ICP [24] and NICP [5].
It is known that conventional eigenvalue decomposition algorithms provide eigenvectors with unpredictable directions [25,26]. The method proposed in [26] reorients each eigenvector so that its sign is coherent with the majority of the vectors. Note that the correct choice of directions of the eigenvectors is critical for the correct work of the λ-ICP algorithm.
In this paper, we propose an algorithm for reorienting eigenvectors that are computed in a neighborhood within a point cloud. We call this algorithm "direction predictor".
Using a computer simulation, we show that the proposed λ-ICP algorithm converges well to a suitable orthogonal transformation for a wide class of regularization tasks.
The paper is organized as follows: In Section 2, we propose a closed-form approximation of the exact solution to the variation problem with λ-functional. In Section 3, we describe the direction predictor algorithm. In Section 4, computer simulation results are presented and discussed. Section 5 summarizes our conclusions.

Information Matrices
Let P = {p 1 , . . . , p s } be a source point cloud, and Q = {q 1 , . . . , q s } be a target point cloud in R 3 . Suppose that the relation between points of P and Q is given in such a manner that for each point p i there is a corresponding point q i .
The information matrices were introduced in [4]. Information matrices had a size of 3 × 3 and were symmetric. Let us consider the k-neighborhood {p 1 i , . . . , p k i } of a point p i ∈ P, i = 1, . . . , s. The mass centerp i of the neighborhood and the covariance Σ i were calculated as follows:p The eigendecomposition of the matrix Σ i was calculated as: where λ 1 , λ 2 , λ 3 are the eigenvalues of Σ i in ascending order, i is the orthogonal matrix of eigenvectors. We defined here an information matrix Ω p i of point p i ∈ P as: where Λ i = diag(λ 1 , λ 2 , λ 3 ). We considered λ 1 , λ 2 , λ 3 as variable parameters. These parameters were selected individually for a given point cloud.
The information matrices Ω q i for point cloud Q were defined similarly. Denote the surfaces constructed from clouds P and Q by S(P) and S(Q), respectively. Let T P (p i ) and T Q (q i ) be the tangent planes of S(P) and S(Q) at points p i and q i , respectively. Let n p i and n q i be the normal vectors to the planes T P (p i ) and T Q (q i ), respectively, i = 1, . . . , s. The normal information matrices Ω n i for the cloud Q were defined in the same way as the information matrices Ω q i .

NICP Functional and Its Decomposition
Denote by J Ω the following functional: where Ω p = {Ω J Ω (R, T) is the NICP functional [4,5]. Let us consider the first term of the functional J Ω . Denote the vector Rp i + T − q i and its coordinates as v The term can be rewritten as: where . Denote by r i1 , r i2 and r i3 columns of the matrix i . Consider the following expression: It follows from Formulas (6) and (7) that:

Definition of the λ-Functional
Let us consider the k-neighborhood of point p i in cloud P. The eigenvectors r

Remark 1.
It is possible to consider not only the correspondence between clouds P and Q, but also the correspondence between Q and P, that is, we established in correspondence point p i in cloud P with point q i in cloud Q.
, ρ a i3 in Q that were defined the same way as vectors r p i1 , r p i2 , r p i3 and r q i1 , r q i2 , r q i3 (for inverse correspondence) were. Denote by J 1 , ..., J 7 the following functionals: The λ-functional J λ was given as: We stated the following variational problem:

λ-Functional in Homogeneous Coordinates
Here, we introduced the following notations in homogeneous coordinates: where the rotation matrix R consisted of elements: The functionals J 1 ,. . . ,J 7 in homogeneous coordinates took the form: The functional J λ in homogeneous coordinates could be written as: The variational problem (19) could be rewritten as:

Gradient of the λ-Functional
The gradients of the functionals J 1 , . . . , J 7 were computed by formulas described in [5]: The gradient of the λ-functional took the form: Consider the following equation: Rewrite (30) by taking into account (26)-(28) as: Denote by M the right part of the Equation (31). Note that M is the known matrix. We rewrote (31) in the following way: We rewrote the system of linear Equation (32) in the matrix form: where B was a 12 × 12 matrix: We introduced the following notation: The elements of matrix B could be computed as: k, α, β, n = 1, 2, 3; , k, α, n = 1, 2, 3, β = 4; Note that matrix B was symmetric since: This means that it was necessary to compute 78 elements of matrix B instead of 144. The computation of all elements for all H was independent.

Projection of Affine Solution to Manifold SO(3) and Translation Vector Revaluation
Let A lu be a 3 × 3 left-upper (affine) submatrix of matrixÂ * . We considered the singular value decomposition UDV t of matrix A lu . The projection R * of A lu to manifold SO(3) was the matrix USV t , where S was given [14] by the formula: The translation vector T * was computed as: where The orthogonal transformation (R * , T * ) was an approximation of the exact solution to variational problem (19). This projection approach was described in [5,24].

Direction Predictor
Standard eigendecomposition algorithms provided eigenvectors with unpredictable directions. This problem has been considered in many papers, in particular in [25,26]. The performance of the proposed λ-ICP algorithm critically depends on the quality of the eigenvector direction predictor.
Suppose that point q i in point cloud Q corresponded to point p i in P. Denote by r The two sets of vectors define the local vector frames in P and Q.
Suppose that the directions of the vectors of the frame in Q were fixed. Our goal was to choose such directions for the vectors of the local frame in P, so that they were co-oriented with the corresponding vectors in Q.
Consider vector r q i3 and the mass centerq i of the neighborhood in Q. Vector r q i3 and pointq i define the oriented axis x of the local frame. Consider the orthogonal projections of all points of cloud Q to axis x.
We denoted by q ent and q ext the first and last projections on axis x. Consider the uniform subdivision of the line segment [q ent ; q ext ] by points {q int(0) = q ent , q int(1) , . . . , q int(m−1) , q int(m) = q ext }, where m is the number of subsegments. Figure 1 shows the described geometrical configuration. We considered each subsegment [q int(j−1) ; q int(j) ] and denote by q k the projection of point q i in Q if this projection belonged to the considered subsegment. Denote by c j the centers of subsegments [q int(j−1) ; q int(j) ], j = 1, . . . , m. We associated value d j with the subsegment in the spirit of the approach proposed in [26]: Denote by ds q i3 the following vector: We compared vectors ds q i3 with ds p i3 (+) and ds q i3 with ds p i3 (−) using L 2 norm. The smallest norm value corresponded to the direction of the third eigenvector in the local frame in P.
In a similar, way we computed the descriptors ds q i1 in Q and ds p i1 in P and vector r p i1 . Vector r p i2 was the vector product r p i3 × r p i1 .

Computer Simulation
In this section, with the help of a computer simulation, we showed the performance of the proposed λ-ICP, DCP [22] and the classical point-to-point ICP (PtP) algorithms. We compared these algorithms in terms of the convergence rate (i.e., the frequency of the convergence of the ICP algorithm to a correct solution).
The algorithms λ-ICP and PtP used the standard method for searching the correspondence between clouds based on the k-d tree.
We used a trained neural network DCP [27]. Since the DCP never returned a true transformation connecting two point clouds, we only used one iteration of the DCP as a rough alignment step and after it used the standard PtP algorithm for a "polish" alignment [22].
Our experiments were organized as follows: An orthogonal geometric transformation given by a known matrix was applied to a source point cloud. The source and transformed clouds were input into a tested algorithm. The ICP algorithm converged if the reconstructed transformation matrix coincided with the original matrix with a given accuracy.
We considered two types of experiments. First, we tested the algorithms on congruent point clouds. Secondly, we tested the algorithms on a more complex problem when clouds P and Q were non-congruent. We applied additive Gaussian and impulse noise, independently, to the two point clouds to obtain non-congruent clouds.
In the case of additive Gaussian noise, each coordinate of a point was distorted by adding a Gaussian random variable with a zero-mean and a standard deviation of σ = 0.05 and σ = 0.15.
The considered impulse noise was defined as follows: the probability of distortion of a point by impulse noise was 0.5; each coordinate of the distorted point was changed by adding a random variable uniformly distributed in the interval [−0.3; 0.3].
In the experiments, we used the four following point clouds: Stanford Bunny and Armadillo; airplane from the ModelNet40 database [22]; human face from Bosphorus database [28]. The Stanford Bunny, Armadillo and airplane clouds consist of 1024 points, the face clouds consist of 1724 points. All points lie in a centered unit sphere. Figure 2 shows the test point clouds. The variable parameter of the DCP algorithm is the quantity of points in the considered neighborhoods. Computational experiments showed that the optimal value of this parameter for congruent clouds was 20. For noisy point clouds, for the DCP we used the maximum possible value of the parameter. The value was limited by the size of GPU memory. Our experiments were carried out on a standard desktop with Ryzen 7 1700X CPU, 32 Gb RAM, GPU NVIDIA GeForce 1080 Ti with 11 Gb GDRAM. The size of the neighborhood in the experiments with Bunny, Armadillo and airplane was 380 points; with the human face it was 220 points. In the case of the non-congruent clouds, we used a neighborhoods size of about 800-1000 points for the λ-ICP. Furthermore, λ-ICP has 21 variable λ-parameters and 1 more variable parameter equal to the number of subsegments in the direction predictor. In the experiments, we used such values of λ-parameters that λ ij1 = λ ij2 = λ ij3 , j = 1, . . . , 7, i = 1, . . . , s.
The statistical experiments were organized as follows: We fixed the value of the rotation angle. We took a random, uniformly distributed direction vector that defined a line containing the origin of the coordinate system. This line was the axis of rotation at a fixed angle. In addition, the components of the translation vector were a random variable uniformly distributed in the interval [0; 1]. The synthesized geometric transformation matrix (true matrix M true ) was applied to source cloud P. The tested algorithms were applied to clouds P and Q. We stated that the registration algorithm converges to true data if the reconstructed transformation matrix M est coincides with the original matrix with a given accuracy. To guarantee statistically correct results, 200 trials for each fixed rotation angle were carried out. The rotation angle varied from 0 to 180 degrees with a step of 10 degrees.

Congruent Case
We considered examples when a registration algorithm converges to a true transformation, and when it does not. Figure 3a shows point clouds P and Q in the initial position. Figure 3b shows P and Q in the position after registration. The estimated matrix M est was equal to matrix M true . In this case, we obtained M true − M est F = 2.44612 × 10 −6 . Figure 3c shows point clouds P and Q in another initial position. Figure 3d shows P and Q in position after registration. In this case, we obtained M true − M est F = 2.82832. For the congruent point clouds, we used the threshold M true − M est F = 0.2 as a criterion for convergence to the true transformation for all tested algorithms. Figure 4a shows the convergence rate of the λ-ICP (blue), DCP (red) and PtP (green) algorithms for Stanford Bunny. Figure 4b shows the convergence rate for Armadillo. Figure 5a shows the convergence rate of the λ-ICP (blue), DCP (red) and PtP (green) algorithms for the airplane. Figure 5b shows the convergence rate for the human face.
The size of the neighborhood of the DCP was equal to 20. Table 1 shows the number of subsegments of the direction predictor for λ-ICP, the size of the neighborhood and the values of parameters λ ijk for the point clouds, where i = 1, . . . , s, j = 1, . . . , 7, k = 1, 2, 3. Table 1. Parameters of λ-ICP algorithm for the congruent case.

Non-Congruent Case
We considered examples when a registration algorithm converged to a true transformation for the case of non-congruent clouds, and when it did not. Figure 6a shows point clouds P and Q in the initial position. Figure 6b shows P and Q in position after registration. In this case, we obtained M true − M est F = 0.113095. Figure 6c shows point clouds P and Q in the another initial position. Figure 6d shows P and Q in the positions after registration. In this case, we obtained M true − M est F = 2.56373. For the non-congruent point clouds, we used the threshold M true − M est F = 0.6 as a criterion for convergence to the true transformation for all tested algorithms.
We considered, besides the convergence rate, the RMSE based on the same experimental results. 4.2.1. Gaussian Noise with σ = 0.05 Figure 7 shows the point clouds distorted by Gaussian noise with σ = 0.05.  Figure 8a shows the convergence rate of the λ-ICP, DCP and PtP algorithms for Stanford Bunny. Figure 8b shows the mean RMSE value computed for transformation matrices based on the same experimental results. Figure 9a shows the convergence rate of the λ-ICP, DCP and PtP algorithms for Armadillo. Figure 9b shows the mean RMSE value computed for transformation matrices based on the same experimental results. Figure 10a shows the convergence rate of the λ-ICP, DCP and PtP algorithms for airplane. Figure 10b shows the mean RMSE value computed for transformation matrices based on the same experimental results. Figure 11a shows the convergence rate of the λ-ICP, DCP and PtP algorithms for human face. Figure 11b shows the mean RMSE value computed for transformation matrices based on the same experimental results. The size of the neighborhood of the DCP for Stanford Bunny and airplane was equal to 380, for Armadillo was 400, and for face was 220.   Table 2 shows the number of subsegments of the direction predictor for λ-ICP, the size of the neighborhood and the values of the parameters λ ijk for the point clouds in the case of Gaussian noise with σ = 0.05, where i = 1, . . . , s, j = 1, . . . , 7, k = 1, 2, 3. Table 2. Parameters of the λ-ICP algorithm for the case of Gaussian noise with σ = 0.05.

Neighborhood
Size Bunny  3  800  2  2  2  1  1  1  1   Armadillo  3  800  2  2  2  1  1  1  1  Airplane  4  1000  2  10  5  1  1  1  1  Face  3  1400  20  1  20  1  1  1  1 4.2.2. Gaussian Noise with σ = 0.15 Figure 12 shows the considered point clouds distorted by Gaussian noise with σ = 0.15. Figure 13a shows the convergence rate of the λ-ICP, DCP and PtP algorithms for Stanford Bunny. Figure 13b shows the mean RMSE value computed for transformation matrices based on the same experimental results.  Figure 14a shows the convergence rate of the λ-ICP, DCP and PtP algorithms for Armadillo. Figure 14b shows the mean RMSE value computed for transformation matrices based on the same experimental results. Figure 15a shows the convergence rate of the λ-ICP, DCP and PtP algorithms for airplane. Figure 15b shows the mean RMSE value computed for transformation matrices based on the same experimental results. Figure 16a shows the convergence rate of the λ-ICP, DCP and PtP algorithms for human face. Figure 16b shows the mean RMSE value computed for transformation matrices based on the same experimental results. The size of the neighborhood of the DCP for Stanford Bunny and airplane was 380, for Armadillo was 400, and for face was 220.  Table 3 shows the number of subsegments of the direction predictor for λ-ICP, the size of the neighborhood and the values of parameters λ ijk for considered point clouds in the case of Gaussian noise with σ = 0.15, where i = 1, . . . , s, j = 1, . . . , 7, k = 1, 2, 3. Table 3. Parameters of the λ-ICP algorithm for the case of Gaussian noise with σ = 0.15.

Neighborhood
Size Bunny  3  800  2  2  2  1  1  1  1   Armadillo  3  800  2  2  2  1  1  1  1  Airplane  4  1000  2  10  5  1  1  1  1  Face  3  800  20  1  20  1  1  1  1 4.2.3. Impulse Noise Figure 17 shows the tested point clouds distorted by Gaussian noise with σ = 0.15. Figure 18a shows the convergence rate of the λ-ICP, DCP and PtP algorithms for Stanford Bunny. Figure 18b shows the mean RMSE value computed for transformation matrices based on the same experimental results.  Figure 19a shows the convergence rate of the λ-ICP, DCP and PtP algorithms for Armadillo. Figure 19b shows the mean RMSE value computed for transformation matrices based on the same experimental results. Figure 20a shows the convergence rate of the λ-ICP, DCP and PtP algorithms for airplane. Figure 20b shows the mean RMSE value computed for transformation matrices based on the same experimental results. Figure 21a shows the convergence rate of the λ-ICP, DCP and PtP algorithms for human face. Figure 21b shows the mean RMSE value computed for transformation matrices based on the same experimental results. The size of the neighborhood of the DCP for Stanford Bunny and airplane was 380, for Armadillo was 400, and for face was 220.    Table 4 shows the number of subsegments of the direction predictor for λ-ICP, the size of the neighborhood and the values of parameters λ ijk for tested point clouds in the case of impulse noise, where i = 1, . . . , s, j = 1, . . . , 7, k = 1, 2, 3. Table 4. Parameters of the λ-ICP algorithm for the case of impulse noise.

Summarizing the Results for Different Cases
Note that the proposed λ-ICP algorithm and DCP had a very high convergence rate for an arbitrary initial position of point clouds in R 3 . In doing so, λ-ICP used a standard method to find the correspondence between point clouds.

Conclusions
In this paper, a new variant of the variational problem of the ICP algorithm was proposed. A closed-form of the exact solution for the rigid registration of point clouds based on the proposed λ-ICP algorithm and a closed-form of an approximate solution to the corresponding variational problem using a multiparameter functional were presented. We also proposed an efficient algorithm for choosing the direction for eigenvectors in point clouds. With the help of a computer simulation, the performance of the proposed algorithm was compared with that of the state-of-the-art registration algorithms. The proposed algorithm yielded a high rate of convergence to an acceptable transformation for a wide class of point clouds.