An improved iterative closest point algorithm based on the particle filter and K-means clustering for fine model matching

Abstract

The rigid matching of two geometric clouds is vital in the computer vision and its intelligent applications, such as computational geometry, robotics, shape modelling, surface reconstruction and mapping, and many other fields. The variants of the iterative closest point algorithm were employed as the most noticeable matching algorithm. In traditional ICP algorithms applications for symmetrical geometry matching, the initial uncertainty and the multiple local minima of the distance function adversely affect the alignment process, which leads to weak performance, such as incorrect correspondence, narrow convergence region, and instability. In this study, the novel algorithm fused the ICP algorithm, particle filter and K-means clustering to correctly estimate the transformation ICP parameters. Further guide to initial values of parameters and their covariance obtained by k-means clustering. Then, a particle filter was implemented to estimate accurate values and perform global optimization. In the introduced PF-ICP algorithm, the alignment parameters: rotation angles, scale factor, and translation, were defined as particles elements optimized using a sequential importance resampling (SIR) particle filter. The proposed algorithm was implemented on a medical robot FPGA board and applied to “three symmetrical models” and “noisy and poor datasets.” The calculated variances and estimated parameters were compared with four modified ICP methods. The results show a significantly increasing accuracy and convergence region with an acceptable speed for the practical conditions.

1 Introduction

Finding a rigid transformation to match a dataset (as measured/scanned data) with a model point set or map is a fundamental problem in computer vision, computational geometry, robotics, shape modelling/analysis, surface reconstruction and mapping, augmented reality and many other fields. 3D registration is based on variants of the iterative closest point (ICP) algorithm. ICP was presented by Besl and McKay in 1992 [1]. In addition, the iterative closest contour point algorithm (ICCP) was eventuated from the ICP method, proposed by Kamgar-Parsi 1999 [2], to solve the gravity navigation problem.

The ICP starts with two meshes as model set and dataset with an initial setting for their relatively rigid-body transformation that repeatedly refines the transformation by obtaining pairs of corresponding points on the model and data. An incremental transformation is found, such that the point-to-point distance is minimized to a local minimum. Owing to the ICP algorithm that can be made efficient and reliable, it has widely considered and adopted in different applications. So, the researchers have focused on addressing the shortcomings and generated variant methods. Variant improvement of the ICP algorithm classifies aspects, such as convergence speed, stability, tolerance of noise or outlier, and maximum initial misalignment. Performance developments can be achieved by modifying the algorithm in three stages:

• 1. Selecting samples of point sets in one or both meshes that may have imperfection data;

• 2. Corresponding the points to samples in the model, weighting pairs and rejecting outliers;

• 3. Defining an error metric due to the point pairs, estimating the rigid transformer, and minimizing the ICP covariance.

For each of these stages, there were different improvements in the performance and reliability of the algorithm. The first issue with ICP is the selection of an input sample points. The sample selection and matching sequence length affect the algorithm’s performance. In some studies, all available points are used [1], while in others, some points are used [3] or uniform random sampling is used at each iteration [4]. Xiao et al. generated a matching sequence method based on the coding principle for an ICCP geomagnetic matching algorithm [5]. They presented an adaptive approach for choosing appropriate matching lengths and matching points in a highly dynamic environment. Also, about the analysis of the measurement data, Zhao et al. studied the geomagnetic map data analysis based on random error modelling and filtering [6].

The next stage is related to assigning weights to pairs or eliminating outlier pairs. This stage may significantly affect minimization and solve several heuristics to prune or reweight the correspondences. Besl and Kamgar used the closest point in another mesh as the corresponding point for each sample. The other ICP variants employ the closest compatible point, normal shooting, and normal shooting to a compatible point [7, 8], and their computation is accelerated using a k-d tree. Also, the data point projection to the model mesh from the model mesh point of view can be used. Figure 1a shows that the closest point matching algorithm can perform wrong pairing in the presence of noise and outliers, then it converges slowly. But Fig. 1b represents that the “projection” matching methods are less sensitive to the presence of noise.

Fig. 1

The pairing of the two cloud data with a closest point matching algorithm and b projection matching method, in presence of noise and outlier data

Bouaziz et al. [9] proposed a formulation of the ICP algorithm that avoids difficulties related to dealing with outliers and incomplete data using sparsity-inducing norm optimization. Guo et al. [10] introduced an adaptive weight vector that eliminated the negative effect of pairs with the most significant registration errors and the retained pairs assigned suitable weights to suppress the noise and outliers.

The final step of the ICP algorithm, is to minimize the error metric cost function and the optimization approach to minimize the distance error. For example, Besl and McKay’s original work minimized the distance between two correspondence points. In contrast, the second method minimizes the distance between a data point and the corresponding model point plane, which is perpendicular to its normal plane. Generally, distance point-to-plane minimization converges faster and with greater accuracy, but not necessarily a better region of convergence [11].

It has also been shown that with a trimmed square distance function, more computational capability, and auxiliary data, more convergence speed and stability could be achieved [12] or by cancelling the greater distance pairs [13]. In [14], a new symmetric distance metric presented to achieve a better convergence region and a higher speed. The research in [15] extended the point-to-hyperplane distance function and combined various position estimation algorithms to minimize different metric errors.

