ArticlePDF Available

A Single LiDAR-Based Feature Fusion Indoor Localization Algorithm

Sensors

April 2018
18(4):1294

DOI:10.3390/s18041294

License
CC BY 4.0

Authors:

Chao-Chung Peng

Ankit Ravankar

Tohoku University

Abhijeet Ravankar

Kitami Institute of Technology

In past years, there has been significant progress in the field of indoor robot localization. To precisely recover the position, the robots usually relies on multiple on-board sensors. Nevertheless, this affects the overall system cost and increases computation. In this research work, we considered a light detection and ranging (LiDAR) device as the only sensor for detecting surroundings and propose an efficient indoor localization algorithm. To attenuate the computation effort and preserve localization robustness, a weighted parallel iterative closed point (WP-ICP) with interpolation is presented. As compared to the traditional ICP, the point cloud is first processed to extract corners and line features before applying point registration. Later, points labeled as corners are only matched with the corner candidates. Similarly, points labeled as lines are only matched with the lines candidates. Moreover, their ICP confidence levels are also fused in the algorithm, which make the pose estimation less sensitive to environment uncertainties. The proposed WP-ICP architecture reduces the probability of mismatch and thereby reduces the ICP iterations. Finally, based on given well-constructed indoor layouts, experiment comparisons are carried out under both clean and perturbed environments. It is shown that the proposed method is effective in significantly reducing computation effort and is simultaneously able to preserve localization precision.

Available via license: CC BY 4.0

Content may be subject to copyright.

sensors

Article

A Single LiDAR-Based Feature Fusion Indoor

Localization Algorithm

Yun-Ting Wang 1, Chao-Chung Peng 1,*, Ankit A. Ravankar 2and Abhijeet Ravankar 3

1Department of Aeronautics and Astronautics, National Cheng Kung University, Tainan 701, Taiwan;

omiwanggg@gmail.com

2Division of Human Mechanical Systems and Design, Faculty of Engineering, Hokkaido University,

Sapporo 060-8628, Japan; ankit@eng.hokudai.ac.jp

3Lab of Smart Systems Engineering, Kitami Institute of Technology, Hokkaido, Kitami 090-8507, Japan;

abhijeetravankar@gmail.com

*Correspondence: ccpeng@mail.ncku.edu.tw; Tel.: +886-6-275-7575 (ext. 63633)

Received: 17 March 2018; Accepted: 18 April 2018; Published: 23 April 2018





Abstract:

In past years, there has been signiﬁcant progress in the ﬁeld of indoor robot localization.

To precisely recover the position, the robots usually relies on multiple on-board sensors. Nevertheless,

this affects the overall system cost and increases computation. In this research work, we considered

a light detection and ranging (LiDAR) device as the only sensor for detecting surroundings and

propose an efﬁcient indoor localization algorithm. To attenuate the computation effort and preserve

localization robustness, a weighted parallel iterative closed point (WP-ICP) with interpolation is

presented. As compared to the traditional ICP, the point cloud is ﬁrst processed to extract corners and

line features before applying point registration. Later, points labeled as corners are only matched with

the corner candidates. Similarly, points labeled as lines are only matched with the lines candidates.

Moreover, their ICP conﬁdence levels are also fused in the algorithm, which make the pose estimation

less sensitive to environment uncertainties. The proposed WP-ICP architecture reduces the probability

of mismatch and thereby reduces the ICP iterations. Finally, based on given well-constructed indoor

layouts, experiment comparisons are carried out under both clean and perturbed environments. It is

shown that the proposed method is effective in signiﬁcantly reducing computation effort and is

simultaneously able to preserve localization precision.

Keywords: indoor localization; pose estimation; iterative closet point; SLAM; LiDAR

1. Introduction

Simultaneous localization and mapping (SLAM) is a method of building a map under exploration

and estimating vehicle pose based on sensor information in an unknown environment. When exploring

an unpredictable environment, an unmanned vehicle is generally employed for exploration as well

as localization. In this regard, the vehicle could be equipped with a single sensor for detecting and

