INTRODUCTION

For effectively solving problems that are performed by mobile robots, it is necessary to construct a 3D model (map) of the space surrounding the robot. An exact map allows the mobile robots to operate under complex conditions by use of only an onboard sensor. Creation of maps of the surrounding medium is called the problem of simultaneous localization and mapping (SLAM). The SLAM problem based on the use of graphs was proposed by Lu and Milios in 1997 [1]. The known approaches to registration of several point clouds consist of the pairwise registration stage and the global refinement stage. Pairwise registration includes attribute matching between pairs of point clouds and minimization of the sum of residuals over all such correspondences to estimate transformation parameters that establish the relative mutual arrangement for each pair of point clouds in the common coordinate system. Pairwise registration involves standard methods of point cloud alignment. The problem of point cloud registration in the three-dimensional space is a fundamental problem of computational geometry and computer vision.

In most cases, global refinement algorithms first find parameters of pairwise transformations by use of [2–14] and then uniformly redistribute errors using graph-based optimization [1, 15]. The graph-based SLAM problem involves the scan graph in which each scanning corresponds to a vertex and each edge corresponds to the spatial connection between pairs of nodes. Globally consistent registration of several point clouds by graph optimization was described in [16]. Further, for the uniform distribution of the error, least squares optimization is used [1]. In [17], the branch-and-bound strategy is used for the global solution of the objective function. The approach proposed in [18] uses surfaces and Bayesian filters for point cloud alignment. The main disadvantage of this method is its high computational cost. Other approaches to global refinement are based on general graph optimization [19], bundle adjustment [20], low-rank sparse decomposition [21], and kernel-based energy function [22].

In [23], the algorithm of global refinement of transformations for point clouds obtained by scanning of the urban environment was described. The algorithm described in [23] first performs global refinement for rotations by use of quaternions and then implements global refinement of parallel translations using the specificity of the urban environment. In the proposed work, for comparisons in computer simulation we use the global refinement algorithm for rotations by use of quaternions as it was presented in [23].

The global refinement algorithm described in the proposed paper first estimates rotations and then estimates parallel translations. For global refinement of rotations, the closed-form algorithm is used by means of matrices. For global refinement of parallel translations, the closed-form algorithm is used.

This paper is organized as follows. Section 1 presents the statement of the problem and describes algorithms of its solution. Section 2 presents results of computer simulation. Section 3 contains the conclusions.

1 GLOBAL REFINEMENT OF RESULTS OF PAIRWISE POINT CLOUD REGISTRATION

Let C0, C1, ..., Cs be the initial set of point clouds and (Rij, Tij), i, j = 0, 1, ..., s be results of pairwise cloud registration, where Cj is the reference cloud, Ci is the objective cloud, Rij ∈ SO(3) is the rotation matrix, and Tij ∈ R3 is the parallel translation vector.

Let (Ri, Ti), i = 0, 1, ..., s, denote transformation mapping of cloud Ci to the coordinate system of cloud C0. Interpreting point clouds and transformations as vertices and edges, respectively, we obtain a graph, an example of which is shown in Fig. 1. Global refinement of pairwise transformations is based on commutativity of cycles contained in the graph.

Fig. 1.
figure 1

Graph describing transformations between point clouds.

Full size image

1.1. Global Refinement of Rotations

The condition of commutativity of cycles with respect to rotations means that the following conditions are satisfied:

$${{R}_{i}} = {{R}_{j}}{{R}_{{ji}}},$$
(1)

where i, j = 0, 1, ..., s. Let us associate system of equations (1) with functional

$$J{\kern 1pt} '(R) = \sum\limits_{i = 0}^s {\sum\limits_{j = 0}^s {{{{\left\| {{{R}_{i}} - {{R}_{j}}{{R}_{{ji}}}} \right\|}}^{2}}} ,} $$
(2)

where

$$R = \left( {{{R}_{0}},...,{{R}_{s}}} \right).$$

Since the graph can contain not only edges, we replace functional J '(R) by functional J(R):

$$J(R) = \sum\limits_{i = 0}^s {\sum\limits_{j = 0}^s {{{w}_{{ji}}}{{{\left\| {{{R}_{i}} - {{R}_{j}}{{R}_{{ji}}}} \right\|}}^{2}}} ,} $$
(3)

