Content uploaded by Qingqing L.

Author content

All content in this area was uploaded by Qingqing L. on May 15, 2020

Content may be subject to copyright.

1

Localization in Unstructured Environments:

Towards Autonomous Robots in Forests

with Delaunay Triangulation

Qingqing Li, Paavo Nevalainen, Jorge Pe˜

na Queralta, Jukka Heikkonen and Tomi Westerlund

Abstract—Autonomous harvesting and transportation is a long-

term goal of the forest industry. One of the main challenges is the

accurate localization of both vehicles and trees in a forest. Forests

are unstructured environments where it is difﬁcult to ﬁnd a group

of signiﬁcant landmarks for current fast feature-based place

recognition algorithms. This paper proposes a novel approach

where local observations are matched to a general tree map using

the Delaunay triangularization as the representation format.

Instead of point cloud based matching methods, we utilize a

topology-based method. First, tree trunk positions are registered

at a prior run done by a forest harvester. Second, the resulting

map is Delaunay triangularized. Third, a local submap of the

autonomous robot is registered, triangularized and matched using

triangular similarity maximization to estimate the position of

the robot. We test our method on a dataset accumulated from

a forestry site at Lieksa, Finland. A total length of 2100 m of

harvester path was recorded by an industrial harvester with a

3D laser scanner and a geolocation unit ﬁxed to the frame. Our

experiments show a 12 cm s.t.d. in the location accuracy and with

real-time data processing for speeds not exceeding 0.5m/s. The

accuracy and speed limit is realistic during forest operations.

Index Terms—Robotics; Localization; Delaunay triangulation;

SLAM; Forest Localization

I. INTRODUCTION

Over the last ten years, autonomous harvesting and trans-

portation have become the long-term goal of the future devel-

opment of the forest industry and attracted the interest of the

research community. Naturally, there are several intermediate

goals such as the organization of the public data [1], and

the gradual increase of autonomy in varying degrees, starting

with short-range transport in forests [2]. Incremental advances

in forest autonomy include driver assistance platforms and

function-speciﬁc automation. For example, these include the

automated selection of tree stems to be processed, micro-

tasks such as sequencing the processing of individual trees,

local route planning, or semi-autonomous transportation and

quality assurance. In all these tasks, one key element is the

availability of tree maps, together with methods enabling the

identiﬁcation and selection of individual trees. Besides, local

route planning requires accurate updates on the position of the

harvester inside the forest in real-time, which can not be relied

on global navigation satellite system (GNSS) sensors.

This work focuses on the real-time localization of an

autonomous forwarder unit, a forestry vehicle that collects

Qingqing Li, Paavo Nevalainen, Jorge Pe˜

na Queralta, Jukka Heikkonen

and Tomi Westerlund are with the Turku Intelligent Embedded and Robotic

Systems (TIERS) Lab, University of Turku, Turku, Finland, e-mails: {qingqli,

ptneva, jopequ, jukhei, tovewe}@utu.ﬁ

felled logs and hauls them to a local loading area. The required

transport distance is usually short, in the range of 100-400 m,

and happens along a rough-terrain track a forest harvester has

previously made while performing cut-to-length logging. To

autonomously navigate in the forest, visual sensors present

signiﬁcant challenges owing to the lack of a stable background

from which contours can be extracted, as well as demanding

low-light and harsh weather conditions [3]. For this type of

environment, with mostly empty space, clearly separated and

mostly homogeneous trees, ranging sensors show a wider

potential. In particular, in our work, we rely on Light Detection

And Ranging (lidar), a viable technology for this environment,

with commercial sensors able of centimeter-level accuracy,

over 100 m range, and wide ﬁeld-of-view.

There are multiple well-established frameworks and algo-

rithms for autonomous driving in urban environments [4], as

well as localization and mapping in roads and buildings [5].

Many of the solutions proposed in these areas have a strong

dependency on lidar scanners [6], among other sensors. In

the ﬁeld of forest mapping and navigation, several researchers

have utilized terrain laser scan (TLS) to build point-cloud

maps [7]–[11]. In some of these works [7], [9], data is

collected from ﬁxed-positioned tripods in order to gain an

understanding of how well individual trees can be detected and

their diameter at the breast height (DBH) can be registered.

Other studies rely on mobile laser scanning (MLS) [8], [10],

[12] to simulate the operation of the harvester.

Navigating in a forest presents several inherent challenges

owing to the complexity and lack of structure in the environ-

ment. A realistic forest harvesting process involves constant

obstructions from cut or fell branches and trunks and irregular

terrain. Furthermore, the movement of a harvester tends to

be rotational most of the time, with constant changes in

orientation along the work cycle and very slow translational

motion or even with no movement at all while trees are

being cut. These particularities of forest navigation bring

both advantages and disadvantages to standard development

of autonomous mobile robots: idle periods and slow motion

aid at realizing real-time operation and data processing, while

fast rotations difﬁcult accurate mapping. A small error in the

orientation estimation can signiﬁcantly affect the mapping of

trees that are farther away.

Research in the ﬁeld of localization and mapping for au-

tonomous robots can be roughly divided into two approaches:

simultaneous localization and mapping (SLAM), and sequen-

tial location and mapping. The main difference between these

2

two is that SLAM has the location and mapping occurring

at the same time, whereas the sequential version has the

mapping part completed after the track of the vehicle has

been constructed for a considerable length. Localization is

often done by ﬁnding either a partial match between sensor

data batches acquired at different times, or ﬁnding a spatial

transformation that maximizes the data matching. A different

approach is then to classify localization methods based on

whether the matching process is done globally or locally. The

same can be applied to different types of sensor data, includ-

ing images [13]. Nonetheless, we are interested in matching

point clouds generated from lidar scanners. Because locally

converging methods have a valley of convergence [14], which

is sometimes deﬁned e.g. by step length limit and the point

cloud overlap limit, we study a global point cloud matching

approach.

In this paper, our objective is to enable fast, real-time global

localization for autonomous forest harvesters and forwarders.

Our experiments have been done using data from a 3D lidar

scanner situated in the front panel of a manually operated

forwarder. The methods presented in this paper rely on the

assumption that a global forest map in the form of a point

cloud is available. In practice, we ﬁrst create the map using

state-of-the-art lidar odometry and mapping algorithms, and

then evaluate our localization approach. The localization is

global and does not depend on previous states or position

estimations, therefore providing a robust solution for long-

term autonomy.

A. Localization in Autonomous Mobile Robots

Autonomous mobile robots meant to operate outdoors often

rely on GNSS sensors as the basic source of global localization

data, and then integrate other sensor data through sensor fusion

techniques for local position estimation [6]. GNSS sensors

alone do not provide enough accuracy in urban or dense

environments, with accuracy often in the order of meters [15].

While GNSS sensors have increased their accuracy in recent

years, multiple challenges remain in environments with struc-

tures affecting the signal path. In particular, GNSS signals are

weak in forests, owing to the irregular land contours and the

coverage that tree foliage provides [16]. Therefore, we focus

