A Plane-based Approach for Indoor Point Clouds

Registration
Ketty Favre, Muriel Pressigout, Eric Marchand, Luce Morin

To cite this version:

Ketty Favre, Muriel Pressigout, Eric Marchand, Luce Morin. A Plane-based Approach for Indoor
Point Clouds Registration. ICPR 2020 - 25th International Conference on Pattern Recognition, Jan
2021, Milan (Virtual), Italy. pp.7072-7079. �hal-03108891�

HAL Id: hal-03108891

https://hal.science/hal-03108891v1
Submitted on 13 Jan 2021

HAL is a multi-disciplinary open access L’archive ouverte pluridisciplinaire HAL, est

archive for the deposit and dissemination of sci- destinée au dépôt et à la diffusion de documents
entific research documents, whether they are pub- scientifiques de niveau recherche, publiés ou non,
lished or not. The documents may come from émanant des établissements d’enseignement et de
teaching and research institutions in France or recherche français ou étrangers, des laboratoires
abroad, or from public or private research centers. publics ou privés.
 A Plane-based Approach for Indoor Point Clouds
Registration
Ketty Favre? Muriel Pressigout† Eric Marchand‡ Luce Morin†
?
Univ Rennes, CNRS, IETR - UMR 6164, Rennes, France.
†
Univ Rennes, INSA Rennes, CNRS, IETR - UMR 6164, Rennes, France.
‡
Univ Rennes, Inria, CNRS, IRISA, Rennes, France.
Email: [email protected]

Abstract—Iterative Closest Point (ICP) is one of the mostly two point clouds. The point-to-point distance is first introduced
used algorithms for 3D point clouds registration. This classical in [1]. Then, the point-to-plane distance is presented in [4],
approach can be impacted by the large number of points proven to be more robust and faster to converge than the
contained in a point cloud. Planar structures, which are less
numerous than points, can be used in well-structured man-made point-to-point one. A linear resolution of this minimization
environment. In this paper we propose a registration method can be found in [5] using the small angle approximation to
inspired by the ICP algorithm in a plane-based registration solve the optimization problem. In [6], the problem is kept
approach for indoor environments. This method is based solely non-linear and solved using a Levenberg-Marquardt approach
on data acquired with a LiDAR sensor. integrating robust estimators. In [7], Normal Distribution
A new metric based on plane characteristics is introduced
to find the best plane correspondences. The optimal transfor- Transform (NDT) takes into account local surface structures
mation is estimated through a two-step minimization approach, around each point and does not match individual points unlike
successively performing robust plane-to-plane minimization and common ICP variants. In Generalized-ICP (G-ICP) [8], the
non-linear robust point-to-plane registration. local neighborhood of points is used in order to assimilate
Experiments on the Autonomous Systems Lab (ASL) dataset this structure to small planar patches. As in point-to-plane
show that the proposed method enables to successfully register
100% of the scans from the three indoor sequences. Experiments approaches, the local normals of the target point cloud are
also show that the proposed method is more robust in large taken into account but also the ones of the source point cloud.
motion scenarios than other state-of-the-art algorithms. It can be assimilated to plane-to-plane registration.
In [9] and [10], the approaches exploit the planar surfaces of
I. I NTRODUCTION man-made environments with a plane-to-plane distance. Those
In robotics, registration of 3D point sets is a key issue in approaches are interesting to use in indoor environments when
localization applications. The trend for autonomous vehicles enough planes are available. However, segmenting planes can
makes it a widely searched field. Nowadays, 3D LiDARs be time consuming. In [11] and [12], the 3D data of the
are becoming cheaper and more frequently used. They have sensor are used as range images. The neighborhood relation
proven their efficiency in localization applications. The raw of the pixels is then used to segment the planes in a region
data generated by a 3D LiDAR are 3D point clouds, meaning growing scheme. It also includes a polygonalization of the
a set of 3D points representing the coordinates of the physical planes in surface models. A region growing process based
point hit by the laser in the sensor reference frame. on smoothness is introduced in [13]. In [14], [15] and [9],
One of the most popular approaches in robotics to register RANSAC approaches are used in order to fit points to planar
3D point clouds is the well-known Iterative Closest Point (ICP) patches.
algorithm [1]. It allows to compute the rigid transformation Matching planes after the segmentation is another chal-
(rotation and translation) that links a source and a target 3D lenging task. In [10], the data rate acquisition is supposed
point cloud. To do so, each point from the source point cloud is very high, which leads to low relative translation between
paired with its closest point in the target one. Then the 3D rigid scans. Thus, planes with the projections of the origin of the
transformation that minimizes the distance between paired sensor close to each other and almost parallel are matched. In
points is estimated. This is achieved within an iterative scheme [16], a plane/line descriptor is proposed to establish structure
until the residual error has reached the desired threshold. A correspondences. Attributes of the planes and the constraints
survey presenting ICP variants are given in [2] and [3]. between them are used in [17].
This introduction provides a short synthesis of the ICP Algorithms such as G-ICP [8], point-to-plane ICP [5] and
variants focused on the different distances that can be used NDT [7] while being efficient for fine registration are sensitive
in order to estimate the 3D rigid transformation that registers to large motion and usually need a good initialization of the
rigid transformation in order to converge. Moreover G-ICP
The authors gratefully acknowledge the financial support of the French Min-
istry of Higher Education, Research and Innovation and from the European can be slow to perform registration as is shown further in this
Interreg Project ADAPT. article. In this paper, we exhibit a framework designed for
 pi-1