In addition, recent studies have attempted to accelerate the traditional ICP method. The Levenberg–Marquardt algorithm to minimize registration was employed to accelerate and yield a basin of convergence that directly minimized the energy function as a nonlinear optimization. In [16, 17], Anderson’s acceleration was employed to speed up convergence. In addition, in considerable research, an established numerical technique based on Welsch’s function was presented, which efficiently minimizes a simple quadratic function [18]. Yue et al. [19] completed point cloud registration through key point extraction, local point-pair feature matching, and coarse and fine registration. They verified the performance of the method by evaluating its RMSE and MAE. However, the uncertainty of the feature fitting affects the estimated parameters accuracy [20, 21].

Owing to the strong nonlinearity of the objective function of matching in some terrain types, linearization methods, such as the extended Kalman filter, are often not well suited for this problem, and the effectiveness of the probabilistic approach is slightly better [22].

Probabilistic matching algorithms obtain higher performance and more robust and global results with a higher computational cost. They are often based on techniques such as histograms, sequential Monte Carlo, and particle filters.

The key contribution of this work is the novel fusion of the particle filter and ICP algorithm to estimate registration parameters and to overcome the weak performance of traditional ICP method in the symmetrical geometry matching related to local minima of the cost function and sensor uncertainty.

The proposed approach defined a sequential Monte Carlo process that enabled importance sampling of each of the registration parameters according to the collected error statistics. The PF-ICP method implemented the particle filter algorithm to estimate the global optimization and accurate values of the ICP transformation parameters. The presented PF-ICP algorithm employed the k-means clustering method to further guide the parameter initialization step and to obtain the ICP parameters covariance.

This article sections included: related works, required fundamentals (A. the ICP algorithm, B. the covariance of ICP, and C. the SIR particle filter), the introduced PF-ICP method, with four parts: A. definitions, B. initialization, C. covariance estimation, and D. particle filter estimation. Finally, the results of the two case studies for different conditions were explained and concluded.

2 Related works

Combesa and Primab [16] presented a registration method for nonlinear 3D point sets. They considered the points of the dataset as draws of the Gaussian mixture model in which the centres are the transformed points of the model set. In the next step, a maximum likelihood estimation of the model parameters is performed, using the expectation–maximization (EM) algorithm. Xiao et al. [23] adopted a probability data association (PDA) algorithm, based on considering constraints of a vehicle’s kinematics, to solve the problem of geomagnetic matching in an interference environment. Rowekamper et al. [24] analysed the accuracy of an integrated laser-based positioning system for mobile platforms. Also, Censi [25] and Maken et al. [26] introduced an algorithm to estimate the uncertainty of the ICP transformation parameters and to align two point clouds. Maken used a gradient-based optimization of the distance cost function to develop a Stein variational inference framework for multi-modal distribution models.

Because modern computer computational power warrants performance vs. complexity, we improved the ICP matching algorithm with a particle filter for the complexity model condition. In this paper, the introduced PF-ICP algorithm determined the transformation parameters (rotation, translation, and scaling) by k-means clustering initialization and sequential importance resampling (SIR) particle filter. The developed PF-ICP algorithm could answer very reliable and comprehensive for local minimums, caused by symmetric spatial and measurement noise.

3 Fundamentals

The fundamental algorithms required for the implementation of PF-ICP method are as follows:

3.1 The iterative closest point algorithm

The iterative closest point (ICP), or the iterative correspondence point algorithm, is an alignment method in which the two point sets are registered based on the function of either point-to-point or point-to-plane distance. These two sets may be represented as point sets, triangle sets, line segment sets, parametric surfaces, implicit surfaces, implicit curves, and parametric curves [27,28,29,30,31]. The most common method is based on the iterative closest point algorithm introduced by [1]. In this method, the points in one cloud and their nearest points in another cloud are repeatedly paired as the corresponding points. Then, an incremental transformation that minimizes the distance of the point pair is calculated.

Let the model shape be represented as a set of points, $D = \{ d_{1} ,d_{2} , \ldots ,d_{ND} \}$, and N_D be the number of points in the model database, and the scanned or scene shape be considered as a set of points, $M = \{ m_{1} ,m_{2} , \ldots m_{NM} \}$. If the defined correspondents are the closest points, the relative transformation can be obtained as follows: For every point m_i, in the scanned shape M = {m_i}, i = 1,..N_M, we seek the closest point model D based on using the Euclidean distance. For a given scanned point m_i and the model points set D, the Euclidean distance between m_i and D can be found as:

$$ d(m_{i} ,D) = \mathop {\min }\limits_{{k = 1...N_{D} }} d(m_{i} ,d_{k} ) = \mathop {\min }\limits_{{k = 1...N_{D} }} \left\| {m_{i} - d_{k} } \right\| $$

(1)

The closest point of the model set, $d_{j} \in D,$ satisfies the equality:

$$ d(m_{i} ,d_{j} ) = d(m_{i} ,D);j = \mathop {\arg \min }\limits_{{k = 1..N_{D} }} d(m_{i} ,d_{k} ); $$

Also, the closest point in the model set D that yields the minimum distance will be denoted by y_i.

$$ d(m_{i} ,y_{i} ) = d(m_{i} ,D) $$