on the design and development of localization methods that

rely solely on lidar data, with GNSS providing only the initial

position before entering the forest or starting the autonomous

operation. The following are the main approaches enabling the

estimation of the movement of a mobile robot based on lidar

scans by either comparing consecutive scans (hereinafter also

called point clouds) or a given scan with a global point cloud.

We refer to comparing scans as scan matching:

Full scan matching. In SLAM algorithms, a necessary

step previous to update the map is to estimate the relative

movement of the robot with respect to its last known position.

This is equivalent to ﬁnding a geometrical transformation

between the corresponding point clouds. One of the most

widely utilized methods for calculating such transformation

is the iterative closes point (ICP) algorithm [17], [18]. In

ICP, pairs of points between the two point clouds being

compared are iteratively selected until a transformation with

acceptable pairing error distance is found. Several variants of

the ICP algorithm are available through the open-source point

cloud library (PLC) [19]. Other popular methods include the

perfect match (PM) [20], or normal distribution transforms

(NDT) [21] algorithms. Most of these algorithms can be ex-

tended and utilized for map-based localization, where a global

point cloud is available and a given lidar scan is matched

with only a subset of the global map. However, in these

cases, an estimation of the previous position is needed, since

global point cloud matching might be unfeasible due to the

computational complexity. Moreover, ensuring convergence

in global matching presents signiﬁcant challenges. Since our

objective is to enable global localization without dependence

on previous states, we develop a novel method that takes into

account the geometry of the environment instead of using the

full point cloud.

Feature-based matching. Generic point cloud matching

algorithms do not take into account the structure of any poten-

tial features within the scans that are being compared. When

the environment where robots operate has known structures,

feature-based scan matching can signiﬁcantly enhance the

accuracy of the matching process [22]–[24]. Moreover, owing

to the preprocessing step in which raw data is transformed

into features, the computational time required to calculate

the transformation can be reduced. In this direction, Zhang

et al. presented the lidar odometry and mapping (LOAM)

method [25], which assumes a structured environment where

planes and edges can be detected. Then, the transformation

between two point clouds can be estimated by aligning the

planar and edge feature points from each of them. Multiple

algorithms have extended the original LOAM method for

other types of features. Among them, Shan et al. presented

the ground-optimized Lego-Loam [26], which delivers opti-

mized real-time three-dimensional point cloud matching in

small scale embedded devices. While feature-based matching

algorithms yield accurate results, their applicability is limited

in forests, with most methods focusing on urban or indoor

environments. In forests or other unstructured environments,

edge and planar features are either noisy or missing. In

this paper, we still rely on the state-of-the-art Lego-Loam

algorithm for building a map of the forest prior to the operation

of the robots. This process is done ofﬂine. However, we

develop an alternative method for localization because of our

needs for real-time operation and global localization.

Geometrical matching methods. Geometry-based localiza-

tion algorithms have the potential to recognize the position in

constant time, which is an attractive property towards real-time

localization with embedded processors. Geometrical methods

are often applied to graph-like structures to ﬁnd geometric

transformations that match either the complete graph or a

set of subgraphs. Thrun et al. developed the notions of

sparse extended information ﬁlters (SEIFs) which exploit the

structure inherent through the local web-like networks of

features maps [27], [28]. In a similar direction, Ulrich et al.

proposed a global topological representation of places with

object graphs serving as local maps for place classiﬁcation

and place recognition [29]–[31]. Among the most relevant

3

approaches to our work are place-recognition algorithms, such

as Bosse’s work on a keypoints similarities voting method in

2D and 3D lidar data [32]–[34].

B. Contribution and Structure

Most of the aforementioned point cloud matching algo-

rithms focus on structured urban environments, or trajec-

tory tracking through ﬁxed, predeﬁned paths such as roads.

Nonetheless, their performance is signiﬁcantly degraded in

unstructured environments and, in particular, in forests. In a

forest harvesting operation, harvesters and forwarders travel

through undeﬁned paths. Moreover, the detection of potential

features (ground and tree stems) is considerately affected by

irregularities (branches and foliage in the trees, and small

plants, rocks or fell trees in the ground). While some of

the state-of-the-art methods yield reasonably good results

for mapping and odometry, our aim is to develop a fast

method for global localization able to run in real-time and

taking into account the known geometrical properties of forest

environments. Therefore, in this work, we pursue the design

and development of a localization method which is reliable,

accurate and of low computational cost. The computation

is aimed to occur in real time onboard of harvesters and

forwarders in the forest. Relatively randomly located but

ubiquitous tree stems are a natural basis for localization efforts.

We also assume that a map of the forest in the form of a point

cloud is available before the mission starts.

We propose a lightweight and geometry-based localization

method for pose estimation within a global map of a harvester

in dense and unstructured forest environments. The proposed

method is lightweight and matches a triangularization of a

single lidar scan with a subgraph of the triangularization of

the global forest map. Compared to matching raw lidar data

point-by-point, we reduce the problem to a triangularization

created based on the positions of tree stems. This allows us

to reduce the amount of point to be matched to approximately

1% of the size of a single lidar scan because individual trees

get an average of 100 laser hits in our setting: a 16-channel

3D lidar with a ﬁeld of view of about 210° and situated at

a height of 1.5 m. Closer trees get, however, considerably

higher amounts of laser hits. The proposed method is based

on triangle dissimilarity minimization, and that is why we say

the method is geometry-based. To the best of our knowledge,

this is the ﬁrst paper to utilize Delaunay triangulation for

the purpose of localizing a vehicle in a forest environment.

Moreover, we provide a fully experimental approach with a

realistic use case in forest harvesting. The main contributions

of our work are the following:

•the introduction of a Delaunay Triangulation graph map

for localization in forests; and

•the design and development of a framework to solve the

vehicle tracking problem in a local coordinate system

relying solely on lidar data, without GNSS or inertial

sensor data.

The remainder of this paper is organized as follows. Section

2 introduces the study area and equipment, the optimization

problem to be solved, and the methodology we have followed.

Section 3 reports experimental results. In Section 4, we discuss

the potential improvements, as well as the main beneﬁts of our

approach. Finally, Section 5 concludes the work and outlines

future research directions.

II. MATE RIAL AND RES ULTS

A. Study area

The study area covers one mature pine stand ready for

second thinning, which represents a typical second thinning

forest for Finnish standards. The location of the study area

is illustrated in Figure 1. The terrain proﬁle was mostly ﬂat,

the maximum height difference over the site was 8 m. The

overall trail length is approximately 1200 m covering roughly

2200 trees.

B. System Hardware

The mobile platform was a Komatsu Forest 931.1 forest

harvester with a GPS unit and the lidar unit attached to the top

of the cabin. The harvester has physical dimensions (length,

width, height) of 7.6 m by 2.9 m by 3.9 m, and its mass is

19610 kg.The maximum driving speed is 8 km/h off-road and

25 km/h on road.

The on-board lidar is Velodyne-16, a 16 channel lidar with

3cm distance accuracy, 360° horizontal ﬁeld of view, 30° ver-

