Preprints and early-stage research may not have been peer reviewed yet.

Abstract and Figures

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 difficult to find a group of significant 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 fixed 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.5 m/s. The accuracy and speed limit is realistic during forest operations.
Content may be subject to copyright.
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 difficult to find a group
of significant 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 fixed 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
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-specific 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
identification 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}
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
significant 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 field-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 field 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 fixed-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 difficult accurate mapping. A small error in the
orientation estimation can significantly affect the mapping of
trees that are farther away.
Research in the field 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
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 finding either a partial match between sensor
data batches acquired at different times, or finding 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 defined e.g. by step length limit and the point
cloud overlap limit, we study a global point cloud matching
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 first 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 finding 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 significant 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 significantly 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 offline. 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 find geometric
transformations that match either the complete graph or a
set of subgraphs. Thrun et al. developed the notions of
sparse extended information filters (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 classification
and place recognition [29]–[31]. Among the most relevant
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 fixed, predefined paths such as roads.
Nonetheless, their performance is significantly degraded in
unstructured environments and, in particular, in forests. In a
forest harvesting operation, harvesters and forwarders travel
through undefined 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 field 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 first 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 benefits of our
approach. Finally, Section 5 concludes the work and outlines
future research directions.
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 profile was mostly flat,
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 field of view, 30° ver-
tical field of view with ±15oup and down, 5-20 Hz scanning
frequency and 100 m scan range. As Figure 2 shows, the lidar
is fixed 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
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 offline by processing the point
cloud data defining the forest map. The latter four steps are
then done online onboard the harvester to estimate its position
in real time. The steps are:
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.
Cloud Lidar Mapping
Global Map Trunk Extraction Delaunay
Trunk Extraction Delaunay
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
2) Delaunay triangulation of the global map from the
segmented trunk points (offline).
3) Aggregation of 3D lidar scans into a local point cloud
defining the robot’s position (online).
4) Segmentation of trunks from the local point cloud (on-
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 VR2and produces a triplet G=
(V, E, T ), where EV2is a set of edges and TV3is
a set of triangles with the so called Delaunay property i.e.
the circumscribed triangle associated with each triangle tT
contains no points vVothers than the three vertex points
of the triangle t.
The first step in the proposed method takes a global point
cloud P Cmap ={Pi}i=1..nmR3, representing the map
of the forest, and produces a robust set of trunk positions
P Ctrunks ={Pi}i=1...ntR2. 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
offline before the robot is deployed, or every time the robot
gets a global map update.
The third, fourth and fifth steps, which happen online, cover
the generation of a local, real-time Delaunay triangulation
of the trees within the field 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 define
the local DT graph Glocal = (Vlocal, Elocal , Tlocal).
Finally, in the sixth step in the process we calculate a rigid
body transformation, defined 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
efficient 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 finding the best
match between local and global topology.
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
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 final extracted trunk point cloud (green) after clustering
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 define
P Ctrunks as a set of landmarks from P Cmap . Compared to
the other environmental features such as the bumpy terrain,
branch structures, forest floor vegetation and the bushes in the
forest, tree trunks are a significant and relatively noise-free
To segment the trunk points, the first 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 find the trunk point cloud’s [37]. Instead of focusing on
finding every trunk in the P Cmap, we focus on extracting the
most significant ones. We define the most significant 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 find 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 ¯pR2of 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 figure 5(a) shows the map to be processed, the
figure 5 (b) shows the extracted point cloud after removing
the ground point cloud and branch point cloud from the input
map. The figure 5 (c) shows the final extracted stable trunk
point cloud after cluster processing.
E. Global Map DT Graph Generation
A DT has multiple beneficial 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 defines 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
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 pP 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
6Ptpi// Add point to trunks point cloud;
9foreach cluster Clusters do
10 if Cluster.points.size() < cluster threshold
11 Delete cluster from Clusters
12 else
13 ¯p=mean pCluster 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 find 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.
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 ), defines a set Vof vertices, a set EV2
of edges and a set TV3. 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 find 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, 0d(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 defined as:
d(t1, t2) = |A2A1|+|l2l1|.
To speed up the real-time matching process, all triangle
perimeters and areas of the global DT graph have been
computed offline. 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 find a matching subset in Gmap. From
{Tselected}, we build the corresponding graph
There may exist multiple closely similar triangles Tcandidate
in Gmap to any specific 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 S0Tselected 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
signified with its end points.
To solve the transformation parameters ptand θ, we first
need to find the correspondence between vertices of trian-
gle stars S={Si}i=0..3Tselected and triangles S0=
i}i=0..3Tcandidate. The definition of a triangle star
is easily seen from Figure 7 (a). As the figure shows, the
vertex match between Tselected and Tcandidate can be divided
into three steps. The first step is in the detail (b). We then
find the first matching pair S0=ABC Tselected and
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
of corresponding vertices. For the second step, the goal is to
find 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 aRof the following formula:
a= ( ~
CA ×~
HM ×~
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 ViTselected 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..3Tselected and S0={S0
i}i=0..3Tcandidate, we
can find 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
definitions in (3), where the translation tpis the best possible
one estimate, since it equates the mean of two patterns. The
angle θis defined by measuring how large a rotation is needed
to transform a vectors ViVto V0
f(i)V0, where the V
represents the mean of Tselected and V0is the mean of the
There are six pairs i= 1...6of vertices coupling Tselected,
Tlocal and Tcandidate from Gmap, and six possible candidates
βito be chosen as the final local rotation θ. We choose the
value of βiwhich fits 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=01
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 fit 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 final position estimation.
Using a zeroth power as a shorthand when producing unit
vectors a0=a/kak, we define counter-clockwise oriented
angles βi, which rotate vectors Viparallel to vectors Vf(i).
Finally, we define approximate values for the translation pt
and the rotation θaccording to (3).
V=mean VTselected V
V0=mean V0Tcandidate V0
βi=sign P Vi·Vf(i)arccos V0
f(i), i = 1..6
θ=argmin βi, i=1...6
"cos βisin βi
sin βicos βi#VjVTVf(j)V0T
2) Geometric Verification, 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 final step is to find 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
final 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
confirm stable position estimation when enough consecutive
lidar scans were aggregated.
(θ, tx, ty) =
cos βsin β x
sin βcos β y
0 0 1
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 defined 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 final estimation of the robot position
is (x0+tx, y0+ty)with an orientation θin the global map.
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, five or
Fig. 7: (a): The selected triangle Tselected in Glocal with candidate matched triangle Tcandidate in Gmap (b): Find the first
correspond point Ain Tselected with it’s Min Tcandidate by finding 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 defined 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 significant 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 influencing 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.
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 significantly 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)
Computing time (sec)
Accuracy vs. computing speed
local map creation
match time
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.
An application topic of the proposed navigation system is an
automated transport robot, which moves logs from the forest to
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
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 significantly influence 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 significant 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 efficient, 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 field 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 final 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.
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 efficient, 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 significantly
larger forests and to apply the proposed method at a system
level for map updating or within the SLAM stack.
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
[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.
[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,
[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. Efficient & 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ˇ
ı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. Efficient 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,
[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 efficient 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 filters. 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 ¨
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,
[32] Simon Lynen, Michael Bosse, and Roland Siegwart. Trajectory-based
place-recognition for efficient 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,
[37] Radu Bogdan Rusu. Semantic 3d object maps for everyday manipulation
in human living environments. KI-K¨
unstliche Intelligenz, 24(4):345–348,
[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 identification of whale
sharks rhincodon typus. Journal of Applied Ecology, 42:999 – 1011, 09
[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.
ResearchGate has not been able to resolve any citations for this publication.
Full-text available
During automated driving in urban areas, decisions must be made while recognizing the surrounding environment using sensors such as camera, Light Detection and Ranging (LiDAR), millimeter-wave radar (MWR), and the global navigation satellite system (GNSS). The ability to drive under various environmental conditions is an important issue for automated driving on any road. In order to introduce the automated vehicles into the markets, the ability to evaluate various traffic conditions and navigate safely presents serious challenges. Another important challenge is the development of a robust recognition system can account for adverse weather conditions. Sun glare, rain, fog, and snow are adverse weather conditions that can occur in the driving environment. This paper summarizes research focused on automated driving technologies and discuss challenges to identifying adverse weather and other situations that make driving difficult, thus complicating the introduction of automated vehicles to the market. Keywords: Automated vehicle, Self-localization, Surrounding recognition, Path planning, Adverse condition
Full-text available
Tree stem detection is a key step toward retrieving detailed stem attributes from terrestrial laser scanning (TLS) data. Various point-based methods have been proposed for the stem point extraction at both individual tree and plot levels. The main limitation of the point-based methods is their high computing demand when dealing with plot-level TLS data. Although segment-based methods can reduce the computational burden and uncertainties of point cloud classification, its application is largely limited to urban scenes due to the complexity of the algorithm, as well as the conditions of natural forests. Here we propose a novel and simple segment-based method for efficient stem detection at the plot level, which is based on the curvature feature of the points and connected component segmentation. We tested our method using a public TLS dataset with six forest plots that were collected for the international TLS benchmarking project in Evo, Finland. Results showed that the mean accuracies of the stem point extraction were comparable to the state-of-art methods (>95%). The accuracies of the stem mappings were also comparable to the methods tested in the international TLS benchmarking project. Additionally, our method was applicable to a wide range of stem forms. In short, the proposed method is accurate and simple; it is a sensible solution for the stem detection of standing trees using TLS data.
Full-text available
Enabling automated 3D mapping in forests is an important component of the future development of forest technology, and has been garnering interest in the scientific community, as can be seen from the many recent publications. Accordingly, the authors of the present paper propose the use of a Simultaneous Localisation and Mapping algorithm, called graph-SLAM, to generate local maps of forests. In their study, the 3D data required for the mapping process were collected using a custom-made, mobile platform equipped with a number of sensors, including Velodyne VLP-16 LiDAR, a stereo camera, an IMU, and a GPS. The 3D map was generated solely from laser scans, first by relying on laser odometry and then by improving it with robust graph optimisation after loop closures, which is the core of the graph-SLAM algorithm. The resulting map, in the form of a 3D point cloud, was then evaluated in terms of its accuracy and precision. Specifically, the accuracy of the fitted diameter at breast height (DBH) and the relative distance between the trees were evaluated. The results show that the DBH estimates using the Pratt circle fit method could enable a mean estimation error of approximately 2 cm (7–12%) and an RMSE of 2.38 cm (9%), whereas for tree positioning accuracy, the mean error was 0.0476 m. The authors conclude that robust SLAM algorithms can support the development of forestry by providing cost-effective and acceptable quality methods for forest mapping. Moreover, such maps open up the possibility for precision localisation for forestry vehicles.
Full-text available
Real-time positioning on mobile devices using global navigation satellite system (GNSS) technology paired with radio frequency (RF) transmission (GNSS-RF) may help to improve safety on logging operations by increasing situational awareness. However, GNSS positional accuracy for ground workers in motion may be reduced by multipath error, satellite signal obstruction, or other factors. Radio propagation of GNSS locations may also be impacted due to line-of-sight (LOS) obstruction in remote, forested areas. The objective of this study was to characterize the effects of forest stand characteristics, topography, and other LOS obstructions on the GNSS accuracy and radio signal propagation quality of multiple Raveon Atlas PT GNSS-RF transponders functioning as a network in a range of forest conditions. Because most previous research with GNSS in forestry has focused on stationary units, we chose to analyze units in motion by evaluating the time-to-signal accuracy of geofence crossings in 21 randomly-selected stands on the University of Idaho Experimental Forest. Specifically, we studied the effects of forest stand characteristics, topography, and LOS obstructions on (1) the odds of missed GNSS-RF signals, (2) the root mean squared error (RMSE) of Atlas PTs, and (3) the time-to-signal accuracy of safety geofence crossings in forested environments. Mixed-effects models used to analyze the data showed that stand characteristics, topography, and obstructions in the LOS affected the odds of missed radio signals while stand variables alone affected RMSE. Both stand characteristics and topography affected the accuracy of geofence alerts.
Full-text available
Place recognition is a core competency for any visual simultaneous localization and mapping system. Identifying previously visited places enables the creation of globally accurate maps, robust relocalization, and multi-user mapping. To match one place to another, most state-of-the-art approaches must decide a priori what constitutes a place, often in terms of how many consecutive views should overlap, or how many consecutive images should be considered together. Unfortunately, such threshold dependencies limit their generality to different types of scenes. In this paper, we present a placeless place recognition algorithm using a novel match-density estimation technique that avoids heuristically discretizing the space. Instead, our approach considers place recognition as a problem of continuous matching between image streams, automatically discovering regions of high match density that represent overlapping trajectory segments. The algorithm uses well-studied statistical tests to identify the relevant matching regions which are subsequently passed to an absolute pose algorithm to recover the geometric alignment. We demonstrate the efficiency and accuracy of our methodology on three outdoor sequences, including a comprehensive evaluation against ground-truth from publicly available datasets that shows our approach outperforms several state-of-the-art algorithms for place recognition. Furthermore we compare our overall algorithm to the currently best performing system for global localization and show how we outperform the approach on challenging indoor and outdoor datasets.
Full-text available
This study focuses on the evaluation of horizontal accuracy of smartphones for collecting and using spatial data in forests in comparison with other Global Navigation Satellite Systems (GNSS) devices. Accuracy evaluation was conducted at 74 points in a mixed deciduous-coniferous forest (during leaf-on and leaf-off season) and additionally at 17 points under open area conditions. Total station theodolite measurements served as a reference for all point positions. Positional accuracies of three smartphones (one with two differing OS versions), a tablet, a mapping- and a survey-grade receiver were compared. Root mean square errors of positional accuracies ranged from 4.96–11.45 m during leaf-on and 4.51–6.72 m during leaf-off season in the forest plots to 1.90–2.36 m under open area conditions. Differences in positional accuracy between leaf-on and leaf-off conditions were only significant in some cases, while differences between forest and open area were always significant. Differences between devices were only significant under leaf-on conditions except for the survey-grade receiver, which was significantly more accurate than all other devices in all tested cases. In a second experiment, two smartphones, a handheld receiver and satellite imagery were used to measure the area of wind damage in disturbed forests. The results obtained with the GNSS devices showed a significantly higher accuracy of area and timber volume assessment compared with visual estimation, particularly in larger disturbed areas. Generally, the results suggest that current smartphones can be successfully used for some tasks in forest management where precision of the spatial data is not of highest priority.
The combination of data from multiple sensors, also known as sensor fusion or data fusion, is a key aspect in the design of autonomous robots. In particular, algorithms able to accommodate sensor fusion techniques enable increased accuracy, and are more resilient against the malfunction of individual sensors. The development of algorithms for autonomous navigation, mapping and localization have seen big advancements over the past two decades. Nonetheless, challenges remain in developing robust solutions for accurate localization in dense urban environments, where the so-called last-mile delivery occurs. In these scenarios, local motion estimation is combined with the matching of real-time data with a detailed pre-built map. In this paper, we utilize data gathered with an autonomous delivery robot to compare different sensor fusion techniques and evaluate which are the algorithms providing the highest accuracy depending on the environment. The techniques we analyze and propose in this paper utilize 3D lidar data, inertial data, GNSS data and wheel encoder readings. We show how lidar scan matching combined with other sensor data can be used to increase the accuracy of the robot localization and, in consequence, its navigation. Moreover, we propose a strategy to reduce the impact on navigation performance when a change in the environment renders map data invalid or part of the available map is corrupted.
Accurately determining the position and orientation from which an image was taken, i.e., computing the camera pose, is a fundamental step in many Computer Vision applications. The pose can be recovered from 2D-3D matches between 2D image positions and points in a 3D model of the scene. Recent advances in Structure-from-Motion allow us to reconstruct large scenes and thus create the need for image-based localization methods that efficiently handle large-scale 3D models while still being effective, i.e., while localizing as many images as possible. This paper presents an approach for large scale image-based localization that is both efficient and effective. At the core of our approach is a novel prioritized matching step that enables us to first consider features more likely to yield 2D-to-3D matches and to terminate the correspondence search as soon as enough matches have been found. Matches initially lost due to quantization are efficiently recovered by integrating 3D-to-2D search. We show how visibility information from the reconstruction process can be used to improve the efficiency of our approach. We evaluate the performance of our method through extensive experiments and demonstrate that it offers the best combination of efficiency and effectiveness among current state-of-the-art approaches for localization.