t
pi
t In order to reduce the number of input data in the optimiza-
t
P pi+1
t
tion step, the plane-to-plane distance is used. A plane Π(ρ, n)
ni
t

di-1 ni-1
t
di+1 is given by the equation n> p = ρ, where ρ is the distance
di ni+1t

s
s
pi+1 from the origin of the sensor in the direction of the unit plane
s
pi-1 pi normal n. The distance between two corresponding planes ([9],
s
P [10]), with s Πi (s ρi , s ni ) the source plane and t Πi (t ρi , t ni ) the
Fig. 1. Point-to-plane distance d⊥
target one, is given as follows:
i as described in [5]. In red the surface on
which the target point cloud lies and the surface normals related to its points.  t
Rs s ni − t ni

In blue the surface where lies the source point cloud to register. dΠi = > (3)
[t Rs s ni ] t ts + s ρi − t ρi
where s ni and t ni are the normal to s Πi and t Πi respectively
plane-to-plane registration in indoor environment. This method and s ρi and t ρi their respective distance to the origin of the
is denoted New Accurate Plane-based ICP (NAP-ICP). The sensor in the target frame.
proposed algorithm is robust to large motion, thus it is less
sensitive to initialization than other evaluated state-of-the-art III. M ETHODOLOGY
algorithms. The main contributions of this article are: This section describes each step of the proposed NAP-ICP
• an efficient score metric for finding best plane correspon- algorithm. The framework is given in Fig. 2. Similarly to
dences; the classical ICP algorithm the method iteratively performs
• a two-step minimization method from coarse to fine the matching step and the minimization step. However in
registration based on plane features; the proposed method the first features to be matched are
• an algorithm performing fast and accurate registration in planes. Once matched, the rigid transformation minimizing
challenging datasets; the plane-to-plane distance is estimated. After the plane-to-
• a method robust to large motion or inaccurate initializa- plane registration is performed, an additional point-to-plane
tion. registration is done. An example of registration using the
First the variants of the distance to minimize are presented. proposed method is given in Fig. 3.
After the methodology of the proposed method is described
with a section related to each step of the algorithm. Then target scan source scan
the experiments and their results are presented. Finally a
conclusion and perspectives are given.
preprocessing preprocessing

II. ICP VARIANTS

target planes source planes
In the following sections, source and target points will be extraction extraction
N M
respectively denoted s P = {s pi }i=1 and t P = {t pj }j=1 .
The target point cloud is fixed. The goal is to find the rigid plane matching
transformation t Ts that best fits the source to the target. This
plane-to-plane registration
transformation is defined as follows:
estimated
t
Rs t ts

t closed-form plane-to- transformation
Ts = (1) plane minimization with application to
03×1 1 RANSAC process source planes