tical ﬁeld of view with ±15oup and down, 5-20 Hz scanning

frequency and 100 m scan range. As Figure 2 shows, the lidar

is ﬁxed on the front of the harvester with a 210° horizontal

view. The harvester has a folding frame, which means the point

cloud frames scanned have constantly alternating horizontal

orientation during the work cycle.

The GPS data was recorded by Spectra SP60 GPS

unit. This unit fully utilized all 6 GNSS systems: GPS,

GLONASS, BeiDou, Galileo, QZSS and SBAS. In SBAS

(WAAS/EGNOS/MSAS/GAGAN) mode, the horizontal posi-

tion error smaller than 50 cm, and the vertical error is smaller

than 85 cm. In differential GPS mode, the accuracy is able

to reach 25 cm in horizontal and 50 cm in vertical accuracy.

In Real-Time Kinematic position (RTK) mode, the accuracy

can reach 8 mm in horizontal and 15 mm in vertical accuracy.

Nonetheless, these values are not achievable under dense

foliage in a forest environment. Regarding the computing

platform, the proposed methods have been tested on an Intel

Core i7-9700K CPU (8 cores, up to 3.60GHz), and 16 GB of

RAM.

C. Methodology

Figure 3 shows an overview of our proposed method. The

complete system takes as a sole input the point cloud data

from the 3D lidar, and outputs the position and orientation

estimation in the reference of the given global map. The

system can be divided into six steps as illustrated in Figure 4.

The former two steps are done ofﬂine by processing the point

cloud data deﬁning the forest map. The latter four steps are

then done online onboard the harvester to estimate its position

in real time. The steps are:

4

Fig. 1: (a) The test site in Lieksa, Eastern Finland. (b) The forest canopy map from Google maps. (c) Point cloud map of the

study area.

Fig. 2: (a) Komatsu Forest 931.1 forest harvester. The lidar

scanner is marked by a red circle in the front of the windshield

of the cabin. (b) A close-up of the lidar scanner.

Point

Cloud Lidar Mapping

Global Map Trunk Extraction Delaunay

Triangulation

Location

Estimation

Trunk Extraction Delaunay

Triangulation

Fig. 3: System overview: sensor data is matched with a global

map to produce a global position estimation. Consecutive runs

are independent.

1) Point-cloud trunk segmentation from the global map

(ofﬂine).

2) Delaunay triangulation of the global map from the

segmented trunk points (ofﬂine).

3) Aggregation of 3D lidar scans into a local point cloud

deﬁning the robot’s position (online).

4) Segmentation of trunks from the local point cloud (on-

line).

5) Delaunay triangulation from the local segmented trunk

points (online).

6) Estimation of a geometrical transformation that matches

the local Delaunay triangulation with a subset

A 2D Delaunay triangulation (DT) process [35] takes in

a set of planar points V⊂R2and produces a triplet G=

(V, E, T ), where E⊂V2is a set of edges and T⊂V3is

a set of triangles with the so called Delaunay property i.e.

the circumscribed triangle associated with each triangle t∈T

contains no points v∈Vothers than the three vertex points

of the triangle t.

The ﬁrst step in the proposed method takes a global point

cloud P Cmap ={Pi}i=1..nm⊂R3, representing the map

of the forest, and produces a robust set of trunk positions

P Ctrunks ={Pi}i=1...nt⊂R2. Note that P Ctrunks is not

a subset of P Cmap. The second step in our method then

subjects the horizontal plane projection P Cmap to the Delau-

nay triangularization graph Gmap = (Vmap, Emap , Tmap)for

matching. As we mentioned earlier, both if this steps happen

ofﬂine before the robot is deployed, or every time the robot

gets a global map update.

The third, fourth and ﬁfth steps, which happen online, cover

the generation of a local, real-time Delaunay triangulation

of the trees within the ﬁeld of view of the robot. First, we

accumulate and aggregate several raw point cloud scans from

the 3D lidar and generate a local point cloud P Clocal ⊂R3.

The aggregation relies on real-time lidar odometry from

LOAM [25]. Then, following the same procedure as with the

global map, we generate a robust two-dimensional set of trunk

positions Ltrunks ⊂R2. From the set of trunks, we can deﬁne

the local DT graph Glocal = (Vlocal, Elocal , Tlocal).

Finally, in the sixth step in the process we calculate a rigid

body transformation, deﬁned by a rotation θand a translation

pt, between the local DT graph Glocal and a subset of the

global DT graph Gmatch ⊂Gmap. The transformation relates

directly to the robot position and orientation in the global map.

Instead of matching large quantities of the 3D point cloud, the

proposed system seeks to match a triangularized representation

of the local data against a similar precomputed triangularized

global representation. The resulting process is computationally

efﬁcient and with a low memory demand.

In the rest of this section, we further describe the process

outlined above and delve into the details of the most critical

steps. In general, the key idea is building a unique 2D graph

topology from the 3D point cloud map, and ﬁnding the best

match between local and global topology.

5

1) Pre-processing : point cloud global map trunk segmentation 2) Pr-processing : global map DT graph generation

3) Local submap building and trunk segmentation 4) Local submap DT graph generation

Matched result based on estimation

5) Similar triangles matching

Search the similar

Triangls on the local

DT graph and global

DT graph

Match the similar corresponding

Triangles vertices, estimate the

transltion and orientation

between local and global

P2

P1

P3

P4

P6

S1

S0

S2

S3

P5

Fig. 4: Schematic representation of the proposed method to predict the transformation between local observation submap and

global map.

(a) (b) (c)

Fig. 5: Trunk point cloud segmentation approach. (a) The original point cloud map. (b) The extracted trunk point cloud (blue

and green) after removing ground and branch point cloud (red). (c) The ﬁnal extracted trunk point cloud (green) after clustering

process

6

D. Trunk Point Cloud Segmentation

With a given map P Cmap of the forest, an essential step in

the proposed method is to extract the trunks points that deﬁne

P Ctrunks as a set of landmarks from P Cmap . Compared to

the other environmental features such as the bumpy terrain,

branch structures, forest ﬂoor vegetation and the bushes in the

forest, tree trunks are a signiﬁcant and relatively noise-free

feature.

To segment the trunk points, the ﬁrst step is extracting the

trunk point cloud P Ctrunks from the map point cloud P Cmap

following the methodology described in Algorithm 1. We em-

ploy the Kd-Tree space partitioning structure to accelerate the

neighborhood search [36], together with a Euclidean Cluster

to ﬁnd the trunk point cloud’s [37]. Instead of focusing on

ﬁnding every trunk in the P Cmap, we focus on extracting the

most signiﬁcant ones. We deﬁne the most signiﬁcant trunks

in this paper as those that show observational stability, i.e.

they can be assumed to be easy to observe by the robot in the

near future locations. Thus, we do not consider small trunks or

bushes, which are not a worthwhile computational investment

to locate and give them a landmark status. As a typical trunk

is an approximately vertical object, we assume that the point

