Coarse Point Cloud Registration Based on Variational Functionals

Abstract

Point cloud collection forming a 3D scene typically uses information from multiple data scans. The common approach is to register the point cloud pairs consequentially using a variant of the iterative closest point (ICP) algorithm, but most versions of the ICP algorithm only work correctly for a small movement between two point clouds. This makes it difficult to accumulate multiple scans. Global registration algorithms are also known, which theoretically process point clouds at arbitrary initial positions. Recently, a multiparameter variational functional was described and used in the ICP variant to register point clouds at arbitrary initial positions. The disadvantage of this algorithm was the need for manual selection of parameters. In this paper, a modified version of the algorithm with automatic selection of the model parameters is proposed. The proposed algorithm is a fusion of the ICP and RANSAC concepts. Moreover, the algorithm can be parallelized. The performance of the proposed algorithm is compared with that of known global registration algorithms.

1. Introduction

In computer vision, splicing different parts of 3D models is necessary to reconstruct the objects in three-dimensional space. Point cloud registration is a basic element to solve this problem. Point cloud registration is a widely studied problem. Known algorithms in this area are described in surveys [1,2]. Registration algorithms can be divided into two groups. The first of them contains refinement algorithms, and the second one contains coarse registration algorithms. Refinement algorithms are used to align the source point cloud $P$ and the target point cloud $Q$if the rotation angles connecting $P$ and $Q$ are small enough. Coarse registration algorithms are used in the case of sufficiently large angles.

The following abbreviations are used throughout the paper: ICP—iterative closest points algorithm; PtP-ICP—point-to-point ICP; PCL-GICP—software implementation of generalized ICP (GICP) from point cloud library; NICP—normal ICP; RANSAC—random sample consensus algorithm; Cluster—registration algorithm based on the cluster approach; Super4PCS—global registration algorithm; Nsamples—overlap–parameters of Super4PCS; λ-ICP—registration algorithm based on the multiparameter λ-functional; λ_r-ICP—registration algorithm based on the proposed λ_r-functional; LCP—largest common pointset; Delta—distance threshold for LCP; SHOT—signature of histograms of orientations descriptor; threshold_PtP—threshold of the distance for corresponding point pairs for PtP-ICP; GL (3)—general linear group in three dimensions; SO(3)—special orthogonal group in three dimensions.

The iterative closest points algorithm (ICP) [3,4] is a common refinement algorithm. The two basic steps of the ICP algorithm alternate with each other, that is, the estimation of a geometric transformation based on a fixed correspondence (variational problem of the ICP algorithm) and updating the correspondences to their closest matches (correspondence step).

The most well-known functionals used in the ICP variational problem are presented in ICP variants, such as point-to-point [3], point-to-plane [4], generalized ICP (GICP) [5], and normal ICP (NICP) [6,7]. Closed-form solutions to the affine point-to-point [8] and point-to-plane [9,10] problems have been proposed. Closed-form solutions to the orthogonal point-to-point problem [11,12,13] have also been obtained. Generalized ICP [5] and NICP [6,7] with other functionals are more robust. In these cases, iterative conjugate gradients and Gauss–Newton methods [5,6] are used to solve the variational ICP problems.

Stochastic methods based on the grey wolf optimizer (GWO) algorithm are exploited to solve the variational ICP problems. The GWO has recently been used to a coarse point cloud alignment [14] and implementation of the point-to-point ICP algorithm [15].

The paper in [6] presents an experimental comparative analysis of the NICP algorithm, generalized ICP (GICP), NDT [16], and KinFu [17]. The analysis shows that the NICP yields better results and higher robustness compared to the state-of-the-art methods [6]. A closed-form solution to the NICP variational problem was proposed [7]. This solution was compared with the performance of the iterative Gauss–Newton method [6].

Coarse registration approaches usually use some form of the well-known RANSAC algorithm [18] or related methods to confirm a congruent relationship [19,20,21]. Based on this technique, 4PCS [22] and Super4PCS [23] methods have achieved a significant increase in speed compared to alternative approaches and provide unstructured and efficient scene acquisition at scales that were previously impossible. In addition, feature-based methods such as SHOT [24] and FPFH [25] are also popular; however, they are sensitive to noise. SDRSAC [26] uses a randomized strategy without correspondences.

Registration methods based on neural networks can also be regarded as coarse registration algorithms. Recently, point cloud classification, segmentation, and shape matching have been carried out using neural networks [27,28,29,30,31,32]. So, to solve the problem of registering point clouds, a neural network referred to as deep closest points (DCP) was designed [33]. DCP is trained on the ModelNet40 3D point clouds database [34].

Recently, the $\lambda$-functional and $\lambda$-ICP algorithms have been proposed [35]. The $\lambda$-functional is a generalized version of the NICP functional. In order to compute the NICP functional, the covariance matrix in the vicinity of each cloud point is calculated. By selecting diagonal elements for a given point cloud, the probability of NICP convergence to acceptable solution can be increased [7]. A similar approach is also used for the λ-functional.

In real applications, a registration algorithm deals with incongruent point clouds. Therefore, the performance of registration algorithms should be compared on incongruent clouds. It was shown in [35] that $\lambda$-ICP based on the $\lambda$-functional is very efficient, but the drawback of $\lambda$-ICP is the need to select the $\lambda$ parameters manually for a given type of point cloud.

In this paper, a coarse registration algorithm based on a new $\lambda$_r-functional, which is a reduced version of the $\lambda$-functional, is proposed. The proposed ICP-type algorithm, referred to as $\lambda$_r-ICP, contains some elements based on RANSAC and deals with coarse registration of incongruent point clouds. To solve the $\lambda$_r-ICP variational problem, first the solution to the variational problem in the class GL (3) is obtained. Next, the result is projected onto the submanifold SO(3). Finally, the translation vector is computed. The variational problem $\lambda$-ICP [35], NICP [7] and point-to-plane ICP [36] is solved similarly.

In this paper, we propose a new solution to the first step of the scheme described above. The $\lambda$_r-functional is a reduced version of the $\lambda$-functional with special properties. In particular, the $\lambda$-functional contains terms that are vector projections onto local vector bases, while the $\lambda$_r-functional contains terms that are vector projections onto the global vector basis. This requires a different approach to calculating the functional gradient. To minimize the $\lambda$-functional, 4 × 4 transformation matrices in homogeneous coordinates were used, while 3 × 3 matrices in Cartesian coordinates were used for the $\lambda$_r-functional. The resulting calculation formulas for the $\lambda$_r-functional differ significantly from the corresponding formulas for the $\lambda$-functional. The sequential (i.e., nonparallel) software implementation of the proposed solution to minimize the $\lambda$_r-functional is about 10 times faster than the sequential software implementation for the $\lambda$-functional. In addition, the proposed solution calculates three rows of the rotation matrix independently, in parallel computational threads.

Note that the $\lambda$-functional and the $\lambda$_r-functional contain, in a certain sense, the point-to-point, point-to-plane, and NICP functionals. The parameters ${\lambda }_1$, ${\lambda }_2,$ and ${\lambda }_3$ of the $\lambda$_r-functional are computed by averaging the local geometric information of all points in the point cloud. After that, two free parameters, ${\lambda }_4$ and $k$, where $k$ is the neighborhood size, are used. The $\lambda$_r-ICP algorithm contains iterating over two values of $k$ and sixteen values of ${\lambda }_4$, i.e., the algorithm forms 32 geometric transformations. The choice of the best transformation is carried out using the largest common pointset (LCP) [22,23]. Note that the proposed$\lambda$_r-ICP algorithm contains elements of both ICP and RANSAC approaches.

The proposed algorithm is compared with known coarse registration algorithms, Super4PCS and RANSAC, using the software implementation of Super4PCS [37]. The program generates coarse geometric transformations and returns the best result according to the LCP criterion. After that, we apply refinement with the point-to-point ICP with the selection of pairs of corresponding points.

The $\lambda$_r-ICP and RANSAC algorithms are organized in the following way. First, these algorithms form a set of coarse geometric transformations. Second, refinements are applied to each coarse transformation. Third, the best transformation is selected according to the LCP criterion.

We also use the following algorithms for comparison: point-to-point ICP with selection of pairs of corresponding points; GICP [5] (its software implementation from the point cloud library (PCL) [38]); and coarse registration algorithm based on the cluster approach [39,40].

It is known that conventional eigenvalue decomposition algorithms produce eigenvectors with unpredictable directions [24,41]. The accurate choice of orientation of the eigenvectors is important for the correct operation of the $\lambda$_r-ICP algorithm. In [35], an algorithm was proposed for reorienting the eigenvectors calculated in a neighborhood inside a point cloud. In this paper, a slightly modified version of this algorithm is described. The modified algorithm is called orientation predictor and is based on a local geometric descriptor. The descriptor is also used for selecting pairs of corresponding points in the $\lambda$_r-ICP algorithm.

The main contributions of the proposed paper are as follows:

• A new $\lambda$_r-functional is proposed, which is a reduced version of the previously described $\lambda$-functional;
• A new computational approach to minimization of the $\lambda$_r-functional is proposed. This approach essentially differs from the corresponding one for the λ-functional and is much faster;
• A new registration algorithm $\lambda$_r-ICP is proposed, which automatically computes the parameters of the $\lambda$_r-functional for the given source and target incongruent noisy point clouds. The $\lambda$_r-ICP algorithm is a fusion of the ICP and RANSAC concepts.

The paper is organized as follows. Section 2 proposes an exact solution approximation to the variation problem with the $\lambda$_r-functional. In Section 3, the orientation predictor and $\lambda$_r-ICP algorithm are described. Section 4 presents and discusses the results of computer simulations. Section 5 summarizes our findings.

2. Reduced $\lambda$-Functional

The proposed approach is based on the $\lambda$-functional described in [35]. In [35], the values of the $\lambda$-functional parameters are selected manually. Here we propose an approach to automatic parameters search.

2.1. Local Geometric Features

Let $P$ and $Q$ be the source and target point clouds, respectively, with a given correspondence between them (from the source cloud $P$ to the target cloud $Q$), $P=\left\{p_1,\dots p_s\right\}$ and $Q=\left\{q_1,\dots q_s\right\}$, $s$ is the number of points in the cloud $P$. Note that we consider clouds $P$ and $Q$ with an already given correspondence, the initial quantity of points in $P$ and $Q$ may be different. Let us consider the k-neighborhood $\left\{q_{i1},\dots ,q_{ik}\right\}$ of the point $q_i$, $i=1,\dots ,s$, in the cloud $Q$. Denoted by $c_i$, the mass center of the neighborhood, $c_i=\frac{1}{k}{{\displaystyle \sum }}_{j=1}^k{q}_{ij}$. The covariance matrix ${\mathsf{\Sigma }}_i$ of the neighborhood is defined as