where the m_i corresponds to y_i. Let Y define the resulting set of closest points and C (.) be the closest point operator.

$$ Y = C(M,D) $$

(2)

$$ Y = \{ y_{i} \}_{i = 1}^{{N_{M} }} ,M = \{ m_{i} \}_{i = 1}^{{N_{M} }} ,D = \{ d_{i} \}_{i = 1}^{{N_{D} }} ,Y \subseteq D $$

The optimal registration parameters (scaling, rotation, and translation) can be found that match the scanned points M to the closest model points Y by minimizing the total error.

$$ E = \sum\limits_{i = 1}^{{N_{M} }} {\left\| {e_{i} } \right\|^{2} } ,e_{i} = \{ y_{i} - [sR(m_{i} ) + T]\} $$

(3)

The ICP algorithm outline is as follows:

• 1. Initialize registration parameters:

  rotation (R,$R \in \{ \left. {{\mathbb{R}}^{3 \times 3} } \right|R^{T} R = I,\det (R) = + 1\}$),

  translation (T = 0, $T \in {\mathbb{R}}^{3}$), and scaling (s = 1, $s \in {\mathbb{R}}$) and registration error; Error = ∞.

• 2. For each scanned point set, find the corresponding closest point in the model point set.

• 3. Calculate registration parameters for given point correspondents from step 2.

• 4. Apply the alignment to the scanned point set.

• 5. Calculate the registration error between the currently aligned scanned set and the model set.

• 6. If error > threshold, return to step 2; otherwise, return with new scanned points set (Fig. 2).

Fig. 2

The iterative closest point flowchart

Algorithm 1:

ICP_Matching

Four major estimating algorithms for 3D rigid-body transformations were reviewed and compared by [32]. These algorithms are used when there is an established correspondence between data cloud M and D. The optimal registration is obtained from linear algebra singular value decomposition (SVD) method. First, the scan point sets $\left\{ {d_{i} } \right\}$ and the model point sets $\left\{ {m_{i} } \right\}$ should have the same centroid in the rotation matrix calculation. The least-squares equation for the centroid datasets is:

$$ \Sigma^{2} = \sum\limits_{i = 1}^{N} {\left\| {d_{ci} - \widehat{{\text{R}}}m_{ci} } \right\|}^{2} = \sum\limits_{i = 1}^{N} ( d_{ci}^{T} d_{ci} + m_{ci}^{T} m_{ci} - 2d_{ci}^{T} \widehat{{\text{R}}}m_{ci} ) $$

(4)

The minimum of this equation occurs when maximizing the $Trace(\hat{R}^{.} C)$, where C is a correlation matrix defined by:

$$ C = \sum\limits_{i = 1}^{N} {[m_{i} } d_{i}^{T} ] - \mu_{m} \mu_{d}^{T} = \sum\limits_{i = 1}^{N} {m_{ci} } d_{ci}^{T} $$

(5)

If the SVD of C is given by $C \, = \, U\Lambda V^{T}$, R must be $\, \widehat{R} = \, VU^{T}$, for trace maximization. In addition, the optimal translation was computed from the alignment of the D centroids with the rotated centroid of M. The algorithm-SVD matching is presented in the following:

Algorithm 2:

SVD_Matching

Therefore, for the case of established correspondence between data cloud M and D, the above SVD matching method would be used for calculating the ICP parameters. At the initialization step of the presented approach, a correspondence relationship is considered between the centroids of two datasets clusters and will be executed the SVD matching algorithm [33].

When the probability distribution function of the ICP parameters is necessary, the covariance is often obtained based on the calculation of the Hessian Matrix [24, 34,35,36]. In this method, the Hessian and covariance matrices are obtained from:

$$ H = \frac{{dj^{2} \left( {\hat{X}} \right)}}{{d\hat{X}^{2} }} $$

(6)

$$ {\text{cov}} \left( {\hat{x}} \right) \cong \left( {\frac{{\partial^{2} }}{{\partial x^{2} }}J} \right)^{ - 1} \frac{{\partial^{2} J^{T} }}{\partial z\partial x},{\text{cov}} \left( z \right) \cong \frac{{\partial^{2} J^{T} }}{\partial z\partial x}\left( {\frac{{\partial^{2} }}{{\partial x^{2} }}J} \right)^{ - 1} $$

where x is the model parameter, z is the observation matrix, and J (z, x) is the error function. The Hessian matrix can be obtained once the ICP algorithm determines the minimum [35]. For more details, we can see [37, 38].

3.2 The particle filter—sequential Monte Carlo estimation

The particle filter as a sequential state estimation method was presented in 1993 for Bayesian nonlinear filtering problems. The particle filter is also known as bootstrap filtering, sequential Monte Carlo algorithm [39]. To date, the particle filter is developing and evolving; thus, it has become a relatively mature theory. A summary of the particle filter algorithm framework is presented herein, and the sequential importance resampling (SIR) particle filter algorithm is defined [40, 41].

The particle filter steps consist of:

• 1. Select a proposal distribution $q\left( {x_{k + 1} |x_{1:k} ,y_{k + 1} } \right)$, resampling method, and the number of particles N.

• 2. Generate and initiate.

  $x_{1}^{i} \sim p_{x0} ,i = 1,....,N$ and $w_{1|0}^{i} = 1/N$.

