A Novel Termination Criterion for Point
Moreover, feature-based registration algorithms usually are
Clouds Registration Algorithms
Simone Fontana1 , Augusto L. Ballardini1 , Daniele Cattaneo1 and
Domenico G. Sorrenti1
AbstractWe present a novel general purpose termination criterion
for point clouds registration algorithms. This criterion, derived experi-
mentally, has been tested on the Probabilistic Point Clouds Registration
algorithm, [1], and does not require any parameter tuning. The experi-
ments show that it provides better results than using a fixed large number
of iterations.
I. INTRODUCTION
Point clouds registration is the problem of aligning two different
point clouds, usually called the source and target point clouds.
Although the alignment could be achieved using any kind of transfor-
mation, including a non-rigid transformation, most of the literature
and also this work deals only with rigid transformations, i.e., roto-
translations. This assumption is very popular in the literature, since
it covers a very large set of cases and also usually provides good
results, while greatly simplifying the task.
One of the most popular algorithm for point clouds registration
is Iterative Closest Point (ICP), [2]. In a previous work, [1], we
proposed an improvement to the standard ICP data association policy,
derived applying statistical inference techniques on a probabilistic
model. Each point in the source point cloud is associated with a set
of points in the target point cloud; each association is then weighted
so that the weights form a probability distribution. For this reason we
called our approach Probabilistic Point Clouds Registration (PPCR).
PPCR is similar to ICP, but is more robust w.r.t. noise and outliers.
While it was designed mainly to deal with the problem of dense-
sparse registration, it can be successfully applied also to standard
point clouds registration problems.
The multi-iteration version of PPCR outperformed the most popu-
lar point clouds registration algorithms on different kinds of datasets.
For this reason, we will test a new termination criterion on PPCR.
Nevertheless, the proposed criterion can be used with any ICP-like
algorithm, as soon as it is iterative and each iteration has an initial
cost and a final cost.
The experiments in [1] have been performed using a predefined
number of iterations. A better solution is to use a termination
criterion that automatically decides when to stop the algorithm,
without introducing any additional parameter. In this work, we present
such a criterion and we test it on different kinds of point clouds, to
prove that it performs better or at least as well as using a fixed large
number of iterations.
II. RELATED WORK
Two big classes of point clouds registration algorithms exist:
those that uses geometric features and closest-point based algorithms.
Geometric features usually require the normal to the surface they
represent, in order to compute the descriptors. Therefore they are
not applicable to every kind of point clouds registration problem,
since, for example, a very sparse point cloud could not have enough
information to represent the surface in a meaningful way. This
typically happens with keypoints maps produced by vision-based
1 The authors are with IRALab, Universita degli Studi di Milano -
Bicocca, Milano, Italy emails: fontana.simone@outlook.com;
augusto.ballardini@disco.unimib.it;
daniele.cattaneo@disco.unimib.it;
domenico.sorrenti@disco.unimib.it
systems.
not accurate enough to be used stand-alone and, for this reason,
they are used to produce a rough alignment to be refined with other
techniques, [3]. Examples of 3D features are PFH [4] and their faster
variant FPFH [5], or angular-invariant features, [6].
The second class contains algorithms that are related, in some way,
to the Iterative Closest Point algorithm (ICP). ICP was developed
independently by Besl and McKay [7], Chen and Medioni [8], and
Zhang [2] and is still the most used algorithm for point clouds
registration. The goal of ICP is not to perform global registration,
i.e.,, to align two point clouds independently from their initial
misplacement, but, instead, to perform a refinement of a rough initial
alignment. It is an iterative algorithm that repeats the following steps
until convergence:
1) For each point xj in the source cloud find the closest point yk
in the target cloud
2) Find the best values for R and T (rotation and translation) that
minimize
X
kRxj + T yk k2 (1)
j
3) Transform the source cloud using R and T
The algorithm could end, for example, after a predefined number of
iterations, or when the sum of the residuals defined by Equation 1
becomes smaller than a certain threshold, or when the difference of
the rototranslations between two consecutive steps becomes smaller
than a threshold.
Many different variants of ICP have been proposed. Usually, they
aim at speeding up the algorithm or at improving the quality of the
result. For an extensive review and comparison of ICP variants, see
[9].
A very important variant of ICP is Generalized ICP [10], an
algorithm derived using a probabilistic model that performs really
well in most situations.
Another important technique used for point clouds registration is
called Normal Distribution Transform (NDT), [11]. This technique
was originally developed to register 2D laser scans, but has been
successfully applied also to 3D point clouds, [12]. Differently from
ICP, it does not establish any explicit correspondence between points.
Instead, the source point cloud or laser scan is subdivided into cells
and a normal distribution is assigned to each cell, so that the points
are represented by a probability distribution. The matching problem
is then solved as a maximization problem, using Newtons algorithm.
III. POINT CLOUDS REGISTRATION WITH
PROBABILISTIC DATA ASSOCIATION
Since we used PPCR to perform the experiments, we introduce it
briefly in this section.
PPCR differs from ICP in the data association. In ICP each
point in the source point cloud is associated only with a point in
the target point cloud, while the proposed algorithm associates a
point in the source point cloud with a set of points in the target
cloud. Moreover, the associations are weighted. PPCR, in its multi-
iteration version, is composed of two nested loops. The inner one
finds the best rototranslation that minimizes the sum of weighted
squared errors, very similarly to ICP. However, differently to ICP,
our problem cannot be solved in closed form and, thus, we use an
iterative algorithm such as Levenberg-Marquard. Notice that at each
iteration of Levenberg Marquard, the associations are not estimated
again, but their weights are recalculated. Thus we solve an iteratively
reweighted mean squared error problem. In the outer loop, we move
the source cloud with the result of the optimization, re-estimate the
associations and build a new optimization problem. This structure has
been already briefly described in our previous work, however here
we present a novel, parameterless, way to decide when the outer loop
should stop, instead than using a predefined number of iterations.
The candidate points to be associated may be found in different
ways, for example nearest neighbors search, feature matching, etc...
We found that the nearest neighbors search proved to be good
enough, while remaining very fast to compute, in contrast with
feature extraction and matching, which is usually a slow process.
However, PPCR can potentially accommodate feature matching as
prior information.
For each point xj in the source point cloud, we look for the n
nearest points, y0 , ..., yn , in the target. For each of these points yk ,
with 0 k n, we define an error term given by
kyk (Rxj + T )k2 (2)
Equation 2 represents the squared error between the point yk in the
target point cloud and the associated point xj from the source point
cloud, transformed using the current estimate of the rototranslation.
Our point cloud registration algorithm is formed by an optimization
problem, whose error terms are calculated according to Equation 2
and that is then solved using a suitable method (such as Levenberg-
Marquardt). However, given a set of points associated to xj , not
all the corresponding error terms should have the same weight.
Intuitively we want to give more importance to the associations that
are in accordance with the current estimate of the transformation and
lower importance to the others. Thus, the weight of the error term
kyk (Rxj + T )k2 is given by
kyk (Rxj +T )k2
wkj e 2 (3)
where the proportionality implies a normalization among all the error
terms associated with xj so that their weights represents a probability
distribution. Equation 3 is derived from the EM algorithm, with an
additive Gaussian noise model.
The Gaussian in Equation 3 works well, provided there are no
outliers and all points in the source point cloud have a corresponding
point in the target point cloud. However, a t-distribution is a better
choice in presence of outliers, especially when there is lot of
distortion in one of the maps that, thus, cannot be aligned perfectly.
Consequently, a more robust equation for the weights, basing on the
t-distribution, is given by
 +d