identifying surroundings, or by attaching two or even more sensors on the vehicle to enhance its

estimation capability.

When considering different kinds of sensors [

], laser range ﬁnders (LRFs), vision, and Wi-Fi

networks are popular sensing techniques for indoor localization tasks. Recently, with advancement in

computer vision and image processing, many researchers have started investigating the vision-based

SLAM [

]. Under the condition that the captured images are matched sufﬁciently, features can

be extracted using Scale-Invariant Feature Transform (SIFT) [

] or Speeded Up Robust Features

(SURF) [

]. Other indoor localization methods consider the amplitude of received signal from Wi-Fi

networks [

–

]. These localization strategies depend on pre-installed wireless hardware devices on

the site and thus may not be applicable in Wi-Fi denied environments.

Sensors 2018,18, 1294; doi:10.3390/s18041294 www.mdpi.com/journal/sensors

Sensors 2018,18, 1294 2 of 19

For an ultra-low-cost SLAM module, previous works have considered a set of on-board ultrasonic

sensors [

], which provided sparse measurements about the environment. However, the robot pose

might lose its pose under complicated environments without the aid of robot kinematics information.

To achieve robust image recognition [

], robot navigation and map construction, the depth sensor,

Kinect v2, was considered [

]. Kinect v2 is based on the time-of-ﬂight measurement principle and can

be used in outdoors environment. Since the multi-depth sensors are able to provide highly dense 3D

data [

], the real-time computation effort is relative higher. Furthermore, for well-constructed indoor

environment, such hardware conﬁguration is not necessary for 2D robot positioning.

Owing to the light weight and portable advantage, light detection and ranging (LiDAR) has

attracted more and more attention [

]. LiDAR possesses a high sampling rate, high angular

resolution, good range detection, and high robustness against environment variability. As a result,

in this research, a single LiDAR is used for indoor localization.

By analyzing the position of features in each frame at every movement of the vehicle, one can

ﬁgure out the vehicle’s traveling distance and heading. With different scanning data, an iterative closed

point (ICP) [

] algorithm is employed to ﬁnd the most appropriate robot pose matching conditions,

including rotation and translation. However, the ICP may not always lead to good pattern matching if

point cloud registration issue is not well addressed. In other words, a better point registration will lead

to better robot pose estimation. To address this, point cloud outliers must be identiﬁed and recognized.

Another issue when applying the ICP is computation efﬁciency. Since the ICP algorithm considers

the closest-point rule to establish correspondences between points in current scan and a given layout,

the searching effort can increase dramatically when the scan or a layout contains large amounts of data.

The researches [

–

] has addressed and solved some of the problems when applying ICP,

including (1) wrong point matching for large initial errors, (2) expensive correspondence searching,

(3) slow convergence speed, and (4) outlier removal. For robot pose subjected to large initial angular

displacement, especially in [

], iterative dual correspondence (IDC) is proposed. However, it demands

higher computation due to its dual-correspondence process. Metric-based ICP (MbICP) [

] considers

geometric distance that takes translation and rotation into account simultaneously. The correspondence

between scans is established with this measure and the minimization of the error is carried out in

terms of this distance. The MbICP shows superior robustness in the case of existing large angular

displacement. Among various planar scan matching strategies, Normal Distribution Transformation

(NDT) [

] and Point-to-Line ICP (PLICP) [

] illustrate state-of-the-art performance in consideration

of scan matching accuracy. NDT transforms scans onto a grid space and tends to ﬁnd the best ﬁt by

maximizing the normal distribution in each cell. The NDT does not require point-to-point registrations,

so it enhances matching speed. However, the selection of the grid size dominates estimation stability.

The PLICP considers normal information from environment’s geometric surface and tries to minimize

the distance projected onto the normal vector of the surface. In addition, a close-form solution is also

given such that the convergence speed can be drastically improved.

To obtain precise and high-bandwidth robot pose estimation, the vehicle’s dynamics and traveling

status can be further integrated into a Kalman ﬁlter [

]. A Kalman ﬁlter provides the best estimate to