$${\mathsf{\Sigma }}_i=\tilde{Q}{\tilde{Q}}^t$$

(1)

where

$$\tilde{Q}=\left(\begin{array}{ccc}{\tilde{q}}_{i11}& & {\tilde{q}}_{ik1}\\ {\tilde{q}}_{i12}& \dots & {\tilde{q}}_{ik2}\\ {\tilde{q}}_{i13}& & {\tilde{q}}_{ik3}\end{array}\right)$$

$${\tilde{q}}_{ij}=q_{ij}-c_i, j=1,\dots ,k$$

The eigendecomposition of the matrix ${\mathsf{\Sigma }}_i$ can be written as

$${\mathsf{\Sigma }}_i={\Re }_i{\mathsf{\Lambda }}_i{\Re }_i{}^t$$

(2)

$${\mathsf{\Lambda }}_i=\left(\begin{array}{ccc}{\lambda }_{i1}& 0& 0\\ 0& {\lambda }_{i2}& 0\\ 0& 0& {\lambda }_{i3}\end{array}\right)$$

where ${\lambda }_{i3}\ge {\lambda }_{i2}\ge {\lambda }_{i1}>0$ are the eigenvalues of the covariance matrix, and ${\Re }_i$ is the orthogonal matrix of the eigenvectors. The matrix ${\Re }_i$ consists of the vector-columns $r_{i1}$, $r_{i2}$, and $r_{i3}$, corresponding to the eigenvalues ${\lambda }_{i1}$, ${\lambda }_{i2}$, and ${\lambda }_{i3}$. The vector $r_{i1}$ can be considered as a normal vector to the tangent plane at the point $q_{ij}$, and the vector $r_{i3}$ corresponds to the main axis of the inertia ellipsoid of the given neighborhood.

The covariance matrix for the point cloud $P$ is defined in a similar manner.

2.2. 𝜆-Functional and Its Variants

The $\mathsf{\lambda }$-functional is given as follows [35]:

$$J_{\lambda }\left(R,T\right)={{\displaystyle \sum }}_{i=1}^s{\left(R{r}_{i1}{}^p-r_{i1}{}^q\right)}^t{\Re }_i{}^q{\mathsf{\Lambda }}_{i1}{}^q{({\Re }_i{}^q)}^t\left(R{r}_{i1}{}^p-r_{i1}{}^q\right)+{\left(R{r}_{i2}{}^p-r_{i2}{}^q\right)}^t{\Re }_i{}^q{\mathsf{\Lambda }}_{i2}{}^q{({\Re }_i{}^q)}^t\left(R{r}_{i2}{}^p-r_{i2}{}^q\right)+$$

$${\left(R{r}_{i3}{}^p-r_{i3}{}^q\right)}^t{\Re }_i{}^q{\mathsf{\Lambda }}_{i3}{}^q{({\Re }_i{}^q)}^t\left(R{r}_{i3}{}^p-r_{i3}{}^q\right)+$$

$${\left(R{p}_i+T-q_i\right)}^t{\Re }_i{}^q{\mathsf{\Lambda }}_{i4}{}^q{({\Re }_i{}^q)}^t\left(R{p}_i+T-q_i\right)+$$

$${\left(R^t{\rho }_{i1}{}^q-{\rho }_{i1}{}^p\right)}^t{\Re }_i{}^p{\mathsf{\Lambda }}_{i1}{}^p{({\Re }_i{}^p)}^t\left(R^t{\rho }_{i1}{}^q-{\rho }_{i1}{}^p\right)+{\left(R^t{\rho }_{i2}{}^q-{\rho }_{i2}{}^p\right)}^t{\Re }_i{}^p{\mathsf{\Lambda }}_{i2}{}^p{({\Re }_i{}^p)}^t\left(R^t{\rho }_{i2}{}^q-{\rho }_{i2}{}^p\right)+$$

$${\left(R^t{\rho }_{i3}{}^q-{\rho }_{i3}{}^p\right)}^t{\Re }_i{}^p{\mathsf{\Lambda }}_{i3}{}^p{({\Re }_i{}^p)}^t\left(R^t{\rho }_{i3}{}^q-{\rho }_{i3}{}^p\right)$$

(3)

where $r_{i1}{}^p$, $r_{i2}{}^p$, and $r_{i3}{}^p$ are vector columns of the orthogonal matrix ${\Re }_i{}^p$ (of point cloud $P$), ${\mathsf{\Lambda }}_{i1}{}^q$, ${\mathsf{\Lambda }}_{i2}{}^q$, and ${\mathsf{\Lambda }}_{i3}{}^q$ are diagonal matrices with elements $\left({\lambda }_{ij1}{}^q,{\lambda }_{ij1}{}^q,{\lambda }_{ij1}{}^q\right)$, $j-1,2,3$, which are considered variable parameters, and the vectors $r_{i1}{}^q$, $r_{i2}{}^q$, and $r_{i3}{}^q$ are vector columns of the orthogonal matrix ${\Re }_i{}^q$ (of point cloud $Q$). The elements of the diagonal matrix ${\mathsf{\Lambda }}_{i4}{}^q$ are also considered variable parameters. The vectors ${\rho }_{i1}{}^q$, ${\rho }_{i2}{}^q$, and ${\rho }_{i3}{}^q$, the vectors ${\rho }_{i1}{}^p$, ${\rho }_{i2}{}^p$, and ${\rho }_{i3}{}^q$, and the matrices ${\mathsf{\Lambda }}_{i1}{}^p$, ${\mathsf{\Lambda }}_{i2}{}^p$, and ${\mathsf{\Lambda }}_{i3}{}^p$ are defined similarly for the inverse correspondence from $P$ to $Q$. $R$ is the rotation matrix and $T$ is the translation vector.

The ICP variational problem (error minimization ICP step) based on the $\mathsf{\lambda }$-functional can be written as

$$\left(R_{*},T_{*}\right)=arg\underset{R,T}{\min }{J}_{\lambda }\left(R,T\right)$$

(4)

subject to $R_{*}\in SO\left(3\right)$. The ICP-type algorithm with the variational problem (4) is called$\mathsf{\lambda }$-ICP. In [35], manual selction of parameters was considered, i.e., elements of the diagonal matrices ${\mathsf{\Lambda }}_{i1}{}^q$, ${\mathsf{\Lambda }}_{i2}{}^q$, ${\mathsf{\Lambda }}_{i3}{}^q$, ${\mathsf{\Lambda }}_{i4}{}^q$, ${\mathsf{\Lambda }}_{i1}{}^p$, ${\mathsf{\Lambda }}_{i2}{}^p$, and ${\mathsf{\Lambda }}_{i3}{}^p$ for a given point cloud pair ($P$,$Q$). In this paper, we propose an approach to find the parameters of the $\mathsf{\lambda }$-functional automatically.

The first term in (3) can be rewritten as

$${\left(R{r}_{i1}{}^p-r_{i1}{}^q\right)}^t{\Re }_i{}^q{\mathsf{\Lambda }}_{i1}{}^q{({\Re }_i{}^q)}^t\left(R{r}_{i1}{}^p-r_{i1}{}^q\right)={\lambda }_{i11}{}^q{\langle r_{i1}{}^q,R{r}_{i1}{}^p-r_{i1}{}^q\rangle }^2+{\lambda }_{i12}{}^q{\langle r_{i2}{}^q,R{r}_{i1}{}^p-r_{i1}{}^q\rangle }^2+$$

$${\lambda }_{i13}{}^q{\langle r_{i3}{}^q,R{r}_{i1}{}^p-r_{i1}{}^q\rangle }^2$$

(5)

where ${\mathsf{\Lambda }}_{i1}{}^q=diag\left({\lambda }_{i11}{}^q,{\lambda }_{i12}{}^q,{\lambda }_{i13}{}^q\right)$ and ${\Re }_i{}^q=\left(r_{i1}{}^q r_{i2}{}^q r_{i3}{}^q\right)$. It follows from (5) that the four first terms in (3) take the form

$${{\displaystyle \sum }}_{i=1}^s{\left(R{r}_{i1}{}^p-r_{i1}{}^q\right)}^t{\Re }_i{}^q{\mathsf{\Lambda }}_{i1}{}^q{({\Re }_i{}^q)}^t\left(R{r}_{i1}{}^p-r_{i1}{}^q\right)+{\left(R{r}_{i2}{}^p-r_{i2}{}^q\right)}^t{\Re }_i{}^q{\mathsf{\Lambda }}_{i2}{}^q{({\Re }_i{}^q)}^t\left(R{r}_{i2}{}^p-r_{i2}{}^q\right)+$$

$${\left(R{r}_{i3}{}^p-r_{i3}{}^q\right)}^t{\Re }_i{}^q{\mathsf{\Lambda }}_{i3}{}^q{({\Re }_i{}^q)}^t\left(R{r}_{i3}{}^p-r_{i3}{}^q\right)+$$

$${\left(R{p}_i+T-q_i\right)}^t{\Re }_i{}^q{\mathsf{\Lambda }}_{i4}{}^q{({\Re }_i{}^q)}^t\left(R{p}_i+T-q_i\right)=$$

$${{\displaystyle \sum }}_{i=1}^s{\lambda }_{i11}{}^q{\langle r_{i1}{}^q,R{r}_{i1}{}^p-r_{i1}{}^q\rangle }^2+{\lambda }_{i12}{}^q{\langle r_{i2}{}^q,R{r}_{i1}{}^p-r_{i1}{}^q\rangle }^2+{\lambda }_{i13}{}^q{\langle r_{i3}{}^q,R{r}_{i1}{}^p-r_{i1}{}^q\rangle }^2+$$

$${\lambda }_{i21}{}^q{\langle r_{i1}{}^q,R{r}_{i2}{}^p-r_{i2}{}^q\rangle }^2+{\lambda }_{i22}{}^q{\langle r_{i2}{}^q,R{r}_{i2}{}^p-r_{i2}{}^q\rangle }^2+{\lambda }_{i23}{}^q{\langle r_{i3}{}^q,R{r}_{i2}{}^p-r_{i2}{}^q\rangle }^2+$$