• 3. Measurement update: For:

  $i = 1,...,N,w_{k|k}^{i} = \frac{1}{{c_{k} }}w_{k|k - 1}^{i} p\left( {y_{k} |x_{k}^{i} } \right)$

  That the normalization weight coefficient $c_{k}$ is calculated by:

  $$ c_{k} = \sum\limits_{i = 1}^{N} {w_{k|k - 1}^{i} p\left( {y_{k} |x_{k}^{i} } \right)} $$

  (7)

• (4) Estimation: The filtering probability is obtained by:

  $$ \widehat{p}\left( {x_{1:k} |y_{1:k} } \right) = \sum\limits_{i = 1}^{N} {w_{k|k}^{i} } \delta \left( {x_{1:k} - x_{1:k}^{i} } \right) $$

  (8)

  That the mean could be calculated by:

  $$ \widehat{x}_{1:k} \approx \sum\limits_{i = 1}^{N} {w_{k|k}^{i} x_{1:k}^{i} .} $$

  (9)

• (5) Resampling:

  At each arbitrary time, obtain N samples with supersedence from the set $\{ x_{1:k}^{i} \}_{i = 1}^{N}$ with probability, $w_{k|k}^{i} = 1/N.$

• (6) Time update:

  Produce predictions with the proposal distribution and correct for the importance weight.

  $$ \begin{aligned} x_{k + 1}^{i} & \sim q\left( {x_{k + 1} |x_{k}^{i} ,y_{k + 1} } \right); \\ w_{k + 1|k}^{i} & = w_{k|k}^{i} \frac{{p\left( {x_{k + 1}^{i} |x_{k}^{i} } \right)}}{{q\left( {x_{k + 1}^{i} |x_{k}^{i} ,y_{k + 1} } \right)}}; \\ \end{aligned} $$

  (10)

Also, the SIR particle filter algorithm is implemented as follows:

As shown in Fig. 3, the nonlinear filter infers states from observations in a likelihood or Bayesian framework. The state posterior distribution is computed or approximated using all available observations at that iteration. Then, the sample quantity and weight of particles are assigned with the same weight value and more quantity for more likelihood estimation.

Fig. 3

Sequential importance resampling particle propagation

Algorithm 3:

SIR_Particle_Filter

3.3 The proposed particle filter–ICP algorithm

The proposed algorithm establishes a fine matching of two geometric data points with symmetric spatial distribution. Its advantage is the reliability of reaching the correct matching and the accuracy of the final result for a “difficult” geometry with a symmetric spatial distribution. The main idea of the algorithm is to represent the estimation belief by a probability distribution over the possible region for the ICP transformation parameters and update the estimation belief whenever each iteration is performed [40].

First, the algorithm estimates the initial value and uncertainty as a Gaussian distribution with a mean and variance. The initial values are required for the ICP algorithm. The probability distribution associated with data uncertainty is required for the particle filter. By applying sensor fusion techniques in nonlinear conditions, such as the particle filtering methods, almost a maximum likelihood estimation is obtained by probabilistic filtering. These probabilities are typically assumed to be normally distributed, so only the mean and covariance matrix are calculated. The main methods introduced for ICP covariance calculation are based on the Hessian matrix method. The researches such as [35, 36, 38, 40] show that the ICP covariance depends not only on sensor noise characteristics but also on the geometry of the environment. Here, a statistical method that approximates the mean and variance of the ICP matching parameters is introduced.

In the second step of the proposed algorithm, a particle filter is defined to estimate the transformation ICP parameters (rotation angles, translation, and scaling) as particle elements. According to the presented PF-ICP algorithm, four strategic concepts and significant achievements were observed.

• (1) In the scanning process, the sensors always impose errors, such as noise, scale factor, bias, and drift, on the measurement data. These errors correspond to and are consistent with the ICP transfer parameters, and the effects of these errors on the parameters can be modelled in the same way.

• (2) Owing to the linear transformation in the regression process, the relative position constraints of the measurement points were held constant. The main advantage of the ICP algorithm is that this concept is also used here.

• (3) In linear equation solving related to determining ICP transformation parameters and their optimization, the existence of outliers measurement data and overlap model data does not pose a serious constraint.

• (4) A valuable property of the PF-ICP algorithm is that it can universally approximate the state probability distributions and escape from the local minimum. The particle filter algorithm arranges its computational particles in global regions with high probability, where things really matter.

Based on the aforementioned advantages, this method has high reliability and a very convincing performance in the presence of uncertainty and nonlinearity, which will be shown in the next sections.

3.4 Definitions

Consider two corresponding point sets $M = \left\{ {m_{i} ,i = 1..N_{M} } \right\}$ for the measurement point cloud and $D = \left\{ {d_{i} ,i = 1..N_{D} } \right\}$ database point cloud (could consider $N_{M} = \, N_{D} = \, N$) and the rigid transformation s, R, T between them, such that they are related by:

$$ d_{i} = {\text{sR}}m_{i} + {\text{T + V}}_{{\text{i}}} $$

(11)

where R is known as a standard 3 × 3 rotation matrix, T contains a 3D translation vector, Vi is noise or uncertainty and the optimal parameters of transformation $\left[ {\widehat{s},\hat{R},\hat{T} \, } \right]$ that match the set $\left\{ {m_{i} } \right\} \, onto \, \left\{ {d_{i} } \right\}$ typically are related to minimizing the least squares error function typically given by:

$$ \Sigma^{2} = \sum\limits_{i = 1}^{N} {\left\| {d_{i} { - }\widehat{{\text{s}}}*\widehat{{\text{R}}}*m_{i} { - }\widehat{{\text{T}}}} \right\|} $$

(12)

3.5 Clustering and parameters initialization

The initialization is very important for the variations of ICP algorithms. The more correctly the ICP parameter initialization is performed, the fewer problems related to local minima arise. In order to reduce the mathematical calculation and increase the speed and integrity, the data clouds are first classified by the k-means clustering method. In the PF-ICP initialization, the k-means algorithm performs two-data cloud clustering. The initial values of the transformation parameters are then obtained by matching the centroids of the two data clusters. K numbers of sequential partitions or clusters are expressed as follows:

$$ \begin{aligned} Clustering(M) & = \{ M_{1} ,M_{2} , \ldots M_{K} \} ;Size(M_{K} ) = {\text{ n\_}}M_{K} ; \\ Clustering(D) & = \{ D_{1} ,D_{2} , \ldots D_{K} \} ;Size(D_{K} ) = {\text{ n\_D}}_{K} ; \\ \end{aligned} $$

(13)

Because of the scanning method, it is assumed that there is a generally (and not exactly) corresponding relation between the two sets, which leads to an established correspondence relationship between the centroids of the clustered subsets and:

$$ D_{i} = \, Pair\left( { \, M_{i} } \right) $$

Based on the total number of cloud points, K was selected such that the number of cluster members could be between 10 and 100. The centroids of these clusters are employed for initializing the ICP transformation parameters. To initialize the ICP transformation parameters, the centroids of all measurement cloud M clusters and database cloud D clusters were calculated.

$$ \begin{aligned} U_{M} & = \{ u_{M1} ,u_{M2} ,u_{M3} ,...,u_{MK} \} ,{\text{that }}u_{Mi} = Centroid(M_{i} ) \\ U_{D} & = \{ u_{D1} ,u_{D2} ,u_{D3} ,...,u_{DK} \} ,{\text{that }}u_{Di} = Centroid(D_{i} ) \\ \end{aligned} $$

(14)

Algorithm 4:

Clustering

The SVD matching algorithm calculates the initial estimate of the transformation parameters with centroids U_M and U_D inputs.

$$ [s_{{\text{int}}} ,R_{{\text{int}}} ,T_{{\text{int}}} ] = SVD\_matching(U_{D} ,U_{M} ); $$

(15)

The results of R_int (initial rotation), T_int (initial translate), and S_int (initial scale factor) are used as the initial values of PF-ICP transformation parameters. In addition, the clustering could be executed several times, and the average of the centroids used for the initialization. The members of the generated clusters are employed for the estimation of the ICP parameters covariance and the calculation of the particle filter weights, which are presented in the following sections.

3.6 The PF-ICP parameters covariance estimation

As previously mentioned, to apply sensor fusion techniques, based on maximum likelihood estimation, the probability distribution of the error related to the applied data is required. Various techniques have been proposed to obtain this covariance, which are often based on the calculation of the Hessian matrix [25, 40]. These techniques require numerous mathematical calculations. To reduce these calculations, the k-means clustering and ICP matching were used to approximate the ICP covariance. The results were compared with those obtained using Censi’s method.

For this purpose,$n_{C}$ subsets of cloud points M are created such that their first member is randomly selected from M₁, the first cluster of cloud points M, the second member of them is selected from M₂, the second cluster of cloud points M… and their K-th member is selected from M_k, the K-th cluster of cloud points M. Thus, the $n_{C}$ subsets of M with K members are built. All members of the subsets are selected from a uniform stochastic distribution. This can be rewritten as follows:

$$ \begin{aligned} {\mathbf{m}}_{1} & = \, \{ \, m_{1,i} |m_{1,i} \in \, M_{i} , \, i:1..K\} \\ {\mathbf{m}}_{nC} & = \{ \, m_{nC,i} | \, m_{nC,i} \in \, M_{i} , \, i:1..K\} \\ \end{aligned} $$

(16)

Then, for each subset m_i and model dataset D, the ICP matching algorithm is executed to determine the ICP transformation parameters, the values of (R rotation matrix, T translate vector, S Scalar scale factor).

$$ \left( { \, R_{i} , \, T_{i} , \, S_{i} ,dist_{i} } \right) \, = \, ICP\left( {{\mathbf{m}}_{i} , \, D \, } \right) \, , \text{for}\, i \, :1...n_{C} \, ; $$

(17)

Parameter dist is the value of distance function of m_i and D. Thus by repeating these calculations for $n_{C}$ subsets, the $n_{C}$ estimations are obtained for transformation parameters, and an estimate of the mean and covariance of the transformation parameters can be calculated.

Algorithm 5:

Approximate_ICP_Variance

3.7 The particle filter implementation

A particle filter can be implemented according to the values obtained for the probability distribution model related to the parameters of the ICP matching algorithm.

First, according to the 2D or 3D space defined for matching, a four- or seven-state particle filter can be defined. In the two-dimensional, the particles have a rotation angle and two translate variables, and a scale variable. Which is defined as:

$$ \begin{aligned} x_{i} & = \left\{ { \, a_{i} ,b_{i} ,s_{i} ,\theta_{i} } \right\}; \\ T_{i} & = \left[ {a_{i} ,b_{i} } \right];\quad Scale_{i} = s_{i} ;\quad R_{i} = R\left( {\theta_{i} } \right); \\ \end{aligned} $$

(18)

In the 3D space, the states have three translate variables in three directions and a scale variable and three rotation angles. Which is defined as follows:

$$ \begin{aligned} x_{i} & = \left\{ { \, a_{i} ,b_{i} ,c_{i} ,s_{i} ,\varphi_{i} ,\theta_{i} ,\psi_{i} } \right\}; \\ T_{i} & = \left[ {a_{i} ,b_{i} ,c_{i} } \right];\quad Scale_{i} = s_{i} ;\quad R_{i} = R\left( {\varphi_{i} ,\theta_{i} ,\psi_{i} } \right); \\ \end{aligned} $$

(19)

As mentioned before, in the particle filter, the more processing data, the greater computational volume required. In each particle filter iteration, the estimation algorithm propagates the particles forward to reach their optimum values and the error distance function is minimized. According to the initial values calculated in the previous section as well as the obtained variance values, a region for particle propagation can be determined (2sigma-standard deviation interval around the mean). The particles are then propagated as uniform random distribution in this region, the value of the distance function for each particle is calculated, and the weights of the particles are defined. This weighting process assumes a Gaussian function of the computed errors and is given by:

$$ w_{i} = \frac{1}{{\sqrt {2\pi Q} }}\exp \left( { - \frac{{\left( {D - s_{i} R_{i} M - T_{i} } \right)^{2} }}{2Q}} \right) $$

(20)

$$ \begin{aligned} i & = 1:n_{P} ;total\_number\_of\_particles \\ w_{i} & = The\_Particles\_Weight; \\ Q & = ICP\_variance; \\ T_{i} & = \left[ {a_{i} ,b_{i} ,c_{i} } \right];\quad Scale_{i} = s_{i} ;\quad R_{i} = R\left( {\varphi_{i} ,\theta_{i} ,\psi_{i} } \right); \\ \end{aligned} $$

After weighing the particles, their sum is normalized to one.

$$ W_{i} = \frac{{w_{i} }}{{\sum\nolimits_{i = 1}^{nSMC} {w_{i} } }}; $$

(21)

The weight functions related to the particles and propagation regions are updated according to the uncertainty for each iteration, and new propagation regions are defined around the new means according to new standard deviation intervals. Then, the resampling step is performed based on the new propagation if necessary. For the resampling scheme, the sequential importance resampling (SIR) technique [42] was used here. The operation of SIR particle filter algorithm is demonstrated in Fig. 4.

Fig. 4

The proposed PF-ICP algorithm: particle propagation, likelihood calculation, and resampling

The number of samples was adapted online, invoking large sample sets only when necessary to avoid the local minimum. As a result, the samples with low-importance are omitted, and the particles with high-importance weights are multiplied by the resampling step. When the PF-ICP sampling step was terminated, the resulting particles were used in the next computational step. The sample set size was selected based on the required accuracy and computational load capability, and it was shown that the variance of the importance sampler converges to zero at a rate of 1/√N [46]. The proposed PF-ICP matching algorithm flowchart is as follows (Fig. 5).

Fig. 5

The proposed PF-ICP matching algorithm chart

Algorithm 6:

The proposed PF-ICP Algorithm

4 Experiments and results

In this section, the capabilities of the proposed method to reach the global optimum value of the ICP parameters are shown by using three models with complexity and highly symmetrical spatial. Under different conditions, the PF-ICP algorithm determines the optimal matching parameters. The first model dataset consisted of three-dimensional continuous data strings, and the other model contained a three- dimensional data surface and model-3 as real 3D scan. The use of the synthetic model examples provides the possibility to artificially the maximum symmetry creation. These examples ensure the performance of the algorithm in complex conditions and obtain the necessary assurance for implementation and utilization steps.

The PF-ICP algorithm was implemented on an XC6SLX45-2FGG484C FPGA, which is employed in laboratory medical-surgery robots, in Fig. 6. The model map was recorded in the FPGA, and the scanned data was generated or simulated by MATLAB and then inputted with a PC USB port. The FPGA board executed the matching algorithm to compensation positioning error and the result sent back to the PC.

Fig. 6

The implemented algorithm on the lab surgery robot FPGA board

As shown in Fig. 7, model-1 datasets are contained continuous data strings in three-dimensional space, and the performance of the presented algorithm is evaluated for sequence strings of data sample matching. In this model, the string of samples is considered similar to a spiral because the traditional ICP algorithms suffer from the local minimum in the objective function minimizing these shape types and do not obtain the correct answer that is discussed.

Fig. 7

The 3D Continuous Strings Symmetrical model-1 dataset:

The following are the results of a study of the model-1 matching, instead of converging toward the optimal parameters and an error function of less than 0.001, the traditional ICP algorithm fluctuated and failed to find the overall minimum (Fig. 8).

Fig. 8

The MSD value of the distance function related to the ICP algorithm output of model-1