from a stable trunk has a neighboring point located 2mabove

it. Then, we traverse all points in the P Cmap, and for each

point pi= (xi, yi, zi)∈P Cmap we try to ﬁnd the nearest

neighbor of a hypothetical point p0

i= (xi, yi, zi+th)which

is thm above the point pi. We add all such points pito the set

P Ctrunks , which truly have near neighbors above them. Next,

we form Euclidean clusters (each cluster connected by close

distances between the members) and drop the clusters that

have a size smaller than a threshold value cluster threshold.

Finally, we compute the mean ¯p∈R2of the horizontal

projections of the cluster points to represent a landmark trunk.

These landmarks then compose the 2D trunk map Mtrunks .

Figure 5 show the proposed trunk point cloud segmentation

result. The ﬁgure 5(a) shows the map to be processed, the

ﬁgure 5 (b) shows the extracted point cloud after removing

the ground point cloud and branch point cloud from the input

map. The ﬁgure 5 (c) shows the ﬁnal extracted stable trunk

point cloud after cluster processing.

E. Global Map DT Graph Generation

A DT has multiple beneﬁcial properties for our problem

setting [38], including the fact that it maximizes the minimal

angle at all the vertices of the triangulation. This means

that the noise in angular values is minimal in a noisy point

set. A DT also deﬁnes the so called natural neighborhood

around a point (the point set connected to a given point along

the triangle edges), which solves the problem of setting the

local point cloud feature scope or number of neighbors in an

intuitive way [35].

Our objective is to match the local trunk graph Glocal

against a subset of the global trunk graph map Gmap, and

the prerequisite is that there exist some similar structures

like triangles or small sets of triangles in both graphs. From

the trunk landmarks Mtrunks of the P Cmap obtained as

Algorithm 1: Extracting trunks from the point cloud

map

Data: A 3D point cloud map P Cmap ={pi}i=1..nm

Result: Trunk segmentation point cloud

P Ctrunks ={Pj}j=1...Nt,

2D Trunk map Mtrunks ={Mk}k=1...nt

1foreach p∈P Cmap do

2Sear chP oint :psp = (p.x, p.y, p.z +hth);

3N earestP ointSear ch :psn =

nearestKSearch(Pm, psp);

4ComputeDistance :distance(psp, psn );

5if distance < d threshold then

6Pt←pi// Add point to trunks point cloud;

7end

8end

9foreach cluster ∈Clusters do

10 if Cluster.points.size() < cluster threshold

then

11 Delete cluster from Clusters

12 else

13 ¯p=mean p∈Cluster p;

14 Mtrunks ←¯p;

15 end

16 end

described in Section II-D, we get the Delaunay triangulation

Mtrunks →Gmap = (Vmap , Emap, Tmap).

F. Local Map and Point Cloud Aggregation

In a dense forest environment, nearby trunks may be

blocked from the lidar view. This may result in not enough

stable tree trunks to match against the global map. Thus, in

our case, we will employ the LOAM method, which builds a

local map from aggregating several consecutive observations.

However, we will also explore the direct construction using

only one frame observation.

Both the construction of the ground truth map and the

aggregation of consecutive lidar frames rely on the LOAM

method [25]. It operates as follows. Let Pt={pi}i=1..n rep-

resent a raw scanned point cloud received at time t. All these

raw point cloud’s are processed with the LOAM algorithm

to build a local map based on one scan or the aggregation

of several consecutive scans. The LOAM algorithm is a

state-of-the-art feature-based lidar odometry algorithm. LOAM

receives the 3D point cloud from the lidar, and projects the

point cloud onto a range image for feature extraction. By

calculating the curvature and some features from each row

of the range image, the registration process selects subsets

Peand Pp(edge and plane points, respectively). Instead

of comparing all the points, LOAM utilizes only those two

subsets to ﬁnd a transformation between scans, and then a two-

step Levenberg-Marquardt optimization method is employed

to optimize the six-degree-of-freedom transformation across

consecutive scans. The complete lidar odometry problem gets

solved with a speed of 1 Hz resulting the local map P Clocal

with the computing platform utilized in our experiments.

7

Fig. 6: (a): The target triangle S0with its neighbors Si, i =

1..3.(b): The all similar triangles found in the local trunk map,

(c): the corresponding triangles found in Global trunk map

In this study, the local point cloud P Clocal was generated

with one or several scans by utilizing the estimated position

from the LOAM method. The raw lidar output frequency is

30 HZ, but not every scan from the lidar stream needs to

be used to calculate the robot odometry. To reduce the ac-

cumulation error and balance the computation burden, LOAM

only considers those scans that the Euclidean distance between

two observation positions is longer than a certain threshold

distance (e.g. 30 cm) or the angular change is larger than a

certain threshold angle (e.g. 30°). As the nearby trunks or ob-

jects may block a sector of the view, aggregating more frames

observation from consecutive positions helps to increase the

number of the stable trunks registered into the map P Clocal.

G. Local DT Graph Generation

After we have obtained the local map P Clocal, the next step

is to generate the DT graph Glocal ={Vlocal, Elocal , Tlocal},

just as with the global map. The methodology explained

in Section II-D applies in this case as well, but this time

producing a local map Llocal, its 2D projected subset Ltr unks,

and a local DT graph Glocal.

H. Matching Local and Global DT

The DT graph obtained from the global point cloud map,

Gmap = (V, E, T ), deﬁnes a set Vof vertices, a set E⊆V2

of edges and a set T⊆V3. Each vertex represents a trunk

in the point cloud map. Edges establish a relation between

a point and its natural neighbors it is connected within the

graph. For each local DT graph that we obtain by aggregating

consecutive lidar frames in real-time, our objective is to ﬁnd a

matching subgraph for Glocal in the graph Gmap. We proceed

as follows in order to obtain such subgraph.

Triangle search based on dissimilarity. A dissimilarity

d(., .)of two triangles t1and t2has two properties: it is always

positive, 0≤d(t1, t2), and zero in case of identity, d(t1, t1)≡

0. Typical triangle dissimilarities use an intermediate vector of

two descriptors, and some sort of norm between these vectors.

For instance, in [39] the authors utilize the ratio of the lengths

of the shortest and longest edges, and the angle between those

edges. We utilize the intermediate vector (A, l)of a triangle t

with an area Aand l=L2, where Lis the perimeter length.

The vector components have the dimensionality of area (in

square meters). Then, the dissimilarity d(., .)is deﬁned as:

d(t1, t2) = |A2−A1|+|l2−l1|.

To speed up the real-time matching process, all triangle

perimeters and areas of the global DT graph have been

computed ofﬂine. In terms of comparing triangles based on

the magnitude of the difference of (A, l)vectors, the triangles

composed by peripheral points in a graph are usually different

because of fewer observed points, so only triangles Tselected

which not include a peripheral point in the local graph Glocal

are selected. Therefore, a set of candidate triangles {Tselected}

are selected and used to ﬁnd a matching subset in Gmap. From

{Tselected}, we build the corresponding graph