with t Rs and t ts respectively a 3×3 rotation matrix and a

Gauss-Newton
3×1 translation vector. plane-to-plane
Each point s pi of the source is matched with its closest minimization

point t pi in the target. Then the rigid transformation min-

imizing a distance metric is estimated and these two steps
robust point-to-plane
are iterated until a threshold is reached. In the original ICP registration
algorithm [1] the distance metric to be minimized is the
Euclidean point-to-point distance. optimal transformation
As corresponding points from one scan to another may not
be exactly identical but may lie on the same surface, it is better
Fig. 2. The proposed NAP-ICP overview.
to choose, as in [4], to minimize the point-to-plane distance
(Fig. 1), defined by: Each step of the framework is described further in this
article:
d⊥
i = kt n>i t s
· ( Ts pi − pi )k t 2
(2)
• preprocessing of the 3D point clouds is briefly discussed
with t ni the surface normal computed from t pi neighborhood. in section III-A;
• plane extraction is described in section III-B;
 C. Plane matching
Once the planes are segmented, the next step is matching
each source plane to the closest one in the target point cloud.
For each extracted plane s Πi in the source, a list of planes
in the target that are potential matches for the source plane is
made, called target candidates. Each target candidate t Πj is
given a score within the range [0; 1]. It is computed from the
following features:
Fig. 3. Example of the registration between two point clouds (scans 3 and • the distance between the projections of the origin on
4 from Apartment sequence from ASL dataset [18]). The overlap between
scans is small, yet the proposed method succeeds in registering the two point source plane and target plane do , expected to be close
clouds accurately. In white the target point cloud - In green the source point to 0:
cloud. Left: before registration - Right: after registration.
do = ks ρi s ni − t ρj t nj k2 (4)

• the distance between the centroids of source and target

• the score metric for finding best plane correspondences
planes dc , expected to be close to 0:
is detailed in section III-C;
• robust plane-to-plane registration is described in sec- dc = ks p̄i − t p̄j k2 (5)
tion III-D;
• the additional point-to-plane minimization leading to finer with s p̄ and t p̄ the centroids of s Πi and t Πj respectively;
registration is detailed in section III-E. • the area ratio between the planes Sr , expected to be close
to 1 as planes are expected to have similar areas:
A. Preprocessing
In order to perform plane-to-plane registration as fast and min(s Si , t Sj )
Sr = (6)
accurately as possible preprocessing is sometimes needed. max(s Si , t Sj )
To speed up computation time, target and source scans are
with s Si and t Sj the area of source and target planes
subsampled using a voxel grid of a given resolution. Also,
respectively;
to avoid points from the acquisition system or operators to
• the dot product of the normals of the planes φn , expected
be part of the point cloud, all points closer than 50cm to the
to be close to 1 as planes are expected to be almost
sensor are discarded.
parallel:
B. Plane extraction φn = s ni · t nj (7)
The first step in a plane-based registration (besides prepro-
Each feature is normalized between [0; 1] and weighted,
cessing) is to extract planar structures. In the presented results,
which leads to a score defined as follows:
planes are extracted using a region growing segmentation
based on [13], using the Point Cloud Library (PCL) [19]. score = α · dˆo + β · dˆc + γ · (1 − Ŝr ) + δ · (1 − φ̂n ) (8)
In this approach, the points in a neighborhood with a small
angle difference between normals are considered to be on the with .̂ denoting the normalized value, and the weights α, β, γ
same smooth surface and are gathered in a cluster. Each cluster and δ subject to:
represents a plane. The normals are estimated by performing a
Principal Component Analysis (PCA) on the neighborhood of α+β+γ+δ =1 (9)
the concerned points [19]. An example of the obtained plane
Parameters α, β, γ, δ were chosen empirically to fixed values
segmentation is given in Fig. 4.
for all experiments such as: α = 0.35, β = 0.4, γ = 0.1 and
δ = 0.15.
A target plane is considered as a valid matching candidate
if it respects the following condition:

score < tscore (10)

Some matched planes following the previous condition

happen to be too far from each other, hence their centroids
are distant. They are discarded with the following condition:

dc > tcentroid (11)

Fig. 4. Plane segmentation with region growing approach. Left: input point
cloud (from ASL dataset [18]) - Right: plane extraction result using region
growing [13]. Each extracted plane is in a different color. Red points are
If the distance dc between the centroids is bigger than a
outliers. threshold tcentroid , the planes are too far from each other to
be a valid correspondence.
 Also, matched planes are supposed to have similar areas. RANSAC process: As the previously created correspon-
To avoid matched planes with a notable difference in area, dence list may contain outliers, it is important to discard them
another condition is added: as they can lead to divergence in the minimization step. To do
so, a RANSAC process is applied to make the minimization
Sr > tS (12) more robust.
Only three non-parallel planes are needed in the source and
If the area ratio Sr is smaller than a threshold tS , the
target respectively. Each sample of the RANSAC algorithm is
correspondence is discarded.
selected respecting this condition.
The valid pairs of matched planes form the list of corre-
spondences between source and target planes. The resulting Gauss-Newton plane-to-plane minimization: The plane
list is used for the transformation estimation. As not only correspondences identified as inliers by the RANSAC process
the correspondence with the smallest score are kept but all are given as input of the Gauss-Newton approach.
correspondences respecting the previous conditions, the list This method requires a minimal representation of the trans-
may contain several occurrences of the same source plane, formation to be estimated t Ts . Such a representation is defined
with different target planes and vice-versa. by a 6 dimensional vector denoted q = (t ts , θu)> where θ
and u are the angle and the axis of the rotation t Rs .
D. Plane-to-plane registration The plane-to-plane error has to be minimized such that:
N
Now that the set of plane correspondences is built, the plane- X
to-plane distance minimization that estimates the rigid trans- q̂ = argmin kdΠ
i k
2
(17)
q
i=1
formation linking source to target planes can be computed.
Equation (17) can be solved using the Gauss-Newton al-
Closed-form plane-to-plane optimization method: In this gorithm. Solving it consists in minimizing the cost function
section a closed-form minimization of the plane-to-plane E(q) = ke(q)k:
distance is presented. The derivation is similar to the one  
presented in [9] without the point-to-point correspondences. n(q) − n
e(q) = (18)
Corresponding planes are denoted s Πi (s ρi , s ni ) and ρ(q) − ρ
t
Πi (t ρi , t ni ). Similarly to the point-to-point problem in [20] with n(q) = (..., t Rs s ni , ...), n = (..., t ni , ...), ρ(q) =
and [21] the rotation and translation are decoupled. >
(..., [t Rs s ni ] t ts + s ρi , ...) and ρ = t ρi the error vector of the
The rotation estimation is obtained by minimizing: distance between the target point cloud and the source point
N cloud transformed with the previous estimated transformation.
The first order Taylor approximation gives:
X
= kt Rs s ni − t ni k2 (13)
i=1
e(q + δq) ≈ e(q) + J(q)δq (19)
First, a 3 × 3 correlation matrix H is built such as: where J(q) is the Jacobian of e(q) in q.
N
X With the Gauss-Newton method, the solution consists in
H= s
ni t n>i (14) minimizing E(q + δq) with:
i=1
E(q + δq) = ke(q + δq)k ≈ e(q) + J(q)δq (20)
Its Singular Value Decomposition (SVD), H = UDV, is
ˆ s is given by:
computed. The optimal rotation matrix t R The minimization problem can be solved by an iterative
least-squares approach which gives:
ˆ
tR = VU> (15)
s δq = −λJ(q)+ e(q) (21)
To compute the translation the following equation is mini- where λ is a coefficient in ]0, 1] and J+ is the pseudo-inverse
mized: of the Jacobian J.
N
X The pose is then updated at each iteration:
kt n>i t ts + s ρi − t ρi k2 (16)
i=1 qk+1 = qk ⊕ δq = expδq q (22)
corresponding to solving the linear system At ts = b where where ⊕ denotes the composition over se(3) obtained via the
exponential map.
n>1 Its associated 4N ×6 Jacobian matrix J stacks each Jacobian
t t
ρ1 − s ρ1
  