To further explain this matter and investigate how to form a local minimum, a geometric distance function is drawn based on different drift values of the spatial transformation parameters. In model-1, the distance function is drawn for the deviation of the Z-axis rotation $\varphi ,T_{Z}$ spatial transformation parameters, form-1 in Fig. 9. Also, in Fig. 9 form-3, the objective function of the matching error exhibits similar behaviour for the various drifts of the $\psi$ parameters.

Fig. 9

The absolute of distance between the origin model-1 and its deviated point sets scan-1 respect to the percent of parameters standard deviation$(\upsigma )$, Form1: The parameters $\varphi ,T_{Z}$ are drifted, Form2: The parameters $\psi ,\varphi_{{}} ,_{{}} T_{x}$, Form3: The $\psi$ They are drifted

As shown, the local minimum is created at some points, which most optimization methods, such as gradient descent-based methods, mistakenly present these points as the solution to the problem, whereas the correct answer is far away from it. With the same aim and similarly, if the data of scan-1 is regenerated with several known values of the transformation parameters, then the traditional ICP method is applied to match the scan and the model. The value of the real distance between the matched datasets and the main model datasets are plotted in terms of input transformation parameter drift values, in Fig. 10.

Fig. 10

The MSE of ICP matching algorithm that it is applied for the three deviation of Fig. 7 for the model-1, respect to percent of parameters standard deviation

As shown in Fig. 10, owing to the complexity and geometric symmetry of the model data, the ICP algorithm cannot calculate the values of the transformation parameters correctly and the MSE of the spatial distance is not less than the threshold of acceptable error value. Note that this case study uses the same points for the model set and the scan set; then, correct matching occurs when the distance function or error is zero. Therefore, when the error is not zero, the ICP algorithm is placed at local minimum and the estimated parameters values are incorrect.

In the other study model, that applicable in geophysics, geomagnetic studies and earth geometry, the model dataset is the 3D mesh of surfaces. Owing to existing symmetries and local similarities, if the proper initial values ​​are not used, the traditional ICP algorithms will make a mistake in the distance function minimization and will have a local minimum and wrong results will be obtained (Fig. 11).

Fig. 11

The model-2 datasets, the 3D mesh of surfaces;

For model-2, the behaviour of the absolute distance function and the performance of the ICP algorithm can be investigated against different drifts of displacements and rotations (Fig. 12).

Fig. 12

The absolute of distance between the origin model-2 and its deviated point sets scan-2 respect to the percent of parameters standard deviation$(\upsigma )$, Thin Line: The parameters $\mathrm{\varphi }.\uptheta$ are drifted, Think Line: The $\uptheta .\uppsi .\mathrm{s}$ parameters are drifted

As shown, owing to the complexity and geometric symmetry in the model-2, the ICP algorithm can only calculate the values of the transformation parameters in narrow range of parameter deviations, and it is often placed in the local minimum where the distance value is very close to the global minimum value.

According to these explanations, the presented PF-ICP method was implemented for these two models and its results were compared with those of the two improved ICP methods. We use these algorithm as state-of-the-art and reliable method in our research studies of shape matching. These ICP methods were improved with Welsch’s robust criterion functions [43, 44] and Cauchy’s function [43] and [45] by Bergström. In addition, to further evaluate the performance of the proposed method under different conditions, the proposed algorithm was implemented separately under thin-scanted and noisy measurement data conditions.

The scanned datasets of the models are shown in Fig. 13, and the values of the rotation, translate, and scale parameters related to the spatial transformation were chosen according to the covariance values of the ICP parameters in such a way that the condition of being in the local minimum exists. These parameters values are (Table 1; Fig. 14):

Fig. 13

The MSE of ICP matching algorithm that is applied for the two deviation (the drifted model-2), respect to percent of parameters standard deviation $\left(\upsigma \right)$

Table 1 The value of transformation parameters

Fig. 14

The 3D scan and the 3D model-1

The following results of the present PF-ICP method and two other improved ICP methods for the matching parameters are provided.

As shown in Table 2, the initial values and variance values of the parameters have been calculated for two clustering k = 10, k = 100. The results are very satisfactory. The estimated initial values are very close to the target value, and the maximum difference and the largest variance are around the z-axis, because the maximum symmetry is around this axis.

Table 2 The result of the ICP initialization algorithm and covariance approximation, model-1

The calculated values of the variances are also smaller than the Hessian method values, which provide better conditions for the filtering stage. Also, as we expected, the actual values of the parameters are in the range of one sigma around the estimated initial value.

In Table 3, as in the previous table, the values of clustering k = 100 have acceptable answers and close to target values. In addition, as we expected, the actual values of the parameters are in the range of one sigma around the estimated initial value. The calculated variance values are smaller compared to the Hessian method values, too.

Table 3 The result of the ICP initialization algorithm and covariance approximation, model-2

Table 4 presents and compares the three different algorithms results for model-1 and scan-1 matching. Also, in the last column, the results of the proposed PF-ICP algorithm have been compared with the results of two variants of ICP and shown that the matching performance is better and it much better estimated three parameters. In Table 5, the results of two variants ICP algorithms and the PF-ICP for model-2 and scan-2 are presented and compared. Also, in the last column, the results of the proposed algorithm have been compared with the results of two types of ICP algorithm and shown that it better estimated four parameters.