kyk (Rxj + T )k2
 2
pkj 1+ (4)
+d
wkj = pkj (5)
+ kyk (Rxj + T )k2
where the is number of degrees of freedom of the t-distribution
and d is the dimension of the error terms (in our case 3, since we
are operating with points in the 3D space).
However, in order to calculate the weights, we need an estimate
of the rotation and translation, but these are estimated by solving
the optimization problem whose error terms are weighted with
the weights we want to calculate. Hence our problem cannot be
formulated as a simple least-square error problem, but it has to be
reformulated as an Expectation Maximization problem. During the
Expectation phase the latent variables, in our case the weights, are
estimated using the previous iteration estimate of the target variables
(the rotation and translation), while during the Maximization phase,
the problem becomes a least-square error optimization problem,
with the latent variables assuming the values estimated during the
Expectation phase.
IV. TERMINATION CRITERIA
In case that the source and the target point clouds are very close, a
single iteration of PPCR may be enough, so is, e.g., one of ICP or G-
ICP. However, in a typical real scenario, more iterations are necessary.
In order for the algorithm to converge, most of the correspondences
used to form the optimization problem needs to be right. Since we
use a data association policy based on the euclidean distance, this
happens only if the two point clouds are close enough. In PPCR,
two parameters control which and how many points in the target
point cloud are associated to a particular point in the source point
cloud: the maximum distance between neighbors and the maximum
number of neighbors. Setting these parameters to very high values
could help the algorithm to converge to a good solution even when the
starting poses of the two point clouds are not really close. However,
this will allow more outliers, i.e.,wrong data associations, to get into
the optimization step. Even tough the probabilistic approach has the
capability to soft-filtering out outliers, thanks to the probabilistic
weighting technique, using too many points will lead to a huge
optimization problem which would be very slow to solve. Usually,
a much more practical and fast solution is to use lower values for
the maximum distance and the maximum number of neighbors and
use multiple iterations of the probabilistic approach, that implies re-
estimating the data associations, in the same way it is done, for
example, in ICP and G-ICP.
With this technique, PPCR becomes composed of two nested
loops. The inner one solves an optimization problem using the
Levenberg-Marquard algorithm. The outer one moves the point cloud
using the solution found in the previous step, estimates the point
correspondences and build the corresponding optimization problem.
This process is repeated until some convergence criterion is met.
The multi iteration version PPCR provides good results, even
compared to other state of the art algorithms, [1]. Of course, in order
to be of practical usefulness, such an algorithm would greatly benefit
from some kind of automatic termination criterion. It would mean
that the algorithm could decide by itself when it should stop.
The most simple termination criterion is to use a fixed predefined
number of iterations. This solution is far from being optimal, since
the number of iterations would become a parameter of the algorithm.
Most importantly, there would be no automatic way of estimating
this parameter a-priori, so this solution is unpractical and has to
be discarded. Lastly, using a fixed value for this parameter would
probably mean using too many iterations in some cases and using
too few in others. On the other hand, using a very large value would
greatly increase the execution time, in many cases without improving
the quality of the result.
To develop an automatic termination criterion, we used the fol-
lowing idea. Suppose we have the ground truth for the source point
cloud, i.e.,we know the rototranslation between the reference frames
of the source and target point clouds. At the end of each iteration,
we get an estimate of this rototranslation. Thus, we can calculate the
difference between our estimate and the ground truth. Theoretically,
this difference should decrease among the steps of the outer loop
of the algorithm, therefore the more the iterations, the smaller the
difference becomes. Practically, at some point this difference will
cease to decrease, or, more precisely, it will start decreasing of a
negligible amount. This is the iteration to which we should stop,
since it means that the algorithm has converged to a solution. Note
that it does not mean that it has converged to the right solution, but,
nevertheless, that is the best solution we can get with that set of
parameters. Obviously, in a real application, the ground truth is not
available, thus we have to find an alternative metric that behaves in
the same way, but uses the data we have.
 Our first choice was to use, as an alternative, the Mean Squared
