In this section we present previous works that have been our inspiration during creation of our modifications of nonrigid ICP algorithm.
Nonrigid ICP for deformable surface registration
The nonrigid ICP is a variant of the classical rigid ICP which was presented by [1]. The new version of algorithm was introduced by [2] as a new approach which, assumes that the source cloud is capable of deformation due to the stiffness term. Source cloud vertices v:
$$v_{i} = \left[ {xyz1} \right]$$
(1)
are arranged such that the first three values of the vector are the x, y, z coordinates of the point. The last value should be ‘one’. This is due to the characteristic of the 4N × 3 transformation matrix:
$$X_{n} = \left[ {\begin{array}{*{20}c} {r_{11} } & {r_{12} } & {r_{13} } \\ {r_{21} } & {r_{22} } & {r_{23} } \\ {r_{31} } & {r_{32} } & {r_{33} } \\ {t_{1} } & {t_{2} } & {t_{3} } \\ \end{array} } \right]$$
(2)
When multiplying the vertex vector and its transformation matrix the bottom row of the rotation matrix corresponds to the ‘one’ values in the vertices vectors and allows the translation of the point. The other values of the transformation matrix allow the rotation in x, y and z axes.
An iteration of the algorithm starts with a search for correspondences between points of both the source and target clouds. This stage is the same as in rigid ICP algorithm. Euclidean or normalshooting approach is available to find closest point pairs. After finding corresponding pairs of points we can define the distance term that has been proposed by [2] as follows:
$$E_{d} \left( X \right) = \left\ {W\left( {DX  U} \right)} \right\^{2}$$
(3)
where E_{d}—distance function cost, W—N × N identity matrix, D—N × 4N source cloud points, U—N × 3 correspondent target cloud points, X—4N × 3 transformation matrix.
In Eq. (3) the matrix W can initially be set the identity matrix and matrix D is defined in following way:
$$D = \left[ {\begin{array}{*{20}c} {v_{1}^{T} } & a & a & a \\ a & {v_{2}^{T} } & a & a \\ a & a & \ddots & a \\ a & a & a & {v_{n}^{T} } \\ \end{array} } \right]$$
(4)
Next two additions to the global cost function were presented as an improvement to the classical approach.
One of them is the landmark term which is similar to the distance term, but applied to landmark vertices. Matrices D_{L} and U_{L} are composed in the same way as the ones in correspondents distance term, except these matrices are filled with only vertices provided as pairs of landmarks. The beta parameter has been initially set to ‘one’. The landmark term is as follows:
$$E_{l} \left( X \right) = \beta \left\ {D_{L} X  U_{L} } \right\^{2}$$
(5)
where E_{l}—landmark distance function cost, D_{L}—source cloud landmarks, U_{L}—correspondent target cloud landmarks, X—transformation matrix.
Second of them is the stiffness term, which sets the rigidness of the source cloud. It needs to be provided with a topology matrix M which describes either 4connected or 8connected neighborhood relations between adjacent points in a mesh. The stiffness term is defined in following way:
$$E_{s} \left( X \right) = \alpha \left\ {\left( {M \otimes G} \right)X} \right\^{2}$$
(6)
where α—stiffness parameter, E_{s}—stiffness function cost, M—nodearc incidence matrix, G—4 × 4 weighting matrix, X—4N × 3 transformation matrix.
The matrix M should be defined such that each column represents a point in source cloud and rows represent single connectivity relation between these points. The relation is directional. In the column representing the starting point, a value of ‘− 1’ was used and in the column of the destination point, a value of ‘1’ was used. The rigidness of the cloud depends directly on the Alpha parameter. The matrix G can initially be set as the identity matrix. The operation done on the M and G matrices is the Kronecker product.
Finally we create the global cost function which is composed of the three terms described above. The global cost function is presented in following way:
$$E\left( X \right) = \left\ {\left[ {\begin{array}{*{20}c} {\alpha \left( {M \otimes G} \right)} \\ {WD} \\ {\beta D_{L} } \\ \end{array} } \right]X  \left[ {\begin{array}{*{20}c} 0 \\ {WU} \\ {U_{L} } \\ \end{array} } \right]} \right\^{2} = \left\ {AX  B} \right\^{2}$$
(7)
The matrix equation defined above is a quadratic function with an unknown variable X. It is possible to minimize that function by setting its derivative to zero and solving the acquired linear equations system. The minimum of the global cost function, with X as input variable, is located at:
$$X = \left( {A^{T} A} \right)^{  1} A^{T} B$$
(8)
Resulting transformations are applied to the source cloud points and it becomes the new initial input cloud for the algorithm. After that, a new iteration begins. The algorithm is repeated until either maximal iteration count has been achieved or clouds distance measure reaches a preset precision.
Anisotropic ICP
Another innovative approach in terms of enhancement of the rigid ICP algorithm was proposed by [3]. This version of the algorithm assumes that the input point clouds contain normally distributed, zeromean anisotropic point localization error. Few methods for computation of the covariance matrix of the noise model have been proposed. In this part, the description is limited to the TimeofFlight noise model which has been used in this modification of ICP algorithm.
The timeofflight localization error model assumes that the camera which the model was acquired from generates a point localization noise. It is a common issue with cameras and it is greater in the axis that connects the point and the camera origin than in perpendicular axes. The noise strength is set using variances that can be established arbitrarily or in accordance with the camera specification. Variance values are placed in a variance matrix which next is used to calculate covariance matrices \(\varSigma_{{x_{i} }}^{k}\), \(\varSigma_{{y_{i} }}\) and crosscovariance matrix \(\varSigma_{ij}^{k}\) in each iteration:
$$\varSigma_{{x_{i} }}^{0} = V_{{x_{i} }} S_{{x_{i} }}^{2} V'_{{x_{i} }}$$
(9)
$$\varSigma_{{y_{j} }} = V_{{y_{j} }} S_{{y_{j} }}^{2} V'_{{y_{j} }}$$
(10)
where \(\varSigma_{{x_{i} }}\)—covariance matrix for source cloud, \(\varSigma_{{y_{i} }}\)—covariance matrix for target cloud, S—diagonal standard deviation matrices for source and target cloud, V—matrices of principal axes of localization error (columns represent axes).
It is important to notice that presented values of the crosscovariance matrix and the covariance matrix of source cloud are only initial. These values change with each iteration and the covariance matrix needs to be recalculated based upon the rotation values and values of the covariance matrix achieved in the previous iteration:
$$\varSigma_{ij}^{k} = \varSigma_{{x_{i} }}^{k} + \varSigma_{{y_{j} }}$$
(11)
$$\varSigma_{{x_{i} }}^{k} = R^{k  1} \varSigma_{{x_{i} }}^{k  1} \left( {R^{k  1} } \right)^{'}$$
(12)
where \(\varSigma_{{x_{i} }}\)—covariance matrix for source cloud, \(\varSigma_{{y_{i} }}\)—covariance matrix for target cloud, \(\varSigma_{ij}^{k}\)—crosscovariance matrix, R^{k−1}—rotation matrix.
Crosscovariance matrix allows the algorithm to calculate the weighting matrix W.
It is then used to modify the correspondents matching stage of the classical ICP algorithm. Article [3] assumes closest Euclidean distances between source and target cloud points. The weighting matrix W is calculated as follows:
$$W_{ij}^{k  1} = w\left( {\varSigma_{ij}^{k} } \right)^{{  \left( {\frac{1}{2}} \right)}}$$
(13)
where W—weighting matrix, \(\varSigma_{ij}^{k}\)—crosscovariance matrix, w—normalization constant.
The modification of the Euclidean distances by application of the weighting matrix leads to a new set of distances used to find correspondent points in source and target clouds:
$$d_{new} \left( {x,y} \right) = \left\ {W_{xy} \left( {x  y} \right)} \right\$$
(14)
where d_{new}—modified Euclidean point distances, W—weighting matrix, x—source cloud points coordinates, y—target cloud points coordinates.
Another alternation of the ICP algorithm presented in [3] is related to the weighting of points at the stage of calculating transformation matrix but it will not be reviewed due to a different method of weighting the points of a mesh introduced in this article.
Proposal of addition of point localization error model and modification of landmark weighting
As mentioned in the introduction, this article is a continuation for work presented by [11]. This approach extends the methods shown in the referenced article by taking the algorithms of both [2] and [3] and creating a method of registering point clouds that would allow the model to remain nonrigid but would also take the point localization error into consideration. The algorithm is based on nonrigid registration algorithm while altering a couple of details.
Timeofflight point localization error model
First, the corresponding points pairs finding stage was changed. Instead of using the plain Euclidean distance norm presented by [2], we modified the norm as equation [15] states. To calculate the correspondents distance weighting matrix \(W_{xy}\), a point localization error is needed in the form of standard deviations on principal axes. This information should be known to parametrize the algorithm. From the details provided by the specification of the TOF camera, the localization error variance in z axis was acquired. The axis which connects the origin of the camera with the spectated surface. For this axis it is the largest and its value is around σ_{z} = 10 mm. The standard deviations of localization errors in x and y axes were set to 0.02 mm. The localization error was set equally for both input point clouds.
The covariance matrices and the crosscovariance matrix were calculated as presented in “Anisotropic ICP” with use of equations [9,10,11,12,13]. The principal axes matrices V were found for each point of the source and target cloud. In case of timeofflight localization error, the principal axes were the z axis that connects the origin of camera with ith point of the cloud and the x and y axes are perpendicular to the z axis and to each other in succeeding iterations.
As mentioned, the source cloud covariance matrix has to be recalculated after each iteration of algorithm because of updated positions of source cloud points [12]. Due to that, the crosscovariance matrix has must also be updated [11]. Once the weighting matrix has been found for an iteration [13], it can be used to modify the corresponding points distances [15].
Landmark’s weighting modification
Using [2] algorithm as a base method, calculation of the transformation matrix is presented next. The landmark term was rejected Eq. (5) in favor of placing the landmark correspondences in the distance term Eq. (3). Now, only the pairs of points with the closest distance that are not landmark points needed to be found. The D matrix remains unchanged, as it would in the case of isotropic nonrigid ICP algorithm. The U matrix has the landmark points’ coordinates set constant in each iteration, while the rest of point coordinates change according to the distance between point pairs. The global cost function for our method lacks the separate landmark term:
$$E\left( X \right) = \left\ {\left[ {\begin{array}{*{20}c} {\alpha \left( {M \otimes G} \right)} \\ {WD} \\ \end{array} } \right]X  \left[ {\begin{array}{*{20}c} 0 \\ {WU} \\ \end{array} } \right]} \right\^{2} = \left\ {AX  B} \right\^{2}$$
(15)
Another modification was that instead of leaving the weighting matrix W as the identity matrix, we set the weights of the points which were landmark points placed in matrix D to value ‘4’. For the rest of the points, weights were set to value ‘0.25’. This change increased the contribution of landmark points in the algorithm.
Test data sets
Artificial data
The artificial data used for testing were two square, flat meshes containing 625 points each, created in Matlab environment [12]. These surfaces were generated using built in Matlab “Meshgrid” function [13]. The boundaries of the meshes were initially equal and are presented as follows: x: (− 125) mm–125 mm, y: (− 125) mm–125 mm. The z coordinates of source and target surface points were 1100 mm and 1200 mm, respectively. The surfaces have been subjected to timeofflight localization error, the same as in the case of real data, which is: σ_{z} = 10 mm, σ_{x}, σ_{y} = 0.02 mm. The purpose of this was only to visually present how finding the corresponding points is affected using the approach presented by [3].
To show what effect had the modified weighting of the landmark points on the outcome of the algorithm, the surfaces without noise have been drawn from each other creating sort of a step pattern. The alpha parameter starts at a value of 100 and decreases every 20 iterations with rate of 0.5*alpha. The maximal iteration count for testing on artificial data was 100 iterations.
It is crucial to mention that in case of artificial data, there were known correspondences of cloud points. This means that the points of one cloud correspond by their number to the points of the other cloud e.g. the first point of source cloud corresponds to the first point of target cloud, both being a part of a regular and square mesh.
Real clinical data
For measuring the suitability of our approach for human based scenarios, the algorithm was tested by registering point clouds containing data of abdominal skin surfaces during inhale and exhale phases of breathing (Fig. 1) recorded by ToF camera (depth image). This data set was chosen for testing due to our work extending modifications and results presented in [11].
The mentioned article explains usage of nonrigid Iterative Closest Point for tracking patients abdominal surfaces. This approach presents a solution for a hard case of surface registration problem. The abdominal skin changes from convex to concave during breathing phases which under geometric distance constraint, in worst case, may lead to attraction of outlying points in source cloud to closest points in the center of target cloud. No extreme folding of point surfaces should be present in this type of deformation tracking, yet the greatest change in amplitude is present for central points in cloud. The surfaces were acquired with timeofflight camera Swiss Ranger SR4000, which has an absolute accuracy of about 1 cm. The markers used as landmark points in our algorithm were placed on the abdominal skin before the acquisition so that they were imaged with the designated object. The markers used were squareshaped and had a size of 15 mm. The frequency of the camera was set to 30 MHz, which allowed for acquisition of within a 5 m radius. The patient was at the distance of from 1 to 1.5 meters from the camera.
Our approach was tested on 10 cases of abdominal skin surface pairs each containing 9 markers distributed in a 3 × 3 mesh. From every marker, we extracted 4 landmark points which gave us 36 landmark points overall. Markers placed on patients abdominal skin were found using a tracking algorithm basing on maximization of normalized cross correlation value [14]. Such algorithm is used for searching and locating patterns present in 1D and 2D images. The maximal iteration count for testing on real data was 200 iterations.
Selected measures of registration quality
For measuring how well the algorithm was able to register surfaces containing real data, we used three measures:

Mean distance of the surfaces in millimeters (M1)—is measured as distance from source point clouds and their closest correspondents in target cloud (preferred minimum).

Landmark distance in numbers of units (M2)—is the localization error of landmarks measured as sum of columns and rows by which source cloud landmark has been misaligned compared to its corresponding landmark, during registration (preferred minimum). This measure was also present in [11], named as “quality of correspondences: average correspondence assignment errors for the points nearest the markers” used for evaluation of registration quality.

Percentage of target cloud points which had only one correspondent (M3)—the target cloud points may have multiple correspondents in source cloud, which is not desired. Great amount of points that have only one correspondences indicate that the clouds have been registered evenly/uniformly (preferred maximum).
Verifying the matching of landmark points and measuring their final distance is important because the landmark points, carry real correspondence information of the points which is set arbitrarily before the acquisition. For making this measure credible, only 9 points out of the landmarks set to play the role of landmarks were used for registration.
The remaining 27 landmark points were used as measurement points for the landmarks registration accuracy. For the 9 landmarks, the first point from each of the markers groups was chosen.