A =  ...  , b= .. matrix Ji :

   
. 
t > t s
[t Rs s ni ]×
 
nN ρN − ρN 03×3
Ji = (23)
−[t Rs s ni ]> 01×3
The least-squares solution of this problem is given by
ˆs = A+ b where A+ is the pseudo-inverse of A.
tt with [x]× the skew matrix of a vector x.
E. Point-to-plane Registration of a wall, a curved ceiling and numerous pillars and
To ensure an accurate registration, a finer step is added to arches which are repetitive elements. The sequence is
find the best expected rigid transformation. To do so a point- composed of 35 scans of about 191,000 points.
to-plane registration is added at the end of the process. A • Stairs: This sequence aims to evaluate robustness to rapid
robust non-linear minimization of the point-to-plane distance variations in scanned volumes. It starts in a long corridor,
is presented. Each point s pi from s P is matched to its closest then a small staircase is crossed and finally the last scan is
point t pi in t P according to the Euclidean distance. Then the captured outside of the building. The path in the staircase
rigid transformation that registers source to target point cloud shows that considering only 2D paths is not valid. The
is computed by minimizing the point-to-plane distance (Eq. 2). sequence is composed of 30 scans of about 191,000
The principle is the same as section III-D with: points.
N
X Metrics: As in [23], the accuracy of the tested metrics
q̂ = argmin kd⊥
i k
2
(24)
q is evaluated with the Euclidean distance ∆t between the
i=1
estimated transformation and the ground truth for translation
Its Jacobian Ji is defined by: and the geodesic distance ∆r for rotation:
Ji = −t n>i t > s


ni [ pi ]× (25)
∆t = kt t̂s − t t∗s k (29)
If outliers are present in the dataset the minimization !
−1 t
problem becomes unstable. Using the Gauss-Newton method trace(t R∗s R̂s ) − 1
for the minimization allows to introduce M-estimators, a class ∆r = arccos (30)
2
of robust functions, in the algorithm to discard outliers [22].
Denoting ρ(.) a robust function, q̂ becomes: with t t̂s and t R̂s the estimated translation and rotation, t t∗s
N
X and t R∗s the ground truth translation and rotation respectively.
q̂ = argmin ρ(d⊥
i ) (26) The thresholds to estimate a successful registration are 0.1m
q
i=1 for translation and 2.5◦ for rotation as suggested in [7]. Note
The introduction of M-estimators can be implemented as an that rotation and translation errors are presented separately but
Iteratively Re-weighted Least Squares (IRLS) where the error a result is valid only if both rotation and translation errors are
to minimize is defined by: smaller than their respective threshold.

eρ (q) = D(e(q)) (27) A. Impact of several minimization steps

This experiment aims to show the need for the point-to-
and
+ plane registration step in NAP-ICP algorithm. As can be
δq = −λ(DJ(q)) De(q) (28)
seen in Fig. 2 the estimation of the rigid transformation
where D is a N × N diagonal matrix containing the weights is performed in two successive steps. First a plane-to-plane
that reflect the confidence in the data. minimization is performed (section III-D) followed by a point-
to-plane registration (section III-E).
IV. E XPERIMENTS In Fig. 5, the impact of the steps can be observed with
To evaluate the efficiency, accuracy and robustness of the curves representing the cumulative probabilities errors on
proposed method, experiments in different scenarios are held. translation and rotation. The more top-left the curve the better
The indoor sequences of the ASL Dataset [18] are used. the algorithm performs. The expected behavior is to attain 1
One of the main advantages of this dataset is that each (meaning all scans of the sequence are registered) before the
sequence comes with the ground truth poses measured for each error threshold is reached. If so, it means that 100% of the
scan with millimeter precision. As the proposed NAP-ICP is scans of the sequence are successfully registered (according
designed to register point clouds in man-made environments, to the threshold previously). If 1 is not reached before the
the outdoor sequences containing various types of surfaces threshold, it means that the registration error is too large
(thus not planar) were discarded. All sequences were recorded to be considered successful. On the plots, one can observe
using a Hokuyo UTM-30LX. that for each sequence, the plane-to-plane registration gives a
• Apartment: This sequence is designed to evaluate algo- good initialization of the rigid transformation but is still far
rithm robustness to outliers coming from dynamic ele- from ground truth. For instance, considering the Apartment
ments (e.g. moved furniture). The sequence was captured sequence, only 41% scans are well registered regarding trans-
moving the sensor on a 2D plane in an apartment. It is a lation error (81% success rate in rotation). On ETH and Stairs,
very structured scene (walls, ceiling, floor). The sequence regarding rotation, even if the plane-to-plane registration gives
is composed of 44 scans of about 365,000 points. results sufficient to reach the expected threshold, the addition
• ETH: This sequence aims to evaluate robustness of regis- of the point-to-plane proves to give a more accurate estimation.
tration to repetitive elements. This scene was captured in For each tested sequence, the point-to-plane ICP step addi-
a long hallway, following a straight path. It is composed tion achieves a 100% success rate in rotation and translation.
 (a) Apartment sequence