Table 4 The result of the proposed PF-ICP algorithm and modified ICP based on Welsch function and Cauchy function method for scanned data of model-1

Table 5 The result of the proposed PF-ICP algorithm and modified ICP based on Welsch function and Cauchy function method for scanned data of model-2

In the previous methods, the distance function approaches the nearest local minimum point and reaches a local minimum in less number of iterations, and based on that, it presents the ICP parameters that are far from their correct values. The presented PF-ICP method performs more iterations and spends more computations and achieves a much lower value of the distance function, which includes the global minimum.

Finally, the parameter values are more correctly and close to the target values are obtained, than the previous methods. The value of error function of model-2 for PF-ICP algorithm and two other variants of ICP algorithm is shown in Fig. 15.

Fig. 15

The value of error function of model-2 for each iteration. a The proposed PF-ICP b Modified ICP- Welsch function c Modified ICP Cauchy function

According to the mentioned reference [46], the variance of the importance sampler should converge at a rate of about $1/\sqrt {50}$. It reaches an error of less than 0.01 after about 40 iterations. However, since the ICP algorithm is executed in each iteration and the variance of the parameters and the area of the particles propagation can change based on the spatial environment symmetry, and the number of iterations increased to about 50.

The CPU execution times of the seven execution for presented examples are between 96 and 124 min with Intel Corei3 5Gen CPU and reach to acceptable accuracy (less than 0.01). In Fig. 16, the graphical results of PF-ICP algorithm and two other variants of ICP algorithm are compared. The effectiveness of the proposed PF-ICP algorithm, correct matching, and its accurate performance are shown.

Fig. 16

The effectiveness of the proposed PF-ICP algorithm related to other variant ICP algorithm

4.1 Experiment with noisy measurement data

Owing to the presence of noise in the measurement sensors, there is often an uncertainty in the scan data. To better evaluate the performance of the PF-ICP algorithm, noise with stochastic uniform distribution and between the (0, 0.1) range was added to the scan data, as shown in Fig. 17, and the results obtained for the transformation parameters are given below and compared with sparse ICP method [9]. The Sparse ICP is proposed to dealing with outliers and incomplete data and to solve the sensitivity of registration to outliers and missing data often observed in 3D scans (Tables 6 and 7).

Fig. 17

The noisy scanned data points for model-1: (a) PF-ICP matching algorithm (b) Spars ICP matching algorithm

Table 6 The result of the PF-ICP algorithm for noisy scanned data of model-1

Table 7 The result of the PF-ICP algorithm for thin scanned data of model-3

When the measurement noise is considered for scanned data, the average error increases, which is an acceptable value. In the last column, the absolute errors between ICP parameters and true value are indicated. As shown, the estimated parameter absolute errors are in acceptable range and this indicates the high capability of the presented algorithm in noisy conditions. Therefore, in this sparse condition, the total MSD distance function remains around 0.035, which could not be achieved with the previous ICP matching algorithm in non-sparse condition.

4.2 Thin measurement datasets 3D scan real experiment

The scanned data as the available data for the model under consideration often do not have the same density and compression in all regions, and even in some places, the amount of given and available data is minimal and the database distribution is thin. This phenomenon causes a local minimum for the objective distance function such that low-density points lose their effect on the distance function. In the traditional ICP algorithm, the transformation parameters values are miscalculated. The presented PF-ICP algorithm can estimate the transformation parameters related to the global minima in this condition with acceptable accuracy. In this experiment, the symmetric bottle is defined as real model-3, in Fig. 18. The result is compared with the EM-ICP method [16]. The EM-ICP uses the likelihood, the EM (expectation–maximization) algorithm and rigid-body transformations used for large data registration with better computation time and a little carelessness toward finding the best matching and reaching to the global minimum with respect to considered variance.

Fig. 18

Real 3D scan of symmetrical model

Therefore, in cases, that there is large data and the accuracy is negligible; the EM-ICP method is used due to less processing time. In addition, in cases with high symmetry and the presence of a complex local minimum in the distance function, the accuracy and correctness of the PF-ICP method is better and more reliable (Fig. 19).

Fig. 19

Thin measurement datasets of real experiment 3D scan. a Model data and scanned data b PF-ICP Matching algorithm c EM- ICP matching algorithm (model is blue, and scan is red)

5 Conclusion

The proposed PF-ICP algorithm is used for computer vision computational geometry, medical robotics positioning, and other intelligent applications. It fuses the ICP algorithm and the particle filter for fine 3D model matching with local minimum and uncertainty conditions, which leads to overcome the weak performance, such as a narrow convergence region, incorrect correspondence, and instability.

In this method, the initial values and covariance approximation of the ICP parameters were determined using k-means clustering. Then, the particle filter algorithm implemented to obtain the global minimum of the ICP distance function and accurate values of the registration parameters in the presence of complicated local minima and symmetrical geometry and nonlinear conditions. The experimental results of improved PF-ICP matching algorithm show that the PF-ICP yields convergence reliability and correct correspondence. In addition, the presented algorithm performed with insufficient available and sparse measurement data and measurement noise with acceptable accuracy. The performance of the PF-ICP method has been evaluated and proven by comparison with four modified ICP methods.