There may exist multiple closely similar triangles Tcandidate

in Gmap to any speciﬁc selected triangle T∈ {Tselected}

in Glocal. Thus the next step is to compare the neighboring

triangles too. As we have excluded the peripheral triangles, the

global subset Gselected that will be the match of {Tselected}

should also have three neighbor triangles. The triangle neigh-

borhood vote is performed using the same dissimilarity mea-

sure. As we show in Figure 6, the triangle S0is one of the

selected triangles S0∈Tselected in the local graph Glocal, and

the three triangles S1, S2, S3are its neighbors.

A feature vector of the triangle S0used in the match process

is combined from the intermediary dissimilarity vectors of the

triangle S0and its neighbors (called a triangle star) according

to (1).

features(S0)=[A0, l0, A1, l1, A2, l2, A3, l3](1)

By comparing the features(S0)of a triangle S0to feature

vectors in Tselected, a set of matching triangles Tcandidate

meeting the error tolerance may be found. After a pair of simi-

lar triangles (S0, S0

0)∈Tselected ×Tcandidate ⊂Glocal ×Gmap

are found, the next would be to estimate the position ptand

the orientation θ, which matches Tselected with Gmap.

Calculation of corresponding vertex pairs. In order to

describe the matching process, we use the following notation.

A triangle S0=ABC consists of vertices A,B, and C, and

an edge vector ~e =~

AB of an edge e=AB is oriented and

signiﬁed with its end points.

To solve the transformation parameters ptand θ, we ﬁrst

need to ﬁnd the correspondence between vertices of trian-

gle stars S={Si}i=0..3∈Tselected and triangles S0=

{S0

i}i=0..3∈Tcandidate. The deﬁnition of a triangle star

is easily seen from Figure 7 (a). As the ﬁgure shows, the

vertex match between Tselected and Tcandidate can be divided

into three steps. The ﬁrst step is in the detail (b). We then

ﬁnd the ﬁrst matching pair S0=ABC ∈Tselected and

S0

0=MHN ∈Tcandidate. Through comparing the side

lengths between two triangles, an edge BC of the ABC can

be selected so that there is an edge |NH|with similar length

in the MHN, so the remaining vertices Aand Mare a pair

8

of corresponding vertices. For the second step, the goal is to

ﬁnd the other corresponding vertices in ABC and M HN. As

Figure 7 (c)shows, selecting one edge which has the vertex

Aas already known, and one edge contains the vertex Min

the 4MHN, then we compute which side of the edge the

remaining vertices Cand Hare located. This happens by

inspecting the value a∈Rof the following formula:

a= ( ~

CA ×~

CB)·(~

HM ×~

HN)(2)

If a > 0, the vertices Cand Hare on the same side

of the edge AB and edge MN, otherwise the two vertices

are on the opposite. In the case shown in Figure 7 (c), the

result is a < 0, so the vertex pairs are (C, M )and (B, H).

The last step is to match vertices in the triangle neighbors.

Since we already know the correspondence between triangles

ABC and M H N , we just need to get which two vertices are

included in the neighboring triangles. For example, as we get

the corresponding pairs of vertices (A, M )and (B, H)from

last two steps, the last vertices Eand Qare a match, too.

The match between vertices of two triangle sets is now com-

plete. The match relation is one-to-one, so it can be recorded

as a permutation function: f: [1,6] ⊂N→[1,6] ⊂N.

This way each vertex Vi∈Tselected has a matching vertex

Vf(i)∈Tcandidate. Next we will use the permutation fwhile

estimating the best possible translation and rotation.

1) Rotation and Translation Estimation: By comparing

the vertex orientations in the found two triangle sets S=

{Si}i=0..3∈Tselected and S0={S0

i}i=0..3∈Tcandidate, we

can ﬁnd a unifying 2D rigid body transformation T[pt, θ]be-

tween the local sample and the global map. The transformation

Tconsists of a rotation of the angle θfollowed up with a

translation pt.

To calculate the transformation parameter ptwe utilize the

deﬁnitions in (3), where the translation tpis the best possible

one estimate, since it equates the mean of two patterns. The

angle θis deﬁned by measuring how large a rotation is needed

to transform a vectors Vi−Vto V0

f(i)−V0, where the V

represents the mean of Tselected and V0is the mean of the

Tcandidate.

There are six pairs i= 1...6of vertices coupling Tselected,

Tlocal and Tcandidate from Gmap, and six possible candidates

βito be chosen as the ﬁnal local rotation θ. We choose the

value of βiwhich ﬁts the triangle patterns Tselected and Tlocal

with the least error. One important detail is arranging a signed

angular measure between two vectors. We use a rectangular

rotation matrix P=0−1

1 0 to get a positive sign sign(P a ·

b)for rotation angles, which move a vector ato a vector b.

We omit possible ﬁt minimizing values in between the angles

{βi}i=1..6because the objective at this stage is to only have

a close estimate and not the ﬁnal position estimation.

Using a zeroth power as a shorthand when producing unit

vectors a0=a/kak, we deﬁne counter-clockwise oriented

angles βi, which rotate vectors Viparallel to vectors Vf(i).

Finally, we deﬁne approximate values for the translation pt

and the rotation θaccording to (3).

V=mean V∈Tselected V

V0=mean V0∈Tcandidate V0

pt=V0−V

βi=sign P Vi·Vf(i)arccos V0

i·V0

f(i), i = 1..6

θ=argmin βi, i=1...6

X

j=1...6

"cos βi−sin βi

sin βicos βi#Vj−VT−Vf(j)−V0T

(3)

2) Geometric Veriﬁcation, Final Translation and Rotation

Estimation: From the Glocal match against Gmap, usually

multiple matches {(Sk,S0

k)}k=1..m between Tselected and

Tcandidate can be found. Therefore, we need to select one

among mtransformation candidates with rotation and transla-

tion parameters θkand tk. This ﬁnal step is to ﬁnd the best

estimation between Glocal and Gmap. Let the corresponding

points of a match candidate be (Vlocal, Vmap ). Starting from

the previously computed initial guesses θand pt, we select the

ﬁnal solution from (4). Note that this does not ensure global

convergence, and unexpected results might be obtained if the

global map is too homogeneous, not enough trees are detected,

or the map is large and multiple positions yield similar error.

Through our experiments, nonetheless, we have been able to

conﬁrm stable position estimation when enough consecutive

lidar scans were aggregated.

(θ, tx, ty) =

argβ,xt,ytmin

β∈B

x∈Dx

y∈DyX

j=1..m

cos β−sin β x

sin βcos β y

0 0 1

hVT

j−VT

f(j)i

(4)

where mis the number of local matches, β∈B=

[min θjj=1..m,max θjj=1..m ]⊂Rgoes through the scope

occurring in the local rotation angles, and xgoes through

a similar scope of Dxoccurring in translations along the x

axis. Dyis deﬁned correspondingly on the yaxis. Note that

both (3) and (4) require the vertices to be in a homogeneous

form V→[V1]. The local start point is (x0, y0)with an

