This document describes modifications made to the iterative closest point (ICP) algorithm. The authors propose a new matching procedure that uses the angles between line segments connecting points to find initial correspondences. They also formulate the ICP variational problem for an arbitrary affine transformation. A computer simulation applies the standard ICP approach and the authors' algorithm to two point sets related by a known transformation, finding the latter estimates the transformation more accurately.

1. Dmitrii Tihonkih
Department of Mathematics, Chelyabinsk State University,
Russian Federation
Artyom Makovetskii
Department of Mathematics, Chelyabinsk State University,
Russian Federation
Vladislav Kuznetsov
Department of Mathematics, Chelyabinsk State University,
Russian Federation
E-mails: sparsemind@gmail.com, artemmac@csu.ru,
k.v.net@rambler.ru
The iterative closest points algorithm and affine
transformations
AIST'2016

2. Introduction
The standard ICP starts with two point clouds for
their relative rigid-body transform, and
iteratively refines the transform by repeatedly
generating pairs of corresponding points in the
clouds and minimizing an error metric.

3. The ICP stages:
1. Selection of some set of points in one
clouds.
2. Matching these points to samples in
the other cloud.
3. Rejecting certain pairs based on
looking at each pair individually or
considering the entire set of pairs.
4. Assigning an error metric based on
the point pairs.
5. Minimizing the error metric
(variational subproblem of the ICP).

4. Our main focus is on the accuracy of the final
answer and the ability of ICP to reach the
correct solution for a given difficult geometry.
We consider transformation that hold the
angles between lines in the cloud of points.
Also we consider the ICP minimizing the error
metric subproblem for the case of an arbitrary
affine transformation.

5. The matching procedure for sets 𝐗
and 𝐘
Let 𝑋 = {𝑥0, … , 𝑥 𝑘−1} be an set consist of 𝑘
points in ℝ3
and 𝑌 = {𝑦0, … , 𝑦 𝑛−1} be an set
consist of 𝑛 points in ℝ3
. Denote by (𝑥𝑖, 𝑦𝑗),
𝑥𝑖 ∈ 𝑋, 𝑦𝑗 ∈ 𝑌 the pair of corresponding
points.
Note, that each point from 𝑋 and 𝑌 can be
included to the set of pairs just one time.

6. At the beginning the set of pairs is empty. Let
𝑚 ∈ ℕ be a number such that:
3 ≤ 𝑚 ≤ min(𝑛, 𝑘).
1. Consider the following subset 𝑋𝑖 of the 𝑋:
𝑋𝑖 = {𝑥 𝑚∗ 𝑖−1 , … , 𝑥 𝑚∗ 𝑖−1 +𝑚−1}.
2. Let 𝐶 be a closed piecewise linear curve in ℝ3
that consist of 𝑚 line segments. The 𝑗-th segment
connects points 𝑥 𝑚∗ 𝑖−1 +𝑗 and 𝑥 𝑚∗ 𝑖−1 +𝑗+1.

7. Denote by 𝛼𝑗 a minimal flat angle that is
constructed by 𝑗-th and (𝑗 + 1)-th segments.
Let 𝑉𝑋 be a vector
𝑉𝑋 = {𝛼0, … , 𝛼 𝑚−1},
where elements αj, j = 0, … , m − 1 are
respective angles.

8. 3. Consider all possible combinations of m
points in the set Y besides the points that
already included to the set of pairs.
For an each combination we construct the
vector 𝑉 by the same way as in step 2.
4. We choose a vector from the set of vectors of
the step 3 such that distance between them and
𝑉𝑋 is minimal relatively the norm 𝐿1. Denote
this vector as 𝑉𝑌.

9. 5. We construct 𝑚 pairs of the points by 𝑉𝑋 and
𝑉𝑌. Add this m pairs to the set of pairs.
6. If the number of remaining points in 𝑋 or 𝑌 less
that 𝑚 then procedure terminates. Else 𝑖 ≔ 𝑖 + 1
and go to step 1.