Error (MSE) with respect to the previous iteration. We take two point
clouds: one aligned with the current estimate of the transformation
and the other aligned with the result of the previous iteration. Since
the two point clouds are actually the same cloud displaced, the point
correspondences are known and exact. Simply, the i th point in the
first point cloud corresponds to the i th point in the other. Thus,
given one point cloud at two consecutive iterations of the algorithm
X t and X t1 , with point xti being the corresponding point of xt1 i inner optimization problem, the initial and final cost of the inner
and N the dimension of the point cloud, we used eq. (6) to calculate
the Mean Squared Error (MSE).
PN t
t1 2
i ||xi xi ||
M SE(X t , X t1 ) = (6)
N discrete derivatives because they do a good job at showing when a
Therefore, we stop the algorithm when the MSE drops under a
certain relative threshold. With relative we mean that we are not using
a fixed absolute threshold, but we want to stop when, for example, the
Mean Squared Error at an iteration becomes smaller than a certain
fraction of that at the previous iteration. This means that we are
stopping the algorithm when it is not able to move (or it is moving
of a negligible amount) the source point cloud any more, thus it
has converged. We use a relative threshold, instead than an absolute,
because it is much more flexible and does not have to be tuned for
each set of point clouds.
Another option is to use the so-called Cost Drop. During each
outer iteration of the multi-iteration version of PPCR, an optimization
problem is solved. Initially the solution of the problem we are going
to optimize will have a certain cost. The optimizer will, hopefully,
reduce this cost to a lower value. The difference between the initial
and the final cost is called Cost Drop. As an alternative to the MSE,
we could use the cost drop, for example stopping the outer loop when
the cost drop of the inner loop drops under a threshold. We want to
avoid absolute thresholds, since they need to be specifically tuned for
each application. Instead, we express this threshold with respect to
the initial cost of the iteration: for example we could stop when the
cost drop is less than 1% of the initial cost of the problem. This is
what we used for our experiments and proved to be a good threshold
for obtaining accurate registrations.
Are these two metrics good for our purposes? Which one is the
best? First of all, a good termination criterion should behave, more or
less, like the difference w.r.t. the ground truth, that is the ideal metric
taken as reference. This criterion is satisfied by both our choices.
However, there is a big difference between the two alternatives.
The MSE has to be specifically calculated after each iteration and is
relatively heavy to calculate, since the whole source point clouds has
to be traversed. This is not a computationally expensive operation per
s, but, on the other hand, the relative cost drop is very fast to compute.
Indeed, while solving an optimization problem we already calculate
the absolute cost drop, since it is used as termination criterion of the
inner loop by the optimization library. Thus, calculating the relative
cost drop requires only few more operations: it comes practically
for free. For this reason we have chosen to use the Cost Drop as
termination criterion: it is very fast to compute and is as good as the
Mean Squared Error. To discover the best termination criterion we
performed a set of experiments on the Standford Bunny and Bremen
datasets, [13], [14]. These experiments have been performed only on
two datasets to find a suitable termination criterion. This criterion
will then be used to perform all the other experiments presented in
this work.
Besides these criteria, we tested also another one: the number of
successful steps of the optimization problem. Solving an optimization
problem with Levenberg-Marquard is an iterative process. Each step
of this process can be successful, if the step managed to reduce
the cost of the problem, or, otherwise, unsuccessful. Since also this
quantity is very easy to calculate, we wanted to test if it could be
used as termination criterion somehow.
In Figure 1 we plotted various termination criteria while aligning
two point clouds from the Standford Bunny dataset. The starting
transformation between the two clouds was a rotation of 45 around
the vertical axis. On the x-axis we have the number of the iteration,
while on the y-axis we can find: the number of successful steps of the
optimization problem, the cost drop (i.e.,, the difference between the
two previous values), the Mean Squared Error w.r.t. the previous
iteration, the Mean Squared Error w.r.t. the ground truth and the
discrete derivatives of the last three variables. We plotted also the
variable is not changing anymore: when the derivative becomes zero,
the value of a variable has stabilized.
We can see that both the cost drop and the MSE w.r.t. the previous
iteration have a very similar trend to the MSE w.r.t. the ground truth.
Most important, the three values stabilizes more or less at the same
iteration. This is particularly obvious if we compare the discrete
derivatives: they become almost zero more or less at the same time.
Although the MSE w.r.t. to the ground truth keeps decreasing for
a few iterations after the other two values stabilizes, its effect on
the quality of the result is negligible. This becomes obvious looking
at Figure 2, where we have two point clouds, one aligned using
a predefined very large number of iteration, the second one using
as stopping criterion the cost drop. We can seen that they overlap
practically perfectly. The difference between the errors with respect
to the ground truth of the two alignments is less than one tenth of
the resolution of the point clouds, thus can be considered definitely
negligible.
On the other hand, the idea of using the number of successful steps
as termination criterion revealed to be a mistake, since that value
oscillates a lot and appears to be not correlated to the MSE w.r.t. the
ground truth. For these reasons it was discarded. In Figures 4 and 5
we show the data we obtained using the Bremen Dataset, to which we
applied, respectively, a small rotation and a small translation, Figure 3
(in these plots, and in the followings, we will not show the derivatives
for space reasons). Also in these cases, we can see that the cost drop
stabilizes more or less when also the MSE w.r.t. the previous iteration
stabilizes and, anyway, when the cloud has already been moved to
the right solution (successive adjustment are negligible compared to
the resolution of the point cloud).
The previous experiments collected data on successful alignments,
but we performed experiments also with clouds that the PPCR
algorithm was not able to align properly. The reason behind this
choice is that we wanted to discover whether the termination criteria
were able to stop the algorithm early enough, so that computational
time is not wasted.
The first unsuccessful alignment involved two point clouds from
the Standford Bunny dataset, whose misalignment is a rotation of 90
around the vertical axis, Figure 6. The second one uses the same point
clouds, but with a rotation of 180 around the vertical axis, Figure 7.
In these cases it can be seen that the cost drop converges much
earlier than the MSE w.r.t. the ground truth. This behavior, indeed,
is good, since it appeared only in unsuccessful alignments, during
which, stopping earlier is an advantage (going further would be only
a waste of time).
V. RESULTS
We tested the proposed termination criterion using PPCR on the
same datasets we presented in our previous work, [1], and on two
pairs of point clouds taken from the Stanford Bunny, [13], and
Fig. 1. Plots of various termination criteria and their derivative for the Standford Bunny dataset.

