See discussions, stats, and author profiles for this publication at: https://www.researchgate.

net/publication/326538689

CICP: Cluster Iterative Closest Point for Sparse-Dense Point Cloud

Registration

Article in Robotics and Autonomous Systems · July 2018

DOI: 10.1016/j.robot.2018.07.003

CITATIONS READS

36 1,516

5 authors, including:

Mohamed Lamine Tazir Gokhool Tawsif

ISIA LAB 7 PUBLICATIONS 73 CITATIONS
9 PUBLICATIONS 89 CITATIONS
SEE PROFILE
SEE PROFILE

Paul Checchin Laurent Malaterre

Université Clermont Auvergne - UCA, Clermont-Ferrand, France Institute Pascal
98 PUBLICATIONS 1,263 CITATIONS 16 PUBLICATIONS 152 CITATIONS

SEE PROFILE SEE PROFILE

All content following this page was uploaded by Mohamed Lamine Tazir on 28 August 2018.

The user has requested enhancement of the downloaded file.

 CICP: Cluster Iterative Closest Point for Sparse-Dense Point Cloud Registration

Mohamed Lamine Tazira,∗, Tawsif Gokhoolb , Paul Checchina , Laurent Malaterrea , Laurent Trassoudainea
a Université Clermont Auvergne, CNRS, SIGMA Clermont, Institut Pascal, F-63000 Clermont-Ferrand, France;
b MIS laboratory, Univesité de Picardie Jules Vernes, 80080, Amiens, France

Abstract
Point cloud registration is an important and fundamental building block of mobile robotics. It forms an integral part
of the processes of mapping, localization, object detection and recognition, loop closure and many other applications.
Throughout the years, registration has been addressed in different ways, based on local features, global descriptor or
object-based. However, all these techniques give meaningful results only if the input data are of the same type and
density (resolution). Recently, with the technological revolution of 3D sensors, accurate ones producing dense clouds
have appeared as well as others faster, more compatible with real-time applications, producing sparse clouds. Accuracy
and speed are two sought-after concepts in every robotic application including those cited above, which involves the
simultaneous use of both types of sensors, resulting in sparse-dense (or dense-sparse) point cloud registration. The
difficulty of sparse to dense registration lies in the fact that there is no direct correspondence between each point in
the two clouds, but rather a point equivalent to a set of points. In this paper, a novel approach that surpasses the
notion of density is proposed. Its main idea consists in matching points representing each local surface of source cloud
with the points representing the corresponding local surfaces in the target cloud. Experiments and comparisons with
state-of-the-art methods show that our approach gives better performance. It handles registration of point clouds of
different densities acquired by the same sensor with varied resolution or taken from different sensors.
Keywords: sparse to dense (dense to sparse) registration, density change, cluster, points selection, matching, ICP.

1. Introduction clouds of different densities depending on the chosen reso-

lution. Nevertheless, the difference in cloud density is gen-
The problem of dense-sparse registration has received erally due to the change of the sensor. For example, two
less attention from the scientific community in the past [1]. different sensors generate two clouds with different point
The majority of research has been focused on dense regis- densities, which requires a calibration step between the
tration [2, 3], or sparse registration [4, 5, 6, 7]. Recently, two sensors in order to exploit the resulting clouds. Cali-
the need for sparse to dense registration has come to the bration is necessary whenever the two sensors are moved,
limelight, and this is due to the emergence of sensors that which is a hard and tedious task. On the other hand,
produce sparse data like Velodyne 1 LiDAR (Light Detec- the main shortcomings of available point cloud registra-
tion And Ranging), which is widely used in autonomous tion methods are their lack of speed due to the increase of
vehicles (Google car [8], DARPA Grand Challenge [9]), be- input data or their lack of precision due to the decrease
cause of its ability to provide 3D data at a high refresh rate in density [7] whereas, for most robotic applications such
and at a long range [7]. Accurate sensors producing dense as localization, these two attributes are highly desired. A
clouds also achieved a technological leap with the appear- recent trend is to use both kinds of sensors [10, 11, 12]
ance of 3D laser sensors like Leica P202 , Riegl VZ400i3 to achieve these two sought-after concepts simultaneously,
or Trimble TX84 , etc. Furthermore, multiple sensors that highlighting the importance of dense to sparse registration
allow the change of scanning resolution have recently ap- techniques.
peared on the market. These sensors can produce point As with all registration methods, an overlapping bet-
ween the two clouds, usually called source cloud and target
∗ Corresponding cloud, is necessary to determine the rigid transformation
author
Email address: [email protected] (Mohamed Lamine between these two clouds perceived from different view-
Tazir) points. With the conventional methods that use coherent
1 Velodyne LiDAR: http://velodynelidar.com/
2 Leica P20: http://leica-geosystems.com/
data from the same sensor, what changes are the represen-
3 Riegl VZ400i: tation of points pertaining to the two views. For sensors
http://www.riegl.com/nc/products/terrestrial-
scanning/produktdetail/product/scanner/48/ that also provide intensity or color, these two attributes
4 Trimble TX8: http://www.trimble.com/3d-laser- can be changed if the two clouds are acquired at two dif-
scanning/tx8.aspx ferent times. Otherwise, except for the noise, nothing else

Preprint submitted to Robotics and Autonomous Systems August 13, 2018