eliminate noise and provides a better robot pose prediction. Other researchers have considered the

wheel odometry fusion-based SLAM [

], which integrates robot kinematics and encoder data for

pose estimation. However, it might not be suitable for realization of a portable localization system.

Based on the aforementioned issues, in this work, the LiDAR is considered as the only sensor for

mapping and localization. To avoid mismatched point registration and to enhance matching speed,

we propose a feature-based weighted parallel iterative closed point (WP-ICP) architecture inspired

by [

–

]. The main advantages of the proposed method are as follows: (a) Point sizes of the

model set and data set are signiﬁcantly reduced so that the ICP speed can be enhanced. (b) A split

and merge algorithm [

] is considered to divide the point cloud into two feature groups, namely,

corner and line segments. The algorithm works by matching points labeled as corners to the corner

Sensors 2018,18, 1294 3 of 19

candidates; similarly, for those points labeled as lines can only be matched to the lines candidates. As a

result, it attenuates any possibilities of point cloud mismatch.

In this paper, it is supposed that the well-constructed indoor layout is given in advance. The main

design object is to reduce the computation effort and to maintain the indoor positioning precision.

The rest of this paper is organized as follows. In Section 2, an adaptive breakpoint detector is ﬁrstly

introduced for scan point segmentation. A clustering algorithm and a split–merge approach is further

considered for point clustering and feature extraction, respectively. In Section 3, a WP-ICP algorithm is

proposed. Section 4presents real experiments to evaluate the effectiveness of the proposed method.

Finally, Section 5outlines conclusions and future work.

2. Feature Extraction

For feature extraction, a robot is employed in an indoor environment and moves in a given layout,

and a localization algorithm is introduced. In addition, real-time sensing and pose estimation is key

for practical realization. Feature extraction plays an important role in reducing the amount of point

cloud data for computational speedup.

2.1. The Main Concept of Feature Extraction

Due to the high scan resolution of LiDAR and the given layout, construction of a KD-Tree will

be highly time-consuming if all data points are fed into the ICP algorithm. Therefore, extracting

informative feature points to represent the environment is one of the important tasks.

In this research, a feature extraction scan matching procedure is proposed as summarized in

Figure 1. Firstly, all the scanning points are separated into many clusters by invoking the adaptive

breakpoint detector (ABD) [

]. The main idea of an ABD is to ﬁnd the breakpoints in each scan,

where the scanning direction of a LiDAR is counterclockwise and continuous. Therefore, by detecting

breakpoints, the algorithm can determine if there exists a discontinuity between two consecutive

scanning points. However, the threshold for the breakpoint determination should be adaptive with

respect to the scanning distance, which is presented in the following subsection.

Sensors 2018, 18, x FOR PEER REVIEW 3 of 19

corner candidates; similarly, for those points labeled as lines can only be matched to the lines

candidates. As a result, it attenuates any possibilities of point cloud mismatch.

In this paper, it is supposed that the well-constructed indoor layout is given in advance. The

main design object is to reduce the computation effort and to maintain the indoor positioning

precision. The rest of this paper is organized as follows. In Section 2, an adaptive breakpoint detector

is firstly introduced for scan point segmentation. A clustering algorithm and a split–merge approach

is further considered for point clustering and feature extraction, respectively. In Section 3, a WP-ICP

algorithm is proposed. Section 4 presents real experiments to evaluate the effectiveness of the

proposed method. Finally, Section 5 outlines conclusions and future work.

2. Feature Extraction

For feature extraction, a robot is employed in an indoor environment and moves in a given

layout, and a localization algorithm is introduced. In addition, real-time sensing and pose estimation

is key for practical realization. Feature extraction plays an important role in reducing the amount of

point cloud data for computational speedup.

2.1. The Main Concept of Feature Extraction

Due to the high scan resolution of LiDAR and the given layout, construction of a KD-Tree will

be highly time-consuming if all data points are fed into the ICP algorithm. Therefore, extracting

informative feature points to represent the environment is one of the important tasks.

