1 Introduction

Product quality inspection is of importance for both final and semi products in industrial production. 3D measurement techniques [1] are increasingly applied in various quality control fields, especially for free-form products. A general approach for 3D product quality inspection is employed by obtaining the point cloud of the inspected product and comparing it with the ideal product template, i.e., a CAD model is used to check the difference between the product and the design. The point cloud can be obtained by using coordinate measurement machine or other non-contact optical techniques, such as structured light system [2, 3].

Before the comparison based quality inspection can be performed, the point cloud must be aligned with the template at first. Otherwise the difference will not be found accurately. Researchers have developed many alignment algorithms in recent years, which can be divided into different classes: iterative closest point (ICP) [46] and robust point matching (RPM) [7, 8]. The ICP algorithm is proposed by Besl and Mckay [4] on the basis of quaternions and singular value decomposition (SVD), which effectively solves the point alignment problem. Many improvements have been developed by researchers on the basis of the original algorithm to achieve the acceleration or improve the accuracy. Researchers studied the effects of point cloud resampling [911], point matching mechanism [12, 13] and the point pairs weighting [14], etc.

In this paper, we propose an algorithm by combining the particle swarm optimization (PSO) with ICP algorithm to achieve the automated alignment. PSO is a type of heuristic algorithm with good adaptivity. It has been widely applied in sorts of research fields. The main objective of using PSO is to explore the global optimized solution in a complex solution space. Unlike other optimisation algorithms, PSO almost has not limitation about the features of the solution space, which means that it can be applied in a black-box problem.

Experiment has been done to test the applicability of PSO-ICP algorithm. Results show that the combination of the two algorithms can achieve more accurate alignment, and the local optimal trap caused by bad initial position of the point cloud can be avoided.

The paper is organized as follows. Section 1 introduces the importance of point cloud alignment in quality inspection problem, the related ICP algorithm and its improvements, and the algorithm proposed in this paper. Section 2 introduces the mechanism of ICP algorithm. Section 3 introduces the principle of PSO algorithm. Section 4 describes the combination of PSO with ICP algorithm in detail. Section 5 shows the evaluated experiment and makes a comparison between PSO-ICP and ICP algorithm. Section 6 comes to the conclusion of the proposed PSO-ICP algorithm.

2 ICP algorithm

The ICP algorithm is proposed for the accurate and computationally efficient alignment of 3D shapes [4]. The algorithm converges monotonically to the nearest local minimum of a mean square distance metric. The rate of convergence is quite fast during the first a few iterations. Given the point set P and the model template X, the ICP algorithm accomplishes the alignment as follows:

  1. (i)

    Compute the closest points Y on X for each point of P.

  2. (ii)

    Compute the alignment vector q from P to Y.

  3. (iii)

    Apply the transformation to P according to the alignment vector q.

  4. (iv)

    Terminate the iteration if the termination rules are met.

ICP algorithm has a rather rapid convergence during the alignment. However, it depends much on the initial relative position of the point set P and the model template X. A bad initial position always leads to a local optimal alignment.

2.1 Spatial rotation on the basis of quaternions

Spatial rotation can be represented in different mathematical ways such as the quaternions and combination of the Euler angles. Quaternion based representation is preferred in ICP algorithm over the SVD approach [4].

The quaternion is a four vector \( \varvec{q}_{R} = \left[ {q_{0} \, q_{1} \, q_{2} \, q_{3} } \right]^{\text{T}} \), where q 0 ≥ 0, and \( q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2} = 1 \). The corresponding 3 × 3 rotation matrix can be computed to calculate the transformation of the point cloud, which is used to pre-multiply column vectors, as shown follows:

$$ \varvec{R} = \left[ {\begin{array}{ccc} {q_{0}^{2} + q_{1}^{2} - q_{2}^{2} - q_{3}^{2} } & {2\left( {q_{1} q_{2} - q_{0} q_{3} } \right)} & {2\left( {q_{1} q_{3} + q_{0} q_{2} } \right)} \\ {2\left( {q_{1} q_{2} + q_{0} q_{3} } \right)} & {q_{0}^{2} - q_{1}^{2} + q_{2}^{2} - q_{3}^{2} } & {2\left( {q_{2} q_{3} - q_{0} q_{1} } \right)} \\ {2\left( {q_{1} q_{3} - q_{0} q_{2} } \right)} & {2\left( {q_{2} q_{3} + q_{0} q_{1} } \right)} & {q_{0}^{2} - q_{1}^{2} - q_{2}^{2} + q_{3}^{2} } \\ \end{array} } \right]. $$
(1)