Fig. 2. The same point cloud aligned with two different termination criteria:
a large number of iterations (green point cloud) and our termination criterion
based on the cost drop (pink point cloud).

Bremen datasets, [14]. Our goal is to show that the criterion is

effective at stopping the algorithm at the right iteration: stopping
too late is a waste of computational time, stopping too early and the
result could be sub-optimal. For this reason we did not pay particular
efforts in tuning the others parameters, since the performances of the
algorithm were already shown in our previous work. To show the
effectiveness of our termination criteria, for each dataset, we executed
the algorithm twice. The first time using a predefined very large
number of iterations. The second one using the proposed criterion
to stop the algorithm. The results are shown in Tables I and II. As
it can be seen, the results using our criterion are usually comparable,
and sometimes better, than when using much more iterations. This
Fig. 3. Two point clouds from the Bremen Dataset, to which we applied a
small translation.
means that it succeeds in stopping the algorithm at the right iteration.
In some cases, such as for the corridor dataset, the results in Table I
are much better than those in table Table II. This happens because,
sometimes, an excessive number of iterations is not only a waste
of time, but could also bring the algorithm to converge to a wrong
solution, even though the right solution was reached. This happens
with every algorithm that uses closest point based associations as a
 Fig. 6. Termination criteria for two point clouds from the Standford Bunny