$${\lambda }_{i31}{}^q{\langle r_{i1}{}^q,R{r}_{i3}{}^p-r_{i3}{}^q\rangle }^2+{\lambda }_{i32}{}^q{\langle r_{i2}{}^q,R{r}_{i3}{}^p-r_{i3}{}^q\rangle }^2+{\lambda }_{i33}{}^q{\langle r_{i3}{}^q,R{r}_{i3}{}^p-r_{i3}{}^q\rangle }^2+$$

$${\lambda }_{i4}{}^q{\left(R{p}_i+T-q_i\right)}^t\left(R{p}_i+T-q_i\right)$$

(6)

where ${\mathsf{\Lambda }}_{i4}{}^q=diag\left({\lambda }_{i4}{}^q,{\lambda }_{i4}{}^q,{\lambda }_{i4}{}^q\right)$.

2.3. Relationship between $\lambda$-Functional and Point-to-Point, Point-to-Plane, and NICP Functionals

Let us consider the relationship between such well-known ICP functionals as point-to-point [3], point-to-plane [4], NICP [6,7], and $\mathsf{\lambda }$-functional [35]. Note that the tens term of the right side of (6) can be written as

$${{\displaystyle \sum }}_{i=1}^s{\lambda }_{i4}{}^q{\left(R{p}_i+T-q_i\right)}^t\left(R{p}_i+T-q_i\right)={{\displaystyle \sum }}_{i=1}^s{\lambda }_{i4}{}^q\left||R{p}_i+T-q_i\right|{|}^2$$

(7)

The functional in (7) is a weighted point-to-point functional.

The first term of the right side of (6) is given as

$${{\displaystyle \sum }}_{i=1}^s{\lambda }_{i11}{}^q{\langle r_{i1}{}^q,R{r}_{i1}{}^p-r_{i1}{}^q\rangle }^2$$

(8)

Since the vector $r_{i1}{}^q$ can be considered normal to the tangent plane at the point $q_i$, the expression (8) is mathematically close to the weighted point-to-plane functional.

The first and fourth terms on the left side of (6) are the NICP functional and given as

$${{\displaystyle \sum }}_{i=1}^s{\left(R{r}_{i1}{}^p-r_{i1}{}^q\right)}^t{\Re }_i{}^q{\mathsf{\Lambda }}_{i1}{}^q{({\Re }_i{}^q)}^t\left(R{r}_{i1}{}^p-r_{i1}{}^q\right)+{\left(R{p}_i+T-q_i\right)}^t{\Re }_i{}^q{\mathsf{\Lambda }}_{i4}{}^q{({\Re }_i{}^q)}^t\left(R{p}_i+T-q_i\right)$$

(9)

Therefore, the $\mathsf{\lambda }$-functional is a generalization of the point-to-point, point-to-plane, and NICP functionals.

2.4. λ_r-Functional

Let us describe a reduced version of the $\mathsf{\lambda }$-functional, referred to as $\mathsf{\lambda }\_\mathrm{r}$-functional. Consider the following functionals:

$$J_{11}\left(R\right)={\lambda }_1{{\displaystyle \sum }}_{i=1}^s{\langle e_1,R{r}_{i1}{}^p-r_{i1}{}^q\rangle }^2,J_{12}\left(R\right)={\lambda }_2{{\displaystyle \sum }}_{i=1}^s{\langle e_2,R{r}_{i1}{}^p-r_{i1}{}^q\rangle }^2,J_{13}\left(R\right)={\lambda }_3{{\displaystyle \sum }}_{i=1}^s{\langle e_3,R{r}_{i1}{}^p-r_{i1}{}^q\rangle }^2$$

$$J_{21}\left(R\right)={\lambda }_1{{\displaystyle \sum }}_{i=1}^s{\langle e_1,R{r}_{i2}{}^p-r_{i2}{}^q\rangle }^2,J_{22}\left(R\right)={\lambda }_2{{\displaystyle \sum }}_{i=1}^s{\langle e_2,R{r}_{i2}{}^p-r_{i2}{}^q\rangle }^2,J_{23}\left(R\right)={\lambda }_3{{\displaystyle \sum }}_{i=1}^s{\langle e_3,R{r}_{i2}{}^p-r_{i2}{}^q\rangle }^2$$

$$J_{31}\left(R\right)={\lambda }_1{{\displaystyle \sum }}_{i=1}^s{\langle e_1,R{r}_{i3}{}^p-r_{i3}{}^q\rangle }^2,J_{32}\left(R\right)={\lambda }_2{{\displaystyle \sum }}_{i=1}^s{\langle e_{2}R{r}_{i3}{}^p-r_{i3}{}^q\rangle }^2,J_{33}\left(R\right)={\lambda }_3{{\displaystyle \sum }}_{i=1}^s{\langle e_3,R{r}_{i3}{}^p-r_{i3}{}^q\rangle }^2$$

$$J_4\left(R,T\right)={\lambda }_4{{\displaystyle \sum }}_{i=1}^s{\langle R{p}_i+T-q_i,R{p}_i+T-q_i\rangle }^2$$

(10)

where $e_1={\left(1 0 0\right)}^t$, $e_2={\left(0 1 0\right)}^t$, and $e_3={\left(0 0 1\right)}^t$ are basis vectors of the coordinate system (in which clouds $P$ and $Q$ are considered). All coefficients $\lambda$ ($\lambda >0$) of (10) do not depend on the number of points.

Definition 1. 

The following functional is called the λ_r-functional:

$$J_{\mathsf{\lambda }\_\mathrm{r}}\left(R,T\right)=J_{11}\left(R\right)+J_{12}\left(R\right)+J_{13}\left(R\right)+J_{21}\left(R\right)+J_{22}\left(R\right)+J_{23}\left(R\right)+J_{31}\left(R\right)+J_{32}\left(R\right)+J_{33}\left(R\right)+J_4\left(R,T\right)$$

(11)

The $\lambda$_r-ICP variational problem can be written as:

$$\left(R_{*},T_{*}\right)=arg\underset{R,T}{\min }{J}_{\mathsf{\lambda }\_\mathrm{r}}\left(R,T\right)$$

(12)

subject to$R_{*}\in SO\left(3\right)$.

The $\mathsf{\lambda }$_r-functional is obtained from the $\mathsf{\lambda }$-functional by reduction. Suppose that ${\Re }_i{}^q=I$, and ${\mathsf{\Lambda }}_{i1}{}^q=diag\left({\mathsf{\lambda }}_1{}^q,{\mathsf{\lambda }}_1{}^q,{\mathsf{\lambda }}_1{}^q\right)$, ${\mathsf{\Lambda }}_{i2}{}^q=diag\left({\mathsf{\lambda }}_2{}^q,{\mathsf{\lambda }}_2{}^q,{\mathsf{\lambda }}_2{}^q\right)$, ${\mathsf{\Lambda }}_{i3}{}^q=diag\left({\mathsf{\lambda }}_3{}^q,{\mathsf{\lambda }}_3{}^q,{\mathsf{\lambda }}_3{}^q\right)$,${ \mathsf{\Lambda }}_4{}^q=diag\left({\mathsf{\lambda }}_4{}^q,{\mathsf{\lambda }}_4{}^q,{\mathsf{\lambda }}_4{}^q\right)$, $i=1,\dots ,s$. Note that information from the matrices ${\Re }_i{}^q$ is used in expressions such as $\left(R{r}_{i1}{}^p-r_{i1}{}^q\right)$.

2.5. Gradient of the λ_r-Functional

$c_p$ and $c_q$ denote the mass centers of the point clouds $P$ and $Q$, respectively. Let us $P\prime$ and $Q\prime$ also denote the following point clouds:

$$P^{\prime }=\left\{p_1^{\prime },\dots p_s^{\prime }\right\}=\left\{p_1-c_p,\dots p_s-c_p\right\}, Q^{\prime }=\left\{q_1^{\prime },\dots q_s^{\prime }\right\}=\left\{q_1-c_q,\dots q_s-c_q\right\}$$

(13)

The $\mathsf{\lambda }$_r-functional for clouds $P\prime$ and $Q\prime$ takes the following form:

$$J_{\mathsf{\lambda }\_\mathrm{r}}\left(R,T\right)=J_{11}\left(R\right)+J_{12}\left(R\right)+J_{13}\left(R\right)+J_{21}\left(R\right)+J_{22}\left(R\right)+J_{23}\left(R\right)+J_{31}\left(R\right)+J_{32}\left(R\right)+J_{33}\left(R\right)+J_4\prime \left(R\right)$$

(14)

where

$$J_4\prime \left(R\right)={\lambda }_4{{\displaystyle \sum }}_{i=1}^s{\langle Rp{\prime }_i-q{\prime }_i,Rp{\prime }_i-q{\prime }_i\rangle }^2$$

(15)

The $\mathsf{\lambda }$_r-ICP variational problem can be rewritten as

$$R_{*}=arg\underset{R}{\min }{J}_{\mathsf{\lambda }\_\mathrm{r}}\left(R\right)$$

(16)

subject to $R_{*}\in SO\left(3\right)$.

We have that

$$\nabla J_{11}\left(R\right)=2{\lambda }_1{{\displaystyle \sum }}_{i=1}^s\langle e_1,R{r}_{i1}{}^p\rangle e_1{\left(r_{i1}{}^p\right)}^t-2{\lambda }_1{{\displaystyle \sum }}_{i=1}^s\langle e_1,r_{i1}{}^q\rangle e_1{\left(r_{i1}{}^p\right)}^t$$

(17)

Let us denote the elements of $R$ and $r_{i1}{}^p$ as

$$R=\left(\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right), r_{i1}{}^p={\left(r_{i11}{}^p r_{i12}{}^p r_{i13}{}^p\right)}^t$$

(18)

Note that $\langle e_1,R{r}_{i1}{}^p\rangle =\langle e_1{\left(r_{i1}{}^p\right)}^t,R\rangle$ and

$$\langle e_1,R{r}_{i1}{}^p\rangle e_1{\left(r_{i1}{}^p\right)}^t=\langle e_1{\left(r_{i1}{}^p\right)}^t,R\rangle e_1{\left(r_{i1}{}^p\right)}^t=$$

$$\langle \left(\begin{array}{ccc}{r}_{i11}{}^p& r_{i12}{}^p& r_{i13}{}^p\\ 0& 0& 0\\ 0& 0& 0\end{array}\right),R\rangle \left(\begin{array}{ccc}{r}_{i11}{}^p& r_{i12}{}^p& r_{i13}{}^p\\ 0& 0& 0\\ 0& 0& 0\end{array}\right)=$$