The translation vector in ICP is defined by \( \varvec{q}_{\text{T}} = \left[ {q_{4} \, q_{5} \, q_{6} } \right]^{\text{T}} \). Thus, the complete alignment vector q is written as \( \varvec{q}_{{}} = \left[ {\varvec{q}_{R} \, \varvec{q}_{\text{T}} } \right]^{\text{T}} \). Suppose P = {P i } is a point cloud to be aligned with a model X = {x i }, where N X and N P are respectively numbers of the point in X and P, and N X  = N P which means that the index of each point p i and x i is corresponding. The object is to minimize the mean square objective function shown as follows:

$$ f\left( \varvec{q} \right) = \frac{1}{{N_{P} }}\sum\limits_{i = 1}^{{N_{P} }} {\left\| {x_{i} - \varvec{R}\left( {\varvec{q}_{R} } \right)p_{i} - \varvec{q}_{\text{T}} } \right\|^{2} } . $$
(2)

The cross-covariance matrix \( \varvec{\varSigma}_{PX}. \) of P and X is computed by

$$ \varvec{\varSigma}_{PX} = \frac{1}{{N_{P} }}\sum\limits_{i = 1}^{{N_{P} }} {\left[ {\left( {p_{i} - \mu_{i} } \right)\left( {x_{i} - \mu_{x} } \right)^{\text{T}} } \right]} , $$
(3)

where μ P and μ X are the mass centers of P and X given by

$$ \mu_{P} = \frac{1}{{N_{P} }}\sum\limits_{i = 1}^{{N_{P} }} {p_{i} } , $$
(4)
$$ \mu_{X} = \frac{1}{{N_{X} }}\sum\limits_{i = 1}^{{N_{X} }} {x_{i} } . $$
(5)

The cyclic components of the anti-symmetric matrix \( \varvec{A}_{ij} = \left( {\varvec{\varSigma}_{PX} - \varvec{\varSigma}_{PX}^{\text{T}} } \right)_{ij} \) are used to get the column vector \( \varvec{\varDelta}= \left[ {A_{23} \, A_{31} \, A_{12} } \right] \), which is used to form the symmetric 4 × 4 matrix

$$ \varvec{Q}\left( {\varvec{\varSigma}_{PX} } \right) = \left[ {\begin{array}{*{20}c} {\text{tr}\left( {\varvec{\varSigma}_{PX} } \right)} & {\varvec{\varDelta}^{\text{T}} } \\\varvec{\varDelta}& {\varvec{\varSigma}_{PX} +\varvec{\varSigma}_{PX}^{\text{T}} - \text{tr}\left( {\varvec{\varSigma}_{PX} } \right)\varvec{I}_{3} } \\ \end{array} } \right], $$
(6)

where I 3 is the 3 × 3 identity matrix. The quaternion vector \( \varvec{q}_{R} = \left[ {q_{0} \,q_{1}\, q_{2}\, q_{3} } \right]^{\text{T}} \) is corresponding to the maximum eigenvalue of the matrix \( \varvec{Q}(\varvec{\varSigma}_{PX} ) \). The translation vector can be computed by

$$ \varvec{q}_{\text{T}} = \mu_{X} - {\varvec{R}}(\varvec{q}_{R} )\mu_{P}. $$
(7)

The mean square error (MSE) d denotes the matching error between P and X, which is computed by Eq. (2). The termination criterion comprises mainly two considerations. One is the MSE threshold and the other one is the difference between the current MSE and last MSE, which is defined as

$$ d = \left| {f(\varvec{q}_{t} } \right.) - \left. {f(\varvec{q}_{t - 1} )} \right|, $$
(8)

where \( \varvec{q}_{t} \) and \( \varvec{q}_{t - 1} \) are respectively the alignment vectors in iteration t and t−1.