In this research, a feature extraction scan matching procedure is proposed as summarized in

Figure 1. Firstly, all the scanning points are separated into many clusters by invoking the adaptive

breakpoint detector (ABD) [28]. The main idea of an ABD is to find the breakpoints in each scan,

where the scanning direction of a LiDAR is counterclockwise and continuous. Therefore, by detecting

breakpoints, the algorithm can determine if there exists a discontinuity between two consecutive

scanning points. However, the threshold for the breakpoint determination should be adaptive with

respect to the scanning distance, which is presented in the following subsection.

Figure 1. Flow chart of feature extraction.

Sensors 2018,18, 1294 4 of 19

2.2. A Novel Method for Finding Clusters

Consider that, for each scan, there exists

beams. The radius for each beam is denoted as

where

···

. To enhance the clustering performance, a simple and straightforward adaptive

radius clustering (ARC) algorithm is developed as follows:

∆S=R×∆θ=R×(α×2π/360),R=min(ri,ri−1)(1)

λ=N×∆S(2)

where

∆S

denotes an adaptive arc length between points,

is the angular resolution provided by

LiDAR speciﬁcation,

is a design scaling factor relative to LiDAR noise level, and

is the threshold

for clustering. Therefore, if the distance between two scanning points in the same scan is larger than

those points are going to be divided into two different clusters; that is

kpi−pi−1k>λ(3)

Based on the ARC, the scan shown in Figure 2b can be clustered into two groups. Else, it may

be divided into several segments due to the radius variations as illustrated in Figure 2a. Therefore,

the ARC algorithm is able to separate clusters according to LiDAR’s measurement characteristics.

After utilizing the ARC algorithm, clusters with fewer scanning points are treated as outliers.

For clusters with lower density points that cannot provide reliable information are discarded before

the feature extraction step.

Sensors 2018, 18, x FOR PEER REVIEW 4 of 19

2.2. A Novel Method for Finding Clusters

Consider that, for each scan, there exists n beams. The radius for each beam is denoted as i

where ni ,,1 . To enhance the clustering performance, a simple and straightforward adaptive