$$\left(\begin{array}{ccc}{r}_{i11}{}^p\left(r_{i11}{}^p{R}_{11}+r_{i12}{}^p{R}_{12}+r_{i13}{}^p{R}_{13}\right)& r_{i12}{}^p\left(r_{i11}{}^p{R}_{11}+r_{i12}{}^p{R}_{12}+r_{i13}{}^p{R}_{13}\right)& r_{i13}{}^p\left(r_{i11}{}^p{R}_{11}+r_{i12}{}^p{R}_{12}+r_{i13}{}^p{R}_{13}\right)\\ 0& 0& 0\\ 0& 0& 0\end{array}\right)=$$

$$\left(\begin{array}{ccc}{\left(r_{i11}{}^p\right)}^2{R}_{11}+r_{i11}{}^p{r}_{i12}{}^p{R}_{12}+r_{i11}{}^p{r}_{i13}{}^p{R}_{13}& r_{i12}{}^p{r}_{i11}{}^p{R}_{11}+{\left(r_{i12}{}^p\right)}^2{R}_{12}+r_{i12}{}^p{r}_{i13}{}^p{R}_{13}& r_{i13}{}^p{r}_{i11}{}^p{R}_{11}+r_{i13}{}^p{r}_{i12}{}^p{R}_{12}+{\left(r_{i13}{}^p\right)}^2{R}_{13}\\ 0& 0& 0\\ 0& 0& 0\end{array}\right)$$

(19)

The gradient $\nabla J_{11}\left(R\right)$ depends only on the first row of the matrix obtained in (19). This row in transposed form can be rewritten as

$$\left(\begin{array}{ccc}{\left({r_{i11}}^p\right)}^2& {r_{i11}}^p{r_{i12}}^p& {r_{i11}}^p{r_{i13}}^p\\ {{r_{i1}}^p}_2{r_{i11}}^p& {\left({r_{i12}}^p\right)}^2& {r_{i12}}^p{r_{i13}}^p\\ {r_{i13}}^p{r_{i11}}^p& {{r_{i1}}^p}_3{r_{i12}}^p& {\left({r_{i13}}^p\right)}^2\end{array}\right)\left(\begin{array}{c}{R}_{11}\\ R_{12}\\ R_{13}\end{array}\right)=\left({r_{i1}}^p{\left({r_{i1}}^p\right)}^t\right)\left(\begin{array}{c}{R}_{11}\\ R_{12}\\ R_{13}\end{array}\right).$$

(20)

The gradient ${\left(\nabla J_{11}\left(R\right)\right)}^t$ takes the form:

$${\left(\nabla J_{11}\left(R\right)\right)}^t=2{\lambda }_1{{\displaystyle \sum }}_{i=1}^s\left(r_{i2}{}^p{\left(r_{i2}{}^p\right)}^t\right)\left(\begin{array}{c}{R}_{11}\\ R_{12}\\ R_{13}\end{array}\right)-2{\lambda }_1{({{\displaystyle \sum }}_{i=1}^s\langle e_1,r_{i2}{}^q\rangle e_1{\left(r_{i2}{}^p\right)}^t)}^t{}_1,$$

(21)

where ${({{\displaystyle \sum }}_{i=1}^s\langle e_1,r_{i2}{}^q\rangle e_1{\left(r_{i2}{}^p\right)}^t)}^t{}_1$ means the first column of the matrix ${{\displaystyle \sum }}_{i=1}^s\langle e_1,r_{i2}{}^q\rangle e_1{\left(r_{i2}{}^p\right)}^t$.

In a similar manner, the gradients $\nabla J_{12}\left(R\right)$, $\nabla J_{13}\left(R\right)$, $\nabla J_{21}\left(R\right)$, $\nabla J_{22}\left(R\right),$ $\nabla J_{23}\left(R\right)$, $\nabla J_{31}\left(R\right)$, $\nabla J_{32}\left(R\right)$, and $\nabla J_{33}\left(R\right)$ are computed as

$${\left(\nabla J_{12}\left(R\right)\right)}^t=2{\lambda }_2{{\displaystyle \sum }}_{i=1}^s\left(r_{i1}{}^p{\left(r_{i1}{}^p\right)}^t\right)\left(\begin{array}{c}{R}_{21}\\ R_{22}\\ R_{23}\end{array}\right)-2{\lambda }_2{({{\displaystyle \sum }}_{i=1}^s\langle e_2,r_{i1}{}^q\rangle e_2{\left(r_{i1}{}^p\right)}^t)}^t{}_2,$$

(22)

$${\left(\nabla J_{13}\left(R\right)\right)}^t=2{\lambda }_3{{\displaystyle \sum }}_{i=1}^s\left(r_{i1}{}^p{\left(r_{i1}{}^p\right)}^t\right)\left(\begin{array}{c}{R}_{31}\\ R_{32}\\ R_{33}\end{array}\right)-2{\lambda }_3{({{\displaystyle \sum }}_{i=1}^s\langle e_3,r_{i1}{}^q\rangle e_3{\left(r_{i1}{}^p\right)}^t)}^t{}_3,$$

(23)

$${\left(\nabla J_{21}\left(R\right)\right)}^t=2{\lambda }_1{{\displaystyle \sum }}_{i=1}^s\left(r_{i2}{}^p{\left(r_{i2}{}^p\right)}^t\right)\left(\begin{array}{c}{R}_{11}\\ R_{12}\\ R_{13}\end{array}\right)-2{\lambda }_1{({{\displaystyle \sum }}_{i=1}^s\langle e_1,r_{i2}{}^q\rangle e_1{\left(r_{i2}{}^p\right)}^t)}^t{}_1,$$

(24)

$${\left(\nabla J_{22}\left(R\right)\right)}^t=2{\lambda }_2{{\displaystyle \sum }}_{i=1}^s\left(r_{i2}{}^p{\left(r_{i2}{}^p\right)}^t\right)\left(\begin{array}{c}{R}_{21}\\ R_{22}\\ R_{23}\end{array}\right)-2{\lambda }_2{({{\displaystyle \sum }}_{i=1}^s\langle e_2,r_{i2}{}^q\rangle e_2{\left(r_{i2}{}^p\right)}^t)}^t{}_2,$$

(25)

$${\left(\nabla J_{23}\left(R\right)\right)}^t=2{\lambda }_3{{\displaystyle \sum }}_{i=1}^s\left(r_{i2}{}^p{\left(r_{i2}{}^p\right)}^t\right)\left(\begin{array}{c}{R}_{31}\\ R_{32}\\ R_{33}\end{array}\right)-2{\lambda }_3{({{\displaystyle \sum }}_{i=1}^s\langle e_3,r_{i2}{}^q\rangle e_3{\left(r_{i2}{}^p\right)}^t)}^t{}_3,$$

(26)

$${\left(\nabla J_{31}\left(R\right)\right)}^t=2{\lambda }_1{{\displaystyle \sum }}_{i=1}^s\left(r_{i3}{}^p{\left(r_{i3}{}^p\right)}^t\right)\left(\begin{array}{c}{R}_{11}\\ R_{12}\\ R_{13}\end{array}\right)-2{\lambda }_1{({{\displaystyle \sum }}_{i=1}^s\langle e_1,r_{i3}{}^q\rangle e_1{\left(r_{i3}{}^p\right)}^t)}^t{}_1,$$

(27)

$${\left(\nabla J_{32}\left(R\right)\right)}^t=2{\lambda }_2{{\displaystyle \sum }}_{i=1}^s\left(r_{i3}{}^p{\left(r_{i3}{}^p\right)}^t\right)\left(\begin{array}{c}{R}_{21}\\ R_{22}\\ R_{23}\end{array}\right)-2{\lambda }_2{({{\displaystyle \sum }}_{i=1}^s\langle e_2,r_{i3}{}^q\rangle e_2{\left(r_{i3}{}^p\right)}^t)}^t{}_2,$$

(28)

$${\left(\nabla J_{33}\left(R\right)\right)}^t=2{\lambda }_3{{\displaystyle \sum }}_{i=1}^s\left(r_{i3}{}^p{\left(r_{i3}{}^p\right)}^t\right)\left(\begin{array}{c}{R}_{31}\\ R_{32}\\ R_{33}\end{array}\right)-2{\lambda }_3{({{\displaystyle \sum }}_{i=1}^s\langle e_3,r_{i3}{}^q\rangle e_3{\left(r_{i3}{}^p\right)}^t)}^t{}_3.$$

(29)

Let us compute the gradient $\nabla J_4\left(R\right)$

$${\left(\nabla J_4\left(R\right)\right)}^t=2{\lambda }_4{{\displaystyle \sum }}_{i=1}^{s}p{\prime }_i{\left(p{\prime }_i\right)}^t{R}^t-2{\lambda }_4{{\displaystyle \sum }}_{i=1}^{s}p{\prime }_i{\left(q{\prime }_i\right)}^t=$$

$$2{\lambda }_4{{\displaystyle \sum }}_{i=1}^s\left(p{\prime }_i{\left(p{\prime }_i\right)}^t\right)\left(\begin{array}{ccc}{R}_{11}& R_{21}& R_{31}\\ R_{12}& R_{22}& R_{32}\\ R_{13}& R_{23}& R_{33}\end{array}\right)-2{\lambda }_4{{\displaystyle \sum }}_{i=1}^{s}p{\prime }_i{\left(q{\prime }_i\right)}^t.$$

(30)

Formula (30) shows that the gradient ${\left(\nabla J_4\left(R\right)\right)}^t$ splits into three independent parts