3 PSO

PSO is proposed by Kennedy and Eberhart [15] for the purpose of the best solution searching. PSO is inspired by the movement of organisms in a bird flock, which simulates birds’ social behaviour. Gradient method is not used in PSO, which means that PSO is feasible for the indifferentiable optimization problems that are partially irregular, noisy even change over time.

3.1 Mechanism of PSO

PSO algorithm has a population of particles, and each particle denotes a candidate solution. The particles move around in the solution space of the optimisation problem, and they are evaluated by the fitness values computed with the fitness function of the problem model. The best known position of each particle and the entire swarm are stored for guiding the movements of the particles, which are defined as the personal best value and the global best value. During the iteration, the movements of the particles are decided by their current positions, the best known individual position and the best known global position. The iteration is repeated until a satisfactory solution is finally discovered.

A particle comprises 3 vectors as shown below:

  1. (i)

    The x-vector records the current position of the particle in the search space;

  2. (ii)

    The p-vector records the best known location so far by the particle, which is named as personal best;

  3. (iii)

    The v-vector contains a velocity for particle, which denotes the moving direction of each particle in the next iteration.

At iteration t, the updating of the velocity and position for the particles follow the equations shown as below:

$$ \nu_{i} (t + 1) = \omega \nu_{i} (t) + c_{1} r_{1} \left( {p_{i} - x_{i} (t)} \right) + c_{2} r_{2} \left( {p_{g} - x_{i} (t)} \right), $$
(9)
$$ x_{i} (t + 1) = x_{i} (t) + \nu_{i} (t + 1), $$
(10)

where v the velocity of the particle, x the current position of the particle, which means the candidate solution, ω the inertia weighting, i the ith particle, c 1 and c 2 the acceleration coefficients of positive constraint, r 1 and r 2 random numbers distribution on [0,1], t the tth iteration.

The pseudo code of PSO can be written, as shown in Fig. 1.

Fig. 1
figure 1

Pseudo code of PSO

The flowchart of PSO algorithm is shown in Fig. 2.

Fig. 2
figure 2

Flowchart of PSO

The given value ω of inertial weight and the acceleration coefficient c affect the solution searching performance of PSO. A larger inertia weight ω determines a global search while a small inertia weight determines a local search. Moreover, the algorithm will have greater local search ability when c 1 > c 2, but greater global search ability when c 1 < c 2.

4 Combination of PSO with ICP

In point cloud alignment problem, ICP algorithm is able to obtain a good alignment result with small time consumption. Manual parameter adjustment is not required, which makes ICP quite applicable in the automated point cloud alignment. However, the alignment result of ICP algorithm depends much on the initial relative position of the point cloud and model template. And it is difficult to find a better solution as soon as the convergence starts towards the local optimum. To overcome this, we combine the PSO with ICP to enhance the global optimal searching ability of the original ICP algorithm. PSO is applied to provide various positions of the point cloud as candidates to ICP algorithm. The MSE value of each particle is used as the fitness value for PSO to control the update of the particles. The combination makes the ICP more robust for the automated point cloud alignment.

4.1 Conversion between axis-angle representation and quaternions

In the proposed PSO-ICP algorithm, axis-angle representation is used for the point cloud rotation transformation. This method uses axis-angle representation to define the rotation of a rigid body in a three-dimensional space. The axis and angle are defined respectively as follows: a unit vector \( \varvec{r} = \left[ {x,y,z} \right] \) indicates the direction of an axis of rotation, and an angle θ describes the magnitude of the rotation about the axis, as shown in Fig. 3.

Fig. 3
figure 3

Axis-angle representation

Because the ICP algorithm uses quaternions to compute the rotation transformation, the axis-angle representation should be converted to quaternions firstly according to

$$ \varvec{q}_{R} = \left[ {\begin{array}{*{20}c} {\cos \left( {\theta /2} \right)} & {x{\text{sin}} \left( {\theta /2} \right)} & {y{\text{sin}} \left( {\theta /2} \right)} & {z{\text{sin}} \left( {\theta /2} \right)} \\ \end{array} } \right]^{\text{T}} , $$
(11)