where

$${{w}_{{ij}}} = \left\{ \begin{gathered} {\text{1}}{\text{, edge }}(j,i){\text{ is contained in the graph}} \hfill \\ {\text{0}}{\text{, edge }}(j,i){\text{ is not contained in the graph}}{\text{.}} \hfill \\ \end{gathered} \right.$$

By the solution of system (1) we mean the solution of the following variational problem:

$${{R}_{*}} = \mathop {\arg \min }\limits_R J(R),$$
(4)

under condition Ri ∈ SO(3), i = 0, ..., s. Let J(Rk), k = 0, 1, ..., s denote a functional containing all summands with occurrences of variable Rk in J(R). We represent J(Rk), k = 0, 1, ..., s, as a sum of functionals J1(Rk) and J2(Rk):

$$\begin{gathered} J({{R}_{k}}) = {{J}_{1}}({{R}_{k}}) + {{J}_{2}}({{R}_{k}}) \\ = \sum\limits_{j = 0}^s {{{w}_{{jk}}}{{{\left\| {{{R}_{k}} - {{R}_{j}}{{R}_{{jk}}}} \right\|}}^{2}}} + \sum\limits_{j = 0}^s {{{w}_{{kj}}}{{{\left\| {{{R}_{j}} - {{R}_{k}}{{R}_{{kj}}}} \right\|}}^{2}}} . \\ \end{gathered} $$
(5)

Gradients \(\nabla {{J}_{1}}({{R}_{k}})\) and \(\nabla {{J}_{2}}({{R}_{k}})\) are specified by formulas:

$$\nabla {{J}_{1}}({{R}_{k}}) = 2\sum\limits_{j = 0}^s {{{w}_{{jk}}}({{R}_{k}} - {{R}_{j}}{{R}_{{jk}}})} ,$$
(6)
$$\nabla {{J}_{2}}({{R}_{k}}) = 2\sum\limits_{j = 0}^s {{{w}_{{kj}}}({{R}_{k}}{{R}_{{kj}}}R_{{kj}}^{t} - {{R}_{j}}R_{{kj}}^{t})} .$$
(7)

Gradient \(\nabla J({{R}_{k}})\) takes form:

$$\begin{gathered} \nabla J({{R}_{k}}) = \nabla {{J}_{1}}({{R}_{k}}) + \nabla {{J}_{2}}({{R}_{k}}) \\ = 2{{R}_{k}}\sum\limits_{j = 0}^s {({{w}_{{jk}}}I + {{w}_{{kj}}}{{R}_{{kj}}}R_{{kj}}^{t})} \\ - \,\,2\sum\limits_{j = 0}^s {{{R}_{j}}({{w}_{{jk}}}{{R}_{{jk}}} + {{w}_{{kj}}}R_{{kj}}^{t})} . \\ \end{gathered} $$
(8)

Taking into account that R0 = I, we introduce the following notation:

$${{A}_{{jk}}} = \left\{ \begin{gathered} - ({{w}_{{jk}}}{{R}_{{jk}}} + {{w}_{{kj}}}R_{{kj}}^{t}),\,\,\,\,j \ne k \hfill \\ \sum\limits_{j = 0,j \ne k}^s {({{w}_{{jk}}}I + {{w}_{{kj}}}I),} \,\,\,\,j = k \hfill \\ \end{gathered} \right.,$$
(9)
$${{B}_{k}} = {{w}_{{0k}}}{{R}_{{0k}}} + {{w}_{{k0}}}R_{{k0}}^{t}.$$
(10)

Equality \(\nabla J({{R}_{k}})\) = 0 takes form:

$$\sum\limits_{i = 1}^s {{{A}_{{ik}}}{{R}_{i}}} = {{B}_{k}},$$
(11)

where k = 1, ..., s.

Vanishing of the gradient yields the following linear system of matrix equations:

$$\left( {\begin{array}{*{20}{c}} {{{A}_{{11}}}}&{{{A}_{{21}}}}&{...}&{{{A}_{{s1}}}} \\ {{{A}_{{12}}}}&{{{A}_{{22}}}}&{...}&{{{A}_{{s2}}}} \\ {...}&{...}&{...}&{...} \\ {{{A}_{{1s}}}}&{{{A}_{{2s}}}}&{...}&{{{A}_{{ss}}}} \end{array}} \right)\left( {\begin{array}{*{20}{c}} {{{R}_{1}}} \\ {{{R}_{2}}} \\ {...} \\ {{{R}_{s}}} \end{array}} \right) = \left( {\begin{array}{*{20}{c}} {{{B}_{1}}} \\ {{{B}_{2}}} \\ {...} \\ {{{B}_{s}}} \end{array}} \right).$$
(12)

Let us rewrite system of equations (12) numerically and calculate the affine solution of variational problem (4). We find projections \({{R}_{{1*}}}\), ..., \({{R}_{{s*}}}\) of obtained matrices R1, ..., Rs on SO(3):

$${{R}_{{k*}}} = \left\{ \begin{gathered} {{U}_{k}}V_{k}^{t},\,\,{\text{if}}\,\,\det ({{U}_{k}})\det ({{V}_{k}}) = 1 \hfill \\ {{U}_{k}}{\text{diag}}(1,1, - 1)V_{k}^{t},\,\,{\text{if}}\,\,\det ({{U}_{k}})\det ({{V}_{k}}) = - 1 \hfill \\ \end{gathered} \right.,$$
(13)

where Uk and Vk are elements of the SVD-representation of matrix Rk.

1.2. Global Refinement of Parallel Translations

The condition of commutativity for cycles of the graph shown in Fig. 1 defines the following system of equations:

$${{T}_{i}} = {{R}_{j}}{{T}_{{ji}}} + {{T}_{j}}.$$
(14)

Let us associate system of equations (14) functional J(T):

$$J(T) = \sum\limits_{i = 0}^s {\sum\limits_{j = 0}^s {{{w}_{{ji}}}{{{\left\| {{{T}_{i}} - {{T}_{j}} - {{R}_{j}}{{T}_{{ji}}}} \right\|}}^{2}}} ,} $$
(15)

where

$${{w}_{{ij}}} = \left\{ \begin{gathered} {\text{1}}{\text{, edge }}(j,i){\text{ is contained in the graph}} \hfill \\ {\text{0}}{\text{, edge }}(j,i){\text{ is not contained in the graph}} \hfill \\ \end{gathered} \right..$$

By the solution of system (14) we mean the solution of the following variational problem:

$${{T}_{*}} = \mathop {\arg \min }\limits_T J(T),$$
(16)

where T = (T0, T1, ..., Ts). The gradient \(\nabla J(T)\) with respect to Tk, k = 0, 1, ..., s is calculated as follows:

$$\begin{gathered} \nabla J(T) = 2\sum\limits_{i = 0}^s {({{w}_{{ik}}} + {{w}_{{ki}}}){{T}_{k}}} - {{w}_{{ik}}}{{R}_{i}}{{T}_{{ik}}} \\ + \,\,{{w}_{{ki}}}{{R}_{k}}{{T}_{{ki}}} - ({{w}_{{ik}}} + {{w}_{{ki}}}){{T}_{i}}. \\ \end{gathered} $$
(17)

Vanishing of the gradient with respect to Tk, k = 0, 1, ..., s yields the following equation:

$$\begin{gathered} \sum\limits_{i = 0,i \ne k}^s { - ({{w}_{{ik}}} + {{w}_{{ki}}}){{T}_{i}}} + \sum\limits_{i = 0,i \ne k}^s {({{w}_{{ik}}} + {{w}_{{ki}}}){{T}_{k}}} \\ = - \sum\limits_{i = 0}^s { - {{w}_{{ik}}}{{R}_{i}}{{T}_{{ik}}} + {{w}_{{ki}}}{{R}_{k}}{{T}_{{ki}}}} . \\ \end{gathered} $$
(18)

Let Bk, k = 0, 1, ..., s, denote the following expression:

$${{B}_{k}} = - \sum\limits_{i = 0}^s { - {{w}_{{ik}}}{{R}_{i}}{{T}_{{ik}}} + {{w}_{{ki}}}{{R}_{k}}{{T}_{{ki}}}} .$$
(19)

Variational problem (16) is reduced to solving a system of linear equations in vectors:

$$\left( {\begin{array}{*{20}{c}} { - ({{w}_{{10}}} + {{w}_{{01}}})}&{ - ({{w}_{{10}}} + {{w}_{{01}}})}&{...}&{ - ({{w}_{{s0}}} + {{w}_{{0s}}})} \\ {\sum\limits_{i = 0,i \ne 1}^s {({{w}_{{i1}}} + {{w}_{{1i}}})} }&{ - ({{w}_{{21}}} + {{w}_{{12}}})}&{...}&{ - ({{w}_{{s1}}} + {{w}_{{1s}}})} \\ { - ({{w}_{{12}}} + {{w}_{{21}}})}&{\sum\limits_{i = 0,i \ne 2}^s {({{w}_{{i2}}} + {{w}_{{2i}}})} }&{...}&{ - ({{w}_{{s2}}} + {{w}_{{2s}}})} \\ {...}&{...}&{...}&{...} \\ { - ({{w}_{{1s}}} + {{w}_{{s1}}})}&{ - ({{w}_{{2s}}} + {{w}_{{s2}}})}&{...}&{\sum\limits_{i = 0,i \ne s}^s {({{w}_{{is}}} + {{w}_{{si}}})} } \end{array}} \right)\left( {\begin{array}{*{20}{c}} {{{T}_{1}}} \\ {{{T}_{2}}} \\ {...} \\ {{{T}_{s}}} \end{array}} \right) = \left( {\begin{array}{*{20}{c}} {{{B}_{0}}} \\ {{{B}_{1}}} \\ {{{B}_{2}}} \\ {...} \\ {{{B}_{s}}} \end{array}} \right).$$
(20)

Let M denote the matrix in Eq. (20). Then, solving the variational problem is reduced to solving three systems of numerical equations:

$$M\left( {\begin{array}{*{20}{c}} {{{T}_{{1i}}}} \\ {{{T}_{{2i}}}} \\ {...} \\ {{{T}_{{si}}}} \end{array}} \right) = \left( {\begin{array}{*{20}{c}} {{{B}_{{0i}}}} \\ {{{B}_{{1i}}}} \\ {{{B}_{{2i}}}} \\ {...} \\ {{{B}_{{si}}}} \end{array}} \right),$$
(21)

where i = 1, 2, 3 is the number of the vector component.

2 COMPUTER SIMULATION

We denote the global refinement algorithm proposed in this paper as GR. Let us describe other algorithms under consideration.

For cloud Ck, k = 1, ..., s, we consider transformation Mk(k – 1) equal to the result of projection of matrix 1/2(R(k – 1)k + Rk(k – 1)) on SO(3). Let Tk, k = 1, ..., s, denote the parallel translation vector equal to vector 1/2(T(k – 1)k + Tk(k – 1)). The transformation ((M1M2  ... Mk), (T1 + T2 + ... + Tk)) maps cloud Ck to the coordinate system of cloud C0. We denote this global refinement algorithm as R_ICP.

In [23], the global refinement algorithm was described individually for rotations and parallel translations. The algorithm for rotations is based on using quaternions. Let GR_Q denote the global refinement algorithm that uses the rotation refinement algorithm described in [23] and the algorithm for parallel translation described in this paper.

The computer experiments were carried out with point clouds from the San Francisco Apollo-SouthBay Dataset [24]. Each cloud in the database contains approximately 100 000 points. We use cloud subsampling to approximately 10 000 points per cloud. The clouds in the dataset were obtained using a lidar mounted on a vehicle. The vehicle moved and the sensor scanned the ambient medium with a certain frequency. The obtained data set consists of a sequence of point clouds. In our experiments, the point clouds are taken from the dataset with a step of 4, i.e., for example, point clouds nos. 1, 5, 9, ... are considered.

The database contains information about transformation Mk mapping each cloud Ck to a certain global coordinate system. Matrix Mk has dimensions of 4 by 4 and specifies a rigid transformation in homogeneous coordinates. The transformation mapping cloud Ci to the coordinate system of cloud CJ is specified by matrix Mji_true = (Mj)–1Mi.

The experiments are organized as follows. Point cloud number k is fixed in the database. The point clouds with numbers k, k + 4, k + 8, ...k + 4 × 99 are considered. For all successive pairs of clouds (k + 4i, k + 4(i + 1)), i = 0, ..., 98, we find transformation (Rk + 4i, k + 4(i + 1), Tk + 4i, k + 4(i + 1)) mapping cloud Ck + 4(i + 1) to the coordinate system of cloud Ck + 4i and transformation (Rk + 4(i + 1), k + 4i, Tk + 4(i + 1), k + 4i) mapping cloud Ck + 4i to the coordinate system of cloud Ck + 4(i + 1) using the point-to-point ICP algorithm. Note that before the use of the ICP algorithm we apply to the reference cloud the coarse alignment algorithm, which means applying to this cloud the transformation obtained for the previous pair of clouds. The transforms relating clouds, the numbers of which differ by more than 4, are calculated using the superposition of intermediate transforms with respect to R and T, respectively.

In this paper, the following quality parameters of global refining algorithms are used. Parameter last_R = \({{\left\| {{{R}_{{{\text{(first}}{\text{, last)}\_\text{true}}}}} - {{R}_{{{\text{(first}}{\text{, last)}\_\text{est}}}}}} \right\|}_{{{{L}_{2}}}}}\), where R(first, last)_true and R(first, last)_est are the true and estimated transforms mapping the last considered cloud to the coordinate system of the first cloud, shows the global error of 3D scene reconstruction. Parameter last_T is defined similarly. Parameters avg_R and avg_T show the average errors with respect to R and T, respectively. Parameters max_R and max_T show the maximum errors. Figure 2 and Tables 1 and 2 show the operation accuracy of the GR_ICP, GR, and GR_Q algorithms for a series of clouds with the origin in cloud no. 1.

Fig. 2.
figure 2

Result of 3D scene formation by different algorithms: (a) GR_ICP (dark gray color); (b) GR (light gray color); and (c) GR_Q (gray color). The correctly formed 3D scene is marked by black color.

Full size image
Table 1. Accuracy of calculation of rotations by the GR_ICP, GR, and GR_Q algorithms
Full size table
Table 2. Accuracy of calculation of parallel translations by the GR_ICP, GR, and GR_Q algorithms
Full size table

Figure 3 and Tables 3 and 4 show the operation accuracy of the GR_ICP, GR, and GR_Q algorithms for a series of clouds with the origin in cloud no. 501.

Fig. 3.
figure 3

Result of 3D scene formation by different algorithms: (a) GR_ICP (dark gray color); (b) GR (light gray color); and (c) GR_Q (gray color). The correctly formed 3D scene is marked by black color.

Full size image
Table 3. Accuracy of calculation of rotations by the GR_ICP, GR, and GR_Q algorithms
Full size table
Table 4. Accuracy of calculation of parallel translations by the GR_ICP, GR, and GR_Q algorithms
Full size table

Figure 4 and Tables 5 and 6 show the operation accuracy of the GR_ICP, GR, and GR_Q algorithms for a series of clouds with the origin in cloud no. 1001. Figure 5 and Tables 7 and 8 show the operation accuracy of the GR_ICP, GR, and GR_Q algorithms for a series of clouds with the origin in cloud no. 1501.

Fig. 4.
figure 4

Result of 3D scene formation by different algorithms: (a) GR_ICP (dark gray color); (b) GR (light gray color); and (c) GR_Q (gray color). The correctly formed 3D scene is marked by black color.

Full size image
Table 5. Accuracy of calculation of rotations by the GR_ICP, GR, and GR_Q algorithms
Full size table
Table 6. Accuracy of calculation of parallel translations by the GR_ICP, GR, and GR_Q algorithms
Full size table
Fig. 5.
figure 5

Result of 3D scene formation by different algorithms: (a) GR_ICP (dark gray color); (b) GR (light gray color); and (c) GR_Q (gray color). The correctly formed 3D scene is marked by black color.

Full size image
Table 7. Accuracy of calculation of rotations by the GR_ICP, GR, and GR_Q algorithms
Full size table
Table 8. Accuracy of calculation of parallel translations by the GR_ICP, GR, and GR_Q algorithms
Full size table

3 CONCLUSIONS

In this paper, we use the global refining algorithm GR for constructing a 3D scene from a set of point clouds. The algorithm is compared with other possible methods of solving the problem of global refining of transformations. The accuracy of the algorithms is estimated using quality criteria that correspond to visual perception of the result accuracy. Computer simulation demonstrates efficiency of the proposed algorithm.