orientation of θ0, and the ﬁnal estimation of the robot position

is (x0+tx, y0+ty)with an orientation θin the global map.

III. RES ULTS

This section presents our experimental results. We analyze

the performance of our method from the point of view of the

trunk segmentation as well as the DT matching between the

local graph and a subset of the global graph.

A. Trunk Segmentation Performance

Figure 8 shows the number of trunks that can be observed

from a single lidar scan, and when aggregating three, ﬁve or

9

A

D

E

F

(a)

B

C

P

M

N

H

P'

H

N

P'

M

BC

P

P

A

BC

P

P

A

H

M

N

P'

Q

R

T

A

D

E

N

H

P'

Q

R

T

M

F

B

C

P

(b) (c) (d)

Fig. 7: (a): The selected triangle Tselected in Glocal with candidate matched triangle Tcandidate in Gmap (b): Find the ﬁrst

correspond point Ain Tselected with it’s Min Tcandidate by ﬁnding the similar featured side length (c): Find the rest points

B,cand it’s correspond points H,N.(d): Find the correspond neighbor points E,D,Fand it’s correspond points Q,R,T.

Fig. 8: Number of trunks observed for different number of consecutive aggregated frames.

ten consecutive frames. In Table I we show the average number

of trunks observed, as well as the average number of trunks

that can be matched with the global map after generating

the DT graph. We also include the overall matching success

rate, which is deﬁned as the number of local DT graphs

that have been successfully matched with the corresponding

subsets of the global DT graph. From individual lidar frames,

we were only able to obtain an average of 54.09 valid trunks

in total. When aggregating consecutive scans, we were able

to obtain approximately a twofold increase in the number

of detected trunks, and a tenfold increase in the number of

matched trunks. The number of aggregated frames thus has

a signiﬁcant impact on the performance of our algorithm.

As the number of trunks and their positions determines the

appearance of the local DT graph, this will directly affect the

number of features detected by our descriptor. The number of

trunks observed is the key factor inﬂuencing the overall results

of our algorithm. From Table I we can see the relationship

between the DT matching success and the number of trunks.

With a single frame observation, only an average 1.71 trunks

are successfully matched; with three aggregated frames, there

are in average 4.97 trunks matched; with 5 and 10 aggregated

frames, there are more than 10 trunks in average that our

algorithm can match with the global map, taking the overall

DT match success rate to 100 % in the latter case.

B. Computational Time and Accuracy

The key matching process is based on the calculation and

matching of the Delaunay triangulations, which is in turn

based on the aggregation of 1, 3, 5 or 10 lidar frames.

10

Fig. 9: Number of trunks matched between Glocal and Gmap for different number of consecutive aggregated frames.

TABLE I: Number of trunks detected and success match for different number of consecutive aggregated frames (average

number of trunks matched per scan and total ratio of successful matches).

#Aggregated Frames Average #Trunks Average #Matching Triangles Overall Match Success Rate

1 54.09 1.71 22.89 %

3 122.98 4.97 88.79 %

5 139.54 10.22 99.28 %

10 133.96 14.37 100%

These four possibilities with a varying number of aggregated

frames are labeled as C1, C3, C5 and C10 in Figure 10. The

aggregation of frames adds complexity and requires computing

time both in the local map creation phase (black line) and

the similarity matching time (blue line). We can see that the

rotational location error signiﬁcantly improves when aggregat-

ing more frames. The computing time of the match process

grows approximately in terms of O(∆−2

r), where ∆ris the

angular accuracy achieved by different choices C1, C3, C5,

C10. In summary, the computation time is about 0.1seconds

for a single frame (C1) and 0.6 seconds when aggregating 10

frames (C10).

To test our pose estimation accuracy, we used the original

LOAM position as the ground truth. Figure 10 depicts only

the rotational error, since the translation error was approx.

0.2 m with an standard deviation of 0.14 m in all cases. It

is worth noting that the rotational error is not cumulative,

since the match between the local and global DT graphs is

being done separately and independently at each location.

The previous location is only used as the initial state. The

maximum occurred translation error was approx. 0.5 m and

the maximum rotation error 2.23°, both for the C1 scenario.

We can also see that the match time is relatively stable in

the C1-C5 scenarios. The local map creation time (aggregation

of frames) stabilizes as the number of frames increases, owing

to the stabilization in the number of trees that can be observed.

The segmentation algorithm is implemented in C++, while the

matching process is implemented in Python.

0.2 0.25 0.3 0.35 0.4 0.45

r (o)

0

0.05

0.1

0.15

0.2

0.25

0.3

0.35

0.4

Computing time (sec)

Accuracy vs. computing speed

local map creation

match time

C1

C1

C3

C3

C5

C5

C10

C10

Fig. 10: Dependency of the rotational accuracy and computa-

tion time on the choice of the number of aggregated frames.

Both the local map creation time (black) and the similarity

matching time (blue) are depicted. Number of frames ranges

from 1 to 10 (C1, C3, C5, C10). Ellipses represent the s.t.d.’s

of the measurements.

IV. DISCUSSION

An application topic of the proposed navigation system is an

automated transport robot, which moves logs from the forest to

11

the collection road. Since work happens in two phases, where

a forest harvester produces stripping roads and fells trees, and

the collection phase, a robot can assume to have the global map

built by the harvester which is not necessarily autonomous.

This way its task is just to orientate itself along the strip road.

A challenge arises from the high utility load of the transfer

robot, which can be up to 8000 kg. A transfer robot may not

have an as big capacity, but the ability to locate itself, and to

detect possible stability risks is a requirement.

Also, a transfer robot moves faster than the harvester going

through its work cycle. A good estimate in rough terrain is

1.0 m/s. Thus, a fast and accurate odometric computation is

essential.

A. Topology mapping

In this paper, the forest trunk topology map is generated

from the previous lidar data by the harvester. There are also

two alternative ways to generate the global map: a large-scale

(e.g. nationwide) digital forest inventory, or an aerial laser

scan (ALS) from a drone working as an autonomous team

member of the harvester and the robot carrier. Compared to the

point cloud map, the topology map has the potential to do fast

localization and dynamic update without much computation

burden and makes a 3 actor teams (harvester-drone-robot) an

interesting target of further study.

B. Potential accuracy improvements

From the experiments, we can see that the number of

trunks detected in each observation signiﬁcantly inﬂuence the

result of the success matching ratio. In our method, due to

the LIDAR sensor characteristic, far trunks are unable to be

recognized because of too few LIDAR points sampled from

the appearance. Therefore, we can employ other sensors like

the depth camera or RGB camera which can obtain more

detail of the trunk appearance, which lets the system get more

capability to recognize the trunks from PC.

The other signiﬁcant error exists in the trunk position

estimation. In our method, the Gmap and Glocal are generated

from the segmented trunks points in P Cmap and P Clocal by

calculating the average coordinate of points. However, it’s

impossible to get each individual trunk whole appearance

observation with several consecutive lidar observations. As the

observation in different positions will get different point cloud