and then converted to 3 × 3 rotation matrix according to Eq. (1).

4.2 Proposed PSO-ICP algorithm

On the basis of the axis-angle representation, the solution space is defined as a 4-dimensional searching space including 3 Cartesian coordinates and 1 rotation angle. The PSO-ICP algorithm comprises 4 main steps, shown as follows:

  1. (i)

    Randomly generate multiple axis-angle variables and transform the point cloud with corresponding rotation matrix;

  2. (ii)

    Perform the ICP algorithm for each point cloud candidate and get the corresponding MSE value as the fitness value;

  3. (iii)

    Perform the PSO algorithm to obtain the personal best value and global best value on the basis of MSE values. Update the axis-angle variables following the equations;

  4. (iv)

    Finish the algorithm if the termination criterion is met, otherwise continue with the algorithm.

The detailed flowchart of PSO-ICP algorithm is shown in Fig. 4.

Fig. 4
figure 4

Flowchart of PSO-ICP algorithm

5 Experimental results

To test the feasibility of the proposed PSO-ICP algorithm, we produce a CNC model according to the CAD model of a free-form mouse, as shown in Fig. 5.

Fig. 5
figure 5

CAD model and CNC model

Then the point cloud of the CNC model is obtained by using a structure light system (SLS), as shown in Fig. 6. The test program is written and run in MATLAB. Regarding that only the top free-form surface is obtained in the point cloud, the vertical and bottom surfaces of the CAD model are removed for the observation of the alignment process, as shown in Fig. 7.

Fig. 6
figure 6

Structured light system

Fig. 7
figure 7

CAD free-form surface and obtained point cloud

In the experiment, the CAD model is converted to STL format to be imported into MATLAB, which stores points, as shown in Fig. 8.

Fig. 8
figure 8

STL formatted CAD model and obtained point loud

The original obtained point cloud contains 105 points. For the purpose of the test, the original point cloud is resampled and only 0.02 % points have been kept. The CAD model contains 2,237 points and the point cloud contains 2,170 points. At first, the alignment is tested by using ICP only. The CAD model is marked in blue and the point cloud is marked in green. During the test, the point cloud is randomly given to different initial positions to test the alignment result. To decide the termination criterions, different d is set to compute the corresponding MSE of the final alignment. The result shows the MSE will not decrease when d < 1 × 10−5, as shown in Fig. 9.

Fig. 9
figure 9

Obtained MSE with respect to different d

The termination parameters are set to be MSE < 1 or d < 1 × 10−5. As shown in Figs. 10 and 11, different initial positions lead to different convergence results.

Fig. 10
figure 10

Registrations using ICP algorithm at a good initial position

The experiment shows that as soon as the ICP goes to the local optimal solution, it is impossible to find the best solution. The same initial position is used to test the PSO-ICP algorithm. In the proposed algorithm, we use 3 particles to search the solutions and the initial position is the same with the example shown in Fig. 11. The result shows that PSO-ICP algorithm finds a global optimum better than using ICP, as shown in Fig. 12, which means a better alignment position.

Fig. 11
figure 11

Alignment using ICP algorithm at a bad initial position

Fig. 12
figure 12

MSE obtained with PSO-ICP algorithm

Without loss of generality, we randomly assign the initial position of the point cloud, and test the alignment result of PSO-ICP algorithm. The test result shows that even if the initial position of the point cloud is random, the algorithm can still obtain better optimums. Among 200 tests, the ideal alignment position is found in 199 tests, as shown in Fig. 13.

Fig. 13
figure 13

Result of 200 tests with random initial position

6 Conclusions

The point cloud alignment is important in 3D quality inspection. The proposed PSO-ICP algorithm combines the advantages of fast convergence of ICP and global optimum exploring ability of PSO. This combination overcomes the drawback of ICP algorithm that strongly relies on the initial relative position of the point cloud and CAD model. The experiment result shows that PSO-ICP is more robust and self-adaptive. Thus, the PSO-ICP is applicable to achieve an automated point cloud alignment without artificial interference, and obtain more accurate alignment for further quality inspection. The algorithm is also applicable for point cloud based 3D object recognition.