Fig. 4. Termination criteria for the Bremen Dataset with a small rotation
Dataset. Initial misalignment of 90 .
applied.

Fig. 7. Termination criteria for two point clouds from the Standford Bunny
Fig. 5. Termination criteria for the Bremen Dataset with a small translation Dataset. Initial misalignment of 180 .
applied.
 TABLE I [13] G. Turk. The stanford bunny. [Online]. Available:
E XPERIMENTAL RESULTS WITH TERMINATION CRITERION http://www.cc.gatech.edu/ turk/bunny/bunny.html
[14] D. Borrmann and A. Neuchter. The bremen dataset. [Online]. Available:
Dataset MSE Iterations Execution Time http://kos.informatik.uni-osnabrueck.de/3Dscans/
Corridor 0.307413 14 0m37.985s
Office 0.423599 19 0m51.465s
Linkoping 0.528188 8 0m40.037s
Bunny 0.00247781 19 1m23.795s
Bremen 0.45468 8 1m33.213s

TABLE II
E XPERIMENTAL RESULTS WITH FIXED NUMBER OF ITERATIONS

Dataset MSE Iterations Execution Time

Corridor 0.665891 100 4m49.263s
Office 0.482999 100 4m26.814s
Linkoping 0.499846 100 7m38.357s
Bunny 0.00237354 100 5m35.177s
Bremen 0.483014 100 5m55.141s