data of different sides of the trunk, the estimation pose of the

trunk will be different in each local graph. However, the global

graph has a more accurate trunks position as more details of

each trunk collect from different sensors and different views,

so the more accurate trunk position estimation in the local

map can increase the chance of successful matching between

local graphs and global graph. To reach a more robust system,

we also can utilize other sensors like a camera through sensor

fusion to get more details about each individual trunk.

C. Potential computational improvements

We proposed an efﬁcient, robust framework to locate the

robot harvester in large area forest and we tested it on a real

harvester. In our case, the GNSS/GPS info is not taken into

account. The location accuracy is approx. 0.3 m from conse-

quential ﬁeld measurements and approx. 2 m, when comparing

to general odometric result. This accuracy is enough to give a

robot an initial position, and this can accelerate the matching

process by conveniently initializing the search with a close

match. For a real application, the GPS also can help decide

which global point cloud map it will access from the cloud

server for the localization.

The local Delaunay triangularization generating the local

trunk map Tlocal can be implemented in a radius-limited way

by using the S-Hull method [40], which limits the size nof

the computational task with the well-established complexity

O(nlog n). A selection of an active subset of triangles can

be made in the global map Tlocal based on the previous

localization result. Together these improvements have the

potential to make the matching process much faster.

There is a possibility to speed the convergence of the search

of the ﬁnal transformation in (4) by using a branch-and-bound

algorithm [41] with properly set estimates for local extreme.

This possibility is left for future research.

All in all, the C1-C5 cases are applicable to the intended

situation where the robot moves at approx. 1 m/s and has a

sampling rate 2 scans/sec. The case C10 is within the reach

after implementing the above-mentioned improvements.

V. CONCLUSION

In this study, we proposed a simple yet effective

segmentation-based approach to detect trunk position and De-

launay Triangulation(DT) geometry-based localization method

for autonomous robots navigating in a forest environment.

The proposed methods can provide accurate positioning based

only on real-time lidar data processing in the unstructured

and relatively complex environments that forests represent.

The proposed method can be utilized for harvesters or other

autonomous robots enabling fast global localization and rec-

ognizing individual trunks in real-time.

Our experiments show the proposed method reach accurate

global localization precision without a good initial pose or

GPS signal. The proposed method is simple and efﬁcient, and

it is a sensible solution to meet localization needs of harvesting

operations in the forest. In future work, we plan to explore

the forest localization algorithm in the context of signiﬁcantly

larger forests and to apply the proposed method at a system

level for map updating or within the SLAM stack.

ACKNOWLEDGMENT

he data from the test site was gathered in co-operation with

Stora Enso oy, Mets¨

ateho oy, and Aalto University.This re-

search are funded by Business Finland grant number 26004155

and Academy of Finland grant number 328755

REFERENCES

[1] Ville Kankare, Jari Vauhkonen, Topi Tanhuanpaa, Markus Holopainen,

Mikko Vastaranta, Marianna Joensuu, Anssi Krooks, Juha Hyyppa,

Hannu Hyyppa, Petteri Alho, and Risto Viitala. Accuracy in estimation

of timber assortments and stem distribution - a comparison of airborne

and terrestrial laser scanning techniques. ISPRS Journal of Photogram-

metry and Remote Sensing, 97:89–97, 2014.

12

[2] Thomas Hellstr¨

om, P. L¨

arkeryd, Tomas Nordfjell, and Ola Ringdahl.

Autonomous forest vehicles: Historic, envisioned, and state-of-the-art.

International Journal of Forest Engineering, 20:31–38, 01 2009.

[3] Keisuke Yoneda, Naoki Suganuma, Ryo Yanase, and Mohammad

Aldibaja. Automated driving recognition technologies for adverse

weather conditions. IATSS Research, 2019.

[4] Claudine Badue, Rˆ

anik Guidolini, Raphael Vivacqua Carneiro, Pedro

Azevedo, Vinicius Brito Cardoso, Avelino Forechi, Luan Jesus, Rodrigo

Berriel, Thiago Paix˜

ao, Filipe Mutz, et al. Self-driving cars: A survey.

arXiv preprint arXiv:1901.04407, 2019.

[5] Rajeev Thakur. Scanning lidar in advanced driver assistance systems

and beyond: Building a road map for next-generation lidar technology.

IEEE Consumer Electronics Magazine, 5(3):48–54, 2016.

[6] Li Qingqing, Jorge Pe˜

na Queralta, Tuan Nguyen Gia, Zhuo Zou, and

Tomi Westerlund. Multi sensor fusion for navigation and mapping

in autonomous vehicles: Accurate localization in urban environments.

Unammned Systems (to appear). Accepted and presented at the 9th IEEE

CIS-RAM conference., 2020.

[7] Wuming Zhang, Peng Wan, Tiejun Wang, Shangshu Cai, Yiming Chen,

Xiuliang Jin, and Guangjian Yan. A novel approach for the detection

of standing tree stems from plot-level terrestrial laser scanning data.

Remote sensing, 11(2):211, 2019.

[8] Marek Pierzchała, Philippe Gigu´

ere, and Rasmus Astrup. Mapping

forests using an unmanned ground vehicle with 3D LiDAR and graph-

SLAM. Computers and Electronics in Agriculture, 145:217 – 225, 2018.

[9] Xinlian Liang, Ville Kankare, Juha Hyypp¨

a, Yunsheng Wang, Antero

Kukko, Henrik Haggr´

en, Xiaowei Yu, Harri Kaartinen, Anttoni Jaakkola,

Fengying Guan, Markus Holopainen, and Mikko Vastaranta. Terrestrial

laser scanning in forest inventories. ISPRS Journal of Photogrammetry

and Remote Sensing, 115:63 – 77, 2016. Theme issue ’State-of-the-art

in photogrammetry, remote sensing and spatial information science’.

[10] Mikko Miettinen, Matti ¨

Ohman, Arto Visala, and Pekka Forsman.

Simultaneous localization and mapping for forest harvesters. In IEEE

International Conference on Robotics and Automation, Rooma April

2007, pages 517–522, 2007.

[11] Jian Tang, Yuwei Chen, Antero Kukko, Harri Kaartinen, Anttoni

Jaakkola, Ehsan Khoramshahi, Teemu Hakala, Juha Hyypp¨

a, Markus

Holopainen, and Hannu Hyypp¨

a. Slam-aided stem mapping for forest

inventory with small-footprint mobile lidar. Forests, 6(12):4588–4606,

2015.

[12] Jian Tang, Yuwei Chen, Antero Kukko, Harri Kaartinen, Anttoni

Jaakkola, Ehsan Khoramshahi, Teemu Hakala, Juha Hyypp¨

a, Markus

Holopainen, and Hannu Hyypp¨

a. SLAM aided stem mapping for forest

inventory with small-footprint mobile LiDAR. Forests, 6(12):4588–

4606, 12 2015.

[13] Torsten Sattler, Bastian Leibe, and Leif Kobbelt. Efﬁcient & effective

prioritized matching for large-scale image-based localization. IEEE

