This article has been accepted for inclusion in a future issue of this journal. Content is final as presented, with the exception of pagination.
2 IEEE TRANSACTIONS ON INTELLIGENT TRANSPORTATION SYSTEMS
obtaining the Kalman gain, and the new state will be updated.
This iteration will terminate once the preset threshold is
satisfied.
II. R
ELATED WORK
There are three main types of registration methods regarding
the different point set models: distance-optimization-based,
probability-based and filtering-based methods. Here, we will
discuss these types of methods in detail and conclude with the
relation between these methods and ours.
A. Distance-Optimization-Based Methods
The distance-optimization-based point set registration gen-
erally follows a two-step scheme. The first step is to measure
a predefined distance between two point sets and compute
the correspondence relation using the closest or some other
criterion. Then, the second step is to minimize the distance
between these two point sets with the computed correspon-
dence using some optimization or heuristic method. Among all
the distance-optimization-based point set registration methods,
ICP [2] is one of the early and widely used methods. Given
two data sets from two different 3D coordinate systems
{P
i
, i = 1, 2,...} and {Q
i
, i = 1, 2,...},ICPaimstofind
a transformation such that the two sets best match each
other. The distance that ICP defines is the L2 distance, and
the corresponding point is the closest point in another point
set. For optimization, ICP uses least squares minimization.
This iteration will terminate after the distance between two
point sets is less than the preset threshold or the number
of iterations reaches the predefined max value. ICP is very
likely to become stuck in a nearby local minimum. Moreover,
because ICP defines the closest point as the correspondence,
it is sensitive to initialization misalignment. Subsequently,
many ICP variants [3]–[6] were proposed to overcome the
disadvantages of ICP.
Another important distance-optimization method is the
cost-function-based registration method. In [7], a kernel-based
method was used to measure the correlation between two
point sets. In that work, it was proven that the kernel cor-
relation had equivalence with the entropy of two point sets.
Thus, the distance here is the entropy between two point
sets. In contrast to ICP, correspondence is multiply linked.
Furthermore, a gradient-descent-based optimization method is
used to minimize the entropy of two point sets. A similar
method can also be found in [8] and [9]. Other entropy- and
distance-based methods share the common property that the
two-step optimization iterates until the termination condition
is satisfied. In [10], the Havrda-Charvat-Tsallis entropy was
used to measure the correlation between two point sets.
Deterministic annealing was used to minimize the divergence.
In [11] and [12], the KL divergence was used to measure
the distance between two point sets. Kernelized Renyi dis-
tance was used as the distance measurement method in [13].
For all the distance-optimization methods, the precision
can be guaranteed if given a proper initialization, but
many of them are not robust to local minima, noise and
outliers.
B. Probability-Based Methods
In recent years, probability-based point set registration
methods have attracted considerable attention. The point sets
are represented by probability density functions, and then the
similarity of two densities or the probability of one point
set represented by another will be optimized. Coherent point
drift (CPD) [14] is a recently developed probability-based
method that has been widely used. This algorithm fits the
Gaussian mixture model (GMM) centroids (representing the
first point set) to the data (the second point set) by maxi-
mizing the likelihood. It forces the GMM centroids to move
coherently as a group to preserve the topological structure
of the point sets. For the localization of intelligent vehicles,
the rigid case of CPD could be adopted. In the rigid case,
the coherence constraint is imposed by reparameterization
of the GMM centroid locations with rigid parameters, and
a closed-form solution of the maximization step of the EM
algorithm in arbitrary dimensions is derived.
In [15], both of the point sets were represented by GMMs,
and then the registration problem was converted to maximize
the similarity of the two GMMs. The similarities of the two
GMMs were measured by the L2 distance between these two
Gaussian mixtures. The optimization relied on a numerical
optimization method (e.g., quasi-Newton algorithm). Another
important point set registration algorithm is normal distribution
transformation (NDT) [16]. This algorithm was first proposed
for matching two continuous scans of laser scanning data
in robotics applications. The NDT algorithm transforms the
discrete set of 2D points reconstructed from a single scan into
a piecewise continuous and differentiable probability density
defined on the 2D plane. This probability density consists of a
set of normal distributions that can easily be calculated. Match-
ing a second scan to the NDT is then defined as maximizing
the sum that the aligned points of the second scan score on
this density. The advantages of NDT are that no explicit corre-
spondences between points or features have to be established
and all derivatives can be calculated analytically. However, the
disadvantage is that not everything can be modeled properly
by a local normal distribution. The size of the cubic cell is set
by users and will greatly influence convergence. Moreover,
NDT also requires the two point clouds to be close to each
other, which means that this algorithm is sensitive to the
initial parameters. Other methods of this type can be found
in [17]–[19]. These probability-based methods are robust to
noise and outliers compared to distance-optimization-based
methods, while the runtime complexity will be high.
C. Filtering-Based Methods
Filtering schemes have been used to register point sets
for more than ten years. First, the state space model should
be defined. In general, the state consists of translation and
rotation parameters with x =[θ
x
,θ
y
,θ
z
, t
x
, t
y
, t
z
].The
process model is a stochastic process with x
k
= x
k−1
+v
k−1
,
where v
k−1
∼ N (0, Q
k−1
) is the Gaussian noise which makes
the system dynamic. The measurement model measures the
distance between the two point sets after adapting the state
parameters. In [20], a particle filter was exploited for point