greedy approximation for the (unknown) correspondences.

VI. CONCLUSIONS
We introduced a novel termination criterion for ICP-like algo-
rithms. This criterion was derived experimentally and provided results
at least as good as when using a high number of iterations, when
used with the Probabilistic Point Clouds Registration Algorithm.
Moreover, it stops the algorithm very early when solving problems
that are not going to converge using more iterations. While it requires
two parameters, they are very generic and thus the values we provide
can reliably be used without the need of any fine-tuning.

R EFERENCES
[1] G. Agamennoni, S. Fontana, R. Y. Siegwart, and D. G. Sorrenti, Point
clouds registration with probabilistic data association, in Intelligent
Robots and Systems (IROS), 2016 IEEE/RSJ International Conference
on. IEEE, 2016, pp. 40924098.
[2] Z. Zhang, Iterative point matching for registration of free-form curves
and surfaces, International journal of computer vision, vol. 13, no. 2,
pp. 119152, 1994.
[3] D. Holz, A. E. Ichim, F. Tombari, R. B. Rusu, and S. Behnke,
Registration with the point cloud library: A modular framework for
aligning in 3-d, IEEE Robotics & Automation Magazine, vol. 22, no. 4,
pp. 110124, 2015.
[4] R. Rusu, N. Blodow, Z. Marton, and M. Beetz, Aligning point cloud
views using persistent feature histograms, in IEEE/RSJ International
Conference on Intelligent Robots and Systems, 2008. IROS 2008, Sep.
2008, pp. 33843391.
[5] R. Rusu, N. Blodow, and M. Beetz, Fast Point Feature Histograms
(FPFH) for 3d registration, in IEEE International Conference on
Robotics and Automation, 2009. ICRA 09, May 2009, pp. 32123217.
[6] J. Jiang, J. Cheng, and X. Chen, Registration for 3-D point cloud using
angular-invariant feature, Neurocomputing, vol. 72, no. 1618,5-05-29,
pp. 38393844, Oct. 2009.
[7] P. Besl and N. D. McKay, A method for registration of 3-d shapes,
Pattern Analysis and Machine Intelligence, IEEE Transactions on,
vol. 14, no. 2, pp. 239256, Feb 1992.
[8] Y. Chen and G. Medioni, Object modeling by registration of multiple
range images, in Robotics and Automation, 1991. Proceedings., 1991
IEEE International Conference on. IEEE, 1991, pp. 27242729.
[9] F. Pomerleau, F. Colas, R. Siegwart, and S. Magnenat, Comparing ICP
variants on real-world data sets, Autonomous Robots, vol. 34, no. 3, pp.
133148, Apr. 2013.
[10] A. Segal, D. Haehnel, and S. Thrun, Generalized-ICP. in Robotics:
Science and Systems, vol. 2, 2009.
[11] P. Biber and W. Straer, The normal distributions transform: A new
approach to laser scan matching, in Intelligent Robots and Systems,
2003.(IROS 2003). Proceedings. 2003 IEEE/RSJ International Confer-
ence on, vol. 3. IEEE, 2003, pp. 27432748.
[12] H. Merten, The three-dimensional normal-distributions transform,
threshold, vol. 10, p. 3, 2008.