transactions on pattern analysis and machine intelligence, 39(9):1744–

1756, 2016.

[14] Martin Magnusson, Andreas Nuchter, Christopher Lorken, Achim J

Lilienthal, and Joachim Hertzberg. Evaluation of 3d registration reliabil-

ity and speed-a comparison of icp and ndt. In 2009 IEEE International

Conference on Robotics and Automation, pages 3907–3912. IEEE, 2009.

[15] Juli´

an Tomaˇ

st´

ık, ˇ

Simon Saloˇ

n, and Rastislav Piroh. Horizontal accuracy

and applicability of smartphone gnss positioning in forests. Forestry:

An International Journal of Forest Research, 90(2):187–198, 2016.

[16] Eloise G. Zimbelman and Robert F. Keefe. Real-time positioning in

logging: Effects of forest stand characteristics, topography, and line-

of-sight obstructions on gnss-rf transponder accuracy and radio signal

propagation. PLOS ONE, 13(1):1–17, 01 2018.

[17] Szymon Rusinkiewicz and Marc Levoy. Efﬁcient variants of the icp

algorithm. In Proceedings Third International Conference on 3-D

Digital Imaging and Modeling, pages 145–152. IEEE, 2001.

[18] Aleksandr Segal, Dirk Haehnel, and Sebastian Thrun. Generalized-icp.

In Robotics: science and systems, volume 2, page 435. Seattle, WA,

2009.

[19] Dirk Holz, Alexandru E Ichim, Federico Tombari, Radu B Rusu, and

Sven Behnke. Registration with the point cloud library: A modular

framework for aligning in 3-d. IEEE Robotics & Automation Magazine,

22(4):110–124, 2015.

[20] Martin Lauer, Sascha Lange, and Martin Riedmiller. Calculating

[21] Peter Biber and Wolfgang Straßer. The normal distributions transform:

A new approach to laser scan matching. In Proceedings 2003 IEEE/RSJ

International Conference on Intelligent Robots and Systems (IROS

2003)(Cat. No. 03CH37453), volume 3. IEEE, 2003.

the perfect match: an efﬁcient and accurate approach for robot self-

localization. In Robot Soccer World Cup. Springer, 2005.

[22] Pedro Nunez, Ricardo Vazquez-Martin, Jose C del Toro, Antonio

Bandera, and Francisco Sandoval. Feature extraction from laser scan

data based on curvature estimation for mobile robotics. In Proceedings

2006 IEEE International Conference on Robotics and Automation, 2006.

ICRA 2006., pages 1167–1172. IEEE, 2006.

[23] Aparajithan Sampath and Jie Shan. Clustering based planar roof

extraction from lidar data. In American Society for Photogrammetry

and Remote Sensing Annual Conference, Reno, Nevada, May, pages 1–

6, 2006.

[24] Jing Liang, Jixian Zhang, Kazhong Deng, Zhengjun Liu, and Qunshan

Shi. A new power-line extraction method based on airborne lidar point

cloud data. In 2011 International Symposium on Image and Data Fusion,

pages 1–4. IEEE, 2011.

[25] J. Zhang et al. Loam: Lidar odometry and mapping in real-time. In

Robotics: Science and Systems, 2014.

[26] Tixiao Shan and Brendan Englot. Lego-loam: Lightweight and ground-

optimized lidar odometry and mapping on variable terrain. In 2018

IEEE/RSJ International Conference on Intelligent Robots and Systems

(IROS), pages 4758–4765. IEEE, 2018.

[27] Sebastian Thrun, Daphne Koller, Zoubin Ghahmarani, and Hugh

Durrant-Whyte. Slam updates require constant time. In Workshop on

the Algorithmic Foundations of Robotics, pages 1–20. Citeseer, 2002.

[28] Yufeng Liu and Sebastian Thrun. Results for outdoor-slam using sparse

extended information ﬁlters. In 2003 IEEE International Conference

on Robotics and Automation (Cat. No. 03CH37422), volume 1, pages

1227–1233. IEEE, 2003.

[29] Iwan Ulrich and Illah Nourbakhsh. Appearance-based place recognition

for topological localization. In Proceedings 2000 ICRA. Millennium

Conference. IEEE International Conference on Robotics and Automa-

tion. Symposia Proceedings (Cat. No. 00CH37065), volume 2, pages

1023–1029. Ieee, 2000.

[30] Jun Chen, Chaomin Luo, Mohan Krishnan, Mark Paulik, and Yipeng

Tang. An enhanced dynamic delaunay triangulation-based path planning

algorithm for autonomous mobile robot navigation. In Intelligent Robots

and Computer Vision XXVII: Algorithms and Techniques, volume 7539,

page 75390P. International Society for Optics and Photonics, 2010.

[31] Marian Himstedt, Jan Frost, Sven Hellbach, Hans-Joachim B ¨

ohme,

and Erik Maehle. Large scale place recognition in 2d lidar scans

using geometrical landmark relations. In 2014 IEEE/RSJ International

Conference on Intelligent Robots and Systems, pages 5030–5035. IEEE,

2014.

[32] Simon Lynen, Michael Bosse, and Roland Siegwart. Trajectory-based

place-recognition for efﬁcient large scale localization. International

Journal of Computer Vision, 124(1):49–64, 2017.

[33] Michael Bosse and Robert Zlot. Place recognition using keypoint voting

in large 3d lidar datasets. In 2013 IEEE International Conference on

Robotics and Automation, pages 2677–2684. IEEE, 2013.

[34] Michael Bosse and Robert Zlot. Keypoint design and evaluation for

place recognition in 2d lidar maps. Robotics and Autonomous Systems,

57(12):1211–1224, 2009.

[35] Herbert Edelsbrunner. Triangulations and meshes in computational

geometry. Acta Numerica 2000, 9:133 – 213, 01 2000.

[36] Jon Louis Bentley. Multidimensional binary search trees used for

associative searching. Communications of the ACM, 18(9):509–517,

1975.

[37] Radu Bogdan Rusu. Semantic 3d object maps for everyday manipulation

in human living environments. KI-K¨

unstliche Intelligenz, 24(4):345–348,

2010.

[38] Der-Tsai Lee and Bruce J Schachter. Two algorithms for constructing a

delaunay triangulation. International Journal of Computer & Informa-

tion Sciences, 9(3):219–242, 1980.

[39] Z. Arzoumanian, Jason Holmberg, and Brad Norman. An astronomical

pattern-matching algorithm for computer-aided identiﬁcation of whale

sharks rhincodon typus. Journal of Applied Ecology, 42:999 – 1011, 09

2005.

[40] David Sinclair. S-hull: a fast radial sweep-hull routine for Delaunay

triangulation. arXiv e-prints, page arXiv:1604.01428, 2016.

[41] Fani Boukouvala, Ruth Misener, and Christodoulos Floudas. Global

optimization advances in mixed-integer nonlinear programming, minlp,

and constrained derivative-free optimization, cdfo. European Journal of

Operational Research, 252, 12 2015.