radius clustering (ARC) algorithm is developed as follows:

)360/2( παRθRS  ,



,min 

ii rrR (1)

SNλ (2)

where S denotes an adaptive arc length between points, α is the angular resolution provided by

LiDAR specification, N is a design scaling factor relative to LiDAR noise level, and λ is the

threshold for clustering. Therefore, if the distance between two scanning points in the same scan is

larger than λ, those points are going to be divided into two different clusters; that is

λpp ii  1 (3)

Based on the ARC, the scan shown in Figure 2b can be clustered into two groups. Else, it may be

divided into several segments due to the radius variations as illustrated in Figure 2a. Therefore, the

ARC algorithm is able to separate clusters according to LiDAR’s measurement characteristics.

After utilizing the ARC algorithm, clusters with fewer scanning points are treated as outliers.

For clusters with lower density points that cannot provide reliable information are discarded before

the feature extraction step.

(a) without adaptive radius (b) with adaptive radius

Figure 2. Illustration of the adaptive radius clustering (ARC) algorithm.

2.3. Split and Merge for Corner and Line Extractions

Comparing a few of the feature extraction methods in different aspects, including the speed of

the algorithm, correctness, and so on, the split and merge [15,28,29] is considered for feature

extraction in this research. The algorithm is capable of extracting corner feature points in the

environment, which can be then taken as stable and reliable feature points.

The splitting procedure also combines the idea of Iterative End Point Fitting (IEPF) [30,32],

which is a recursive algorithm for extracting features. For a cluster





kiyx

,,1|,  , the split and

merge algorithm connects the first point and the last point to form a straight line. The algorithm then

calculates deviations from all the points to this line. If a point where the corresponding maximum

distance is larger than a predefined deviation threshold c

d, this point is labeled as a feature point

and it further splits the cluster into two subsets. The feature point





iic

yxP , can be determined

by the following rule:



iic d

byax

dyxP









 22

maxarg:, (4)

Figure 2. Illustration of the adaptive radius clustering (ARC) algorithm.

2.3. Split and Merge for Corner and Line Extractions

Comparing a few of the feature extraction methods in different aspects, including the speed of the

algorithm, correctness, and so on, the split and merge [

] is considered for feature extraction in

this research. The algorithm is capable of extracting corner feature points in the environment, which

can be then taken as stable and reliable feature points.

The splitting procedure also combines the idea of Iterative End Point Fitting (IEPF) [

], which

is a recursive algorithm for extracting features. For a cluster

Ω={xi,yi|i=1, ·· · ,k}

, the split and

merge algorithm connects the ﬁrst point and the last point to form a straight line. The algorithm then

calculates deviations from all the points to this line. If a point where the corresponding maximum

distance is larger than a predeﬁned deviation threshold

, this point is labeled as a feature point and

it further splits the cluster into two subsets. The feature point

Pc(xi,yi)∈Ω

can be determined by the

following rule:

Pc(xi,yi)←d:=argmax

Pc∈Ω

|axi+byi+1|

√a2+b2>dc(4)

Sensors 2018,18, 1294 5 of 19

where

are the coefﬁcients of a line equation

L:ax +by +

0, which is composed of

and

Figure 3demonstrates the main process of the split and merge algorithm. The algorithm recursively

splits the set of points into two subsets until the condition, Equation (4), is not satisﬁed.

Sensors 2018, 18, x FOR PEER REVIEW 5 of 19

where ba, are the coefficients of a line equation 01:  byaxL , which is composed of 1

P and k

Figure 3 demonstrates the main process of the split and merge algorithm. The algorithm recursively spl its

the set of points into two subsets until the condition, Equation (4), is not satisfied.

Figure 3. Illustration of the split and merge scheme.

3. The Proposed Method: WP-ICP

3.1. Pose Estimation Algorithm

SLAM is considered to be a chicken and egg problem since precise localization needs a reference

map, and a good mapping result comes from a correct estimation of the robot pose [33]. To achieve

SLAM, these two issues must be solved simultaneously [34]. However, in this work, only localization

is considered. Therefore, by assuming that a complete layout of the environment is given in advance,

a novel scan matching algorithm is presented and will be introduced in the next subsection.

Suppose that the correspondences are already known, the pose estimation can be considered as

an estimate of rigid body transform, which can be solved efficiently via singular value decomposition

(SVD) technique [35].

Let





pppP ,, 21

 be data set from a current scan and





qqqQ ,, 21

 be a model set

received from a given layout. The goal is to find a rigid body transformation pair



tR, such that the

best alignment can be achieved in the least error sense. It can be stated as



 N

iiii

minarg, qtRptR (5)

where i

w are the weights for each point pair.

The optimal translation vector can be calculated by

qt  (6)

where





 N

iii

1,q

p (7)

can be taken as weighted centroids for the data set and the model set, respectively.

Let ppx  ii and qqy  ii . Consider also matrices X, Y, and W which are defined by



xxX 

,



yyY 

, and



wwdiag ,,

1W, respectively.

Defining T

XWYS  and then applying the singular value decomposition (SVD) on S yields

VΣUS  (8)

where U and V are unitary matrices, and Σ is a diagonal matrix. It has been proved that the

optimal rotation matrix is available by considering

VUR  (9)

Based on Equation (9), the translational vector given in Equation (6) can also be solved.

Figure 3. Illustration of the split and merge scheme.

3. The Proposed Method: WP-ICP

3.1. Pose Estimation Algorithm

SLAM is considered to be a chicken and egg problem since precise localization needs a reference

map, and a good mapping result comes from a correct estimation of the robot pose [

]. To achieve

SLAM, these two issues must be solved simultaneously [

]. However, in this work, only localization

is considered. Therefore, by assuming that a complete layout of the environment is given in advance,

a novel scan matching algorithm is presented and will be introduced in the next subsection.

Suppose that the correspondences are already known, the pose estimation can be considered as

an estimate of rigid body transform, which can be solved efﬁciently via singular value decomposition

(SVD) technique [35].

Let

P={p1,p2,·· ·pN}

be data set from a current scan and

Q={q1,q2,·· ·qN}

be a model set

received from a given layout. The goal is to ﬁnd a rigid body transformation pair

(R,t)

such that the

best alignment can be achieved in the least error sense. It can be stated as

(R,t)=argmin

∑

i=1

wikRpi+t−qik2(5)

where wiare the weights for each point pair.

The optimal translation vector can be calculated by

t=q−Rp (6)

where

p=∑N

i=1wipi

∑N

i=1wi

,q=∑N

i=1wiqi

∑N

i=1wi

(7)

can be taken as weighted centroids for the data set and the model set, respectively.

Let

xi=pi−p

and

yi=qi−q

. Consider also matrices

, and

which are deﬁned by

X=hx1·· · xNi,Y=hy1·· · yNi, and W=diag(w1,·· · ,wN), respectively.

Deﬁning S=XWYTand then applying the singular value decomposition (SVD) on Syields

S=UΣVT(8)

where

and

are unitary matrices, and

is a diagonal matrix. It has been proved that the optimal

rotation matrix is available by considering

R=VUT(9)

Sensors 2018,18, 1294 6 of 19

Based on Equation (9), the translational vector given in Equation (6) can also be solved.

According to the ARC, the split–merge algorithm, and the ICP, the procedure of the

corner-feature-based ICP pose estimation is summarized in Figure 4. To reject the outlier during

the feature point registration, the weightings wican be designed according to the Euclidean distance,

and the values for certain unreasonable feature pairs can be set to zero.

Sensors 2018, 18, x FOR PEER REVIEW 6 of 19

According to the ARC, the split–merge algorithm, and the ICP, the procedure of the

corner-feature-based ICP pose estimation is summarized in Figure 4. To reject the outlier during the

feature point registration, the weightings i

w can be designed according to the Euclidean distance,

and the values for certain unreasonable feature pairs can be set to zero.

Start

Scanning points (Data Set)

Clustering by ARC algorithm

Corner feature extraction by

Split and Merge algorithm

ICP algorithm

Update the vehicle pose

in Global Map

Is there any new

scanning data?

End

Yes

Feature

Extraction Module

(Model Set)

Layout

Lidar

Find the rotation &

translation matrix

Feature

Extraction

Module

Figure 4. Corner-feature-based pose estimation.

3.2. A Weighted Parallel ICP Pose Estimation Algorithm

An environment that has a similar layout generally results in good estimates. However, in

practice, it is not always the case. To verify the feasibility of the proposed method, the proposed

algorithm needs to be robust enough even in the presence of environment uncertainties such as

moving people.

Based on the results presented in Sections 2 and 3, a WP-ICP is proposed. The WP-ICP considers

two features for point clouds that are pre-processed: one is the corner feature and the other is the line

feature. Corners are important feature points in the environment as they are distinct, whereas walls

are stable feature points and a good candidate for feature extraction in structured environments

[15,31,36]. Taking advantage of LiDAR for detecting surroundings, walls can be represented as line

segments, which are composed of two corners. Furthermore, we also considered the center point of

a line segment as another matching reference point.

The motivation for such features are that indoor environments, e.g., offices and buildings, are

generally well structured. Therefore, feature-based localization is suitable for such environments.

Examining the traditional full-points ICP algorithm, point cloud registration is achieved by means of

the nearest neighbor search (NNS) concept. There is no further information attached to those points.

Th erefor e, it is easy to obtain inc orrect corres ponden ce as sh own in Figu re 5. Un der this ci rcu msta nce ,

several iterations are usually needed to converge the point registration. In addition, the ICP gives rise

to an obvious time cost for registration especially when the size of point cloud is large. To solve the

incorrect correspondence and iteration time cost issues, a feature-based point cloud reduction

method is developed.

For the proposed WP-ICP, the point cloud is first characterized by fewer corners and lines. This

has two main advantages: (a) First, the size of the point cloud is reduced significantly and thus

enhances the ICP speed. (b) Second, the polished points are labeled as corner or line features. Only

Figure 4. Corner-feature-based pose estimation.

3.2. A Weighted Parallel ICP Pose Estimation Algorithm

An environment that has a similar layout generally results in good estimates. However, in practice,

it is not always the case. To verify the feasibility of the proposed method, the proposed algorithm

needs to be robust enough even in the presence of environment uncertainties such as moving people.

Based on the results presented in Sections 2and 3, a WP-ICP is proposed. The WP-ICP considers

two features for point clouds that are pre-processed: one is the corner feature and the other is the line

feature. Corners are important feature points in the environment as they are distinct, whereas walls are

stable feature points and a good candidate for feature extraction in structured environments [

Taking advantage of LiDAR for detecting surroundings, walls can be represented as line segments,

which are composed of two corners. Furthermore, we also considered the center point of a line segment

as another matching reference point.

The motivation for such features are that indoor environments, e.g., ofﬁces and buildings, are

generally well structured. Therefore, feature-based localization is suitable for such environments.

Examining the traditional full-points ICP algorithm, point cloud registration is achieved by means of

the nearest neighbor search (NNS) concept. There is no further information attached to those points.

Therefore, it is easy to obtain incorrect correspondence as shown in Figure 5. Under this circumstance,

several iterations are usually needed to converge the point registration. In addition, the ICP gives rise

to an obvious time cost for registration especially when the size of point cloud is large. To solve the

incorrect correspondence and iteration time cost issues, a feature-based point cloud reduction method

is developed.

For the proposed WP-ICP, the point cloud is ﬁrst characterized by fewer corners and lines. This

has two main advantages: (a) First, the size of the point cloud is reduced signiﬁcantly and thus

Sensors 2018,18, 1294 7 of 19

enhances the ICP speed. (b) Second, the polished points are labeled as corner or line features. Only

those points labeled as corners can be matched to the corner candidates; similarly, only those points

labeled as lines can be matched to the lines candidates. The result is shown in Figure 6. Figure 6a

illustrates the matching condition for corners while Figure 6b represents the matching condition for

lines. The parallel-ICP results in correct point registration and thereby reduces the number of iterations.

Sensors 2018, 18, x FOR PEER REVIEW 7 of 19

those points labeled as corners can be matched to the corner candidates; similarly, only those points

labeled as lines can be matched to the lines candidates. The result is shown in Figure 6. Figure 6a

illustrates the matching condition for corners while Figure 6b represents the matching condition for

lines. The parallel-ICP results in correct point registration and thereby reduces the number

of iterations.

Figure 5. Incorrect point registration caused by the nearest neighbor search (NNS) Iterative Closest

Point (ICP) algorithm.

(a) (b)

Figure 6. Illustration of feature-based point cloud registration. (a) Corner features are matched to

corresponding corner features (b) Line features are matched to corresponding line features.

The main advantage of the WP-ICP is that the number of data points in a data set as well as a

model set can be significantly reduced. On the contrary, the full-points ICP algorithm includes all

data points for correspondence searching and thus leads to low computation efficiency.

Moreover, in the proposed WP-ICP, the scan points are clustered into the corner or the line

groups, respectively. Based on the parallel mechanism, the points from a corner set can never be

matched with the points from a line set. It can thus avoid mismatching during the point registration.

However, for full point ICP, many mismatches could happen once the distances between those two

set points are close enough.

Since the WP-ICP has two ICP processes at the same time, it generates two pairs of robot pose,

namely



CC tR , and



LL tR ,. Therefore, it is desired to fuse these two poses to come out with a

more confident pose estimate.

0123456 78

-2

-1

Full Points Model Set

Full Points Data Set

Corner Feature

0123456 78

-2

-1

Full Points Model Set

Full Points Data Set

Corner Feature Model Set

Corner Feature Data Set

012345678

-2

-1

Full Points Model Set

Full Points Data Set

Line Feature Model Set

Line Feature Data Set

Figure 5.

Incorrect point registration caused by the nearest neighbor search (NNS) Iterative Closest