can be changed, neither the number of points nor the spac- to mapping and localization in mobile robotics [3], which is
ing between the points. Regarding dense-sparse data, the our main research interest. Registration is located in the
degree of sampling changes, affecting the number of points front-end of the mapping pipeline [18]. In recent years,
and the distance separating these points. In other words, the interest and demand for 3D mapping has been greatly
for the same part of the scanning environment, taken from increased. This is mainly due to the improvement of ac-
two different viewpoints, the dense cloud will exhibit a quisition systems on the one hand and the growth of the
larger number of points with a smaller distance separat- range of potential applications on the other. Currently, 3D
ing them, as opposed to the sparser cloud. This change data can be obtained using two technologies: photogram-
affects all the local characteristics of the points (normals, metry and laser scanning [19]. The laser technology pro-
curvatures), making the conventional methods unsuitable vides direct 3D data, while photogrammetry reconstructs
for this type of registration [13, 6]. 3D information by techniques such as triangulation from
Recently, Agamennoni et al. [1] addressed the sparse- several images of the area under exploration. The advan-
dense registration issue and proposed a method that im- tage of direct 3D data acquisition makes the laser scanner
proves the standard point-to-point ICP [14, 15] by intro- popular for mapping the environment either indoors or
ducing a probabilistic model for data association. The outdoors [11]. Moreover, localization can be done at a dif-
main idea of this work is to align each point from the ferent timescale compared to the mapping, which requires
sparser cloud with a set of points from the denser cloud. that the process of localization should be robust to the
The association with each point is weighted taking into ac- environment change (such as the lighting change) [11]. In
count the uncertainty of the transformation estimate. The this study, we only focus on laser technology.
problem is formulated as an Expectation-Maximization Registration algorithms assemble two representations
procedure, during which, weights are calculated through- of an environment in a single reference frame. The problem
out the E-step, whilst during the M-step, the rigid trans- of registration has been dealt with extensively in several
formation is updated from current associations. However, studies over the last 25 years. This started with geomet-
the weakness of this method is that the associations do ric approaches leading to the appearance of the Iterative
not change at each iteration. The iterations serve only to Closest Point (ICP) algorithm [15, 14]. ICP is used to cal-
optimize the weights. That is why the method should be culate the optimal transformation fitting two point clouds
executed several times, using as input the solution of the by a two-step process: matching of points and minimizing
previous run, which consumes a lot of time. Moreover, a metric describing the misalignment [20]. These two-steps
the fact that the point associations do not change at every iterate to minimize the matching error and thus improve
iteration, makes this method very sensitive to the initial alignment. In the literature, three groups of registration
data association. methods are identified:
In this work, we propose a method to align dense and
sparse clouds to achieve accuracy and convergence speed. • sparse methods (approaches based on features ex-
This method surpasses the notion of density by replacing traction);
the points sharing the same local surface of the two clouds • dense methods (approaches exploit all the points in
by a single representative point for the matching step. It is the cloud);
not about sampling, but only for the matching process, the
points most likely to match each other are selected. Then, • approaches based on objects.
the resulting transformation is used to transform all the
source points. The process evolves iteratively in an ICP- 2.1. Sparse Approaches
like framework, starting with a selection process followed Sparse methods are generally used in outdoor environ-
by a pose estimation process. The main contribution of ments [12]. They are based on the use of features, which
this paper lies in the selection points for the matching may be points that are easily identified by their appar-
process. First, a voxelization is performed on both clouds ent character (position, local information contents, math-
to maintain the topological details of the scene. Then ematical definition, etc.) with respect to the other points.
for each voxel, a normal-based classification of its points A good feature requires stability and distinctiveness [21].
is done. Thereafter, only one point of each local surface In other words, detected features should be consistent in
is maintained for the associating step. As a result, fewer all the frames. They should be robust to noise and in-
points are used for the matching process, but they are most variant to rotation, perspective distortion and changes of
likely to be associated. Thereby, improving convergence scale [21, 22, 23]. There are numerous sparse methods in
and accuracy simultaneously. the literature, each one is adapted to specific needs, but
all of them share the same workflow. They begin by the
2. Related Work identification of the feature estimation model, then the
extraction of the set of relevant points (keypoints [24])
Registration is a crucial step in several applications, corresponding to the feature model. Afterwards, for every
ranging from inspection in the medical domain [16], pass- point, a local descriptor is computed collecting the shape
ing through the detection of objects in computer vision [17], and appearance of the neighborhood around each point.
2
Finally, keypoints found in different frames are used to some key points of the scene [21]. They are based on lo-
determine correspondences and align the different point cal characteristics of these points, often only geometric
clouds. characteristics are taken into account [39] although there
Among the well-known algorithms is the 3D Scale In- are other descriptors such as color, intensity, etc. Meth-
variant Feature Transform (3DSIFT), which is an exten- ods which fall into this category do not require any prior
sion of the 2D version proposed by Lowe in 1999 [25]. The knowledge [40]. Despite this advantage of sparse methods,
3D version was adapted by the PCL [26] community us- many of them are not completely adapted to real-time ap-
ing the curvature of points instead of the intensity of pix- plications [22]. Indeed, features are generally cumbersome
els [27]. The method uses a pyramidal approach to reach to determine, and it is unwise to compute them at each
the scale invariance characteristic of features. To achieve point [22]. Some methods identify a few numbers of lo-
invariance against rotation, it assigns orientations to key- cations where their computing may be more efficient [41],
points. but the way these points are determined is time-consuming
A multitude of methods exploiting 2D information ob- and hence are often not suitable for the applications that
tained from 3D points have emerged since. Ranging from require efficiency. When using sparse methods, another
FAST (Features from Accelerated Segment Test) [28], and problem occurs which is the necessity of very dense clouds
going through SURF (Speeded Up Robust Features) [29], in order to obtain good features, which compromises the
until ORB (Oriented FAST and Rotated BRIEF) [30]. use of sparse clouds [1, 21, 42, 4]. More importantly, these
These methods, unfortunately, are less robust [23] and are methods are environment specific [43], which may result
affected by parasitic phenomena such as illumination and in the rejection of good data [44].
weather conditions [21].
Normal Aligned Radial Feature (NARF) [31] is a rota- 2.2. Dense Approaches
tion invariant 3D feature that also operates in range image Dense approaches make use of all the points from both
and has two goals: extract points from stable local surfaces clouds, and require an initial guess (transformation) bet-
that are near significant changes and from borders. The ween the two clouds, which makes them sensitive to wrong
authors argue that working on range image makes bor- initialization [41, 40, 22]. Despite the use of all the points,
ders explicitly identified by transitions from foreground these methods are generally faster than sparse approa-
to background. Indeed, borders usually appear as non- ches [40].
continuous traversals from foreground to background. Still ICP algorithm belongs to this class of methods. Its
according to the authors, points from stable surface that strategy consists of supposing an optimistic assumption
represent a significant change in a local neighborhood rep- that there are a number of points in common between
resent robust points that can be detected and observed the two clouds taken from two different viewpoints. In
from different perspectives. this way, the algorithm will have an adequate initial esti-
However, all those methods cited above, operate on mate of the translation and the rotation, which moves the
2D representation of 3D point cloud. Recently, a method points of the source cloud to correspond with the points
developed by [23] highlights the use of these two strategies of the target cloud. Applying this assumption, the cor-
together (2D representation and 3D information). It uses respondence of a point will be the closest point to it. In
a 2D range image, as well as information calculated from this way, the algorithm will find the closest points of the
3D points such as normals and curvatures to extract 3D source cloud in the target cloud. After each iteration, bet-
feature point from LiDAR data. ter matches are found, which gradually produce better reg-
The last category of sparse methods exploits the 3D istration. This is repeated until the convergence of the al-
points directly. Most well-known approaches include the gorithm is reached. At this stage, the final translation and
Point Feature Histograms (PFH) which was used in [32] rotation between the two sets of points are obtained. As
to describe the local geometry around each point, in or- pointed out by Pomerleau [3], its easy implementation and
der to classify them, by means of a multi-dimensional his- simplicity, are both its strength and its weakness. This
togram, according to its local nature (flat surface, corner, has led to the emergence of many variants of the origi-
edge). PFH is a global feature descriptor as it computes nal solution, adapted in many ways, throughout the years.
a single descriptor for the entire cloud. Fast Point Fea- Most well-known examples, Chen et al. [14] improved the
ture Histograms (FPFH) [33] modifies the mathematical standard ICP by using point-to-plane metric instead of
model of FPH in order to reduce its computational com- the Euclidean distance error. This approach takes advan-
plexity. Similar approaches are formed elsewhere, such as tage of surface normal information to reject wrong pairing.
VFH (Viewpoint Feature Histogram) [34], CVFH (Clus- However, this approach fails when dealing with clouds of
tered Viewpoint Feature Histogram) [35], where features different densities, since normals computation are affected
are determined based on the geometric information of 3D by the change in resolution, presence of noise and distor-
points. In the same vein, PCA (Principal Component tion [45, 13].
Analysis) [36] are used in [37] and [38] to establish 3D The Normal Distributions Transform (3D-NDT) [46]
features for use in recognition and pose estimation. discretizes the environment in cells, where each one is mod-
In any case, sparse methods exploit information about elled by matrix representing the probability of occupation
3
of its points (linear, planar and spherical). Then, a non- shapes), 2D forms (plans), or 1D (segments). This con-
linear optimization is performed to calculate the transfor- cept allows them to benefit from a massive compression of
mation between the two clouds. Nonetheless, according information [49].
to [45], the NDT is not suitable for systems with low com- In [50], the authors propose a SLAM algorithm that
puting power capability. combines recognition and 3D reconstruction of maps at
An efficient approach for dense 3D data registration the level of the object. During the navigation process, the
was presented in [47]. This probabilistic version of ICP algorithm uses prior knowledge of specific objects that are
called Generalized ICP (GICP) is based on a Maximum supposed to be in the environment, to perform a recog-
Likelihood Estimation (MLE) probabilistic model. It ex- nition task. These objects are used as top-level features
ploits local planar patches in both point clouds which leads to optimize the ICP-based pose refinement. However, this
to plane-to-plane concept. The authors in that paper show work is limited to the indoor environment and the specific
that this algorithm is a generalization of point-to-point known objects.
and point-to-plane metrics, and the only difference lies in Fernandez-Moral et al. [51] propose a registration me-
their choice of covariance matrices. Since this algorithm thod based on planar surface. This paper represents an ex-
is point-to-plane variant of ICP, it has similar drawbacks, tension of the work published in [52] which deals with the
especially those related to normals computation. For in- recognition places in indoor environments by extraction of
stance in [13], it is shown that the non-uniform point den- planes. The extension is mainly focused on adding a pro-
sities cause inaccurate estimates, which degrade the per- babilistic framework to account for the uncertainty model
formance of the algorithm. Moreover, in [48, 1] the authors of the sensor. Whereas for [49], this approach is applica-
affirm that the GICP does not work well in outdoor and ble only to small and indoor environments. The method
unstructured environment. uses the region growing technique [53] to obtain the planar
Serafin et al. [40] extended the GICP algorithm by us- patches from the scene and represents them using a graph.
ing the normals in the error function and in the selection of Other techniques may also be used such as RANSAC [54]
correspondences, which according to the authors, increases and Hough transform [55] as in [26] and [7]. However, for
the robustness of the registration. NICP [40] works on the our proposal (dense-sparse registration), sparsity poses a
projection of the two clouds on range images. For the ref- real problem to get accurate segmentation [7].
erence cloud, this range image is recomputed at each itera- The segments are also used in the process of matching.
tion, which consumes time. These range images serve pri- In [4], the authors introduce a Velodyne point cloud regis-
marily for the selection of matched points. The Matched tration method based on line clouds. The algorithm starts
points are selected from the range image, so that they are by sampling the two clouds into sets of random segments,
points that share the same pixel and have compatible nor- then the correspondence is made by a strategy similar to
mals and curvatures. the ICP between the two sets of lines. Dubé et al. [49]
Our approach, called CICP for Cluster Iterative Clos- use a segment-based method for a loop-closure purpose.
est Point, uses an (NDT and NICP)-like representation, This method has the advantage of compressing the point
however, it is different from the NDT in the way it uses cloud into a group of distinct elements, which reduces false
the points of each voxel to determine local surfaces and matches and optimizes the time required for correspon-
get one representative point from each local surface to the dence.
matching process. In contrast, NDT computes a Gaussian Object based methods suffer from imperfect segmenta-
distribution in points of each voxel using the vicinity of tion [49]. Their matching tends to reject a lot of poten-
each point. Whereas, NICP uses an image projection of tially useful data, since they exploit information belonging
the voxel grid representation to compute statistics, and to some simplistic geometric models [44].
considers each point with the local features of the sur- CICP differs from these three sub-categories in the na-
rounding surface. These features, namely normal and cur- ture of its data and how these data are used. As input,
vature, are calculated for each point from its neighboring it takes point clouds of different resolution, gathered by
points, with a computational complexity of O(K × N ), different sensors, or with the same sensor. It is based only
where K is the number of the neighboring points used to on the geometric information of points, which makes it
compute each normal and N the total number of points. independent of weather and illumination conditions. The
Additionally, these features are used later in the process proposed approach aims to cluster points of the same sur-
of point matching between the two clouds, as opposed to face as one topological pattern, and replace all the points
our method, that does not use normals in the matching held by this model by one representing point for the match-
process. Because of the difference in density, pattern scan- ing step. The main algorithm is based on point-to-point
ning, and presence of noise, will lead to noisy normals and, matching alignment.
hence, inaccurate results. Table 1 summarizes the main differences between the
proposed method and the three sub-categories identified
2.3. Approaches based on Objects in the prior related work.
Object-based methods have chosen to take advantage
of higher-level representations, including 3D objects (solid
4
 Table 1: Main differences between CICP and the state-of-the-art methods.

Dense Sparse Object- CICP

methods methods based methods
No prior information required x [46] [40] X [13] [56] X [57] X
Use all points X [40] x [34] [35] x [4] X
No risk of loss of good data X [14] [15] x [58] [59] x [7] X
Based on geometric information X [41] X [23] [32] [35] X [21] X
Use of normals in correspondences choice X [47] [14] [40] X [37] [56] X [51] X

Compress point cloud in a set of distinct elements not concerned x [33] X [49] [52] [60] X
Not environment specific X [46] x [43] x [50] [51] X
Not affected by imperfect segmentation not concerned not concerned x [52] [61] X
Does not require a very dense cloud X [6] x [62] X [21] X

3. Our Contributions volumes, the two clouds are divided into voxels (subdivi-
sions) of the same size. At this stage, the main clouds
In this paper, a novel approach for sparse to dense (or integral characteristics are the set of voxels V and the set
dense to sparse) registration is introduced, exploiting nor- of points P . The relation between these two sets deter-
mals differently. The desired contributions are as follows: mines whether the cloud is sparse or dense.
Below, we give some basic definitions, and we introduce
1. A new selection strategy is proposed by keeping only
points which are most likely to be associated in the the “voxelic density” definition in theoretical and practical
matching phase, thereby improving on convergence cases:
and accuracy simultaneously. Definition 1 (Voxel in R3 ). A voxel v with center ω =
2. The proposed method is totally independent of the (x0 , y0 , z0 ) and rayon r, is a set of points P (x, y, z) if:
density (amount of points, scanning resolution) of v = {(x, y, z) : max {|x − x0 |, |y − y0 |, |z − z0 |} <= r}
the two clouds, scanning patterns (nature of sen-
sors). It takes as input point clouds of different res- Definition 2 ( Set of all voxels in P ). Let v a voxel of
olution, gathered by different sensors, or with the VP , we say that VP is a set of all voxels in P if:
same sensor. VP = {∀v ∈ Vp , v ⊂ P }
3. A novel mathematical definition of sparse and dense
concept is proposed to accomplish these objectives. Theoretically, a dense cloud is:

All the considerations outlined in this section will be Definition 3 (Voxelic density). P is dense ↔
demonstrated in the results section and these claims are ∀v ∈ Vp , ∃p ∈ P : p ∈ v
further consolidated in the Discussion section (cf. Sec- According to this last definition, a point cloud is called
tion 7). dense, if and only if, there is always at least one point
belonging to a voxel, whatever the voxel size.
4. General Formulation Unfortunately, for practical reasons, this definition is
not always verified. Because of this, we propose defini-
4.1. Mathematical Definition tions 4 and 5:
Dense and sparse are terms used to describe the state
Definition 4 (Sparse Cloud). A sparse cloud is a cloud
of points within a cloud. This includes their quantity, dis-
C = (V, P ) in which:
tribution and resolution. The distinction between these
|P | = O(|V |).
two terms is rather vague, and depends on the context.
Suppose we have two clouds of the same environment, Definition 5 (Dense Cloud). A dense cloud is a cloud
with the same dimensions and taken from the same view- C = (V, P ) in which:
point, they will probably have been taken by different sen- |P | = O(k.|V |), with k > 2.
sors or by the same sensor with varied resolutions. Let us
assume that there is a large degree of variation between O: proportionality operator.
their densities. The dense cloud will exhibit a larger num- Definitions 4 and 5 are proposed to frame the notions
ber of points with a smaller distance separating them, as of sparsity and density of point clouds. The voxel size is
opposite to the sparser cloud. The concept of density in set according to the number of points in the sparse cloud,
3D is always linked to a given volume. To get the same so that each voxel contains at least one point. This choice
ensures a significant difference in density between the two
5
clouds. A dense cloud, in our case, contains at least twice but for sparse point clouds, they might cause further
as many points as the sparse cloud. Otherwise, they are degradation to the characteristics and information
considered as equivalent. carried by these points. For this reason, we chose to
use all the points in the two clouds without making
4.2. Iterative Closest Point: The Algorithm any modification on points of both clouds.
Our proposed algorithm, named CICP in short for clus- 2. Matching: this step represents the key operation
ter iterative closest point, adopts the general scheme of the in the ICP algorithm. It consists in coupling corre-
ICP algorithm. For this reason, we will discuss here the sponding points from both clouds. These correspon-
most important items of that algorithm. As it is well- dences are obtained by seeking, for each point of the
known, the ICP algorithm is an iterative registration me- source cloud, the nearest point in the target cloud.
thod, which consists in putting the points of the source The definition of the “nearest point” determines the
cloud into the frame of the target cloud in order to gen- matching technique used [68]. Several techniques of
erate a unique and consistent point cloud. To do this, a nearest-neighbor-search (NNS) are used to optimize
translation and a rotation, which make the points of the the time of this step, as it used to be the most de-
source cloud move to correspond with the points of the manding step in terms of computation time [68]. The
target cloud must be found by the algorithm. Moreover, authors of [69] and [68] assert that “k-d trees” is the
this algorithm must identify the points of the two clouds best technique to find the nearest neighbor. For this
that correspond to each other. The strategy of the ICP reason, we use it in our implementation.
algorithm consists in taking an optimistic assumption that 3. Weighting: assignment of weights to matched pairs
there are a number of points in common between the two of points. It aims to strengthen the contribution of
clouds taken from two different points of view. In this way, correspondences believed to be correct and mitigate
the algorithm will have a good initial estimate of the rota- the effect of false matches [13].
tion R and the translation t. Applying this assumption, 4. Rejection: reject the pairs of points that do not
the correspondence of a point will be the closest point to contribute positively to the convergence of the algo-
it. In this way, the algorithm will find the closest points of rithm, such as outliers, occluded points (points that
all source points corresponding to the points of the target are not visible in one of the acquisitions) or unpaired
cloud. Once it has these correspondences, it can improve points (points of one cloud that do not find corre-
the estimate of R and t, by solving this optimization: spondents in the second cloud).
N
5. Error metrics: it defines the objective function
X which is minimized at each iteration of the algo-
R, t = argminRj ,tj kd(pi , qi )k2 (1)
rithm. Three metrics are commonly used: point-to-
i=1
point [15], point-to-plane [14] and plane-to-plane [47].
where p and q denote the pairs of corresponding points in 6. Minimization: minimize the error metric to bring
the two clouds and d represents the distance separating the points of the source cloud and align them with
the points of each pair. the points of the target cloud.
After each iteration, better matches are found produc-
ing progressively better registration. This is repeated until The algorithm terminates when a maximum number
the algorithm converges. It converges when the distance d of iterations is reached or a variation relative to the error
is less than a certain threshold. Once this convergence is metric is reached. In many cases, the algorithm converges
reached, the final rotation and the translation between the quickly but not necessarily towards the optimal solution.
two sets of points are obtained. Rusinkiewicz [63] identify Several problems may arise, namely:
six distinct stages in this algorithm: • noises and outliers that can cause biased results,
1. Selection: selection of a set of points from one or
• partial overlap.
both clouds of input points. This first stage seeks
to reduce the number of points of the input clouds
by applying one or more filters [64]. The way in 5. Proposed Method
which these points are selected has a direct impact
on the convergence of the algorithm, and especially This work proposes a novel approach that deals with
on the computation time necessary for convergence, dense-sparse registration. We adopt the Rusinkiewicz de-
in particular, when handling very dense datasets. In composition and propose a new selection strategy, which
this stage, we can find strategies like uniform [65] aims to improve the pairing process and make it reliable for
or random sampling [66], sampling according to the the purpose of this paper. Figure 1 illustrates the workflow
orientation of normals [63], statistical sampling [67] of the proposed method.
and outliers filtering [22]. However, as we deal with CICP starts with the estimation of normals of the two
sparse and dense clouds, classical selection strate- clouds. Then, it takes the target cloud first and subdi-
gies can bring improvements for dense point clouds, vides it into small voxels. The points of each voxel are

6
subjected to a classification process based on their nor- an HDL-32E Velodyne. Figures 2 (c), (d), (e), (f) are
mals, giving rise to different groups of points, according samples of various places in a scene. The 3D points of
to the geometric variation of each voxel. Each group of the source and target clouds are represented in blue and
points represents a local surface since they share the same green respectively, whilst their normals are in white and
normal vector. Next, from the points of each small local red. These figures depict the dissimilarity between nor-
surface, a single point is chosen to represent this surface mals pertaining to the same surface, which theoretically
during the matching process. In our case, we take the clos- should have the same orientations. This change is due to
est point to centroid of each local surface. Regarding the the different disturbances mentioned above. This is the
source point cloud, its points are transformed with their major problem of the methods that use geometric features
normals by the initial guess of the relative transformation. according to [21, 13]. Additionally, according to the au-
Then, this cloud undergoes the same steps as the target thors of [70], the calculation of normals on a large dataset
cloud: voxelization, normals-based classification, designa- is computationally expensive.
tion of points representatives. At the end of these steps, To overcome this problem, we use normals only to dis-
the method comes up with two sets of points from the tinguish the different local surfaces (group each surface
two clouds. Each set contains the most probable points to alone). For the rest of the algorithm, we use x, y and z
match with the points of the second set (this is specifically coordinates of each point without having recourse to their
in the overlapping area of the two clouds, as it reflects the normals. In other words, we use normals only to distin-
same geometry seen from two different viewpoints). guish the different surfaces, but we do not use them in the
alignment process.
5.1. Selection Normals are computed once for each cloud at the be-
The main contribution of this paper is the proposal ginning of the algorithm. Source normals are transformed
of a new selection strategy. As mentioned above, instead at each iteration by the transformation found. We use
of matching point-to-point as the classical ICP variants, Principal Component Analysis (PCA) [36] to determine
points pass through an election process, which gives rise to normals vectors of point cloud, as it is the most perfor-
one representative point for each small region. These rep- mant method used to compute normals according to [68]
resentatives appear as the most likely points to be matched and [71]. PCA-based algorithm is usually used to analyze
between each other. These good matches ultimately result the variation of points in the three directions. Normal vec-
in an accurate motion between the two clouds. This elec- tor corresponds to the direction with minimum variation.
tion process is based on 3D position of points and their We can also imagine the use of the dominant directions of
normals. It consists of three sub-tasks: (1) voxelization, the points as a characteristic of designation of the cluster
(2) clustering and (3) Representative election. The first within each voxel, instead of normals. We have chosen to
task performs a spatial grouping which attempts to pre- use normals, as they are classical and common features.
serve the topological information based on the 3D position From the eigen decomposition of the covariance ma-
of the points. A set of 3D cubic regions (voxels) is gen- trix of considered nearest neighbors, the eigenvector cor-
erated where all points within the voxel have very close responding to the minimum eigenvalue represents the nor-
spatial positions. The second task bundles all points of mal vector. The covariance matrix can be calculated from
each voxel based on their normals. Once this grouping is the following equation:
done, we perform the last task, which selects one point for 1 k T
each cluster (local surface) in the voxel for the matching C= Σ (pi − p̄) (pi − p̄) (2)
k i=1
process. But before that, normals need to be calculated.
where k is the number of considered nearest neighbors;
5.1.1. Normal Estimation pi , i = 1 : k are kNN points and p̄ is the mean of all k
Normal segmentation of geometric range data has been neighbors.
a common practice integrated in the building blocks of Algorithm 1 shows how to calculate the normals with
point cloud registration. Most well-known point to plane the PCA method.
and plane to plane state-of-the-art registration techniques
make use of normal features to ensure a better alignment. 5.1.2. Voxelization
However, the latter is influenced by noise, pattern scan- The voxelization is applied in order to maintain the
ning and difference in densities. Consequently, the result- topological details of the scrutinized surface. As normal
ing normals in both a source point cloud and a target computation depends on the number of neighbouring points
point cloud will not be perfectly adapted, thereby influ- and as the resolution of points of the two clouds is differ-
encing the alignment process, due to weak inter-surface ent, voxelization with the same voxel size aims to generate
correspondences. In order to support these claims, an il- equivalent local regions in the two clouds. A common cri-
lustration of sparse to dense registration is given in Fig- terion of comparison now becomes feasible. Therefore, the
ure 2. A dense point cloud is obtained from a 3D LiDAR voxel size parameter is of paramount importance for our
Leica P20 scanner whilst the sparser one is extracted from technique and it should be chosen carefully in order to keep

7
 Pre-processing Correspondances selection Optimization

Normal computing

Gauss Newton
Yes Convergence
T cloud Representative <ε

Rejection
Matching
•Selection Voxelization Clustering reached
election
•Noise No
Filtering
•Sampling Intial N
Clustering Representative
Voxelization Yes
S cloud transform election >Itermax Divergence

No
Transformation

Figure 1: Overview of the CICP pipeline. Given two point clouds, CICP starts by computing the surface normals of the two clouds. It
looks for points sharing the same local properties, and then elects one representative point from each local cluster. This election process
is based on 3D position of points and their normals. It consists of three sub-tasks: (1) Voxelization: a set of 3D cubic regions (voxels) is
generated where all voxel points have very close spatial positions. (2) Clustering: classify all points of each voxel according to their normals.
(3) Representative election: once this grouping step is completed, the last task consists in selecting one point from each cluster (local surface)
in each voxel. Representative points serve as candidates for correspondence process. As a result, few points are used in the matching process,
but which are most likely to be associated, thereby, improving on convergence and accuracy simultaneously.

Algorithm 1: Compute normals with PCA.

Input: pointXYZ P, num neighbors
Output: vector normals
1 Initialize: vector normal, vector neighbors, vector
P̄ , matrix Q, H, U, V
2 begin
3 foreach point p in P do
(a) (b)
4 // Extract the neighbors
5 neighbors =
nearestKSearch(p, num neighbors, P )
6 // Calculate the centroid of neighbors
7 P̄ = sum(neighbors)/num neighbors
8 // Compute the covariance matrix
9 Q = (neighbors − P̄ )
10 H = Q.transpose() ∗ Q
(c) (d) 11 // Compute the eigenvectors
12 [U, V] = svd decomposition(H)
13 // Sort the eigenvectors by decreasing
eigenvalues
14 U = sort decrease(U )
15 // Extract the normal
16 normal [0] = U (0, 2)
17 normal [1] = U (1, 2)
18 normal [2] = U (2, 2)
(e) (f)
19 // Stack normal in container
20 normals.emplace back(normal[0], normal[1],
Figure 2: Dense to sparse registration: (a) registration of a dense
point cloud obtained from the Leica P20 LiDAR with 88 556 380
21 normal[2])
points and a sparse point cloud obtained from an HDL-32E Velodyne 22 end
with 69 984 points using our proposed method; (b) normal vectors 23 return normals
corresponding to (a); (c), (d) and (e) are exploded views of places
24 end
indicated in (a); (f) is a close up view of (e).

the fundamental characteristics of both point clouds; be it Y and Z. The number of voxels for this bounding box
dense or sparse with topological details. In our case, the is determined by the number of points and the voxel size
set of rules mentioned previously in the Section 4.1 must is deduced. The same procedure is applied to the dense
be verified. cloud. For more details, see voxelized point clouds in Al-
At the beginning, the procedure applies a bounding gorithm 2.
box to the entire sparse cloud by finding the minimum
and maximum positions of points along the three axes X, 1. Voxel assignment: each voxel is identified by a unique

8
 Algorithm 2: Voxelization of a point cloud. 5.1.3. Clustering
Input: pointXYZ P, voxelSize The process of electing one point from each local sur-
Output: vector voxels face makes them good candidates for point correspondence
1 Initialize: minX = maxX = P [0] .x, minY= searching, thereby rejecting wrong matches impacting align-
maxY= P [0] .y, minZ = maxZ = P [0] .z, ment accuracy. At first, all the “voxelized” points are
numDivX= numDivY =0 taken and a classification method is applied to identify
2 begin points belonging to the same surface. In our work, k-means
3 // Create a bounding box for all the points of P clustering [74] is used as the classification technique based
4 foreach point p in P do on the normal of each point. The appropriate number of
5 if (p.x < minX) then minX = p.x; clusters (local surfaces) within each voxel is determined
6 if (p.x > maxX) then maxX = p.x; using the Elbow method [72, 75]. An illustration of the
7 if (p.y < minY ) then minY = p.y; described approach is given in Figure 3.
8 if (p.y > maxY ) then maxY = p.y; Grouping the point clouds using their normal aims at:
9 if (p.z < minZ) then minZ = p.z; • improving the robustness of the matching step by
10 if (p.z > maxZ) then maxZ = p.z; only allowing the pairing of compatible points,
11 end
12 // Calculate number of voxels along each axis • reducing the amount of data to be processed during
13 numDivX = (maxX − minX) /voxelSize + 1 the matching stage.
14 numDivY =
(maxY − minY ) /voxelSize + 1 5.2. Matching
15 numDivZ = (maxZ − minZ) /voxelSize + 1
The clustering process generates a reduced, but differ-
16 // Assign each point p to its corresponding voxel
ent number of points in both clouds. These two sets of
17 foreach point p in P do
points are used for matching. Based on our bibliographic
18 i = (p.x − minX) /voxelSize
research, the best technique identified is the k-d trees (Sec-
19 j = (p.y − minY ) /voxelSize
tion 4.2). We use the k-d trees implemented in PCL [26]
20 k = (p.z − minZ) /voxelSize
directly, which is based on the FLANN library [13]. The
21 idx = i + j × numDivX + k × numDivX ×
correspondence is obtained by finding, for each point of
numDivY (3)
the source cloud, the nearest point in the target cloud.
22 voxels [idx] .push bach (p)
This is accomplished using L2 norm. Since the number
23 end
of points used for matching is different in the two pairing
24 // Erase empty voxels
sets, there will be some wrong correspondences. This is
25 voxel.erase
handled in the rejection step, which aims to reduce these
(remove if (voxels.begin, voxels.end, container is
false matches.
26 empty) , voxels.end)
27 return voxels 5.3. Weighting
28 end
The aim of weighting is to reduce the influence of out-
liers on the alignment process. We tested two weighting
strategies: Huber weighting [76] and Tukey weighting [77].
linear index. If i, j, k represent the voxel indices in However, the tests showed a minimal influence of these
the X, Y , Z dimensions, respectively, numDivX, strategies on our data. Consequently, on our implemen-
numDivY are the number of voxels along X and Y tation, we do not use any weighting strategy. This is the
axes, the formula to encode the linear index [72] is:

idx = i+j ×numDivX +k ×numDivX ×numDivY

(3)
According to (3), we assign an index idx to each
point. This relationship allows direct access to the
desired voxel, thereby avoiding a linear search [73].
2. Voxels suppression: as the shape of the point cloud is
arbitrary, the step of delimiting points by a bound-
ing box creates many empty voxels which are later
pruned out. Eventually, voxelization helps to filter
noise from voxels where there is insufficient occupa- (a) Voxelized and clusterized point (b) Electing one point from each
tional evidence. cloud cluster for the matching phase

Figure 3: Voxelized/normal-based clustering for matching process.

9
same conclusion as found in [68], which affirms that the ~
Since an approximation of the displacement T(x̃) is
weighting stage might be removed from the ICP algorithm. known, the increment T(x) is assumed to be small. In
this case, it is possible to linearize the vector E(x) by
5.4. Rejection performing a Taylor series approximation around x = 0:
We opted for the distance-based rejection [39, 22], as
1
it is the most basic way to eliminate the wrong pairings. e(x) = e(0) + J(0)x + H(0, x)x + O(kxk3 ) (8)
This simple and powerful strategy consists in eliminating 2
the correspondences which have distances greater than a where J is the Jacobian matrix of the error vector E, with
given threshold [65, 22]. This aims to reject the pairs of dimensions (m×n)×6 and represents the variation of e(x)
points that do not contribute positively to the convergence as a function of each component of x:
of the algorithm, such as unpaired points (as the number
of points used for matching is different in the two pairing J(x) = ∇x e(x) (9)
sets) and outliers.
and the matrix H(x1 , x2 ) of dimensions (m × n) × 6, is
5.5. Error metrics defined ∀(x1 , x2 ) ∈ R6 × R6 by:
After the matching step, which results in a selection h
∂ 2 e1 (x1 ) ∂ 2 e (x ) ∂ 2 en (x1 )
iT
H(x1 , x2 ) = ∇x1 (J(x1 )x2 ) =
∂x1 2
x2 ∂x2 2 1 x2 ... ∂x1 2
x2
of an equivalent number of representative points in both 1
(10)
clouds, the three metrics commonly exploited in the liter- 2

ature, namely point-to-point, point to plane and plane to where each Hessian matrix ∂ ∂xe1 (x1 )
1
2 x2 represents the sec-
plane, can be used. However, for the sake of simplicity, we ond derivative of E with respect to x.
use the point-to-point metric: The system of equations (2) can be solved with a least-
squares method. This is equivalent to minimizing the fol-
N
X lowing cost function:
E= kRpi + ~t − qi k2 (4)
1 1
i=1 O(x) = ke(0) + J(0)x + H(0, x)xk2 (11)
2 2
5.6. Optimization
A necessary condition for the vector x to be a minimum
The optimization is used to determine the transforma- of the cost function is that the derivative of O(x) is zero
tion from the set of finding pairs. Given a set of correspon- at the solution, i.e. x = x̃:
dences, rotation and translation between the two frames
are calculated using Gauss-Newton iterative least square ∇x O(x)|x=x̃ = 0; (12)
algorithm [78] (Algorithm 3).
In the case of the point-to-point metric, the error func- In this case, the derivative of the cost function can be
tion to be minimized is given by: written:
T
N
∇x O(x) = (J(0) + H(0, x)) e(0) + J(0)x + O(kxk2 )


X
E (x) = ~ (x̃) pi − qi k2
kT (5) (13)
i=1 The standard method for solving (12) is Newton’s me-
~ thod. It consists of incrementally determining a solution
where T(x̃) defines the displacement between the points of
x by:
the source cloud pi and the points of the target cloud qi , T
and x a vector which belongs to R6 , representing linear ve- x = −Q−1 J(0) e; (14)
locities ϑ = [ϑx ϑy ϑz ] and angular velocities ω = [ωx ωy ωz ]. where the matrix Q is written:
Suppose now that only an approximation T̂ of T ~ (x̃) is
N
known. In this case, the registration problem consists in T
X ∂ 2 e1 (x1 )
finding the incremental transformation T(x): Q = J(0) J(0) + |x=0 ei (15)
i=0
∂x1 2
~
T(x̃) = T̂T(x) (6) However, Newton’s method requires the calculation of
the Hessian matrices, which is expensive in computation
Such that the differences between the positions of the time. Nevertheless, it is possible to approximate the ma-
source points registered by the transformation T̂T(x) and trix Q with a first order approximation by the Gauss-
those of the target cloud, are zero. Newton method:
N T
Q = J(0) J(0) (16)
X
E (x) = ~ (x̃) pi − qi k2 = 0
kT (7)
i=1
For this genre of non-linear optimization problem, the
E(x) is the vector of dimensions (m × n) × 1 containing Gauss-Newton method is preferred, because, on the one
the errors associated to each point.
10
hand, it makes it possible to ensure a definite positive ma- the solution is impacted by several factors; sensor observ-
trix Q and, on the other hand, to avoid the rather expen- ability, sensor noise, uncertainties induced whilst taking
sive calculation of the Hessian matrices. measurements. Therefore, a mathematical condition for
Under these conditions, at each iteration, a new error convergence is generally difficult to establish.
E and a new Jacobian matrix J(0) are computed in order However, the optimization domain can be sampled to
to obtain the new value of x by: provide a qualitative analysis of the convexity of the es-
 −1 timator. This is illustrated in Figure 4 where the root-
T T mean-square error (RM SE) (in meters) is shown versus
x = − J(0) J(0) J(0) e(x) (17)
two groups; translational and rotational couples. It is ob-
and to update the rigid transformation by: served that for a typically chosen subsampled point cloud
set (dense: 926 725 points, sparse: 71 584 points), the es-
T̂ ←− T̂T(x) (18) timator is convex for the translation as inferred from its
formulation and hence, the result is not directly concerned
In general, the minimization is stopped when the er- by the initialization of the algorithm. However, the min-
ror: k e k2 < α occurs, or when the calculated increment imiser, though it exhibits a globally convex profile, con-
becomes too small: kxk2 < ε, where α and ε are predefined tains one or several local minima along the way. This
stop criteria. implies that initial values for the relative rotation must be
carefully given locally around the solution.
Algorithm 3: CICP Algorithm.
Input: targetCloud, sourceCloud; voxelSeize, T̂ 6. Results
Output: Optimal T
1 Intialize: NormalXYZ T normals, S normals; We implement our CICP approach in C++ without
PointXYZ T match, S match code optimization, and we conduct multiple experiments
2 begin to evaluate it. Two different data sets are used: (1) point
3 T normals = normalComputing (targetCloud) clouds acquired by different sensors; (2) point clouds gen-
4 S normals = normalComputing (sourceCloud) erated by a single sensor by varying the scan resolution.
5 T match = normalClustering (targetCloud, These two datasets are carried out on indoor and outdoor
T normals, voxelSeize) environments. The indoor scene is represented by a typical
6 while (iteration < iter maxk|x| > ε) do office environment, which is symbolized with walls, desks
7 sourceCloud = transform (sourceCloud, and chairs. And the outdoor environment (PAVIN 5 ) is
S normals, T̂ ) an experimental site for the development of automated
8 S match = normalClustering (sourceCloud, vehicles in realistic urban environment. These different
S normals, voxelSeize) datasets provide a good platform to investigate the perfor-
9 EstablishCorrespondences (T match, mance of the proposed method. We first show its results
S match) for the registration of two sparse and dense clouds, ac-
10 distanceRejection (distThreshold) quired with two different sensors, and enumerating differ-
11 compute the Jacobian J (9) ent indoor and outdoor environments (Section 6.1). Then,
12 compute the error vector e(x) (8) we compare our results with the existing sparse and dense
13 compute the increment x (17) methods (Section 6.2). Thereafter, we perform registra-
14 update the pose T (18) tions between multiple clouds from the same sensor, but
15 iteration ← iteration + 1 with different resolutions (Section 6.3). Finally, registra-
tions between clouds from different sensors are carried out
16 end
(Section 6.5).
17 return T
The computational efficiency of the algorithm is be-
18 end
yond the scope of this paper. We would rather focus on
the methodology.
The experimental is set up as shown in Figure 5. The
5.7. Analysis of the cost function center of the two sensors; Velodyne HDL32 and that of
the Leica P20 are perfectly superimposed with the help
In this section, a more in-depth analysis of the cost
of the STANLEY Cubix cross line laser. The velodyne
function is carried out. The cost function is based on a
is then physically displaced and rotated by known trans-
sum of squared error (SSE) term as shown by equation (5).
lations and rotations from the graduated set up in order
This is an undeniable problem considering the fact that for
to perturb the 6 degrees of freedom transformation. Data
a non-linear optimization problem (as is our case), whose
domain is non-convex, it may contain several local min-
ima. The local convexity of the SSE estimator around 5 PAVIN: http://www.institutpascal.uca.fr/index.php/en/the-
institut-pascal/equipments

11
Figure 4: Convergence domain for the office scene. First row represents the RM SE with respect to the three possible DoFs in translation.
tx , ty , tz ∈ [−2 m, 2 m] with a descretization of 10 cm. The second row shows the rotation domain where each DoF takes values in [−20◦ , 20◦ ]
with a step of 2◦ .

acquisition is then performed under different scenarios in point clouds that do not exceed 70 000 points. This repre-
order to test our CICP algorithm. Table 3 below sum- sents a ratio of 14 times between the two clouds from the
marizes the various experiments performed in a controlled first environment and a ratio of 40 times, for clouds of the
environment. For each experiment, CICP is initialized at second environment.
Identity, i.e. x = [0, 0, 0, 0, 0, 0]. Figure 7 shows the registration process of such point
clouds using the CICP method. On the left, the green
6.1. Dense-Sparse Registration with CICP cloud is from the LiDAR Leica P20 and the blue cloud is
The purpose of this first experiment is to evaluate our from the Velodyne HDL32-E. The corresponding results
CICP method. For that, we choose two clouds acquired are shown on the right. Table 2 includes the various pa-
with different sensors; the denser cloud produced by a 3D rameters that manage the registration. The voxel size is
LiDAR Leica P20 laser scanner and the sparser cloud with set according to the number of points in the sparse cloud
an HDL-32E Velodyne LiDAR sensor. A Leica P20 gener- in order to verify the definition proposed in Section 4.1.
ates very detailed and dense point clouds as shown in Fig- In order to optimize the computing time, it is better to
ures 6(a), 6(c). Depending on the resolution chosen during choose the sparse cloud as the source cloud, since the lat-
the scanning process, these clouds can exceed 100 millions ter is transformed and clustered at each iteration.
of points for a single scan. For reasons of compatibility Table 2: CICP configuration parameters.
with the available computational equipment, which pro-
vided an Intel Core i74800MQ processor, 2.7 GHz, and 32 Parameter Value
GB of RAM, we perform a sampling process using method
Voxel size 8 cm
described in [72] in order to reduce the number of points Rejection distance 0.2 − 0.5 m
to the order of few millions without losing useful informa- Max iteration 500
tion. By compressing data, we provide a more compact 3D Translation tolerance 10−3 m
Rotation tolerance 10−4 ◦
representation of point clouds whilst maintaining the no-
tion of density and without affecting the initial structure
of the scanned subject. Figures 6(b), 6(d) illustrate the In order to verify the convergence of the optimization, a
output of the sampling process with 986 344 and 2 732 783 comparison between the two clouds at the start and at the
points for the office and PAVIN scenes, respectively. As end of the registration process is recorded together with
for the Velodyne HDL-32E, this sensor generates sparse the convergence profile obtained. It is summarized in the
12
 Figure 5: Experimental set up for data collection from Velodyne HDL32 (left) and Leica P20(right) sensors.

Table 3: CICP registration applied to mainly two compiled datasets; OFFICE and PAVIN. The resolutions of corresponding dense and sparse
cloud are given along with the initial physical measured transformation from our set up given by the first row of each experiment, whilst the
second row depicts the results output by our algorithm. Convergence is evaluated from the RM SE and the number of iterations required for
full registration.

Expt Envir # dense # sparse tx ty tz θx θy θz RM SE # iter.

-onment cloud cloud (mm) (mm) (mm) (◦ ) (◦ ) (◦ ) (m)
150.0 170.0 35.0 5.0 0.0 0.0 - -
1 Office 1 411 924 69 952
155.0 172.9 34.8 4.7 0.4 0.1 0.0198 40
0.0 30.0 200.0 5.0 5.0 0.0 - -
2 PAVIN 1 665 260 67 488
0.2 29.8 191.9 4.8 4.8 0.1 0.0188 36
350.0 350.0 0.0 0.0 0.0 0.0 - -
3 Office 2 986 344 69 984
357.3 342.8 0.3 0.3 0.1 0.8 0.0199 39
65.0 45.0 200.0 0.0 7.0 5.0 - -
4 PAVIN 2 1 364 245 67 768
64.1 45.7 200.9 0.3 6.9 4.9 0.0184 34
20.0 110.0 70.0 10.0 5.0 0.0 - -
5 Office 3 2 550 564 69 728
21.7 111.1 66.9 10.1 4.2 0.1 0.0191 39
0.0 0.0 0.0 10.0 10.0 0.0 - -
6 PAVIN 3 3 218 879 67 936
0.5 0.1 0.3 9.4 9.8 0.2 0.0184 55
30.0 470.0 300.0 20.0 0.0 0.0 - -
7 Office 4 4 490 859 69 996
27.8 470.3 307.3 19.6 0.1 0.1 0.0190 57
50.0 50.0 50.0 5.0 5.0 5.0 - -
8 PAVIN 4 5 025 457 67 904
52.1 48.2 53.3 5.3 7.5 5.3 0.0152 56
50.0 50.0 50.0 5.0 5.0 5.0 - -
9 Office 5 7 076 192 69 760
55.2 50.6 53.9 5.5 4.2 4.6 0.0190 35
0.0 50.0 200.0 10.0 0.0 5.0 - -
10 PAVIN 5 19 615 433 67 488
0.1 49.8 200.3 9.7 0.1 5.2 0.0169 48

evolution of the RM SE as a function of the number of it- rithm to converge. A more complete analysis is summa-
erations (see Figure 8). As expected, the convergence error rized in Table 3, along with the RM SE recorded and the
draws to a minimum until the imposed stopping condition number of iterations achieved at convergence. The analy-
is reached. It is normally a tolerance on the translation sis of this table allows us to identify three elements that
and rotation rates. In our case, this tolerance is 10−3 and influence the registration results. Namely, the inter-frame
10−4 for the translation and rotation, respectively. We displacement and the difference in density between the two
run the algorithm for several indoor and outdoor scenes, clouds, as well as the nature of the environment (indoor or
with different viewpoints, as depicted in Table 3 and shown outdoor). For the displacement, we can observe that the
in Figure 8. Each experiment is performed more than 20 larger the initial displacement, the more difficult the reg-
times. For instance, for the experiment 1 (Expt 1), the dis- istration is. We would like to draw the reader’s attention
placement between the two clouds is [150, 170, 35, 5, 0, 0], to the fact that the displacements experimented here are
where the first three values correspond to the translation quite large, keeping in mind that dense techniques gen-
in millimeters and the last three to the rotation in de- erally require an inter-frame displacement since the cost
grees. As for the fourth experiment, the displacement is function is linearized around x = 0.
[65, 45, 200, 0, 7, 5], which took 34 iterations for the algo- Continuing with our discussion on the influence of ini-

13
 should be noted that, although the actual values are sub-
jected to systematic errors of ±2◦ in rotation and ±1 cm
in translation, they do not affect, one way or the other,
the correct functioning in the various steps of our method.
The true discrepancy between the two corresponding point
clouds at convergence is measured using the RM SE. Re-
garding the influence of density, a quick look shows that
the denser the clouds becomes, the more RM SE decreases,
(a) Indoor point cloud delivered (b) Indoor point cloud after leading to better registration. We will examine this pa-
by the LiDAR Leica P20 with sampling containing 986 344
88 539 380 points points rameter in detail in the Section 6.3. Finally, we observe
that CICP performance depends on the scene. In fact,
the impact of scene affects the performance of registration
as observed by the difference of the number of iterations
required to reach the convergence between PAVIN’s and
that of the office. It is clear that the indoor environment
achieves better registration than the outdoor scene. This
is possibly caused by the richness in planar regions of the
former. It should not be overlooked that the outdoor en-
vironment contains a large amount of noise and outliers.
(c) Outdoor point cloud deliv- (d) Outdoor point cloud after
ered by the LiDAR Leica P20 sampling containing 2 732 783 This can be seen on Expt 8 and Expt 9, in which the initial
with 72 947 044 points points displacement is the same in both experiments. However,
the alignment for the office dataset requires 34 iterations
Figure 6: Point cloud sampling. to converge instead of 56 for the PAVIN dataset.
For the sake of illustration, we take four experiments
arbitrarily (Expt 3, Expt 4, Expt 7, Expt 8), and show
tial displacement, let us take the example of experiments their state before and after the registration with their con-
Expt 3 and Expt 6, which represent a pure translation and vergence profile in Figure 8. A closer look to the RM SE
a pure rotation, respectively. These two experiments, as a curves in the third column of this figure shows that the
sample of several experiments that we carry out, show that residues which are far away are successfully minimized.
generally, the pure rotation requires more energy to reach However, the convergence begins very quickly and then
the convergence with respect to the case of pure transla- stabilizes for a while before it reaches its minimum. This
tion. Tables 10 to 15 demonstrate further rigorous testing is mainly due to the fact that there is not a perfect point-
of the cost function with our selection strategy. All six to-point equivalence in the two pairing sets. This is quite
degrees of freedom (DoF) are activated and tested either logical and it can be explained by the large difference in
independently or permuted arbitrarily. The results show density between the two clouds, the noise, and the cluster-
the convergence values against the actual values, always ing defects on the two clouds.
with an initialization at identity, i.e. x = [0, 0, 0, 0, 0, 0]. It
6.2. Comparison with Existing Methods
In order to compare our method with the existing state-
of-the-art methods, we use implemented routines of PCL [26]
library for the NDT algorithm, GICP, point-to-plane ICP
and simple ICP for dense methods. For the case of sparse
methods (methods based on features extraction) we also
use PCL implementations of SIFT3D and FPFH to ex-
tract characteristic points from the two clouds, and use
(a) Clouds before registration (b) Clouds after registration
simple ICP to perform matching. The performance of
each method is evaluated using three metrics: the accu-
racy, the relative translational error and the relative ro-
tational error. The former describes the evolution of the
root-mean-square point-to-point distance; this can be ex-
pressed mathematically as:
r
1 n
(c) Clouds before registration (d) Clouds after registration RM SE = Σ k Ei k2 (19)
n i=1
Figure 7: Registration results with CICP algorithm. where n is the number of points and Ei is the distance
error between the source points and its correspondent in
14
Expt 3

(a) before alignment (b) after alignment (c) RM SE vs Iteration

Expt 7

(d) (e) (f)

Expt 4

(g) (h) (i)

Expt 8

(j) (k) (l)

Figure 8: CICP results applied to different datasets from various environments.

the target cloud in each iteration. This can be expressed tors.

as follows: RT E = ktGT − tE k2 (21)
Ei = Σmi=0 pi − qi (20)
For the Relative Rotational Error (RRE), we use the
where m is the total number of points in the sparse cloud. metric defined on the tangent space of SO(3):
pi and qi which represent two points of the source and
target cloud, respectively, whereby pi is transformed in RRE = k logm(RTE RGT )kF (22)
the reference frame of qi .
The second metric is the Relative Translational Error where logm(.) is the matrix logarithm, RE is the estimated
(RT E), which measures the translation gap between the rotation matrix, RGT is the ground truth rotation matrix
ground truth (tGT ) and the estimated (tE ) translation vec- and k.kF is the Frobenious norm.
Table 4 presents the results gathered in processing two
indoor and outdoor scenes with the state-of-the-art meth-
15
 Table 4: Comparison with the state-of-the-art methods.

Dense Sparse
ICP pt2pl NDT GICP CICP SIFT FPFH
ICP 3D+ICP +ICP
RM SE (m) 0.0602 0.0620 0.0636 0.0636 0.0299 0.0516 failed
Office
m, m, m,◦ ,◦ ,◦ RT E 0.2811 0.2482 0.2019 0.2178 0.0169 0.0191 failed
 
(m)

[0, 0.5, 0.5, 20, 0, 10] RRE (◦ ) 0.0517 0.0.0186 0.0285 0.0213 0.0005 0.0201 failed
RM SE (m) 0.0836 0.0804 0.0824 0.0860 0.0347 0.0678 failed
PAVIN
m, m, m,◦ ,◦ ,◦ RT E 0.0365 0.0253 0.0315 0.0642 0.0092 0.021 failed
 
(m)

[0, 0.5, 0.3, 0, 0, 10] RRE (◦ ) 0.0058 0.0050 0.0058 0.0090 0.0034 0.0058 failed

ods. Bold values show the best result. Quantitatively, the Table 6: Tuning parameters used of each algorithm for experimental
section (§ 6.2.1).
RM SE value of the indoor scene reaches 6 cm in the case
of point-to-point, 6.2 cm point-to-plane ICP, 6.3 cm for the
NDT and the GICP, more than 5 cm for SIFT3D and less Parameter ICP ICP P2PL NDT GICP CICP
than 3 cm for the proposed method. The maximum num- ε(stop criteria)(m) 10−6 10−6 10−6 10−6 10−6
ber of iterations for each test is fixed at 500 beyond which
Rejection distance (m) 0.5 0.5 0.5 0.5 0.5
the algorithm is considered as not having converged if it
Maximum iterations 500 500 500 500 500
reaches that ceiling, as is the case of the FPFH method.
# neighbors normals estimation - 10 - - 10
Figure 9 shows the comparison of convergences bet-
ween different registration methods. CICP outperforms # neighbors compute covariance - - - 20 -

the state-of-the-art methods on both datasets. In addition, maximum step size for MT search - - 0.1 - -
it is shown that CICP is robust against scene variation. Resolution of NDT grid (m) - - 1 - -

6.2.1. Experiment on semi structured environment

The objective of this experimental set is to compare
6.3. Changes in Density
the performance of CICP with state-of-the-art algorithms
(ICP, P2Pl, NDT and GICP) using a newly acquired dataset To investigate the role of density on CICP performance,
for a semi structured scenario type of environment where we conduct two experiments. The first is on two clouds col-
planar surfaces are far less pronounced. The results are lected with two different sensors, while for the second, the
given in Table 5, which presents the ground truth displace- clouds are acquired with the same sensor but by varying
ment versus the final results found by each algorithm. The the scanning resolution.
tuning parameters using for each one of them is laid out in
Table 6. Figure 10 gives the RM SE error against the num- 6.3.1. Data from two different sensors
ber of iterations to convergence. Again, the performance Our first fold of experiments in this section is per-
of CICP is very apparent and surpasses state-of-the-art, formed on clouds from two different sensors. The original
as shown in Figure 11. dense cloud for the first dataset “office” is acquired with
the LiDAR Leica P20. It is of size of 19 615 433 points.
We perform different sampling using the method described
Table 5: Performance comparison of each algorithm related to ex- in [72], which result in seven clouds having a size of bet-
perimental section (§ 6.2.1).
ween 4 million points and 100 000 points. These clouds are
registered with a sparse cloud obtained from the Velodyne
tx ty tz θx θy θz RM SE # HDL-32E sensor of size 69 952 points. The results of this
(m) (m) (m) (◦ ) (◦ ) (◦ ) (m) iter experiment are presented in Figure 12.
Actual 1.00 0.50 0.00 0.00 0.00 20.00 - - This experiment shows that despite the substantial dif-
ference in density between the two clouds in each test,
ICP 1.66 −0.28 0.00 −0.01 0.00 17.50 0.3406 500
there is only a slight change in the RM SE (in the order
P2Pl ICP 0.99 −0.52 0.00 −0.01 0.00 20.45 0.2482 361 of few millimeters) and in the iterations required to reach
NDT 1.71 −0.18 0.00 −0.01 −0.01 19.22 0.2876 131 the convergence. This is mainly due to the fact that the
GICP 1.14 0.16 −0.01 −0.06 0.00 15.02 0.5785 188 density does not directly affect the error which is calcu-
CICP 0.99 0.50 0.00 0.02 0.05 20.24 0.0444 163 lated from the pairing set of points. Density acts mainly
on the correctness of the clustering. Indeed, the high den-
sity gives rise to properly grouped regions, which lead to
points exhibiting the surface characteristics that it repre-
sents as much as possible, implying good matching and
thus high accuracy. This represents the strength of our
16
(a) Comparison of RM SE results of the different registration (b) Comparison of number of iterations achieved at convergence of
methods different registration methods

Figure 9: Convergence comparison between different registration methods.

method. Clustering always ensures the availability of rep- viewpoint). The outcome of this experiment is shown in
resentative points from which the matching is performed, Table 7.
even in the case of low density. The only difference is in The conclusion that can be drawn from the findings of
the minimal change present in RM SE, as illustrated by this evaluation is that the difference of density between the
the graphs in Figures 12a and 12c. This can be expressed two clouds only affects the convergence of the algorithm.
as: high density implies a superior accuracy. As in this experiment, nothing changes between the two
clouds except the density. The final RM SE values are all
6.3.2. Data from the same sensor equal and are close to the accuracy of the sensor (3 mm).
The second batch of experimentation in this section The only difference is in the number of iterations needed to
consists in aligning different clouds acquired by the same reach the convergence. This can be formulated by: a small
sensor. Figure 13 shows seven clouds with different den- difference in density between the two clouds extends the
sities. All these clouds are taken with the same sensor convergence process. In fact, this is due to several reasons.
by changing the scanning resolution each time. Indeed, First, choosing the sparse cloud as the source cloud (in or-
the LiDAR Leica P20 allows the change of resolution, by der to optimize the computing time as mentioned above).
increasing or decreasing the sampling distance (distance This means that the search for nearest neighbors is done
that separates two points of the cloud at a given distance in the dense cloud for the points of sparse cloud. Secondly,
from the scanner). In the case of the Leica P20, this dis- according to the mathematical definition proposed in this
tance varies from 0.8 mm per 10 m to 50 mm/ 10 m, giv- article (Section 4.1), the voxel size for clustering is fixed
ing rise to different density variation as illustrated in Fig- according to the number of points in the sparse cloud, in
ure 13. In this experiment, the dimensions of clouds are order to ensure points for matching. These two reasons
fixed within the sensor. This latter is placed in one fixed impact the search of the nearest neighbor, which is easier
position, and the only difference between these 7 clouds is and more accurate in a dense cloud and less precise and
the scanning resolution (neither the dimensions, nor the longer in a less dense cloud.

6.4. Changes in density and viewpoint

Table 8 shows the results of the alignment of different
clouds acquired by the same sensor and with viewpoint

Table 7: Density change results.

Resolution cloud 1 1.6 3.1 6.3 12.5

(mm at 10 m)
Resolution could 2 25 25 25 25
(mm at 10 m)
Resolution ratio 1/16 1/8 1/4 1/2
RM SE (m) 0.0027 0.0027 0.0027 0.0027

Figure 10: Cost function evolution for the unstructured environment. Iteration 3 6 7 10

17
 (a) Google earth top view (b) Site front view (c) Initial state of two scans (d) ICP

(e) Pt2pl ICP (f) NDT (g) GICP (h) CICP

Figure 11: Results with a semi structured environment. Registration between a Leica P20 scan and the Velodyne HDL32-E is performed
using state-of-the-art algorithms and CICP.

Table 8: Density & viewpoint change results.

Resolution cloud 0.8 1.6 3.1 6.3 12.5 25

1 (mm at 10 m)
# points 23 870 726 9 800 513 2 527 043 1 219 973 673 537 65 626
of cloud1
Resolution cloud 50 50 50 50 50 50
2 (mm at 10 m)
(a) Density change vs accuracy (b) Density change vs conver- # points 20 676 20 676 20 676 20 676 20 676 20 676
for office environment gence for office environment of cloud 2
Ratio 1/1150 1/470 1/120 1/60 1/32 1/3
of points
RM SE (m) 0.0060 0.0065 0.0073 0.0082 0.0107 0.0166
# iteration 40 44 46 50 58 64

kinds of sensors, the Leica P20 LiDAR, Velodyne HDL32-

(c) Density change vs accuracy (d) Density change vs conver- E LiDAR and SR4000 Time-of-Flight camera. Figure 14
for PAVIN environment gence for PAVIN environment shows these different sensors and Table 9 exhibits their
hardware specifications.
Figure 12: Density change effects on the convergence and accuracy
of the CICP method.
6.5.1. Leica P20
Many varieties of 3D LiDAR sensors are available on
change, which was [50, 50, 50, 5, 0, 0]. Our main motivation the market, but they all work with the same basic prin-
to conduct this experiment, which differs from the previous ciple [79]. They emit pulses and detect their reflection in
one by the addition of the view change, is to confirm the order to explore the object or the environment. Leica P20
influence of the density change as the scanned pattern is is a Time-of-Flight scanner which offers greater range and
the same. precision.
Findings from this second experiment provide further
evidence about the conclusions drawn earlier and the role 6.5.2. Velodyne HDL32-E
of density. Indeed, as we explained previously, high density The Velodyne HDL-32 produces 3D scans by rotat-
gives rise to superior accuracy and improves the conver- ing a 32-beam array around its vertical axis at 10 Hz.
gence speed (this can be seen on the sixth and seventh It produces approximately 700 000 points per second or
rows of the Table 8). 2200 points per laser beam at a range of 1 through 70
meters. This sensor provides an angular resolution of ap-
6.5. Comparison with Various Sensors proximately 0.16◦ with a field of view (FOV) of 360◦ . Its
To investigate the potential of our approach to align vertical field of view is from −30.67◦ to +10.67◦ with an
point clouds from different sensors, we test it with three

18
 (a) 0.8 mm at 10 m (b) 1.6 mm at 10 m (c) 3.1 mm at 10 m (d) 6.3 mm at 10 m

(e) 12.5 mm at 10 m (f) 25 mm at 10 m (g) 50 mm at 10 m

Figure 13: Density change within the same sensor (the colors from blue to red correspond to the scale of the intensity of the reflectance).

Figure 14: Sensors used to test the CICP approach. From left to Figure 15: Registration of two clouds captured by Leica P20 LiDAR
right: SR4000 Time-of-Flight camera, Velodyne HDL 32-E, and Le- for the dense cloud and Velodyne HDL-32E for the sparser cloud.
ica P20 Laser.

Table 9: Sensors hardware specifications. 6.5.5. Leica P20 vs SR4000

Here, the sparse cloud is produced by the SR4000 Time-
Sensor Range Field of view (◦ ) Scanning Accuracy
(m) Horizontal Vertical Frequency (Hz) (mm) of-Flight camera, while the dense cloud is produced by the
Leica P20 [0.5 . . . 120] 360 270 50 3
Leica P20 sensor (Figure 16). The latter is less affected by
Velodyne [1 . . . 70] 360 [−30 ± 10] 10 20 noise than the former.
HDL-32E
SR4000 [0.1 . . . 10] 43.6 34.6 50 15
The results of this convergence are 100 and 0.0203 for
the number of iterations and the RM SE, respectively.
The reader can observe that the RM SE of P20 vs SR4000
angular resolution of 1.33◦ . Its measurement accuracy is experiment is higher than the RM SE of the test per-
generally less than 2 cm. formed between P20 and Velodyne sensors. This is jus-
tified by the large presence of noise in the cloud delivered
6.5.3. SR4000 Time of Flight camera by the ToF camera.
The Time of Flight (ToF) camera is a two-dimensional
scanner which captures full depth per frame and with a
single light pulse.
The rest of this section discusses the registration of
different clouds acquired by these sensors.

6.5.4. Leica P20 vs Velodyne

In this experiment, the point cloud produced by the
Velodyne sensor is the sparse cloud and the second cloud,
which represents the dense one, is produced by the LiDAR
Leica P20 (Figure 15). The algorithm converges after 64 Figure 16: Registration of two clouds captured by Leica P20 LiDAR
iterations, with 0.0199 mm as the RM SE value. for the dense cloud and SR4000 ToF camera for the sparser cloud.

19
6.5.6. SR4000 vs Velodyne of points used for matching. As clustering generates a few
Here the two sensors are affected by noise. This ex- representative points relative to the input points, thereby
plains why the registration takes more than 124 iterations increasing the convergence speed, but decreasing the accu-
to converge. The final RM SE is about 0.0231. In contrast racy. Setting a small voxel size decreases the convergence
to the previous experiment, the dense cloud is produced speed, but increases accuracy, due to the availability of
by the ToF camera, while the Velodyne cloud represents sufficient points for matching. Therefore, a reliable trade
the sparse one. Even though the total number of points off needs to be determined in order to find the optimal
in the Velodyne cloud is almost 3 times higher than the discretization of the point cloud.
cloud ToF, in the part that interests us (representing ap-
proximately (2 × 1 × 1) m3 , which enclose the table and
objects exposed on it and the table behind), the ToF cloud
is denser than the cloud of the Velodyne (Figure 17).

Figure 17: Registration of two clouds captured by a ToF camera for

the dense cloud and a Velodyne for the sparse cloud.

Figure 19: Clustering voxel size effect on registration accuracy and

6.6. Demonstration with Dense-to-Dense Data convergence speed.
At the end of this experimental section, we wish to
highlight that the CICP method can be used with clouds
of the same nature (dense to dense or sparse to sparse). 7. Discussion
Figure 18 shows the state of the two dense clouds be-
fore and after the registration. Despite the large number 1. Two or more point clouds are acquired from the
of points, the final result is correctly aligned. Here is an- same scene but with different sensors (e.g. vision
other benefit of our approach, the fact of not considering based system producing sparse cloud and 3D LiDAR
the entire set of points for matching, but only a collected producing dense cloud), leading to different clouds
set of points from each local surface, which improves the with their own local coordinate system, resolution
convergence speed. and number of points. Registration methods based
on the classical point-to-point ICP metrics fail to
6.7. Impact of the voxel size provide an accurate pose estimate because of the
The voxel size plays a very important role in the con- large discrepancies in density between the two point
vergence of the algorithm. Figure 19 illustrates how the clouds. The difficulty lies in the fact that there are
convergence is impacted by the change of this parameter. no direct correspondences between the source and
When increasing the voxel size, the number of points in- the target point clouds. What is more, methods
cluded in this voxel is increased, which reduces the number based on the geometric characteristics are also un-
suitable as these (mainly normal and curvature [70]),
which are essentially based on their estimations on
the neighboring points, are affected by the change in
resolution and scanning patterns [45, 13].
2. The information carried by a 3D point can contain
color, reflectance (intensity), positions, normals and
curvatures. Whilst color or reflectance differs from
one sensor to another, for the case of a static en-
vironment, the global geometric aspect of the scene
remains unchanged. This is obviously, why CICP
capitalizes on geometric primitives. In addition, this
Figure 18: Registration of two dense clouds of an indoor scene cap- makes it independent of weather and illumination
tured by a Leica P20 sensor.
conditions.
20
3. It complements outperforms the famous the state- at reduced computational cost.
of-the-art methods (ICP, NDT, GICP) for this kind
of multi sensors applications. These classical algo-
8. Conclusion
rithms find their limits with dense-sparse registra-
tion, because they are all based on point-to-point In this paper, a novel approach for sparse to dense point
matching. Whereas our method is based on the con- cloud registration has been presented. The traditional ICP
cept of surface-to-surface matching, this concept can pipeline is modified to accommodate a smarter way of sur-
be generalized to any type of common clustering bet- face patch correspondence consisting of three main blocks;
ween the two point clouds. We can imagine the use voxelization, clustering, representative election. The moti-
of segments or dominant direction of variation of the vation behind this strategy is to match identical surfaces in
points which corresponds to the highest eigenvalue in the 3D world but scanned using different type of depth sen-
the PCA. The matched surfaces are chosen to be very sors. With various sensors come different resolutions and
small local surfaces. To do that, a common voxeliza- hence different point cloud representations. Throughout
tion between the two clouds with a very small voxel our experimental phase, we demonstrate the efficiency of
size is performed. The choice of matching small sur- our algorithm in terms of alignment accuracy where other
faces was made in order to preserve the topological state-of-the-art techniques perform poorly since they do
details of the scanned environment and to guarantee not cater for these above-mentioned differences. Further-
a considerable number of matched points between more, we show that the alignment technique works per-
the two clouds. fectly even by changing the density of the points of the
4. Normals are computed once before starting the pro- two clouds.
cess and are used only to distinguish the different To summarize, our proposed methodology provides the
local surfaces. They are not used in the alignment following improvements:
process. The use of surface normals in point to plane
ICP and its variants is motivated by the fact that • patch surface segmentation contributing to noise re-
the former are robustly estimated in the presence of duction,
noisy surfaces. Otherwise, they will cause ICP to • improved selection by a novel surface point represen-
diverge. In the case of CICP, we make use of nor- tative approach,
mals to perform surface segmentation. Such an ap-
proach improves the surface estimate for noisy mea- • reduced amount of processed data during matching
surements. Figure 2 shows normal vectors extracted phase,
from the same surface scanned by two distinct sen-
sors. It can be clearly observed that though the • dense to sparse registration applicable to the various
surfaces are piecewise planar, their estimated nor- depth sensors on the market,
mals do not correspond. Therefore, this reinforces • a novel mathematical definition of sparse and dense
the idea of not retaining the normals for the opti- clouds.
mization phase.
5. Whether in the case of dense or sparse cloud, there is Our algorithm has been successfully tested on various
a certain number of points that are redundant. Re- indoor an outdoor datasets. Our future work will be con-
dundancy, though useful to make robust the overde- centrated on the notion of uncertainty in the model to
termined system of the normal equation comes at the probabilistically incorporate a fusion stage in our pipeline.
cost of increased computation. Therefore, the use of Furthermore, on top of normal feature extraction, other
representative subsets of points give the same if not primitives might also be considered such as surface patch
better accuracy with less computational time. curvatures or their relative intensity (if available) to be
6. As shown in this paper, the use of normals is a able to perform a robust check on the matching and cor-
double-edged sword and it can guarantee good qual- respondence phase. Finally, the scope of this paper lies
ity results if accurately exploited. In the same way, within the generic problem of localization for either hand-
normals can amplify noise leading to divergence of held applications for augmented or virtual reality or robotic
the ICP method. The proposed approach makes use platforms navigating inside an a priori mapped environ-
of normals for clustering points from the same sur- ment. The latter is among potential applications of our ap-
face, and we avoid using them to establish corre- proach, since CICP is suitable to localize a vehicle equipped
spondences due to various disturbances coming from with a sensor that provides sparse data (e.g. Velodyne) in
the sensor. This strategy enables us to overcome the a dense and accurate map. In this perspective, an effort
main weakness of dense to sparse/sparse to dense towards code optimization will therefore be considered.
registration. It allows us to surpass the problem of
density in search of correspondences. The cascaded Disclosure statement
effects of an improved surface match correspondence
lead to better accuracy in the registration pipeline “The authors declare no conflict of interest.”
21
Table 10: Variation only of rotation around one axis: θz . The second column represents the actual registration parameters and the third one
represents the CICP estimated values.

θz (◦ ) Actual Final state Iterations RM SE

5 [0, 0, 0, 0, 0, 5] [0.00 , 0.01 , 0.00 , 0.03 , 0.05 , 4.73] 64 0.0197
10 [0, 0, 0, 0, 0, 10] [0.00 , 0.01 , 0.00 , 0.01 , 0.15 , 10.01] 99 0.0196
15 [0, 0, 0, 0, 0, 15] [0.00 , 0.00 , 0.00 , 0.62 , 0.13 , 14.68] 95 0.0198
20 [0, 0, 0, 0, 0, 20] [0.00 , 0.01 , 0.01 , 0.65 , 0.10 , 19.71] 157 0.0197
25 [0, 0, 0, 0, 0, 25] [0.00 , 0.00 , 0.00 , 0.79 , 0.25 , 24.64] 220 0.0196
30 [0, 0, 0, 0, 0, 30] [0.00 , 0.00 , 0.03 , 1.85 , 0.12 , 29.59] 303 0.022
35 [0, 0, 0, 0, 0, 35] [0.05 , 0.03 , 0.01 , 0.95 , 0.14 , 36.05] 418 0.0221
40 [0, 0, 0, 0, 0, 40] [0.06 , 0.03 , 0.01 , 0.85 , 0.10 , 40.64] 547 0.0215

Table 11: Variation only of rotation around two axes.

θy (◦ ) θz (◦ ) Initial Final state Iteration RM SE

15 10 [0, 0, 0, 0, 15, 10] [0.00 , 0.00 , −0.01 , 0.09 , 14.59 , 9.62] 83 0.0183
15 15 [0, 0, 0, 0, 15, 15] [0.00 , 0.00 , −0.01 , 0.50 , 15.03 , 14.71] 140 0.0188
15 25 [0, 0, 0, 0, 15, 25] [0.00 , 0.00 , 0.00 , 0.65 , 15.00 , 24.92] 387 0.0184
20 15 [0, 0, 0, 0, 20, 15] [0.00 , 0.00 , 0.01 , 0.89 , 19.49 , 14.91] 167 0.0184
20 25 [0, 0, 0, 0, 20, 25] [0.00 , 0.00 , −0.01 , 0.82 , 19.61 , 24.68] 337 0.0179
30 20 [0, 0, 0, 0, 30, 20] [0.01 , 0.00 , 0.02 , 0.93 , 28.81 , 19.30] 394 0.01807
30 25 [0, 0, 0, 0, 30, 25] [0.01 , 0.00 , 0.03 , 0.91 , 28.58 , 24.68] 616 0.01807

Table 12: Variation only of the translation along one axis: ty .

tz (cm) Actual Final state Iteration RM SE

10 [0, 0.10 , 0, 0, 0, 0] [0.00 , 0.10 , 0.00 , 0.23 , −0.33 , −0.08] 25 0.0204
20 [0, 0.20, 0, 0, 0, 0] [0.00 , 0.20 , 0.00 , 0.30 , 0.04 , −0.42] 27 0.0205
30 [0, 0.30 , 0, 0, 0, 0] [0.00 , 0.30 , 0.00 , 0.02 , 0.27 , 0.78] 35 0.0202
40 [0, 0.40 , 0, 0, 0, 0] [0.00 , 0.41 , 0.00 , 0.18 , 0.26 , 0.77] 41 0.0205
50 [0, 0.50 , 0, 0, 0, 0] [0.00 , 0.51 , 0.01 , 0.37 , 0.42 , 0.90] 65 0.0205
60 [0, 0.60 , 0, 0, 0, 0] [0.00 , 0.61 , 0.01 , 0.51 , 0.41 , −0.59] 94 0.0228
70 [0, 0.70 , 0, 0, 0, 0] [0.00 , 0.71 , 0.01 , 0.37 , 0.39 , −0.01] 117 0.0217
80 [0, 0.80 , 0, 0, 0, 0] [0.00 , 0.80 , 0.01 , 0.19 , 0.48 , −0.32] 145 0.0211
90 [0, 0.90 , 0, 0, 0, 0] [0.00 , 0.91 , 0.01 , 0.09 , 0.46 , 0.75] 160 0.0213
100 [0, 1.00 , 0, 0, 0, 0] [0.00 , 1.01 , 0.00 , 0.09 , 0.30 , 0.05] 215 0.0212
120 [0, 1.20 , 0, 0, 0, 0] [0.00 , 1.20 , 0.00 , 0.04 , −0.04 , 0.89] 278 0.0198
150 [0, 1.50 , 0, 0, 0, 0] [0.00 , 1.51 , 0.00 , 0.20 , 0.33 , 0.76] 386 0.0209
170 [0, 1.70 , 0, 0, 0, 3] [0.00 , 1.70 , 0.01 , 0.11 , 0.22 , 0.16] 477 0.0196
200 [0, 2.00 , 0, 0, 0, 4] [−0.01 , 2.00 , 0.01 , 0.26 , 0.09 , 0.83] 618 0.0201

References in: IEEE/RSJ International Conference on Intelligent Robots

and Systems (IROS), 2016, pp. 4092–4098.
[1] G. Agamennoni, S. Fontana, R. Y. Siegwart, D. G. Sorrenti, [2] B. Bellekens, V. Spruyt, R. B. Maarten Weyn, A survey of rigid
Point Clouds Registration with Probabilistic Data Association, 3D pointcloud registration algorithms, in: Fourth International

22
 Table 13: Variation only of the translation along two axes: tx and ty .

tx (cm) ty (cm) Actual Final state Iteration RM SE

30 30 [0.30 , 0.30 , 0, 0, 0, 0] [0.30 , 0.31 , 0.00 , 0.17 , 0.46 , 0.36] 33 0.0186
30 50 [0.30 , 0.50 , 0, 0, 0, 0] [0.31 , 0.51 , 0.00 , 0.53 , 0.48 , 0.41] 67 0.0198
30 70 [0.30 , 0.70 , 0, 0, 0, 0] [0.31 , 0.71 , 0.01 , 0.54 , 0.49 , 0.41] 92 0.0198
30 100 [0.30 , 1.00 , 0, 0, 0, 0] [0.32 , 1.01 , 0.02 , 0.55 , 0.49 , 0.41] 166 0.0198
50 50 [0.50 , 0.50 , 0, 0, 0, 0] [0.50 , 0.51 , 0.01 , 0.35 , 0.53 , 0.55] 87 0.0204
50 70 [0.50 , 0.70 , 0, 0, 0, 0] [0.50 , 0.71 , 0.00 , 0.36 , 0.53 , 0.55] 108 0.0204
50 100 [0.50 , 1.00 , 0, 0, 0, 0] [0.51 , 1.01 , 0.01 , 0.37 , 0.54 , 0.55] 185 0.0205
50 150 [0.50 , 1.50 , 0, 0, 0, 0] [0.51 , 1.51 , 0.02 , 0.39 , 0.54 , 0.55] 298 0.0204
100 100 [1.00 , 1.00 , 0, 0, 0, 0] [0.99 , 0.99 , 0.01 , 0.66 , 0.46 , 0.26] 262 0.0199
100 120 [1.00 , 1.20 , 0, 0, 0, 0] [1.00 , 1.19 , 0.01 , 0.72 , 0.50 , 0.26] 309 0.0200
100 150 [1.00 , 1.50 , 0, 0, 0, 0] [1.00 , 1.49 , 0.00 , 0.72 , 0.50 , 0.26] 450 0.0199
100 200 [1.00 , 2.00 , 0, 0, 0, 0] [1.00 , 1.99 , 0.00 , 0.72 , 0.50 , 0.26] 590 0.0200

Table 14: Variation only of the translation along three axes: tx , ty and tz .

tx (cm) ty (cm) tz (cm) Actual Final state Iteration RM SE

30 30 30 [0.30 , 0.30 , 0.30 , 0, 0, 0] [0.31 , 0.31 , 0.29 , 0.50 , 0.47 , 0.42] 74 0.0197
50 50 50 [0.50 , 0.50 , 0.50 , 0, 0, 0] [0.50 , 0.52 , 0.50 , 0.37 , 0.47 , 0.55] 124 0.0205
100 100 100 [1.00 , 1.00 , 1.00 , 0, 0, 0] [1.00 , 1.01 , 1.00 , 0.64 , 0.44 , 0.27] 408 0.0200

Table 15: Variation of translation along one axis (ty ) and rotation around one axis (θz ).

ty (cm) θz (◦ ) Actual Final state Iteration RM SE

30 20 [0.00 , 0.30 , 0, 0, 0, 0.20] [0.00 , 0.30 , 0.00 , 0.77 , 0.07 , 19.50] 85 0.0199
30 30 [0.00 , 0.30 , 0, 0, 0, 0.30] [0.00 , 0.29 , 0.00 , 0.46 , 0.16 , 29.30] 246 0.0200
50 20 [0.00 , 0.50 , 0, 0, 0, 0.20] [0.00 , 0.50 , 0.01 , 0.05 , 0.08 , 20.12] 98 0.0206
50 30 [0.00 , 0.50 , 0, 0, 0, 0.30] [0.00 , 0.50 , 0.00 , 0.48 , 0.21 , 29.92] 350 0.0203
100 20 [0.00 , 1.00 , 0, 0, 0, 0.20] [0.00 , 1.01 , 0.01 , 0.74 , 0.10 , 20.85] 186 0.0206
100 25 [0.00 , 1.00 , 0, 0, 0, 0.25] [0.00 , 1.01 , 0.01 , 0.67 , 0.16 , 25.89] 227 0.0197
150 20 [0.00 , 1.50 , 0, 0, 0, 0.20] [0.00 , 1.50 , 0.01 , 0.87 , 0.17 , 21.09] 473 0.0204

Conference on Ambient Computing, Applications, Services and Conference on Intelligent Robots and Systems, 2013, pp. 4347–
Technologies, Proceedings, IARA, 2014, pp. 8–13. 4354.
[3] F. Pomerleau, F. Colas, R. Siegwart, A Review of Point Cloud [8] A. Patidar, P. Sanjiv, A Review Paper on Self - Driving Car ’ s
Registration Algorithms for Mobile Robotics, Foundations and and its Applications, in: National Conference on Innovations in
Trends in Robotics 4 (1-104) (2015) 1–104. Micro-electronics, Signal Processing and Communication Tech-
[4] M. Velas, M. Spanel, A. Herout, Collar Line Segments for fast nologies IJIRST, 2016, pp. 33–35.
odometry estimation from Velodyne point clouds, in: IEEE In- [9] S. Thrun, M. Montemerlo, H. Dahlkamp, D. Stavens, A. Aron,
ternational Conference on Robotics and Automation (ICRA), J. Diebel, P. Fong, J. Gale, M. Halpenny, G. Hoffmann, K. Lau,
2016, pp. 4486–4495. C. Oakley, M. Palatucci, V. Pratt, P. Stang, S. Strohband,
[5] T. Zhang, Surface Reconstruction with Sparse Point Clouds of C. Dupont, L.-E. Jendrossek, C. Koelen, C. Markey, C. Rum-
Velodyne Sensor, in: The 14th IFToMM World Congress, 2015. mel, J. van Niekerk, E. Jensen, P. Alessandrini, G. Bradski,
[6] J. Razlaw, D. Droeschel, D. Holz, S. Behnke, Evaluation of reg- B. Davies, S. Ettinger, A. Kaehler, A. Nefian, P. Mahoney,
istration methods for sparse 3d laser scans, in: Mobile Robots Stanley: The Robot That Won the DARPA Grand Challenge,
(ECMR), 2015 European Conference on, IEEE, 2015, pp. 1–7. Springer Berlin Heidelberg, Berlin, Heidelberg, 2007, Ch. 1, pp.
[7] W. S. Grant, R. C. Voorhies, L. Itti, Finding planes in LiDAR 1–43. doi:10.1007/978-3-540-73429-1_1.
point clouds for real-time registration, in: IEEE International URL http://dx.doi.org/10.1007/978-3-540-73429-1_1

23
[10] R. W. Wolcott, R. M. Eustice, Visual localization within LI- 2571.
DAR maps for automated urban driving, in: Intelligent Robots [31] B. Steder, R. B. Rusu, K. Konolige, W. Burgard, Point Fea-
and Systems (IROS 2014), 2014 IEEE/RSJ International Con- ture Extraction on 3D Range Scans Taking into Account Ob-
ference on, IEEE, 2014, pp. 176–183. ject Boundaries Bastian, in: IEEE International Conference on
[11] T. Caselitz, B. Steder, M. Ruhnke, W. Burgard, Monocular Robotics and Automation (ICRA),, 2011, pp. 2601–2608.
Camera Localization in 3D LiDAR Maps, in: IEEE/RSJ Inter- [32] R. B. Rusu, N. Blodow, Z. C. Marton, M. Beetz, Aligning
national Conference on Intelligent Robots and Systems (IROS), Point Cloud Views using Persistent Feature Histograms, in:
2016, pp. 1–6. IEEE/RSJ International Conference on Intelligent Robots and
[12] W. Maddern, P. Newman, Real-time probabilistic fusion of Systems, IROS, 2008, pp. 3384–3391.
sparse 3d lidar and dense stereo, in: Intelligent Robots and [33] R. B. Rusu, N. Blodow, M. Beetz, Fast Point Feature His-
Systems (IROS), 2016 IEEE/RSJ International Conference on, tograms (FPFH) for 3D registration, in: IEEE International
IEEE, 2016, pp. 2181–2188. Conference on Robotics and Automation, 2009, pp. 3212–3217.
[13] D. Holz, A. E. Ichim, F. Tombari, R. B. Rusu, S. Behnke, Reg- [34] R. B. Rusu, G. Bradski, R. Thibaux, J. Hsu, Fast 3d recogni-
istration with the Point Cloud Library PCL, IEEE Robotics & tion and pose using the viewpoint feature histogram, in: Intelli-
Automation Magazine 22 (4) (2015) 1–13. gent Robots and Systems (IROS), 2010 IEEE/RSJ International
[14] Y. Chen, G. Medioni, Object modeling by registration of mul- Conference on, IEEE, 2010, pp. 2155–2162.
tiple range images, in: IEEE International Conference on [35] A. Aldoma, M. Vincze, N. Blodow, D. Gossow, S. Gedikli, R. B.
Robotics and Automation, 1991, pp. 2724–2729. Rusu, G. Bradski, Cad-model recognition and 6dof pose esti-
[15] P. J. Besl, N. D. McKay, A Method for Registration of 3-D mation using 3d cues, in: Computer Vision Workshops (ICCV
Shapes, IEEE Trans. Pattern Anal. Mach. Intell. 14 (2) (1992) Workshops), 2011 IEEE International Conference on, IEEE,
239–256. 2011, pp. 585–592.
[16] P. Markelj, D. Tomaževič, B. Likar, F. Pernuš, A review of [36] I. T. Jolliffe, Principal Component Analysis for Special Types
3D/2D registration methods for image-guided interventions, of Data, Springer New York, New York, NY, 1986, Ch. 11, pp.
Medical image analysis 16 (3) (2012) 642–661. 199–222.
[17] J. Salvi, C. Matabosch, D. Fofi, J. Forest, A review of recent [37] Y. Zhong, Intrinsic shape signatures: A shape descriptor for
range image registration methods with accuracy evaluation, Im- 3d object recognition, in: Computer Vision Workshops (ICCV
age and Vision computing 25 (5) (2007) 578–596. Workshops), 2009 IEEE 12th International Conference on,
[18] F. Pomerleau, Applied registration for robotics, Ph.D. thesis, IEEE, 2009, pp. 689–696.
ETH ZURICH (2013). [38] A. Mian, M. Bennamoun, R. Owens, On the repeatability and
[19] R. Fabio, From point cloud to surface: the modeling and visual- quality of keypoints for local feature-based 3d object retrieval
ization problem, International Archives of the Photogrammetry, from cluttered scenes, International Journal of Computer Vision
Remote Sensing and Spatial Information Sciences 34 (2003) 11. 89 (2) (2010) 348–361.
[20] R. Marani, V. Reno, M. Nitti, T. D’Orazio, E. Stella, A modified [39] T. Weber, R. Hänsch, O. Hellwich, Automatic registration of
iterative closest point algorithm for 3D point cloud registration, unordered point clouds acquired by Kinect sensors using an
Computer-Aided Civil and Infrastructure Engineering 31 (7) overlap heuristic, ISPRS Journal of Photogrammetry and Re-
(2016) 515–534. mote Sensing 102 (2015) 96–109.
[21] J. Serafin, E. Olson, G. Grisetti, Fast and Robust 3D Feature [40] J. Serafin, G. Grisetti, NICP: Dense normal based point cloud
Extraction from Sparse Point Clouds, in: IEEE/RSJ Interna- registration, in: IEEE International Conference on Intelligent
tional Conference on Intelligent Robots and Systems (IROS), Robots and Systems, Vol. 2015-Decem, 2015, pp. 742–749.
2016, pp. 4105–4112. [41] J. Yang, H. Li, Y. Jia, Go-icp: Solving 3d registration efficiently
[22] C. M. Costa, H. M. Sobreira, A. J. Sousa, G. M. Veiga, Ro- and globally optimally, in: Proceedings of the IEEE Interna-
bust 3/6 DoF self-localization system with selective map update tional Conference on Computer Vision, 2013, pp. 1457–1464.
for mobile robot platforms, Robotics and Autonomous Systems [42] B. Yang, Z. Dong, F. Liang, Y. Liu, Automatic registration of
76 (C) (2016) 113–140. large-scale urban scene point clouds based on semantic feature
[23] Y. Feng, A. Schlichting, C. Brenner, 3D feature point extrac- points, ISPRS Journal of Photogrammetry and Remote Sensing
tion from LiDAR data using a neural network, International 113 (2016) 43–58.
Archives of the Photogrammetry, Remote Sensing and Spatial [43] C. Cadena, L. Carlone, H. Carrillo, Y. Latif, D. Scaramuzza,
Information Sciences-ISPRS Archives 41 (2016) 41 (2016) 563– J. Neira, I. Reid, J. Leonard, Past, Present, and Future of Si-
569. multaneous Localization And Mapping: Towards the Robust-
[24] S. Filipe, L. A. Alexandre, A comparative evaluation of 3D key- Perception Age, IEEE Transactions on Robotics 32 (6) (2016)
point detectors in a RGB-D object dataset, in: Computer Vision 13091332.
Theory and Applications (VISAPP), 2014 International Confer- [44] J. Nieto, T. Bailey, E. Nebot, Scan-SLAM: Combining EKF-
ence on, Vol. 1, IEEE, 2014, pp. 476–483. SLAM and scan correlation, Springer Tracts in Advanced
[25] D. G. Lowe, Object recognition from local scale-invariant fea- Robotics 25 (2006) 167–178.
tures, in: Proceedings of the Seventh IEEE International Con- [45] A. Das, M. Diu, N. Mathew, C. Scharfenberger, J. Servos,
ference on Computer Vision, 1999, pp. 1150–1157. A. Wong, J. S. Zelek, D. A. Clausi, S. L. Waslander, Map-
[26] R. B. Rusu, S. Cousins, 3D is here: point cloud library, in: IEEE ping, planning, and sample detection strategies for autonomous
International Conference on Robotics and Automation (ICRA), exploration, Journal of Field Robotics 31 (1) (2014) 75–106.
2011, pp. 1 – 4. [46] M. Magnusson, A. Lilienthal, T. Duckett, Scan registration for
[27] R. Hänsch, T. Weber, O. Hellwich, Comparison of 3D interest autonomous mining vehicles using 3D-NDT, Journal of Field
point detectors and descriptors for point cloud fusion, ISPRS Robotics 24 (10) (2007) 803–827.
Annals of Photogrammetry, Remote Sensing and Spatial Infor- [47] A. Segal, D. Haehnel, S. Thrun, Generalized-ICP, in: Robotics:
mation Sciences II-3 (September) (2014) 57–64. Science and Systems, Vol. 5, 2009, pp. 168–176.
[28] E. Rosten, T. Drummond, Machine learning for high-speed cor- [48] F. Pomerleau, F. Colas, R. Siegwart, S. Magnenat, Comparing
ner detection, Computer Vision–ECCV 2006 (2006) 430–443. icp variants on real-world data sets, Autonomous Robots 34 (3)
[29] H. Bay, A. Ess, T. Tuytelaars, L. Van Gool, Speeded-up ro- (2013) 133–148.
bust features (surf), Computer vision and image understanding [49] R. Dubé, D. Dugas, E. Stumm, J. Nieto, R. Siegwart, C. Ca-
110 (3) (2008) 346–359. dena, Segmatch: Segment based loop-closure for 3d point
[30] E. Rublee, V. Rabaud, K. Konolige, G. Bradski, Orb: An effi- clouds, arXiv preprint arXiv:1609.07720.
cient alternative to sift or surf, in: Computer Vision (ICCV), [50] R. F. Salas-Moreno, R. A. Newcombe, H. Strasdat, P. H. Kelly,
2011 IEEE international conference on, IEEE, 2011, pp. 2564– A. J. Davison, Slam++: Simultaneous localisation and mapping

24
 at the level of objects, in: Proceedings of the IEEE Conference tions, in: Robotics and Automation, 2009. ICRA’09. IEEE In-
on Computer Vision and Pattern Recognition, 2013, pp. 1352– ternational Conference on, IEEE, 2009, pp. 3206–3211.
1359. [72] M. L. Tazir, P. Checchin, L. Trassoudaine, Color-based 3D Point
[51] E. Fernández-Moral, P. Rives, V. Arévalo, J. González-Jiménez, Cloud Reduction, in: the 14th International Conference on Con-
Scene structure registration for localization and mapping, in: trol, Automation, Robotics and Vision, ICARCV, 2016, pp. 1–7.
Robotics and Autonomous Systems, 2016, pp. 649–660. [73] T. Wiemann, M. Mrozinski, D. Feldschnieders, K. Lingemann,
[52] E. Fernández-Moral, W. Mayol-Cuevas, V. Arévalo, J. Hertzberg, Data Handling in Large-Scale Surface Recon-
J. Gonzalez-Jimenez, Fast place recognition with plane- struction, in: 13th International Conference on Intelligent Au-
based maps, in: Robotics and Automation (ICRA), 2013 IEEE tonomous Systems, 2014, pp. 1–12.
International Conference on, IEEE, 2013, pp. 2719–2724. [74] D. Arthur, S. Vassilvitskii, K-means++: The Advantages of
[53] K. Georgiev, R. T. Creed, R. Lakaemper, Fast plane extraction Careful Seeding, in: Proceedings of the Eighteenth Annual
in 3d range data based on line segments, in: Intelligent Robots ACM-SIAM Symposium on Discrete Algorithms, SODA ’07,
and Systems (IROS), 2011 IEEE/RSJ International Conference Society for Industrial and Applied Mathematics, Philadelphia,
on, IEEE, 2011, pp. 3808–3815. PA, USA, 2007, pp. 1027–1035.
[54] M. A. Fischler, R. C. Bolles, Random sample consensus: a [75] R. Tibshirani, G. Walther, T. Hastie, Estimating the number of
paradigm for model fitting with applications to image analy- clusters in a data set via the gap statistic, Journal of the Royal
sis and automated cartography, Communications of the ACM Statistical Society: Series B (Statistical Methodology) 63 (2)
24 (6) (1981) 381–395. (2001) 411–423.
[55] J. Forsberg, U. Larsson, A. Wernersson, Mobile robot naviga- [76] P. J. Huber, E. M. Ronchetti, Robust statistics, John Wiley &
tion using the range-weighted hough transform, IEEE Robotics Sons, 2009.
& Automation Magazine 2 (1) (1995) 18–26. [77] P. J. Huber, Robust statistics, Wiley, New York, New York,
[56] G. Pandey, S. Savarese, J. R. McBride, R. M. Eustice, Visu- 1981, Ch. The Basic Types of Estimates, pp. 45–67.
ally bootstrapped generalized icp, in: Robotics and Automation [78] A. Bjorck, Numerical methods for least squares problems, Siam,
(ICRA), 2011 IEEE International Conference on, IEEE, 2011, 1996.
pp. 2660–2667. [79] E. Puttonen, M. Lehtomäki, H. Kaartinen, L. Zhu, A. Kukko,
[57] K. Lenac, A. Kitanov, R. Cupec, I. Petrović, Fast planar sur- A. Jaakkola, Improved sampling for terrestrial and mobile laser
face 3d slam using lidar, Robotics and Autonomous Systems 92 scanner point cloud data, Remote Sensing 5 (4) (2013) 1754–
(2017) 197–220. 1773.
[58] N. Mellado, D. Aiger, N. J. Mitra, Super 4PCS fast global point-
cloud registration via smart indexing, Computer Graphics Fo-
rum 33 (5) (2014) 205–215.
[59] A. Zeng, S. Song, M. Nießner, M. Fisher, J. Xiao,
T. Funkhouser, 3dmatch: Learning the matching of local 3d
geometry in range scans, arXiv 1603.
[60] M. Li, X. Gao, L. Wang, G. Li, Automatic registration of laser-
scanned point clouds based on planar features, in: 2nd ISPRS
International Conference on Computer Vision in Remote Sens-
ing (CVRS 2015), Vol. 9901, International Society for Optics
and Photonics, 2016, p. 990103.
[61] Toward Object-based Place Recognition in Dense RGB-D Maps.
[62] A. Sehgal, D. Cernea, M. Makaveeva, Real-time scale invariant
3d range point cloud registration, in: International Conference
Image Analysis and Recognition, Springer, 2010, pp. 220–229.
[63] S. Rusinkiewicz, M. Levoy, Efficient variants of the ICP algo-
rithm, in: Proceedings of International Conference on 3-D Dig-
ital Imaging and Modeling, 3DIM, 2001, pp. 145–152.
[64] A. Gressin, C. Mallet, N. David, Improving 3D Lidar Point
Cloud Registration Using Optimal Neighborhood Knowledge,
ISPRS Annals of Photogrammetry, Remote Sensing and Spatial
Information Sciences I-3 (September) (2012) 111–116.
[65] Z. Zhang, Iterative point matching for registration of free-form
curves and surfaces, International journal of computer vision
13 (2) (1994) 119–152.
[66] J. S. Vitter, Faster Methods for Random Sampling, Commun.
ACM 27 (7) (1984) 703–718.
[67] R. B. Rusu, Semantic 3D object maps for everyday manipu-
lation in human living environments, Ph.D. thesis, Technische
Universitt Mnchen, Diss., 2009 (2009).
[68] F. Donoso, K. Austin, P. McAree, How do ICP variants perform
when used for scan matching terrain point clouds?, Robotics
and Autonomous Systems 87 (2017) 147 – 161.
[69] J. Elseberg, S. Magnenat, R. Siegwart, N. Andreas, Comparison
of nearest-neighbor-search strategies and implementations for
efficient shape registration, Journal of Software Engineering for
Robotics (JOSER) 3 (1) (2012) 2–12.
[70] L. He, X. Wang, H. Zhang, M2DP: A novel 3D point cloud
descriptor and its application in loop closure detection, in: In-
telligent Robots and Systems (IROS), 2016 IEEE/RSJ Interna-
tional Conference on, IEEE, 2016, pp. 231–237.
[71] K. Klasing, D. Althoff, D. Wollherr, M. Buss, Comparison of
surface normal estimation methods for range sensing applica-

25

View publication stats