$$2{\lambda }_4{{\displaystyle \sum }}_{i=1}^s\left(p{\prime }_i{\left(p{\prime }_i\right)}^t\right)\left(\begin{array}{c}{R}_{11}\\ R_{12}\\ R_{13}\end{array}\right)-2{\lambda }_4({\left({{\displaystyle \sum }}_{i=1}^{s}p{\prime }_i{\left(q{\prime }_i\right)}^t\right)}_1,$$

(31)

$$2{\lambda }_4{{\displaystyle \sum }}_{i=1}^s\left(p{\prime }_i{\left(p{\prime }_i\right)}^t\right)\left(\begin{array}{c}{R}_{21}\\ R_{22}\\ R_{23}\end{array}\right)-2{\lambda }_4({\left({{\displaystyle \sum }}_{i=1}^{s}p{\prime }_i{\left(q{\prime }_i\right)}^t\right)}_2,$$

(32)

$$2{\lambda }_4{{\displaystyle \sum }}_{i=1}^s\left(p{\prime }_i{\left(p{\prime }_i\right)}^t\right)\left(\begin{array}{c}{R}_{11}\\ R_{12}\\ R_{13}\end{array}\right)-2{\lambda }_4({\left({{\displaystyle \sum }}_{i=1}^{s}p{\prime }_i{\left(q{\prime }_i\right)}^t\right)}_3,$$

(33)

where $({\left({{\displaystyle \sum }}_{i=1}^{s}p{\prime }_i{\left(q{\prime }_i\right)}^t\right)}_j$ means $j$-th column of the matrix ${{\displaystyle \sum }}_{i=1}^{s}p{\prime }_i{\left(q{\prime }_i\right)}^t$, $j=1,2,3$.

Formulas (21)–(29) and (31)–(33) show that the full gradient ${\left(\nabla J\left(R\right)\right)}^t$ split into three independent parts with respect to variables ${\left(R_{11} R_{12} R_{13}\right)}^t$, ${\left(R_{21} R_{22} R_{23}\right)}^t$, and ${\left(R_{31} R_{32} R_{33}\right)}^t$.

2.6. Solution to the Variational Problem

First, we solve the variation problem (16) without the restriction $R_{*}\in SO\left(3\right)$, i.e., we look for a solution in the class of affine transformations. Let us equate the gradient ${\left(\nabla J\left(R\right)\right)}^t$ to zero. $M_{123}$ and $M_4$ denote following matrices:

$$M_{123}=({{\displaystyle \sum }}_{i=1}^s\left(r_{i1}{}^p{\left(r_{i1}{}^p\right)}^t\right)+\left(r_{i2}{}^p{\left(r_{i2}{}^p\right)}^t\right)+\left(r_{i3}{}^p{\left(r_{i3}{}^p\right)}^t\right))$$

(34)

$$M_4={{\displaystyle \sum }}_{i=1}^s\left(p{\prime }_i{\left(p{\prime }_i\right)}^t\right)$$

(35)

So, three independent linear systems of equations are obtained

$$\left({\lambda }_1{M}_{123}+{\lambda }_4{M}_4\right)\left(\begin{array}{c}{R}_{11}\\ R_{12}\\ R_{13}\end{array}\right)={\lambda }_1{({{\displaystyle \sum }}_{i=1}^s\langle e_1,r_{i1}{}^q\rangle e_1{\left(r_{i1}{}^p\right)}^t)}^t{}_1+{\lambda }_1{({{\displaystyle \sum }}_{i=1}^s\langle e_1,r_{i2}{}^q\rangle e_1{\left(r_{i2}{}^p\right)}^t)}^t{}_1+$$

$${\lambda }_1{({{\displaystyle \sum }}_{i=1}^s\langle e_1,r_{i3}{}^q\rangle e_1{\left(r_{i3}{}^p\right)}^t)}^t{}_1+{\lambda }_4({\left({{\displaystyle \sum }}_{i=1}^{s}p{\prime }_i{\left(q{\prime }_i\right)}^t\right)}_1,$$

(36)

$$\left({\lambda }_2{M}_{123}+{\lambda }_4{M}_4\right)\left(\begin{array}{c}{R}_{21}\\ R_{22}\\ R_{23}\end{array}\right)={\lambda }_2{({{\displaystyle \sum }}_{i=1}^s\langle e_2,r_{i1}{}^q\rangle e_2{\left(r_{i1}{}^p\right)}^t)}^t{}_2+{\lambda }_2{({{\displaystyle \sum }}_{i=1}^s\langle e_2,r_{i2}{}^q\rangle e_2{\left(r_{i2}{}^p\right)}^t)}^t{}_2+$$

$${\lambda }_2{({{\displaystyle \sum }}_{i=1}^s\langle e_2,r_{i3}{}^q\rangle e_2{\left(r_{i3}{}^p\right)}^t)}^t{}_2+{\lambda }_4({\left({{\displaystyle \sum }}_{i=1}^{s}p{\prime }_i{\left(q{\prime }_i\right)}^t\right)}_2,$$

(37)

$$\left({\lambda }_3{M}_{123}+{\lambda }_4{M}_4\right)\left(\begin{array}{c}{R}_{31}\\ R_{32}\\ R_{33}\end{array}\right)={\lambda }_3{({{\displaystyle \sum }}_{i=1}^s\langle e_3,r_{i1}{}^q\rangle e_3{\left(r_{i1}{}^p\right)}^t)}^t{}_3+{\lambda }_3{({{\displaystyle \sum }}_{i=1}^s\langle e_3,r_{i2}{}^q\rangle e_3{\left(r_{i2}{}^p\right)}^t)}^t{}_3+$$

$${\lambda }_3{({{\displaystyle \sum }}_{i=1}^s\langle e_3,r_{i3}{}^q\rangle e_3{\left(r_{i3}{}^p\right)}^t)}^t{}_3+{\lambda }_4({\left({{\displaystyle \sum }}_{i=1}^{s}p{\prime }_i{\left(q{\prime }_i\right)}^t\right)}_3.$$

(38)

Remark 1. 

Formulas (36)–(38) allow parallel computation of the rows of the matrix $R$.

After obtaining an affine solution $R,$ it can be projected onto SO(3). The orthogonal solution is given as

$$R_{*}=US{V}^t$$

(39)

where the matrices $U$ and $V$ are the elements of the singular value decomposition $U\mathcal{D}{V}^t$ of the matrix $A\prime$ (assume that $\mathcal{D}$ is a diagonal matrix with nonincreasing diagonal entries), and $S$ is defined as

$$S=\{\begin{array}{c}I, if\det \left(U\right)\det \left(V\right)=1\\ diag\left(1,1,-1\right), if \det \left(U\right)\det \left(V\right)=-1 \end{array}$$

(40)

The translation vector can be computed as

$$T_{*}=\frac{1}{s}{{\displaystyle \sum }}_{i=1}^s{q}_i-R_{*}{p}_i$$

(41)

Note that in Formula (41), we take points $p_i$ and $q_i$ from the original point clouds $P$ and $Q$ without centralization.

2.7. Selection of Parameters

In [35], manual selection of the parameters of the λ-functional for a given point pair $\left(P,Q\right)$ was used. Here we propose an automatic selection of the parameters of of the $\mathsf{\lambda }$_r-functional parameters. Let $k$ be the number of points in the neighborhood. Let us compute all matrices ${\mathsf{\Lambda }}_{i1}{}^q$, ${\mathsf{\Lambda }}_{i2}{}^q$, and ${\mathsf{\Lambda }}_{i3}{}^q$. Next, we define the matrix

$$\mathsf{\Lambda }=\left(\begin{array}{ccc}{\lambda }_1& 0& 0\\ 0& {\lambda }_2& 0\\ 0& 0& {\lambda }_3\end{array}\right)$$

(42)

as

$$\mathsf{\Lambda }=\frac{1}{s}{{\displaystyle \sum }}_{i=1}^s{\mathsf{\Lambda }}_{i1}{}^q+{\mathsf{\Lambda }}_{i2}{}^q+{\mathsf{\Lambda }}_{i3}{}^q$$

(43)

The obtained values ${\lambda }_1$, ${\lambda }_2$, and ${\lambda }_3$ are used in (10). Note that this method for calculating ${\lambda }_1$, ${\lambda }_2$, and ${\lambda }_3$ was found experimentally.

If the values ${\lambda }_1$, ${\lambda }_2$, and ${\lambda }_3$ are fixed, we have two variable parameters, $k$ and ${\lambda }_4$. In this paper, two neighborhood sizes are used, that is, $k=\left[0.15*s\right]$ and $k=\left[0.85*s\right]$. Let us apply the following values of ${\lambda }_4$:

$${\lambda }_{4j}={10}^{-8}·4^j, j=1,..,16$$

(44)

Thus, one can obtain $16·2=32$ parameter values and corresponding 32 transformations $\left(R_{*},T_{*}\right)$.

Suppose that the point clouds$P$ and $Q$ are given. Let us consider a point $p_i$ in $P$ and compute (by the k-d tree algorithm) the closest point $q_i$ in $Q$. If the distance between $p_i$ and $q_i$ is less than the value of the parameter 𝛿, then we call $p_i$ a suitable point. The total number of suitable points is called the largest common pointset (LCP) parameter.

In our case, 32 LCP values corresponding to geometric transformations are calculated. After that, we select the transformation corresponding to the minimum value of LCP.

3. Vector Orientation Predictor and Full Registration Algorithm

3.1. Vector Orientation Predictor

It is known that eigendecomposition algorithms produce eigenvectors with unpredictable orientation. This problem is described, in particular, in [24,41]. The performance of the ICP algorithm based on the proposed$\mathsf{\lambda }$_r-functional critically depends on the quality of the eigenvector orientation predictor. In [35], we described a method to obtain a predictable orientation of vectors. Let us recall this algorithm and then modify it.

Let $p_i$ and $q_i$ be the corresponding points in the clouds $P$ and $Q$, respectively. Consider the $k$-neighborhoods of $p_i$ and $q_i$ and compute the corresponding covariance matrices. The eigendecomposition of these matrices gives us the orthogonal matrices ${\Re }_i{}^p$ and ${\Re }_i{}^q$. Recall that these matrices consist of the vector-columns $\left(r_{i1}{}^p r_{i2}{}^p r_{i3}{}^p\right)$ and $\left(r_{i1}{}^q r_{i2}{}^q r_{i3}{}^q\right)$.

Suppose that eigenvalues of the covariance matrices are in ascending order. In this case, the vector $r_{i3}{}^p$ corresponds to the main axis of inertia of the neighborhood, and vector $r_{i1}{}^p$ corresponds to the normal vector, provided that the neighborhood is sufficiently flat.

The vector sets $\left(r_{i1}{}^p r_{i2}{}^p r_{i3}{}^p\right)$ and $\left(r_{i1}{}^q r_{i2}{}^q r_{i3}{}^q\right)$ to define the local vector frames in $P$ and $Q$. The origins of these frames are mass centers of the neighborhoods in $P$ and $Q$. Assume that the orientations of vectors of the frame in $Q$ are fixed.

The goal is to choose such orientations for the vectors of the local frame in $P$ that they are co-oriented with those in $Q$.

Consider the vector $r_{i3}{}^q$ and the center of mass $\widehat{q_i}$ of the neighborhood in $Q$. The vector $r_{i3}{}^q$ and the point $\widehat{q_i}$ define the oriented $x$-axis of the local frame. Let us consider the orthogonal projections of all points of the cloud $Q$ onto the $x$. $q_{ent}$ and $q_{ext}$ denote the first and last projections on the $x$-axis. Consider a uniform partition of the line segment $\left[q_{ent};q_{ext}\right]$ by points $\left\{q_{int\left(0\right)}=q_{ent},q_{int\left(1\right)}, \dots ,q_{int\left(m\right)}=q_{ext}\right\}$, where $m$ is the number of subsegments. Leat us consider each subsegment $\left[q_{int\left(j-1\right)};q_{int\left(j\right)}\right]$. We denoted by $q{\prime }_k$ the projection, the point $q_k$ onto the axis, if this projection belongs to the considered subsegment.

$c_j$ denotes the center of subsegment $\left[q_{int\left(j-1\right)};q_{int\left(j\right)}\right]$, $j=1,\dots ,m$. Let us associate the value $d_j$ with the subsegment as

$$\begin{gathered} \hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}\hspace{1em}{d}_j=\frac{1}{{{\displaystyle \sum }}_{q_j\prime \in \left[q_{int\left(j-1\right)};q_{int\left(j\right)}\right]}^{}\left(\left|c_j-q_{int\left(j\right)}\right|-\left|c_j-q_j\prime \right|\right)}· \\ {{\displaystyle \sum }}_{q_j\prime \in \left[q_{int\left(j-1\right)};q_{int\left(j\right)}\right]}^{}\left(\left|c_j-q_{int\left(j\right)}\right|-\left|c_j-q_j\prime \right|\right)\parallel q_j-q_j^{\prime }{\parallel }_{L_2}. \end{gathered}$$

(45)

$d{s}_{i3}{}^q$ denotes the following vector:

$$d{s}_{i3}{}^q=\left(d_1,\dots ,d_m\right)$$

(46)

We call the vector $d{s}_{i3}{}^q$ the descriptor of the third eigenvector of the local frame in $Q$. Let us compute similar descriptors $d{s}_{i3}{}^p(+)$ and $d{s}_{i3}{}^p(-)$. The vector $d{s}_{i3}{}^p(-)$ is the inverse version of the vector $d{s}_{i3}{}^p(+)$.

Compare vectors $d{s}_{i3}{}^q$ with $d{s}_{i3}{}^p(+)$ and $d{s}_{i3}{}^q$ with $d{s}_{i3}{}^p(-)$ using the $L_2$ norm. The smallest norm value corresponds to the orientation of the third eigenvector $r_{i3}{}^p$ in the local frame in $P$.

Similarly, we compute the descriptors $d{s}_{i1}{}^q$ in $Q$ and $d{s}_{i1}{}^p$ in $P$ and vector $r_{i1}{}^p$. The vector $r_{i2}{}^p$ is the vector product $r_{i3}{}^p\times r_{i1}{}^p$.

Remark 2. 

The descriptors$\left(d{s}_{i1}{}^p,d{s}_{i2}{}^p,d{s}_{i3}{}^p\right)$ and$\left(d{s}_{i1}{}^q,d{s}_{i2}{}^q,d{s}_{i3}{}^q\right)$ are orthogonal invariants of local neighborhoods.

Note that the algorithm described above is a simplified version of the following approach.

Suppose that we have a mass center and a main axis of the inertia ellipsoid of the point cloud. Also, assume that the surface defined by the point cloud is triangulated. Consider a set of planes perpendicular to the axis. Each plane intersects the triangulation edges at some points. Let us compute the distances between these points and their orthogonal projections onto the axis. After that, we calculate the average distance and draw the circle in the considered plane. The circle has a radius equal to the calculated average distance. The center of the circle lies on the axis. One can assign to a set of such coaxial circles an array containing the radii of the circles. If the axis orientation changes, the array is inverted. The original and inverted variants of the array determine the agreed vector orientations as follows. Suppose we have a vector and a mass center of a local vector frame in the source point cloud. The vector and the mass center define the oriented axis.

Let us consider the corresponding axis in the target point cloud. One can compute a similar array for the axis in the target point cloud. After that, the difference between the array of the target point cloud and two arrays of the source point cloud is compared by the norm. The smaller value of the norm indicates which axis orientation in the source point cloud should be chosen. To simplify the calculations, the following version of the described approach can be used; that is, the average distance (of orthogonal projections onto the axis) is calculated for points where the projections lie between two adjacent coaxial planes. This is done instead of averaging distances of points belonging to the intersection of the co-axial plane and the triangulated surface (given by the point cloud). The described simplified version is used, since the construction of a triangulated surface is a complex computational task.

Figure 1 illustrates the described approach. The point cloud shown is from the ModelNet40 database [34].

3.2. Full Registration Algorithm Based on λ_r-Functional

The proposed algorithm is called $\mathsf{\lambda }$_r-ICP. In this paper, we use the $\mathsf{\lambda }$_r-ICP algorithm for coarse registration of the incongruent point clouds.

It is known that the classical ICP algorithm cannot work well with incongruent point clouds. The ICP algorithm requires the selection of pairs of corresponding points to solve the registration problem for the incongruent case. Moreover, the common ICP algorithm is used to register point clouds for sufficiently small rotation angles. Thus, the point pair selection is based on threshold values for the distance between corresponding points.

This approach does not work in coarse registration problems because coarse registration deals with large rotation angles. The coarse registration algorithm requires the selection of pairs of points based on orthogonal invariant descriptors.

Note that the lengths of the inertia ellipsoid semi-axes are orthogonal invariants. These lengths are determined by the eigenvalues ${\lambda }_{i1}$, ${\lambda }_{i2}$, and ${\lambda }_{i3}$. The descriptors $\left(d{s}_{i1}{}^p,d{s}_{i2}{}^p,d{s}_{i3}{}^p\right)$ and $\left(d{s}_{i1}{}^q,d{s}_{i2}{}^q,d{s}_{i3}{}^q\right)$ from Section 3.1 are also orthogonal invariants.

Next, we describe the application of these orthogonal invariant descriptors in the proposed version of the $\mathsf{\lambda }$_r-ICP algorithm. Note that the size of neighborhood $k$ is fixed. Suppose that points $p_i$ and $q_i$ form a corresponding pair. Compute the local vector frames for $p_i$ and $q_i$ (item 2.1). Let us denote by ${\lambda }_{i1}{}^p$, ${\lambda }_{i2}{}^p$, ${\lambda }_{i3}{}^p$ and ${\lambda }_{i1}{}^q$, ${\lambda }_{i2}{}^q$, ${\lambda }_{i3}{}^q$ the eigenvalues for $p_i$ and $q_i$, respectively. The parameters can be defined the follows:

$$dist\_{\mathsf{\lambda }}_i=\max \left\{\left|{\lambda }_{i1}{}^p-{\lambda }_{i1}{}^q\right|,\left|{\lambda }_{i2}{}^p-{\lambda }_{i2}{}^q\right|,\left|{\lambda }_{i3}{}^p-{\lambda }_{i3}{}^q\right|\right\}, i=1,\dots s$$

(47)

$$dist\_d{s}_i=max\left\{\parallel d{s}_{i1}{}^p-d{s}_{i1}{}^q{\parallel }_{L_1},\parallel d{s}_{i2}{}^p-d{s}_{i2}{}^q{\parallel }_{L_1},\parallel d{s}_{i3}{}^p-d{s}_{i3}{}^q{\parallel }_{L_1}\right\}, i=1,\dots s$$

(48)

Let us calculate values $dist\_{\mathsf{\lambda }}_i$ for all corresponding pairs and select 20% of the best pairs (i.e., with minimum values) from the total. After that, calculate values $dist\_{\mathrm{ds}}_i$ for all remaining pairs and select 25% of the best pairs (i.e., with minimal values). After these two selection steps, we are dealing with 5% of pairs. These steps are called the $\mathsf{\lambda }$_r-ICP selection procedure.

Remark 3. 

In the $\lambda$_r-ICP algorithm (for pairs selection), other orthogonal invariant descriptors can be used instead of those metioned above.

Since the $\mathsf{\lambda }$_r-ICP algorithm is used for coarse registration, we also apply the point-to-point ICP with thresholds to refine the registration result. The threshold_PtP denotes the percentage of best pairs in terms of distance between points. This parameter is used on each iteration of the point-to-point ICP to select the threshold_PtP% of pairs.

The $\mathsf{\lambda }$_r-ICP algorithm is summarized as follows in Algorithm 1.

Algorithm 1 𝜆_r-ICP Algorithm

1. input: cloud P, cloud Q, s = number of points in P, $K=\left\{\left[0.45*s],[0.85*s\right]\right\}$ 2. for counter_1:=1 to 2 do 3. k:=K[counter_1] 4. compute local vector frames in P and Q for k-neighborhoods 5. for counter_2:=1 to 16 do 6. λ4 = 10(−8) · 4counter_2-1 7. for counter_3:= 1 to 300 do 8. compute correspondence between P and Q (by k-d tree algorithm) 9. apply vector orientation predictor for corresponding local frames in P and Q (here local orthogonal invariant descriptors can be changed) 10. apply λ_r-ICP selection procedure 11. solve λ_r-ICP variation problem and obtain geometric transformation (R,T) 12. check the convergence criterion, if the criterion is valid then go to 14 13. end // for counter_3 14. append obtained geometric transformation (R,T) to the list tansform_list 15. end // for counter_2 16. end // for counter_1 17. make refinement by point-to-point ICP algorithm with treshold_PtP to all geometric transformation from tansform_list and choose transformation (R,T) which gives the minimum LCP value for clouds RP + T and Q 18. output: geometric transformation (R*,T*)

4. Computer Simulation

4.1. Compared Algorithms

With the help of a computer simulation, the performance the proposed algorithm $\mathsf{\lambda }$_r-ICP, known as the Super4PCS algorithm [23], and version of the RANSAC algorithm [18] for coarse registration is illustrated and discussed.

We used the software implementation of the Super4PCS algorithm [37]. Super4PCS generates several geometric transformations and chooses one of them that is the best according to the LCP criterion. After that, refinement by the point-to-point ICP algorithm with the parameter threshold_PtP (see Section 3.2) is performed.

The $\lambda$_r-ICP algorithm is implemented as follows. This algorithm computes 32 geometric transformations and uses refinement for all of them. After that, the algorithm selects one of them, which is the best according to the LCP criterion.

The RANSAC is implemented similarly to the $\lambda$_r-ICP algorithm. The algorithm takes four random points in $P$ and four random points in $Q$. The initial correspondence is given by indices. After that, the standard point-to point ICP algorithm is used. The algorithm generates some geometric transformations and uses refinement for all of them. After that, the algorithm selects one of them, which is the best according to the LCP criterion.

In our experiments, the choice of four points is optimal for RANSAC.

Another type of the coarse registration algorithm is described in [39] and its modified version in [40]. This algorithm uses the SHOT descriptor [24] and is based on the cluster approach. We refer to this algorithm as “Cluster”.

The point-to-point ICP algorithm (PtP-ICP) with the selection of pairs of corresponding points and the GICP algorithm (PCL-GICP) [5] (its software implementation from the point cloud library (PCL) [38]) are also used. Note that the considered version of the PtP-ICP algorithm uses the selection of pairs of corresponding points and is more suitable for processing truncated point clouds than PCL-GICP.

4.2. Organisation of Experiments

All point clouds used in our experiments have a diameter $\approx 2$. The following four point clouds were used in the experiments: Stanford Bunny; Airplane from the ModelNet40 database [34]; Scan_1; and Scan_2. The Bunny and Airplane clouds consist of 1024 points. The point clouds Scan_1 and Scan_2 were acquired by the Intel RealSense D435 depth camera. Using the depth camera, we made two maps of the depth of the room for the same camera position, but at different time. We converted depth maps to point clouds, which are similar but not identical. In particular, these point clouds are noisy independently of each other and have a different number of points. The first and second clouds consist of 73,340 and 73,081 points, respectively. We applied downsampling to these point clouds and acquired the clouds Scan_1 and Scan_2, which consist of 2631 and 2362 points, respectively. Clouds Scan_1 and Scan_2 are scaled to a diameter of ≈2. Figure 2 shows the Airplane, Bunny, Scan_1, and Scan_2 point clouds.

Suppose that we have a point cloud $Clou{d}_{init}$. Let us fix a value of the parameter $truncate\_rate$ as a percentage. Calculate the mass center of the cloud and take a randomly oriented line containing the mass center. Let us project all points of $Clou{d}_{init}$ onto a straight line. Find the first and last projections on the line. We select truncate_rate% projections to lie on the line after the first projection. Delete corresponding points from the cloud $Clou{d}_{init}$. Let us denote the resulting point cloud as $P_{init}$.

Select truncate_rate% projections lying on the line before the last projection. Delete corresponding points from the cloud $Clou{d}_{init}$. Let us denote the obtained point cloud as $Q_{init}$.

In our experiments, the following values of the parameter $truncate\_rate$ are used: truncate_rate $=10\%$, truncate_rate $=20\%$, and truncate_rate $=25\%$. Note that the parameter $truncate\_rate$ is related to the parameter overlap_rate. This parameter describes the percentage of common parts of the clouds $P_{init}$ and $Q_{init}$. The corresponding values for overlap_rate are as follows: overlap_rate $\approx 89\%$, overlap_rate $=75\%$, and overlap_rate$\approx 67\%$.

The point cloud $Q\prime$ is obtained from the point cloud $Q_{init}$ as

$$Q^{\prime }=R_{true}{Q}_{init}+T_{true}$$

(49)

Information about the rotation matrix $R_{true}$ and the translation vector $T_{true}$ is contained in the matrix $M_{true}$ in homogeneous coordinates.

The point clouds $P_{init}$ and $Q^{\prime }$ are degraded by additive Gaussian and impulse noise independently of each other. Denote the obtained point clouds as $P$ and $Q$, respectively.

In the case of additive Gaussian noise, each point coordinate was corrupted by adding a Gaussian random variable with a zero mean and a standard deviation of $\sigma =0.05$ and $\sigma =0.10$.

All points are also distorted by impulse noise as follows: each point coordinate is changed by adding a random variable uniformly distributed in the interval $\left[-0.2; 0.2\right].$ The values of the noise level and truncate_rate (overlap_rate) are as follows: (σ = 0.10, truncate_rate =10% (overlap_rate ≈ 89%)); (σ = 0.05, truncate_rate = 20% (overlap_rate = 75%)); (impulse noise, truncate_rate = 10% (overlap_rate ≈ 89%)). We also carried out experiments with the Scan_1 and Scan_2 point clouds using the following conditions: only sensor noise, truncate_rate = 20% (overlap_rate = 75%). The registration task for the Airplane point cloud was also considered with conditions: noise-free, truncate_rate = 25% (overlap_rate ≈ 67%). Note that, in our experiments, we sought a balance between the level of noise and truncation, so that, on the one hand, the registration problem would be quite difficult for the compared algorithms. On the other hand, the algorithms should show a satisfactory level of results.

$M_{\mathrm{est}}$ denotes the matrix of geometric transformation in homogeneous coordinates, which is the result of the registration algorithm. Consider the following expression:

$$dist\_m=\parallel M_{true}-M_{\mathrm{est}}{\parallel }_{L_2}$$

(50)

The parameter $dist\_m$ describes the difference between the true and estimated transforms. Since we are dealing with noisy point clouds, the true transformation cannot be restored exactly. Therefore, the following criterion for the quality of the result of the registration algorithm is applied. We say that the result is good if $0\le dist\_m<0.2$, and the result is medium if $0.2\le dist\_m<0.6$.

Remark 4. 

The parameter dist_m evaluates the quality of the registration result for noisy incongruent point clouds with a single number. The selected parameter values work only for point clouds scaled to a diameter of about 2.

Figure 3a shows the initial point clouds P and $Q$. Figure 3b shows the registration result with the parameter $dist\_m=0.112$. Figure 3c shows the registration result with the parameter $dist\_m=0.204$. Figure 3d shows the registration result with the parameter $dist\_m=2.827$.

The statistical experiments are organized as follows. Let us fix the value of the rotation angle. We took a random, uniformly distributed direction vector defining a line containing the origin of the coordinate system. This line is the axis of rotation at a fixed angle. In addition, the components of the translation vector are a random variable uniformly distributed in the interval $\left[0;1\right]$. The synthesized geometric transformation matrix (true matrix $M_{\mathrm{true}}$) was applied to source cloud $P$. The algorithms were tested on the clouds $P$ and $Q$. To guarantee statistically correct results, 500 trials for each fixed rotation angle were carried out. The rotation angle varied from 0 to 180 degrees with a step of 30 degrees. The number of trials satisfied to the condition of good and medium results was counted.

The percentage of good results ($0\le dist\_m<0.2$) is shown in the graphs. Note that the notation “Convergence rate” for this value is also used in the graphs. The abscissa axis shows the values of rotation angles in degrees. Good and medium results ($0\le dist\_m<0.6$) are also shown in the graphs.

Information about the algorithm parameters are given in the tables. The parameter “Number of ICP iterations” refers to the maximum number of ICP iterations per trial.

Our experiments were carried out on a standard desktop with Ryzen 7 1700XCPU, 32 Gb RAM, GPU NVIDIA GeForce 1080 Ti with 11 Gb GDRAM.

4.3. Experimental Results

4.3.1. Gaussian Noise with $\sigma =0.10$, truncate_rate = 10% (overlap_rate ≈ 89%)

Figure 4 shows examples of pairs $\left(P,Q\right)$ with Gaussian noise with $\sigma =0.10$ and truncate_rate $=10\%$ (overlap_rate $\approx 89\%$). These pairs are the input to the coarse registration algorithms under consideration.

Figure 5a shows the convergence rate of good results, i.e., the frequency of results with $0\le dist\_m<0.2$ for Airplane. Figure 5b shows the convergence rate of good and medium results together, i.e., the frequency of results with $0\le dist\_m<0.6$ for Airplane.

Table 1. Parameters of the tested algorithms for Airplane with $\sigma =0.10$ and truncate_rate $=10\%$ (overlap_rate $\approx 89\%$).

	Total Time per 3500 Trials, s	Number of ICP Iterations	Threshold PtP	Delta (LCP)	Nsamples (Super4PCS)	Overlap (Super4PCS)
λ_r-ICP	28,958	300	0.95	0.06	-	-
RANSAC	29,192	-	0.95	0.06	-	-
Super 4PCS	28,751	-	0.95	0.06	110	0.85
Cluster	8253	-	0.95	0.06	-	-
PCL-GICP	592	300	-	-	-	-
PtP-ICP	117	300	0.95	-	-	-

shows the parameters of the tested algorithms for Airplane with $\sigma =0.10$ and truncate_rate $=10\%$ (overlap_rate $\approx 89\%$).

Figure 6a shows the convergence rate of good results, i.e., the frequency of results with $0\le dist\_m<0.2$ for Bunny. Figure 6b shows the convergence rate of good and medium results together, i.e., the frequency of results with $0\le dist\_m<0.6$ for Bunny.

Table 2. Parameters of the considered algorithms for Bunny with $\sigma =0.10$ and truncate_rate $=10\%$ (overlap_rate $\approx 89\%$).

	Total Time per 3500 Trials, s	Number of ICP Iterations	Threshold PtP	Delta (LCP)	Nsamples (Super4PCS)	Overlap (Super4PCS)
λ_r-ICP	32,853	300	0.95	0.06	-	-
RANSAC	29,344	-	0.95	0.06	-	-
Super 4PCS	33,665	-	0.95	0.06	120	0.85
Cluster	4909	-	0.95	0.06	-	-
PCL-GICP	371	300	-	-	-	-
PtP-ICP	114	300	0.95	-	-	-

shows the parameters of the tested algorithms for Bunny, with $\sigma =0.10$ and truncate_rate $=10\%$ (overlap_rate $\approx 89\%$).

Figure 7a shows the convergence rate of good results, i.e., the frequency of results with $0\le dist\_m<0.2$ for the Scans. Figure 7b shows the convergence rate of good and medium results together, i.e., the frequency of results with $0\le dist\_m<0.6$ for the Scans.

Table 3. Parameters of the tested algorithms for Scan_1 and Scan_2 with $\sigma =0.10$ and truncate_rate $=10\%$ (overlap_rate $\approx 89\%$).

	Total Time per 3500 Trials, sec	Number of ICP Iterations	Threshold PtP	Delta (LCP)	Nsamples (Super4PCS)	Overlap (Super4PCS)
λ_r-ICP	30,194	300	0.95	0.06	-	-
RANSAC	28,920	-	0.95	0.06	-	-
Super 4PCS	30,090	-	0.95	0.06	120	0.85
Cluster	5589	-	0.95	0.06	-	-
PCL-GICP	319	300	-	-	-	-
PtP-ICP	119	300	0.95	-	-	-

shows the parameters of the tested algorithms for Scan_1 and Scan_2 with $\sigma =0.10$ and truncate_rate $=10\%$ (overlap_rate $\approx 89\%$).

4.3.2. Gaussian Noise with $\sigma =0.05$, $\mathrm{truncate}\_\mathrm{rate}=20\%$ ($\mathrm{overlap}\_\mathrm{rate}=75\%$)

Figure 8 shows examples of pairs $\left(P,Q\right)$ with Gaussian noise with $\sigma =0.05$ and truncate_rate = 20% (overlap_rate ≈ 75%). These pairs are the input to the coarse registration algorithms under consideration.

Note that we consider here point clouds Scan_1 and Scan_2 without additional noise, only with sensor noise.

Figure 9a shows the convergence rate of good results, i.e., the frequency of results with $0\le dist\_m<0.2$ for Airplane. Figure 9b shows the convergence rate of good and medium results together, i.e., the frequency of results with $0\le dist\_m<0.6$ for Airplane.

Table 4. Parameters of the tested algorithms for Airplane with $\sigma =0.05$ and truncate_rate $=20\%$ (overlap_rate $=75\%$).

	Total Time per 3500 Trials, s	Number of ICP Iterations	Threshold PtP	Delta (LCP)	Nsamples (Super4PCS)	Overlap (Super4PCS)
λ_r-ICP	24,731	300	0.85	0.06	-	-
RANSAC	28,989	-	0.85	0.06	-	-
Super 4PCS	29,694	-	0.85	0.06	110	0.75
Cluster	4404	-	0.85	0.06	-	-
PCL-GICP	280	300	-	-	-	-
PtP-ICP	88	300	0.85	-	-	-

shows the parameters of the tested algorithms for Airplane with $\sigma =0.05$ and truncate_rate $=20\%$ (overlap_rate $=75\%$).

Figure 10a shows the convergence rate of good results, i.e., the frequency of results with $0\le dist\_m<0.2$ for Bunny. Figure 10b shows the convergence rate of good and medium results together, i.e., the frequency of results with $0\le dist\_m<0.6$ for Bunny.

Table 5. Parameters of the tested algorithms for Bunny with $\sigma =0.05$ and truncate_rate $=20\%$ (overlap_rate $=75\%$).

	Total Time per 3500 Trials, s	Number of ICP Iterations	Threshold PtP	Delta (LCP)	Nsamples (Super4PCS)	Overlap (Super4PCS)
λ_r-ICP	29,243	300	0.85	0.06	-	-
RANSAC	29,238	-	0.85	0.06	-	-
Super 4PCS	22,966	-	0.85	0.06	120	0.75
Cluster	4740	-	0.85	0.06	-	-
PCL-GICP	350	300	-	-	-	-
PtP-ICP	100	300	0.85	-	-	-

shows the parameters of the tested algorithms for Bunny with $\sigma =0.05$ and truncate_rate $=20\%$ (overlap_rate $=75\%$).

Figure 11a shows the convergence rate of good results, i.e., the frequency of results with $0\le dist\_m<0.2$ for the Scans. Figure 11b shows the convergence rate of good and medium results together, i.e., the frequency of results with $0\le dist\_m<0.6$ for the Scans.

Table 6. Parameters of the tested algorithms for Scan_1 and Scan_2 with $\sigma =0.05$ and truncate_rate $=20\%$ (overlap_rate $=75\%$).

	Total Time per 3500 Trials, s	Number of ICP Iterations	Threshold PtP	Delta (LCP)	Nsamples (Super4PCS)	Overlap (Super4PCS)
λ_r-ICP	27,109	300	0.85	0.04	-	-
RANSAC	29,026	-	0.85	0.04	-	-
Super 4PCS	31,291	-	0.85	0.04	120	0.75
Cluster	2113	-	0.85	0.04	-	-
PCL-GICP	301	300	-	-	-	-
PtP-ICP	77	300	0.85	-	-	-

shows the parameters of the tested algorithms for Scan_1 and Scan_2 with $\sigma =0.05$ and truncate_rate $=20\%$ (overlap_rate $=75\%$).

4.3.3. Impulse Noise with, truncate_rate $=10\%$ (overlap_rate ≈ 89%)

The parameters of impulse noise are described in Section 4.2. Figure 12 shows examples of pairs $\left(P,Q\right)$ with impulse noise and truncate_rate $=10\%$ (overlap_rate $\approx 89\%$). These pairs are the input to the coarse registration algorithms under consideration.

Figure 13a shows the convergence rate of good results, i.e., the frequency of results with $0\le dist\_m<0.2$ for Airplane. Figure 13b shows the convergence rate of good and medium results together, i.e., the frequency of results with $0\le dist\_m<0.6$ for Airplane.

Table 7. Parameters of the tested algorithms for Airplane with the impulse noise and truncate_rate $=10\%$ (overlap_rate $\approx 89\%$).

	Total Time per 3500 Trials, s	Number of ICP Iterations	Threshold PtP	Delta (LCP)	Nsamples (Super4PCS)	Overlap (Super4PCS)
λ_r-ICP	28,610	300	0.95	0.06	-	-
RANSAC	29,223	-	0.95	0.06	-	-
Super 4PCS	28,624	-	0.95	0.06	110	0.85
Cluster	6572	-	0.95	0.06	-	-
PCL-GICP	340	300	-	-	-	-
PtP-ICP	116	300	0.95	-	-	-

shows the parameters of the tested algorithms for Airplane with the impulse noise and truncate_rate $=10\%$ (overlap_rate $\approx 89\%$).

Figure 14a shows the convergence rate of good results, i.e., the frequency of results with $0\le dist\_m<0.2$ for Bunny. Figure 14b shows the convergence rate of good and medium results together, i.e., the frequency of results with $0\le dist\_m<0.6$ for Bunny.

Table 8. Parameters of the tested algorithms for Bunny with the impulse noise and truncate_rate $=10\%$ (overlap_rate $\approx 89\%$).

	Total Time per 3500 Trials, s	Number of ICP Iterations	Threshold PtP	Delta (LCP)	Nsamples (Super4PCS)	Overlap (Super4PCS)
λ_r-ICP	29,669	300	0.95	0.06	-	-
RANSAC	29,357	-	0.95	0.06	-	-
Super 4PCS	33,560	-	0.95	0.06	120	0.85
Cluster	4599	-	0.95	0.06	-	-
PCL-GICP	373	300	-	-	-	-
PtP-ICP	108	300	0.95	-	-	-

shows the parameters of the tested algorithms for Bunny with the impulse noise and truncate_rate $=10\%$ (overlap_rate $\approx 89\%$).

Figure 15a shows the convergence rate of good results, i.e., the frequency of results with $0\le dist\_m<0.2$ for the Scans. Figure 15b shows the convergence rate of good and medium results together, i.e., the frequency of results with $0\le dist\_m<0.6$ for the Scans.

Table 9. Parameters of the tested algorithms for Scan_1 and Scan_2 with the impulse noise and truncate_rate $=10\%$ (overlap_rate $\approx 89\%$).

	Total Time per 3500 Trials, s	Number of ICP Iterations	Threshold PtP	Delta (LCP)	Nsamples (Super4PCS)	Overlap (Super4PCS)
λ_r-ICP	31,676	300	0.95	0.06	-	-
RANSAC	29,248	-	0.95	0.06	-	-
Super 4PCS	29,723	-	0.95	0.06	120	0.85
Cluster	5668	-	0.95	0.06	-	-
PCL-GICP	333	300	-	-	-	-
PtP-ICP	118	300	0.95	-	-	-

shows the parameters of the tested algorithms for Scan_1 and Scan_2 with the impulse noise and truncate_rate $=10\%$ (overlap_rate $\approx 89\%$).

4.3.4. Noiseless, truncate_rate = 25% (overlap_rate ≈ 67%)

In addition, we carry out experiments with point clouds without noise and with a large truncation value (i.e., with sufficiently small common parts of clouds $P$ and $Q$). Figure 16 shows an example of point clouds$P$ (blue color) and $Q$ (red color) without noise and truncate_rate $=25\%$ (overlap_rate $\approx 67\%$). This pair is the input to the tested coarse registration algorithms.

Figure 17a shows the convergence rate of good results, i.e., the frequency of results with $0\le dist\_m<0.2$ for Airplane. Figure 17b shows the convergence rate of good and medium results together, i.e., the frequency of results with $0\le dist\_m<0.6$ for Airplane.

Table 10. Parameters of the tested algorithms for airplane without noise and truncate_rate $=25\%$ (overlap_rate $\approx 67\%$).

	Total Time per 3500 Trials, s	Number of ICP Iterations	Threshold PtP	Delta (LCP)	Nsamples (Super4PCS)	Overlap (Super4PCS)
λ_r-ICP	30,097	300	0.6	0.004	-	-
RANSAC	29,302	-	0.6	0.004	-	-
Super 4PCS	5881	-	0.6	0.004	1000	0.66
Cluster	1523	-	0.6	0.004	-	-
PCL-GICP	256	300	-	-	-	-
PtP-ICP	64	300	0.6	-	-	-

shows the parameters of the tested algorithms for airplane without noise and truncate_rate $=25\%$ (overlap_rate $\approx 67\%$).

5. Conclusions

In this paper, a new version of the variational problem of the ICP algorithm was considered. The introduced variational functional is a version of the $\lambda$-functional. The drawback of the previously described $\lambda$-ICP is that it is necessary to manually select the values of the parameters $\lambda$ for the given source and target point clouds. Here we described a method for the automatic calculating of these parameters. An approximate solution to the corresponding variational problem using the proposed functional was presented. An efficient $\lambda$_r-ICP algorithm for the coarse registration of incongruent point clouds was also proposed. The algorithm contains elements of both the ICP and RANSAC approaches. With the help of computer simulation, the performance of the proposed algorithms was compared with that of the state-of-the-art coarse registration algorithms such as Super4PCS and RANSAC. The proposed algorithm yields the best results among tested algorithms in the case of strong noise.