10. We use this procedure only as first iteration on
the ICP algorithm. Obtained after the first
iteration the transformation matrix and the
translation vector are used for a second
iteration. In the next iterations we use the
standard nearest neighbor approach.
The described above approach can good work
not for rigid transformation only but for
sufficiently wide subset of the affine
transformations.

11. The ICP variational subproblem for
an arbitrary affine transformation
Suppose that the relationship between points
in 𝑋 and 𝑌 is done by such a way that for each
point 𝑥𝑖 is calculated corresponding point 𝑦𝑖.
The ICP algorithm is offten considered as a
geometrical transformation for rigid objects
mapping 𝑋 to 𝑌:

12. 𝑅𝑥𝑖 + 𝑡,
where 𝑅 is a rotation matrix, 𝑇 is a translation
vector, 𝑖 = 0, … , 𝑛 − 1.
The S-ICP algorithm is given by
𝑅𝑆𝑥𝑖 + 𝑡,
where 𝑆 is a scaling matrix.

13. ICP variational problem for the case of an
arbitrary affine transformation.
Let 𝐽(𝐴, 𝑇) be the following function:
𝐽 𝐴, 𝑇 = 𝑖=0
𝑛−1
∥ 𝐴 𝑥𝑖 + 𝑡 − 𝑦𝑖 ∥2
.
The ICP variational problem can be stated as
follows:
arg 𝑚𝑖𝑛 𝐽 𝐴, 𝑡 ,
𝐴, 𝑡

14. where
𝐴 =
𝑎11 𝑎12 𝑎13
𝑎21 𝑎22 𝑎23
𝑎31 𝑎32 𝑎33
, 𝑡 =
𝑡1
𝑡2
𝑡3
,
𝑥𝑖 =
𝑥1𝑖
𝑥2𝑖
𝑥3𝑖
, 𝑦𝑖 =
𝑦1𝑖
𝑦2𝑖
𝑦3𝑖
.

15. The elements of the first row of the matrix 𝐴∗
that minimizes 𝐽 are computed as
𝑎11 = 𝑖=0
𝑛−1
𝑦1𝑖− 𝑎12 𝑥2𝑖− 𝑎13 𝑥3𝑖 𝑥1𝑖
𝑖=0
𝑛−1
𝑥1𝑖
2 ,
𝑎12 =
𝑗=0
𝑛−1
𝛾 𝑗 𝛼 𝑗−𝑎13 𝑗=0
𝑛−1
𝛽 𝑗 𝛼 𝑗
𝑗=0
𝑛−1
𝛼 𝑗
2 ,
𝑎13 = 𝑘=0
𝑛−1
𝜑 𝑘 𝜓 𝑘
𝑘=0
𝑛−1
𝜑 𝑘
2 .

16. Computer simulation
Let 𝑋 be the set consists of 80 points. The
coordinates of points are randomly generated
(by the uniform distribution). The values of all
coordinates belong to the range [0, . . . , 100].
The set 𝑌 is obtained from the set 𝑋 by the
geometrical transformation 𝑌 = 𝑅 ∗ 𝑋 + 𝑡,
where 𝑅 and 𝑡 are described below:
𝑅 =
1 0 0
0 0.5 −0.866025
0 0.866025 0.5
,

17. 𝑡T = 5 6 7 .
The standard approach based on nearest
neighbor method gives the following results
(open source, C++):
𝑅 =
0.45 0.64 0.61
0.89 −0.36 −0.27
−0.05 −0.67 0.73
,
𝑡T = −7.56 11.22 −11.42 .

18. Estimated matrix 𝑅 and vector 𝑡 (our algorithm):
𝑅 =
0.99 9.78e−06 9.41e−05
−0.000100597 0.499911 −0.866098
−2.66211e−05 0.865921 0.499979
,
𝑡T = 5.0028 6.0127 7.00388 .

19. Conclusion
In this work we considered matching and error
minimizing steps of the ICP algorithm. On the
base of the obtained results, a new efficient
algorithm for the sets alignment was designed.
The obtained results are illustrated with the
help of computer simulation.