(b) ETH sequence

Fig. 6. 3D mapping of the Apartment sequence using the proposed method.

The points of the map are colored regarding their height (blue being the
lowest value, green in between and yellow the highest). In white the ground
truth trajectory. In purple dots the trajectory computed with the plane-to-plane
registration only. In red dots the trajectory computed with the full proposed
NAP-ICP (combination of plane-to-plane and point-to-plane registration).
Ceiling has been removed to ease visualization.

• Point-to-plane ICP [4], with the PCL implementation

(c) Stairs sequence
(denoted ICP-PCL), similarly to G-ICP has three major
Fig. 5. Cumulative probabilities of translation and rotation errors for each step
addition of the algorithm. Left: translation error (in meters) on the horizontal
parameters. Maximum iterations is set to 100, Euclidean
axis. The vertical bar represents a threshold (0.1m) for successful registration. fitness epsilon is set to 10−6 and maximum correspon-
Right: rotation error (in degrees) on the horizontal axis. The vertical bar dence distance is set to 0.8m.
represents a threshold (2.5◦ ) for successful registration.
Accuracy: To evaluate accuracy, equations (29) and (30) are
used with all tested algorithms. The results of this experiment
This proves the ability of the plane-to-plane minimization to are summarized in Table I.
give a result close enough to what is expected in order to Globally, the proposed NAP-ICP algorithm gives more ac-
obtain an accurate registration with a robust point-to-plane curate results than G-ICP, NDT and ICP-PCL. On Apartment,
registration. ETH and Stairs sequences NAP-ICP achieves a 100% rate
In Figure 6 all scans of Apartment sequence are registered of successful registration. G-ICP, NDT and ICP-PCL give a
in a mapping intention with the full algorithm. Each scan is 100% success rate on ETH sequence. However, their results on
registered with the previous one using the previously found Stairs are not as good, even if still satisfying. On Apartment
transformation as initialization1 . NAP-ICP outperforms the state-of-the-art algorithms. G-ICP
achieves 75% of successful registrations, 77% for NDT and
B. Comparison with state-of-the-art Algorithms only 43% for ICP-PCL. This sequence includes large rotations
In the following experiment, NAP-ICP is compared to three (38% of the sequence is composed of motion with more than
state-of-the-art registration algorithms in terms of accuracy and ±35◦ rotation on yaw axis) and G-ICP, NDT and ICP-PCL
computation time.
• G-ICP [8] has three major parameters. Maximum itera-
TABLE I
tions is set to 10, Euclidean fitness epsilon is set to 10−6 P ERCENTAGE OF SUCCESSFUL REGISTRATION ( TRANSLATION AND
and maximum correspondence distance is set to 0.8m. ROTATION COMBINED ) FOR THE EVALUATED ALGORITHMS ON EACH
SEQUENCE
• NDT, with the steps recommended in [7]. Transformation
epsilon is set to 10−3 , step size 0.1, maximum iteration
5, first step resolution 1.0m, second step resolution 2.0m, Sequence NAP-ICP G-ICP NDT ICP-PCL
third step resolution 1.0m and last step 0.5m. Apartment 100 75 77 43
ETH 100 100 100 100
1 A short video presenting the mapping process can be found at: Stairs 100 97 97 90
https://youtu.be/CL9gulE68rU
sometimes struggle to find the right solution when our method TABLE II
succeeds. More detailed results with rotation and translation AVERAGE PROCESSING TIME FOR EACH SEQUENCE IN MILLISECONDS
errors are presented in Fig. 7 for all sequences with curves
representing the cumulative probabilities errors on translation Sequence NAP-ICP G-ICP NDT ICP-PCL
and rotation for each tested algorithm. The most significant Apartment 500 1790 233 339
ETH 1000 1800 484 808
Stairs 360 1300 211 375

V. C ONCLUSION AND P ERSPECTIVES

In this paper, NAP-ICP, an efficient plane-based registration
algorithm for indoor 3D point clouds, is presented. This
proposed method is based solely on LiDAR data.
(a) Apartment sequence A new metric based on planes characteristics is proposed
to efficiently find the best plane correspondences. The robust
plane-to-plane minimization followed by a point-to-plane min-
imization reaches 100% of successful registration on the tested
sequences. Experiments show that NAP-ICP performs better
than other state-of-the-art algorithms in well-structured envi-
ronments (more specifically with large motion initialization
between scans). They also showed that NAP-ICP algorithm is
not only accurate but also fast. As it was not the main goal
of this study, there is still room for optimization.
(b) ETH sequence
A more thorough study about weighting parameters for
plane matching will be proposed in future works. This method
also needs to be improved to handle outdoor urban environ-
ments.

R EFERENCES
[1] P. J. Besl and N. D. McKay, “A method for registration of 3-D shapes,”
IEEE Transactions on Pattern Analysis and Machine Intelligence,
vol. 14, pp. 239–256, Feb. 1992.
(c) Stairs sequence [2] S. Rusinkiewicz and M. Levoy, “Efficient variants of the ICP algorithm,”
Fig. 7. Cumulative probabilities of translation and rotation errors for each in Proceedings Third International Conference on 3-D Digital Imaging
sequence on each evaluated algorithm. Left: translation error (in meters) on the and Modeling, pp. 145–152, May 2001.
horizontal axis. The vertical bar represents a threshold (0.1m) for successful [3] F. Pomerleau, F. Colas, and R. Siegwart, “A Review of Point Cloud
registration. Right: rotation error (in degrees) on the horizontal axis. The Registration Algorithms for Mobile Robotics,” Foundations and Trends
vertical bar represents a threshold (2.5◦ ) for successful registration. in Robotics, vol. 4, pp. 1–104, May 2015.
[4] Y. Chen and G. Medioni, “Object modelling by registration of multiple
range images,” Image and Vision Computing, vol. 10, pp. 145–155, Apr.
1992.
feature of the proposed method in this experiment is its [5] K.-l. Low, “Linear least-squares optimization for point-toplane ICP
robustness to large motion scenarios (especially rotations) in surface registration,” tech. rep., Department of Computer Science, Uni-
comparison with other algorithms. versity of North Carolina at Chapel Hill, 2004.
[6] A. Fitzgibbon, “Robust Registration of 2D and 3D Point Sets,” Image
and Vision Computing, vol. 21, Jan. 2003.
Computation time: No speed optimization are performed [7] M. Magnusson, N. Vaskevicius, T. Stoyanov, K. Pathak, and A. Birk,
“Beyond points: Evaluating recent 3D scan-matching algorithms,” in
in NAP-ICP, however it is important to estimate the perfor- 2015 IEEE International Conference on Robotics and Automation
mances of the proposed method at this point. The experiments (ICRA), pp. 3631–3637, May 2015.
were held on a desktop computer with an Intel Xeon W-2133, [8] A. Segal, D. Haehnel, and S. Thrun, “Generalized-ICP,” Proc. of
Robotics : Science and Systems, vol. 2, p. 4, 2009.
3.6GHz CPU and 32GB RAM. Processing time for the tested
[9] Y. Taguchi, Y. Jian, S. Ramalingam, and C. Feng, “Point-plane SLAM
algorithms on each sequence is detailed in Table II. For each for hand-held 3D sensors,” in 2013 IEEE International Conference on
method, it includes all steps from point clouds preprocessing Robotics and Automation, pp. 5182–5189, May 2013.
to transformation estimation. [10] W. S. Grant, R. C. Voorhies, and L. Itti, “Efficient Velodyne SLAM with
point and plane features,” Autonomous Robots, vol. 43, pp. 1207–1224,
On all sequences NDT is the fastest algorithm, followed by June 2019.
ICP-PCL. NAP-ICP method is slower than the aforementioned [11] J. Poppinga, N. Vaskevicius, A. Birk, and K. Pathak, “Fast plane
detection and polygonalization in noisy 3D range images,” in 2008
algorithms but compensate with its accuracy. G-ICP is the IEEE/RSJ International Conference on Intelligent Robots and Systems,
slowest method to handle this dataset. pp. 3378–3383, Sept. 2008.
[12] K. Pathak, N. Vaskevicius, J. Poppinga, M. Pfingsthorn, S. Schwertfeger,
and A. Birk, “Fast 3D mapping by matching planes extracted from range
sensor point-clouds,” in 2009 IEEE/RSJ International Conference on
Intelligent Robots and Systems, pp. 1150–1155, Oct. 2009.
[13] T. Rabbani, F. A. van den Heuvel, and G. Vosselman, “Segmentation
of point clouds using smoothness constraints,” in ISPRS 2006 : Pro-
ceedings of the ISPRS commission V symposium Vol. 35, part 6 : image
engineering and vision metrology, Dresden, Germany 25-27 September
2006, pp. 248–253, 2006.
[14] M. A. Fischler and R. C. Bolles, “Random sample consensus: a paradigm
for model fitting with applications to image analysis and automated
cartography,” Communications of the ACM, vol. 24, pp. 381–395, June
1981.
[15] K. Pathak, A. Birk, N. Vakeviius, and J. Poppinga, “Fast Registration
Based on Noisy Planes With Unknown Correspondences for 3-D Map-
ping,” IEEE Transactions on Robotics, vol. 26, pp. 424–441, June 2010.
[16] S. Chen, L. Nan, R. Xia, J. Zhao, and P. Wonka, “PLADE: A
Plane-Based Descriptor for Point Cloud Registration With Small Over-
lap,” IEEE Transactions on Geoscience and Remote Sensing, vol. 58,
pp. 2530–2540, Apr. 2020.
[17] W. Zong, M. Li, Y. Zhou, L. Wang, F. Xiang, and G. Li, “A Fast and
Accurate Planar-Feature-Based Global Scan Registration Method,” IEEE
Sensors Journal, vol. 19, pp. 12333–12345, Dec. 2019.
[18] F. Pomerleau, M. Liu, F. Colas, and R. Siegwart, “Challenging data sets
for point cloud registration algorithms,” The International Journal of
Robotics Research, vol. 31, pp. 1705–1711, Dec. 2012.
[19] R. B. Rusu and S. Cousins, “3D is here: Point Cloud Library (PCL),”
in 2011 IEEE International Conference on Robotics and Automation,
(Shanghai, China), pp. 1–4, May 2011.
[20] K. S. Arun, T. S. Huang, and S. D. Blostein, “Least-squares fitting of
two 3-d point sets,” IEEE transactions on pattern analysis and machine
intelligence, vol. 9, pp. 698–700, May 1987.
[21] A. Lorusso, D. W. Eggert, and R. B. Fisher, “A Comparison of Four
Algorithms forEstimating 3-D Rigid Transformations,” 1995 BMVC,
vol. 1, pp. 237 –246, 1995.
[22] A. Comport, E. Marchand, M. Pressigout, and F. Chaumette, “Real-time
markerless tracking for augmented reality: the virtual visual servoing
framework,” IEEE Transactions on Visualization and Computer Graph-
ics, vol. 12, no. 4, pp. 615–628, 2006.
[23] F. Pomerleau, F. Colas, R. Siegwart, and S. Magnenat, “Comparing ICP
variants on real-world data sets Open-source library and experimental
protocol,” Autonomous Robots, vol. 34, no. 3, pp. 133–148, 2013.